Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_float_mlkf.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011-2012 Antoine Drouin <poinix@gmail.com>
3  * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
31 #include "modules/ahrs/ahrs_float_mlkf.h"
32 #include "modules/ahrs/ahrs_float_utils.h"
33 
34 #include <string.h> /* for memcpy */
35 
36 #include "math/pprz_algebra_float.h"
37 #include "math/pprz_algebra_int.h"
38 #include "math/pprz_simple_matrix.h"
39 #include "generated/airframe.h"
40 
41 //#include <stdio.h>
42 
43 #ifndef AHRS_MAG_NOISE_X
44 #define AHRS_MAG_NOISE_X 0.2
45 #define AHRS_MAG_NOISE_Y 0.2
46 #define AHRS_MAG_NOISE_Z 0.2
47 #endif
48 
49 static inline void propagate_ref(struct FloatRates *gyro, float dt);
50 static inline void propagate_state(float dt);
51 static inline void update_state(const struct FloatVect3 *i_expected,
52  struct FloatVect3 *b_measured,
53  struct FloatVect3 *noise);
54 static inline void update_state_heading(const struct FloatVect3 *i_expected,
55  struct FloatVect3 *b_measured,
56  struct FloatVect3 *noise);
57 static inline void reset_state(void);
58 
59 struct AhrsMlkf ahrs_mlkf;
60 
61 
62 void ahrs_mlkf_init(void)
63 {
64 
65  ahrs_mlkf.is_aligned = false;
66 
67  /* init ltp_to_body quaternion as zero/identity rotation */
68  float_quat_identity(&ahrs_mlkf.ltp_to_body_quat);
69  FLOAT_RATES_ZERO(ahrs_mlkf.body_rate);
70 
71  VECT3_ASSIGN(ahrs_mlkf.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
72 
73  /*
74  * Initialises our state
75  */
76  FLOAT_RATES_ZERO(ahrs_mlkf.gyro_bias);
77  const float P0_a = 1.;
78  const float P0_b = 1e-4;
79  float P0[6][6] = {{ P0_a, 0., 0., 0., 0., 0. },
80  { 0., P0_a, 0., 0., 0., 0. },
81  { 0., 0., P0_a, 0., 0., 0. },
82  { 0., 0., 0., P0_b, 0., 0. },
83  { 0., 0., 0., 0., P0_b, 0. },
84  { 0., 0., 0., 0., 0., P0_b}
85  };
86  memcpy(ahrs_mlkf.P, P0, sizeof(P0));
87 
88  VECT3_ASSIGN(ahrs_mlkf.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
89 }
90 
91 bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
92  struct FloatVect3 *lp_mag)
93 {
94 
95  /* Compute an initial orientation from accel and mag directly as quaternion */
96  ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_body_quat, lp_accel, lp_mag);
97 
98  /* used averaged gyro as initial value for bias */
99  ahrs_mlkf.gyro_bias = *lp_gyro;
100 
101  ahrs_mlkf.is_aligned = true;
102 
103  return true;
104 }
105 
106 void ahrs_mlkf_propagate(struct FloatRates *gyro, float dt)
107 {
108  propagate_ref(gyro, dt);
109  propagate_state(dt);
110 }
111 
112 void ahrs_mlkf_update_accel(struct FloatVect3 *accel)
113 {
114  struct FloatVect3 imu_g = *accel;
115  const float alpha = 0.92;
116  ahrs_mlkf.lp_accel = alpha * ahrs_mlkf.lp_accel +
117  (1. - alpha) * (float_vect3_norm(&imu_g) - 9.81);
118  const struct FloatVect3 earth_g = {0., 0., -9.81 };
119  const float dn = 250 * fabs(ahrs_mlkf.lp_accel);
120  struct FloatVect3 g_noise = {1. + dn, 1. + dn, 1. + dn};
121  update_state(&earth_g, &imu_g, &g_noise);
122  reset_state();
123 }
124 
125 void ahrs_mlkf_update_mag(struct FloatVect3 *mag)
126 {
127 #if AHRS_MAG_UPDATE_ALL_AXES
128  ahrs_mlkf_update_mag_full(mag);
129 #else
130  ahrs_mlkf_update_mag_2d(mag);
131 #endif
132 }
133 
134 void ahrs_mlkf_update_mag_2d(struct FloatVect3 *mag)
135 {
136  update_state_heading(&ahrs_mlkf.mag_h, mag, &ahrs_mlkf.mag_noise);
137  reset_state();
138 }
139 
140 void ahrs_mlkf_update_mag_full(struct FloatVect3 *mag)
141 {
142  update_state(&ahrs_mlkf.mag_h, mag, &ahrs_mlkf.mag_noise);
143  reset_state();
144 }
145 
146 
147 static inline void propagate_ref(struct FloatRates *gyro, float dt)
148 {
149  struct FloatRates rates = *gyro;
150 
151  /* unbias measurement */
152  RATES_SUB(rates, ahrs_mlkf.gyro_bias);
153 
154 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
155  /* lowpass angular rates */
156  const float alpha = 0.1;
157  FLOAT_RATES_LIN_CMB(ahrs_mlkf.body_rate, ahrs_mlkf.body_rate,
158  (1. - alpha), rates, alpha);
159 #else
160  RATES_COPY(ahrs_mlkf.body_rate, rates);
161 #endif
162 
163  /* propagate reference quaternion */
164  float_quat_integrate(&ahrs_mlkf.ltp_to_body_quat, &ahrs_mlkf.body_rate, dt);
165 
166 }
167 
172 static inline void propagate_state(float dt)
173 {
174 
175  /* predict covariance */
176  const float dp = ahrs_mlkf.body_rate.p * dt;
177  const float dq = ahrs_mlkf.body_rate.q * dt;
178  const float dr = ahrs_mlkf.body_rate.r * dt;
179 
180  float F[6][6] = {{ 1., dr, -dq, -dt, 0., 0. },
181  { -dr, 1., dp, 0., -dt, 0. },
182  { dq, -dp, 1., 0., 0., -dt },
183  { 0., 0., 0., 1., 0., 0. },
184  { 0., 0., 0., 0., 1., 0. },
185  { 0., 0., 0., 0., 0., 1. }
186  };
187  // P = FPF' + GQG
188  float tmp[6][6];
189  MAT_MUL(6, 6, 6, tmp, F, ahrs_mlkf.P);
190  MAT_MUL_T(6, 6, 6, ahrs_mlkf.P, tmp, F);
191  const float dt2 = dt * dt;
192  const float GQG[6] = {dt2 * 10e-3, dt2 * 10e-3, dt2 * 10e-3, dt2 * 9e-6, dt2 * 9e-6, dt2 * 9e-6 };
193  for (int i = 0; i < 6; i++) {
194  ahrs_mlkf.P[i][i] += GQG[i];
195  }
196 
197 }
198 
199 
206 static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured,
207  struct FloatVect3 *noise)
208 {
209 
210  /* converted expected measurement from inertial to body frame */
211  struct FloatVect3 b_expected;
212  float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_body_quat, i_expected);
213 
214  // S = HPH' + JRJ
215  float H[3][6] = {{ 0., -b_expected.z, b_expected.y, 0., 0., 0.},
216  { b_expected.z, 0., -b_expected.x, 0., 0., 0.},
217  { -b_expected.y, b_expected.x, 0., 0., 0., 0.}
218  };
219  float tmp[3][6];
220  MAT_MUL(3, 6, 6, tmp, H, ahrs_mlkf.P);
221  float S[3][3];
222  MAT_MUL_T(3, 6, 3, S, tmp, H);
223 
224  /* add the measurement noise */
225  S[0][0] += noise->x;
226  S[1][1] += noise->y;
227  S[2][2] += noise->z;
228 
229  float invS[3][3];
230  MAT_INV33(invS, S);
231 
232  // K = PH'invS
233  float tmp2[6][3];
234  MAT_MUL_T(6, 6, 3, tmp2, ahrs_mlkf.P, H);
235  float K[6][3];
236  MAT_MUL(6, 3, 3, K, tmp2, invS);
237 
238  // P = (I-KH)P
239  float tmp3[6][6];
240  MAT_MUL(6, 3, 6, tmp3, K, H);
241  float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
242  { 0., 1., 0., 0., 0., 0. },
243  { 0., 0., 1., 0., 0., 0. },
244  { 0., 0., 0., 1., 0., 0. },
245  { 0., 0., 0., 0., 1., 0. },
246  { 0., 0., 0., 0., 0., 1. }
247  };
248  float tmp4[6][6];
249  MAT_SUB(6, 6, tmp4, I6, tmp3);
250  float tmp5[6][6];
251  MAT_MUL(6, 6, 6, tmp5, tmp4, ahrs_mlkf.P);
252  memcpy(ahrs_mlkf.P, tmp5, sizeof(ahrs_mlkf.P));
253 
254  // X = X + Ke
255  struct FloatVect3 e;
256  VECT3_DIFF(e, *b_measured, b_expected);
257  ahrs_mlkf.gibbs_cor.qx += K[0][0] * e.x + K[0][1] * e.y + K[0][2] * e.z;
258  ahrs_mlkf.gibbs_cor.qy += K[1][0] * e.x + K[1][1] * e.y + K[1][2] * e.z;
259  ahrs_mlkf.gibbs_cor.qz += K[2][0] * e.x + K[2][1] * e.y + K[2][2] * e.z;
260  ahrs_mlkf.gyro_bias.p += K[3][0] * e.x + K[3][1] * e.y + K[3][2] * e.z;
261  ahrs_mlkf.gyro_bias.q += K[4][0] * e.x + K[4][1] * e.y + K[4][2] * e.z;
262  ahrs_mlkf.gyro_bias.r += K[5][0] * e.x + K[5][1] * e.y + K[5][2] * e.z;
263 
264 }
265 
266 
274 static inline void update_state_heading(const struct FloatVect3 *i_expected,
275  struct FloatVect3 *b_measured,
276  struct FloatVect3 *noise)
277 {
278 
279  /* converted expected measurement from inertial to body frame */
280  struct FloatVect3 b_expected;
281  float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_body_quat, i_expected);
282 
283  /* set roll/pitch errors to zero to only correct heading */
284  struct FloatVect3 i_h_2d = {i_expected->y, -i_expected->x, 0.f};
285  struct FloatVect3 b_yaw;
286  float_quat_vmult(&b_yaw, &ahrs_mlkf.ltp_to_body_quat, &i_h_2d);
287  // S = HPH' + JRJ
288  float H[3][6] = {{ 0., 0., b_yaw.x, 0., 0., 0.},
289  { 0., 0., b_yaw.y, 0., 0., 0.},
290  { 0., 0., b_yaw.z, 0., 0., 0.}
291  };
292  float tmp[3][6];
293  MAT_MUL(3, 6, 6, tmp, H, ahrs_mlkf.P);
294  float S[3][3];
295  MAT_MUL_T(3, 6, 3, S, tmp, H);
296 
297  /* add the measurement noise */
298  S[0][0] += noise->x;
299  S[1][1] += noise->y;
300  S[2][2] += noise->z;
301 
302  float invS[3][3];
303  MAT_INV33(invS, S);
304 
305  // K = PH'invS
306  float tmp2[6][3];
307  MAT_MUL_T(6, 6, 3, tmp2, ahrs_mlkf.P, H);
308  float K[6][3];
309  MAT_MUL(6, 3, 3, K, tmp2, invS);
310 
311  // P = (I-KH)P
312  float tmp3[6][6];
313  MAT_MUL(6, 3, 6, tmp3, K, H);
314  float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
315  { 0., 1., 0., 0., 0., 0. },
316  { 0., 0., 1., 0., 0., 0. },
317  { 0., 0., 0., 1., 0., 0. },
318  { 0., 0., 0., 0., 1., 0. },
319  { 0., 0., 0., 0., 0., 1. }
320  };
321  float tmp4[6][6];
322  MAT_SUB(6, 6, tmp4, I6, tmp3);
323  float tmp5[6][6];
324  MAT_MUL(6, 6, 6, tmp5, tmp4, ahrs_mlkf.P);
325  memcpy(ahrs_mlkf.P, tmp5, sizeof(ahrs_mlkf.P));
326 
327  // X = X + Ke
328  struct FloatVect3 e;
329  VECT3_DIFF(e, *b_measured, b_expected);
330  ahrs_mlkf.gibbs_cor.qx += K[0][0] * e.x + K[0][1] * e.y + K[0][2] * e.z;
331  ahrs_mlkf.gibbs_cor.qy += K[1][0] * e.x + K[1][1] * e.y + K[1][2] * e.z;
332  ahrs_mlkf.gibbs_cor.qz += K[2][0] * e.x + K[2][1] * e.y + K[2][2] * e.z;
333  ahrs_mlkf.gyro_bias.p += K[3][0] * e.x + K[3][1] * e.y + K[3][2] * e.z;
334  ahrs_mlkf.gyro_bias.q += K[4][0] * e.x + K[4][1] * e.y + K[4][2] * e.z;
335  ahrs_mlkf.gyro_bias.r += K[5][0] * e.x + K[5][1] * e.y + K[5][2] * e.z;
336 
337 }
341 static inline void reset_state(void)
342 {
343 
344  ahrs_mlkf.gibbs_cor.qi = 2.;
345  struct FloatQuat q_tmp;
346  float_quat_comp(&q_tmp, &ahrs_mlkf.ltp_to_body_quat, &ahrs_mlkf.gibbs_cor);
347  float_quat_normalize(&q_tmp);
348  ahrs_mlkf.ltp_to_body_quat = q_tmp;
349  float_quat_identity(&ahrs_mlkf.gibbs_cor);
350 
351 }
ahrs_mlkf_update_mag
void ahrs_mlkf_update_mag(struct FloatVect3 *mag)
Definition: ahrs_float_mlkf.c:125
propagate_state
static void propagate_state(float dt)
Progagate filter's covariance We don't propagate state as we assume to have reseted.
Definition: ahrs_float_mlkf.c:172
ahrs_mlkf_update_accel
void ahrs_mlkf_update_accel(struct FloatVect3 *accel)
Definition: ahrs_float_mlkf.c:112
ahrs_mlkf_init
void ahrs_mlkf_init(void)
Definition: ahrs_float_mlkf.c:62
ahrs_mlkf_align
bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
Definition: ahrs_float_mlkf.c:91
AHRS_MAG_NOISE_Z
#define AHRS_MAG_NOISE_Z
Definition: ahrs_float_mlkf.c:46
update_state_heading
static void update_state_heading(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, struct FloatVect3 *noise)
Incorporate one 3D vector measurement, only correcting heading.
Definition: ahrs_float_mlkf.c:274
ahrs_mlkf_update_mag_full
void ahrs_mlkf_update_mag_full(struct FloatVect3 *mag)
Definition: ahrs_float_mlkf.c:140
propagate_ref
static void propagate_ref(struct FloatRates *gyro, float dt)
Definition: ahrs_float_mlkf.c:147
AHRS_MAG_NOISE_X
#define AHRS_MAG_NOISE_X
Definition: ahrs_float_mlkf.c:44
update_state
static void update_state(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, struct FloatVect3 *noise)
Incorporate one 3D vector measurement.
Definition: ahrs_float_mlkf.c:206
reset_state
static void reset_state(void)
Incorporate errors to reference and zeros state.
Definition: ahrs_float_mlkf.c:341
ahrs_mlkf
struct AhrsMlkf ahrs_mlkf
Definition: ahrs_float_mlkf.c:59
ahrs_mlkf_propagate
void ahrs_mlkf_propagate(struct FloatRates *gyro, float dt)
Definition: ahrs_float_mlkf.c:106
ahrs_mlkf_update_mag_2d
void ahrs_mlkf_update_mag_2d(struct FloatVect3 *mag)
Definition: ahrs_float_mlkf.c:134
AHRS_MAG_NOISE_Y
#define AHRS_MAG_NOISE_Y
Definition: ahrs_float_mlkf.c:45
ahrs_float_mlkf.h
Multiplicative linearized Kalman Filter in quaternion formulation.
AhrsMlkf::P
float P[6][6]
Definition: ahrs_float_mlkf.h:53
AhrsMlkf::is_aligned
bool is_aligned
Definition: ahrs_float_mlkf.h:57
AhrsMlkf::mag_h
struct FloatVect3 mag_h
Definition: ahrs_float_mlkf.h:48
AhrsMlkf::gibbs_cor
struct FloatQuat gibbs_cor
Definition: ahrs_float_mlkf.h:52
AhrsMlkf::gyro_bias
struct FloatRates gyro_bias
Definition: ahrs_float_mlkf.h:46
AhrsMlkf::mag_noise
struct FloatVect3 mag_noise
Definition: ahrs_float_mlkf.h:50
AhrsMlkf::ltp_to_body_quat
struct FloatQuat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs_float_mlkf.h:44
AhrsMlkf::lp_accel
float lp_accel
Definition: ahrs_float_mlkf.h:54
AhrsMlkf::body_rate
struct FloatRates body_rate
Rotational velocity in body frame.
Definition: ahrs_float_mlkf.h:45
AhrsMlkf
Definition: ahrs_float_mlkf.h:43
ahrs_float_utils.h
Utility functions for floating point AHRS implementations.
ahrs_float_get_quat_from_accel_mag
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
Definition: ahrs_float_utils.h:85
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
float_quat_normalize
static void float_quat_normalize(struct FloatQuat *q)
Definition: pprz_algebra_float.h:380
float_quat_identity
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
Definition: pprz_algebra_float.h:365
float_quat_integrate
void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place quaternion integration with constant rotational velocity
Definition: pprz_algebra_float.c:400
FLOAT_RATES_LIN_CMB
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
Definition: pprz_algebra_float.h:197
float_quat_vmult
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
Definition: pprz_algebra_float.c:421
float_vect3_norm
static float float_vect3_norm(struct FloatVect3 *v)
Definition: pprz_algebra_float.h:171
FLOAT_RATES_ZERO
#define FLOAT_RATES_ZERO(_r)
Definition: pprz_algebra_float.h:191
float_quat_comp
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_float.c:320
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
FloatVect3
Definition: pprz_algebra_float.h:54
RATES_COPY
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
RATES_SUB
#define RATES_SUB(_a, _b)
Definition: pprz_algebra.h:351
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
VECT3_DIFF
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
dn
uint16_t dn[LIGHT_NB]
Definition: light_solar.c:48
H
static struct FloatVect3 H
Definition: mag_calib_ukf.c:127
pprz_algebra_float.h
Paparazzi floating point algebra.
pprz_algebra_int.h
Paparazzi fixed point algebra.
pprz_simple_matrix.h
Simple matrix helper macros.
MAT_MUL_T
#define MAT_MUL_T(_i, _k, _j, C, A, B)
Definition: pprz_simple_matrix.h:59
MAT_MUL
#define MAT_MUL(_i, _k, _j, C, A, B)
Definition: pprz_simple_matrix.h:46
MAT_SUB
#define MAT_SUB(_i, _j, C, A, B)
Definition: pprz_simple_matrix.h:73
MAT_INV33
#define MAT_INV33(_invS, _S)
Definition: pprz_simple_matrix.h:99
alpha
float alpha
Definition: textons.c:133
K
static float K[9]
Definition: undistort_image.c:41