iekf.h

Go to the documentation of this file.

 
#ifndef IEKF_H
#define IEKF_H
 
#include <stdint.h>
 
#ifdef __cplusplus
extern "C" {
#endif
 
#define IEKF_NX 6
#define IEKF_P_ATT_MIN   1e-12f   
#define IEKF_P_BIAS_MIN  1e-16f   
typedef struct {
    float   sigma_gyro;     
    float   sigma_bias;     
    float   sigma_acc;      
    float   sigma_mag;
    float   acc_tol;        
    float   mag_tol;
    float   p0_att;         
    float   p0_bias;        
    uint8_t estimate_bias;
    uint8_t adaptive;
    uint8_t tilt_comp_mag;
    float   acc_nis_gate;    /* NIS gate (chi^2, 2 dof) above which R_acc is
                                inflated by NIS/gate. 0 = off. 9.21 = 99%        */
    float   mag_nis_gate;    /* same for the magnetometer. NOT RECOMMENDED: a high
                                mag NIS is often a true yaw error, and de-weighting
                                it slows recovery. Prefer dip_tol.                */
    float   dip_tol;         /* rad: the mag is de-weighted when the angle between
                                the MEASURED field and gravity departs from the
                                calibration value (magnetic disturbance). Independent
                                of the estimate. 0 = off                          */
    float   infl_decay;      /* per-update decay of the inflation (memory):
                                exp(-dt_update/tau), e.g. tau = 0.25 s           */
    uint8_t decouple;        
    uint8_t reset_jacobian;  
    uint8_t tangent_innov;   /* 1: keep only the part of the vector innovation
                                tangent to the predicted direction (default). The
                                radial part (1 - cos e along yhat) is outside the
                                range of H: no attitude error explains it, so in S
                                it is weighted by R alone and inflates the NIS of
                                large errors for nothing (c4dynamics review, 2026-11) */
    float    infl_max;          
    uint16_t lockout_updates;   
    float    lockout_p_bump;    
    uint16_t grace_updates;     
    float    persist_decay;     
    float    persist_ratio;     
    float    persist_min;       
} iekf_config_t;
 
typedef struct {
    float R0[9];
    float b0[3];
    float g_ref[3];         
    float m_ref[3];         
    float nom_acc;          
    float nom_mag;
} iekf_seed_t;
 
typedef struct {
    iekf_config_t cfg;
    float R[9];
    float b[3];
    float P[IEKF_NX * IEKF_NX];
    float g_ref[3], m_ref[3];
    float nom_acc, nom_mag;
    float last_acc_dev, last_mag_dev;   
    float acc_infl, mag_infl;           
    float last_acc_nis, last_mag_nis;
    float last_acc_dir[3];              
    float dip_ref;                      
    float last_dip_dev;
    uint8_t acc_trusted;                /* set from outside (stillness detector):
                                           no NIS inflation on the next acc update */
    uint16_t acc_gated_run;             
    uint16_t acc_grace;                 
    uint32_t lockouts;                  
    float    innov_w_mean[3];           
    float    innov_w_pow;               
    float    last_acc_innov_b[3];       
    float E_last[9];                    
    float wF[36], wA[36], wB[36];
    float wH[18], wHt[18], wPHt[18], wK[18];
} iekf_t;
 
void iekf_default_config(iekf_config_t *cfg);
void iekf_init(iekf_t *f, const iekf_config_t *cfg, const iekf_seed_t *seed);
void iekf_propagate_dtheta(iekf_t *f, const float dtheta[3], float dt);
void iekf_propagate(iekf_t *f, const float gyro[3], float dt);
int  iekf_update_acc(iekf_t *f, const float acc[3]);
int  iekf_update_mag(iekf_t *f, const float mag[3]);
int  iekf_update_mag_ex(iekf_t *f, const float mag[3], float extra_var);
void iekf_get_quat(const iekf_t *f, float q[4]);
 
void iekf_reset_attitude(iekf_t *f, const float R0[9], float p0_att);
 
#ifdef __cplusplus
}
#endif
#endif /* IEKF_H */