ins_float_invariant.h File Reference

INS using invariant filter. More...

#include "modules/ins/ins.h"
#include "modules/gps/gps.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"

Include dependency graph for ins_float_invariant.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	inv_state
	Invariant filter state. More...
struct	inv_measures
	Invariant filter measurement vector. More...
struct	inv_command
	Invariant filter command vector. More...
struct	inv_correction_gains
	Invariant filter correction gains. More...
struct	inv_gains
	Invariant filter tuning gains. More...
struct	InsFloatInv
	Invariant filter structure. More...

Macros
#define	INV_STATE_DIM   15
	Invariant filter state dimension. More...
#define	INV_MEASURE_DIM   10
	Invariant filter measurement vector dimension. More...
#define	INV_COMMAND_DIM   6
	Invariant filter command vector dimension. More...

Functions
void	ins_float_invariant_init (void)
void	ins_float_invariant_reset_ref (void)
void	ins_float_invariant_reset_vertical_ref (void)
void	ins_float_invariant_align (struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void	ins_float_invariant_propagate (struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
void	ins_float_invariant_update_mag (struct FloatVect3 *mag)
void	ins_float_invariant_update_baro (float pressure)
void	ins_float_invariant_update_gps (struct GpsState *gps_s)

Variables
struct InsFloatInv	ins_float_inv

Detailed Description

INS using invariant filter.

For more information, please send an email to "jp.condomines@gmail.com"

Definition in file ins_float_invariant.h.

---

Data Structure Documentation

inv_state

struct inv_state

Invariant filter state.

Definition at line 40 of file ahrs_float_invariant.h.

Collaboration diagram for inv_state:

Data Fields
float	as	Estimated accelerometer sensitivity.
struct FloatRates	bias	Estimated gyro biases.
float	cs	Estimates magnetometer sensitivity.
float	hb	Estimates barometers bias.
struct NedCoor_f	pos	Estimates position.
struct FloatQuat	quat	Estimated attitude (quaternion)
struct NedCoor_f	speed	Estimates speed.

inv_measures

struct inv_measures

Invariant filter measurement vector.

Definition at line 53 of file ahrs_float_invariant.h.

Collaboration diagram for inv_measures:

Data Fields
struct FloatVect3	accel	Measured accelerometers.
float	baro_alt	Measured barometric altitude.
struct FloatVect3	mag	Measured magnetic field.
struct NedCoor_f	pos_gps	Measured gps position.
struct NedCoor_f	speed_gps	Measured gps speed.

inv_command

struct inv_command

Invariant filter command vector.

Definition at line 64 of file ahrs_float_invariant.h.

Collaboration diagram for inv_command:

Data Fields
struct FloatVect3	accel	Input accelerometers.
struct FloatRates	rates	Input gyro rates.

inv_correction_gains

struct inv_correction_gains

Invariant filter correction gains.

Definition at line 70 of file ahrs_float_invariant.h.

Collaboration diagram for inv_correction_gains:

Data Fields
struct FloatVect3	LE	Correction gains on attitude.
struct FloatVect3	ME	Correction gains on gyro biases. Correction gains on speed.
float	NE	Correction gains on accel bias.
struct FloatVect3	NE	Correction gains on position.
float	OE	Correction gains on magnetometer sensitivity.
struct FloatVect3	OE	Correction gains on gyro biases.
float	RE	Correction gains on accel bias.
float	SE	Correction gains on barometer bias.

inv_gains

struct inv_gains

Invariant filter tuning gains.

Definition at line 79 of file ahrs_float_invariant.h.

Data Fields
float	lb	Tuning parameter of mag error on attitude.
float	lv	Tuning parameter of speed error on attitude.
float	lx	Tuning parameter of accel and mag on attitude (longitudinal subsystem)
float	ly	Tuning parameter of accel and mag on attitude (lateral subsystem)
float	lz	Tuning parameter of accel and mag on attitude (heading subsystem)
float	mh	Tuning parameter of baro error on vertical speed.
float	mv	Tuning parameter of horizontal speed error on speed.
float	mvz	Tuning parameter of vertical speed error on speed.
float	mx	Tuning parameter of accel and mag on gyro bias (longitudinal subsystem)
float	my	Tuning parameter of accel and mag on gyro bias (lateral subsystem)
float	mz	Tuning parameter of accel and mag on gyro bias (heading subsystem)
float	n	Tuning parameter of accel and mag on accel bias (scaling subsystem)
float	nh	Tuning parameter of baro error on vertical position.
float	nx	Tuning parameter of horizontal position error on position.
float	nxz	Tuning parameter of vertical position error on position.
float	o	Tuning parameter of accel and mag on mag bias (scaling subsystem)
float	ob	Tuning parameter of mag error on gyro biases.
float	ov	Tuning parameter of speed error on gyro biases.
float	rh	Tuning parameter of baro error on accel biases (vertical projection)
float	rv	Tuning parameter of speed error on accel biases.
float	sh	Tuning parameter of baro error on baro bias.

InsFloatInv

struct InsFloatInv

Invariant filter structure.

Definition at line 106 of file ins_float_invariant.h.

Collaboration diagram for InsFloatInv:

Data Fields
struct inv_command	cmd	command vector
struct inv_correction_gains	corr	correction gains
struct inv_gains	gains	tuning gains
bool	is_aligned
struct FloatVect3	mag_h
struct inv_measures	meas	measurement vector
bool	reset	flag to request reset/reinit the filter
struct inv_state	state	state vector

Macro Definition Documentation

INV_COMMAND_DIM

#define INV_COMMAND_DIM   6

Invariant filter command vector dimension.

Definition at line 66 of file ins_float_invariant.h.

INV_MEASURE_DIM

#define INV_MEASURE_DIM   10

Invariant filter measurement vector dimension.

Definition at line 53 of file ins_float_invariant.h.

INV_STATE_DIM

#define INV_STATE_DIM   15

Invariant filter state dimension.

Definition at line 37 of file ins_float_invariant.h.

Function Documentation

ins_float_invariant_align()

void ins_float_invariant_align	(	struct FloatRates *	lp_gyro,
		struct FloatVect3 *	lp_accel,
		struct FloatVect3 *	lp_mag
	)

Definition at line 323 of file ins_float_invariant.c.

References ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), inv_state::bias, ins_float_inv, InsFloatInv::is_aligned, inv_state::quat, InsFloatInv::state, and stateSetNedToBodyQuat_f().

Referenced by aligner_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_float_invariant_init()

void ins_float_invariant_init

(

void

)

Definition at line 219 of file ins_float_invariant.c.

References UtmCoor_f::alt, LlaCoor_i::alt, B, DefaultPeriodic, UtmCoor_f::east, ecef_of_lla_i(), InsFloatInv::gains, LtpDef_i::hmsl, init_invariant_state(), ins_float_inv, INS_INV_LB, INS_INV_LV, INS_INV_MH, INS_INV_MV, INS_INV_MVZ, INS_INV_NH, INS_INV_NX, INS_INV_NXZ, INS_INV_OB, INS_INV_OV, INS_INV_RH, INS_INV_RV, INS_INV_SH, InsFloatInv::is_aligned, LlaCoor_i::lat, inv_gains::lb, LlaCoor_i::lon, ltp_def, ltp_def_from_ecef_i(), inv_gains::lv, inv_gains::mh, inv_gains::mv, inv_gains::mvz, nav_utm_east0, nav_utm_north0, nav_utm_zone0, inv_gains::nh, UtmCoor_f::north, inv_gains::nx, inv_gains::nxz, inv_gains::ob, inv_gains::ov, register_periodic_telemetry(), InsFloatInv::reset, inv_gains::rh, inv_gains::rv, send_inv_filter(), inv_gains::sh, stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), stateSetPositionUtm_f(), and UtmCoor_f::zone.

