Paparazzi UAS v7.0_unstable
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
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
49static inline void propagate_ref(struct FloatRates *gyro, float dt);
50static inline void propagate_state(float dt);
51static inline void update_state(const struct FloatVect3 *i_expected,
52 struct FloatVect3 *b_measured,
53 struct FloatVect3 *noise);
54static inline void update_state_heading(const struct FloatVect3 *i_expected,
55 struct FloatVect3 *b_measured,
56 struct FloatVect3 *noise);
57static inline void reset_state(void);
58
59struct AhrsMlkf ahrs_mlkf;
60
61
62void 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
91bool 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
106void ahrs_mlkf_propagate(struct FloatRates *gyro, float dt)
107{
108 propagate_ref(gyro, dt);
109 propagate_state(dt);
110}
111
112void 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
125void 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
139
145
146
147static 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
172static 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
206static 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
274static 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}
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
foo
uint16_t foo
Definition main_demo5.c:58
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