Integrate asynchronous GPS IMU fusion

2026-07-16 23:49:43 +03:00
parent 38a96dd88f
commit 7cc3eb2a54
80 changed files with 3901 additions and 553 deletions
@@ -0,0 +1,106 @@
+#ifndef ESKF_H
+#define ESKF_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define ESKF_VECTOR_DIM 3u
+#define ESKF_QUATERNION_DIM 4u
+#define ESKF_ERROR_STATE_DIM 15u
+
+typedef enum {
+	ESKF_UPDATE_INVALID = 0,
+	ESKF_UPDATE_ACCEPTED,
+	ESKF_UPDATE_REJECTED_NIS
+} eskf_update_result_t;
+
+typedef struct {
+	double accel_noise_density[ESKF_VECTOR_DIM];
+	double gyro_noise_density[ESKF_VECTOR_DIM];
+	double accel_bias_random_walk[ESKF_VECTOR_DIM];
+	double gyro_bias_random_walk[ESKF_VECTOR_DIM];
+	double initial_position_std_m[ESKF_VECTOR_DIM];
+	double initial_velocity_std_mps[ESKF_VECTOR_DIM];
+	double initial_attitude_std_rad[ESKF_VECTOR_DIM];
+	double initial_accel_bias_std_mps2[ESKF_VECTOR_DIM];
+	double initial_gyro_bias_std_rps[ESKF_VECTOR_DIM];
+	double gravity_mps2;
+	double max_propagation_step_s;
+	double position_nis_gate;
+	double covariance_diagonal_floor;
+} eskf_config_t;
+
+typedef struct {
+	double position_m[ESKF_VECTOR_DIM];
+	double velocity_mps[ESKF_VECTOR_DIM];
+	double body_to_nav_quaternion[ESKF_QUATERNION_DIM];
+	double accel_bias_mps2[ESKF_VECTOR_DIM];
+	double gyro_bias_rps[ESKF_VECTOR_DIM];
+	double covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM];
+	double last_linear_accel_nav_mps2[ESKF_VECTOR_DIM];
+	double last_unbiased_gyro_rps[ESKF_VECTOR_DIM];
+	double last_position_nis;
+	uint64_t propagation_count;
+	uint64_t position_update_accepted_count;
+	uint64_t position_update_rejected_count;
+	bool initialized;
+} eskf_t;
+
+void eskf_default_config(eskf_config_t* config);
+
+bool eskf_quaternion_align_vectors(const double source[ESKF_VECTOR_DIM],
+	                               const double target[ESKF_VECTOR_DIM],
+	                               double quaternion_out[ESKF_QUATERNION_DIM]);
+
+bool eskf_initialize(eskf_t* filter,
+	                 const eskf_config_t* config,
+	                 const double initial_position_m[ESKF_VECTOR_DIM],
+	                 const double body_to_nav_quaternion[ESKF_QUATERNION_DIM],
+	                 const double accel_bias_mps2[ESKF_VECTOR_DIM],
+	                 const double gyro_bias_rps[ESKF_VECTOR_DIM]);
+
+bool eskf_propagate(eskf_t* filter,
+	                const eskf_config_t* config,
+	                const double accel_mps2[ESKF_VECTOR_DIM],
+	                const double gyro_rps[ESKF_VECTOR_DIM],
+	                double dt_s);
+
+/* Midpoint inertial integration over two timestamped raw samples.  The
+ * constant-input eskf_propagate() entry point remains available for callers
+ * that already provide an interval-integrated/held measurement. */
+bool eskf_propagate_interval(
+	eskf_t* filter,
+	const eskf_config_t* config,
+	const double start_accel_mps2[ESKF_VECTOR_DIM],
+	const double start_gyro_rps[ESKF_VECTOR_DIM],
+	const double end_accel_mps2[ESKF_VECTOR_DIM],
+	const double end_gyro_rps[ESKF_VECTOR_DIM],
+	double dt_s);
+
+eskf_update_result_t eskf_position_update(
+	eskf_t* filter,
+	const eskf_config_t* config,
+	const double position_m[ESKF_VECTOR_DIM],
+	const double covariance_m2[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
+ * cleared because they are no longer valid after the state discontinuity. */
+bool eskf_reset_position(
+	eskf_t* filter,
+	const double position_m[ESKF_VECTOR_DIM],
+	const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM]);
+
+bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps);
+
+bool eskf_zero_angular_rate_update(
+	eskf_t* filter,
+	const double measured_gyro_rps[ESKF_VECTOR_DIM],
+	const double measurement_std_rps[ESKF_VECTOR_DIM]);
+
+double eskf_speed_mps(const eskf_t* filter);
+bool eskf_state_is_finite(const eskf_t* filter);
+
+#endif