Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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 #ifndef AHRS_MAG_NOISE_X
54 #define AHRS_MAG_NOISE_X 0.2
55 #define AHRS_MAG_NOISE_Y 0.2
56 #define AHRS_MAG_NOISE_Z 0.2
57 #endif
58 
59 static inline void propagate_ref(void);
60 static inline void propagate_state(void);
61 static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise);
62 static inline void reset_state(void);
63 static inline void set_body_state_from_quat(void);
64 
66 
67 #if PERIODIC_TELEMETRY
69 
70 static void send_geo_mag(void) {
71  DOWNLINK_SEND_GEO_MAG(DefaultChannel, DefaultDevice,
73 }
74 #endif
75 
76 void ahrs_init(void) {
77 
79 
80  /* Set ltp_to_imu so that body is zero */
82  sizeof(struct FloatQuat));
83 
85 
86  VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
87 
88  /*
89  * Initialises our state
90  */
92  const float P0_a = 1.;
93  const float P0_b = 1e-4;
94  float P0[6][6] = {{ P0_a, 0., 0., 0., 0., 0. },
95  { 0., P0_a, 0., 0., 0., 0. },
96  { 0., 0., P0_a, 0., 0., 0. },
97  { 0., 0., 0., P0_b, 0., 0. },
98  { 0., 0., 0., 0., P0_b, 0. },
99  { 0., 0., 0., 0., 0., P0_b}};
100  memcpy(ahrs_impl.P, P0, sizeof(P0));
101 
103 
104 #if PERIODIC_TELEMETRY
105  register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
106 #endif
107 }
108 
109 void ahrs_align(void) {
110 
111  /* Compute an initial orientation from accel and mag directly as quaternion */
113 
114  /* set initial body orientation */
116 
117  /* used averaged gyro as initial value for bias */
118  struct Int32Rates bias0;
121 
123 }
124 
125 void ahrs_propagate(void) {
126  propagate_ref();
127  propagate_state();
129 }
130 
131 void ahrs_update_accel(void) {
132  struct FloatVect3 imu_g;
133  ACCELS_FLOAT_OF_BFP(imu_g, imu.accel);
134  const float alpha = 0.92;
136  (1. - alpha) *(FLOAT_VECT3_NORM(imu_g) - 9.81);
137  const struct FloatVect3 earth_g = {0., 0., -9.81 };
138  const float dn = 250*fabs( ahrs_impl.lp_accel );
139  struct FloatVect3 g_noise = {1.+dn, 1.+dn, 1.+dn};
140  update_state(&earth_g, &imu_g, &g_noise);
141  reset_state();
142 }
143 
144 
145 void ahrs_update_mag(void) {
146  struct FloatVect3 imu_h;
147  MAGS_FLOAT_OF_BFP(imu_h, imu.mag);
149  reset_state();
150 }
151 
152 
153 static inline void propagate_ref(void) {
154  /* converts gyro to floating point */
155  struct FloatRates gyro_float;
156  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
157 
158  /* unbias measurement */
159  RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
160 
161 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
162  /* lowpass angular rates */
163  const float alpha = 0.1;
165  (1.-alpha), gyro_float, alpha);
166 #else
167  RATES_COPY(ahrs_impl.imu_rate, gyro_float);
168 #endif
169 
170  /* propagate reference quaternion */
171  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
173 
174 }
175 
180 static inline void propagate_state(void) {
181 
182  /* predict covariance */
183  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
184  const float dp = ahrs_impl.imu_rate.p*dt;
185  const float dq = ahrs_impl.imu_rate.q*dt;
186  const float dr = ahrs_impl.imu_rate.r*dt;
187 
188  float F[6][6] = {{ 1., dr, -dq, -dt, 0., 0. },
189  { -dr, 1., dp, 0., -dt, 0. },
190  { dq, -dp, 1., 0., 0., -dt },
191  { 0., 0., 0., 1., 0., 0. },
192  { 0., 0., 0., 0., 1., 0. },
193  { 0., 0., 0., 0., 0., 1. }};
194  // P = FPF' + GQG
195  float tmp[6][6];
196  MAT_MUL(6,6,6, tmp, F, ahrs_impl.P);
197  MAT_MUL_T(6,6,6, ahrs_impl.P, tmp, F);
198  const float dt2 = dt * dt;
199  const float GQG[6] = {dt2*10e-3, dt2*10e-3, dt2*10e-3, dt2*9e-6, dt2*9e-6, dt2*9e-6 };
200  for (int i=0;i<6;i++)
201  ahrs_impl.P[i][i] += GQG[i];
202 
203 }
204 
205 
209 static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise) {
210 
211  /* converted expected measurement from inertial to body frame */
212  struct FloatVect3 b_expected;
213  FLOAT_QUAT_VMULT(b_expected, ahrs_impl.ltp_to_imu_quat, *i_expected);
214 
215  // S = HPH' + JRJ
216  float H[3][6] = {{ 0., -b_expected.z, b_expected.y, 0., 0., 0.},
217  { b_expected.z, 0., -b_expected.x, 0., 0., 0.},
218  {-b_expected.y, b_expected.x, 0., 0., 0., 0.}};
219  float tmp[3][6];
220  MAT_MUL(3,6,6, tmp, H, ahrs_impl.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_impl.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  float tmp4[6][6];
248  MAT_SUB(6,6, tmp4, I6, tmp3);
249  float tmp5[6][6];
250  MAT_MUL(6,6,6, tmp5, tmp4, ahrs_impl.P);
251  memcpy(ahrs_impl.P, tmp5, sizeof(ahrs_impl.P));
252 
253  // X = X + Ke
254  struct FloatVect3 e;
255  VECT3_DIFF(e, *b_measured, b_expected);
256  ahrs_impl.gibbs_cor.qx += K[0][0]*e.x + K[0][1]*e.y + K[0][2]*e.z;
257  ahrs_impl.gibbs_cor.qy += K[1][0]*e.x + K[1][1]*e.y + K[1][2]*e.z;
258  ahrs_impl.gibbs_cor.qz += K[2][0]*e.x + K[2][1]*e.y + K[2][2]*e.z;
259  ahrs_impl.gyro_bias.p += K[3][0]*e.x + K[3][1]*e.y + K[3][2]*e.z;
260  ahrs_impl.gyro_bias.q += K[4][0]*e.x + K[4][1]*e.y + K[4][2]*e.z;
261  ahrs_impl.gyro_bias.r += K[5][0]*e.x + K[5][1]*e.y + K[5][2]*e.z;
262 
263 }
264 
265 
269 static inline void reset_state(void) {
270 
271  ahrs_impl.gibbs_cor.qi = 2.;
272  struct FloatQuat q_tmp;
274  FLOAT_QUAT_NORMALIZE(q_tmp);
275  memcpy(&ahrs_impl.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_impl.ltp_to_imu_quat));
277 
278 }
279 
283 static inline void set_body_state_from_quat(void) {
284  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu);
285  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu);
286 
287  /* Compute LTP to BODY quaternion */
288  struct FloatQuat ltp_to_body_quat;
289  FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat);
290  /* Set in state interface */
291  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
292 
293  /* compute body rates */
294  struct FloatRates body_rate;
295  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate);
296  /* Set state */
297  stateSetBodyRates_f(&body_rate);
298 
299 }
#define FLOAT_RATES_ZERO(_r)
Interface to align the AHRS via low-passed measurements at startup.
rotation matrix
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)
#define AHRS_MAG_NOISE_Z
Periodic telemetry system header (includes downlink utility and generated code).
#define AHRS_MAG_NOISE_Y
angular rates
#define AHRS_MAG_NOISE_X
static const float dt
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:41
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:326
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 in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
struct Int32Rates gyro_prev
previous gyroscope measurements
Definition: imu.h:44
#define RATES_SUB(_a, _b)
Definition: pprz_algebra.h:340
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
#define MAT_MUL_T(_i, _k, _j, C, A, B)
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:42
Multiplicative linearized Kalman Filter in quaternion formulation.
struct FloatRates gyro_bias
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:720
float p
in rad/s
#define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
Roation quaternion.
static void update_state(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, struct FloatVect3 *noise)
Incorporate one 3D vector measurement.
#define FLOAT_VECT3_NORM(_v)
void ahrs_align(void)
Aligns the AHRS.
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:35
struct AhrsMlkf ahrs_impl
Paparazzi floating point algebra.
uint16_t dn[LIGHT_NB]
Definition: light_solar.c:48
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
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:672
static void propagate_ref(void)
#define FLOAT_QUAT_VMULT(v_out, q, v_in)
float r
in rad/s
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 scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:43
#define FLOAT_QUAT_NORMALIZE(_q)
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:107
struct FloatQuat gibbs_cor
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
#define AHRS_PROPAGATE_FREQUENCY
struct FloatVect3 mag_h
static void reset_state(void)
Incorporate errors to reference and zeros state.
API to get/set the generic vehicle states.
#define MAT_INV33(_invS, _S)
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:171
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:708
#define MAT_SUB(_i, _j, C, A, B)
float lp_accel
struct FloatVect3 mag_noise
float q
in rad/s
#define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt)
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
#define FLOAT_QUAT_ZERO(_q)
#define AHRS_RUNNING
Definition: ahrs.h:36
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
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)