Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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
182 
183  // update correction gains
185 
186  // propagate model
187  struct inv_state new_state;
188  runge_kutta_4_float((float *)&new_state,
191  invariant_model, dt);
192  ahrs_float_inv.state = new_state;
193 
194  // normalize quaternion
196 
197  //------------------------------------------------------------//
198 
199 #if SEND_INVARIANT_FILTER
200  struct FloatEulers eulers;
201  float foo = 0.f;
203  RunOnceEvery(3,
204  pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device,
205  AC_ID,
207  &eulers.phi,
208  &eulers.theta,
209  &eulers.psi,
210  &foo,
211  &foo,
212  &foo,
213  &foo,
214  &foo,
215  &foo,
221  &foo,
222  &foo);
223  );
224 #endif
225 
226 }
227 
229 {
230  ahrs_float_inv.meas.accel = *accel;
231 }
232 
233 // assume mag is dead when values are not moving anymore
234 #define MAG_FROZEN_COUNT 30
235 
237 {
238  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
239  static int32_t last_mx = 0;
240 
241  if (last_mx == mag->x) {
242  mag_frozen_count--;
243  if (mag_frozen_count == 0) {
244  // if mag is dead, better set measurements to zero
246  mag_frozen_count = MAG_FROZEN_COUNT;
247  }
248  } else {
249  // new values in body frame
251  // reset counter
252  mag_frozen_count = MAG_FROZEN_COUNT;
253  }
254  last_mx = mag->x;
255 }
256 
261 static inline void invariant_model(float *o, const float *x, const int n, const float *u,
262  const int m __attribute__((unused)))
263 {
264 
265 #pragma GCC diagnostic push // require GCC 4.6
266 #pragma GCC diagnostic ignored "-Wcast-qual"
267  struct inv_state *s = (struct inv_state *)x;
268  struct inv_command *c = (struct inv_command *)u;
269 #pragma GCC diagnostic pop // require GCC 4.6
270  struct inv_state s_dot;
271  struct FloatRates rates_unbiased;
272  struct FloatVect3 tmp_vect;
273  struct FloatQuat tmp_quat;
274 
275  // test accel sensitivity
276  if (fabs(s->as) < 0.1) {
277  // too small, return x_dot = 0 to avoid division by 0
278  float_vect_zero(o, n);
279  // TODO set ins state to error
280  return;
281  }
282 
283  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
284  RATES_DIFF(rates_unbiased, c->rates, s->bias);
285  /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
286  float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
287 
288  float_quat_vmul_right(&tmp_quat, &(s->quat), &ahrs_float_inv.corr.LE);
289  QUAT_ADD(s_dot.quat, tmp_quat);
290 
291  float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat);
292  QUAT_SMUL(tmp_quat, s->quat, norm2_r);
293  QUAT_ADD(s_dot.quat, tmp_quat);
294 
295  /* bias_dot = q-1 * (ME) * q */
296  float_quat_vmult(&tmp_vect, &(s->quat), &ahrs_float_inv.corr.ME);
297  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
298 
299  /* as_dot = as * NE */
300  s_dot.as = (s->as) * (ahrs_float_inv.corr.NE);
301 
302  /* cs_dot = OE */
303  s_dot.cs = ahrs_float_inv.corr.OE;
304 
305  // set output
306  memcpy(o, &s_dot, n * sizeof(float));
307 }
308 
313 static inline void error_output(struct AhrsFloatInv *_ahrs)
314 {
315 
316  struct FloatVect3 YAt, C, YC, YCt, Ec, Ea;
317 
318  // test accel sensitivity
319  if (fabs(_ahrs->state.as) < 0.1) {
320  // too small, don't do anything to avoid division by 0
321  return;
322  }
323 
324  /* C = A X B Cross product */
325  VECT3_CROSS_PRODUCT(C, A, B);
326  /* YC = YA X YB Cross product */
327  VECT3_CROSS_PRODUCT(YC, _ahrs->meas.accel, _ahrs->meas.mag);
328  /* YCt = (1 / cs) * q * YC * q-1 => invariant output on magnetometers */
329  struct FloatQuat q_b2n;
330  float_quat_invert(&q_b2n, &(_ahrs->state.quat));
331  float_quat_vmult(&YCt, &q_b2n, &YC);
332  VECT3_SMUL(YCt, YCt, 1. / (_ahrs->state.cs));
333 
334  /* YAt = q * yA * q-1 => invariant output on accelerometers */
335  float_quat_vmult(&YAt, &q_b2n, &(_ahrs->meas.accel));
336  VECT3_SMUL(YAt, YAt, 1. / (_ahrs->state.as));
337 
338  /*--------- E = ( ŷ - y ) ----------*/
339 
340  /* EC = ( C - YCt ) */
341  VECT3_DIFF(Ec, C, YCt);
342  /* EA = ( A - YAt ) */
343  VECT3_DIFF(Ea, A, YAt);
344 
345  /*-------------- Gains --------------*/
346 
347  /**** LaEa + LcEc *****/
348  _ahrs->corr.LE.x = (-_ahrs->gains.lx * Ea.y) / (2.f * A.z);
349  _ahrs->corr.LE.y = (_ahrs->gains.ly * Ea.x) / (2.f * A.z);
350  _ahrs->corr.LE.z = (-_ahrs->gains.lz * Ec.x) / (B.x * 2.f * A.z);
351 
352  /***** MaEa + McEc ********/
353  _ahrs->corr.ME.x = (_ahrs->gains.mx * Ea.y) / (2.f * A.z);
354  _ahrs->corr.ME.y = (-_ahrs->gains.my * Ea.x)/(2.f * A.z);
355  _ahrs->corr.ME.z = (_ahrs->gains.mz * Ec.x) / (B.x * 2.f * A.z);
356 
357  /****** NaEa + NcEc ********/
358  _ahrs->corr.NE = (-_ahrs->gains.n * Ea.z) / A.z;
359 
360  /****** OaEa + OcEc ********/
361  _ahrs->corr.OE = (-_ahrs->gains.o * Ec.y) / (B.x * A.z);
362 }
363 
364 
365 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
366  struct FloatVect3 *vi)
367 {
368  struct FloatVect3 qvec, v1, v2;
369  float qi;
370 
371  FLOAT_QUAT_EXTRACT(qvec, *q);
372  qi = - VECT3_DOT_PRODUCT(*vi, qvec);
373  VECT3_CROSS_PRODUCT(v1, *vi, qvec);
374  VECT3_SMUL(v2, *vi, q->qi);
375  VECT3_ADD(v2, v1);
376  QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
377 }
#define AHRS_INV_LZ
#define B
struct AhrsFloatInv ahrs_float_inv
void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, struct FloatVect3 *vi)
Right multiplication by a quaternion.
#define AHRS_INV_LY
void ahrs_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
static void init_invariant_state(void)
#define AHRS_INV_O
#define AHRS_INV_MY
static const struct FloatVect3 A
static void error_output(struct AhrsFloatInv *_ins)
Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error )
static void invariant_model(float *o, const float *x, const int n, const float *u, const int m)
Compute dynamic mode.
#define AHRS_INV_MZ
#define AHRS_INV_LX
#define AHRS_INV_N
void ahrs_float_invariant_init(void)
void ahrs_float_invariant_propagate(struct FloatRates *gyro, float dt)
#define MAG_FROZEN_COUNT
void ahrs_float_invariant_update_accel(struct FloatVect3 *accel)
#define AHRS_INV_MX
void ahrs_float_invariant_update_mag(struct FloatVect3 *mag)
AHRS using invariant filter.
#define INV_STATE_DIM
Invariant filter state dimension.
struct inv_measures meas
measurement vector
#define INV_COMMAND_DIM
Invariant filter command vector dimension.
struct inv_command cmd
command vector
bool reset
flag to request reset/reinit the filte
struct inv_correction_gains corr
correction gains
struct inv_gains gains
tuning gains
struct inv_state state
state vector
struct FloatVect3 mag_h
Invariant filter structure.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
Utility functions for fixed point AHRS implementations.
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
static void float_quat_normalize(struct FloatQuat *q)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
static void float_vect_zero(float *a, const int n)
a = 0
#define FLOAT_QUAT_EXTRACT(_vo, _qi)
#define FLOAT_VECT3_ZERO(_v)
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
#define FLOAT_QUAT_NORM2(_q)
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
static void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
#define FLOAT_RATES_ZERO(_r)
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
euler angles
Roation quaternion.
angular rates
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:244
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:580
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:611
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:619
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:250
float lx
Tuning parameter of accel and mag on attitude (longitudinal subsystem)
float o
Tuning parameter of accel and mag on mag bias (scaling subsystem)
float n
Tuning parameter of accel and mag on accel bias (scaling subsystem)
float mz
Tuning parameter of accel and mag on gyro bias (heading subsystem)
struct FloatRates bias
Estimated gyro biases.
float OE
Correction gains on magnetometer sensitivity.
float ly
Tuning parameter of accel and mag on attitude (lateral subsystem)
struct FloatVect3 LE
Correction gains on attitude.
float lz
Tuning parameter of accel and mag on attitude (heading subsystem)
struct FloatVect3 ME
Correction gains on gyro biases.
float as
Estimated accelerometer sensitivity.
float cs
Estimates magnetometer sensitivity.
struct FloatVect3 mag
Measured magnetic field.
struct FloatQuat quat
Estimated attitude (quaternion)
float NE
Correction gains on accel bias.
struct FloatRates rates
Input gyro rates.
float mx
Tuning parameter of accel and mag on gyro bias (longitudinal subsystem)
struct FloatVect3 accel
Measured accelerometers.
float my
Tuning parameter of accel and mag on gyro bias (lateral subsystem)
Invariant filter command vector.
Invariant filter state.
static uint32_t s
uint16_t foo
Definition: main_demo5.c:58
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
Runge-Kutta library (float version)
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.
Periodic telemetry system header (includes downlink utility and generated code).
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78