Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_mekf_wind.h File Reference

Multiplicative Extended Kalman Filter in rotation matrix formulation. More...

#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
+ Include dependency graph for ins_mekf_wind.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures

struct  ins_mekf_wind_parameters
 

Macros

#define ins_mekf_wind_update_Q_gyro(_v)
 
#define ins_mekf_wind_update_Q_accel(_v)
 
#define ins_mekf_wind_update_Q_rates_bias(_v)
 
#define ins_mekf_wind_update_Q_accel_bias(_v)
 
#define ins_mekf_wind_update_Q_baro_bias(_v)
 
#define ins_mekf_wind_update_Q_wind(_v)
 
#define ins_mekf_wind_update_R_speed(_v)
 
#define ins_mekf_wind_update_R_pos(_v)
 
#define ins_mekf_wind_update_R_speed_z(_v)
 
#define ins_mekf_wind_update_R_pos_z(_v)
 
#define ins_mekf_wind_update_R_mag(_v)
 
#define ins_mekf_wind_update_R_baro(_v)
 
#define ins_mekf_wind_update_R_airspeed(_v)
 
#define ins_mekf_wind_update_R_aoa(_v)
 
#define ins_mekf_wind_update_R_aos(_v)
 

Functions

void ins_mekf_wind_init (void)
 Init function. More...
 
void ins_mekf_wind_align (struct FloatRates *gyro_bias, struct FloatQuat *quat)
 
void ins_mekf_wind_set_mag_h (const struct FloatVect3 *mag_h)
 
void ins_mekf_wind_reset (void)
 
