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

feat: add fixed yaw bias compensation

Browse Source
This commit is contained in:
zequn.liu
2026-06-18 14:41:08 +08:00
parent 97cd9a56c6
commit 93b9b91db7
2 changed files with 355 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/test_run_imu_ekf.py
+182 -5
View File
@@ -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("</script><script>alert", html)
self.assertNotIn("window.EKF_DATA", html)
self.assertNotIn("https://", html)
self.assertIn("canvas.className = 'chart'", html)
self.assertIn("requestAnimationFrame", html)
self.assertIn("laneHeight", html)
def test_html_sample_includes_fixed_yaw_bias(self):
sample = run_imu_ekf._sample_for_html(
{
"sensor_uptime_s": 0.0,
"roll_deg": 0.0,
"pitch_deg": 0.0,
"relative_yaw_deg": 0.0,
"gyro_bias_x_dps": 0.0,
"gyro_bias_y_dps": 0.0,
"gyro_bias_z_dps": 0.0,
"fixed_yaw_bias_z_dps": 1.25,
"acc_residual_norm": 0.0,
}
)
self.assertEqual(sample["fixed_yaw_bias_z_dps"], 1.25)
if __name__ == "__main__":