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:

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:

{ \
  ins_mekf_wind_params.Q_accel = _v; \
  ins_mekf_wind_update_params(); \
}

Definition at line 106 of file ins_mekf_wind.h.

#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.

#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.

#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.

#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.

#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.

#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.

#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.

#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.

#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.

#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.

#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.

#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.

#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.

#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

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.

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

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 )

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.

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:

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().

Data Structures

Macros

Functions

Variables

Detailed Description

Data Structure Documentation

Macro Definition Documentation

Function Documentation

Variable Documentation