iekf.h File Reference

Attitude IEKF on SO(3), C port of iekfAttitude.m. More...

#include <stdint.h>

Include dependency graph for iekf.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	iekf_config_t
struct	iekf_seed_t
	Initial conditions, typically from calib_finish() More...
struct	iekf_t

Macros
#define	IEKF_NX   6
#define	IEKF_P_ATT_MIN   1e-12f
	rad^2 : floor on the diagonal of P
#define	IEKF_P_BIAS_MIN   1e-16f
	(rad/s)^2

Functions
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)
	Propagation with a measured angle increment (bias NOT yet removed): phi = dtheta - b*dt.
void	iekf_propagate (iekf_t *f, const float gyro[3], float dt)
	Single-sample case: dtheta = gyro*dt (compatible with the previous port)
int	iekf_update_acc (iekf_t *f, const float acc[3])
	Return 1 if the measurement was "gated" (de-weighted), 0 otherwise.
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)
	As above, with an additional per-component variance (e.g.
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)
	Re-initialisation of the attitude away from rest (estimator reset after a failsafe, re-init in the air, hand launch): R <- R0, the attitude block of P set to p0_att*I with no attitude-bias correlation (an HONEST p0_att: as large as the possible error, otherwise the filter is confidently wrong), the NIS inflation and the persistence memory cleared, and the accelerometer trusted for cfg.grace_updates updates so the measurements the filter needs are not rejected while P_aa shrinks.

Detailed Description

Attitude IEKF on SO(3), C port of iekfAttitude.m.

Same model, same parameters, same equations: left-invariant error R_true = R_hat * exp(delta) error state x = [delta; beta], beta = b_true - b_hat F = [exp(-w*h), -h*I; 0, I], H = [skew(y_hat), 0] 6x6 covariance with CMSIS-DSP (arm_mat_*), 3x3 geometry hand-written.

Multi-rate use (phase 1, see 03 - Docs/theory/iekf-embedded.md): 960 Hz : iekf_propagate_dtheta(&f, dtheta, dt) dtheta = sum of omega*dt over the gyro samples of the step 240 Hz : iekf_update_acc(&f, rotated_acc_mean) (see accwin.h) ~100 Hz: iekf_update_mag(&f, mag) only on new data

State: 3 (attitude) + 3 (gyro bias) by default; cfg.estimate_bias = 0 reduces the filter exactly to the 3-state version.

Definition in file iekf.h.

Macro Definition Documentation

IEKF_NX

#define IEKF_NX   6

Definition at line 29 of file iekf.h.

IEKF_P_ATT_MIN

#define IEKF_P_ATT_MIN   1e-12f

rad^2 : floor on the diagonal of P

Definition at line 30 of file iekf.h.

IEKF_P_BIAS_MIN

#define IEKF_P_BIAS_MIN   1e-16f

(rad/s)^2

Definition at line 31 of file iekf.h.

Function Documentation

iekf_default_config()

void iekf_default_config

(

iekf_config_t *

cfg

)

Definition at line 45 of file iekf.c.

References iekf_config_t::acc_nis_gate, iekf_config_t::acc_tol, iekf_config_t::adaptive, iekf_config_t::decouple, iekf_config_t::dip_tol, iekf_config_t::estimate_bias, iekf_config_t::grace_updates, iekf_config_t::infl_decay, iekf_config_t::infl_max, iekf_config_t::lockout_p_bump, iekf_config_t::lockout_updates, iekf_config_t::mag_nis_gate, iekf_config_t::mag_tol, iekf_config_t::p0_att, iekf_config_t::p0_bias, iekf_config_t::persist_decay, iekf_config_t::persist_min, iekf_config_t::persist_ratio, iekf_config_t::reset_jacobian, iekf_config_t::sigma_acc, iekf_config_t::sigma_bias, iekf_config_t::sigma_gyro, iekf_config_t::sigma_mag, iekf_config_t::tangent_innov, and iekf_config_t::tilt_comp_mag.

Referenced by app_est_config().

Here is the caller graph for this function:

iekf_init()

void iekf_init	(	iekf_t *	f,
		const iekf_config_t *	cfg,
		const iekf_seed_t *	seed
	)

Definition at line 74 of file iekf.c.

References iekf_t::acc_infl, iekf_t::b, iekf_t::cfg, iekf_t::dip_ref, iekf_t::E_last, iekf_config_t::estimate_bias, iekf_t::g_ref, iekf_t::last_acc_dir, iekf_t::m_ref, iekf_t::mag_infl, mat3t_vec(), iekf_t::nom_acc, iekf_t::nom_mag, NX, iekf_t::P, iekf_config_t::p0_att, iekf_config_t::p0_bias, iekf_t::R, s, so3_orthonormalize(), vec3_dot(), and vec3_normalize().

Referenced by on_mag().

Here is the call graph for this function:

Here is the caller graph for this function:

iekf_propagate_dtheta()

void iekf_propagate_dtheta	(	iekf_t *	f,
		const float	dtheta[3],
		float	dt
	)

Propagation with a measured angle increment (bias NOT yet removed): phi = dtheta - b*dt.

Use this form when a step spans several samples.

Definition at line 99 of file iekf.c.

References iekf_t::b, iekf_t::cfg, condition_covariance(), iekf_t::E_last, iekf_config_t::estimate_bias, mat3_mul(), mmul(), mtrans(), NX, iekf_t::P, iekf_t::R, iekf_config_t::sigma_bias, iekf_config_t::sigma_gyro, so3_exp(), so3_orthonormalize(), iekf_t::wA, iekf_t::wB, and iekf_t::wF.

Referenced by iekf_propagate(), and on_step().

Here is the call graph for this function:

Here is the caller graph for this function:

iekf_propagate()

void iekf_propagate	(	iekf_t *	f,
		const float	gyro[3],
		float	dt
	)

Single-sample case: dtheta = gyro*dt (compatible with the previous port)

Definition at line 134 of file iekf.c.

References iekf_propagate_dtheta().

Here is the call graph for this function:

iekf_update_acc()

int iekf_update_acc	(	iekf_t *	f,
		const float	acc[3]
	)

Return 1 if the measurement was "gated" (de-weighted), 0 otherwise.

Definition at line 315 of file iekf.c.

References iekf_t::acc_gated_run, iekf_t::acc_grace, iekf_t::acc_infl, iekf_config_t::acc_nis_gate, iekf_config_t::acc_tol, iekf_t::acc_trusted, iekf_config_t::adaptive, adaptive_var(), iekf_t::cfg, iekf_config_t::decouple, iekf_t::g_ref, iekf_config_t::grace_updates, iekf_t::innov_w_mean, iekf_t::innov_w_pow, iekf_t::last_acc_dev, iekf_t::last_acc_dir, iekf_t::last_acc_innov_b, iekf_t::last_acc_nis, iekf_config_t::lockout_p_bump, iekf_config_t::lockout_updates, iekf_t::lockouts, mat3_vec(), mat3t_vec(), iekf_t::nom_acc, NX, iekf_t::P, iekf_config_t::persist_decay, iekf_config_t::persist_min, iekf_config_t::persist_ratio, PROJ_NONE, PROJ_TILT_ONLY, iekf_t::R, iekf_config_t::sigma_acc, iekf_config_t::tangent_innov, tangent_part(), vec3_dot(), vec3_norm(), and vector_update().

Referenced by on_acc_window().

Here is the call graph for this function:

Here is the caller graph for this function:

iekf_update_mag()

int iekf_update_mag	(	iekf_t *	f,
		const float	mag[3]
	)

Definition at line 366 of file iekf.c.

References iekf_update_mag_ex().

Here is the call graph for this function:

iekf_update_mag_ex()

int iekf_update_mag_ex	(	iekf_t *	f,
		const float	mag[3],
		float	extra_var
	)

As above, with an additional per-component variance (e.g.

uncertainty due to the age of the sample during a fast rotation).

Definition at line 371 of file iekf.c.

References iekf_config_t::adaptive, adaptive_var(), iekf_t::cfg, iekf_config_t::decouple, iekf_t::dip_ref, iekf_config_t::dip_tol, iekf_t::last_acc_dir, iekf_t::last_dip_dev, iekf_t::last_mag_dev, iekf_t::last_mag_nis, iekf_t::m_ref, iekf_t::mag_infl, iekf_config_t::mag_nis_gate, iekf_config_t::mag_tol, mat3_vec(), mat3t_vec(), iekf_t::nom_mag, PROJ_NONE, PROJ_YAW_ONLY, iekf_t::R, iekf_config_t::sigma_mag, iekf_config_t::tilt_comp_mag, vec3_dot(), vec3_norm(), and vector_update().

Referenced by iekf_update_mag(), and on_mag().

Here is the call graph for this function:

Here is the caller graph for this function:

iekf_get_quat()

void iekf_get_quat	(	const iekf_t *	f,
		float	q[4]
	)

Definition at line 408 of file iekf.c.

References iekf_t::R, and so3_to_quat().

Referenced by flight_service(), on_acc_window(), on_state(), publish_est(), tlm_build_replay(), and tlm_build_state().

Here is the call graph for this function:

Here is the caller graph for this function:

iekf_reset_attitude()

void iekf_reset_attitude	(	iekf_t *	f,
		const float	R0[9],
		float	p0_att
	)

Re-initialisation of the attitude away from rest (estimator reset after a failsafe, re-init in the air, hand launch): R <- R0, the attitude block of P set to p0_att*I with no attitude-bias correlation (an HONEST p0_att: as large as the possible error, otherwise the filter is confidently wrong), the NIS inflation and the persistence memory cleared, and the accelerometer trusted for cfg.grace_updates updates so the measurements the filter needs are not rejected while P_aa shrinks.

The bias estimate and its covariance are kept.

Definition at line 413 of file iekf.c.

References iekf_t::acc_gated_run, iekf_t::acc_grace, iekf_t::acc_infl, iekf_t::cfg, condition_covariance(), iekf_config_t::grace_updates, iekf_t::innov_w_mean, iekf_t::innov_w_pow, iekf_t::mag_infl, NX, iekf_t::P, iekf_t::R, and so3_orthonormalize().

Here is the call graph for this function: