ahrs_float_invariant.h File Reference

AHRS using invariant filter. More...

#include "subsystems/ahrs.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"

Include dependency graph for ahrs_float_invariant.h:

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

Go to the source code of this file.

Data Structures
struct	inv_state
	Invariant filter state. More...
struct	inv_measures
	Invariant filter measurement vector. More...
struct	inv_command
	Invariant filter command vector. More...
struct	inv_correction_gains
	Invariant filter correction gains. More...
struct	inv_gains
	Invariant filter tuning gains. More...
struct	AhrsFloatInv
	Invariant filter structure. More...

Macros
#define	INV_STATE_DIM   9
	Invariant filter state dimension. More...
#define	INV_MEASURE_DIM   6
	Invariant filter measurement vector dimension. More...
#define	INV_COMMAND_DIM   3
	Invariant filter command vector dimension. More...

Functions
void	ahrs_float_invariant_init (void)
void	ahrs_float_inv_set_body_to_imu_quat (struct FloatQuat *q_b2i)
void	ahrs_float_invariant_align (struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void	ahrs_float_invariant_propagate (struct FloatRates *gyro, float dt)
void	ahrs_float_invariant_update_accel (struct FloatVect3 *accel)
void	ahrs_float_invariant_update_mag (struct FloatVect3 *mag)

Variables
struct AhrsFloatInv	ahrs_float_inv

Detailed Description

AHRS using invariant filter.

For more information, please send an email to "jp.condomines@gmail.com"

Definition in file ahrs_float_invariant.h.

---

Data Structure Documentation

struct inv_state

Invariant filter state.

Definition at line 40 of file ahrs_float_invariant.h.

Collaboration diagram for inv_state:

Data Fields
float	as	Estimated accelerometer sensitivity.
struct FloatRates	bias	Estimated gyro biases.
float	cs	Estimates magnetometer sensitivity.
float	hb	Estimates barometers bias.
struct NedCoor_f	pos	Estimates position.
struct FloatQuat	quat	Estimated attitude (quaternion)
struct NedCoor_f	speed	Estimates speed.

struct inv_measures

Invariant filter measurement vector.

Definition at line 53 of file ahrs_float_invariant.h.

Collaboration diagram for inv_measures:

Data Fields
struct FloatVect3	accel	Measured accelerometers.
float	baro_alt	Measured barometric altitude.
struct FloatVect3	mag	Measured magnetic field.
struct NedCoor_f	pos_gps	Measured gps position.
struct NedCoor_f	speed_gps	Measured gps speed.

struct inv_command

Invariant filter command vector.

Definition at line 64 of file ahrs_float_invariant.h.

Collaboration diagram for inv_command:

Data Fields
struct FloatVect3	accel	Input accelerometers.
struct FloatRates	rates	Input gyro rates.

struct inv_correction_gains

Invariant filter correction gains.

Definition at line 70 of file ahrs_float_invariant.h.

Collaboration diagram for inv_correction_gains:

Data Fields
struct FloatVect3	LE	Correction gains on attitude.
struct FloatVect3	ME	Correction gains on gyro biases. Correction gains on speed.
struct FloatVect3	NE	Correction gains on position.
float	NE	Correction gains on accel bias.
struct FloatVect3	OE	Correction gains on gyro biases.
float	OE	Correction gains on magnetometer sensitivity.
float	RE	Correction gains on accel bias.
float	SE	Correction gains on barometer bias.

struct inv_gains

Invariant filter tuning gains.

Definition at line 79 of file ahrs_float_invariant.h.

Data Fields
float	lb	Tuning parameter of mag error on attitude.
float	lv	Tuning parameter of speed error on attitude.
float	lx	Tuning parameter of accel and mag on attitude (longitudinal subsystem)
float	ly	Tuning parameter of accel and mag on attitude (lateral subsystem)
float	lz	Tuning parameter of accel and mag on attitude (heading subsystem)
float	mh	Tuning parameter of baro error on vertical speed.
float	mv	Tuning parameter of horizontal speed error on speed.
float	mvz	Tuning parameter of vertical speed error on speed.
float	mx	Tuning parameter of accel and mag on gyro bias (longitudinal subsystem)
float	my	Tuning parameter of accel and mag on gyro bias (lateral subsystem)
float	mz	Tuning parameter of accel and mag on gyro bias (heading subsystem)
float	n	Tuning parameter of accel and mag on accel bias (scaling subsystem)
float	nh	Tuning parameter of baro error on vertical position.
float	nx	Tuning parameter of horizontal position error on position.
float	nxz	Tuning parameter of vertical position error on position.
float	o	Tuning parameter of accel and mag on mag bias (scaling subsystem)
float	ob	Tuning parameter of mag error on gyro biases.
float	ov	Tuning parameter of speed error on gyro biases.
float	rh	Tuning parameter of baro error on accel biases (vertical projection)
float	rv	Tuning parameter of speed error on accel biases.
float	sh	Tuning parameter of baro error on baro bias.

struct AhrsFloatInv

Invariant filter structure.

Definition at line 92 of file ahrs_float_invariant.h.

Collaboration diagram for AhrsFloatInv:

Data Fields
struct OrientationReps	body_to_imu	body_to_imu rotation
struct inv_command	cmd	command vector
struct inv_correction_gains	corr	correction gains
struct inv_gains	gains	tuning gains
bool	is_aligned
struct FloatVect3	mag_h
struct inv_measures	meas	measurement vector
bool	reset	flag to request reset/reinit the filter
struct inv_state	state	state vector

Macro Definition Documentation

#define INV_COMMAND_DIM   3

Invariant filter command vector dimension.

Definition at line 60 of file ahrs_float_invariant.h.

Referenced by ahrs_float_invariant_propagate(), and ins_float_invariant_propagate().

#define INV_MEASURE_DIM   6

Invariant filter measurement vector dimension.

Definition at line 49 of file ahrs_float_invariant.h.

#define INV_STATE_DIM   9

Invariant filter state dimension.

Definition at line 36 of file ahrs_float_invariant.h.

Referenced by ahrs_float_invariant_propagate(), and ins_float_invariant_propagate().

Function Documentation

void ahrs_float_inv_set_body_to_imu_quat

(

struct FloatQuat *

q_b2i

)

Definition at line 384 of file ahrs_float_invariant.c.

References ahrs_float_inv, AhrsFloatInv::body_to_imu, AhrsFloatInv::is_aligned, orientationSetQuat_f(), inv_state::quat, and AhrsFloatInv::state.

Referenced by body_to_imu_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_float_invariant_align	(	struct FloatRates *	lp_gyro,
		struct FloatVect3 *	lp_accel,
		struct FloatVect3 *	lp_mag
	)

Definition at line 156 of file ahrs_float_invariant.c.

References ahrs_float_get_quat_from_accel_mag(), ahrs_float_inv, inv_state::bias, AhrsFloatInv::is_aligned, inv_state::quat, and AhrsFloatInv::state.

Referenced by aligner_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_float_invariant_init

(

void

)

Definition at line 131 of file ahrs_float_invariant.c.

References ahrs_float_inv, AHRS_INV_LX, AHRS_INV_LY, AHRS_INV_LZ, AHRS_INV_MX, AHRS_INV_MY, AHRS_INV_MZ, AHRS_INV_N, AHRS_INV_O, AhrsFloatInv::gains, init_invariant_state(), AhrsFloatInv::is_aligned, inv_gains::lx, inv_gains::ly, inv_gains::lz, AhrsFloatInv::mag_h, inv_gains::mx, inv_gains::my, inv_gains::mz, inv_gains::n, inv_gains::o, AhrsFloatInv::reset, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_float_invariant_register().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_float_invariant_propagate	(	struct FloatRates *	gyro,
		float	dt
	)

Definition at line 170 of file ahrs_float_invariant.c.

References ahrs_float_inv, inv_state::as, inv_state::bias, AhrsFloatInv::body_to_imu, AhrsFloatInv::cmd, inv_state::cs, DefaultChannel, DefaultDevice, error_output(), float_eulers_of_quat(), float_quat_normalize(), float_rmat_transp_ratemult(), foo, init_invariant_state(), INV_COMMAND_DIM, INV_STATE_DIM, invariant_model(), AhrsFloatInv::is_aligned, orientationGetRMat_f(), FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatQuat::qi, inv_state::quat, FloatRates::r, inv_command::rates, AhrsFloatInv::reset, runge_kutta_4_float(), AhrsFloatInv::state, and FloatEulers::theta.

Referenced by gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_float_invariant_update_accel

(

struct FloatVect3 *

accel

)

Definition at line 231 of file ahrs_float_invariant.c.

References inv_measures::accel, ahrs_float_inv, and AhrsFloatInv::meas.

Referenced by accel_cb().

Here is the caller graph for this function:

void ahrs_float_invariant_update_mag

(

struct FloatVect3 *

mag

)

Definition at line 239 of file ahrs_float_invariant.c.

References ahrs_float_inv, AhrsFloatInv::body_to_imu, float_rmat_transp_vmult(), FLOAT_VECT3_ZERO, inv_measures::mag, MAG_FROZEN_COUNT, AhrsFloatInv::meas, orientationGetRMat_f(), and FloatVect3::x.

Referenced by mag_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

struct AhrsFloatInv ahrs_float_inv

Definition at line 95 of file ahrs_float_invariant.c.

Referenced by accel_cb(), ahrs_float_inv_set_body_to_imu_quat(), ahrs_float_invariant_align(), ahrs_float_invariant_init(), ahrs_float_invariant_propagate(), ahrs_float_invariant_update_accel(), ahrs_float_invariant_update_mag(), aligner_cb(), compute_body_orientation_and_rates(), geo_mag_cb(), gyro_cb(), init_invariant_state(), invariant_model(), mag_cb(), send_att(), send_filter_status(), and send_geo_mag().