Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ins_float_invariant.h File Reference
+ Include dependency graph for ins_float_invariant.h:
+ This graph shows which files directly or indirectly include this file:

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  InsFloatInv
 Invariant filter structure. More...
 

Macros

#define INV_STATE_DIM   15
 Invariant filter state dimension. More...
 
#define INV_MEASURE_DIM   10
 Invariant filter measurement vector dimension. More...
 
#define INV_COMMAND_DIM   6
 Invariant filter command vector dimension. More...
 

Functions

void ins_float_invariant_init (void)
 
void ins_float_inv_set_body_to_imu_quat (struct FloatQuat *q_b2i)
 
void ins_float_invariant_align (struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
 
void ins_float_invariant_propagate (struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
 
void ins_float_invariant_update_mag (struct FloatVect3 *mag)
 
void ins_float_invariant_update_baro (float pressure)
 
void ins_float_invariant_update_gps (struct GpsState *gps_s)
 

Variables

struct InsFloatInv ins_float_inv
 

Detailed Description

INS using invariant filter. For more information, please send an email to "jp.condomines@gmail.com"

Definition in file ins_float_invariant.h.


Data Structure Documentation

◆ inv_state

struct inv_state

Invariant filter state.

Definition at line 40 of file ahrs_float_invariant.h.

+ Collaboration diagram for inv_state:
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.

◆ inv_measures

struct inv_measures

Invariant filter measurement vector.

Definition at line 53 of file ahrs_float_invariant.h.

+ Collaboration diagram for inv_measures:
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.

◆ inv_command

struct inv_command

Invariant filter command vector.

Definition at line 64 of file ahrs_float_invariant.h.

+ Collaboration diagram for inv_command:
Data Fields
struct FloatVect3 accel Input accelerometers.
struct FloatRates rates Input gyro rates.

◆ inv_correction_gains

struct inv_correction_gains

Invariant filter correction gains.

Definition at line 70 of file ahrs_float_invariant.h.

+ Collaboration diagram for inv_correction_gains:
Data Fields
struct FloatVect3 LE Correction gains on attitude.
struct FloatVect3 ME Correction gains on gyro biases.

Correction gains on speed.

struct FloatVect3 NE Correction gains on position.
float NE Correction gains on accel bias.
struct FloatVect3 OE Correction gains on gyro biases.
float OE Correction gains on magnetometer sensitivity.
float RE Correction gains on accel bias.
float SE Correction gains on barometer bias.

◆ inv_gains

struct inv_gains

Invariant filter tuning gains.

Definition at line 79 of file ahrs_float_invariant.h.

Data Fields
float lb Tuning parameter of mag error on attitude.
float lv Tuning parameter of speed error on attitude.
float lx Tuning parameter of accel and mag on attitude (longitudinal subsystem)
float ly Tuning parameter of accel and mag on attitude (lateral subsystem)
float lz Tuning parameter of accel and mag on attitude (heading subsystem)
float mh Tuning parameter of baro error on vertical speed.
float mv Tuning parameter of horizontal speed error on speed.
float mvz Tuning parameter of vertical speed error on speed.
float mx Tuning parameter of accel and mag on gyro bias (longitudinal subsystem)
float my Tuning parameter of accel and mag on gyro bias (lateral subsystem)
float mz Tuning parameter of accel and mag on gyro bias (heading subsystem)
float n Tuning parameter of accel and mag on accel bias (scaling subsystem)
float nh Tuning parameter of baro error on vertical position.
float nx Tuning parameter of horizontal position error on position.
float nxz Tuning parameter of vertical position error on position.
float o Tuning parameter of accel and mag on mag bias (scaling subsystem)
float ob Tuning parameter of mag error on gyro biases.
float ov Tuning parameter of speed error on gyro biases.
float rh Tuning parameter of baro error on accel biases (vertical projection)
float rv Tuning parameter of speed error on accel biases.
float sh Tuning parameter of baro error on baro bias.

◆ InsFloatInv

struct InsFloatInv

Invariant filter structure.

Definition at line 106 of file ins_float_invariant.h.

+ Collaboration diagram for InsFloatInv:
Data Fields
struct OrientationReps body_to_imu body_to_imu rotation
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 filter
struct inv_state state state vector

Macro Definition Documentation

◆ INV_COMMAND_DIM

#define INV_COMMAND_DIM   6

Invariant filter command vector dimension.

Definition at line 66 of file ins_float_invariant.h.

◆ INV_MEASURE_DIM

#define INV_MEASURE_DIM   10

Invariant filter measurement vector dimension.

Definition at line 53 of file ins_float_invariant.h.

◆ INV_STATE_DIM

#define INV_STATE_DIM   15

Invariant filter state dimension.

Definition at line 37 of file ins_float_invariant.h.

Function Documentation

◆ ins_float_inv_set_body_to_imu_quat()

void ins_float_inv_set_body_to_imu_quat ( struct FloatQuat q_b2i)

Definition at line 713 of file ins_float_invariant.c.

References InsFloatInv::body_to_imu, ins_float_inv, InsFloatInv::is_aligned, orientationSetQuat_f(), inv_state::quat, and InsFloatInv::state.

Referenced by body_to_imu_cb().

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

◆ ins_float_invariant_align()

void ins_float_invariant_align ( struct FloatRates lp_gyro,
struct FloatVect3 lp_accel,
struct FloatVect3 lp_mag 
)

Definition at line 315 of file ins_float_invariant.c.

References ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), inv_state::bias, ins_float_inv, InsFloatInv::is_aligned, inv_state::quat, InsFloatInv::state, and stateSetNedToBodyQuat_f().

Referenced by aligner_cb().

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

◆ ins_float_invariant_init()

◆ ins_float_invariant_propagate()

◆ ins_float_invariant_update_baro()

void ins_float_invariant_update_baro ( float  pressure)

Definition at line 491 of file ins_float_invariant.c.

References alpha, inv_measures::baro_alt, ins_baro_initialized, ins_float_inv, InsFloatInv::meas, pprz_isa_height_of_pressure(), and PPRZ_ISA_SEA_LEVEL_PRESSURE.

Referenced by baro_cb().

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

◆ ins_float_invariant_update_gps()

◆ ins_float_invariant_update_mag()

void ins_float_invariant_update_mag ( struct FloatVect3 mag)

Definition at line 527 of file ins_float_invariant.c.

References InsFloatInv::body_to_imu, float_rmat_transp_vmult(), FLOAT_VECT3_ZERO, ins_float_inv, inv_measures::mag, MAG_FROZEN_COUNT, InsFloatInv::meas, orientationGetRMat_f(), and FloatVect3::x.

Referenced by ins_float_invariant_update_gps(), and mag_cb().

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

Variable Documentation

◆ ins_float_inv