Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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_aligner.h"
#include "subsystems/ahrs/ahrs_float_utils.h"
#include <float.h>
#include <string.h>
#include <math.h>
#include "state.h"
#include "subsystems/imu.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_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY
 

Functions

static void propagate_ref (void)
 
static void propagate_state (void)
 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, const float noise[])
 Incorporate one 3D vector measurement. More...
 
static void reset_state (void)
 Incorporate errors to reference and zeros state. More...
 
static void set_body_state_from_quat (void)
 Compute body orientation and rates from imu orientation and rates. More...
 
void ahrs_init (void)
 AHRS initialization. More...
 
void ahrs_align (void)
 Aligns the AHRS. More...
 
void ahrs_propagate (void)
 Propagation. More...
 
void ahrs_update_accel (void)
 Update AHRS state with accerleration measurements. More...
 
void ahrs_update_mag (void)
 Update AHRS state with magnetometer measurements. More...
 

Variables

struct AhrsMlkf ahrs_impl
 

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_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY

Definition at line 50 of file ahrs_float_mlkf.c.

Referenced by propagate_ref(), and propagate_state().

Function Documentation

void ahrs_align ( void  )

Aligns the AHRS.

Called after ahrs_aligner has run to set initial attitude and biases. Must set the ahrs status to AHRS_RUNNING. Needs to be implemented by each AHRS algorithm.

Definition at line 97 of file ahrs_float_mlkf.c.

References ahrs, ahrs_aligner, ahrs_float_get_quat_from_accel_mag(), ahrs_impl, AHRS_RUNNING, FLOAT_RMAT_OF_QUAT, AhrsMlkf::gyro_bias, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, AhrsAligner::lp_mag, AhrsMlkf::ltp_to_imu_quat, AhrsMlkf::ltp_to_imu_rmat, RATES_COPY, RATES_FLOAT_OF_BFP, set_body_state_from_quat(), and Ahrs::status.

+ Here is the call graph for this function:

void ahrs_propagate ( void  )

Propagation.

Usually integrates the gyro rates to angles. Reads the global imu data struct. Needs to be implemented by each AHRS algorithm.

Definition at line 116 of file ahrs_float_mlkf.c.

References propagate_ref(), propagate_state(), and set_body_state_from_quat().

+ Here is the call graph for this function:

void ahrs_update_accel ( void  )

Update AHRS state with accerleration measurements.

Reads the global imu data struct. Needs to be implemented by each AHRS algorithm.

Definition at line 122 of file ahrs_float_mlkf.c.

References Imu::accel, ACCELS_FLOAT_OF_BFP, ahrs_impl, dn, FLOAT_VECT3_NORM, imu, AhrsMlkf::lp_accel, reset_state(), and update_state().

+ Here is the call graph for this function:

void ahrs_update_mag ( void  )

Update AHRS state with magnetometer measurements.

Reads the global imu data struct. Needs to be implemented by each AHRS algorithm.

Definition at line 137 of file ahrs_float_mlkf.c.

References AHRS_H_X, AHRS_H_Y, imu, Imu::mag, MAGS_FLOAT_OF_BFP, reset_state(), and update_state().

+ Here is the call graph for this function:

static void propagate_state ( void  )
inlinestatic

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

Definition at line 195 of file ahrs_float_mlkf.c.

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

Referenced by ahrs_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 280 of file ahrs_float_mlkf.c.

References ahrs_impl, FLOAT_QUAT_COMP, FLOAT_QUAT_NORMALIZE, FLOAT_QUAT_ZERO, FLOAT_RMAT_OF_QUAT, AhrsMlkf::gibbs_cor, AhrsMlkf::ltp_to_imu_quat, AhrsMlkf::ltp_to_imu_rmat, and FloatQuat::qi.

Referenced by ahrs_update_accel(), and ahrs_update_mag().

+ Here is the caller graph for this function:

static void set_body_state_from_quat ( void  )
inlinestatic

Compute body orientation and rates from imu orientation and rates.

Definition at line 295 of file ahrs_float_mlkf.c.

References ahrs_impl, AhrsMlkf::body_to_imu_quat, AhrsMlkf::body_to_imu_rmat, FLOAT_QUAT_COMP_INV, FLOAT_RMAT_TRANSP_RATEMULT, AhrsMlkf::imu_rate, AhrsMlkf::ltp_to_imu_quat, stateSetBodyRates_f(), and stateSetNedToBodyQuat_f().

Referenced by ahrs_align(), and ahrs_propagate().

+ 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,
const float  noise[] 
)
inlinestatic

Variable Documentation