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

feat: add stationary yaw self-calibration

Browse Source
This commit is contained in:
zequn.liu
2026-06-18 16:32:28 +08:00
parent 93b9b91db7
commit 478fd706b3
4 changed files with 564 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/test_imu_static_calibrator.py
+113
View File
@@ -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
+126 -3
View File
@@ -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__":