feat(fusion): fuse Doppler velocity in ESKF

2026-07-25 13:11:33 +03:00
parent 2da5fb7153
commit 971be036a0
19 changed files with 647 additions and 106 deletions
@@ -27,6 +27,7 @@ typedef struct {
 	double gravity_mps2;
 	double max_propagation_step_s;
 	double position_nis_gate;
+	double velocity_nis_gate;
 	double covariance_diagonal_floor;
 } eskf_config_t;

@@ -40,9 +41,12 @@ typedef struct {
 	double last_linear_accel_nav_mps2[ESKF_VECTOR_DIM];
 	double last_unbiased_gyro_rps[ESKF_VECTOR_DIM];
 	double last_position_nis;
+	double last_velocity_nis;
 	uint64_t propagation_count;
 	uint64_t position_update_accepted_count;
 	uint64_t position_update_rejected_count;
+	uint64_t velocity_update_accepted_count;
+	uint64_t velocity_update_rejected_count;
 	bool initialized;
 } eskf_t;

@@ -84,6 +88,13 @@ eskf_update_result_t eskf_position_update(
 	const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM],
 	double* nis_out);

+eskf_update_result_t eskf_velocity_update(
+	eskf_t* filter,
+	const eskf_config_t* config,
+	const double velocity_mps[ESKF_VECTOR_DIM],
+	const double covariance_mps2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM],
+	double* nis_out);
+
 /* Re-anchor position after a higher-level fusion timeout has established that
 * repeated, valid position observations are mutually coherent.  Velocity,
 * attitude and IMU biases are preserved; position cross-covariances are
@@ -80,6 +80,9 @@ typedef struct {
 	uint64_t position_pending_overflow;
 	uint64_t position_accepted;
 	uint64_t position_recovery_count;
+	uint64_t velocity_received;
+	uint64_t velocity_nis_rejected;
+	uint64_t velocity_accepted;
 	uint64_t replay_count;
 	uint64_t replayed_imu_count;
 	uint64_t zero_velocity_update_count;
@@ -104,7 +107,7 @@ typedef struct {
 	fusion_runtime_t checkpoint;
 	fusion_calibration_accumulator_t calibration;
 	fusion_imu_measurement_t imu_history[FUSION_IMU_HISTORY_CAPACITY];
-	fusion_position_measurement_t pending_positions[FUSION_POSITION_PENDING_CAPACITY];
+	fusion_navigation_measurement_t pending_positions[FUSION_POSITION_PENDING_CAPACITY];
 	fusion_position_measurement_t initial_position;
 	fusion_position_measurement_t position_recovery_candidate;
 	fusion_engine_diagnostics_t diagnostics;
@@ -133,6 +136,13 @@ bool fusion_engine_push_position(
 	const fusion_position_measurement_t* measurement,
 	eskf_update_result_t* update_result_out,
 	double* nis_out);
+bool fusion_engine_push_navigation(
+	fusion_engine_t* engine,
+	const fusion_navigation_measurement_t* measurement,
+	eskf_update_result_t* position_update_result_out,
+	double* position_nis_out,
+	eskf_update_result_t* velocity_update_result_out,
+	double* velocity_nis_out);

 bool fusion_engine_get_runtime(const fusion_engine_t* engine,
 	                           fusion_runtime_t* runtime_out);
@@ -9,6 +9,9 @@ typedef struct {
 	double minimum_horizontal_std_m;
 	double minimum_vertical_std_m;
 	double maximum_position_std_m;
+	double minimum_horizontal_velocity_std_mps;
+	double minimum_vertical_velocity_std_mps;
+	double maximum_velocity_std_mps;
 } fusion_gps_adapter_config_t;

 typedef struct {
@@ -27,6 +30,10 @@ bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter,
 bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter,
 	                            const fusion_gps_measurement_t* gps,
 	                            fusion_position_measurement_t* position_out);
+bool fusion_gps_adapter_convert_navigation(
+	fusion_gps_adapter_t* adapter,
+	const fusion_gps_measurement_t* gps,
+	fusion_navigation_measurement_t* navigation_out);
 bool fusion_gps_adapter_local_to_geodetic(
 	const fusion_gps_adapter_t* adapter,
 	const double position_enu_m[3],
@@ -22,6 +22,7 @@ typedef struct {
 	double residual_rms_m;
 	double chi_square_ratio;
 	double velocity_ecef_mps[3];
+	double velocity_covariance_ecef_mps2[3][3];
 	double speed_mps;
 	double velocity_residual_rms_mps;
 	unsigned int used_satellites;
@@ -49,4 +50,16 @@ typedef struct {
 	bool source_valid;
 } fusion_position_measurement_t;

+/*
+ * Position and Doppler velocity from one GNSS solve. Keeping them in one
+ * message makes the fixed-lag coordinator apply both observations at exactly
+ * the captured GPS epoch before replaying newer IMU samples.
+ */
+typedef struct {
+	fusion_position_measurement_t position;
+	double velocity_enu_mps[3];
+	double velocity_covariance_enu_mps2[9];
+	bool velocity_valid;
+} fusion_navigation_measurement_t;
+
 #endif
@@ -20,10 +20,13 @@ typedef struct {
 	double position_std_m[3];
 	double speed_mps;
 	double last_position_nis;
+	double last_velocity_nis;
 	fusion_motion_state_t motion_state;
 	uint64_t gps_accepted;
 	uint64_t gps_nis_rejected;
 	uint64_t gps_position_recoveries;
+	uint64_t gps_velocity_accepted;
+	uint64_t gps_velocity_nis_rejected;
 	uint64_t zero_update_count;
 	uint64_t replay_count;
 	uint64_t stale_gps_count;
@@ -54,8 +57,9 @@ void navigation_fusion_set_platform_landed(
 * ESKF observation and is never rate-selected in this layer. */
 bool navigation_fusion_push_imu(const imu_sample_t* measurement);

-/* Observation input: convert a valid LLH fix to local ENU, apply it at the
- * captured GPS epoch, then replay newer IMU history to current time. */
+/* Observation input: convert a valid LLH fix and optional Doppler velocity to
+ * local ENU, apply both at the captured GPS epoch, then replay newer IMU
+ * history to current time. */
 bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement);

 /* local_valid can exist without GPS. global_valid/valid require a GPS origin. */
@@ -10,6 +10,7 @@
 			double ecef_m[3];
 			double clock_bias_m;
 			double velocity_ecef_mps[3];
+			double velocity_covariance_ecef_mps2[3][3];
 			double clock_drift_mps;
 			double speed_mps;
 			double velocity_residual_rms_mps;
@@ -55,6 +55,7 @@ bool gps_tracking_velocity_wls(
 	const gps_tracking_measurement_t measurements[],
 	unsigned int count,
 	double receiver_velocity_ecef_mps_out[3],
+	double receiver_velocity_covariance_ecef_mps2_out[3][3],
 	double* receiver_clock_drift_mps_out,
 	double* residual_rms_mps_out);