Paparazzi UAS  v5.15_devel-47-g0391b4d
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_float_invariant.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 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 
30 
32 
33 #include "generated/airframe.h"
34 
36 #include "math/pprz_algebra_int.h"
37 #include "math/pprz_rk_float.h"
38 
39 #if SEND_INVARIANT_FILTER
41 #endif
42 
43 /*---------Invariant Observers-----------
44  *
45  * State vector :
46  * x = [q0 q1 q2 q3 wb1 wb2 wb3 as cs]
47  *
48  *
49  * Dynamic model (dim = 9) :
50  * x_qdot = 0.5 * x_quat * ( x_rates - x_bias );
51  * x_bias_dot = 0;
52  * x_asdot = 0;
53  * x_csdot = 0;
54  *
55  * Observation model (dim = 6) :
56  * ya = as * (q)-1 * A * q; (A : accelerometers)
57  * yc = cs * (q)-1 * C * q; (C = A x B (cross product), B : magnetometers)
58  *
59  *------------------------------------------*/
60 
61 // Default values for the tuning gains
62 // Tuning parameter of accel and mag on attitude
63 #ifndef AHRS_INV_LX
64 #define AHRS_INV_LX 2. * (0.06 + 0.1)
65 #endif
66 // Tuning parameter of accel and mag on attitude
67 #ifndef AHRS_INV_LY
68 #define AHRS_INV_LY 2. * (0.06 + 0.06)
69 #endif
70 // Tuning parameter of accel and mag on attitude
71 #ifndef AHRS_INV_LZ
72 #define AHRS_INV_LZ 2. * (0.1 + 0.06)
73 #endif
74 // Tuning parameter of accel and mag on gyro bias
75 #ifndef AHRS_INV_MX
76 #define AHRS_INV_MX 2. * 0.05 * (0.06 + 0.1)
77 #endif
78 // Tuning parameter of accel and mag on gyro bias
79 #ifndef AHRS_INV_MY
80 #define AHRS_INV_MY 2. * 0.05 * (0.06 + 0.06)
81 #endif
82 // Tuning parameter of accel and mag on gyro bias
83 #ifndef AHRS_INV_MZ
84 #define AHRS_INV_MZ 2. * 0.05 * (0.1 + 0.06)
85 #endif
86 // Tuning parameter of accel and mag on accel bias
87 #ifndef AHRS_INV_N
88 #define AHRS_INV_N 0.25
89 #endif
90 // Tuning parameter of accel and mag on mag bias
91 #ifndef AHRS_INV_O
92 #define AHRS_INV_O 0.5
93 #endif
94 
96 
97 /* earth gravity model */
98 static const struct FloatVect3 A = { 0.f, 0.f, -9.81f };
99 
100 /* earth magnetic model */
101 #define B ahrs_float_inv.mag_h
102 
103 /* error computation */
104 static inline void error_output(struct AhrsFloatInv *_ins);
105 
106 /* propagation model (called by runge-kutta library) */
107 static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m);
108 
109 
113 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
114  struct FloatVect3 *vi);
115 
116 /* init state and measurements */
117 static inline void init_invariant_state(void)
118 {
119  // init state
122  ahrs_float_inv.state.as = 1.0f;
123  ahrs_float_inv.state.cs = 1.0f;
124 
125  // init measures
128 
129 }
130 
132 {
133  // init magnetometers
134 
135  ahrs_float_inv.mag_h.x = AHRS_H_X;
136  ahrs_float_inv.mag_h.y = AHRS_H_Y;
137  ahrs_float_inv.mag_h.z = AHRS_H_Z;
138 
139  // init state and measurements
141 
142  // init gains
151 
152  ahrs_float_inv.is_aligned = false;
153  ahrs_float_inv.reset = false;
154 }
155 
157  struct FloatVect3 *lp_accel,
158  struct FloatVect3 *lp_mag)
159 {
160  /* Compute an initial orientation from accel and mag directly as quaternion */
162 
163  /* use average gyro as initial value for bias */
164  ahrs_float_inv.state.bias = *lp_gyro;
165 
166  // ins and ahrs are now running
167  ahrs_float_inv.is_aligned = true;
168 }
169 
170 void ahrs_float_invariant_propagate(struct FloatRates* gyro, float dt)
171 {
172  // realign all the filter if needed
173  // a complete init cycle is required
174  if (ahrs_float_inv.reset) {
175  ahrs_float_inv.reset = false;
176  ahrs_float_inv.is_aligned = false;
178  }
179 
180  // fill command vector
181  struct FloatRates gyro_meas_body;
182  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
183  float_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro);
184  ahrs_float_inv.cmd.rates = gyro_meas_body;
185 
186  // update correction gains
188 
189  // propagate model
190  struct inv_state new_state;
191  runge_kutta_4_float((float *)&new_state,
194  invariant_model, dt);
195  ahrs_float_inv.state = new_state;
196 
197  // normalize quaternion
199 
200  //------------------------------------------------------------//
201 
202 #if SEND_INVARIANT_FILTER
203  struct FloatEulers eulers;
204  float foo = 0.f;
206  RunOnceEvery(3,
207  pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device,
208  AC_ID,
210  &eulers.phi,
211  &eulers.theta,
212  &eulers.psi,
213  &foo,
214  &foo,
215  &foo,
216  &foo,
217  &foo,
218  &foo,
224  &foo,
225  &foo);
226  );
227 #endif
228 
229 }
230 
232 {
233  ahrs_float_inv.meas.accel = *accel;
234 }
235 
236 // assume mag is dead when values are not moving anymore
237 #define MAG_FROZEN_COUNT 30
238 
240 {
241  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
242  static int32_t last_mx = 0;
243 
244  if (last_mx == mag->x) {
245  mag_frozen_count--;
246  if (mag_frozen_count == 0) {
247  // if mag is dead, better set measurements to zero
249  mag_frozen_count = MAG_FROZEN_COUNT;
250  }
251  } else {
252  // values are moving
253  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
254  // new values in body frame
255  float_rmat_transp_vmult(&ahrs_float_inv.meas.mag, body_to_imu_rmat, mag);
256  // reset counter
257  mag_frozen_count = MAG_FROZEN_COUNT;
258  }
259  last_mx = mag->x;
260 }
261 
266 static inline void invariant_model(float *o, const float *x, const int n, const float *u,
267  const int m __attribute__((unused)))
268 {
269 
270 #pragma GCC diagnostic push // require GCC 4.6
271 #pragma GCC diagnostic ignored "-Wcast-qual"
272  struct inv_state *s = (struct inv_state *)x;
273  struct inv_command *c = (struct inv_command *)u;
274 #pragma GCC diagnostic pop // require GCC 4.6
275  struct inv_state s_dot;
276  struct FloatRates rates_unbiased;
277  struct FloatVect3 tmp_vect;
278  struct FloatQuat tmp_quat;
279 
280  // test accel sensitivity
281  if (fabs(s->as) < 0.1) {
282  // too small, return x_dot = 0 to avoid division by 0
283  float_vect_zero(o, n);
284  // TODO set ins state to error
285  return;
286  }
287 
288  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
289  RATES_DIFF(rates_unbiased, c->rates, s->bias);
290  /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
291  float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
292 
293  float_quat_vmul_right(&tmp_quat, &(s->quat), &ahrs_float_inv.corr.LE);
294  QUAT_ADD(s_dot.quat, tmp_quat);
295 
296  float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat);
297  QUAT_SMUL(tmp_quat, s->quat, norm2_r);
298  QUAT_ADD(s_dot.quat, tmp_quat);
299 
300  /* bias_dot = q-1 * (ME) * q */
301  float_quat_vmult(&tmp_vect, &(s->quat), &ahrs_float_inv.corr.ME);
302  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
303 
304  /* as_dot = as * NE */
305  s_dot.as = (s->as) * (ahrs_float_inv.corr.NE);
306 
307  /* cs_dot = OE */
308  s_dot.cs = ahrs_float_inv.corr.OE;
309 
310  // set output
311  memcpy(o, &s_dot, n * sizeof(float));
312 }
313 
318 static inline void error_output(struct AhrsFloatInv *_ahrs)
319 {
320 
321  struct FloatVect3 YAt, C, YC, YCt, Ec, Ea;
322 
323  // test accel sensitivity
324  if (fabs(_ahrs->state.as) < 0.1) {
325  // too small, don't do anything to avoid division by 0
326  return;
327  }
328 
329  /* C = A X B Cross product */
330  VECT3_CROSS_PRODUCT(C, A, B);
331  /* YC = YA X YB Cross product */
332  VECT3_CROSS_PRODUCT(YC, _ahrs->meas.accel, _ahrs->meas.mag);
333  /* YCt = (1 / cs) * q * YC * q-1 => invariant output on magnetometers */
334  struct FloatQuat q_b2n;
335  float_quat_invert(&q_b2n, &(_ahrs->state.quat));
336  float_quat_vmult(&YCt, &q_b2n, &YC);
337  VECT3_SMUL(YCt, YCt, 1. / (_ahrs->state.cs));
338 
339  /* YAt = q * yA * q-1 => invariant output on accelerometers */
340  float_quat_vmult(&YAt, &q_b2n, &(_ahrs->meas.accel));
341  VECT3_SMUL(YAt, YAt, 1. / (_ahrs->state.as));
342 
343  /*--------- E = ( ŷ - y ) ----------*/
344 
345  /* EC = ( C - YCt ) */
346  VECT3_DIFF(Ec, C, YCt);
347  /* EA = ( A - YAt ) */
348  VECT3_DIFF(Ea, A, YAt);
349 
350  /*-------------- Gains --------------*/
351 
352  /**** LaEa + LcEc *****/
353  _ahrs->corr.LE.x = (-_ahrs->gains.lx * Ea.y) / (2.f * A.z);
354  _ahrs->corr.LE.y = (_ahrs->gains.ly * Ea.x) / (2.f * A.z);
355  _ahrs->corr.LE.z = (-_ahrs->gains.lz * Ec.x) / (B.x * 2.f * A.z);
356 
357  /***** MaEa + McEc ********/
358  _ahrs->corr.ME.x = (_ahrs->gains.mx * Ea.y) / (2.f * A.z);
359  _ahrs->corr.ME.y = (-_ahrs->gains.my * Ea.x)/(2.f * A.z);
360  _ahrs->corr.ME.z = (_ahrs->gains.mz * Ec.x) / (B.x * 2.f * A.z);
361 
362  /****** NaEa + NcEc ********/
363  _ahrs->corr.NE = (-_ahrs->gains.n * Ea.z) / A.z;
364 
365  /****** OaEa + OcEc ********/
366  _ahrs->corr.OE = (-_ahrs->gains.o * Ec.y) / (B.x * A.z);
367 }
368 
369 
370 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
371  struct FloatVect3 *vi)
372 {
373  struct FloatVect3 qvec, v1, v2;
374  float qi;
375 
376  FLOAT_QUAT_EXTRACT(qvec, *q);
377  qi = - VECT3_DOT_PRODUCT(*vi, qvec);
378  VECT3_CROSS_PRODUCT(v1, *vi, qvec);
379  VECT3_SMUL(v2, *vi, q->qi);
380  VECT3_ADD(v2, v1);
381  QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
382 }
383 
385 {
387 
388  if (!ahrs_float_inv.is_aligned) {
389  /* Set ltp_to_imu so that body is zero */
390  ahrs_float_inv.state.quat = *q_b2i;
391  }
392 }
393 
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:244
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:250
#define INV_STATE_DIM
Invariant filter state dimension.
struct VehicleInterface vi
Definition: vi.c:30
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
#define INV_COMMAND_DIM
Invariant filter command vector dimension.
bool reset
flag to request reset/reinit the filter
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
float lx
Tuning parameter of accel and mag on attitude (longitudinal subsystem)
struct FloatVect3 ME
Correction gains on gyro biases.
float phi
in radians
float OE
Correction gains on magnetometer sensitivity.
struct OrientationReps body_to_imu
body_to_imu rotation
float n
Tuning parameter of accel and mag on accel bias (scaling subsystem)
struct FloatVect3 accel
Measured accelerometers.
float o
Tuning parameter of accel and mag on mag bias (scaling subsystem)
void ahrs_float_invariant_init(void)
float mx
Tuning parameter of accel and mag on gyro bias (longitudinal subsystem)
Periodic telemetry system header (includes downlink utility and generated code).
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:611
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
static void init_invariant_state(void)
struct FloatQuat quat
Estimated attitude (quaternion)
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
#define AHRS_INV_N
float r
in rad/s
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
#define AHRS_INV_MX
#define FLOAT_RATES_ZERO(_r)
float psi
in radians
#define AHRS_INV_LX
static void invariant_model(float *o, const float *x, const int n, const float *u, const int m)
Compute dynamic mode.
Invariant filter command vector.
float q
in rad/s
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
void ahrs_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
float p
in rad/s
static const struct FloatVect3 A
struct inv_command cmd
command vector
void ahrs_float_invariant_propagate(struct FloatRates *gyro, float dt)
#define B
struct FloatVect3 LE
Correction gains on attitude.
float mz
Tuning parameter of accel and mag on gyro bias (heading subsystem)
float my
Tuning parameter of accel and mag on gyro bias (lateral subsystem)
euler angles
Roation quaternion.
AHRS using invariant filter.
float ly
Tuning parameter of accel and mag on attitude (lateral subsystem)
float theta
in radians
#define FLOAT_VECT3_ZERO(_v)
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
#define MAG_FROZEN_COUNT
void ahrs_float_invariant_update_accel(struct FloatVect3 *accel)
void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Paparazzi floating point algebra.
float NE
Correction gains on accel bias.
struct inv_correction_gains corr
correction gains
#define FLOAT_QUAT_NORM2(_q)
static void float_vect_zero(float *a, const int n)
a = 0
unsigned long uint32_t
Definition: types.h:18
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:580
struct FloatRates rates
Input gyro rates.
void ahrs_float_invariant_update_mag(struct FloatVect3 *mag)
#define AHRS_INV_LZ
uint16_t foo
Definition: main_demo5.c:59
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
static void float_quat_normalize(struct FloatQuat *q)
static void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
#define AHRS_INV_MZ
float lz
Tuning parameter of accel and mag on attitude (heading subsystem)
#define AHRS_INV_O
struct FloatVect3 mag
Measured magnetic field.
signed long int32_t
Definition: types.h:19
struct FloatRates bias
Estimated gyro biases.
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
float m[3 *3]
Utility functions for fixed point AHRS implementations.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
#define FLOAT_QUAT_EXTRACT(_vo, _qi)
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
float cs
Estimates magnetometer sensitivity.
struct inv_measures meas
measurement vector
rotation matrix
#define AHRS_INV_MY
struct FloatVect3 mag_h
#define AHRS_INV_LY
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:619
struct AhrsFloatInv ahrs_float_inv
Runge-Kutta library (float version)
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
struct inv_gains gains
tuning gains
static void error_output(struct AhrsFloatInv *_ins)
Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error ) ...
Invariant filter state.
angular rates
float as
Estimated accelerometer sensitivity.
struct inv_state state
state vector
Paparazzi fixed point algebra.
static void runge_kutta_4_float(float *xo, const float *x, const int n, const float *u, const int m, void(*f)(float *o, const float *x, const int n, const float *u, const int m), const float dt)
Fourth-Order Runge-Kutta.
void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, struct FloatVect3 *vi)
Right multiplication by a quaternion.
Invariant filter structure.