Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
Data Structures | Macros | Typedefs | Enumerations | Functions | Variables
ins_mekf_wind.cpp File Reference

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

#include "modules/ins/ins_mekf_wind.h"
#include "generated/airframe.h"
#include <Eigen/Dense>
+ Include dependency graph for ins_mekf_wind.cpp:

Go to the source code of this file.

Data Structures

struct  MekfWindState
 filter state vector More...
 
struct  MekfWindInputs
 filter command vector More...
 
struct  MekfWindMeasurements
 filter measurement vector More...
 
struct  InsMekfWindPrivate
 private filter structure More...
 

Macros

#define eigen_assert(_cond)   { if (!(_cond)) { while(1) ; } }
 
#define INS_MEKF_WIND_P0_QUAT   0.007615f
 
#define INS_MEKF_WIND_P0_SPEED   1.E+2f
 
#define INS_MEKF_WIND_P0_POS   1.E+1f
 
#define INS_MEKF_WIND_P0_RATES_BIAS   1.E-3f
 
#define INS_MEKF_WIND_P0_ACCEL_BIAS   1.E-3f
 
#define INS_MEKF_WIND_P0_BARO_BIAS   1.E-3f
 
#define INS_MEKF_WIND_P0_WIND   1.E-0f
 
#define INS_MEKF_WIND_Q_GYRO   1.E-2f
 
#define INS_MEKF_WIND_Q_ACCEL   1.E-2f
 
#define INS_MEKF_WIND_Q_RATES_BIAS   1.E-6f
 
#define INS_MEKF_WIND_Q_ACCEL_BIAS   1.E-6f
 
#define INS_MEKF_WIND_Q_BARO_BIAS   1.E-3f
 
#define INS_MEKF_WIND_Q_WIND   1.E+1f
 
#define INS_MEKF_WIND_R_SPEED   0.1f
 
#define INS_MEKF_WIND_R_SPEED_Z   0.2f
 
#define INS_MEKF_WIND_R_POS   2.0f
 
#define INS_MEKF_WIND_R_POS_Z   4.0f
 
#define INS_MEKF_WIND_R_MAG   1.f
 
#define INS_MEKF_WIND_R_BARO   2.f
 
#define INS_MEKF_WIND_R_AIRSPEED   0.1f
 
#define INS_MEKF_WIND_R_AOA   0.1f
 
#define INS_MEKF_WIND_R_AOS   0.1f
 
#define INS_MEKF_WIND_DISABLE_WIND   true
 
#define mwp   mekf_wind_private
 

Typedefs

typedef Matrix< float,
MEKF_WIND_COV_SIZE,
MEKF_WIND_COV_SIZE > 		MEKFWCov
 
typedef Matrix< float,
MEKF_WIND_PROC_NOISE_SIZE,
MEKF_WIND_PROC_NOISE_SIZE > 		MEKFWPNoise
 
typedef Matrix< float,
MEKF_WIND_MEAS_NOISE_SIZE,
MEKF_WIND_MEAS_NOISE_SIZE > 		MEKFWMNoise
 

Enumerations

enum  MekfWindCovVar {
  MEKF_WIND_qx, MEKF_WIND_qy, MEKF_WIND_qz, MEKF_WIND_vx,
  MEKF_WIND_vy, MEKF_WIND_vz, MEKF_WIND_px, MEKF_WIND_py,
  MEKF_WIND_pz, MEKF_WIND_rbp, MEKF_WIND_rbq, MEKF_WIND_rbr,
  MEKF_WIND_abx, MEKF_WIND_aby, MEKF_WIND_abz, MEKF_WIND_bb,
  MEKF_WIND_wx, MEKF_WIND_wy, MEKF_WIND_wz, MEKF_WIND_COV_SIZE
}
 Covariance matrix elements and size. More...
 
enum  MekfWindPNoiseVar {
  MEKF_WIND_qgp, MEKF_WIND_qgq, MEKF_WIND_qgr, MEKF_WIND_qax,
  MEKF_WIND_qay, MEKF_WIND_qaz, MEKF_WIND_qrbp, MEKF_WIND_qrbq,
  MEKF_WIND_qrbr, MEKF_WIND_qabx, MEKF_WIND_qaby, MEKF_WIND_qabz,
  MEKF_WIND_qbb, MEKF_WIND_qwx, MEKF_WIND_qwy, MEKF_WIND_qwz,
  MEKF_WIND_PROC_NOISE_SIZE
}
 Process noise elements and size. More...
 
