86 lines
2.8 KiB
C
86 lines
2.8 KiB
C
#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);
|
|
|
|
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);
|
|
|
|
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
|