ahrs_float_invariant.c File Reference

#include "subsystems/ahrs/ahrs_float_invariant.h"
#include "subsystems/ahrs/ahrs_int_utils.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "generated/airframe.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_rk_float.h"

Include dependency graph for ahrs_float_invariant.c:

Go to the source code of this file.

Macros
#define	AHRS_INV_LX   2. * (0.06 + 0.1)
#define	AHRS_INV_LY   2. * (0.06 + 0.06)
#define	AHRS_INV_LZ   2. * (0.1 + 0.06)
#define	AHRS_INV_MX   2. * 0.05 * (0.06 + 0.1)
#define	AHRS_INV_MY   2. * 0.05 * (0.06 + 0.06)
#define	AHRS_INV_MZ   2. * 0.05 * (0.1 + 0.06)
#define	AHRS_INV_N   0.25
#define	AHRS_INV_O   0.5
#define	B   ahrs_float_inv.mag_h
#define	MAG_FROZEN_COUNT   30

Functions
static void	error_output (struct AhrsFloatInv *_ahrs)
	Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error ) More...
static void	invariant_model (float *o, const float *x, const int n, const float *u, const int m)
	Compute dynamic mode. More...
void	float_quat_vmul_right (struct FloatQuat *mright, const struct FloatQuat *q, struct FloatVect3 *vi)
	Right multiplication by a quaternion. More...
static void	init_invariant_state (void)
void	ahrs_float_invariant_init (void)
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)
void	ahrs_float_inv_set_body_to_imu_quat (struct FloatQuat *q_b2i)

Variables
struct AhrsFloatInv	ahrs_float_inv
static const struct FloatVect3	A = { 0.f, 0.f, -9.81f }

Detailed Description

Author

Jean-Philippe Condomines jp.co.nosp@m.ndom.nosp@m.ines@.nosp@m.gmai.nosp@m.l.com

AHRS using invariant filter.

Definition in file ahrs_float_invariant.c.

Macro Definition Documentation

#define AHRS_INV_LX   2. * (0.06 + 0.1)

Definition at line 65 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_init().

#define AHRS_INV_LY   2. * (0.06 + 0.06)

Definition at line 69 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_init().

#define AHRS_INV_LZ   2. * (0.1 + 0.06)

Definition at line 73 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_init().

#define AHRS_INV_MX   2. * 0.05 * (0.06 + 0.1)

Definition at line 77 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_init().

#define AHRS_INV_MY   2. * 0.05 * (0.06 + 0.06)

Definition at line 81 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_init().

#define AHRS_INV_MZ   2. * 0.05 * (0.1 + 0.06)

Definition at line 85 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_init().

#define AHRS_INV_N   0.25

Definition at line 89 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_init().

#define AHRS_INV_O   0.5

Definition at line 93 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_init().

#define B   ahrs_float_inv.mag_h

Definition at line 102 of file ahrs_float_invariant.c.

Referenced by AbsDiff(), error_output(), and imu_quality_assessment_periodic().

#define MAG_FROZEN_COUNT   30

Definition at line 238 of file ahrs_float_invariant.c.

Referenced by ahrs_float_invariant_update_mag().

Function Documentation

void ahrs_float_inv_set_body_to_imu_quat

(

struct FloatQuat *

q_b2i

)

Definition at line 385 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 157 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 132 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 171 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 232 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 240 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:

static void error_output ( struct AhrsFloatInv *  _ins )

inlinestatic

Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error )

Definition at line 319 of file ahrs_float_invariant.c.

References inv_measures::accel, inv_state::as, B, AhrsFloatInv::corr, inv_state::cs, float_quat_invert(), float_quat_vmult(), AhrsFloatInv::gains, inv_correction_gains::LE, inv_gains::lx, inv_gains::ly, inv_gains::lz, inv_measures::mag, inv_correction_gains::ME, AhrsFloatInv::meas, inv_gains::mx, inv_gains::my, inv_gains::mz, inv_gains::n, inv_correction_gains::NE, inv_gains::o, inv_correction_gains::OE, inv_state::quat, AhrsFloatInv::state, VECT3_CROSS_PRODUCT, VECT3_DIFF, VECT3_SMUL, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_float_invariant_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

void float_quat_vmul_right	(	struct FloatQuat *	mright,
		const struct FloatQuat *	q,
		struct FloatVect3 *	vi
	)

Right multiplication by a quaternion.

vi * q

Definition at line 371 of file ahrs_float_invariant.c.

References FLOAT_QUAT_EXTRACT, FloatQuat::qi, QUAT_ASSIGN, VECT3_ADD, VECT3_CROSS_PRODUCT, VECT3_DOT_PRODUCT, VECT3_SMUL, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by invariant_model().

Here is the caller graph for this function:

static void init_invariant_state ( void  )

inlinestatic

Definition at line 118 of file ahrs_float_invariant.c.

References inv_measures::accel, ahrs_float_inv, inv_state::as, inv_state::bias, inv_state::cs, float_quat_identity(), FLOAT_RATES_ZERO, FLOAT_VECT3_ZERO, inv_measures::mag, AhrsFloatInv::meas, inv_state::quat, and AhrsFloatInv::state.

Referenced by ahrs_float_invariant_init(), and ahrs_float_invariant_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

static void invariant_model ( float *  o, const float *  x, const int  n, const float *  u, const int  m  )

inlinestatic

Compute dynamic mode.

x_dot = evolution_model + (gain_matrix * error)

Definition at line 267 of file ahrs_float_invariant.c.

References ahrs_float_inv, inv_state::as, inv_state::bias, AhrsFloatInv::corr, inv_state::cs, float_quat_derivative(), FLOAT_QUAT_NORM2, float_quat_vmul_right(), float_quat_vmult(), float_vect_zero(), inv_correction_gains::LE, inv_correction_gains::ME, inv_correction_gains::NE, inv_correction_gains::OE, inv_state::quat, QUAT_ADD, QUAT_SMUL, inv_command::rates, RATES_ASSIGN, RATES_DIFF, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_float_invariant_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

const struct FloatVect3 A = { 0.f, 0.f, -9.81f }

static

Definition at line 99 of file ahrs_float_invariant.c.

struct AhrsFloatInv ahrs_float_inv

Definition at line 96 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().