enum  MekfWindMNoiseVar {
  MEKF_WIND_rvx, MEKF_WIND_rvy, MEKF_WIND_rvz, MEKF_WIND_rpx,
  MEKF_WIND_rpy, MEKF_WIND_rpz, MEKF_WIND_rmx, MEKF_WIND_rmy,
  MEKF_WIND_rmz, MEKF_WIND_rb, MEKF_WIND_ras, MEKF_WIND_raoa,
  MEKF_WIND_raos, MEKF_WIND_MEAS_NOISE_SIZE
}
 Measurement noise elements and size. More...
 

Functions

static const Vector3f gravity (0.f, 0.f, 9.81f)
 
static void init_mekf_state (void)
 
static Quaternionf quat_add (const Quaternionf &q1, const Quaternionf &q2)
 
static Quaternionf quat_smul (const Quaternionf &q1, float scal)
 
static Matrix3f skew_sym (const Vector3f &v)
 build skew symetric matrix from vector m = [ 0, -v(2), v(1) ] [ v(2), 0, -v(0) ] [ -v(1), v(0), 0 ] More...
 
void ins_mekf_wind_init (void)
 Init function. More...
 
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 *acc, float dt)
 Full INS propagation. More...
 
void ins_mekf_wind_propagate_ahrs (struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
 AHRS-only propagation + accel correction. More...
 
void ins_mekf_wind_align (struct FloatRates *gyro_bias, struct FloatQuat *quat)
 
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
 
static struct InsMekfWindPrivate mekf_wind_private
 

Detailed Description

Multiplicative Extended Kalman Filter in rotation matrix formulation.

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

Using Eigen library

Definition in file ins_mekf_wind.cpp.

Data Structure Documentation

struct MekfWindState

filter state vector

Definition at line 93 of file ins_mekf_wind.cpp.

Data Fields
Vector3f accel
Vector3f accel_bias
float baro_bias
Vector3f pos
Quaternionf quat
Vector3f rates_bias
Vector3f speed
Vector3f wind
struct MekfWindInputs

filter command vector

Definition at line 106 of file ins_mekf_wind.cpp.

Data Fields
Vector3f accel
Vector3f rates
struct MekfWindMeasurements

filter measurement vector

Definition at line 113 of file ins_mekf_wind.cpp.

Data Fields
float airspeed
float aoa
float aos
float baro_alt
Vector3f mag
Vector3f pos
Vector3f speed
struct InsMekfWindPrivate

private filter structure

Definition at line 125 of file ins_mekf_wind.cpp.

+ Collaboration diagram for InsMekfWindPrivate:
Data Fields
struct MekfWindInputs inputs
Vector3f mag_h
struct MekfWindMeasurements measurements
MEKFWCov P
MEKFWPNoise Q
MEKFWMNoise R
struct MekfWindState state

Macro Definition Documentation

#define eigen_assert (   _cond)    { if (!(_cond)) { while(1) ; } }

Definition at line 38 of file ins_mekf_wind.cpp.

#define INS_MEKF_WIND_DISABLE_WIND   true

Definition at line 213 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_P0_ACCEL_BIAS   1.E-3f

Definition at line 153 of file ins_mekf_wind.cpp.

Referenced by init_mekf_state().

#define INS_MEKF_WIND_P0_BARO_BIAS   1.E-3f

Definition at line 156 of file ins_mekf_wind.cpp.

Referenced by init_mekf_state().

#define INS_MEKF_WIND_P0_POS   1.E+1f

Definition at line 147 of file ins_mekf_wind.cpp.

Referenced by init_mekf_state().

#define INS_MEKF_WIND_P0_QUAT   0.007615f

Definition at line 141 of file ins_mekf_wind.cpp.

Referenced by init_mekf_state().

#define INS_MEKF_WIND_P0_RATES_BIAS   1.E-3f

Definition at line 150 of file ins_mekf_wind.cpp.

Referenced by init_mekf_state().

#define INS_MEKF_WIND_P0_SPEED   1.E+2f

Definition at line 144 of file ins_mekf_wind.cpp.

Referenced by init_mekf_state().

#define INS_MEKF_WIND_P0_WIND   1.E-0f

Definition at line 159 of file ins_mekf_wind.cpp.

Referenced by init_mekf_state(), and ins_mekf_wind_propagate().

#define INS_MEKF_WIND_Q_ACCEL   1.E-2f

Definition at line 167 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_Q_ACCEL_BIAS   1.E-6f

Definition at line 173 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_Q_BARO_BIAS   1.E-3f

Definition at line 176 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_Q_GYRO   1.E-2f

Definition at line 164 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_Q_RATES_BIAS   1.E-6f

Definition at line 170 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_Q_WIND   1.E+1f

Definition at line 179 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_R_AIRSPEED   0.1f

Definition at line 202 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_R_AOA   0.1f

Definition at line 205 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_R_AOS   0.1f

Definition at line 208 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_R_BARO   2.f

Definition at line 199 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_R_MAG   1.f

Definition at line 196 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_R_POS   2.0f

Definition at line 190 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_R_POS_Z   4.0f

Definition at line 193 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_R_SPEED   0.1f

Definition at line 184 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define INS_MEKF_WIND_R_SPEED_Z   0.2f

Definition at line 187 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_init().

#define mwp   mekf_wind_private

Definition at line 222 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_align(), ins_mekf_wind_get_accel_bias(), ins_mekf_wind_get_accel_ned(), ins_mekf_wind_get_airspeed_body(), ins_mekf_wind_get_airspeed_norm(), ins_mekf_wind_get_baro_bias(), ins_mekf_wind_get_body_rates(), ins_mekf_wind_get_pos_ned(), ins_mekf_wind_get_quat(), ins_mekf_wind_get_rates_bias(), ins_mekf_wind_get_speed_ned(), ins_mekf_wind_get_wind_ned(), ins_mekf_wind_propagate(), ins_mekf_wind_propagate_ahrs(), ins_mekf_wind_set_pos_ned(), ins_mekf_wind_set_quat(), ins_mekf_wind_set_speed_ned(), ins_mekf_wind_update_airspeed(), ins_mekf_wind_update_baro(), ins_mekf_wind_update_incidence(), ins_mekf_wind_update_mag(), and ins_mekf_wind_update_pos_speed().

Typedef Documentation

typedef Matrix<float, MEKF_WIND_COV_SIZE, MEKF_WIND_COV_SIZE> MEKFWCov

Definition at line 62 of file ins_mekf_wind.cpp.

typedef Matrix<float, MEKF_WIND_MEAS_NOISE_SIZE, MEKF_WIND_MEAS_NOISE_SIZE> MEKFWMNoise

Definition at line 89 of file ins_mekf_wind.cpp.

typedef Matrix<float, MEKF_WIND_PROC_NOISE_SIZE, MEKF_WIND_PROC_NOISE_SIZE> MEKFWPNoise

Definition at line 76 of file ins_mekf_wind.cpp.

Enumeration Type Documentation

enum MekfWindCovVar

Covariance matrix elements and size.

Enumerator
MEKF_WIND_qx 
MEKF_WIND_qy 
MEKF_WIND_qz 
MEKF_WIND_vx 
MEKF_WIND_vy 
MEKF_WIND_vz 
MEKF_WIND_px 
MEKF_WIND_py 
MEKF_WIND_pz 
MEKF_WIND_rbp 
MEKF_WIND_rbq 
MEKF_WIND_rbr 
MEKF_WIND_abx 
MEKF_WIND_aby 
MEKF_WIND_abz 
MEKF_WIND_bb 
MEKF_WIND_wx 
MEKF_WIND_wy 
MEKF_WIND_wz 
MEKF_WIND_COV_SIZE 

Definition at line 51 of file ins_mekf_wind.cpp.

enum MekfWindMNoiseVar

Measurement noise elements and size.

Enumerator
MEKF_WIND_rvx 
MEKF_WIND_rvy 
MEKF_WIND_rvz 
MEKF_WIND_rpx 
MEKF_WIND_rpy 
MEKF_WIND_rpz 
MEKF_WIND_rmx 
MEKF_WIND_rmy 
MEKF_WIND_rmz 
MEKF_WIND_rb 
MEKF_WIND_ras 
MEKF_WIND_raoa 
MEKF_WIND_raos 
MEKF_WIND_MEAS_NOISE_SIZE 

Definition at line 80 of file ins_mekf_wind.cpp.

enum MekfWindPNoiseVar

Process noise elements and size.

Enumerator
MEKF_WIND_qgp 
MEKF_WIND_qgq 
MEKF_WIND_qgr 
MEKF_WIND_qax 
MEKF_WIND_qay 
MEKF_WIND_qaz 
MEKF_WIND_qrbp 
MEKF_WIND_qrbq 
MEKF_WIND_qrbr 
MEKF_WIND_qabx 
MEKF_WIND_qaby 
MEKF_WIND_qabz 
MEKF_WIND_qbb 
MEKF_WIND_qwx 
MEKF_WIND_qwy 
MEKF_WIND_qwz 
MEKF_WIND_PROC_NOISE_SIZE 

Definition at line 66 of file ins_mekf_wind.cpp.

Function Documentation

static const Vector3f gravity ( 0.  f,
0.  f,
9.  81f 
)
static

Referenced by ins_mekf_wind_propagate(), and ins_mekf_wind_propagate_ahrs().

+ Here is the caller graph for this function:

static void init_mekf_state ( void  )
static

Definition at line 229 of file ins_mekf_wind.cpp.

References MekfWindInputs::accel, MekfWindState::accel_bias, MekfWindMeasurements::airspeed, MekfWindMeasurements::aoa, MekfWindMeasurements::aos, MekfWindMeasurements::baro_alt, MekfWindState::baro_bias, InsMekfWindPrivate::inputs, INS_MEKF_WIND_P0_ACCEL_BIAS, INS_MEKF_WIND_P0_BARO_BIAS, INS_MEKF_WIND_P0_POS, INS_MEKF_WIND_P0_QUAT, INS_MEKF_WIND_P0_RATES_BIAS, INS_MEKF_WIND_P0_SPEED, INS_MEKF_WIND_P0_WIND, ins_mekf_wind_update_params(), MekfWindMeasurements::mag, InsMekfWindPrivate::measurements, MEKF_WIND_abx, MEKF_WIND_aby, MEKF_WIND_abz, MEKF_WIND_bb, mekf_wind_private, MEKF_WIND_px, MEKF_WIND_py, MEKF_WIND_pz, MEKF_WIND_qx, MEKF_WIND_qy, MEKF_WIND_qz, MEKF_WIND_rbp, MEKF_WIND_rbq, MEKF_WIND_rbr, MEKF_WIND_vx, MEKF_WIND_vy, MEKF_WIND_vz, MEKF_WIND_wx, MEKF_WIND_wy, MEKF_WIND_wz, InsMekfWindPrivate::P, MekfWindState::pos, MekfWindMeasurements::pos, MekfWindState::quat, MekfWindInputs::rates, MekfWindState::rates_bias, MekfWindState::speed, MekfWindMeasurements::speed, InsMekfWindPrivate::state, and MekfWindState::wind.

Referenced by ins_mekf_wind_init(), and ins_mekf_wind_reset().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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, 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_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:

void ins_mekf_wind_propagate ( struct FloatRates *  gyro,
struct FloatVect3 *  acc,
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:

void ins_mekf_wind_propagate_ahrs ( struct FloatRates *  gyro,
struct FloatVect3 *  acc,
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)

Definition at line 541 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_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:

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_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:

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:

static Quaternionf quat_add ( const Quaternionf &  q1,
const Quaternionf &  q2 
)
static

Definition at line 280 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_propagate(), and ins_mekf_wind_propagate_ahrs().

+ Here is the caller graph for this function:

static Quaternionf quat_smul ( const Quaternionf &  q1,
float  scal 
)
static

Definition at line 284 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_propagate(), and ins_mekf_wind_propagate_ahrs().

+ Here is the caller graph for this function:

static Matrix3f skew_sym ( const Vector3f &  v)
static

build skew symetric matrix from vector m = [ 0, -v(2), v(1) ] [ v(2), 0, -v(0) ] [ -v(1), v(0), 0 ]

Definition at line 294 of file ins_mekf_wind.cpp.

Referenced by ins_mekf_wind_propagate(), ins_mekf_wind_propagate_ahrs(), ins_mekf_wind_update_airspeed(), ins_mekf_wind_update_incidence(), and ins_mekf_wind_update_mag().

+ Here is the caller graph for this function:

Variable Documentation

struct ins_mekf_wind_parameters ins_mekf_wind_params

Definition at line 217 of file ins_mekf_wind.cpp.

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

struct InsMekfWindPrivate mekf_wind_private
static

Definition at line 220 of file ins_mekf_wind.cpp.

Referenced by init_mekf_state(), ins_mekf_wind_init(), ins_mekf_wind_propagate(), ins_mekf_wind_propagate_ahrs(), ins_mekf_wind_set_mag_h(), and ins_mekf_wind_update_params().