Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/ins.h"
31 #include "subsystems/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 
float nxz
Tuning parameter of vertical position error on position.
struct FloatVect3 mag_h
struct NedCoor_f speed_gps
Measured gps speed.
struct FloatVect3 ME
Correction gains on gyro biases.
float mv
Tuning parameter of horizontal speed error on speed.
float OE
Correction gains on magnetometer sensitivity.
void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
float ov
Tuning parameter of speed error on gyro biases.
struct InsFloatInv ins_float_inv
float rh
Tuning parameter of baro error on accel biases (vertical projection)
void ins_float_invariant_propagate(struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
bool reset
flag to request reset/reinit the filter
struct FloatVect3 accel
Input accelerometers.
float RE
Correction gains on accel bias.
struct inv_command cmd
command vector
Invariant filter tuning gains.
Integrated Navigation System interface.
void ins_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
float mh
Tuning parameter of baro error on vertical speed.
Invariant filter command vector.
float baro_alt
Measured barometric altitude.
struct FloatVect3 LE
Correction gains on attitude.
Roation quaternion.
struct OrientationReps body_to_imu
body_to_imu rotation
float ob
Tuning parameter of mag error on gyro biases.
float mvz
Tuning parameter of vertical speed error on speed.
void ins_float_invariant_update_baro(float pressure)
vector in North East Down coordinates Units: meters
Paparazzi floating point algebra.
data structure for GPS information
Definition: gps.h:81
float NE
Correction gains on accel bias.
Device independent GPS code (interface)
struct inv_state state
state vector
void ins_float_invariant_init(void)
void ins_float_invariant_update_mag(struct FloatVect3 *mag)
float lb
Tuning parameter of mag error on attitude.
Invariant filter correction gains.
struct inv_gains gains
tuning gains
Invariant filter measurement vector.
float lv
Tuning parameter of speed error on attitude.
struct FloatVect3 mag
Measured magnetic field.
struct inv_measures meas
measurement vector
void ins_float_invariant_update_gps(struct GpsState *gps_s)
float sh
Tuning parameter of baro error on baro bias.
float nh
Tuning parameter of baro error on vertical position.
struct NedCoor_f speed
Estimates speed.
float SE
Correction gains on barometer bias.
float rv
Tuning parameter of speed error on accel biases.
float hb
Estimates barometers bias.
Invariant filter structure.
Generic orientation representation and conversions.
struct NedCoor_f pos
Estimates position.
Invariant filter state.
angular rates
float as
Estimated accelerometer sensitivity.
struct NedCoor_f pos_gps
Measured gps position.
float nx
Tuning parameter of horizontal position error on position.
struct inv_correction_gains corr
correction gains