ahrs_float_mlkf.c File Reference

Multiplicative linearized Kalman Filter in quaternion formulation. More...

#include "subsystems/ahrs/ahrs_float_mlkf.h"
#include "subsystems/ahrs/ahrs_float_utils.h"
#include <string.h>
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_simple_matrix.h"
#include "generated/airframe.h"

Include dependency graph for ahrs_float_mlkf.c:

Go to the source code of this file.

Macros
#define	AHRS_MAG_NOISE_X   0.2
#define	AHRS_MAG_NOISE_Y   0.2
#define	AHRS_MAG_NOISE_Z   0.2

Functions
static void	propagate_ref (struct FloatRates *gyro, float dt)
static void	propagate_state (float dt)
	Progagate filter's covariance We don't propagate state as we assume to have reseted. More...
static void	update_state (const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, struct FloatVect3 *noise)
	Incorporate one 3D vector measurement. More...
static void	update_state_heading (const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, struct FloatVect3 *noise)
	Incorporate one 3D vector measurement, only correcting heading. More...
static void	reset_state (void)
	Incorporate errors to reference and zeros state. More...
void	ahrs_mlkf_init (void)
void	ahrs_mlkf_set_body_to_imu (struct OrientationReps *body_to_imu)
void	ahrs_mlkf_set_body_to_imu_quat (struct FloatQuat *q_b2i)
bool	ahrs_mlkf_align (struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void	ahrs_mlkf_propagate (struct FloatRates *gyro, float dt)
void	ahrs_mlkf_update_accel (struct FloatVect3 *accel)
void	ahrs_mlkf_update_mag (struct FloatVect3 *mag)
void	ahrs_mlkf_update_mag_2d (struct FloatVect3 *mag)
void	ahrs_mlkf_update_mag_full (struct FloatVect3 *mag)

Variables
struct AhrsMlkf	ahrs_mlkf

Detailed Description

Multiplicative linearized Kalman Filter in quaternion formulation.

Estimate the attitude, heading and gyro bias.

Definition in file ahrs_float_mlkf.c.

Macro Definition Documentation

#define AHRS_MAG_NOISE_X   0.2

Definition at line 44 of file ahrs_float_mlkf.c.

Referenced by ahrs_mlkf_init().

#define AHRS_MAG_NOISE_Y   0.2

Definition at line 45 of file ahrs_float_mlkf.c.

Referenced by ahrs_mlkf_init().

#define AHRS_MAG_NOISE_Z   0.2

Definition at line 46 of file ahrs_float_mlkf.c.

Referenced by ahrs_mlkf_init().

Function Documentation

bool ahrs_mlkf_align	(	struct FloatRates *	lp_gyro,
		struct FloatVect3 *	lp_accel,
		struct FloatVect3 *	lp_mag
	)

Definition at line 107 of file ahrs_float_mlkf.c.

References ahrs_float_get_quat_from_accel_mag(), ahrs_mlkf, AhrsMlkf::gyro_bias, AhrsMlkf::is_aligned, and AhrsMlkf::ltp_to_imu_quat.

Referenced by aligner_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_mlkf_init

(

void

)

Definition at line 62 of file ahrs_float_mlkf.c.

References AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z, ahrs_mlkf, float_quat_identity(), FLOAT_RATES_ZERO, AhrsMlkf::gyro_bias, AhrsMlkf::imu_rate, AhrsMlkf::is_aligned, AhrsMlkf::ltp_to_imu_quat, AhrsMlkf::mag_h, AhrsMlkf::mag_noise, AhrsMlkf::P, and VECT3_ASSIGN.

Referenced by ahrs_mlkf_register().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_mlkf_propagate	(	struct FloatRates *	gyro,
		float	dt
	)

Definition at line 122 of file ahrs_float_mlkf.c.

References propagate_ref(), and propagate_state().

Referenced by gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_mlkf_set_body_to_imu

(

struct OrientationReps *

body_to_imu

)

Definition at line 91 of file ahrs_float_mlkf.c.

References ahrs_mlkf_set_body_to_imu_quat(), and orientationGetQuat_f().

Here is the call graph for this function:

void ahrs_mlkf_set_body_to_imu_quat

(

struct FloatQuat *

q_b2i

)

Definition at line 96 of file ahrs_float_mlkf.c.

References ahrs_mlkf, AhrsMlkf::body_to_imu, AhrsMlkf::is_aligned, AhrsMlkf::ltp_to_imu_quat, orientationGetQuat_f(), and orientationSetQuat_f().

Referenced by ahrs_mlkf_set_body_to_imu(), and body_to_imu_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_mlkf_update_accel

(

struct FloatVect3 *

accel

)

Definition at line 128 of file ahrs_float_mlkf.c.

References ahrs_mlkf, alpha, dn, float_vect3_norm(), AhrsMlkf::lp_accel, reset_state(), and update_state().

Referenced by accel_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_mlkf_update_mag

(

struct FloatVect3 *

mag

)

Definition at line 141 of file ahrs_float_mlkf.c.

References ahrs_mlkf_update_mag_2d(), and ahrs_mlkf_update_mag_full().

Referenced by mag_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_mlkf_update_mag_2d

(

struct FloatVect3 *

mag

)

Definition at line 150 of file ahrs_float_mlkf.c.

References ahrs_mlkf, AhrsMlkf::mag_h, AhrsMlkf::mag_noise, reset_state(), and update_state_heading().

Referenced by ahrs_mlkf_update_mag().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_mlkf_update_mag_full

(

struct FloatVect3 *

mag

)

Definition at line 156 of file ahrs_float_mlkf.c.

References ahrs_mlkf, AhrsMlkf::mag_h, AhrsMlkf::mag_noise, reset_state(), and update_state().

Referenced by ahrs_mlkf_update_mag().

Here is the call graph for this function:

Here is the caller graph for this function:

static void propagate_ref ( struct FloatRates *  gyro, float  dt  )

inlinestatic

Definition at line 163 of file ahrs_float_mlkf.c.

References ahrs_mlkf, alpha, float_quat_integrate(), FLOAT_RATES_LIN_CMB, AhrsMlkf::gyro_bias, AhrsMlkf::imu_rate, AhrsMlkf::ltp_to_imu_quat, RATES_COPY, and RATES_SUB.

Referenced by ahrs_mlkf_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

static void propagate_state ( float  dt )

inlinestatic

Progagate filter's covariance We don't propagate state as we assume to have reseted.

Definition at line 188 of file ahrs_float_mlkf.c.

References ahrs_mlkf, AhrsMlkf::imu_rate, MAT_MUL, MAT_MUL_T, AhrsMlkf::P, FloatRates::p, FloatRates::q, and FloatRates::r.

Referenced by ahrs_mlkf_propagate().

Here is the caller graph for this function:

static void reset_state ( void  )

inlinestatic

Incorporate errors to reference and zeros state.

Definition at line 357 of file ahrs_float_mlkf.c.

References ahrs_mlkf, float_quat_comp(), float_quat_identity(), float_quat_normalize(), AhrsMlkf::gibbs_cor, AhrsMlkf::ltp_to_imu_quat, and FloatQuat::qi.

Referenced by ahrs_mlkf_update_accel(), ahrs_mlkf_update_mag_2d(), and ahrs_mlkf_update_mag_full().

Here is the call graph for this function:

Here is the caller graph for this function:

static void update_state ( const struct FloatVect3 *  i_expected, struct FloatVect3 *  b_measured, struct FloatVect3 *  noise  )

inlinestatic

Incorporate one 3D vector measurement.

Parameters

i_expected	expected 3d vector in inertial frame
b_measured	measured 3d vector in body/imu frame
noise	measurement noise vector (diagonal of covariance)

Definition at line 222 of file ahrs_float_mlkf.c.

References ahrs_mlkf, float_quat_vmult(), AhrsMlkf::gibbs_cor, AhrsMlkf::gyro_bias, H, K, AhrsMlkf::ltp_to_imu_quat, MAT_INV33, MAT_MUL, MAT_MUL_T, MAT_SUB, AhrsMlkf::P, FloatRates::p, FloatRates::q, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, VECT3_DIFF, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_mlkf_update_accel(), and ahrs_mlkf_update_mag_full().

Here is the call graph for this function:

Here is the caller graph for this function:

static void update_state_heading ( const struct FloatVect3 *  i_expected, struct FloatVect3 *  b_measured, struct FloatVect3 *  noise  )

inlinestatic

Incorporate one 3D vector measurement, only correcting heading.

Parameters

i_expected	expected 3d vector in inertial frame
b_measured	measured 3d vector in body/imu frame
noise	measurement noise vector (diagonal of covariance) TODO: optimize

Definition at line 290 of file ahrs_float_mlkf.c.

References ahrs_mlkf, float_quat_vmult(), AhrsMlkf::gibbs_cor, AhrsMlkf::gyro_bias, H, K, AhrsMlkf::ltp_to_imu_quat, MAT_INV33, MAT_MUL, MAT_MUL_T, MAT_SUB, AhrsMlkf::P, FloatRates::p, FloatRates::q, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, VECT3_DIFF, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_mlkf_update_mag_2d().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

struct AhrsMlkf ahrs_mlkf

Definition at line 59 of file ahrs_float_mlkf.c.

Referenced by accel_cb(), ahrs_mlkf_align(), ahrs_mlkf_init(), ahrs_mlkf_set_body_to_imu_quat(), ahrs_mlkf_update_accel(), ahrs_mlkf_update_mag_2d(), ahrs_mlkf_update_mag_full(), aligner_cb(), geo_mag_cb(), gyro_cb(), mag_cb(), propagate_ref(), propagate_state(), reset_state(), send_bias(), send_euler(), send_filter_status(), send_geo_mag(), set_body_state_from_quat(), update_state(), and update_state_heading().