ins_float_invariant.c File Reference

#include "modules/ins/ins_float_invariant.h"
#include "modules/ahrs/ahrs_int_utils.h"
#include "modules/ins/ins.h"
#include "modules/gps/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 "modules/datalink/telemetry.h"

Include dependency graph for ins_float_invariant.c:

Go to the source code of this file.

Macros
#define	INS_INV_LV   2.f
#define	INS_INV_LB   6.f
#define	INS_INV_MV   8.f
#define	INS_INV_MVZ   15.f
#define	INS_INV_MH   0.2f
#define	INS_INV_NX   0.8f
#define	INS_INV_NXZ   0.5f
#define	INS_INV_NH   1.2f
#define	INS_INV_OV   1.2f
#define	INS_INV_OB   1.f
#define	INS_INV_RV   4.f
#define	INS_INV_RH   8.f
#define	INS_INV_SH   0.01f
#define	B   ins_float_inv.mag_h
#define	INS_INV_HEADING_UPDATE_GPS_MIN_SPEED   5.f
#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)
static void	send_inv_filter (struct transport_tx *trans, struct link_device *dev)
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_gps (struct GpsState *gps_s)
void	ins_float_invariant_update_baro (float pressure)
void	ins_float_invariant_update_mag (struct FloatVect3 *mag)

Variables
struct InsFloatInv	ins_float_inv
static const struct FloatVect3	A = { 0.f, 0.f, 9.81f }
bool	ins_baro_initialized
bool	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

B

#define B   ins_float_inv.mag_h

Definition at line 144 of file ins_float_invariant.c.

INS_INV_HEADING_UPDATE_GPS_MIN_SPEED

#define INS_INV_HEADING_UPDATE_GPS_MIN_SPEED   5.f

Definition at line 154 of file ins_float_invariant.c.

INS_INV_LB

#define INS_INV_LB   6.f

Definition at line 89 of file ins_float_invariant.c.

INS_INV_LV

#define INS_INV_LV   2.f

Definition at line 85 of file ins_float_invariant.c.

INS_INV_MH

#define INS_INV_MH   0.2f

Definition at line 101 of file ins_float_invariant.c.

INS_INV_MV

#define INS_INV_MV   8.f

Definition at line 93 of file ins_float_invariant.c.

INS_INV_MVZ

#define INS_INV_MVZ   15.f

Definition at line 97 of file ins_float_invariant.c.

INS_INV_NH

#define INS_INV_NH   1.2f

Definition at line 113 of file ins_float_invariant.c.

INS_INV_NX

#define INS_INV_NX   0.8f

Definition at line 105 of file ins_float_invariant.c.

INS_INV_NXZ

#define INS_INV_NXZ   0.5f

Definition at line 109 of file ins_float_invariant.c.

INS_INV_OB

#define INS_INV_OB   1.f

Definition at line 121 of file ins_float_invariant.c.

INS_INV_OV

#define INS_INV_OV   1.2f

Definition at line 117 of file ins_float_invariant.c.

INS_INV_RH

#define INS_INV_RH   8.f

Definition at line 129 of file ins_float_invariant.c.

INS_INV_RV

#define INS_INV_RV   4.f

Definition at line 125 of file ins_float_invariant.c.

INS_INV_SH

#define INS_INV_SH   0.01f

Definition at line 133 of file ins_float_invariant.c.

MAG_FROZEN_COUNT

#define MAG_FROZEN_COUNT   30

Definition at line 564 of file ins_float_invariant.c.

Function Documentation

error_output()

static void error_output ( struct InsFloatInv *  _ins )

inlinestatic

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

Definition at line 651 of file ins_float_invariant.c.

References inv_command::accel, inv_state::as, B, inv_measures::baro_alt, InsFloatInv::cmd, InsFloatInv::corr, Ex, GpsState::fix, float_quat_invert(), float_quat_vmult(), FLOAT_VECT3_ZERO, InsFloatInv::gains, gps, GPS_FIX_3D, inv_state::hb, 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, InsFloatInv::state, 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:

float_quat_vmul_right()

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

References FLOAT_QUAT_EXTRACT, FloatQuat::qi, QUAT_ASSIGN, v1, v2, VECT3_ADD, VECT3_CROSS_PRODUCT, VECT3_DOT_PRODUCT, and VECT3_SMUL.

Referenced by invariant_model().

Here is the caller graph for this function:

init_invariant_state()

static void init_invariant_state ( void  )

inlinestatic

Definition at line 172 of file ins_float_invariant.c.

References inv_state::as, inv_measures::baro_alt, inv_state::bias, 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:

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:

invariant_model()

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

References A, 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(), 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, s, 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:

send_inv_filter()

static void send_inv_filter ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 193 of file ins_float_invariant.c.

References inv_state::as, inv_measures::baro_alt, inv_state::bias, dev, float_eulers_of_quat(), inv_state::hb, ins_float_inv, InsFloatInv::meas, FloatRates::p, FloatEulers::phi, inv_state::pos, inv_measures::pos_gps, FloatEulers::psi, FloatRates::q, FloatQuat::qi, inv_state::quat, FloatRates::r, inv_state::speed, InsFloatInv::state, FloatEulers::theta, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

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:

Variable Documentation

A

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

static

Definition at line 1 of file ins_float_invariant.c.

Referenced by ins_float_invariant_propagate(), and invariant_model().

ins_baro_initialized

bool ins_baro_initialized

Definition at line 147 of file ins_float_invariant.c.

Referenced by init_invariant_state(), and ins_float_invariant_update_baro().

ins_float_inv

struct InsFloatInv ins_float_inv

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

ins_gps_fix_once

bool ins_gps_fix_once

Definition at line 150 of file ins_float_invariant.c.

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