Paparazzi UAS  v5.18.0_stable
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 "subsystems/ahrs/ahrs_float_mlkf.h"
32 #include "subsystems/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_imu quaternion as zero/identity rotation */
68  float_quat_identity(&ahrs_mlkf.ltp_to_imu_quat);
69  FLOAT_RATES_ZERO(ahrs_mlkf.imu_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 void ahrs_mlkf_set_body_to_imu(struct OrientationReps *body_to_imu)
92 {
93  ahrs_mlkf_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
94 }
95 
96 void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i)
97 {
98  orientationSetQuat_f(&ahrs_mlkf.body_to_imu, q_b2i);
99 
100  if (!ahrs_mlkf.is_aligned) {
101  /* Set ltp_to_imu so that body is zero */
102  ahrs_mlkf.ltp_to_imu_quat = *orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
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 */
112  ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag);
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 
128 void ahrs_mlkf_update_accel(struct FloatVect3 *accel)
129 {
130  struct FloatVect3 imu_g = *accel;
131  const float alpha = 0.92;
132  ahrs_mlkf.lp_accel = alpha * ahrs_mlkf.lp_accel +
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 
141 void ahrs_mlkf_update_mag(struct FloatVect3 *mag)
142 {
143 #if AHRS_MAG_UPDATE_ALL_AXES
144  ahrs_mlkf_update_mag_full(mag);
145 #else
146  ahrs_mlkf_update_mag_2d(mag);
147 #endif
148 }
149 
150 void ahrs_mlkf_update_mag_2d(struct FloatVect3 *mag)
151 {
152  update_state_heading(&ahrs_mlkf.mag_h, mag, &ahrs_mlkf.mag_noise);
153  reset_state();
154 }
155 
156 void ahrs_mlkf_update_mag_full(struct FloatVect3 *mag)
157 {
158  update_state(&ahrs_mlkf.mag_h, mag, &ahrs_mlkf.mag_noise);
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;
173  FLOAT_RATES_LIN_CMB(ahrs_mlkf.imu_rate, ahrs_mlkf.imu_rate,
174  (1. - alpha), rates, alpha);
175 #else
176  RATES_COPY(ahrs_mlkf.imu_rate, rates);
177 #endif
178 
179  /* propagate reference quaternion */
180  float_quat_integrate(&ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.imu_rate, dt);
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;
362  float_quat_comp(&q_tmp, &ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.gibbs_cor);
363  float_quat_normalize(&q_tmp);
364  ahrs_mlkf.ltp_to_imu_quat = q_tmp;
365  float_quat_identity(&ahrs_mlkf.gibbs_cor);
366 
367 }
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
OrientationReps
Definition: pprz_orientation_conversion.h:79
AhrsMlkf::is_aligned
bool is_aligned
Definition: ahrs_float_mlkf.h:61
AhrsMlkf::mag_h
struct FloatVect3 mag_h
Definition: ahrs_float_mlkf.h:49
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
float_quat_identity
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
Definition: pprz_algebra_float.h:365
H
static struct FloatVect3 H
Definition: mag_calib_ukf.c:127
propagate_ref
static void propagate_ref(struct FloatRates *gyro, float dt)
Definition: ahrs_float_mlkf.c:163
FLOAT_RATES_ZERO
#define FLOAT_RATES_ZERO(_r)
Definition: pprz_algebra_float.h:191
AHRS_MAG_NOISE_X
#define AHRS_MAG_NOISE_X
Definition: ahrs_float_mlkf.c:44
MAT_MUL_T
#define MAT_MUL_T(_i, _k, _j, C, A, B)
Definition: pprz_simple_matrix.h:59
ahrs_mlkf_align
bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
Definition: ahrs_float_mlkf.c:107
VECT3_DIFF
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
ahrs_float_utils.h
AHRS_MAG_NOISE_Y
#define AHRS_MAG_NOISE_Y
Definition: ahrs_float_mlkf.c:45
AhrsMlkf::mag_noise
struct FloatVect3 mag_noise
Definition: ahrs_float_mlkf.h:51
alpha
float alpha
Definition: textons.c:107
AhrsMlkf::gyro_bias
struct FloatRates gyro_bias
Definition: ahrs_float_mlkf.h:47
ahrs_float_mlkf.h
pprz_algebra_float.h
Paparazzi floating point algebra.
AhrsMlkf::body_to_imu
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ahrs_float_mlkf.h:58
ahrs_mlkf_update_mag_2d
void ahrs_mlkf_update_mag_2d(struct FloatVect3 *mag)
Definition: ahrs_float_mlkf.c:150
pprz_algebra_int.h
Paparazzi fixed point algebra.
AhrsMlkf::imu_rate
struct FloatRates imu_rate
Rotational velocity in IMU frame.
Definition: ahrs_float_mlkf.h:46
FloatVect3
Definition: pprz_algebra_float.h:54
FLOAT_RATES_LIN_CMB
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
Definition: pprz_algebra_float.h:197
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
RATES_SUB
#define RATES_SUB(_a, _b)
Definition: pprz_algebra.h:351
float_vect3_norm
static float float_vect3_norm(struct FloatVect3 *v)
Definition: pprz_algebra_float.h:171
ahrs_mlkf_update_mag
void ahrs_mlkf_update_mag(struct FloatVect3 *mag)
Definition: ahrs_float_mlkf.c:141
orientationGetQuat_f
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Definition: pprz_orientation_conversion.h:225
orientationSetQuat_f
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
Definition: pprz_orientation_conversion.h:173
ahrs_mlkf_update_accel
void ahrs_mlkf_update_accel(struct FloatVect3 *accel)
Definition: ahrs_float_mlkf.c:128
MAT_SUB
#define MAT_SUB(_i, _j, C, A, B)
Definition: pprz_simple_matrix.h:73
float_quat_normalize
static void float_quat_normalize(struct FloatQuat *q)
Definition: pprz_algebra_float.h:380
ahrs_mlkf_set_body_to_imu
void ahrs_mlkf_set_body_to_imu(struct OrientationReps *body_to_imu)
Definition: ahrs_float_mlkf.c:91
AhrsMlkf::lp_accel
float lp_accel
Definition: ahrs_float_mlkf.h:55
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
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
AHRS_MAG_NOISE_Z
#define AHRS_MAG_NOISE_Z
Definition: ahrs_float_mlkf.c:46
ahrs_mlkf_update_mag_full
void ahrs_mlkf_update_mag_full(struct FloatVect3 *mag)
Definition: ahrs_float_mlkf.c:156
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
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
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:222
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
MAT_MUL
#define MAT_MUL(_i, _k, _j, C, A, B)
Definition: pprz_simple_matrix.h:46
K
static float K[9]
Definition: undistort_image.c:41
AhrsMlkf::gibbs_cor
struct FloatQuat gibbs_cor
Definition: ahrs_float_mlkf.h:53
body_to_imu
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
reset_state
static void reset_state(void)
Incorporate errors to reference and zeros state.
Definition: ahrs_float_mlkf.c:357
AhrsMlkf::ltp_to_imu_quat
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
Definition: ahrs_float_mlkf.h:44
AhrsMlkf
Definition: ahrs_float_mlkf.h:43
pprz_simple_matrix.h
Simple matrix helper macros.
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
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:290
ahrs_mlkf
struct AhrsMlkf ahrs_mlkf
Definition: ahrs_float_mlkf.c:59
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
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:188
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
RATES_COPY
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
dn
uint16_t dn[LIGHT_NB]
Definition: light_solar.c:48
ahrs_mlkf_init
void ahrs_mlkf_init(void)
Definition: ahrs_float_mlkf.c:62
ahrs_mlkf_propagate
void ahrs_mlkf_propagate(struct FloatRates *gyro, float dt)
Definition: ahrs_float_mlkf.c:122
AhrsMlkf::P
float P[6][6]
Definition: ahrs_float_mlkf.h:54
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
ahrs_mlkf_set_body_to_imu_quat
void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ahrs_float_mlkf.c:96
MAT_INV33
#define MAT_INV33(_invS, _S)
Definition: pprz_simple_matrix.h:99
FloatRates
angular rates
Definition: pprz_algebra_float.h:93