First_Dev_Dept/IMU
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: add stationary yaw self-calibration

Browse Source
This commit is contained in:
zequn.liu
2026-06-18 16:32:28 +08:00
parent 93b9b91db7
commit 478fd706b3
4 changed files with 564 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/imu_static_calibrator.py
+180
View File
@@ -0,0 +1,180 @@
"""Numeric-only stationary detector and yaw gyro bias estimator."""
from __future__ import annotations
from dataclasses import dataclass
import math
import numpy as np
MOVING = 0
CANDIDATE = 1
STATIC = 2
TIME_EPSILON_S = 1.0e-12
@dataclass(frozen=True)
class StaticCorrectionConfig:
enter_seconds: float = 2.0
gyro_threshold_dps: float = 0.5
acc_norm_tolerance_g: float = 0.2
acc_stability_threshold_g: float = 0.02
@dataclass
class StaticCorrectionState:
config: StaticCorrectionConfig
mode: int
active_yaw_bias_z_dps: float
candidate_elapsed_s: float
candidate_count: int
candidate_acc_mean_g: np.ndarray
candidate_gyro_z_mean_dps: float
static_count: int
static_acc_mean_g: np.ndarray
static_gyro_z_mean_dps: float
def initialize(
config: StaticCorrectionConfig,
initial_yaw_bias_z_dps: float,
) -> StaticCorrectionState:
_validate_config(config)
if not math.isfinite(initial_yaw_bias_z_dps):
raise ValueError("initial_yaw_bias_z_dps must be finite")
return StaticCorrectionState(
config=config,
mode=MOVING,
active_yaw_bias_z_dps=float(initial_yaw_bias_z_dps),
candidate_elapsed_s=0.0,
candidate_count=0,
candidate_acc_mean_g=np.zeros(3),
candidate_gyro_z_mean_dps=0.0,
static_count=0,
static_acc_mean_g=np.zeros(3),
static_gyro_z_mean_dps=0.0,
)
def step(
state: StaticCorrectionState,
dt_s: float,
acc_g: np.ndarray,
gyro_dps: np.ndarray,
gyro_bias_xy_dps: np.ndarray,
) -> bool:
if not math.isfinite(dt_s) or dt_s < 0.0:
raise ValueError("dt_s must be finite and non-negative")
acc = _vector(acc_g, 3, "acc_g")
gyro = _vector(gyro_dps, 3, "gyro_dps")
bias_xy = _vector(gyro_bias_xy_dps, 2, "gyro_bias_xy_dps")
gyro_residual = np.array(
[
gyro[0] - bias_xy[0],
gyro[1] - bias_xy[1],
gyro[2] - state.active_yaw_bias_z_dps,
]
)
absolute_gate_ok = (
abs(float(np.linalg.norm(acc)) - 1.0) <= state.config.acc_norm_tolerance_g
and float(np.linalg.norm(gyro_residual)) <= state.config.gyro_threshold_dps
)
if state.mode == STATIC:
stable_acc = (
float(np.linalg.norm(acc - state.static_acc_mean_g))
<= state.config.acc_stability_threshold_g
)
if not absolute_gate_ok or not stable_acc:
_reset_candidate(state)
state.mode = MOVING
return False
state.static_count += 1
state.static_acc_mean_g += (acc - state.static_acc_mean_g) / state.static_count
state.static_gyro_z_mean_dps += (
gyro[2] - state.static_gyro_z_mean_dps
) / state.static_count
state.active_yaw_bias_z_dps = state.static_gyro_z_mean_dps
return True
if not absolute_gate_ok:
_reset_candidate(state)
state.mode = MOVING
return False
if state.mode == MOVING:
_start_candidate(state, acc, gyro[2])
return False
stable_acc = (
float(np.linalg.norm(acc - state.candidate_acc_mean_g))
<= state.config.acc_stability_threshold_g
)
if not stable_acc:
_start_candidate(state, acc, gyro[2])
return False
state.candidate_count += 1
state.candidate_elapsed_s += dt_s
state.candidate_acc_mean_g += (
acc - state.candidate_acc_mean_g
) / state.candidate_count
state.candidate_gyro_z_mean_dps += (
gyro[2] - state.candidate_gyro_z_mean_dps
) / state.candidate_count
if state.candidate_elapsed_s + TIME_EPSILON_S < state.config.enter_seconds:
return False
state.mode = STATIC
state.static_count = state.candidate_count
state.static_acc_mean_g = state.candidate_acc_mean_g.copy()
state.static_gyro_z_mean_dps = state.candidate_gyro_z_mean_dps
state.active_yaw_bias_z_dps = state.static_gyro_z_mean_dps
return True
def _start_candidate(
state: StaticCorrectionState,
acc_g: np.ndarray,
gyro_z_dps: float,
) -> None:
state.mode = CANDIDATE
state.candidate_elapsed_s = 0.0
state.candidate_count = 1
state.candidate_acc_mean_g = acc_g.copy()
state.candidate_gyro_z_mean_dps = float(gyro_z_dps)
def _reset_candidate(state: StaticCorrectionState) -> None:
state.candidate_elapsed_s = 0.0
state.candidate_count = 0
state.candidate_acc_mean_g.fill(0.0)
state.candidate_gyro_z_mean_dps = 0.0
def _vector(value, size: int, name: str) -> np.ndarray:
vector = np.asarray(value, dtype=float)
if vector.shape != (size,) or not np.all(np.isfinite(vector)):
raise ValueError(f"{name} must be a finite {size}-element vector")
return vector
def _validate_config(config: StaticCorrectionConfig) -> None:
values = (
config.enter_seconds,
config.gyro_threshold_dps,
config.acc_norm_tolerance_g,
config.acc_stability_threshold_g,
)
if not all(math.isfinite(value) for value in values):
raise ValueError("static correction configuration must be finite")
if config.enter_seconds <= 0.0:
raise ValueError("enter_seconds must be positive")
if config.gyro_threshold_dps <= 0.0:
raise ValueError("gyro_threshold_dps must be positive")
if config.acc_norm_tolerance_g <= 0.0:
raise ValueError("acc_norm_tolerance_g must be positive")
if config.acc_stability_threshold_g <= 0.0:
raise ValueError("acc_stability_threshold_g must be positive")
scripts/run_imu_ekf.py
+145 -22
View File
@@ -12,6 +12,7 @@ from pathlib import Path
import numpy as np
from scripts import imu_ekf_core as ekf
from scripts import imu_static_calibrator as static_calibrator
REQUIRED_COLUMNS = (
@@ -62,10 +63,24 @@ def process_file(
output_dir: Path,
init_seconds: int = 3,
yaw_bias_seconds: int = 60,
static_correction_seconds: float = 2.0,
static_gyro_threshold_dps: float = 0.5,
static_acc_norm_tolerance_g: float = 0.2,
static_acc_stability_threshold_g: float = 0.02,
max_points: int = 2500,
) -> EkfFileResult:
init_seconds = _validate_init_seconds(init_seconds)
yaw_bias_seconds = _validate_yaw_bias_seconds(yaw_bias_seconds)
static_correction_seconds = _validate_static_correction_seconds(static_correction_seconds)
static_gyro_threshold_dps = _validate_positive_float(
static_gyro_threshold_dps, "static_gyro_threshold_dps"
)
static_acc_norm_tolerance_g = _validate_positive_float(
static_acc_norm_tolerance_g, "static_acc_norm_tolerance_g"
)
static_acc_stability_threshold_g = _validate_positive_float(
static_acc_stability_threshold_g, "static_acc_stability_threshold_g"
)
input_csv = Path(input_csv)
output_dir = Path(output_dir)
output_dir.mkdir(parents=True, exist_ok=True)
@@ -93,6 +108,8 @@ def process_file(
"gyro_bias_y_dps",
"gyro_bias_z_dps",
"fixed_yaw_bias_z_dps",
"active_yaw_bias_z_dps",
"is_static",
"acc_residual_norm",
"dt_s",
"acc_update_used",
@@ -102,44 +119,79 @@ def process_file(
segment_id = 0
state: ekf.ImuEkfState | None = None
static_state: static_calibrator.StaticCorrectionState | None = None
yaw_bias_buffer: list[ImuRow] = []
init_buffer: list[ImuRow] = []
fixed_yaw_bias_enabled = yaw_bias_seconds > 0
fixed_yaw_bias_z_dps: float | None = 0.0 if yaw_bias_seconds == 0 else None
active_yaw_bias_z_dps: float | None = fixed_yaw_bias_z_dps
static_correction_enabled = static_correction_seconds > 0.0
wrapper_yaw_bias_enabled = fixed_yaw_bias_enabled or static_correction_enabled
static_config = None
if static_correction_enabled:
static_config = static_calibrator.StaticCorrectionConfig(
enter_seconds=static_correction_seconds,
gyro_threshold_dps=static_gyro_threshold_dps,
acc_norm_tolerance_g=static_acc_norm_tolerance_g,
acc_stability_threshold_g=static_acc_stability_threshold_g,
)
previous_input_time: float | None = None
previous_output_time: float | None = None
relative_yaw_deg = 0.0
previous_yaw_deg: float | None = None
def reset_segment() -> None:
nonlocal state, yaw_bias_buffer, init_buffer, fixed_yaw_bias_z_dps
nonlocal state, static_state, yaw_bias_buffer, init_buffer
nonlocal fixed_yaw_bias_z_dps, active_yaw_bias_z_dps
nonlocal previous_output_time, relative_yaw_deg, previous_yaw_deg
state = None
static_state = None
yaw_bias_buffer = []
init_buffer = []
fixed_yaw_bias_z_dps = 0.0 if yaw_bias_seconds == 0 else None
active_yaw_bias_z_dps = fixed_yaw_bias_z_dps
previous_output_time = None
relative_yaw_deg = 0.0
previous_yaw_deg = None
def write_row(row: ImuRow) -> None:
nonlocal input_rows, previous_output_time, relative_yaw_deg, previous_yaw_deg
nonlocal active_yaw_bias_z_dps
if state is None:
raise ValueError("EKF state is not initialized")
if fixed_yaw_bias_z_dps is None:
raise ValueError("fixed yaw bias is not initialized")
if fixed_yaw_bias_enabled:
if active_yaw_bias_z_dps is None:
raise ValueError("active yaw bias is not initialized")
if wrapper_yaw_bias_enabled:
state.gyro_bias_rad_s[2] = 0.0
dt_s = 0.0 if previous_output_time is None else row.sensor_uptime_s - previous_output_time
previous_output_time = row.sensor_uptime_s
used_update, residual_norm = ekf.step(state, dt_s, _acc_mps2(row), _gyro_rad_s(row))
if fixed_yaw_bias_enabled:
is_static = False
if static_state is not None:
bias_xy_dps = np.degrees(state.gyro_bias_rad_s[0:2])
is_static = static_calibrator.step(
static_state,
dt_s,
np.array(row.acc_g, dtype=float),
np.array(row.gyro_dps, dtype=float),
bias_xy_dps,
)
active_yaw_bias_z_dps = static_state.active_yaw_bias_z_dps
corrected_row = _row_with_yaw_bias(row, active_yaw_bias_z_dps)
used_update, residual_norm = ekf.step(
state, dt_s, _acc_mps2(corrected_row), _gyro_rad_s(corrected_row)
)
if wrapper_yaw_bias_enabled:
state.gyro_bias_rad_s[2] = 0.0
roll, pitch, yaw = ekf.quaternion_to_euler_deg(state.q)
if previous_yaw_deg is None:
relative_yaw_deg = 0.0
previous_yaw_deg = yaw
elif is_static:
previous_yaw_deg = yaw
else:
relative_yaw_deg += _unwrap_delta_deg(yaw - previous_yaw_deg)
previous_yaw_deg = yaw
@@ -159,6 +211,8 @@ def process_file(
"gyro_bias_y_dps": bias_dps[1],
"gyro_bias_z_dps": bias_dps[2],
"fixed_yaw_bias_z_dps": fixed_yaw_bias_z_dps,
"active_yaw_bias_z_dps": active_yaw_bias_z_dps,
"is_static": int(is_static),
"acc_residual_norm": residual_norm,
"dt_s": dt_s,
"acc_update_used": int(used_update),
@@ -169,37 +223,47 @@ def process_file(
_append_bounded(samples, _sample_for_html(out), max_points)
def initialize_and_write_buffer() -> None:
nonlocal state, init_buffer
state = _initialize_state(init_buffer, init_seconds)
if fixed_yaw_bias_enabled:
nonlocal state, static_state, init_buffer
if active_yaw_bias_z_dps is None:
raise ValueError("active yaw bias is not initialized")
corrected_init_rows = [
_row_with_yaw_bias(row, active_yaw_bias_z_dps) for row in init_buffer
]
state = _initialize_state(corrected_init_rows, init_seconds)
if wrapper_yaw_bias_enabled:
state.gyro_bias_rad_s[2] = 0.0
if static_config is not None:
static_state = static_calibrator.initialize(
static_config,
initial_yaw_bias_z_dps=active_yaw_bias_z_dps,
)
buffered_rows = init_buffer
init_buffer = []
for buffered_row in buffered_rows:
write_row(buffered_row)
def process_row_with_fixed_yaw_bias(row: ImuRow) -> None:
if fixed_yaw_bias_z_dps is None:
raise ValueError("fixed yaw bias is not initialized")
corrected_row = _row_with_fixed_yaw_bias(row, fixed_yaw_bias_z_dps)
def process_row_with_yaw_bias(row: ImuRow) -> None:
if active_yaw_bias_z_dps is None:
raise ValueError("active yaw bias is not initialized")
if state is None:
init_buffer.append(corrected_row)
if init_seconds == 0 or corrected_row.sensor_uptime_s - init_buffer[0].sensor_uptime_s >= init_seconds:
init_buffer.append(row)
if init_seconds == 0 or row.sensor_uptime_s - init_buffer[0].sensor_uptime_s >= init_seconds:
initialize_and_write_buffer()
else:
write_row(corrected_row)
write_row(row)
def initialize_fixed_yaw_bias_from_buffer() -> None:
nonlocal yaw_bias_buffer, fixed_yaw_bias_z_dps
nonlocal yaw_bias_buffer, fixed_yaw_bias_z_dps, active_yaw_bias_z_dps
if fixed_yaw_bias_z_dps is not None:
return
if not yaw_bias_buffer:
raise ValueError("at least one IMU row is required for fixed yaw bias initialization")
fixed_yaw_bias_z_dps = _fixed_yaw_bias_z_dps(yaw_bias_buffer)
active_yaw_bias_z_dps = fixed_yaw_bias_z_dps
buffered_rows = yaw_bias_buffer
yaw_bias_buffer = []
for buffered_row in buffered_rows:
process_row_with_fixed_yaw_bias(buffered_row)
process_row_with_yaw_bias(buffered_row)
def flush_segment() -> None:
if fixed_yaw_bias_z_dps is None and yaw_bias_buffer:
@@ -223,11 +287,11 @@ def process_file(
if fixed_yaw_bias_z_dps is None:
if yaw_bias_buffer and row.sensor_uptime_s - yaw_bias_buffer[0].sensor_uptime_s >= yaw_bias_seconds:
initialize_fixed_yaw_bias_from_buffer()
process_row_with_fixed_yaw_bias(row)
process_row_with_yaw_bias(row)
else:
yaw_bias_buffer.append(row)
else:
process_row_with_fixed_yaw_bias(row)
process_row_with_yaw_bias(row)
previous_input_time = row.sensor_uptime_s
@@ -291,6 +355,16 @@ def write_html_report(results: list[EkfFileResult], html_path: Path, max_points:
const labels = {{ roll_deg: 'roll deg', pitch_deg: 'pitch deg', relative_yaw_deg: 'relative yaw deg' }};
const redraws = [];
function drawStaticRanges(ctx, rows, xmin, xspan, left, top, plotWidth, plotHeight) {{
ctx.fillStyle = 'rgba(20, 108, 46, 0.10)';
for (let i = 0; i + 1 < rows.length; i += 1) {{
if (rows[i].is_static < 0.5) continue;
const x0 = left + ((rows[i].sensor_uptime_s - xmin) / xspan) * plotWidth;
const x1 = left + ((rows[i + 1].sensor_uptime_s - xmin) / xspan) * plotWidth;
ctx.fillRect(x0, top, Math.max(1, x1 - x0), plotHeight);
}}
}}
function drawChart(canvas, rows, fields) {{
const ctx = canvas.getContext('2d');
const dpr = window.devicePixelRatio || 1;
@@ -323,6 +397,8 @@ def write_html_report(results: list[EkfFileResult], html_path: Path, max_points:
const gap = 18;
const plotWidth = Math.max(1, width - left - right);
const laneHeight = Math.max(40, (height - top - bottom - gap * (fields.length - 1)) / fields.length);
const plotHeight = laneHeight * fields.length + gap * (fields.length - 1);
drawStaticRanges(ctx, finiteRows, xmin, xspan, left, top, plotWidth, plotHeight);
fields.forEach((field, fieldIndex) => {{
const laneTop = top + fieldIndex * (laneHeight + gap);
@@ -384,7 +460,10 @@ def write_html_report(results: list[EkfFileResult], html_path: Path, max_points:
const fixedYawBias = lastSample && Number.isFinite(lastSample.fixed_yaw_bias_z_dps)
? ` | fixed yaw bias z: ${{lastSample.fixed_yaw_bias_z_dps.toFixed(5)}} dps`
: '';
summary.textContent = `Rows: ${{file.input_rows}} | Output: ${{file.output_csv}}${{fixedYawBias}}`;
const activeYawBias = lastSample && Number.isFinite(lastSample.active_yaw_bias_z_dps)
? ` | active yaw bias z: ${{lastSample.active_yaw_bias_z_dps.toFixed(5)}} dps`
: '';
summary.textContent = `Rows: ${{file.input_rows}} | Output: ${{file.output_csv}}${{fixedYawBias}}${{activeYawBias}}`;
section.appendChild(summary);
const table = document.createElement('table');
@@ -406,7 +485,7 @@ def write_html_report(results: list[EkfFileResult], html_path: Path, max_points:
const legend = document.createElement('div');
legend.className = 'meta';
legend.textContent = 'roll red, pitch green, relative yaw blue';
legend.textContent = 'roll red, pitch green, relative yaw blue, static intervals shaded';
section.appendChild(legend);
app.appendChild(section);
@@ -429,6 +508,10 @@ def main(argv: list[str] | None = None) -> int:
parser.add_argument("--output-dir", type=Path, default=Path("output") / "ekf")
parser.add_argument("--init-seconds", type=_parse_init_seconds, default=3)
parser.add_argument("--yaw-bias-seconds", type=_parse_yaw_bias_seconds, default=60)
parser.add_argument("--static-correction-seconds", type=_parse_static_correction_seconds, default=2.0)
parser.add_argument("--static-gyro-threshold-dps", type=_parse_positive_float, default=0.5)
parser.add_argument("--static-acc-norm-tolerance-g", type=_parse_positive_float, default=0.2)
parser.add_argument("--static-acc-stability-threshold-g", type=_parse_positive_float, default=0.02)
parser.add_argument("--max-points", type=int, default=2500)
args = parser.parse_args(argv)
@@ -442,6 +525,10 @@ def main(argv: list[str] | None = None) -> int:
args.output_dir,
init_seconds=args.init_seconds,
yaw_bias_seconds=args.yaw_bias_seconds,
static_correction_seconds=args.static_correction_seconds,
static_gyro_threshold_dps=args.static_gyro_threshold_dps,
static_acc_norm_tolerance_g=args.static_acc_norm_tolerance_g,
static_acc_stability_threshold_g=args.static_acc_stability_threshold_g,
max_points=args.max_points,
)
for path in csv_files
@@ -527,6 +614,24 @@ def _validate_yaw_bias_seconds(value) -> int:
return value
def _validate_static_correction_seconds(value) -> float:
if isinstance(value, bool) or not isinstance(value, (int, float)):
raise ValueError("static_correction_seconds must be a 0-10 second number")
value = float(value)
if not math.isfinite(value) or not 0.0 <= value <= 10.0:
raise ValueError("static_correction_seconds must be a 0-10 second number")
return value
def _validate_positive_float(value, name: str) -> float:
if isinstance(value, bool) or not isinstance(value, (int, float)):
raise ValueError(f"{name} must be a finite positive number")
value = float(value)
if not math.isfinite(value) or value <= 0.0:
raise ValueError(f"{name} must be a finite positive number")
return value
def _parse_init_seconds(value: str) -> int:
try:
parsed = int(value)
@@ -553,6 +658,22 @@ def _parse_yaw_bias_seconds(value: str) -> int:
raise argparse.ArgumentTypeError(str(exc)) from exc
def _parse_static_correction_seconds(value: str) -> float:
try:
parsed = float(value)
return _validate_static_correction_seconds(parsed)
except ValueError as exc:
raise argparse.ArgumentTypeError(str(exc)) from exc
def _parse_positive_float(value: str) -> float:
try:
parsed = float(value)
return _validate_positive_float(parsed, "value")
except ValueError as exc:
raise argparse.ArgumentTypeError(str(exc)) from exc
def _initialize_state(rows: list[ImuRow], init_seconds: int) -> ekf.ImuEkfState:
if not rows:
raise ValueError("at least one IMU row is required for initialization")
@@ -575,12 +696,12 @@ def _fixed_yaw_bias_z_dps(rows: list[ImuRow]) -> float:
return sum(row.gyro_dps[2] for row in rows) / len(rows)
def _row_with_fixed_yaw_bias(row: ImuRow, fixed_yaw_bias_z_dps: float) -> ImuRow:
def _row_with_yaw_bias(row: ImuRow, yaw_bias_z_dps: float) -> ImuRow:
return ImuRow(
sensor_uptime_s=row.sensor_uptime_s,
temp_c=row.temp_c,
acc_g=row.acc_g,
gyro_dps=(row.gyro_dps[0], row.gyro_dps[1], row.gyro_dps[2] - fixed_yaw_bias_z_dps),
gyro_dps=(row.gyro_dps[0], row.gyro_dps[1], row.gyro_dps[2] - yaw_bias_z_dps),
)
@@ -594,6 +715,8 @@ def _sample_for_html(row: dict[str, float]) -> dict[str, float]:
"gyro_bias_y_dps",
"gyro_bias_z_dps",
"fixed_yaw_bias_z_dps",
"active_yaw_bias_z_dps",
"is_static",
"acc_residual_norm",
)
sample = {key: float(row[key]) for key in keys}