Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_float_invariant.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013 Jean-Philippe Condomines, Gautier Hattenberger
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
27 #ifndef INS_FLOAT_INVARIANT_H
28 #define INS_FLOAT_INVARIANT_H
29 
30 #include "modules/ins/ins.h"
31 #include "modules/gps/gps.h"
34 
37 #define INV_STATE_DIM 15
38 
41 struct inv_state {
42  struct FloatQuat quat;
43  struct FloatRates bias;
44  struct NedCoor_f speed;
45  struct NedCoor_f pos;
46  float hb;
47  float as;
48 //float cs; ///< Estimated magnetic sensitivity
49 };
50 
53 #define INV_MEASURE_DIM 10
54 
57 struct inv_measures {
58  struct NedCoor_f pos_gps;
60  struct FloatVect3 mag;
61  float baro_alt;
62 };
63 
66 #define INV_COMMAND_DIM 6
67 
70 struct inv_command {
71  struct FloatRates rates;
72  struct FloatVect3 accel;
73 };
74 
77 struct inv_correction_gains {
78  struct FloatVect3 LE;
79  struct FloatVect3 ME;
80  struct FloatVect3 NE;
81  struct FloatVect3 OE;
82  float RE;
83  float SE;
84 };
85 
88 struct inv_gains {
89  float lv;
90  float lb;
91  float mv;
92  float mvz;
93  float mh;
94  float nx;
95  float nxz;
96  float nh;
97  float ov;
98  float ob;
99  float rv;
100  float rh;
101  float sh;
102 };
103 
106 struct InsFloatInv {
107  struct inv_state state;
109  struct inv_command cmd;
111  struct inv_gains gains;
112 
113  bool reset;
114 
117 
120 };
121 
122 extern struct InsFloatInv ins_float_inv;
123 
124 extern void ins_float_invariant_init(void);
125 extern void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i);
126 extern void ins_float_invariant_align(struct FloatRates *lp_gyro,
127  struct FloatVect3 *lp_accel,
128  struct FloatVect3 *lp_mag);
129 extern void ins_float_invariant_propagate(struct FloatRates* gyro,
130  struct FloatVect3* accel, float dt);
131 extern void ins_float_invariant_update_mag(struct FloatVect3* mag);
132 extern void ins_float_invariant_update_baro(float pressure);
133 extern void ins_float_invariant_update_gps(struct GpsState *gps_s);
134 
135 #endif /* INS_FLOAT_INVARIANT_H */
136 
inv_gains::mvz
float mvz
Tuning parameter of vertical speed error on speed.
Definition: ins_float_invariant.h:92
inv_gains::nxz
float nxz
Tuning parameter of vertical position error on position.
Definition: ins_float_invariant.h:95
InsFloatInv::reset
bool reset
flag to request reset/reinit the filter
Definition: ins_float_invariant.h:113
ins_float_invariant_align
void ins_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
Definition: ins_float_invariant.c:321
OrientationReps
Definition: pprz_orientation_conversion.h:79
ins.h
NedCoor_f
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_float.h:63
inv_correction_gains::ME
struct FloatVect3 ME
Correction gains on gyro biases.
Definition: ahrs_float_invariant.h:72
inv_gains::rh
float rh
Tuning parameter of baro error on accel biases (vertical projection)
Definition: ins_float_invariant.h:100
inv_gains::sh
float sh
Tuning parameter of baro error on baro bias.
Definition: ins_float_invariant.h:101
InsFloatInv::cmd
struct inv_command cmd
command vector
Definition: ins_float_invariant.h:109
inv_command::rates
struct FloatRates rates
Input gyro rates.
Definition: ahrs_float_invariant.h:65
inv_state::speed
struct NedCoor_f speed
Estimates speed.
Definition: ins_float_invariant.h:44
InsFloatInv::corr
struct inv_correction_gains corr
correction gains
Definition: ins_float_invariant.h:110
GpsState
data structure for GPS information
Definition: gps.h:87
inv_state
Invariant filter state.
Definition: ahrs_float_invariant.h:40
ins_float_inv_set_body_to_imu_quat
void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ins_float_invariant.c:751
inv_correction_gains
Invariant filter correction gains.
Definition: ahrs_float_invariant.h:70
pprz_algebra_float.h
Paparazzi floating point algebra.
inv_state::bias
struct FloatRates bias
Estimated gyro biases.
Definition: ahrs_float_invariant.h:42
inv_gains
Invariant filter tuning gains.
Definition: ahrs_float_invariant.h:79
inv_correction_gains::RE
float RE
Correction gains on accel bias.
Definition: ins_float_invariant.h:82
inv_gains::ov
float ov
Tuning parameter of speed error on gyro biases.
Definition: ins_float_invariant.h:97
inv_gains::lv
float lv
Tuning parameter of speed error on attitude.
Definition: ins_float_invariant.h:89
inv_gains::mv
float mv
Tuning parameter of horizontal speed error on speed.
Definition: ins_float_invariant.h:91
InsFloatInv
Invariant filter structure.
Definition: ins_float_invariant.h:106
inv_state::pos
struct NedCoor_f pos
Estimates position.
Definition: ins_float_invariant.h:45
FloatVect3
Definition: pprz_algebra_float.h:54
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
InsFloatInv::gains
struct inv_gains gains
tuning gains
Definition: ins_float_invariant.h:111
gps.h
Device independent GPS code (interface)
inv_correction_gains::SE
float SE
Correction gains on barometer bias.
Definition: ins_float_invariant.h:83
InsFloatInv::meas
struct inv_measures meas
measurement vector
Definition: ins_float_invariant.h:108
InsFloatInv::state
struct inv_state state
state vector
Definition: ins_float_invariant.h:107
inv_gains::nh
float nh
Tuning parameter of baro error on vertical position.
Definition: ins_float_invariant.h:96
inv_measures::pos_gps
struct NedCoor_f pos_gps
Measured gps position.
Definition: ins_float_invariant.h:58
inv_correction_gains::NE
float NE
Correction gains on accel bias.
Definition: ahrs_float_invariant.h:73
inv_gains::lb
float lb
Tuning parameter of mag error on attitude.
Definition: ins_float_invariant.h:90
ins_float_invariant_init
void ins_float_invariant_init(void)
Definition: ins_float_invariant.c:219
inv_gains::mh
float mh
Tuning parameter of baro error on vertical speed.
Definition: ins_float_invariant.h:93
inv_measures::speed_gps
struct NedCoor_f speed_gps
Measured gps speed.
Definition: ins_float_invariant.h:59
InsFloatInv::mag_h
struct FloatVect3 mag_h
Definition: ins_float_invariant.h:118
inv_command
Invariant filter command vector.
Definition: ahrs_float_invariant.h:64
inv_correction_gains::OE
float OE
Correction gains on magnetometer sensitivity.
Definition: ahrs_float_invariant.h:74
inv_correction_gains::LE
struct FloatVect3 LE
Correction gains on attitude.
Definition: ahrs_float_invariant.h:71
inv_measures
Invariant filter measurement vector.
Definition: ahrs_float_invariant.h:53
InsFloatInv::is_aligned
bool is_aligned
Definition: ins_float_invariant.h:119
pprz_orientation_conversion.h
inv_command::accel
struct FloatVect3 accel
Input accelerometers.
Definition: ins_float_invariant.h:72
ins_float_invariant_update_baro
void ins_float_invariant_update_baro(float pressure)
Definition: ins_float_invariant.c:529
InsFloatInv::body_to_imu
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ins_float_invariant.h:116
inv_state::as
float as
Estimated accelerometer sensitivity.
Definition: ahrs_float_invariant.h:44
inv_gains::nx
float nx
Tuning parameter of horizontal position error on position.
Definition: ins_float_invariant.h:94
inv_measures::mag
struct FloatVect3 mag
Measured magnetic field.
Definition: ahrs_float_invariant.h:55
ins_float_invariant_update_mag
void ins_float_invariant_update_mag(struct FloatVect3 *mag)
Definition: ins_float_invariant.c:565
ins_float_invariant_update_gps
void ins_float_invariant_update_gps(struct GpsState *gps_s)
Definition: ins_float_invariant.c:445
ins_float_inv
struct InsFloatInv ins_float_inv
Definition: ins_float_invariant.c:137
ins_float_invariant_propagate
void ins_float_invariant_propagate(struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
Definition: ins_float_invariant.c:343
inv_state::quat
struct FloatQuat quat
Estimated attitude (quaternion)
Definition: ahrs_float_invariant.h:41
inv_gains::rv
float rv
Tuning parameter of speed error on accel biases.
Definition: ins_float_invariant.h:99
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
inv_state::hb
float hb
Estimates barometers bias.
Definition: ins_float_invariant.h:46
inv_measures::baro_alt
float baro_alt
Measured barometric altitude.
Definition: ins_float_invariant.h:61
inv_gains::ob
float ob
Tuning parameter of mag error on gyro biases.
Definition: ins_float_invariant.h:98