void ins_mekf_wind_propagate (struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
 Full INS propagation. More...
 
void ins_mekf_wind_propagate_ahrs (struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
 AHRS-only propagation + accel correction. More...
 
void ins_mekf_wind_update_mag (struct FloatVect3 *mag, bool attitude_only)
 
void ins_mekf_wind_update_baro (float baro_alt)
 
void ins_mekf_wind_update_pos_speed (struct FloatVect3 *pos, struct FloatVect3 *speed)
 
void ins_mekf_wind_update_airspeed (float airspeed)
 
void ins_mekf_wind_update_incidence (float aoa, float aos)
 
struct NedCoor_f ins_mekf_wind_get_pos_ned (void)
 Getter/Setter functions. More...
 
void ins_mekf_wind_set_pos_ned (struct NedCoor_f *p)
 
struct NedCoor_f ins_mekf_wind_get_speed_ned (void)
 
void ins_mekf_wind_set_speed_ned (struct NedCoor_f *s)
 
struct NedCoor_f ins_mekf_wind_get_accel_ned (void)
 
struct FloatQuat ins_mekf_wind_get_quat (void)
 
void ins_mekf_wind_set_quat (struct FloatQuat *quat)
 
struct FloatRates ins_mekf_wind_get_body_rates (void)
 
struct NedCoor_f ins_mekf_wind_get_wind_ned (void)
 
struct NedCoor_f ins_mekf_wind_get_airspeed_body (void)
 
float ins_mekf_wind_get_airspeed_norm (void)
 
struct FloatVect3 ins_mekf_wind_get_accel_bias (void)
 
struct FloatRates ins_mekf_wind_get_rates_bias (void)
 
float ins_mekf_wind_get_baro_bias (void)
 
void ins_mekf_wind_update_params (void)
 

Variables

struct ins_mekf_wind_parameters ins_mekf_wind_params
 

Detailed Description

Multiplicative Extended Kalman Filter in rotation matrix formulation.

Estimate attitude, ground speed, position, gyro bias, accelerometer bias and wind speed.

Definition in file ins_mekf_wind.h.


Data Structure Documentation

struct ins_mekf_wind_parameters

Definition at line 42 of file ins_mekf_wind.h.

Data Fields
bool disable_wind disable wind estimation
float Q_accel accel process noise
float Q_accel_bias accel bias process noise
float Q_baro_bias baro bias process noise
float Q_gyro gyro process noise
float Q_rates_bias rates bias process noise
float Q_wind wind process noise
float R_airspeed airspeed measurement noise
float R_aoa angle of attack measurement noise
float R_aos sideslip angle measurement noise
float R_baro baro measurement noise
float R_mag mag measurement noise
float R_pos pos measurement noise
float R_pos_z vertical pos measurement noise
float R_speed speed measurement noise
float R_speed_z vertical speed measurement noise

Macro Definition Documentation

#define ins_mekf_wind_update_Q_accel (   _v)
Value:
{ \
}
float Q_accel
accel process noise
Definition: ins_mekf_wind.h:44
void ins_mekf_wind_update_params(void)
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 106 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_Q_accel_bias (   _v)
Value:
{ \
}
void ins_mekf_wind_update_params(void)
float Q_accel_bias
accel bias process noise
Definition: ins_mekf_wind.h:46
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 116 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_Q_baro_bias (   _v)
Value:
{ \
}
float Q_baro_bias
baro bias process noise
Definition: ins_mekf_wind.h:47
void ins_mekf_wind_update_params(void)
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 121 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_Q_gyro (   _v)
Value:
{ \
}
void ins_mekf_wind_update_params(void)
float Q_gyro
gyro process noise
Definition: ins_mekf_wind.h:43
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 101 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_Q_rates_bias (   _v)
Value:
{ \
}
void ins_mekf_wind_update_params(void)
float Q_rates_bias
rates bias process noise
Definition: ins_mekf_wind.h:45
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 111 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_Q_wind (   _v)
Value:
{ \
}
float Q_wind
wind process noise
Definition: ins_mekf_wind.h:48
void ins_mekf_wind_update_params(void)
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 126 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_R_airspeed (   _v)
Value:
{ \
}
float R_airspeed
airspeed measurement noise
Definition: ins_mekf_wind.h:55
void ins_mekf_wind_update_params(void)
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 161 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_R_aoa (   _v)
Value:
{ \
}
void ins_mekf_wind_update_params(void)
float R_aoa
angle of attack measurement noise
Definition: ins_mekf_wind.h:56
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 166 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_R_aos (   _v)
Value:
{ \
}
void ins_mekf_wind_update_params(void)
float R_aos
sideslip angle measurement noise
Definition: ins_mekf_wind.h:57
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 171 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_R_baro (   _v)
Value:
{ \
}
float R_baro
baro measurement noise
Definition: ins_mekf_wind.h:54
void ins_mekf_wind_update_params(void)
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 156 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_R_mag (   _v)
Value:
{ \
}
float R_mag
mag measurement noise
Definition: ins_mekf_wind.h:53
void ins_mekf_wind_update_params(void)
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 151 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_R_pos (   _v)
Value:
{ \
}
float R_pos
pos measurement noise
Definition: ins_mekf_wind.h:50
void ins_mekf_wind_update_params(void)
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 136 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_R_pos_z (   _v)
Value:
{ \
}
void ins_mekf_wind_update_params(void)
float R_pos_z
vertical pos measurement noise
Definition: ins_mekf_wind.h:52
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 146 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_R_speed (   _v)
Value:
{ \
}
float R_speed
speed measurement noise
Definition: ins_mekf_wind.h:49
void ins_mekf_wind_update_params(void)
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 131 of file ins_mekf_wind.h.

#define ins_mekf_wind_update_R_speed_z (   _v)
Value:
{ \
}
float R_speed_z
vertical speed measurement noise
Definition: ins_mekf_wind.h:51
void ins_mekf_wind_update_params(void)
struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 141 of file ins_mekf_wind.h.

Function Documentation

void ins_mekf_wind_align ( struct FloatRates gyro_bias,
struct FloatQuat quat 
)

Definition at line 487 of file ins_mekf_wind.cpp.

References mwp, FloatRates::p, FloatRates::q, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and FloatRates::r.

Referenced by aligner_cb().

+ Here is the caller graph for this function:

struct FloatVect3 ins_mekf_wind_get_accel_bias ( void  )

Definition at line 821 of file ins_mekf_wind.cpp.

References mwp, and FloatVect3::x.

Referenced by gyro_cb(), and send_inv_filter().

+ Here is the caller graph for this function:

struct NedCoor_f ins_mekf_wind_get_accel_ned ( void  )

Definition at line 755 of file ins_mekf_wind.cpp.

References mwp, and NedCoor_f::x.

Referenced by gyro_cb(), and set_state_from_ins().

+ Here is the caller graph for this function:

struct NedCoor_f ins_mekf_wind_get_airspeed_body ( void  )

Definition at line 804 of file ins_mekf_wind.cpp.

References mwp, and NedCoor_f::x.

float ins_mekf_wind_get_airspeed_norm ( void  )

Definition at line 816 of file ins_mekf_wind.cpp.

References mwp.

Referenced by gyro_cb(), send_inv_filter(), and send_wind().

+ Here is the caller graph for this function:

float ins_mekf_wind_get_baro_bias ( void  )

Definition at line 841 of file ins_mekf_wind.cpp.

References mwp.

Referenced by gyro_cb().

+ Here is the caller graph for this function:

struct FloatRates ins_mekf_wind_get_body_rates ( void  )

Definition at line 784 of file ins_mekf_wind.cpp.

References mwp, FloatRates::p, and FloatRates::r.

Referenced by gyro_cb(), and set_state_from_ins().

+ Here is the caller graph for this function:

struct NedCoor_f ins_mekf_wind_get_pos_ned ( void  )

Getter/Setter functions.

Definition at line 721 of file ins_mekf_wind.cpp.

References mwp, p, and NedCoor_f::x.

Referenced by gyro_cb(), send_inv_filter(), and set_state_from_ins().

+ Here is the caller graph for this function:

struct FloatQuat ins_mekf_wind_get_quat ( void  )

Definition at line 765 of file ins_mekf_wind.cpp.

References mwp, and FloatQuat::qi.

Referenced by gyro_cb(), send_euler(), send_inv_filter(), and set_state_from_ins().

+ Here is the caller graph for this function:

struct FloatRates ins_mekf_wind_get_rates_bias ( void  )

Definition at line 831 of file ins_mekf_wind.cpp.

References mwp, and FloatRates::p.

Referenced by gyro_cb(), and send_inv_filter().

+ Here is the caller graph for this function:

struct NedCoor_f ins_mekf_wind_get_speed_ned ( void  )

Definition at line 738 of file ins_mekf_wind.cpp.

References mwp, s, and NedCoor_f::x.

Referenced by gyro_cb(), send_inv_filter(), and set_state_from_ins().

+ Here is the caller graph for this function:

struct NedCoor_f ins_mekf_wind_get_wind_ned ( void  )

Definition at line 794 of file ins_mekf_wind.cpp.

References mwp, and NedCoor_f::x.

Referenced by gyro_cb(), and send_wind().

+ Here is the caller graph for this function:

void ins_mekf_wind_propagate_ahrs ( struct FloatRates gyro,
struct FloatVect3 accel,
float  dt 
)

AHRS-only propagation + accel correction.

Definition at line 422 of file ins_mekf_wind.cpp.

References A, MekfWindInputs::accel, gravity(), H, InsMekfWindPrivate::inputs, K, mekf_wind_private, MEKF_WIND_qgp, MEKF_WIND_qrbp, MEKF_WIND_qx, MEKF_WIND_rbp, MEKF_WIND_rmx, mwp, FloatRates::p, FloatRates::q, quat_add(), quat_smul(), FloatRates::r, MekfWindInputs::rates, skew_sym(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by gyro_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ins_mekf_wind_reset ( void  )

Definition at line 343 of file ins_mekf_wind.cpp.

References init_mekf_state().

+ Here is the call graph for this function:

void ins_mekf_wind_set_mag_h ( const struct FloatVect3 mag_h)

Definition at line 335 of file ins_mekf_wind.cpp.

References InsMekfWindPrivate::mag_h, mekf_wind_private, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by geo_mag_cb(), and ins_mekf_wind_wrapper_init().

+ Here is the caller graph for this function:

void ins_mekf_wind_set_pos_ned ( struct NedCoor_f p)

Definition at line 731 of file ins_mekf_wind.cpp.

References mwp, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by gps_cb().

+ Here is the caller graph for this function:

void ins_mekf_wind_set_quat ( struct FloatQuat quat)

Definition at line 776 of file ins_mekf_wind.cpp.

References mwp, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, and FloatQuat::qz.

Referenced by body_to_imu_cb().

+ Here is the caller graph for this function:

void ins_mekf_wind_set_speed_ned ( struct NedCoor_f s)

Definition at line 748 of file ins_mekf_wind.cpp.

References mwp, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by gps_cb().

+ Here is the caller graph for this function:

void ins_mekf_wind_update_airspeed ( float  airspeed)

Definition at line 614 of file ins_mekf_wind.cpp.

References ins_mekf_wind_parameters::disable_wind, H, ins_mekf_wind_params, K, MEKF_WIND_abx, MEKF_WIND_bb, MEKF_WIND_px, MEKF_WIND_qx, MEKF_WIND_ras, MEKF_WIND_rbp, MEKF_WIND_vx, MEKF_WIND_wx, mwp, and skew_sym().

Referenced by airspeed_cb(), and pressure_diff_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ins_mekf_wind_update_baro ( float  baro_alt)
void ins_mekf_wind_update_incidence ( float  aoa,
float  aos 
)

Definition at line 652 of file ins_mekf_wind.cpp.

References B, ins_mekf_wind_parameters::disable_wind, H, ins_mekf_wind_params, K, MEKF_WIND_abx, MEKF_WIND_bb, MEKF_WIND_px, MEKF_WIND_qx, MEKF_WIND_raoa, MEKF_WIND_rbp, MEKF_WIND_vx, MEKF_WIND_wx, mwp, and skew_sym().

Referenced by incidence_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ins_mekf_wind_update_mag ( struct FloatVect3 mag,
bool  attitude_only 
)

Definition at line 501 of file ins_mekf_wind.cpp.

References ins_mekf_wind_parameters::disable_wind, H, ins_mekf_wind_params, K, MEKF_WIND_abx, MEKF_WIND_bb, MEKF_WIND_px, MEKF_WIND_qx, MEKF_WIND_rbp, MEKF_WIND_rmx, MEKF_WIND_vx, MEKF_WIND_wx, mwp, skew_sym(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by mag_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ins_mekf_wind_update_pos_speed ( struct FloatVect3 pos,
struct FloatVect3 speed 
)

Variable Documentation