Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Data Structures | Macros | Functions | Variables
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

◆ ins_mekf_wind_parameters

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

◆ ins_mekf_wind_update_Q_accel

#define ins_mekf_wind_update_Q_accel (   _v)
Value:
{ \
ins_mekf_wind_params.Q_accel = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 106 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_Q_accel_bias

#define ins_mekf_wind_update_Q_accel_bias (   _v)
Value:
{ \
ins_mekf_wind_params.Q_accel_bias = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 116 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_Q_baro_bias

#define ins_mekf_wind_update_Q_baro_bias (   _v)
Value:
{ \
ins_mekf_wind_params.Q_baro_bias = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 121 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_Q_gyro

#define ins_mekf_wind_update_Q_gyro (   _v)
Value:
{ \
ins_mekf_wind_params.Q_gyro = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 101 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_Q_rates_bias

#define ins_mekf_wind_update_Q_rates_bias (   _v)
Value:
{ \
ins_mekf_wind_params.Q_rates_bias = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 111 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_Q_wind

#define ins_mekf_wind_update_Q_wind (   _v)
Value:
{ \
ins_mekf_wind_params.Q_wind = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 126 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_R_airspeed

#define ins_mekf_wind_update_R_airspeed (   _v)
Value:
{ \
ins_mekf_wind_params.R_airspeed = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 161 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_R_aoa

#define ins_mekf_wind_update_R_aoa (   _v)
Value:
{ \
ins_mekf_wind_params.R_aoa = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 166 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_R_aos

#define ins_mekf_wind_update_R_aos (   _v)
Value:
{ \
ins_mekf_wind_params.R_aos = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 171 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_R_baro

#define ins_mekf_wind_update_R_baro (   _v)
Value:
{ \
ins_mekf_wind_params.R_baro = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 156 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_R_mag

#define ins_mekf_wind_update_R_mag (   _v)
Value:
{ \
ins_mekf_wind_params.R_mag = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 151 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_R_pos

#define ins_mekf_wind_update_R_pos (   _v)
Value:
{ \
ins_mekf_wind_params.R_pos = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 136 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_R_pos_z

#define ins_mekf_wind_update_R_pos_z (   _v)
Value:
{ \
ins_mekf_wind_params.R_pos_z = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 146 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_R_speed

#define ins_mekf_wind_update_R_speed (   _v)
Value:
{ \
ins_mekf_wind_params.R_speed = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 131 of file ins_mekf_wind.h.

◆ ins_mekf_wind_update_R_speed_z

#define ins_mekf_wind_update_R_speed_z (   _v)
Value:
{ \
ins_mekf_wind_params.R_speed_z = _v; \
ins_mekf_wind_update_params(); \
}

Definition at line 141 of file ins_mekf_wind.h.

Function Documentation

◆ ins_mekf_wind_align()

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:

◆ ins_mekf_wind_get_accel_bias()

struct FloatVect3 ins_mekf_wind_get_accel_bias ( void  )

Definition at line 816 of file ins_mekf_wind.cpp.

References mwp.

Referenced by gyro_cb(), and send_inv_filter().

+ Here is the caller graph for this function:

◆ ins_mekf_wind_get_accel_ned()

struct NedCoor_f ins_mekf_wind_get_accel_ned ( void  )

Definition at line 748 of file ins_mekf_wind.cpp.

References mwp, and s.

Referenced by gyro_cb(), and set_state_from_ins().

+ Here is the caller graph for this function:

◆ ins_mekf_wind_get_airspeed_body()

struct NedCoor_f ins_mekf_wind_get_airspeed_body ( void  )

Definition at line 776 of file ins_mekf_wind.cpp.

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

◆ ins_mekf_wind_get_airspeed_norm()

float ins_mekf_wind_get_airspeed_norm ( void  )

Definition at line 816 of file ins_mekf_wind.cpp.

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

+ Here is the caller graph for this function:

◆ ins_mekf_wind_get_baro_bias()

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:

◆ ins_mekf_wind_get_body_rates()

struct FloatRates ins_mekf_wind_get_body_rates ( void  )

Definition at line 776 of file ins_mekf_wind.cpp.

Referenced by gyro_cb(), and set_state_from_ins().

+ Here is the caller graph for this function:

◆ ins_mekf_wind_get_pos_ned()

struct NedCoor_f ins_mekf_wind_get_pos_ned ( void  )

Getter/Setter functions.

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 gyro_cb(), send_inv_filter(), and set_state_from_ins().

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

◆ ins_mekf_wind_get_quat()

struct FloatQuat ins_mekf_wind_get_quat ( void  )

Definition at line 748 of file ins_mekf_wind.cpp.

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

+ Here is the caller graph for this function:

◆ ins_mekf_wind_get_rates_bias()

struct FloatRates ins_mekf_wind_get_rates_bias ( void  )

Definition at line 816 of file ins_mekf_wind.cpp.

Referenced by gyro_cb(), and send_inv_filter().

+ Here is the caller graph for this function:

◆ ins_mekf_wind_get_speed_ned()

struct NedCoor_f ins_mekf_wind_get_speed_ned ( void  )

Definition at line 731 of file ins_mekf_wind.cpp.

References mwp, and p.

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

+ Here is the caller graph for this function:

◆ ins_mekf_wind_get_wind_ned()

struct NedCoor_f ins_mekf_wind_get_wind_ned ( void  )

Definition at line 776 of file ins_mekf_wind.cpp.

Referenced by gyro_cb(), and send_wind().

+ Here is the caller graph for this function:

◆ ins_mekf_wind_init()

void ins_mekf_wind_init ( void  )

Init function.

Definition at line 308 of file ins_mekf_wind.cpp.

References ins_mekf_wind_parameters::disable_wind, init_mekf_state(), INS_MEKF_WIND_DISABLE_WIND, ins_mekf_wind_params, INS_MEKF_WIND_Q_ACCEL, INS_MEKF_WIND_Q_ACCEL_BIAS, INS_MEKF_WIND_Q_BARO_BIAS, INS_MEKF_WIND_Q_GYRO, INS_MEKF_WIND_Q_RATES_BIAS, INS_MEKF_WIND_Q_WIND, INS_MEKF_WIND_R_AIRSPEED, INS_MEKF_WIND_R_AOA, INS_MEKF_WIND_R_AOS, INS_MEKF_WIND_R_BARO, INS_MEKF_WIND_R_MAG, INS_MEKF_WIND_R_POS, INS_MEKF_WIND_R_POS_Z, INS_MEKF_WIND_R_SPEED, INS_MEKF_WIND_R_SPEED_Z, InsMekfWindPrivate::mag_h, mekf_wind_private, ins_mekf_wind_parameters::Q_accel, ins_mekf_wind_parameters::Q_accel_bias, ins_mekf_wind_parameters::Q_baro_bias, ins_mekf_wind_parameters::Q_gyro, ins_mekf_wind_parameters::Q_rates_bias, ins_mekf_wind_parameters::Q_wind, ins_mekf_wind_parameters::R_airspeed, ins_mekf_wind_parameters::R_aoa, ins_mekf_wind_parameters::R_aos, ins_mekf_wind_parameters::R_baro, ins_mekf_wind_parameters::R_mag, ins_mekf_wind_parameters::R_pos, ins_mekf_wind_parameters::R_pos_z, ins_mekf_wind_parameters::R_speed, and ins_mekf_wind_parameters::R_speed_z.

Referenced by ins_mekf_wind_wrapper_init().

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

◆ ins_mekf_wind_propagate()

void ins_mekf_wind_propagate ( struct FloatRates *  gyro,
struct FloatVect3 *  accel,
float  dt 
)

Full INS propagation.

Definition at line 350 of file ins_mekf_wind.cpp.

References A, MekfWindInputs::accel, ins_mekf_wind_parameters::disable_wind, gravity(), InsMekfWindPrivate::inputs, INS_MEKF_WIND_P0_WIND, ins_mekf_wind_params, MEKF_WIND_abx, MEKF_WIND_bb, MEKF_WIND_COV_SIZE, mekf_wind_private, MEKF_WIND_px, MEKF_WIND_qabx, MEKF_WIND_qax, MEKF_WIND_qbb, MEKF_WIND_qgp, MEKF_WIND_qrbp, MEKF_WIND_qwx, MEKF_WIND_qx, MEKF_WIND_rbp, MEKF_WIND_vx, MEKF_WIND_wx, MEKF_WIND_wy, MEKF_WIND_wz, 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:

◆ ins_mekf_wind_propagate_ahrs()

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:

◆ ins_mekf_wind_reset()

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:

◆ ins_mekf_wind_set_mag_h()

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, and mekf_wind_private.

Referenced by geo_mag_cb(), and ins_mekf_wind_wrapper_init().

+ Here is the caller graph for this function:

◆ ins_mekf_wind_set_pos_ned()

void ins_mekf_wind_set_pos_ned ( struct NedCoor_f *  p)

Definition at line 731 of file ins_mekf_wind.cpp.

Referenced by gps_cb().

+ Here is the caller graph for this function:

◆ ins_mekf_wind_set_quat()

void ins_mekf_wind_set_quat ( struct FloatQuat *  quat)

Definition at line 776 of file ins_mekf_wind.cpp.

◆ ins_mekf_wind_set_speed_ned()

void ins_mekf_wind_set_speed_ned ( struct NedCoor_f *  s)

Definition at line 748 of file ins_mekf_wind.cpp.

Referenced by gps_cb().

+ Here is the caller graph for this function:

◆ ins_mekf_wind_update_airspeed()

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:

◆ ins_mekf_wind_update_baro()

void ins_mekf_wind_update_baro ( float  baro_alt)

Definition at line 541 of file ins_mekf_wind.cpp.

References baro_alt, ins_mekf_wind_parameters::disable_wind, H, ins_mekf_wind_params, K, MEKF_WIND_abx, MEKF_WIND_bb, MEKF_WIND_px, MEKF_WIND_pz, MEKF_WIND_qx, MEKF_WIND_rb, MEKF_WIND_rbp, MEKF_WIND_vx, MEKF_WIND_wx, and mwp.

Referenced by baro_cb().

+ Here is the caller graph for this function:

◆ ins_mekf_wind_update_incidence()

void ins_mekf_wind_update_incidence ( float  aoa,
float  aos 
)

Definition at line 652 of file ins_mekf_wind.cpp.

Referenced by incidence_cb().

+ Here is the caller graph for this function:

◆ ins_mekf_wind_update_mag()

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:

◆ ins_mekf_wind_update_params()

void ins_mekf_wind_update_params ( void  )

Definition at line 846 of file ins_mekf_wind.cpp.

References ins_mekf_wind_params, mekf_wind_private, MEKF_WIND_qabx, MEKF_WIND_qaby, MEKF_WIND_qabz, MEKF_WIND_qax, MEKF_WIND_qay, MEKF_WIND_qaz, MEKF_WIND_qbb, MEKF_WIND_qgp, MEKF_WIND_qgq, MEKF_WIND_qgr, MEKF_WIND_qrbp, MEKF_WIND_qrbq, MEKF_WIND_qrbr, MEKF_WIND_qwx, MEKF_WIND_qwy, MEKF_WIND_qwz, MEKF_WIND_raoa, MEKF_WIND_raos, MEKF_WIND_ras, MEKF_WIND_rb, MEKF_WIND_rmx, MEKF_WIND_rmy, MEKF_WIND_rmz, MEKF_WIND_rpx, MEKF_WIND_rpy, MEKF_WIND_rpz, MEKF_WIND_rvx, MEKF_WIND_rvy, MEKF_WIND_rvz, InsMekfWindPrivate::Q, ins_mekf_wind_parameters::Q_accel, ins_mekf_wind_parameters::Q_accel_bias, ins_mekf_wind_parameters::Q_baro_bias, ins_mekf_wind_parameters::Q_gyro, ins_mekf_wind_parameters::Q_rates_bias, ins_mekf_wind_parameters::Q_wind, InsMekfWindPrivate::R, ins_mekf_wind_parameters::R_airspeed, ins_mekf_wind_parameters::R_aoa, ins_mekf_wind_parameters::R_aos, ins_mekf_wind_parameters::R_baro, ins_mekf_wind_parameters::R_mag, ins_mekf_wind_parameters::R_pos, ins_mekf_wind_parameters::R_pos_z, ins_mekf_wind_parameters::R_speed, and ins_mekf_wind_parameters::R_speed_z.

Referenced by init_mekf_state().

+ Here is the caller graph for this function:

◆ ins_mekf_wind_update_pos_speed()

void ins_mekf_wind_update_pos_speed ( struct FloatVect3 *  pos,
struct FloatVect3 *  speed 
)

Definition at line 575 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_rvx, MEKF_WIND_vx, MEKF_WIND_wx, mwp, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by gps_cb().

+ Here is the caller graph for this function:

Variable Documentation

◆ ins_mekf_wind_params

struct ins_mekf_wind_parameters ins_mekf_wind_params
extern

Definition at line 89 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_get_pos_ned(), ins_mekf_wind_init(), ins_mekf_wind_propagate(), ins_mekf_wind_update_airspeed(), ins_mekf_wind_update_baro(), ins_mekf_wind_update_mag(), ins_mekf_wind_update_params(), and ins_mekf_wind_update_pos_speed().