Paparazzi UAS  v5.15_devel-110-g5cf27ee
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
33 
34 #include <string.h> /* for memcpy */
35 
37 #include "math/pprz_algebra_int.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 
60 
61 
62 void ahrs_mlkf_init(void)
63 {
64 
65  ahrs_mlkf.is_aligned = false;
66 
67  /* init ltp_to_imu quaternion as zero/identity rotation */
70 
71  VECT3_ASSIGN(ahrs_mlkf.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
72 
73  /*
74  * Initialises our state
75  */
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 
89 }
90 
92 {
94 }
95 
97 {
99 
100  if (!ahrs_mlkf.is_aligned) {
101  /* Set ltp_to_imu so that body is zero */
103  }
104 }
105 
106 
107 bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
108  struct FloatVect3 *lp_mag)
109 {
110 
111  /* Compute an initial orientation from accel and mag directly as quaternion */
113 
114  /* used averaged gyro as initial value for bias */
115  ahrs_mlkf.gyro_bias = *lp_gyro;
116 
117  ahrs_mlkf.is_aligned = true;
118 
119  return true;
120 }
121 
122 void ahrs_mlkf_propagate(struct FloatRates *gyro, float dt)
123 {
124  propagate_ref(gyro, dt);
125  propagate_state(dt);
126 }
127 
129 {
130  struct FloatVect3 imu_g = *accel;
131  const float alpha = 0.92;
133  (1. - alpha) * (float_vect3_norm(&imu_g) - 9.81);
134  const struct FloatVect3 earth_g = {0., 0., -9.81 };
135  const float dn = 250 * fabs(ahrs_mlkf.lp_accel);
136  struct FloatVect3 g_noise = {1. + dn, 1. + dn, 1. + dn};
137  update_state(&earth_g, &imu_g, &g_noise);
138  reset_state();
139 }
140 
142 {
143 #if AHRS_MAG_UPDATE_ALL_AXES
145 #else
147 #endif
148 }
149 
151 {
153  reset_state();
154 }
155 
157 {
159  reset_state();
160 }
161 
162 
163 static inline void propagate_ref(struct FloatRates *gyro, float dt)
164 {
165  struct FloatRates rates = *gyro;
166 
167  /* unbias measurement */
168  RATES_SUB(rates, ahrs_mlkf.gyro_bias);
169 
170 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
171  /* lowpass angular rates */
172  const float alpha = 0.1;
174  (1. - alpha), rates, alpha);
175 #else
176  RATES_COPY(ahrs_mlkf.imu_rate, rates);
177 #endif
178 
179  /* propagate reference quaternion */
181 
182 }
183 
188 static inline void propagate_state(float dt)
189 {
190 
191  /* predict covariance */
192  const float dp = ahrs_mlkf.imu_rate.p * dt;
193  const float dq = ahrs_mlkf.imu_rate.q * dt;
194  const float dr = ahrs_mlkf.imu_rate.r * dt;
195 
196  float F[6][6] = {{ 1., dr, -dq, -dt, 0., 0. },
197  { -dr, 1., dp, 0., -dt, 0. },
198  { dq, -dp, 1., 0., 0., -dt },
199  { 0., 0., 0., 1., 0., 0. },
200  { 0., 0., 0., 0., 1., 0. },
201  { 0., 0., 0., 0., 0., 1. }
202  };
203  // P = FPF' + GQG
204  float tmp[6][6];
205  MAT_MUL(6, 6, 6, tmp, F, ahrs_mlkf.P);
206  MAT_MUL_T(6, 6, 6, ahrs_mlkf.P, tmp, F);
207  const float dt2 = dt * dt;
208  const float GQG[6] = {dt2 * 10e-3, dt2 * 10e-3, dt2 * 10e-3, dt2 * 9e-6, dt2 * 9e-6, dt2 * 9e-6 };
209  for (int i = 0; i < 6; i++) {
210  ahrs_mlkf.P[i][i] += GQG[i];
211  }
212 
213 }
214 
215 
222 static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured,
223  struct FloatVect3 *noise)
224 {
225 
226  /* converted expected measurement from inertial to body frame */
227  struct FloatVect3 b_expected;
228  float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, i_expected);
229 
230  // S = HPH' + JRJ
231  float H[3][6] = {{ 0., -b_expected.z, b_expected.y, 0., 0., 0.},
232  { b_expected.z, 0., -b_expected.x, 0., 0., 0.},
233  { -b_expected.y, b_expected.x, 0., 0., 0., 0.}
234  };
235  float tmp[3][6];
236  MAT_MUL(3, 6, 6, tmp, H, ahrs_mlkf.P);
237  float S[3][3];
238  MAT_MUL_T(3, 6, 3, S, tmp, H);
239 
240  /* add the measurement noise */
241  S[0][0] += noise->x;
242  S[1][1] += noise->y;
243  S[2][2] += noise->z;
244 
245  float invS[3][3];
246  MAT_INV33(invS, S);
247 
248  // K = PH'invS
249  float tmp2[6][3];
250  MAT_MUL_T(6, 6, 3, tmp2, ahrs_mlkf.P, H);
251  float K[6][3];
252  MAT_MUL(6, 3, 3, K, tmp2, invS);
253 
254  // P = (I-KH)P
255  float tmp3[6][6];
256  MAT_MUL(6, 3, 6, tmp3, K, H);
257  float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
258  { 0., 1., 0., 0., 0., 0. },
259  { 0., 0., 1., 0., 0., 0. },
260  { 0., 0., 0., 1., 0., 0. },
261  { 0., 0., 0., 0., 1., 0. },
262  { 0., 0., 0., 0., 0., 1. }
263  };
264  float tmp4[6][6];
265  MAT_SUB(6, 6, tmp4, I6, tmp3);
266  float tmp5[6][6];
267  MAT_MUL(6, 6, 6, tmp5, tmp4, ahrs_mlkf.P);
268  memcpy(ahrs_mlkf.P, tmp5, sizeof(ahrs_mlkf.P));
269 
270  // X = X + Ke
271  struct FloatVect3 e;
272  VECT3_DIFF(e, *b_measured, b_expected);
273  ahrs_mlkf.gibbs_cor.qx += K[0][0] * e.x + K[0][1] * e.y + K[0][2] * e.z;
274  ahrs_mlkf.gibbs_cor.qy += K[1][0] * e.x + K[1][1] * e.y + K[1][2] * e.z;
275  ahrs_mlkf.gibbs_cor.qz += K[2][0] * e.x + K[2][1] * e.y + K[2][2] * e.z;
276  ahrs_mlkf.gyro_bias.p += K[3][0] * e.x + K[3][1] * e.y + K[3][2] * e.z;
277  ahrs_mlkf.gyro_bias.q += K[4][0] * e.x + K[4][1] * e.y + K[4][2] * e.z;
278  ahrs_mlkf.gyro_bias.r += K[5][0] * e.x + K[5][1] * e.y + K[5][2] * e.z;
279 
280 }
281 
282 
290 static inline void update_state_heading(const struct FloatVect3 *i_expected,
291  struct FloatVect3 *b_measured,
292  struct FloatVect3 *noise)
293 {
294 
295  /* converted expected measurement from inertial to body frame */
296  struct FloatVect3 b_expected;
297  float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, i_expected);
298 
299  /* set roll/pitch errors to zero to only correct heading */
300  struct FloatVect3 i_h_2d = {i_expected->y, -i_expected->x, 0.f};
301  struct FloatVect3 b_yaw;
302  float_quat_vmult(&b_yaw, &ahrs_mlkf.ltp_to_imu_quat, &i_h_2d);
303  // S = HPH' + JRJ
304  float H[3][6] = {{ 0., 0., b_yaw.x, 0., 0., 0.},
305  { 0., 0., b_yaw.y, 0., 0., 0.},
306  { 0., 0., b_yaw.z, 0., 0., 0.}
307  };
308  float tmp[3][6];
309  MAT_MUL(3, 6, 6, tmp, H, ahrs_mlkf.P);
310  float S[3][3];
311  MAT_MUL_T(3, 6, 3, S, tmp, H);
312 
313  /* add the measurement noise */
314  S[0][0] += noise->x;
315  S[1][1] += noise->y;
316  S[2][2] += noise->z;
317 
318  float invS[3][3];
319  MAT_INV33(invS, S);
320 
321  // K = PH'invS
322  float tmp2[6][3];
323  MAT_MUL_T(6, 6, 3, tmp2, ahrs_mlkf.P, H);
324  float K[6][3];
325  MAT_MUL(6, 3, 3, K, tmp2, invS);
326 
327  // P = (I-KH)P
328  float tmp3[6][6];
329  MAT_MUL(6, 3, 6, tmp3, K, H);
330  float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
331  { 0., 1., 0., 0., 0., 0. },
332  { 0., 0., 1., 0., 0., 0. },
333  { 0., 0., 0., 1., 0., 0. },
334  { 0., 0., 0., 0., 1., 0. },
335  { 0., 0., 0., 0., 0., 1. }
336  };
337  float tmp4[6][6];
338  MAT_SUB(6, 6, tmp4, I6, tmp3);
339  float tmp5[6][6];
340  MAT_MUL(6, 6, 6, tmp5, tmp4, ahrs_mlkf.P);
341  memcpy(ahrs_mlkf.P, tmp5, sizeof(ahrs_mlkf.P));
342 
343  // X = X + Ke
344  struct FloatVect3 e;
345  VECT3_DIFF(e, *b_measured, b_expected);
346  ahrs_mlkf.gibbs_cor.qx += K[0][0] * e.x + K[0][1] * e.y + K[0][2] * e.z;
347  ahrs_mlkf.gibbs_cor.qy += K[1][0] * e.x + K[1][1] * e.y + K[1][2] * e.z;
348  ahrs_mlkf.gibbs_cor.qz += K[2][0] * e.x + K[2][1] * e.y + K[2][2] * e.z;
349  ahrs_mlkf.gyro_bias.p += K[3][0] * e.x + K[3][1] * e.y + K[3][2] * e.z;
350  ahrs_mlkf.gyro_bias.q += K[4][0] * e.x + K[4][1] * e.y + K[4][2] * e.z;
351  ahrs_mlkf.gyro_bias.r += K[5][0] * e.x + K[5][1] * e.y + K[5][2] * e.z;
352 
353 }
357 static inline void reset_state(void)
358 {
359 
360  ahrs_mlkf.gibbs_cor.qi = 2.;
361  struct FloatQuat q_tmp;
363  float_quat_normalize(&q_tmp);
364  ahrs_mlkf.ltp_to_imu_quat = q_tmp;
366 
367 }
void ahrs_mlkf_propagate(struct FloatRates *gyro, float dt)
struct FloatQuat gibbs_cor
static void propagate_ref(struct FloatRates *gyro, float dt)
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
void ahrs_mlkf_update_mag_full(struct FloatVect3 *mag)
Simple matrix helper macros.
static float K[9]
#define AHRS_MAG_NOISE_Z
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
#define AHRS_MAG_NOISE_Y
struct FloatVect3 mag_noise
#define AHRS_MAG_NOISE_X
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
struct FloatVect3 mag_h
float r
in rad/s
struct FloatRates imu_rate
Rotational velocity in IMU frame.
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
float alpha
Definition: textons.c:107
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
float P[6][6]
Utility functions for floating point AHRS implementations.
#define FLOAT_RATES_ZERO(_r)
void ahrs_mlkf_update_accel(struct FloatVect3 *accel)
void ahrs_mlkf_set_body_to_imu(struct OrientationReps *body_to_imu)
float lp_accel
struct FloatRates gyro_bias
#define RATES_SUB(_a, _b)
Definition: pprz_algebra.h:351
float q
in rad/s
float p
in rad/s
#define MAT_MUL_T(_i, _k, _j, C, A, B)
Multiplicative linearized Kalman Filter in quaternion formulation.
Roation quaternion.
bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
static float float_vect3_norm(struct FloatVect3 *v)
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.
static void update_state(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, struct FloatVect3 *noise)
Incorporate one 3D vector measurement.
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
void ahrs_mlkf_update_mag(struct FloatVect3 *mag)
Paparazzi floating point algebra.
void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i)
uint16_t dn[LIGHT_NB]
Definition: light_solar.c:48
void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place quaternion integration with constant rotational velocity
bool is_aligned
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)
void ahrs_mlkf_update_mag_2d(struct FloatVect3 *mag)
void ahrs_mlkf_init(void)
struct AhrsMlkf ahrs_mlkf
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
static void reset_state(void)
Incorporate errors to reference and zeros state.
#define MAT_INV33(_invS, _S)
static struct FloatVect3 H
struct OrientationReps body_to_imu
body_to_imu rotation
#define MAT_SUB(_i, _j, C, A, B)
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
static void propagate_state(float dt)
Progagate filter's covariance We don't propagate state as we assume to have reseted.
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
angular rates
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Paparazzi fixed point algebra.
#define MAT_MUL(_i, _k, _j, C, A, B)