Integrate asynchronous GPS IMU fusion

soc/software/fusion/tests/eskf_motion_test.c

@@ -3,11 +3,11 @@
 #include <stdio.h>
 #include <string.h>
 
-#include "eskf.h"
-#include "fusion_engine.h"
-#include "fusion_bus.h"
-#include "fusion_gps_adapter.h"
-#include "fusion_motion.h"
+#include "fusion/eskf.h"
+#include "fusion/fusion_engine.h"
+#include "fusion/fusion_bus.h"
+#include "fusion/fusion_gps_adapter.h"
+#include "fusion/fusion_motion.h"
 
 #define TEST_PI 3.14159265358979323846264338327950288
 
@@ -78,6 +78,47 @@ static void test_stationary_propagation(void)
 	CHECK(eskf_state_is_finite(&filter), "stationary state remains finite");
 }
 
+static void test_zero_update_primitives(void)
+{
+	eskf_t filter;
+	eskf_config_t config;
+	identity_filter(&filter, &config);
+	filter.velocity_mps[0] = 1.2;
+	filter.velocity_mps[1] = -0.8;
+	filter.velocity_mps[2] = 0.3;
+	CHECK(eskf_zero_velocity_update(&filter, 0.03),
+	      "direct ZUPT measurement update succeeds");
+	CHECK(eskf_speed_mps(&filter) < 0.005,
+	      "direct ZUPT drives a stale velocity estimate close to zero");
+
+	double stationary_gyro[3] = {0.0010, -0.0006, 0.0003};
+	double gyro_std[3] = {0.0002, 0.0002, 0.0002};
+	for(unsigned int update = 0u ; update < 20u ; update++)
+	{
+		CHECK(eskf_zero_angular_rate_update(
+			&filter, stationary_gyro, gyro_std),
+		      "direct zero-angular-rate update succeeds");
+	}
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		CHECK(fabs(filter.gyro_bias_rps[axis] - stationary_gyro[axis]) < 1.0e-5,
+		      "zero-angular-rate update converges gyro bias to stationary reading");
+	}
+	CHECK(eskf_state_is_finite(&filter),
+	      "ZUPT and zero-angular-rate updates preserve finite ESKF state");
+	for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		CHECK(filter.covariance[row][row] > 0.0,
+		      "zero updates preserve positive covariance diagonal");
+		for(unsigned int col = 0u ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			CHECK(fabs(filter.covariance[row][col] -
+			           filter.covariance[col][row]) < 1.0e-12,
+			      "zero updates preserve covariance symmetry");
+		}
+	}
+}
+
 static void test_gravity_alignment(void)
 {
 	eskf_t filter;
@@ -446,6 +487,7 @@ static void initialize_engine_for_replay(fusion_engine_t* engine)
 		imu.accel_mps2[2] = config.eskf.gravity_mps2;
 		imu.temperature_c = 25.0;
 		imu.sequence = (uint32_t)(tick / 10u);
+		imu.vehicle_at_rest = true;
 		CHECK(fusion_engine_push_imu(engine, &imu),
 		      "stationary calibration IMU accepted");
 	}
@@ -475,6 +517,7 @@ static void push_replay_imu(fusion_engine_t* engine, uint64_t tick)
 	imu.accel_mps2[2] = engine->config.eskf.gravity_mps2;
 	imu.temperature_c = 25.0;
 	imu.sequence = (uint32_t)(tick / 10u);
+	imu.vehicle_at_rest = fabs(imu.accel_mps2[0]) < 0.1;
 	CHECK(fusion_engine_push_imu(engine, &imu), "replay profile IMU accepted");
 }
 
@@ -703,6 +746,7 @@ static void test_history_rolls_checkpoint_forward(void)
 int main(void)
 {
 	test_stationary_propagation();
+	test_zero_update_primitives();
 	test_gravity_alignment();
 	test_position_nis_gate();
 	test_motion_transitions();