iekf.c File Reference

Attitude IEKF on SO(3) - port of iekfAttitude.m (see iekf.h). More...

#include "iekf.h"
#include "so3.h"
#include "arm_math.h"
#include <string.h>
#include <math.h>

Include dependency graph for iekf.c:

Go to the source code of this file.

Macros
#define	NX   IEKF_NX

Enumerations
enum	proj_t { PROJ_NONE = 0 , PROJ_TILT_ONLY = 1 , PROJ_YAW_ONLY = 2 }

Functions
static void	condition_covariance (iekf_t *f)
static void	mmul (float *C, const float *A, uint16_t ar, uint16_t ac, const float *B, uint16_t bc)
static void	mtrans (float *At, const float *A, uint16_t r, uint16_t c)
static void	symmetrize (float *P)
void	iekf_default_config (iekf_config_t *c)
void	iekf_init (iekf_t *f, const iekf_config_t *cfg, const iekf_seed_t *s)
void	iekf_propagate_dtheta (iekf_t *f, const float dtheta[3], float h)
	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 h)
	Single-sample case: dtheta = gyro*dt (compatible with the previous port)
static void	reset_covariance (iekf_t *f, const float dth[3])
static void	tangent_part (float e[3], const float u[3])
static void	project_gain (iekf_t *f, proj_t proj)
static float	vector_update (iekf_t *f, const float y[3], const float yhat[3], float r, float nis_gate, float *infl, proj_t proj)
static float	adaptive_var (float r0, float n, float nom, float tol, int adaptive, float *dev_out)
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) - port of iekfAttitude.m (see iekf.h).

Definition in file iekf.c.

Macro Definition Documentation

NX

#define NX   IEKF_NX

Definition at line 11 of file iekf.c.

Enumeration Type Documentation

proj_t

enum proj_t

Enumerator
PROJ_NONE
PROJ_TILT_ONLY
PROJ_YAW_ONLY

Definition at line 178 of file iekf.c.

Function Documentation

condition_covariance()

static void condition_covariance ( iekf_t *  f )

static

Definition at line 168 of file iekf.c.

References iekf_t::cfg, iekf_config_t::estimate_bias, IEKF_P_ATT_MIN, IEKF_P_BIAS_MIN, NX, iekf_t::P, and symmetrize().

Referenced by iekf_propagate_dtheta(), iekf_reset_attitude(), and vector_update().

Here is the call graph for this function:

Here is the caller graph for this function:

mmul()

static void mmul ( float *  C, const float *  A, uint16_t  ar, uint16_t  ac, const float *  B, uint16_t  bc  )

static

Definition at line 16 of file iekf.c.

References A, B, and C.

Referenced by iekf_propagate_dtheta(), and vector_update().

Here is the caller graph for this function:

mtrans()

static void mtrans ( float *  At, const float *  A, uint16_t  r, uint16_t  c  )

static

Definition at line 26 of file iekf.c.

References A.

Referenced by iekf_propagate_dtheta(), and vector_update().

Here is the caller graph for this function:

symmetrize()

static void symmetrize ( float *  P )

static

Definition at line 34 of file iekf.c.

References NX.

Referenced by condition_covariance(), and vector_update().

Here is the caller graph for this function:

iekf_default_config()

void iekf_default_config

(

iekf_config_t *

c

)

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 *	s
	)

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	h
	)

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:

reset_covariance()

static void reset_covariance ( iekf_t *  f, const float  dth[3]  )

static

Definition at line 141 of file iekf.c.

References A, iekf_t::cfg, iekf_config_t::estimate_bias, mat3_mul(), NX, iekf_t::P, and so3_skew().

Referenced by vector_update().

Here is the call graph for this function:

Here is the caller graph for this function:

tangent_part()

static void tangent_part ( float  e[3], const float  u[3]  )

static

Definition at line 184 of file iekf.c.

References vec3_dot().

Referenced by iekf_update_acc(), and vector_update().

Here is the call graph for this function:

Here is the caller graph for this function:

project_gain()

static void project_gain ( iekf_t *  f, proj_t  proj  )

static

Definition at line 196 of file iekf.c.

References iekf_t::cfg, iekf_config_t::estimate_bias, iekf_t::g_ref, mat3t_vec(), PROJ_NONE, PROJ_TILT_ONLY, iekf_t::R, vec3_normalize(), and iekf_t::wK.

Referenced by vector_update().

Here is the call graph for this function:

Here is the caller graph for this function:

vector_update()

static float vector_update ( iekf_t *  f, const float  y[3], const float  yhat[3], float  r, float  nis_gate, float *  infl, proj_t  proj  )

static

Definition at line 226 of file iekf.c.

References iekf_t::b, iekf_t::cfg, condition_covariance(), iekf_config_t::estimate_bias, iekf_config_t::infl_decay, iekf_config_t::infl_max, mat3_inv(), mat3_mul(), mat3_vec(), mmul(), mtrans(), NX, iekf_t::P, project_gain(), iekf_t::R, reset_covariance(), iekf_config_t::reset_jacobian, so3_exp(), so3_orthonormalize(), so3_skew(), symmetrize(), iekf_config_t::tangent_innov, tangent_part(), vec3_dot(), iekf_t::wA, iekf_t::wB, iekf_t::wF, iekf_t::wH, iekf_t::wHt, iekf_t::wK, and iekf_t::wPHt.

Referenced by iekf_update_acc(), and iekf_update_mag_ex().

Here is the call graph for this function:

Here is the caller graph for this function:

adaptive_var()

static float adaptive_var ( float  r0, float  n, float  nom, float  tol, int  adaptive, float *  dev_out  )

static

Definition at line 306 of file iekf.c.

References n.

Referenced by iekf_update_acc(), and iekf_update_mag_ex().

Here is the caller 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: