ins_float_invariant.c File Reference

#include "subsystems/ins/ins_float_invariant.h"
#include "subsystems/ahrs/ahrs_int_utils.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ins.h"
#include "subsystems/gps.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_rk_float.h"
#include "math/pprz_isa.h"
#include "state.h"

Include dependency graph for ins_float_invariant.c:

Go to the source code of this file.

Macros
#define	INS_INV_LV   2.
#define	INS_INV_LB   6.
#define	INS_INV_MV   8.
#define	INS_INV_MVZ   15.
#define	INS_INV_MH   0.2
#define	INS_INV_NX   0.8
#define	INS_INV_NXZ   0.5
#define	INS_INV_NH   1.2
#define	INS_INV_OV   1.2
#define	INS_INV_OB   1.
#define	INS_INV_RV   4.
#define	INS_INV_RH   8.
#define	INS_INV_SH   0.01
#define	B   ins_float_inv.mag_h
#define	MAG_FROZEN_COUNT   30

Functions
static void	error_output (struct InsFloatInv *_ins)
	Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error ) More...
static void	invariant_model (float *o, const float *x, const int n, const float *u, const int m)
	Compute dynamic mode. More...
void	float_quat_vmul_right (struct FloatQuat *mright, const struct FloatQuat *q, struct FloatVect3 *vi)
	Right multiplication by a quaternion. More...
static void	init_invariant_state (void)
void	ins_float_invariant_init (void)
void	ins_reset_local_origin (void)
	INS local origin reset. More...
void	ins_reset_altitude_ref (void)
	INS altitude reference reset. More...
void	ins_float_invariant_align (struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void	ins_float_invariant_propagate (struct Int32Rates *gyro, struct Int32Vect3 *accel, float dt)
void	ins_float_invariant_update_gps (struct GpsState *gps_s)
void	ins_float_invariant_update_baro (float pressure)
void	ins_float_invariant_update_mag (struct Int32Vect3 *mag)
void	ins_float_inv_set_body_to_imu_quat (struct FloatQuat *q_b2i)

Variables
struct InsFloatInv	ins_float_inv
static const struct FloatVect3	A = { 0.f, 0.f, 9.81f }
bool_t	ins_baro_initialized
bool_t	ins_gps_fix_once

Detailed Description

Author

Jean-Philippe Condomines jp.co.nosp@m.ndom.nosp@m.ines@.nosp@m.gmai.nosp@m.l.com

INS using invariant filter.

Definition in file ins_float_invariant.c.

Macro Definition Documentation

#define B   ins_float_inv.mag_h

Definition at line 146 of file ins_float_invariant.c.

Referenced by error_output(), and ins_float_invariant_init().

#define INS_INV_LB   6.

Definition at line 91 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_LV   2.

Definition at line 87 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_MH   0.2

Definition at line 103 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_MV   8.

Definition at line 95 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_MVZ   15.

Definition at line 99 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_NH   1.2

Definition at line 115 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_NX   0.8

Definition at line 107 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_NXZ   0.5

Definition at line 111 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_OB   1.

Definition at line 123 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_OV   1.2

Definition at line 119 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_RH   8.

Definition at line 131 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_RV   4.

Definition at line 127 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define INS_INV_SH   0.01

Definition at line 135 of file ins_float_invariant.c.

Referenced by ins_float_invariant_init().

#define MAG_FROZEN_COUNT   30

Definition at line 505 of file ins_float_invariant.c.

Referenced by ins_float_invariant_update_mag().

Function Documentation

static void error_output ( struct InsFloatInv *  _ins )

inlinestatic

Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error )

Definition at line 600 of file ins_float_invariant.c.

References inv_command::accel, inv_state::as, B, inv_measures::baro_alt, InsFloatInv::cmd, InsFloatInv::corr, GpsState::fix, float_quat_invert(), float_quat_vmult(), FLOAT_VECT3_ZERO, InsFloatInv::gains, gps, GPS_FIX_3D, inv_state::hb, ins_float_inv, ins_gps_fix_once, InsFloatInv::is_aligned, inv_gains::lb, inv_correction_gains::LE, inv_gains::lv, inv_measures::mag, inv_correction_gains::ME, InsFloatInv::meas, inv_gains::mh, inv_gains::mv, inv_gains::mvz, inv_correction_gains::NE, State::ned_initialized_f, inv_gains::nh, inv_gains::nx, inv_gains::nxz, inv_gains::ob, inv_correction_gains::OE, inv_gains::ov, inv_state::pos, inv_measures::pos_gps, inv_state::quat, inv_correction_gains::RE, inv_gains::rh, inv_gains::rv, inv_correction_gains::SE, inv_gains::sh, inv_state::speed, inv_measures::speed_gps, state, InsFloatInv::state, State::utm_initialized_f, VECT3_ADD, VECT3_COPY, VECT3_CROSS_PRODUCT, VECT3_DIFF, VECT3_DOT_PRODUCT, VECT3_SMUL, FloatVect3::x, FloatVect3::y, FloatVect3::z, and NedCoor_f::z.

Referenced by ins_float_invariant_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

void float_quat_vmul_right	(	struct FloatQuat *	mright,
		const struct FloatQuat *	q,
		struct FloatVect3 *	vi
	)

Right multiplication by a quaternion.

vi * q

Definition at line 691 of file ins_float_invariant.c.

References FLOAT_QUAT_EXTRACT, FloatQuat::qi, QUAT_ASSIGN, VECT3_ADD, VECT3_CROSS_PRODUCT, VECT3_DOT_PRODUCT, VECT3_SMUL, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by invariant_model().

Here is the caller graph for this function:

static void init_invariant_state ( void  )

inlinestatic

Definition at line 169 of file ins_float_invariant.c.

References inv_state::as, inv_measures::baro_alt, inv_state::bias, FALSE, float_quat_identity(), FLOAT_RATES_ZERO, FLOAT_VECT3_ZERO, inv_state::hb, ins_baro_initialized, ins_float_inv, ins_gps_fix_once, InsFloatInv::meas, inv_state::pos, inv_measures::pos_gps, inv_state::quat, inv_state::speed, inv_measures::speed_gps, and InsFloatInv::state.

Referenced by ins_float_invariant_init(), and ins_float_invariant_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_float_inv_set_body_to_imu_quat

(

struct FloatQuat *

q_b2i

)

Definition at line 705 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 Int32Rates *	lp_gyro,
		struct Int32Vect3 *	lp_accel,
		struct Int32Vect3 *	lp_mag
	)

Definition at line 319 of file ins_float_invariant.c.

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

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 216 of file ins_float_invariant.c.

References LlaCoor_i::alt, UtmCoor_f::alt, B, DefaultPeriodic, UtmCoor_f::east, ecef_of_lla_i(), FALSE, 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, inv_gains::sh, stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), stateSetPositionUtm_f(), and UtmCoor_f::zone.

Referenced by ins_float_invariant_register().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_float_invariant_propagate	(	struct Int32Rates *	gyro,
		struct Int32Vect3 *	accel,
		float	dt
	)

Definition at line 338 of file ins_float_invariant.c.

References inv_command::accel, ACCELS_FLOAT_OF_BFP, inv_state::as, inv_measures::baro_alt, inv_state::bias, InsFloatInv::body_to_imu, InsFloatInv::cmd, DefaultChannel, DefaultDevice, error_output(), FALSE, float_quat_invert(), FLOAT_QUAT_NORMALIZE, float_quat_vmult(), inv_state::hb, init_invariant_state(), ins_float_inv, int32_rmat_transp_ratemult(), int32_rmat_transp_vmult(), INV_COMMAND_DIM, INV_STATE_DIM, invariant_model(), InsFloatInv::is_aligned, log_started, inv_measures::mag, InsFloatInv::meas, orientationGetRMat_i(), FloatRates::p, inv_state::pos, inv_measures::pos_gps, FloatRates::q, FloatQuat::qi, inv_state::quat, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, inv_command::rates, RATES_DIFF, RATES_FLOAT_OF_BFP, InsFloatInv::reset, runge_kutta_4_float(), inv_state::speed, inv_measures::speed_gps, InsFloatInv::state, stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyQuat_f(), stateSetPositionNed_f(), stateSetSpeedNed_f(), TRUE, 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 471 of file ins_float_invariant.c.

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

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 440 of file ins_float_invariant.c.

References UtmCoor_f::alt, UtmCoor_f::east, UtmCoor_i::east, ECEF_FLOAT_OF_BFP, GpsState::ecef_pos, GpsState::ecef_vel, GpsState::fix, GPS_FIX_3D, GpsState::hmsl, ins_float_inv, ins_gps_fix_once, InsFloatInv::is_aligned, InsFloatInv::meas, State::ned_initialized_f, ned_of_ecef_point_f(), ned_of_ecef_vect_f(), State::ned_origin_f, GpsState::ned_vel, UtmCoor_f::north, UtmCoor_i::north, inv_measures::pos_gps, inv_measures::speed_gps, state, TRUE, State::utm_initialized_f, State::utm_origin_f, GpsState::utm_pos, NedCoor_f::x, NedCoor_i::x, 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 Int32Vect3 *

mag

)

Definition at line 507 of file ins_float_invariant.c.

References InsFloatInv::body_to_imu, FLOAT_VECT3_ZERO, ins_float_inv, int32_rmat_transp_vmult(), inv_measures::mag, MAG_FROZEN_COUNT, MAGS_FLOAT_OF_BFP, InsFloatInv::meas, orientationGetRMat_i(), and Int32Vect3::x.

Referenced by mag_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_reset_altitude_ref

(

void

)

INS altitude reference reset.

Reset only vertical reference to the current altitude. Does nothing if not implemented by specific INS algorithm.

Definition at line 300 of file ins_float_invariant.c.

References LlaCoor_i::alt, UtmCoor_f::alt, gps, GpsState::hmsl, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, GpsState::lla_pos, LlaCoor_i::lon, ltp_def_from_lla_i(), State::ned_origin_i, state, stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), and State::utm_origin_f.

Here is the call graph for this function:

void ins_reset_local_origin

(

void

)

INS local origin reset.

Reset horizontal and vertical reference to the current position. Does nothing if not implemented by specific INS algorithm.

INS local origin reset.

Definition at line 273 of file ins_float_invariant.c.

References UtmCoor_f::alt, UtmCoor_f::east, UtmCoor_i::east, GpsState::ecef_pos, gps, GpsState::hmsl, LtpDef_i::hmsl, LLA_FLOAT_OF_BFP, GpsState::lla_pos, LlaCoor_i::lon, ltp_def_from_ecef_i(), UtmCoor_f::north, UtmCoor_i::north, stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), utm_of_lla_f(), GpsState::utm_pos, UtmCoor_f::zone, and UtmCoor_i::zone.

Here is the call graph for this function:

static void invariant_model ( float *  o, const float *  x, const int  n, const float *  u, const int  m  )

inlinestatic

Compute dynamic mode.

x_dot = evolution_model + (gain_matrix * error)

Definition at line 537 of file ins_float_invariant.c.

References inv_command::accel, inv_state::as, inv_state::bias, InsFloatInv::corr, float_quat_derivative(), float_quat_invert(), FLOAT_QUAT_NORM2, float_quat_vmul_right(), float_quat_vmult(), float_vect_zero(), inv_state::hb, ins_float_inv, inv_correction_gains::LE, inv_correction_gains::ME, inv_correction_gains::NE, inv_correction_gains::OE, inv_state::pos, inv_state::quat, QUAT_ADD, QUAT_SMUL, inv_command::rates, RATES_ASSIGN, RATES_DIFF, inv_correction_gains::RE, inv_correction_gains::SE, inv_state::speed, VECT3_ADD, VECT3_SMUL, VECT3_SUM, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ins_float_invariant_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

const struct FloatVect3 A = { 0.f, 0.f, 9.81f }

static

Definition at line 142 of file ins_float_invariant.c.

bool_t ins_baro_initialized

Definition at line 149 of file ins_float_invariant.c.

Referenced by init_invariant_state(), and ins_float_invariant_update_baro().

struct InsFloatInv ins_float_inv

Definition at line 139 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(), and mag_cb().

bool_t ins_gps_fix_once

Definition at line 152 of file ins_float_invariant.c.

Referenced by error_output(), init_invariant_state(), and ins_float_invariant_update_gps().