Referenced by ins_float_invariant_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_float_invariant_propagate()

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

Definition at line 345 of file ins_float_invariant.c.

References A, inv_command::accel, ACCELS_BFP_OF_REAL, inv_state::as, inv_measures::baro_alt, inv_state::bias, InsFloatInv::cmd, DefaultChannel, DefaultDevice, error_output(), float_quat_invert(), float_quat_normalize(), float_quat_vmult(), inv_state::hb, init_invariant_state(), ins_float_inv, INV_COMMAND_DIM, INV_STATE_DIM, invariant_model(), InsFloatInv::is_aligned, log_started, inv_measures::mag, InsFloatInv::meas, FloatRates::p, inv_state::pos, inv_measures::pos_gps, pprzLogFile, FloatRates::q, FloatQuat::qi, inv_state::quat, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, inv_command::rates, RATES_COPY, RATES_DIFF, InsFloatInv::reset, runge_kutta_4_float(), send_inv_filter(), inv_state::speed, inv_measures::speed_gps, InsFloatInv::state, stateSetAccelBody_i(), stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyQuat_f(), stateSetPositionNed_f(), stateSetSpeedNed_f(), VECT3_ADD, VECT3_COPY, VECT3_SMUL, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Referenced by gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_float_invariant_reset_ref()

