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