diff --git a/scripts/run_imu_ekf.py b/scripts/run_imu_ekf.py
index f92de97..405fcca 100644
--- a/scripts/run_imu_ekf.py
+++ b/scripts/run_imu_ekf.py
@@ -61,9 +61,11 @@ def process_file(
input_csv: Path,
output_dir: Path,
init_seconds: int = 3,
+ yaw_bias_seconds: int = 60,
max_points: int = 2500,
) -> EkfFileResult:
init_seconds = _validate_init_seconds(init_seconds)
+ yaw_bias_seconds = _validate_yaw_bias_seconds(yaw_bias_seconds)
input_csv = Path(input_csv)
output_dir = Path(output_dir)
output_dir.mkdir(parents=True, exist_ok=True)
@@ -90,6 +92,7 @@ def process_file(
"gyro_bias_x_dps",
"gyro_bias_y_dps",
"gyro_bias_z_dps",
+ "fixed_yaw_bias_z_dps",
"acc_residual_norm",
"dt_s",
"acc_update_used",
@@ -99,16 +102,22 @@ def process_file(
segment_id = 0
state: ekf.ImuEkfState | 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
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, init_buffer, previous_output_time, relative_yaw_deg, previous_yaw_deg
+ nonlocal state, yaw_bias_buffer, init_buffer, fixed_yaw_bias_z_dps
+ nonlocal previous_output_time, relative_yaw_deg, previous_yaw_deg
state = None
+ yaw_bias_buffer = []
init_buffer = []
+ fixed_yaw_bias_z_dps = 0.0 if yaw_bias_seconds == 0 else None
previous_output_time = None
relative_yaw_deg = 0.0
previous_yaw_deg = None
@@ -117,10 +126,16 @@ def process_file(
nonlocal input_rows, previous_output_time, relative_yaw_deg, previous_yaw_deg
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:
+ 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:
+ 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
@@ -143,6 +158,7 @@ def process_file(
"gyro_bias_x_dps": bias_dps[0],
"gyro_bias_y_dps": bias_dps[1],
"gyro_bias_z_dps": bias_dps[2],
+ "fixed_yaw_bias_z_dps": fixed_yaw_bias_z_dps,
"acc_residual_norm": residual_norm,
"dt_s": dt_s,
"acc_update_used": int(used_update),
@@ -155,17 +171,47 @@ def process_file(
def initialize_and_write_buffer() -> None:
nonlocal state, init_buffer
state = _initialize_state(init_buffer, init_seconds)
+ if fixed_yaw_bias_enabled:
+ state.gyro_bias_rad_s[2] = 0.0
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)
+ 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:
+ initialize_and_write_buffer()
+ else:
+ write_row(corrected_row)
+
+ def initialize_fixed_yaw_bias_from_buffer() -> None:
+ nonlocal yaw_bias_buffer, fixed_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)
+ buffered_rows = yaw_bias_buffer
+ yaw_bias_buffer = []
+ for buffered_row in buffered_rows:
+ process_row_with_fixed_yaw_bias(buffered_row)
+
+ def flush_segment() -> None:
+ if fixed_yaw_bias_z_dps is None and yaw_bias_buffer:
+ initialize_fixed_yaw_bias_from_buffer()
+ if state is None and init_buffer:
+ initialize_and_write_buffer()
+
for data_row_index, row in enumerate(iter_imu_rows(input_csv), start=1):
rows_seen += 1
if previous_input_time is not None and row.sensor_uptime_s < previous_input_time:
if _is_device_restart(previous_input_time, row.sensor_uptime_s):
- if state is None and init_buffer:
- initialize_and_write_buffer()
+ flush_segment()
segment_id += 1
reset_segment()
else:
@@ -174,19 +220,20 @@ def process_file(
f"previous {previous_input_time}, current {row.sensor_uptime_s}"
)
- if state is None:
- 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()
+ 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)
+ else:
+ yaw_bias_buffer.append(row)
else:
- write_row(row)
+ process_row_with_fixed_yaw_bias(row)
previous_input_time = row.sensor_uptime_s
if rows_seen == 0:
raise ValueError(f"{input_csv} has no IMU rows")
- if state is None and init_buffer:
- initialize_and_write_buffer()
+ flush_segment()
return EkfFileResult(
input_csv=input_csv,
@@ -227,7 +274,7 @@ def write_html_report(results: list[EkfFileResult], html_path: Path, max_points:
h2 {{ font-size: 18px; margin: 20px 0 8px; }}
.meta {{ color: #5f6368; font-size: 13px; }}
.file {{ margin-bottom: 26px; }}
- canvas {{ width: 100%; height: 260px; display: block; background: #ffffff; border: 1px solid #d8d8d0; }}
+ canvas.chart {{ width: 100%; height: 390px; display: block; background: #ffffff; border: 1px solid #b8bec5; }}
table {{ border-collapse: collapse; margin: 10px 0; font-size: 13px; }}
td {{ border: 1px solid #d8d8d0; padding: 4px 8px; }}
@@ -241,37 +288,83 @@ def write_html_report(results: list[EkfFileResult], html_path: Path, max_points:
@@ -328,6 +428,7 @@ def main(argv: list[str] | None = None) -> int:
parser.add_argument("csv_files", nargs="*", type=Path, help="CSV files. Defaults to imu_*.csv.")
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("--max-points", type=int, default=2500)
args = parser.parse_args(argv)
@@ -336,7 +437,13 @@ def main(argv: list[str] | None = None) -> int:
raise SystemExit("No CSV files found.")
results = [
- process_file(path, args.output_dir, init_seconds=args.init_seconds, max_points=args.max_points)
+ process_file(
+ path,
+ args.output_dir,
+ init_seconds=args.init_seconds,
+ yaw_bias_seconds=args.yaw_bias_seconds,
+ max_points=args.max_points,
+ )
for path in csv_files
]
write_html_report(results, args.output_dir / "ekf_viewer.html", max_points=args.max_points)
@@ -414,6 +521,12 @@ def _validate_init_seconds(value) -> int:
return value
+def _validate_yaw_bias_seconds(value) -> int:
+ if type(value) is not int or value < 0:
+ raise ValueError("yaw_bias_seconds must be a non-negative integer")
+ return value
+
+
def _parse_init_seconds(value: str) -> int:
try:
parsed = int(value)
@@ -427,6 +540,19 @@ def _parse_init_seconds(value: str) -> int:
raise argparse.ArgumentTypeError(str(exc)) from exc
+def _parse_yaw_bias_seconds(value: str) -> int:
+ try:
+ parsed = int(value)
+ except ValueError as exc:
+ raise argparse.ArgumentTypeError("yaw_bias_seconds must be a non-negative integer") from exc
+ if str(parsed) != value:
+ raise argparse.ArgumentTypeError("yaw_bias_seconds must be a non-negative integer")
+ try:
+ return _validate_yaw_bias_seconds(parsed)
+ 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")
@@ -445,6 +571,19 @@ def _gyro_rad_s(row: ImuRow) -> np.ndarray:
return np.radians(np.array(row.gyro_dps, dtype=float))
+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:
+ 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),
+ )
+
+
def _sample_for_html(row: dict[str, float]) -> dict[str, float]:
keys = (
"sensor_uptime_s",
@@ -454,6 +593,7 @@ def _sample_for_html(row: dict[str, float]) -> dict[str, float]:
"gyro_bias_x_dps",
"gyro_bias_y_dps",
"gyro_bias_z_dps",
+ "fixed_yaw_bias_z_dps",
"acc_residual_norm",
)
sample = {key: float(row[key]) for key in keys}
diff --git a/tests/test_run_imu_ekf.py b/tests/test_run_imu_ekf.py
index 4d8068b..1811ec7 100644
--- a/tests/test_run_imu_ekf.py
+++ b/tests/test_run_imu_ekf.py
@@ -71,8 +71,95 @@ class RunImuEkfTests(unittest.TestCase):
self.assertIn("relative_yaw_deg", rows[0])
self.assertIn("segment_id", rows[0])
self.assertIn("gyro_bias_z_dps", rows[0])
+ self.assertIn("fixed_yaw_bias_z_dps", rows[0])
self.assertEqual(result.input_rows, 20)
+ 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"
+ output_dir = Path(tmp) / "out"
+ self._write_sample_csv(input_path, [(float(index), 5.0, 1.0) for index in range(65)])
+
+ result = run_imu_ekf.process_file(input_path, output_dir, init_seconds=0)
+
+ with result.output_csv.open("r", encoding="utf-8", newline="") as handle:
+ rows = list(csv.DictReader(handle))
+
+ self.assertLess(abs(float(rows[-1]["relative_yaw_deg"])), 0.1)
+ self.assertAlmostEqual(float(rows[-1]["fixed_yaw_bias_z_dps"]), 5.0, delta=1e-9)
+ self.assertAlmostEqual(float(rows[-1]["gyro_bias_z_dps"]), 0.0, delta=1e-9)
+
+ def test_yaw_bias_seconds_zero_preserves_z_integrated_drift(self):
+ with tempfile.TemporaryDirectory() as tmp:
+ input_path = Path(tmp) / "imu_sample.csv"
+ output_dir = Path(tmp) / "out"
+ self._write_sample_csv(input_path, [(float(index), 5.0, 1.0) for index in range(5)])
+
+ result = run_imu_ekf.process_file(input_path, output_dir, init_seconds=0, yaw_bias_seconds=0)
+
+ with result.output_csv.open("r", encoding="utf-8", newline="") as handle:
+ rows = list(csv.DictReader(handle))
+
+ self.assertGreater(float(rows[-1]["relative_yaw_deg"]), 15.0)
+ self.assertTrue(all(float(row["fixed_yaw_bias_z_dps"]) == 0.0 for row in rows))
+
+ def test_fixed_yaw_bias_uses_window_mean_in_output(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, 2.0, 1.0), (0.5, 4.0, 1.0), (1.0, 100.0, 1.0), (1.5, 100.0, 1.0)],
+ )
+
+ result = run_imu_ekf.process_file(input_path, output_dir, init_seconds=0, yaw_bias_seconds=1)
+
+ with result.output_csv.open("r", encoding="utf-8", newline="") as handle:
+ rows = list(csv.DictReader(handle))
+
+ self.assertEqual(len(rows), 4)
+ self.assertTrue(all(float(row["fixed_yaw_bias_z_dps"]) == 3.0 for row in rows))
+
+ def test_short_file_uses_available_rows_for_fixed_yaw_bias_and_flushes_all_rows(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, 2.0, 1.0), (0.5, 4.0, 1.0), (1.0, 6.0, 1.0)])
+
+ result = run_imu_ekf.process_file(input_path, output_dir, init_seconds=0)
+
+ with result.output_csv.open("r", encoding="utf-8", newline="") as handle:
+ rows = list(csv.DictReader(handle))
+
+ self.assertEqual(len(rows), 3)
+ self.assertTrue(all(float(row["fixed_yaw_bias_z_dps"]) == 4.0 for row in rows))
+
+ def test_fixed_yaw_bias_restarts_per_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,
+ [
+ (0.0, 2.0, 1.0),
+ (0.5, 4.0, 1.0),
+ (1.1, 100.0, 1.0),
+ (0.002, 8.0, 1.0),
+ (0.502, 10.0, 1.0),
+ (1.002, 100.0, 1.0),
+ ],
+ )
+
+ result = run_imu_ekf.process_file(input_path, output_dir, init_seconds=0, yaw_bias_seconds=1)
+
+ with result.output_csv.open("r", encoding="utf-8", newline="") as handle:
+ rows = list(csv.DictReader(handle))
+
+ segment0_bias = {float(row["fixed_yaw_bias_z_dps"]) for row in rows if row["segment_id"] == "0"}
+ segment1_bias = {float(row["fixed_yaw_bias_z_dps"]) for row in rows if row["segment_id"] == "1"}
+ self.assertEqual(segment0_bias, {3.0})
+ self.assertEqual(segment1_bias, {9.0})
+
def test_process_file_reads_imu_rows_once(self):
with tempfile.TemporaryDirectory() as tmp:
input_path = Path(tmp) / "imu_sample.csv"
@@ -102,13 +189,46 @@ class RunImuEkfTests(unittest.TestCase):
self.assertEqual(call_count, 1)
self.assertEqual(result.input_rows, 3)
+ def test_process_file_reads_imu_rows_once_with_short_fixed_yaw_bias_window(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, 0.0, 1.0)])
+ rows = [
+ run_imu_ekf.ImuRow(0.0, 28.0, (0.0, 0.0, 1.0), (0.0, 0.0, 2.0)),
+ run_imu_ekf.ImuRow(0.5, 28.0, (0.0, 0.0, 1.0), (0.0, 0.0, 4.0)),
+ run_imu_ekf.ImuRow(1.0, 28.0, (0.0, 0.0, 1.0), (0.0, 0.0, 6.0)),
+ ]
+ call_count = 0
+ original_iter = run_imu_ekf.iter_imu_rows
+
+ def single_use_iter(path):
+ nonlocal call_count
+ call_count += 1
+ if call_count > 1:
+ raise AssertionError("process_file must stream iter_imu_rows once")
+ return iter(rows)
+
+ run_imu_ekf.iter_imu_rows = single_use_iter
+ try:
+ result = run_imu_ekf.process_file(input_path, output_dir, init_seconds=0)
+ finally:
+ run_imu_ekf.iter_imu_rows = original_iter
+
+ with result.output_csv.open("r", encoding="utf-8", newline="") as handle:
+ output_rows = list(csv.DictReader(handle))
+
+ self.assertEqual(call_count, 1)
+ self.assertEqual(result.input_rows, 3)
+ self.assertTrue(all(float(row["fixed_yaw_bias_z_dps"]) == 4.0 for row in output_rows))
+
def test_init_seconds_zero_disables_gyro_bias_initialization(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.1, 7.5, 1.0)])
- result = run_imu_ekf.process_file(input_path, output_dir, init_seconds=0)
+ result = run_imu_ekf.process_file(input_path, output_dir, init_seconds=0, yaw_bias_seconds=0)
with result.output_csv.open("r", encoding="utf-8", newline="") as handle:
rows = list(csv.DictReader(handle))
@@ -116,6 +236,21 @@ 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):
+ 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)
+
+ with result.output_csv.open("r", encoding="utf-8", newline="") as handle:
+ rows = list(csv.DictReader(handle))
+
+ self.assertEqual(float(rows[0]["fixed_yaw_bias_z_dps"]), 0.0)
+ self.assertAlmostEqual(float(rows[0]["gyro_bias_z_dps"]), 7.5, delta=0.01)
+ self.assertAlmostEqual(float(rows[-1]["gyro_bias_z_dps"]), 7.5, delta=0.01)
+
def test_init_seconds_rejects_values_outside_integer_range(self):
with tempfile.TemporaryDirectory() as tmp:
input_path = Path(tmp) / "imu_sample.csv"
@@ -127,13 +262,30 @@ class RunImuEkfTests(unittest.TestCase):
with self.assertRaisesRegex(ValueError, "init_seconds.*0.*10.*integer"):
run_imu_ekf.process_file(input_path, Path(tmp) / "out", init_seconds=1.5)
+ def test_yaw_bias_seconds_rejects_negative_and_non_integer_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, "yaw_bias_seconds.*non-negative integer"):
+ run_imu_ekf.process_file(input_path, Path(tmp) / "out", yaw_bias_seconds=-1)
+
+ with self.assertRaisesRegex(ValueError, "yaw_bias_seconds.*non-negative integer"):
+ run_imu_ekf.process_file(input_path, Path(tmp) / "out", yaw_bias_seconds=1.5)
+
+ with self.assertRaises(SystemExit):
+ run_imu_ekf.main(["--yaw-bias-seconds", "-1"])
+
+ with self.assertRaises(SystemExit):
+ run_imu_ekf.main(["--yaw-bias-seconds", "1.5"])
+
def test_relative_yaw_is_unwrapped_in_csv_and_html_samples(self):
with tempfile.TemporaryDirectory() as tmp:
input_path = Path(tmp) / "imu_sample.csv"
output_dir = Path(tmp) / "out"
self._write_sample_csv(input_path, [(float(index), 100.0, 1.0) for index in range(5)])
- result = run_imu_ekf.process_file(input_path, output_dir, init_seconds=0)
+ result = run_imu_ekf.process_file(input_path, output_dir, init_seconds=0, yaw_bias_seconds=0)
with result.output_csv.open("r", encoding="utf-8", newline="") as handle:
rows = list(csv.DictReader(handle))
@@ -199,12 +351,14 @@ class RunImuEkfTests(unittest.TestCase):
with result.output_csv.open("r", encoding="utf-8", newline="") as handle:
rows = list(csv.DictReader(handle))
- segment0_bias = [float(row["gyro_bias_z_dps"]) for row in rows if row["segment_id"] == "0"]
- segment1_bias = [float(row["gyro_bias_z_dps"]) for row in rows if row["segment_id"] == "1"]
+ segment0_bias = [float(row["fixed_yaw_bias_z_dps"]) for row in rows if row["segment_id"] == "0"]
+ segment1_bias = [float(row["fixed_yaw_bias_z_dps"]) for row in rows if row["segment_id"] == "1"]
+ core_z_bias = [float(row["gyro_bias_z_dps"]) for row in rows]
self.assertTrue(segment0_bias)
self.assertTrue(segment1_bias)
self.assertTrue(all(abs(value) < 0.01 for value in segment0_bias))
self.assertAlmostEqual(segment1_bias[-1], 20.0, delta=0.01)
+ self.assertTrue(all(value == 0.0 for value in core_z_bias))
def test_process_file_uses_restart_initialization_window_not_single_row(self):
with tempfile.TemporaryDirectory() as tmp:
@@ -227,8 +381,10 @@ class RunImuEkfTests(unittest.TestCase):
with result.output_csv.open("r", encoding="utf-8", newline="") as handle:
rows = list(csv.DictReader(handle))
- segment1_bias = [float(row["gyro_bias_z_dps"]) for row in rows if row["segment_id"] == "1"]
+ segment1_bias = [float(row["fixed_yaw_bias_z_dps"]) for row in rows if row["segment_id"] == "1"]
+ core_z_bias = [float(row["gyro_bias_z_dps"]) for row in rows]
self.assertAlmostEqual(segment1_bias[-1], 40.0 / 3.0, delta=0.01)
+ self.assertTrue(all(value == 0.0 for value in core_z_bias))
def test_process_file_rejects_timestamp_drop_not_near_zero(self):
with tempfile.TemporaryDirectory() as tmp:
@@ -285,6 +441,7 @@ class RunImuEkfTests(unittest.TestCase):
"gyro_bias_x_dps": 0.0,
"gyro_bias_y_dps": 0.0,
"gyro_bias_z_dps": 0.0,
+ "fixed_yaw_bias_z_dps": 0.0,
"acc_residual_norm": 0.0,
}
],
@@ -302,6 +459,26 @@ class RunImuEkfTests(unittest.TestCase):
self.assertNotIn("