Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
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 
33 
34 #include "generated/airframe.h"
35 
37 #include "math/pprz_algebra_int.h"
38 #include "math/pprz_rk_float.h"
39 
40 #if SEND_INVARIANT_FILTER
42 #endif
43 
44 /*---------Invariant Observers-----------
45  *
46  * State vector :
47  * x = [q0 q1 q2 q3 wb1 wb2 wb3 as cs]
48  *
49  *
50  * Dynamic model (dim = 9) :
51  * x_qdot = 0.5 * x_quat * ( x_rates - x_bias );
52  * x_bias_dot = 0;
53  * x_asdot = 0;
54  * x_csdot = 0;
55  *
56  * Observation model (dim = 6) :
57  * ya = as * (q)-1 * A * q; (A : accelerometers)
58  * yc = cs * (q)-1 * C * q; (C = A x B (cross product), B : magnetometers)
59  *
60  *------------------------------------------*/
61 
62 // Default values for the tuning gains
63 // Tuning parameter of accel and mag on attitude
64 #ifndef AHRS_INV_LX
65 #define AHRS_INV_LX 2. * (0.06 + 0.1)
66 #endif
67 // Tuning parameter of accel and mag on attitude
68 #ifndef AHRS_INV_LY
69 #define AHRS_INV_LY 2. * (0.06 + 0.06)
70 #endif
71 // Tuning parameter of accel and mag on attitude
72 #ifndef AHRS_INV_LZ
73 #define AHRS_INV_LZ 2. * (0.1 + 0.06)
74 #endif
75 // Tuning parameter of accel and mag on gyro bias
76 #ifndef AHRS_INV_MX
77 #define AHRS_INV_MX 2. * 0.05 * (0.06 + 0.1)
78 #endif
79 // Tuning parameter of accel and mag on gyro bias
80 #ifndef AHRS_INV_MY
81 #define AHRS_INV_MY 2. * 0.05 * (0.06 + 0.06)
82 #endif
83 // Tuning parameter of accel and mag on gyro bias
84 #ifndef AHRS_INV_MZ
85 #define AHRS_INV_MZ 2. * 0.05 * (0.1 + 0.06)
86 #endif
87 // Tuning parameter of accel and mag on accel bias
88 #ifndef AHRS_INV_N
89 #define AHRS_INV_N 0.25
90 #endif
91 // Tuning parameter of accel and mag on mag bias
92 #ifndef AHRS_INV_O
93 #define AHRS_INV_O 0.5
94 #endif
95 
97 
98 /* earth gravity model */
99 static const struct FloatVect3 A = { 0.f, 0.f, -9.81f };
100 
101 /* earth magnetic model */
102 #define B ahrs_float_inv.mag_h
103 
104 /* error computation */
105 static inline void error_output(struct AhrsFloatInv *_ins);
106 
107 /* propagation model (called by runge-kutta library) */
108 static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m);
109 
110 
114 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
115  struct FloatVect3 *vi);
116 
117 /* init state and measurements */
118 static inline void init_invariant_state(void)
119 {
120  // init state
123  ahrs_float_inv.state.as = 1.0f;
124  ahrs_float_inv.state.cs = 1.0f;
125 
126  // init measures
129 
130 }
131 
133 {
134  // init magnetometers
135 
136  ahrs_float_inv.mag_h.x = AHRS_H_X;
137  ahrs_float_inv.mag_h.y = AHRS_H_Y;
138  ahrs_float_inv.mag_h.z = AHRS_H_Z;
139 
140  // init state and measurements
142 
143  // init gains
152 
153  ahrs_float_inv.is_aligned = false;
154  ahrs_float_inv.reset = false;
155 }
156 
158  struct FloatVect3 *lp_accel,
159  struct FloatVect3 *lp_mag)
160 {
161  /* Compute an initial orientation from accel and mag directly as quaternion */
163 
164  /* use average gyro as initial value for bias */
165  ahrs_float_inv.state.bias = *lp_gyro;
166 
167  // ins and ahrs are now running
168  ahrs_float_inv.is_aligned = true;
169 }
170 
172 {
173  // realign all the filter if needed
174  // a complete init cycle is required
175  if (ahrs_float_inv.reset) {
176  ahrs_float_inv.reset = false;
177  ahrs_float_inv.is_aligned = false;
179  }
180 
181  // fill command vector
182  struct FloatRates gyro_meas_body;
183  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
184  float_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro);
185  ahrs_float_inv.cmd.rates = gyro_meas_body;
186 
187  // update correction gains
189 
190  // propagate model
191  struct inv_state new_state;
192  runge_kutta_4_float((float *)&new_state,
195  invariant_model, dt);
196  ahrs_float_inv.state = new_state;
197 
198  // normalize quaternion
200 
201  //------------------------------------------------------------//
202 
203 #if SEND_INVARIANT_FILTER
204  struct FloatEulers eulers;
205  float foo = 0.f;
207  RunOnceEvery(3,
208  pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device,
209  AC_ID,
211  &eulers.phi,
212  &eulers.theta,
213  &eulers.psi,
214  &foo,
215  &foo,
216  &foo,
217  &foo,
218  &foo,
219  &foo,
225  &foo,
226  &foo);
227  );
228 #endif
229 
230 }
231 
233 {
234  ahrs_float_inv.meas.accel = *accel;
235 }
236 
237 // assume mag is dead when values are not moving anymore
238 #define MAG_FROZEN_COUNT 30
239 
241 {
242  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
243  static int32_t last_mx = 0;
244 
245  if (last_mx == mag->x) {
246  mag_frozen_count--;
247  if (mag_frozen_count == 0) {
248  // if mag is dead, better set measurements to zero
250  mag_frozen_count = MAG_FROZEN_COUNT;
251  }
252  } else {
253  // values are moving
254  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
255  // new values in body frame
256  float_rmat_transp_vmult(&ahrs_float_inv.meas.mag, body_to_imu_rmat, mag);
257  // reset counter
258  mag_frozen_count = MAG_FROZEN_COUNT;
259  }
260  last_mx = mag->x;
261 }
262 
267 static inline void invariant_model(float *o, const float *x, const int n, const float *u,
268  const int m __attribute__((unused)))
269 {
270 
271 #pragma GCC diagnostic push // require GCC 4.6
272 #pragma GCC diagnostic ignored "-Wcast-qual"
273  struct inv_state *s = (struct inv_state *)x;
274  struct inv_command *c = (struct inv_command *)u;
275 #pragma GCC diagnostic pop // require GCC 4.6
276  struct inv_state s_dot;
277  struct FloatRates rates_unbiased;
278  struct FloatVect3 tmp_vect;
279  struct FloatQuat tmp_quat;
280 
281  // test accel sensitivity
282  if (fabs(s->as) < 0.1) {
283  // too small, return x_dot = 0 to avoid division by 0
284  float_vect_zero(o, n);
285  // TODO set ins state to error
286  return;
287  }
288 
289  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
290  RATES_DIFF(rates_unbiased, c->rates, s->bias);
291  /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
292  float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
293 
294  float_quat_vmul_right(&tmp_quat, &(s->quat), &ahrs_float_inv.corr.LE);
295  QUAT_ADD(s_dot.quat, tmp_quat);
296 
297  float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat);
298  QUAT_SMUL(tmp_quat, s->quat, norm2_r);
299  QUAT_ADD(s_dot.quat, tmp_quat);
300 
301  /* bias_dot = q-1 * (ME) * q */
302  float_quat_vmult(&tmp_vect, &(s->quat), &ahrs_float_inv.corr.ME);
303  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
304 
305  /* as_dot = as * NE */
306  s_dot.as = (s->as) * (ahrs_float_inv.corr.NE);
307 
308  /* cs_dot = OE */
309  s_dot.cs = ahrs_float_inv.corr.OE;
310 
311  // set output
312  memcpy(o, &s_dot, n * sizeof(float));
313 }
314 
319 static inline void error_output(struct AhrsFloatInv *_ahrs)
320 {
321 
322  struct FloatVect3 YAt, C, YC, YCt, Ec, Ea;
323 
324  // test accel sensitivity
325  if (fabs(_ahrs->state.as) < 0.1) {
326  // too small, don't do anything to avoid division by 0
327  return;
328  }
329 
330  /* C = A X B Cross product */
331  VECT3_CROSS_PRODUCT(C, A, B);
332  /* YC = YA X YB Cross product */
333  VECT3_CROSS_PRODUCT(YC, _ahrs->meas.accel, _ahrs->meas.mag);
334  /* YCt = (1 / cs) * q * YC * q-1 => invariant output on magnetometers */
335  struct FloatQuat q_b2n;
336  float_quat_invert(&q_b2n, &(_ahrs->state.quat));
337  float_quat_vmult(&YCt, &q_b2n, &YC);
338  VECT3_SMUL(YCt, YCt, 1. / (_ahrs->state.cs));
339 
340  /* YAt = q * yA * q-1 => invariant output on accelerometers */
341  float_quat_vmult(&YAt, &q_b2n, &(_ahrs->meas.accel));
342  VECT3_SMUL(YAt, YAt, 1. / (_ahrs->state.as));
343 
344  /*--------- E = ( ŷ - y ) ----------*/
345 
346  /* EC = ( C - YCt ) */
347  VECT3_DIFF(Ec, C, YCt);
348  /* EA = ( A - YAt ) */
349  VECT3_DIFF(Ea, A, YAt);
350 
351  /*-------------- Gains --------------*/
352 
353  /**** LaEa + LcEc *****/
354  _ahrs->corr.LE.x = (-_ahrs->gains.lx * Ea.y) / (2.f * A.z);
355  _ahrs->corr.LE.y = (_ahrs->gains.ly * Ea.x) / (2.f * A.z);
356  _ahrs->corr.LE.z = (-_ahrs->gains.lz * Ec.x) / (B.x * 2.f * A.z);
357 
358  /***** MaEa + McEc ********/
359  _ahrs->corr.ME.x = (_ahrs->gains.mx * Ea.y) / (2.f * A.z);
360  _ahrs->corr.ME.y = (-_ahrs->gains.my * Ea.x)/(2.f * A.z);
361  _ahrs->corr.ME.z = (_ahrs->gains.mz * Ec.x) / (B.x * 2.f * A.z);
362 
363  /****** NaEa + NcEc ********/
364  _ahrs->corr.NE = (-_ahrs->gains.n * Ea.z) / A.z;
365 
366  /****** OaEa + OcEc ********/
367  _ahrs->corr.OE = (-_ahrs->gains.o * Ec.y) / (B.x * A.z);
368 }
369 
370 
371 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
372  struct FloatVect3 *vi)
373 {
374  struct FloatVect3 qvec, v1, v2;
375  float qi;
376 
377  FLOAT_QUAT_EXTRACT(qvec, *q);
378  qi = - VECT3_DOT_PRODUCT(*vi, qvec);
379  VECT3_CROSS_PRODUCT(v1, *vi, qvec);
380  VECT3_SMUL(v2, *vi, q->qi);
381  VECT3_ADD(v2, v1);
382  QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
383 }
384 
386 {
388 
389  if (!ahrs_float_inv.is_aligned) {
390  /* Set ltp_to_imu so that body is zero */
391  ahrs_float_inv.state.quat = *q_b2i;
392  }
393 }
394 
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:243
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:249
Interface to align the AHRS via low-passed measurements at startup.
#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:146
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.
#define FLOAT_QUAT_NORMALIZE(_q)
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:547
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:181
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:329
#define AHRS_INV_MX
#define FLOAT_RATES_ZERO(_r)
float psi
in radians
#define AHRS_INV_LX
float dt
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:371
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:516
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_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:188
#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:555
struct AhrsFloatInv ahrs_float_inv
Runge-Kutta library (float version)
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
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.