feat: add stationary yaw self-calibration

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")

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))
+            is_static = False
-            if fixed_yaw_bias_enabled:
+            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
+            nonlocal state, static_state, init_buffer
-            state = _initialize_state(init_buffer, init_seconds)
+            if active_yaw_bias_z_dps is None:
-            if fixed_yaw_bias_enabled:
+                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:
+        def process_row_with_yaw_bias(row: ImuRow) -> None:
-            if fixed_yaw_bias_z_dps is None:
+            if active_yaw_bias_z_dps is None:
-                raise ValueError("fixed yaw bias is not initialized")
+                raise ValueError("active yaw bias is not initialized")
-            corrected_row = _row_with_fixed_yaw_bias(row, fixed_yaw_bias_z_dps)
             if state is None:
-                init_buffer.append(corrected_row)
+                init_buffer.append(row)
-                if init_seconds == 0 or corrected_row.sensor_uptime_s - init_buffer[0].sensor_uptime_s >= init_seconds:
+                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}

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()

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__":