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.
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.
void	ins_mekf_wind_propagate_ahrs (struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
	AHRS-only propagation + accel correction.
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.
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  )

extern

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  )

extern

Definition at line 821 of file ins_mekf_wind.cpp.

References foo, mwp, and FloatVect3::x.

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  )

extern

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:

ins_mekf_wind_get_airspeed_body()

struct NedCoor_f ins_mekf_wind_get_airspeed_body ( void  )

extern

Definition at line 804 of file ins_mekf_wind.cpp.

References foo, mwp, and NedCoor_f::x.

ins_mekf_wind_get_airspeed_norm()

float ins_mekf_wind_get_airspeed_norm ( void  )

extern

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:

ins_mekf_wind_get_baro_bias()

float ins_mekf_wind_get_baro_bias ( void  )

extern

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  )

extern

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:

ins_mekf_wind_get_pos_ned()

struct NedCoor_f ins_mekf_wind_get_pos_ned ( void  )

extern

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:

ins_mekf_wind_get_quat()

struct FloatQuat ins_mekf_wind_get_quat ( void  )

extern

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:

ins_mekf_wind_get_rates_bias()

struct FloatRates ins_mekf_wind_get_rates_bias ( void  )

extern

Definition at line 831 of file ins_mekf_wind.cpp.

References foo, mwp, and FloatRates::p.

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  )

extern

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:

ins_mekf_wind_get_wind_ned()

struct NedCoor_f ins_mekf_wind_get_wind_ned ( void  )

extern

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:

ins_mekf_wind_init()

void ins_mekf_wind_init ( void  )

extern

Init function.

Definition at line 308 of file ins_mekf_wind.cpp.

References ins_mekf_wind_parameters::disable_wind, foo, 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  )

extern

Full INS propagation.

Definition at line 350 of file ins_mekf_wind.cpp.

References A, MekfWindInputs::accel, ins_mekf_wind_parameters::disable_wind, foo, 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  )

extern

AHRS-only propagation + accel correction.

Definition at line 422 of file ins_mekf_wind.cpp.

References A, MekfWindInputs::accel, foo, 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  )

extern

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 )

extern

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 )

extern

Definition at line 731 of file ins_mekf_wind.cpp.

References mwp, and p.

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 )

extern

Definition at line 776 of file ins_mekf_wind.cpp.

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

ins_mekf_wind_set_speed_ned()

void ins_mekf_wind_set_speed_ned ( struct NedCoor_f *  s )

extern

Definition at line 748 of file ins_mekf_wind.cpp.

References mwp, and s.

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 )

extern

Definition at line 614 of file ins_mekf_wind.cpp.

References ins_mekf_wind_parameters::disable_wind, foo, 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 )

extern

Definition at line 541 of file ins_mekf_wind.cpp.

References baro_alt, ins_mekf_wind_parameters::disable_wind, foo, 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  )

extern

Definition at line 652 of file ins_mekf_wind.cpp.

References B, ins_mekf_wind_parameters::disable_wind, foo, 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:

ins_mekf_wind_update_mag()

void ins_mekf_wind_update_mag ( struct FloatVect3 *  mag, bool  attitude_only  )

extern

Definition at line 501 of file ins_mekf_wind.cpp.

References ins_mekf_wind_parameters::disable_wind, foo, 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  )

extern

Definition at line 846 of file ins_mekf_wind.cpp.

References foo, 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  )

extern

Definition at line 575 of file ins_mekf_wind.cpp.

References ins_mekf_wind_parameters::disable_wind, foo, 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 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().