void ins_float_invariant_reset_ref

(

void

)

Definition at line 281 of file ins_float_invariant.c.

References LlaCoor_i::alt, ecef_int_from_gps(), FLOAT_VECT3_ZERO, gps, LtpDef_i::hmsl, GpsState::hmsl, ins_float_inv, LtpDef_i::lla, lla_int_from_gps(), ltp_def, ltp_def_from_ecef_i(), inv_state::pos, inv_state::speed, InsFloatInv::state, stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), and utm_float_from_gps().

Referenced by reset_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_float_invariant_reset_vertical_ref()

void ins_float_invariant_reset_vertical_ref

(

void

)

Definition at line 301 of file ins_float_invariant.c.

References UtmCoor_f::alt, LlaCoor_i::alt, gps, LtpDef_i::hmsl, GpsState::hmsl, ins_float_inv, LlaCoor_i::lat, LtpDef_i::lla, GpsState::lla_pos, LlaCoor_i::lon, ltp_def, ltp_def_from_lla_i(), inv_state::pos, inv_state::speed, InsFloatInv::state, stateGetLlaOrigin_i(), stateGetUtmOrigin_f(), stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), and NedCoor_f::z.

Referenced by reset_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_float_invariant_update_baro()

void ins_float_invariant_update_baro

(

float

pressure

)

Definition at line 530 of file ins_float_invariant.c.

References alpha, inv_measures::baro_alt, ins_baro_initialized, ins_float_inv, InsFloatInv::meas, pprz_isa_height_of_pressure(), and PPRZ_ISA_SEA_LEVEL_PRESSURE.

Referenced by baro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_float_invariant_update_gps()

void ins_float_invariant_update_gps

(

struct GpsState *

gps_s

)

Definition at line 446 of file ins_float_invariant.c.

References UtmCoor_f::alt, GpsState::cacc, GpsState::course, course, UtmCoor_f::east, ecef_int_from_gps(), ecef_vel_float_from_gps(), GpsState::fix, float_eulers_of_quat(), float_quat_inv_comp_norm_shortest(), float_quat_vmult(), float_vect2_norm(), FLOAT_VECT3_ZERO, GPS_FIX_3D, GpsState::gspeed, ins_float_inv, ins_float_invariant_update_mag(), ins_gps_fix_once, INS_INV_HEADING_UPDATE_GPS_MIN_SPEED, INT32_POS_OF_CM_DEN, INT32_POS_OF_CM_NUM, INT32_VECT3_SCALE_2, InsFloatInv::is_aligned, inv_measures::mag, InsFloatInv::meas, nav_utm_zone0, NED_FLOAT_OF_BFP, State::ned_initialized_f, ned_of_ecef_point_i(), ned_of_ecef_vect_f(), ned_vel_float_from_gps(), UtmCoor_f::north, inv_state::pos, inv_measures::pos_gps, FloatEulers::psi, inv_state::quat, inv_state::speed, inv_measures::speed_gps, InsFloatInv::state, state, stateGetHmslOrigin_f(), stateGetNedOrigin_f(), stateGetNedOrigin_i(), stateGetUtmOrigin_f(), utm_float_from_gps(), State::utm_initialized_f, FloatVect2::x, FloatVect3::x, NedCoor_f::x, FloatVect2::y, FloatVect3::y, NedCoor_f::y, and NedCoor_f::z.

Referenced by gps_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_float_invariant_update_mag()

void ins_float_invariant_update_mag

(

struct FloatVect3 *

mag

)

Definition at line 566 of file ins_float_invariant.c.

References FLOAT_VECT3_ZERO, ins_float_inv, inv_measures::mag, MAG_FROZEN_COUNT, InsFloatInv::meas, VECT3_COPY, and FloatVect3::x.

Referenced by ins_float_invariant_update_gps(), and mag_cb().

Here is the caller graph for this function:

Variable Documentation

ins_float_inv

struct InsFloatInv ins_float_inv

extern

Definition at line 1 of file ins_float_invariant.c.

Referenced by aligner_cb(), geo_mag_cb(), gyro_cb(), init_invariant_state(), ins_float_invariant_align(), ins_float_invariant_init(), ins_float_invariant_propagate(), ins_float_invariant_reset_ref(), ins_float_invariant_reset_vertical_ref(), ins_float_invariant_update_baro(), ins_float_invariant_update_gps(), ins_float_invariant_update_mag(), invariant_model(), mag_cb(), send_filter_status(), and send_inv_filter().