Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_float_mlkf.c File Reference

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

#include "modules/ahrs/ahrs_float_mlkf.h"
#include "modules/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)
 
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

◆ AHRS_MAG_NOISE_X

#define AHRS_MAG_NOISE_X   0.2

Definition at line 44 of file ahrs_float_mlkf.c.

◆ AHRS_MAG_NOISE_Y

#define AHRS_MAG_NOISE_Y   0.2

Definition at line 45 of file ahrs_float_mlkf.c.

◆ AHRS_MAG_NOISE_Z

#define AHRS_MAG_NOISE_Z   0.2

Definition at line 46 of file ahrs_float_mlkf.c.

Function Documentation

◆ ahrs_mlkf_align()

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

Definition at line 91 of file ahrs_float_mlkf.c.

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

Referenced by aligner_cb().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ ahrs_mlkf_init()

void ahrs_mlkf_init ( void  )

◆ ahrs_mlkf_propagate()

void ahrs_mlkf_propagate ( struct FloatRates gyro,
float  dt 
)

Definition at line 106 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:

◆ ahrs_mlkf_update_accel()

void ahrs_mlkf_update_accel ( struct FloatVect3 accel)

Definition at line 112 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:

◆ ahrs_mlkf_update_mag()

void ahrs_mlkf_update_mag ( struct FloatVect3 mag)

Definition at line 125 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:

◆ ahrs_mlkf_update_mag_2d()

void ahrs_mlkf_update_mag_2d ( struct FloatVect3 mag)

Definition at line 134 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:

◆ ahrs_mlkf_update_mag_full()

void ahrs_mlkf_update_mag_full ( struct FloatVect3 mag)

Definition at line 140 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:

◆ propagate_ref()

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

Definition at line 147 of file ahrs_float_mlkf.c.

References ahrs_mlkf, alpha, AhrsMlkf::body_rate, float_quat_integrate(), FLOAT_RATES_LIN_CMB, AhrsMlkf::gyro_bias, AhrsMlkf::ltp_to_body_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:

◆ propagate_state()

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 172 of file ahrs_float_mlkf.c.

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

Referenced by ahrs_mlkf_propagate().

+ Here is the caller graph for this function:

◆ reset_state()

static void reset_state ( void  )
inlinestatic

Incorporate errors to reference and zeros state.

Definition at line 341 of file ahrs_float_mlkf.c.

References ahrs_mlkf, float_quat_comp(), float_quat_identity(), float_quat_normalize(), AhrsMlkf::gibbs_cor, AhrsMlkf::ltp_to_body_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:

◆ update_state()

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

Incorporate one 3D vector measurement.

Parameters
i_expectedexpected 3d vector in inertial frame
b_measuredmeasured 3d vector in body/imu frame
noisemeasurement noise vector (diagonal of covariance)

Definition at line 206 of file ahrs_float_mlkf.c.

References ahrs_mlkf, float_quat_vmult(), AhrsMlkf::gibbs_cor, AhrsMlkf::gyro_bias, H, K, AhrsMlkf::ltp_to_body_quat, MAT_INV33, MAT_MUL, MAT_MUL_T, MAT_SUB, FloatRates::p, AhrsMlkf::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:

◆ update_state_heading()

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_expectedexpected 3d vector in inertial frame
b_measuredmeasured 3d vector in body/imu frame
noisemeasurement noise vector (diagonal of covariance) TODO: optimize

Definition at line 274 of file ahrs_float_mlkf.c.

References ahrs_mlkf, float_quat_vmult(), AhrsMlkf::gibbs_cor, AhrsMlkf::gyro_bias, H, K, AhrsMlkf::ltp_to_body_quat, MAT_INV33, MAT_MUL, MAT_MUL_T, MAT_SUB, FloatRates::p, AhrsMlkf::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

◆ ahrs_mlkf