Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
AHRS using invariant filter. More...
#include "modules/ahrs/ahrs.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"
Go to the source code of this file.
Data Structures | |
struct | inv_state |
Invariant filter state. More... | |
struct | inv_measures |
Invariant filter measurement vector. More... | |
struct | inv_command |
Invariant filter command vector. More... | |
struct | inv_correction_gains |
Invariant filter correction gains. More... | |
struct | inv_gains |
Invariant filter tuning gains. More... | |
struct | AhrsFloatInv |
Invariant filter structure. More... | |
Macros | |
#define | INV_STATE_DIM 9 |
Invariant filter state dimension. More... | |
#define | INV_MEASURE_DIM 6 |
Invariant filter measurement vector dimension. More... | |
#define | INV_COMMAND_DIM 3 |
Invariant filter command vector dimension. More... | |
Functions | |
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) |
Variables | |
struct AhrsFloatInv | ahrs_float_inv |
AHRS using invariant filter.
For more information, please send an email to "jp.condomines@gmail.com"
Definition in file ahrs_float_invariant.h.
struct inv_state |
Invariant filter state.
Definition at line 40 of file ahrs_float_invariant.h.
Data Fields | ||
---|---|---|
float | as | Estimated accelerometer sensitivity. |
struct FloatRates | bias | Estimated gyro biases. |
float | cs | Estimates magnetometer sensitivity. |
float | hb | Estimates barometers bias. |
struct NedCoor_f | pos | Estimates position. |
struct FloatQuat | quat | Estimated attitude (quaternion) |
struct NedCoor_f | speed | Estimates speed. |
struct inv_measures |
Invariant filter measurement vector.
Definition at line 53 of file ahrs_float_invariant.h.
Data Fields | ||
---|---|---|
struct FloatVect3 | accel | Measured accelerometers. |
float | baro_alt | Measured barometric altitude. |
struct FloatVect3 | mag | Measured magnetic field. |
struct NedCoor_f | pos_gps | Measured gps position. |
struct NedCoor_f | speed_gps | Measured gps speed. |
struct inv_command |
Invariant filter command vector.
Definition at line 64 of file ahrs_float_invariant.h.
Data Fields | ||
---|---|---|
struct FloatVect3 | accel | Input accelerometers. |
struct FloatRates | rates | Input gyro rates. |
struct inv_correction_gains |
Invariant filter correction gains.
Definition at line 70 of file ahrs_float_invariant.h.
Data Fields | ||
---|---|---|
struct FloatVect3 | LE | Correction gains on attitude. |
struct FloatVect3 | ME |
Correction gains on gyro biases. Correction gains on speed. |
float | NE | Correction gains on accel bias. |
struct FloatVect3 | NE | Correction gains on position. |
float | OE | Correction gains on magnetometer sensitivity. |
struct FloatVect3 | OE | Correction gains on gyro biases. |
float | RE | Correction gains on accel bias. |
float | SE | Correction gains on barometer bias. |
struct inv_gains |
Invariant filter tuning gains.
Definition at line 79 of file ahrs_float_invariant.h.
struct AhrsFloatInv |
Invariant filter structure.
Definition at line 92 of file ahrs_float_invariant.h.
Data Fields | ||
---|---|---|
struct inv_command | cmd | command vector |
struct inv_correction_gains | corr | correction gains |
struct inv_gains | gains | tuning gains |
bool | is_aligned | |
struct FloatVect3 | mag_h | |
struct inv_measures | meas | measurement vector |
bool | reset | flag to request reset/reinit the filte |
struct inv_state | state | state vector |
#define INV_COMMAND_DIM 3 |
Invariant filter command vector dimension.
Definition at line 60 of file ahrs_float_invariant.h.
#define INV_MEASURE_DIM 6 |
Invariant filter measurement vector dimension.
Definition at line 49 of file ahrs_float_invariant.h.
#define INV_STATE_DIM 9 |
Invariant filter state dimension.
Definition at line 36 of file ahrs_float_invariant.h.
void ahrs_float_invariant_align | ( | struct FloatRates * | lp_gyro, |
struct FloatVect3 * | lp_accel, | ||
struct FloatVect3 * | lp_mag | ||
) |
Definition at line 156 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().
void ahrs_float_invariant_init | ( | void | ) |
Definition at line 131 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_finv_wrapper_init().
void ahrs_float_invariant_propagate | ( | struct FloatRates * | gyro, |
float | dt | ||
) |
Definition at line 170 of file ahrs_float_invariant.c.
References ahrs_float_inv, inv_state::as, inv_state::bias, AhrsFloatInv::cmd, inv_state::cs, DefaultChannel, DefaultDevice, error_output(), float_eulers_of_quat(), float_quat_normalize(), foo, init_invariant_state(), INV_COMMAND_DIM, INV_STATE_DIM, invariant_model(), AhrsFloatInv::is_aligned, FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatQuat::qi, inv_state::quat, FloatRates::r, inv_command::rates, RATES_COPY, AhrsFloatInv::reset, runge_kutta_4_float(), AhrsFloatInv::state, and FloatEulers::theta.
Referenced by gyro_cb().
void ahrs_float_invariant_update_accel | ( | struct FloatVect3 * | accel | ) |
Definition at line 228 of file ahrs_float_invariant.c.
References inv_measures::accel, ahrs_float_inv, and AhrsFloatInv::meas.
Referenced by accel_cb().
void ahrs_float_invariant_update_mag | ( | struct FloatVect3 * | mag | ) |
Definition at line 236 of file ahrs_float_invariant.c.
References ahrs_float_inv, FLOAT_VECT3_ZERO, inv_measures::mag, MAG_FROZEN_COUNT, AhrsFloatInv::meas, VECT3_COPY, and FloatVect3::x.
Referenced by mag_cb().
|
extern |
Definition at line 1 of file ahrs_float_invariant.c.
Referenced by accel_cb(), 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().