ahrs_float_mlkf.h File Reference

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

#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"

Include dependency graph for ahrs_float_mlkf.h:

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

Go to the source code of this file.

Data Structures
struct	AhrsMlkf

Enumerations
enum	AhrsMlkfStatus { AHRS_MLKF_UNINIT, AHRS_MLKF_RUNNING }

Functions
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_t	ahrs_mlkf_align (struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void	ahrs_mlkf_propagate (struct Int32Rates *gyro, float dt)
void	ahrs_mlkf_update_accel (struct Int32Vect3 *accel)
void	ahrs_mlkf_update_mag (struct Int32Vect3 *mag)
void	ahrs_mlkf_update_mag_2d (struct Int32Vect3 *mag)
void	ahrs_mlkf_update_mag_full (struct Int32Vect3 *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.h.

---

Data Structure Documentation

struct AhrsMlkf

Definition at line 43 of file ahrs_float_mlkf.h.

Collaboration diagram for AhrsMlkf:

Data Fields
struct OrientationReps	body_to_imu	body_to_imu rotation
struct FloatQuat	gibbs_cor
struct FloatRates	gyro_bias
struct FloatRates	imu_rate	Rotational velocity in IMU frame.
bool_t	is_aligned
float	lp_accel
struct FloatQuat	ltp_to_body_quat	Rotation from LocalTangentPlane to body frame as unit quaternion.
struct FloatQuat	ltp_to_imu_quat	Rotation from LocalTangentPlane to IMU frame as unit quaternion.
struct FloatVect3	mag_h
struct FloatVect3	mag_noise
float	P[6][6]
enum AhrsMlkfStatus	status

Enumeration Type Documentation

enum AhrsMlkfStatus

Enumerator
AHRS_MLKF_UNINIT
AHRS_MLKF_RUNNING

Definition at line 38 of file ahrs_float_mlkf.h.

Function Documentation

bool_t ahrs_mlkf_align	(	struct Int32Rates *	lp_gyro,
		struct Int32Vect3 *	lp_accel,
		struct Int32Vect3 *	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, AhrsMlkf::ltp_to_imu_quat, RATES_COPY, RATES_FLOAT_OF_BFP, and TRUE.

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, FALSE, 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 Int32Rates *	gyro,
		float	dt
	)

Definition at line 124 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 Int32Vect3 *

accel

)

Definition at line 130 of file ahrs_float_mlkf.c.

References ACCELS_FLOAT_OF_BFP, ahrs_mlkf, 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 Int32Vect3 *

mag

)

Definition at line 144 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 Int32Vect3 *

mag

)

Definition at line 153 of file ahrs_float_mlkf.c.

References ahrs_mlkf, AhrsMlkf::mag_h, AhrsMlkf::mag_noise, MAGS_FLOAT_OF_BFP, 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 Int32Vect3 *

mag

)

Definition at line 161 of file ahrs_float_mlkf.c.

References ahrs_mlkf, AhrsMlkf::mag_h, AhrsMlkf::mag_noise, MAGS_FLOAT_OF_BFP, 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:

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(), set_body_state_from_quat(), update_state(), and update_state_heading().