From 478fd706b34c096cf6cc159bd0e7596a4ae781b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=88=98=E6=B3=BD=E7=BE=A4?= Date: Thu, 18 Jun 2026 16:32:28 +0800 Subject: [PATCH] feat: add stationary yaw self-calibration --- scripts/imu_static_calibrator.py | 180 ++++++++++++++++++++++++++++ scripts/run_imu_ekf.py | 167 ++++++++++++++++++++++---- tests/test_imu_static_calibrator.py | 113 +++++++++++++++++ tests/test_run_imu_ekf.py | 129 +++++++++++++++++++- 4 files changed, 564 insertions(+), 25 deletions(-) create mode 100644 scripts/imu_static_calibrator.py create mode 100644 tests/test_imu_static_calibrator.py diff --git a/scripts/imu_static_calibrator.py b/scripts/imu_static_calibrator.py new file mode 100644 index 0000000..7291a28 --- /dev/null +++ b/scripts/imu_static_calibrator.py @@ -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") diff --git a/scripts/run_imu_ekf.py b/scripts/run_imu_ekf.py index 405fcca..4c7105a 100644 --- a/scripts/run_imu_ekf.py +++ b/scripts/run_imu_ekf.py @@ -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} diff --git a/tests/test_imu_static_calibrator.py b/tests/test_imu_static_calibrator.py new file mode 100644 index 0000000..7edf83c --- /dev/null +++ b/tests/test_imu_static_calibrator.py @@ -0,0 +1,113 @@ +import unittest + +import numpy as np + +from scripts import imu_static_calibrator as calibrator + + +class ImuStaticCalibratorTests(unittest.TestCase): + def _state(self, enter_seconds=0.01): + config = calibrator.StaticCorrectionConfig(enter_seconds=enter_seconds) + return calibrator.initialize(config, initial_yaw_bias_z_dps=0.0) + + def test_stationary_samples_enter_static_and_estimate_z_bias(self): + state = self._state() + + for _ in range(7): + is_static = calibrator.step( + state, + 0.002, + np.array([0.0, 0.0, 1.1]), + np.array([0.0, 0.0, 0.12]), + np.zeros(2), + ) + + self.assertTrue(is_static) + self.assertAlmostEqual(state.active_yaw_bias_z_dps, 0.12, places=9) + + def test_rotation_above_threshold_never_enters_static(self): + state = self._state() + + for _ in range(20): + is_static = calibrator.step( + state, + 0.002, + np.array([0.0, 0.0, 1.0]), + np.array([0.0, 0.0, 1.0]), + np.zeros(2), + ) + + self.assertFalse(is_static) + self.assertEqual(state.mode, calibrator.MOVING) + + def test_acceleration_change_restarts_candidate_window(self): + state = self._state() + for _ in range(4): + calibrator.step( + state, + 0.002, + np.array([0.0, 0.0, 1.0]), + np.zeros(3), + np.zeros(2), + ) + + calibrator.step( + state, + 0.002, + np.array([0.05, 0.0, 1.0]), + np.zeros(3), + np.zeros(2), + ) + + self.assertEqual(state.mode, calibrator.CANDIDATE) + self.assertEqual(state.candidate_count, 1) + self.assertEqual(state.candidate_elapsed_s, 0.0) + + def test_static_period_updates_running_z_bias_mean(self): + state = self._state(enter_seconds=0.004) + for value in [0.1, 0.1, 0.1]: + calibrator.step( + state, + 0.002, + np.array([0.0, 0.0, 1.0]), + np.array([0.0, 0.0, value]), + np.zeros(2), + ) + + calibrator.step( + state, + 0.002, + np.array([0.0, 0.0, 1.0]), + np.array([0.0, 0.0, 0.2]), + np.zeros(2), + ) + + self.assertAlmostEqual(state.active_yaw_bias_z_dps, 0.125, places=9) + + def test_motion_exits_static_and_keeps_last_bias(self): + state = self._state(enter_seconds=0.004) + for _ in range(3): + calibrator.step( + state, + 0.002, + np.array([0.0, 0.0, 1.0]), + np.array([0.0, 0.0, 0.1]), + np.zeros(2), + ) + bias_before_motion = state.active_yaw_bias_z_dps + + is_static = calibrator.step( + state, + 0.002, + np.array([0.0, 0.0, 1.0]), + np.array([0.0, 0.0, 1.0]), + np.zeros(2), + ) + + self.assertFalse(is_static) + self.assertEqual(state.mode, calibrator.MOVING) + self.assertEqual(state.active_yaw_bias_z_dps, bias_before_motion) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_run_imu_ekf.py b/tests/test_run_imu_ekf.py index 1811ec7..51043b1 100644 --- a/tests/test_run_imu_ekf.py +++ b/tests/test_run_imu_ekf.py @@ -72,8 +72,61 @@ class RunImuEkfTests(unittest.TestCase): self.assertIn("segment_id", rows[0]) self.assertIn("gyro_bias_z_dps", rows[0]) self.assertIn("fixed_yaw_bias_z_dps", rows[0]) + self.assertIn("active_yaw_bias_z_dps", rows[0]) + self.assertIn("is_static", rows[0]) self.assertEqual(result.input_rows, 20) + def test_static_correction_updates_active_bias_and_freezes_relative_yaw(self): + with tempfile.TemporaryDirectory() as tmp: + input_path = Path(tmp) / "imu_sample.csv" + output_dir = Path(tmp) / "out" + self._write_sample_csv( + input_path, + [(index * 0.002, 0.12, 1.0) for index in range(20)], + ) + + result = run_imu_ekf.process_file( + input_path, + output_dir, + init_seconds=0, + yaw_bias_seconds=0, + static_correction_seconds=0.01, + ) + + with result.output_csv.open("r", encoding="utf-8", newline="") as handle: + rows = list(csv.DictReader(handle)) + + first_static_index = next(index for index, row in enumerate(rows) if row["is_static"] == "1") + frozen_yaw = float(rows[first_static_index]["relative_yaw_deg"]) + self.assertTrue(all(row["is_static"] == "1" for row in rows[first_static_index:])) + self.assertTrue( + all(abs(float(row["relative_yaw_deg"]) - frozen_yaw) < 1e-9 for row in rows[first_static_index:]) + ) + self.assertAlmostEqual(float(rows[-1]["active_yaw_bias_z_dps"]), 0.12, delta=1e-9) + + def test_rotation_above_static_threshold_does_not_freeze_yaw(self): + with tempfile.TemporaryDirectory() as tmp: + input_path = Path(tmp) / "imu_sample.csv" + output_dir = Path(tmp) / "out" + self._write_sample_csv( + input_path, + [(index * 0.002, 1.0, 1.0) for index in range(100)], + ) + + result = run_imu_ekf.process_file( + input_path, + output_dir, + init_seconds=0, + yaw_bias_seconds=0, + static_correction_seconds=0.01, + ) + + with result.output_csv.open("r", encoding="utf-8", newline="") as handle: + rows = list(csv.DictReader(handle)) + + self.assertTrue(all(row["is_static"] == "0" for row in rows)) + self.assertGreater(float(rows[-1]["relative_yaw_deg"]), 0.1) + def test_default_fixed_yaw_bias_keeps_constant_z_bias_from_drifting(self): with tempfile.TemporaryDirectory() as tmp: input_path = Path(tmp) / "imu_sample.csv" @@ -236,13 +289,19 @@ class RunImuEkfTests(unittest.TestCase): self.assertEqual(float(rows[0]["gyro_bias_z_dps"]), 0.0) self.assertEqual(float(rows[1]["gyro_bias_z_dps"]), 0.0) - def test_yaw_bias_seconds_zero_preserves_core_z_bias_initialization(self): + def test_disabling_all_wrapper_yaw_bias_preserves_core_z_bias(self): with tempfile.TemporaryDirectory() as tmp: input_path = Path(tmp) / "imu_sample.csv" output_dir = Path(tmp) / "out" self._write_sample_csv(input_path, [(0.0, 7.5, 1.0), (0.5, 7.5, 1.0), (1.0, 7.5, 1.0)]) - result = run_imu_ekf.process_file(input_path, output_dir, init_seconds=1, yaw_bias_seconds=0) + result = run_imu_ekf.process_file( + input_path, + output_dir, + init_seconds=1, + yaw_bias_seconds=0, + static_correction_seconds=0, + ) with result.output_csv.open("r", encoding="utf-8", newline="") as handle: rows = list(csv.DictReader(handle)) @@ -279,6 +338,28 @@ class RunImuEkfTests(unittest.TestCase): with self.assertRaises(SystemExit): run_imu_ekf.main(["--yaw-bias-seconds", "1.5"]) + def test_static_correction_configuration_rejects_invalid_values(self): + with tempfile.TemporaryDirectory() as tmp: + input_path = Path(tmp) / "imu_sample.csv" + self._write_sample_csv(input_path, [(0.0, 0.0, 1.0)]) + + with self.assertRaisesRegex(ValueError, "static_correction_seconds.*0.*10"): + run_imu_ekf.process_file( + input_path, + Path(tmp) / "out", + static_correction_seconds=10.1, + ) + + with self.assertRaisesRegex(ValueError, "static_gyro_threshold_dps.*positive"): + run_imu_ekf.process_file( + input_path, + Path(tmp) / "out", + static_gyro_threshold_dps=0.0, + ) + + with self.assertRaises(SystemExit): + run_imu_ekf.main(["--static-correction-seconds", "-1"]) + def test_relative_yaw_is_unwrapped_in_csv_and_html_samples(self): with tempfile.TemporaryDirectory() as tmp: input_path = Path(tmp) / "imu_sample.csv" @@ -312,6 +393,40 @@ class RunImuEkfTests(unittest.TestCase): self.assertEqual([row["segment_id"] for row in rows], ["0", "0", "1", "1"]) self.assertEqual(float(rows[2]["dt_s"]), 0.0) + def test_static_correction_restarts_with_device_segment(self): + with tempfile.TemporaryDirectory() as tmp: + input_path = Path(tmp) / "imu_sample.csv" + output_dir = Path(tmp) / "out" + self._write_sample_csv( + input_path, + [ + (9.996, 0.1, 1.0), + (9.998, 0.1, 1.0), + (10.0, 0.1, 1.0), + (0.002, 0.2, 1.0), + (0.004, 0.2, 1.0), + (0.006, 0.2, 1.0), + ], + ) + + result = run_imu_ekf.process_file( + input_path, + output_dir, + init_seconds=0, + yaw_bias_seconds=0, + static_correction_seconds=0.004, + ) + + with result.output_csv.open("r", encoding="utf-8", newline="") as handle: + rows = list(csv.DictReader(handle)) + + segment0 = [row for row in rows if row["segment_id"] == "0"] + segment1 = [row for row in rows if row["segment_id"] == "1"] + self.assertEqual([row["is_static"] for row in segment0], ["0", "0", "1"]) + self.assertEqual([row["is_static"] for row in segment1], ["0", "0", "1"]) + self.assertAlmostEqual(float(segment0[-1]["active_yaw_bias_z_dps"]), 0.1, delta=1e-9) + self.assertAlmostEqual(float(segment1[-1]["active_yaw_bias_z_dps"]), 0.2, delta=1e-9) + def test_process_file_flushes_short_uninitialized_segment_before_restart(self): with tempfile.TemporaryDirectory() as tmp: input_path = Path(tmp) / "imu_sample.csv" @@ -442,6 +557,8 @@ class RunImuEkfTests(unittest.TestCase): "gyro_bias_y_dps": 0.0, "gyro_bias_z_dps": 0.0, "fixed_yaw_bias_z_dps": 0.0, + "active_yaw_bias_z_dps": 0.0, + "is_static": 1.0, "acc_residual_norm": 0.0, } ], @@ -462,8 +579,10 @@ class RunImuEkfTests(unittest.TestCase): self.assertIn("canvas.className = 'chart'", html) self.assertIn("requestAnimationFrame", html) self.assertIn("laneHeight", html) + self.assertIn("drawStaticRanges", html) + self.assertIn("active yaw bias z", html) - def test_html_sample_includes_fixed_yaw_bias(self): + def test_html_sample_includes_yaw_bias_and_static_state(self): sample = run_imu_ekf._sample_for_html( { "sensor_uptime_s": 0.0, @@ -474,11 +593,15 @@ class RunImuEkfTests(unittest.TestCase): "gyro_bias_y_dps": 0.0, "gyro_bias_z_dps": 0.0, "fixed_yaw_bias_z_dps": 1.25, + "active_yaw_bias_z_dps": 1.5, + "is_static": 1, "acc_residual_norm": 0.0, } ) self.assertEqual(sample["fixed_yaw_bias_z_dps"], 1.25) + self.assertEqual(sample["active_yaw_bias_z_dps"], 1.5) + self.assertEqual(sample["is_static"], 1.0) if __name__ == "__main__":