ins_float_invariant.h File Reference

INS using invariant filter. More...

#include "subsystems/ins.h"
#include "subsystems/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_inv_set_body_to_imu_quat (struct FloatQuat *q_b2i)
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

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.

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.

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.

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.
struct FloatVect3	NE	Correction gains on position.
float	NE	Correction gains on accel bias.
struct FloatVect3	OE	Correction gains on gyro biases.
float	OE	Correction gains on magnetometer sensitivity.
float	RE	Correction gains on accel bias.
float	SE	Correction gains on barometer bias.

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.

struct InsFloatInv

Invariant filter structure.

Definition at line 106 of file ins_float_invariant.h.

Collaboration diagram for InsFloatInv:

Data Fields
struct OrientationReps	body_to_imu	body_to_imu rotation
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

#define INV_COMMAND_DIM   6

Invariant filter command vector dimension.

Definition at line 66 of file ins_float_invariant.h.

#define INV_MEASURE_DIM   10

Invariant filter measurement vector dimension.

Definition at line 53 of file ins_float_invariant.h.

#define INV_STATE_DIM   15

Invariant filter state dimension.

Definition at line 37 of file ins_float_invariant.h.

Function Documentation

void ins_float_inv_set_body_to_imu_quat

(

struct FloatQuat *

q_b2i

)

Definition at line 713 of file ins_float_invariant.c.

References InsFloatInv::body_to_imu, ins_float_inv, InsFloatInv::is_aligned, orientationSetQuat_f(), inv_state::quat, and InsFloatInv::state.

Referenced by body_to_imu_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

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

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

void ins_float_invariant_init

(

void

)

Definition at line 219 of file ins_float_invariant.c.

References LlaCoor_i::alt, UtmCoor_f::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_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:

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

Definition at line 337 of file ins_float_invariant.c.

References inv_command::accel, ACCELS_BFP_OF_REAL, inv_state::as, inv_measures::baro_alt, inv_state::bias, InsFloatInv::body_to_imu, InsFloatInv::cmd, DefaultChannel, DefaultDevice, error_output(), float_quat_invert(), float_quat_normalize(), float_quat_vmult(), float_rmat_transp_ratemult(), float_rmat_transp_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, orientationGetRMat_f(), 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_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_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:

void ins_float_invariant_update_baro

(

float

pressure

)

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

void ins_float_invariant_update_gps

(

struct GpsState *

gps_s

)

Definition at line 439 of file ins_float_invariant.c.

References UtmCoor_f::alt, UtmCoor_f::east, ECEF_FLOAT_OF_BFP, GpsState::ecef_pos, GpsState::ecef_vel, GpsState::fix, float_vect2_norm(), FLOAT_VECT3_ZERO, GPS_FIX_3D, 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(), State::ned_origin_f, State::ned_origin_i, GpsState::ned_vel, UtmCoor_f::north, inv_measures::pos_gps, inv_measures::speed_gps, state, utm_float_from_gps(), State::utm_initialized_f, State::utm_origin_f, FloatVect2::x, NedCoor_f::x, NedCoor_i::x, FloatVect2::y, NedCoor_f::y, NedCoor_i::y, NedCoor_f::z, and NedCoor_i::z.

Referenced by gps_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_float_invariant_update_mag

(

struct FloatVect3 *

mag

)

Definition at line 527 of file ins_float_invariant.c.

References InsFloatInv::body_to_imu, float_rmat_transp_vmult(), FLOAT_VECT3_ZERO, ins_float_inv, inv_measures::mag, MAG_FROZEN_COUNT, InsFloatInv::meas, orientationGetRMat_f(), and FloatVect3::x.

Referenced by ins_float_invariant_update_gps(), and mag_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

struct InsFloatInv ins_float_inv

Definition at line 137 of file ins_float_invariant.c.

Referenced by aligner_cb(), error_output(), geo_mag_cb(), init_invariant_state(), ins_float_inv_set_body_to_imu_quat(), ins_float_invariant_align(), ins_float_invariant_init(), ins_float_invariant_propagate(), 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().