Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 
34 
35 #include <float.h> /* for FLT_MIN */
36 #include <string.h> /* for memcpy */
37 #include <math.h> /* for M_PI */
38 
39 #include "state.h"
40 
41 #include "subsystems/imu.h"
43 #include "math/pprz_algebra_int.h"
45 #include "generated/airframe.h"
46 
47 //#include <stdio.h>
48 
49 #ifndef AHRS_PROPAGATE_FREQUENCY
50 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
51 #endif
52 
53 static inline void propagate_ref(void);
54 static inline void propagate_state(void);
55 static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, const float noise[]);
56 static inline void reset_state(void);
57 static inline void set_body_state_from_quat(void);
58 
60 
61 
62 void ahrs_init(void) {
63 
65 
66  /*
67  * Initialises our IMU alignement variables
68  * This should probably done in the IMU code instead
69  */
70  struct FloatEulers body_to_imu_euler =
74 
75  /* Set ltp_to_imu so that body is zero */
78 
80 
81  /*
82  * Initialises our state
83  */
85  const float P0_a = 1.;
86  const float P0_b = 1e-4;
87  float P0[6][6] = {{ P0_a, 0., 0., 0., 0., 0. },
88  { 0., P0_a, 0., 0., 0., 0. },
89  { 0., 0., P0_a, 0., 0., 0. },
90  { 0., 0., 0., P0_b, 0., 0. },
91  { 0., 0., 0., 0., P0_b, 0. },
92  { 0., 0., 0., 0., 0., P0_b}};
93  memcpy(ahrs_impl.P, P0, sizeof(P0));
94 
95 }
96 
97 void ahrs_align(void) {
98 
99  /* Compute an initial orientation from accel and mag directly as quaternion */
101 
102  /* Convert initial orientation from quat to rotation matrix representations. */
104 
105  /* set initial body orientation */
107 
108  /* used averaged gyro as initial value for bias */
109  struct Int32Rates bias0;
112 
114 }
115 
116 void ahrs_propagate(void) {
117  propagate_ref();
118  propagate_state();
120 }
121 
122 void ahrs_update_accel(void) {
123  struct FloatVect3 imu_g;
124  ACCELS_FLOAT_OF_BFP(imu_g, imu.accel);
125  const float alpha = 0.92;
127  (1. - alpha) *(FLOAT_VECT3_NORM(imu_g) - 9.81);
128  const struct FloatVect3 earth_g = {0., 0., -9.81 };
129  const float dn = 250*fabs( ahrs_impl.lp_accel );
130  const float g_noise[] = {1.+dn, 1.+dn, 1.+dn};
131  // const float g_noise[] = {150., 150., 150.};
132  update_state(&earth_g, &imu_g, g_noise);
133  reset_state();
134 }
135 
136 
137 void ahrs_update_mag(void) {
138  struct FloatVect3 imu_h;
139  MAGS_FLOAT_OF_BFP(imu_h, imu.mag);
140  const struct FloatVect3 earth_h = { AHRS_H_X , AHRS_H_Y, AHRS_H_Z };
141  const float h_noise[] = { 0.1610, 0.1771, 0.2659};
142  update_state(&earth_h, &imu_h, h_noise);
143  reset_state();
144 }
145 
146 
147 static inline void propagate_ref(void) {
148  /* converts gyro to floating point */
149  struct FloatRates gyro_float;
150  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
151 
152  /* unbias measurement */
153  RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
154 
155 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
156  /* lowpass angular rates */
157  const float alpha = 0.1;
159  (1.-alpha), gyro_float, alpha);
160 #else
161  RATES_COPY(ahrs_impl.imu_rate, gyro_float);
162 #endif
163 
164 
165  /* propagate reference quaternion only if rate is non null */
166  const float no = FLOAT_RATES_NORM(ahrs_impl.imu_rate);
167  if (no > FLT_MIN) {
168  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
169  const float a = 0.5*no*dt;
170  const float ca = cosf(a);
171  const float sa_ov_no = sinf(a)/no;
172  const float dp = sa_ov_no*ahrs_impl.imu_rate.p;
173  const float dq = sa_ov_no*ahrs_impl.imu_rate.q;
174  const float dr = sa_ov_no*ahrs_impl.imu_rate.r;
175  const float qi = ahrs_impl.ltp_to_imu_quat.qi;
176  const float qx = ahrs_impl.ltp_to_imu_quat.qx;
177  const float qy = ahrs_impl.ltp_to_imu_quat.qy;
178  const float qz = ahrs_impl.ltp_to_imu_quat.qz;
179  ahrs_impl.ltp_to_imu_quat.qi = ca*qi - dp*qx - dq*qy - dr*qz;
180  ahrs_impl.ltp_to_imu_quat.qx = dp*qi + ca*qx + dr*qy - dq*qz;
181  ahrs_impl.ltp_to_imu_quat.qy = dq*qi - dr*qx + ca*qy + dp*qz;
182  ahrs_impl.ltp_to_imu_quat.qz = dr*qi + dq*qx - dp*qy + ca*qz;
183 
184  // printf("%f\n", ahrs_impl.ltp_to_imu_quat.qi);
185 
187  }
188 
189 }
190 
195 static inline void propagate_state(void) {
196 
197  /* predict covariance */
198  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
199  const float dp = ahrs_impl.imu_rate.p*dt;
200  const float dq = ahrs_impl.imu_rate.q*dt;
201  const float dr = ahrs_impl.imu_rate.r*dt;
202 
203  float F[6][6] = {{ 1., dr, -dq, -dt, 0., 0. },
204  { -dr, 1., dp, 0., -dt, 0. },
205  { dq, -dp, 1., 0., 0., -dt },
206  { 0., 0., 0., 1., 0., 0. },
207  { 0., 0., 0., 0., 1., 0. },
208  { 0., 0., 0., 0., 0., 1. }};
209  // P = FPF' + GQG
210  float tmp[6][6];
211  MAT_MUL(6,6,6, tmp, F, ahrs_impl.P);
212  MAT_MUL_T(6,6,6, ahrs_impl.P, tmp, F);
213  const float dt2 = dt * dt;
214  const float GQG[6] = {dt2*10e-3, dt2*10e-3, dt2*10e-3, dt2*9e-6, dt2*9e-6, dt2*9e-6 };
215  for (int i=0;i<6;i++)
216  ahrs_impl.P[i][i] += GQG[i];
217 
218 }
219 
220 
224 static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, const float noise[]) {
225 
226  /* converted expected measurement from inertial to body frame */
227  struct FloatVect3 b_expected;
228  FLOAT_RMAT_VECT3_MUL(b_expected, ahrs_impl.ltp_to_imu_rmat, *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  float tmp[3][6];
235  MAT_MUL(3,6,6, tmp, H, ahrs_impl.P);
236  float S[3][3];
237  MAT_MUL_T(3,6,3, S, tmp, H);
238  for (int i=0;i<3;i++)
239  S[i][i] += noise[i];
240  float invS[3][3];
241  MAT_INV33(invS, S);
242 
243  // K = PH'invS
244  float tmp2[6][3];
245  MAT_MUL_T(6,6,3, tmp2, ahrs_impl.P, H);
246  float K[6][3];
247  MAT_MUL(6,3,3, K, tmp2, invS);
248 
249  // P = (I-KH)P
250  float tmp3[6][6];
251  MAT_MUL(6,3,6, tmp3, K, H);
252  float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
253  { 0., 1., 0., 0., 0., 0. },
254  { 0., 0., 1., 0., 0., 0. },
255  { 0., 0., 0., 1., 0., 0. },
256  { 0., 0., 0., 0., 1., 0. },
257  { 0., 0., 0., 0., 0., 1. }};
258  float tmp4[6][6];
259  MAT_SUB(6,6, tmp4, I6, tmp3);
260  float tmp5[6][6];
261  MAT_MUL(6,6,6, tmp5, tmp4, ahrs_impl.P);
262  memcpy(ahrs_impl.P, tmp5, sizeof(ahrs_impl.P));
263 
264  // X = X + Ke
265  struct FloatVect3 e;
266  VECT3_DIFF(e, *b_measured, b_expected);
267  ahrs_impl.gibbs_cor.qx += K[0][0]*e.x + K[0][1]*e.y + K[0][2]*e.z;
268  ahrs_impl.gibbs_cor.qy += K[1][0]*e.x + K[1][1]*e.y + K[1][2]*e.z;
269  ahrs_impl.gibbs_cor.qz += K[2][0]*e.x + K[2][1]*e.y + K[2][2]*e.z;
270  ahrs_impl.gyro_bias.p += K[3][0]*e.x + K[3][1]*e.y + K[3][2]*e.z;
271  ahrs_impl.gyro_bias.q += K[4][0]*e.x + K[4][1]*e.y + K[4][2]*e.z;
272  ahrs_impl.gyro_bias.r += K[5][0]*e.x + K[5][1]*e.y + K[5][2]*e.z;
273 
274 }
275 
276 
280 static inline void reset_state(void) {
281 
282  ahrs_impl.gibbs_cor.qi = 2.;
283  struct FloatQuat q_tmp;
285  FLOAT_QUAT_NORMALIZE(q_tmp);
286  memcpy(&ahrs_impl.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_impl.ltp_to_imu_quat));
289 
290 }
291 
295 static inline void set_body_state_from_quat(void) {
296 
297  /* Compute LTP to BODY quaternion */
298  struct FloatQuat ltp_to_body_quat;
300  /* Set in state interface */
301  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
302 
303  /* compute body rates */
304  struct FloatRates body_rate;
306  /* Set state */
307  stateSetBodyRates_f(&body_rate);
308 
309 }
#define FLOAT_RATES_ZERO(_r)
Interface to align the AHRS via low-passed measurements at startup.
#define IMU_BODY_TO_IMU_THETA
Definition: imu.h:82
struct FloatRates imu_rate
Rotational velocity in IMU frame.
void ahrs_propagate(void)
Propagation.
static void set_body_state_from_quat(void)
Compute body orientation and rates from imu orientation and rates.
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
angular rates
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:41
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:301
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
Utility functions for floating point AHRS implementations.
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:985
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
static void update_state(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, const float noise[])
Incorporate one 3D vector measurement.
struct Int32Rates gyro_prev
previous gyroscope measurements
Definition: imu.h:43
#define RATES_SUB(_a, _b)
Definition: pprz_algebra.h:315
euler angles
#define MAT_MUL_T(_i, _k, _j, C, A, B)
#define FLOAT_QUAT_OF_EULERS(_q, _e)
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:42
struct FloatRates gyro_bias
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:653
float p
in rad/s^2
#define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
Roation quaternion.
struct FloatRMat body_to_imu_rmat
#define FLOAT_VECT3_NORM(_v)
void ahrs_align(void)
Aligns the AHRS.
struct FloatQuat body_to_imu_quat
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:35
struct AhrsMlkf ahrs_impl
Paparazzi floating point algebra.
#define RMAT_COPY(_o, _i)
Definition: pprz_algebra.h:562
uint16_t dn[LIGHT_NB]
Definition: light_solar.c:51
struct FloatRMat ltp_to_imu_rmat
Rotation from LocalTangentPlane to IMU frame as Rotation Matrix.
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
Inertial Measurement Unit interface.
void ahrs_init(void)
AHRS initialization.
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
angular rates
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:617
static void propagate_ref(void)
float r
in rad/s^2
#define AHRS_H_Y
float P[6][6]
static void propagate_state(void)
Progagate filter's covariance We don't propagate state as we assume to have reseted.
#define AHRS_UNINIT
Definition: ahrs.h:35
struct Int32Vect3 mag
magnetometer measurements
Definition: imu.h:42
#define FLOAT_QUAT_NORMALIZE(_q)
struct FloatQuat gibbs_cor
#define AHRS_H_X
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
#define AHRS_PROPAGATE_FREQUENCY
static void reset_state(void)
Incorporate errors to reference and zeros state.
API to get/set the generic vehicle states.
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:476
#define MAT_INV33(_invS, _S)
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:153
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:641
#define MAT_SUB(_i, _j, C, A, B)
float lp_accel
#define FLOAT_RMAT_OF_QUAT(_rm, _q)
#define FLOAT_RMAT_OF_EULERS(_rm, _e)
#define FLOAT_RATES_NORM(_v)
float q
in rad/s^2
#define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin)
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)
#define IMU_BODY_TO_IMU_PSI
Definition: imu.h:83
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
#define FLOAT_QUAT_ZERO(_q)
#define IMU_BODY_TO_IMU_PHI
Definition: imu.h:81
#define AHRS_RUNNING
Definition: ahrs.h:36
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:40
Paparazzi fixed point algebra.
#define MAT_MUL(_i, _k, _j, C, A, B)