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_ekf.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
23 
24 /* our estimated attitude */
26 /* our estimated gyro biases */
28 /* we get unbiased body rates as byproduct */
30 /* maintain a euler angles representation */
32 /* maintains a rotation matrix representation */
34 /* time derivative of our quaternion */
36 
37 #define BAFE_SSIZE 7
38 /* covariance matrix and its time derivative */
41 
42 /*
43  * F represents the Jacobian of the derivative of the system with respect
44  * its states. We do not allocate the bottom three rows since we know that
45  * the derivatives of bias_dot are all zero.
46  */
47 float bafe_F[4][7];
48 
49 /*
50  * Kalman filter variables.
51  */
52 float bafe_PHt[7];
53 float bafe_K[7];
54 float bafe_E;
55 /*
56  * H represents the Jacobian of the measurements of the attitude
57  * with respect to the states of the filter. We do not allocate the bottom
58  * three rows since we know that the attitude measurements have no
59  * relationship to gyro bias.
60  */
61 float bafe_H[4];
62 /* temporary variable used during state covariance update */
63 float bafe_FP[4][7];
64 
65 /*
66  * Q is our estimate noise variance. It is supposed to be an NxN
67  * matrix, but with elements only on the diagonals. Additionally,
68  * since the quaternion has no expected noise (we can't directly measure
69  * it), those are zero. For the gyro, we expect around 5 deg/sec noise,
70  * which is 0.08 rad/sec. The variance is then 0.08^2 ~= 0.0075.
71  */
72 /* I measured about 0.009 rad/s noise */
73 #define bafe_Q_gyro 8e-03
74 
75 /*
76  * R is our measurement noise estimate. Like Q, it is supposed to be
77  * an NxN matrix with elements on the diagonals. However, since we can
78  * not directly measure the gyro bias, we have no estimate for it.
79  * We only have an expected noise in the pitch and roll accelerometers
80  * and in the compass.
81  */
82 #define BAFE_R_PHI 1.3 * 1.3
83 #define BAFE_R_THETA 1.3 * 1.3
84 #define BAFE_R_PSI 2.5 * 2.5
85 
86 #ifndef BAFE_DT
87 #define BAFE_DT (1./512.)
88 #endif
89 
90 extern void ahrs_init(void);
91 extern void ahrs_align(void);
92 
93 
94 /*
95  * Propagate our dynamic system
96  *
97  * quat_dot = Wxq(pqr) * quat
98  * bias_dot = 0
99  *
100  * Wxq is the quaternion omega matrix:
101  *
102  * [ 0, -p, -q, -r ]
103  * 1/2 * [ p, 0, r, -q ]
104  * [ q, -r, 0, p ]
105  * [ r, q, -p, 0 ]
106  *
107  *
108  *
109  *
110  * [ 0 -p -q -r q1 q2 q3]
111  * F = 1/2 * [ p 0 r -q -q0 q3 -q2]
112  * [ q -r 0 p -q3 -q0 q1]
113  * [ r q -p 0 q2 -q1 -q0]
114  * [ 0 0 0 0 0 0 0]
115  * [ 0 0 0 0 0 0 0]
116  * [ 0 0 0 0 0 0 0]
117  *
118  */
119 
120 void ahrs_propagate(void) {
121  /* compute unbiased rates */
124 
125  /* compute F
126  F is only needed later on to update the state covariance P.
127  However, its [0:3][0:3] region is actually the Wxq(pqr) which is needed to
128  compute the time derivative of the quaternion, so we compute F now
129  */
130 
131  /* Fill in Wxq(pqr) into F */
132  bafe_F[0][0] = bafe_F[1][1] = bafe_F[2][2] = bafe_F[3][3] = 0;
133  bafe_F[1][0] = bafe_F[2][3] = bafe_rates.p * 0.5;
134  bafe_F[2][0] = bafe_F[3][1] = bafe_rates.q * 0.5;
135  bafe_F[3][0] = bafe_F[1][2] = bafe_rates.r * 0.5;
136 
137  bafe_F[0][1] = bafe_F[3][2] = -bafe_F[1][0];
138  bafe_F[0][2] = bafe_F[1][3] = -bafe_F[2][0];
139  bafe_F[0][3] = bafe_F[2][1] = -bafe_F[3][0];
140  /* Fill in [4:6][0:3] region */
141  bafe_F[0][4] = bafe_F[2][6] = bafe_quat.qx * 0.5;
142  bafe_F[0][5] = bafe_F[3][4] = bafe_quat.qy * 0.5;
143  bafe_F[0][6] = bafe_F[1][5] = bafe_quat.qz * 0.5;
144 
145  bafe_F[1][4] = bafe_F[2][5] = bafe_F[3][6] = bafe_quat.qi * -0.5;
146  bafe_F[3][5] = -bafe_F[0][4];
147  bafe_F[1][6] = -bafe_F[0][5];
148  bafe_F[2][4] = -bafe_F[0][6];
149  /* quat_dot = Wxq(pqr) * quat */
154  /* propagate quaternion */
159 
160 
161 }
162 
163 
164 extern void ahrs_update(void);
#define BAFE_SSIZE
float bafe_F[4][7]
struct FloatEulers bafe_eulers
#define BAFE_DT
angular rates
float bafe_FP[4][7]
void ahrs_update(void)
void ahrs_propagate(void)
Propagation.
#define RATES_SUB(_a, _b)
Definition: pprz_algebra.h:315
euler angles
float p
in rad/s^2
Roation quaternion.
struct FloatRates bafe_bias
void ahrs_align(void)
Paparazzi floating point algebra.
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
float bafe_H[4]
float bafe_Pdot[BAFE_SSIZE][BAFE_SSIZE]
void ahrs_init(void)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:617
float bafe_PHt[7]
float r
in rad/s^2
struct FloatRates bafe_rates
struct FloatQuat bafe_qdot
struct FloatQuat bafe_quat
struct FloatEulers bafe_dcm
struct Int32Rates gyro
gyroscope measurements
Definition: imu.h:40
float q
in rad/s^2
float bafe_K[7]
float bafe_E
float bafe_P[BAFE_SSIZE][BAFE_SSIZE]