|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
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"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. | |
| #define | INV_MEASURE_DIM 10 |
| Invariant filter measurement vector dimension. | |
| #define | INV_COMMAND_DIM 6 |
| Invariant filter command vector dimension. | |
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 |
INS using invariant filter.
For more information, please send an email to "jp.condomines@gmail.com"
Definition in file ins_float_invariant.h.
| struct inv_state |
Invariant filter state.
Definition at line 40 of file ahrs_float_invariant.h.
| 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.
| 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.
| 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.
| 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. |
| struct inv_gains |
Invariant filter tuning gains.
Definition at line 79 of file ahrs_float_invariant.h.
| struct InsFloatInv |
Invariant filter structure.
Definition at line 106 of file ins_float_invariant.h.
| 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 |
| #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.
|
extern |
Definition at line 323 of file ins_float_invariant.c.
References ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), ins_float_inv, and stateSetNedToBodyQuat_f().
Referenced by aligner_cb().
|
extern |
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(), 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, LlaCoor_i::lat, LlaCoor_i::lon, ltp_def, ltp_def_from_ecef_i(), nav_utm_east0, nav_utm_north0, nav_utm_zone0, UtmCoor_f::north, register_periodic_telemetry(), send_inv_filter(), stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), stateSetPositionUtm_f(), and UtmCoor_f::zone.
Referenced by ins_float_invariant_wrapper_init().
|
extern |
Definition at line 345 of file ins_float_invariant.c.
References A, ACCELS_BFP_OF_REAL, DefaultChannel, DefaultDevice, error_output(), float_quat_invert(), float_quat_normalize(), float_quat_vmult(), init_invariant_state(), ins_float_inv, INV_COMMAND_DIM, INV_STATE_DIM, invariant_model(), log_started, pprzLogFile, RATES_COPY, RATES_DIFF, runge_kutta_4_float(), sdLogWriteLog(), send_inv_filter(), stateSetAccelBody_i(), stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyQuat_f(), stateSetPositionNed_f(), stateSetSpeedNed_f(), VECT3_ADD, VECT3_COPY, and VECT3_SMUL.
Referenced by gyro_cb().
|
extern |
Definition at line 281 of file ins_float_invariant.c.
References LlaCoor_i::alt, ecef_int_from_gps(), FLOAT_VECT3_ZERO, gps, ins_float_inv, lla_int_from_gps(), ltp_def, ltp_def_from_ecef_i(), stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), and utm_float_from_gps().
Referenced by reset_cb().
|
extern |
Definition at line 301 of file ins_float_invariant.c.
References UtmCoor_f::alt, gps, ins_float_inv, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, ltp_def, ltp_def_from_lla_i(), stateGetLlaOrigin_i(), stateGetUtmOrigin_f(), stateSetLocalOrigin_i(), and stateSetLocalUtmOrigin_f().
Referenced by reset_cb().
|
extern |
Definition at line 532 of file ins_float_invariant.c.
References alpha, ins_baro_initialized, ins_float_inv, pprz_isa_height_of_pressure(), and PPRZ_ISA_SEA_LEVEL_PRESSURE.
Referenced by baro_cb().
|
extern |
Definition at line 448 of file ins_float_invariant.c.
References UtmCoor_f::alt, GpsState::cacc, course, GpsState::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, nav_utm_zone0, NED_FLOAT_OF_BFP, ned_of_ecef_point_i(), ned_of_ecef_vect_f(), ned_vel_float_from_gps(), UtmCoor_f::north, FloatEulers::psi, state, stateGetHmslOrigin_f(), stateGetNedOrigin_f(), stateGetNedOrigin_i(), stateGetUtmOrigin_f(), utm_float_from_gps(), FloatVect2::x, FloatVect3::x, FloatVect2::y, and FloatVect3::y.
Referenced by gps_cb().
|
extern |
Definition at line 568 of file ins_float_invariant.c.
References FLOAT_VECT3_ZERO, ins_float_inv, MAG_FROZEN_COUNT, VECT3_COPY, and FloatVect3::x.
Referenced by ins_float_invariant_update_gps(), and mag_cb().
|
extern |
Definition at line 137 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().