?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
update credits
[librepilot.git] / flight / libraries / insgps13state.c
blob6710dd944ebc1ad27f7dd39a6f32f7141a068f0c
1 /**
2 ******************************************************************************
3 * @addtogroup AHRS
4 * @{
5 * @addtogroup INSGPS
6 * @{
7 * @brief INSGPS is a joint attitude and position estimation EKF
8 *
9 * @file insgps.c
10 * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
11 * @brief An INS/GPS algorithm implemented with an EKF.
12 *
13 * @see The GNU Public License (GPL) Version 3
14 *
15 *****************************************************************************/
16 /*
17 * This program is free software; you can redistribute it and/or modify
18 * it under the terms of the GNU General Public License as published by
19 * the Free Software Foundation; either version 3 of the License, or
20 * (at your option) any later version.
21 *
22 * This program is distributed in the hope that it will be useful, but
23 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
24 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
25 * for more details.
26 *
27 * You should have received a copy of the GNU General Public License along
28 * with this program; if not, write to the Free Software Foundation, Inc.,
29 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30 */
31
32 #include "insgps.h"
33 #include <math.h>
34 #include <stdint.h>
35 #include <pios_math.h>
36 #include <mathmisc.h>
37
38 // constants/macros/typdefs
39 #define NUMX 13 // number of states, X is the state vector
40 #define NUMW 9 // number of plant noise inputs, w is disturbance noise vector
41 #define NUMV 10 // number of measurements, v is the measurement noise vector
42 #define NUMU 6 // number of deterministic inputs, U is the input vector
43 #pragma GCC optimize "O3"
44 // Private functions
45 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
46 float Q[NUMW], float dT, float P[NUMX][NUMX]);
47 static void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
48 float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
49 uint16_t SensorsUsed);
50 static void RungeKutta(float X[NUMX], float U[NUMU], float dT);
51 static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
52 static void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
53 float G[NUMX][NUMW]);
54 static void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
55 static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
56
57 // Private variables
58
59 // speed optimizations, describe matrix sparsity
60 // derived from state equations in
61 // LinearizeFG() and LinearizeH():
62 //
63 // usage F: usage G: usage H:
64 // -0123456789abc 012345678 0123456789abc
65 // 0...X......... ......... X............
66 // 1....X........ ......... .X...........
67 // 2.....X....... ......... ..X..........
68 // 3......XXXX... ...XXX... ...X.........
69 // 4......XXXX... ...XXX... ....X........
70 // 5......XXXX... ...XXX... .....X.......
71 // 6.....ooXXXXXX XXX...... ......XXXX...
72 // 7.....oXoXXXXX XXX...... ......XXXX...
73 // 8.....oXXoXXXX XXX...... ......XXXX...
74 // 9.....oXXXoXXX XXX...... ..X..........
75 // a............. ......Xoo
76 // b............. ......oXo
77 // c............. ......ooX
78
79 static int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 5, 5, 5, 5, 13, 13, 13 };
80 static int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 };
81
82 static int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 };
83 static int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 };
84
85 static int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
86 static int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
87
88 static struct EKFData {
89 // linearized system matrices
90 float F[NUMX][NUMX];
91 float G[NUMX][NUMW];
92 float H[NUMV][NUMX];
93 // local magnetic unit vector in NED frame
94 float Be[3];
95 // covariance matrix and state vector
96 float P[NUMX][NUMX];
97 float X[NUMX];
98 // input noise and measurement noise variances
99 float Q[NUMW];
100 float R[NUMV];
101 } ekf;
102
103 // Global variables
104 struct NavStruct Nav;
105
106 // ************* Exposed Functions ****************
107 // *************************************************
108
109 uint16_t ins_get_num_states()
110 {
111 return NUMX;
112 }
113
114 void INSGPSInit() // pretty much just a place holder for now
115 {
116 ekf.Be[0] = 1.0f;
117 ekf.Be[1] = 0.0f;
118 ekf.Be[2] = 0.0f; // local magnetic unit vector
119
120 for (int i = 0; i < NUMX; i++) {
121 for (int j = 0; j < NUMX; j++) {
122 ekf.P[i][j] = 0.0f; // zero all terms
123 ekf.F[i][j] = 0.0f;
124 }
125
126 for (int j = 0; j < NUMW; j++) {
127 ekf.G[i][j] = 0.0f;
128 }
129
130 for (int j = 0; j < NUMV; j++) {
131 ekf.H[j][i] = 0.0f;
132 }
133
134 ekf.X[i] = 0.0f;
135 }
136 for (int i = 0; i < NUMW; i++) {
137 ekf.Q[i] = 0.0f;
138 }
139 for (int i = 0; i < NUMV; i++) {
140 ekf.R[i] = 0.0f;
141 }
142
143
144 ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25.0f; // initial position variance (m^2)
145 ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5.0f; // initial velocity variance (m/s)^2
146 ekf.P[6][6] = ekf.P[7][7] = ekf.P[8][8] = ekf.P[9][9] = 1e-5f; // initial quaternion variance
147 ekf.P[10][10] = ekf.P[11][11] = ekf.P[12][12] = 1e-9f; // initial gyro bias variance (rad/s)^2
148
149 ekf.X[0] = ekf.X[1] = ekf.X[2] = ekf.X[3] = ekf.X[4] = ekf.X[5] = 0.0f; // initial pos and vel (m)
150 ekf.X[6] = 1.0f;
151 ekf.X[7] = ekf.X[8] = ekf.X[9] = 0.0f; // initial quaternion (level and North) (m/s)
152 ekf.X[10] = ekf.X[11] = ekf.X[12] = 0.0f; // initial gyro bias (rad/s)
153
154 ekf.Q[0] = ekf.Q[1] = ekf.Q[2] = 50e-4f; // gyro noise variance (rad/s)^2
155 ekf.Q[3] = ekf.Q[4] = ekf.Q[5] = 0.00001f; // accelerometer noise variance (m/s^2)^2
156 ekf.Q[6] = ekf.Q[7] = ekf.Q[8] = 2e-8f; // gyro bias random walk variance (rad/s^2)^2
157
158 ekf.R[0] = ekf.R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2)
159 ekf.R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2)
160 ekf.R[3] = ekf.R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2
161 ekf.R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2
162 ekf.R[6] = ekf.R[7] = ekf.R[8] = 0.005f; // magnetometer unit vector noise variance
163 ekf.R[9] = .25f; // High freq altimeter noise variance (m^2)
164 }
165
166 // ! Set the current flight state
167 void INSSetArmed(bool armed)
168 {
169 return;
170
171 // Speed up convergence of accel and gyro bias when not armed
172 if (armed) {
173 ekf.Q[9] = 1e-4f;
174 ekf.Q[8] = 2e-9f;
175 } else {
176 ekf.Q[9] = 1e-2f;
177 ekf.Q[8] = 2e-8f;
178 }
179 }
180
181 void INSResetP(const float PDiag[NUMX])
182 {
183 uint8_t i, j;
184
185 // if PDiag[i] nonzero then clear row and column and set diagonal element
186 for (i = 0; i < NUMX; i++) {
187 if (PDiag != 0) {
188 for (j = 0; j < NUMX; j++) {
189 ekf.P[i][j] = ekf.P[j][i] = 0.0f;
190 }
191 ekf.P[i][i] = PDiag[i];
192 }
193 }
194 }
195
196 void INSGetVariance(float PDiag[NUMX])
197 {
198 uint8_t i;
199
200 // retrieve diagonal elements (aka state variance)
201 if (PDiag != 0) {
202 for (i = 0; i < NUMX; i++) {
203 PDiag[i] = ekf.P[i][i];
204 }
205 }
206 }
207 void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], __attribute__((unused)) const float accel_bias[3])
208 {
209 /* Note: accel_bias not used in 13 state INS */
210 ekf.X[0] = pos[0];
211 ekf.X[1] = pos[1];
212 ekf.X[2] = pos[2];
213 ekf.X[3] = vel[0];
214 ekf.X[4] = vel[1];
215 ekf.X[5] = vel[2];
216 ekf.X[6] = q[0];
217 ekf.X[7] = q[1];
218 ekf.X[8] = q[2];
219 ekf.X[9] = q[3];
220 ekf.X[10] = gyro_bias[0];
221 ekf.X[11] = gyro_bias[1];
222 ekf.X[12] = gyro_bias[2];
223 }
224
225 void INSPosVelReset(const float pos[3], const float vel[3])
226 {
227 for (int i = 0; i < 6; i++) {
228 for (int j = i; j < NUMX; j++) {
229 ekf.P[i][j] = 0; // zero the first 6 rows and columns
230 ekf.P[j][i] = 0;
231 }
232 }
233
234 ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25; // initial position variance (m^2)
235 ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5; // initial velocity variance (m/s)^2
236
237 ekf.X[0] = pos[0];
238 ekf.X[1] = pos[1];
239 ekf.X[2] = pos[2];
240 ekf.X[3] = vel[0];
241 ekf.X[4] = vel[1];
242 ekf.X[5] = vel[2];
243 }
244
245 void INSSetPosVelVar(const float PosVar[3], const float VelVar[3])
246 {
247 ekf.R[0] = PosVar[0];
248 ekf.R[1] = PosVar[1];
249 ekf.R[2] = PosVar[2];
250 ekf.R[3] = VelVar[0];
251 ekf.R[4] = VelVar[1];
252 ekf.R[5] = VelVar[2];
253 }
254
255 void INSSetGyroBias(const float gyro_bias[3])
256 {
257 ekf.X[10] = gyro_bias[0];
258 ekf.X[11] = gyro_bias[1];
259 ekf.X[12] = gyro_bias[2];
260 }
261
262 void INSSetAccelVar(const float accel_var[3])
263 {
264 ekf.Q[3] = accel_var[0];
265 ekf.Q[4] = accel_var[1];
266 ekf.Q[5] = accel_var[2];
267 }
268
269 void INSSetGyroVar(const float gyro_var[3])
270 {
271 ekf.Q[0] = gyro_var[0];
272 ekf.Q[1] = gyro_var[1];
273 ekf.Q[2] = gyro_var[2];
274 }
275
276 void INSSetGyroBiasVar(const float gyro_bias_var[3])
277 {
278 ekf.Q[6] = gyro_bias_var[0];
279 ekf.Q[7] = gyro_bias_var[1];
280 ekf.Q[8] = gyro_bias_var[2];
281 }
282
283 void INSSetMagVar(const float scaled_mag_var[3])
284 {
285 ekf.R[6] = scaled_mag_var[0];
286 ekf.R[7] = scaled_mag_var[1];
287 ekf.R[8] = scaled_mag_var[2];
288 }
289
290 void INSSetBaroVar(const float baro_var)
291 {
292 ekf.R[9] = baro_var;
293 }
294
295 void INSSetMagNorth(const float B[3])
296 {
297 float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
298
299 ekf.Be[0] = B[0] * invmag;
300 ekf.Be[1] = B[1] * invmag;
301 ekf.Be[2] = B[2] * invmag;
302 }
303
304 void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT)
305 {
306 float U[6];
307 float invqmag;
308
309 // rate gyro inputs in units of rad/s
310 U[0] = gyro_data[0];
311 U[1] = gyro_data[1];
312 U[2] = gyro_data[2];
313
314 // accelerometer inputs in units of m/s
315 U[3] = accel_data[0];
316 U[4] = accel_data[1];
317 U[5] = accel_data[2];
318
319 // EKF prediction step
320 LinearizeFG(ekf.X, U, ekf.F, ekf.G);
321 RungeKutta(ekf.X, U, dT);
322 invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
323 ekf.X[6] *= invqmag;
324 ekf.X[7] *= invqmag;
325 ekf.X[8] *= invqmag;
326 ekf.X[9] *= invqmag;
327 // CovariancePrediction(ekf.F,ekf.G,ekf.Q,dT,ekf.P);
328
329 // Update Nav solution structure
330 Nav.Pos[0] = ekf.X[0];
331 Nav.Pos[1] = ekf.X[1];
332 Nav.Pos[2] = ekf.X[2];
333 Nav.Vel[0] = ekf.X[3];
334 Nav.Vel[1] = ekf.X[4];
335 Nav.Vel[2] = ekf.X[5];
336 Nav.q[0] = ekf.X[6];
337 Nav.q[1] = ekf.X[7];
338 Nav.q[2] = ekf.X[8];
339 Nav.q[3] = ekf.X[9];
340 Nav.gyro_bias[0] = ekf.X[10];
341 Nav.gyro_bias[1] = ekf.X[11];
342 Nav.gyro_bias[2] = ekf.X[12];
343 }
344
345 void INSCovariancePrediction(float dT)
346 {
347 CovariancePrediction(ekf.F, ekf.G, ekf.Q, dT, ekf.P);
348 }
349
350 float zeros[3] = { 0, 0, 0 };
351
352 void MagCorrection(float mag_data[3])
353 {
354 INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
355 }
356
357 void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
358 {
359 INSCorrection(mag_data, zeros, Vel, BaroAlt,
360 MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
361 BARO_SENSOR);
362 }
363
364 void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
365 {
366 INSCorrection(zeros, Pos, Vel, BaroAlt,
367 HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
368 }
369
370 void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
371 float BaroAlt)
372 {
373 INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
374 }
375
376 void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
377 {
378 INSCorrection(mag_data, Pos, Vel, zeros[0],
379 POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
380 }
381
382 void VelBaroCorrection(float Vel[3], float BaroAlt)
383 {
384 INSCorrection(zeros, zeros, Vel, BaroAlt,
385 HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
386 }
387
388 void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3],
389 const float BaroAlt, uint16_t SensorsUsed)
390 {
391 float Z[10] = { 0 };
392 float Y[10] = { 0 };
393
394 // GPS Position in meters and in local NED frame
395 Z[0] = Pos[0];
396 Z[1] = Pos[1];
397 Z[2] = Pos[2];
398
399 // GPS Velocity in meters and in local NED frame
400 Z[3] = Vel[0];
401 Z[4] = Vel[1];
402 Z[5] = Vel[2];
403
404 if (SensorsUsed & MAG_SENSORS) {
405 // magnetometer data in any units (use unit vector) and in body frame
406 float invBmag = invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
407 Z[6] = mag_data[0] * invBmag;
408 Z[7] = mag_data[1] * invBmag;
409 Z[8] = mag_data[2] * invBmag;
410 }
411
412 // barometric altimeter in meters and in local NED frame
413 Z[9] = BaroAlt;
414
415 // EKF correction step
416 LinearizeH(ekf.X, ekf.Be, ekf.H);
417 MeasurementEq(ekf.X, ekf.Be, Y);
418 SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed);
419
420 float invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
421 ekf.X[6] *= invqmag;
422 ekf.X[7] *= invqmag;
423 ekf.X[8] *= invqmag;
424 ekf.X[9] *= invqmag;
425 // Update Nav solution structure
426 Nav.Pos[0] = ekf.X[0];
427 Nav.Pos[1] = ekf.X[1];
428 Nav.Pos[2] = ekf.X[2];
429 Nav.Vel[0] = ekf.X[3];
430 Nav.Vel[1] = ekf.X[4];
431 Nav.Vel[2] = ekf.X[5];
432 Nav.q[0] = ekf.X[6];
433 Nav.q[1] = ekf.X[7];
434 Nav.q[2] = ekf.X[8];
435 Nav.q[3] = ekf.X[9];
436 Nav.gyro_bias[0] = ekf.X[10];
437 Nav.gyro_bias[1] = ekf.X[11];
438 Nav.gyro_bias[2] = ekf.X[12];
439 }
440
441 // ************* CovariancePrediction *************
442 // Does the prediction step of the Kalman filter for the covariance matrix
443 // Output, Pnew, overwrites P, the input covariance
444 // Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
445 // Q is the discrete time covariance of process noise
446 // Q is vector of the diagonal for a square matrix with
447 // dimensions equal to the number of disturbance noise variables
448 // The General Method is very inefficient,not taking advantage of the sparse F and G
449 // The first Method is very specific to this implementation
450 // ************************************************
451
452 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
453 float Q[NUMW], float dT, float P[NUMX][NUMX])
454 {
455 // Pnew = (I+F*T)*P*(I+F*T)' + (T^2)*G*Q*G' = (T^2)[(P/T + F*P)*(I/T + F') + G*Q*G')]
456
457 const float dT1 = 1.0f / dT; // multiplication is faster than division on fpu.
458 const float dTsq = dT * dT;
459
460 float Dummy[NUMX][NUMX];
461 int8_t i;
462 int8_t k;
463
464 for (i = 0; i < NUMX; i++) { // Calculate Dummy = (P/T +F*P)
465 float *Firow = F[i];
466 float *Pirow = P[i];
467 float *Dirow = Dummy[i];
468 const int8_t Fistart = FrowMin[i];
469 const int8_t Fiend = FrowMax[i];
470 int8_t j;
471
472 for (j = 0; j < NUMX; j++) {
473 Dirow[j] = Pirow[j] * dT1; // Dummy = P / T ...
474 }
475 for (k = Fistart; k <= Fiend; k++) {
476 for (j = 0; j < NUMX; j++) {
477 Dirow[j] += Firow[k] * P[k][j]; // [] + F * P
478 }
479 }
480 }
481 for (i = 0; i < NUMX; i++) { // Calculate Pnew = (T^2) [Dummy/T + Dummy*F' + G*Qw*G']
482 float *Dirow = Dummy[i];
483 float *Girow = G[i];
484 float *Pirow = P[i];
485 const int8_t Gistart = GrowMin[i];
486 const int8_t Giend = GrowMax[i];
487 int8_t j;
488
489
490 for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
491 float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ...
492
493 const float *Fjrow = F[j];
494 int8_t Fjstart = FrowMin[j];
495 int8_t Fjend = FrowMax[j];
496 k = Fjstart;
497
498 while (k <= Fjend - 3) {
499 Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ...
500 Ptmp += Dirow[k + 1] * Fjrow[k + 1];
501 Ptmp += Dirow[k + 2] * Fjrow[k + 2];
502 Ptmp += Dirow[k + 3] * Fjrow[k + 3];
503 k += 4;
504 }
505 while (k <= Fjend) {
506 Ptmp += Dirow[k] * Fjrow[k];
507 k++;
508 }
509
510 float *Gjrow = G[j];
511 const int8_t Gjstart = MAX(Gistart, GrowMin[j]);
512 const int8_t Gjend = MIN(Giend, GrowMax[j]);
513 k = Gjstart;
514 while (k <= Gjend - 2) {
515 Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ...
516 Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
517 Ptmp += Q[k + 2] * Girow[k + 2] * Gjrow[k + 2];
518 k += 3;
519 }
520 if (k <= Gjend) {
521 Ptmp += Q[k] * Girow[k] * Gjrow[k];
522 if (k <= Gjend - 1) {
523 Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
524 }
525 }
526
527 P[j][i] = Pirow[j] = Ptmp * dTsq; // [] * (T^2)
528 }
529 }
530 }
531
532 // ************* SerialUpdate *******************
533 // Does the update step of the Kalman filter for the covariance and estimate
534 // Outputs are Xnew & Pnew, and are written over P and X
535 // Z is actual measurement, Y is predicted measurement
536 // Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
537 // where K=P*H'*inv[H*P*H'+R]
538 // NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
539 // i.e. the measurment noises are uncorrelated.
540 // It therefore uses a serial update that requires no matrix inversion by
541 // processing the measurements one at a time.
542 // Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
543 // - or see Simon, "Optimal State Estimation," 1st Ed, p.150
544 // The SensorsUsed variable is a bitwise mask indicating which sensors
545 // should be used in the update.
546 // ************************************************
547 void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
548 float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
549 uint16_t SensorsUsed)
550 {
551 float HP[NUMX], HPHR, Error;
552 uint8_t i, j, k, m;
553 float Km[NUMX];
554
555 for (m = 0; m < NUMV; m++) {
556 if (SensorsUsed & (0x01 << m)) { // use this sensor for update
557 for (j = 0; j < NUMX; j++) { // Find Hp = H*P
558 HP[j] = 0;
559 }
560
561 for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
562 for (j = 0; j < NUMX; j++) { // Find Hp = H*P
563 HP[j] += H[m][k] * P[k][j];
564 }
565 }
566 HPHR = R[m]; // Find HPHR = H*P*H' + R
567 for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
568 HPHR += HP[k] * H[m][k];
569 }
570 float invHPHR = 1.0f / HPHR;
571 for (k = 0; k < NUMX; k++) {
572 Km[k] = HP[k] * invHPHR; // find K = HP/HPHR
573 }
574 for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
575 for (j = i; j < NUMX; j++) {
576 P[i][j] = P[j][i] = P[i][j] - Km[i] * HP[j];
577 }
578 }
579
580 Error = Z[m] - Y[m];
581 for (i = 0; i < NUMX; i++) { // Find X(m)= X(m-1) + K*Error
582 X[i] = X[i] + Km[i] * Error;
583 }
584 }
585 }
586 }
587
588 // ************* RungeKutta **********************
589 // Does a 4th order Runge Kutta numerical integration step
590 // Output, Xnew, is written over X
591 // NOTE the algorithm assumes time invariant state equations and
592 // constant inputs over integration step
593 // ************************************************
594
595 void RungeKutta(float X[NUMX], float U[NUMU], float dT)
596 {
597 const float dT2 = dT / 2.0f;
598 float K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
599 uint8_t i;
600
601 for (i = 0; i < NUMX; i++) {
602 Xlast[i] = X[i]; // make a working copy
603 }
604 StateEq(X, U, K1); // k1 = f(x,u)
605 for (i = 0; i < NUMX; i++) {
606 X[i] = Xlast[i] + dT2 * K1[i];
607 }
608 StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
609 for (i = 0; i < NUMX; i++) {
610 X[i] = Xlast[i] + dT2 * K2[i];
611 }
612 StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
613 for (i = 0; i < NUMX; i++) {
614 X[i] = Xlast[i] + dT * K3[i];
615 }
616 StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
617
618 // Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
619 for (i = 0; i < NUMX; i++) {
620 X[i] =
621 Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] +
622 K4[i]) * (1.0f / 6.0f);
623 }
624 }
625
626 // ************* Model Specific Stuff ***************************
627 // *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
628 //
629 // State Variables = [Pos Vel Quaternion GyroBias NO-AccelBias]
630 // Deterministic Inputs = [AngularVel Accel]
631 // Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise NO-AccelRandomWalkNoise]
632 //
633 // Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
634 // Inputs to Measurement = [EarthFrameMagField]
635 //
636 // Notes: Pos and Vel in earth frame
637 // AngularVel and Accel in body frame
638 // MagFields are unit vectors
639 // Xdot is output of StateEq()
640 // F and G are outputs of LinearizeFG(), all elements not set should be zero
641 // y is output of OutputEq()
642 // H is output of LinearizeH(), all elements not set should be zero
643 // ************************************************
644
645 static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
646 {
647 float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
648
649 // ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
650 ax = U[3];
651 ay = U[4];
652 az = U[5]; // NO BIAS STATES ON ACCELS
653 wx = U[0] - X[10];
654 wy = U[1] - X[11];
655 wz = U[2] - X[12]; // subtract the biases on gyros
656 q0 = X[6];
657 q1 = X[7];
658 q2 = X[8];
659 q3 = X[9];
660
661 // Pdot = V
662 Xdot[0] = X[3];
663 Xdot[1] = X[4];
664 Xdot[2] = X[5];
665
666 // Vdot = Reb*a
667 Xdot[3] =
668 (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2.0f * (q1 * q2 -
669 q0 * q3) *
670 ay + 2.0f * (q1 * q3 + q0 * q2) * az;
671 Xdot[4] =
672 2.0f * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
673 q3 * q3) * ay + 2 * (q2 * q3 -
674 q0 * q1) *
675 az;
676 Xdot[5] =
677 2.0f * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
678 (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81f;
679
680 // qdot = Q*w
681 Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f;
682 Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2.0f;
683 Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2.0f;
684 Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2.0f;
685
686 // best guess is that bias stays constant
687 Xdot[10] = Xdot[11] = Xdot[12] = 0;
688 }
689
690 void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
691 float G[NUMX][NUMW])
692 {
693 float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
694
695 // ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
696 ax = U[3];
697 ay = U[4];
698 az = U[5]; // NO BIAS STATES ON ACCELS
699 wx = U[0] - X[10];
700 wy = U[1] - X[11];
701 wz = U[2] - X[12]; // subtract the biases on gyros
702 q0 = X[6];
703 q1 = X[7];
704 q2 = X[8];
705 q3 = X[9];
706
707 // Pdot = V
708 F[0][3] = F[1][4] = F[2][5] = 1.0f;
709
710 // dVdot/dq
711 F[3][6] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
712 F[3][7] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
713 F[3][8] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
714 F[3][9] = 2.0f * (-q3 * ax - q0 * ay + q1 * az);
715 F[4][6] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
716 F[4][7] = 2.0f * (q2 * ax - q1 * ay - q0 * az);
717 F[4][8] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
718 F[4][9] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
719 F[5][6] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
720 F[5][7] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
721 F[5][8] = 2.0f * (-q0 * ax + q3 * ay - q2 * az);
722 F[5][9] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
723
724 // dVdot/dabias & dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G BELOW
725 // F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
726 // F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
727 // F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
728
729 // dqdot/dq
730 F[6][6] = 0;
731 F[6][7] = -wx / 2.0f;
732 F[6][8] = -wy / 2.0f;
733 F[6][9] = -wz / 2.0f;
734 F[7][6] = wx / 2.0f;
735 F[7][7] = 0;
736 F[7][8] = wz / 2.0f;
737 F[7][9] = -wy / 2.0f;
738 F[8][6] = wy / 2.0f;
739 F[8][7] = -wz / 2.0f;
740 F[8][8] = 0;
741 F[8][9] = wx / 2.0f;
742 F[9][6] = wz / 2.0f;
743 F[9][7] = wy / 2.0f;
744 F[9][8] = -wx / 2.0f;
745 F[9][9] = 0;
746
747 // dqdot/dwbias
748 F[6][10] = q1 / 2.0f;
749 F[6][11] = q2 / 2.0f;
750 F[6][12] = q3 / 2.0f;
751 F[7][10] = -q0 / 2.0f;
752 F[7][11] = q3 / 2.0f;
753 F[7][12] = -q2 / 2.0f;
754 F[8][10] = -q3 / 2.0f;
755 F[8][11] = -q0 / 2.0f;
756 F[8][12] = q1 / 2.0f;
757 F[9][10] = q2 / 2.0f;
758 F[9][11] = -q1 / 2.0f;
759 F[9][12] = -q0 / 2.0f;
760
761 // dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G HERE
762 G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3;
763 G[3][4] = 2.0f * (-q1 * q2 + q0 * q3);
764 G[3][5] = -2.0f * (q1 * q3 + q0 * q2);
765 G[4][3] = -2.0f * (q1 * q2 + q0 * q3);
766 G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3;
767 G[4][5] = 2.0f * (-q2 * q3 + q0 * q1);
768 G[5][3] = 2.0f * (-q1 * q3 + q0 * q2);
769 G[5][4] = -2.0f * (q2 * q3 + q0 * q1);
770 G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3;
771
772 // dqdot/dnw
773 G[6][0] = q1 / 2.0f;
774 G[6][1] = q2 / 2.0f;
775 G[6][2] = q3 / 2.0f;
776 G[7][0] = -q0 / 2.0f;
777 G[7][1] = q3 / 2.0f;
778 G[7][2] = -q2 / 2.0f;
779 G[8][0] = -q3 / 2.0f;
780 G[8][1] = -q0 / 2.0f;
781 G[8][2] = q1 / 2.0f;
782 G[9][0] = q2 / 2.0f;
783 G[9][1] = -q1 / 2.0f;
784 G[9][2] = -q0 / 2.0f;
785
786 // dwbias = random walk noise
787 G[10][6] = G[11][7] = G[12][8] = 1.0f;
788 // dabias = random walk noise
789 // G[13][9]=G[14][10]=G[15][11]=1; // NO BIAS STATES ON ACCELS
790 }
791
792 void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
793 {
794 float q0, q1, q2, q3;
795
796 q0 = X[6];
797 q1 = X[7];
798 q2 = X[8];
799 q3 = X[9];
800
801 // first six outputs are P and V
802 Y[0] = X[0];
803 Y[1] = X[1];
804 Y[2] = X[2];
805 Y[3] = X[3];
806 Y[4] = X[4];
807 Y[5] = X[5];
808
809 // Bb=Rbe*Be
810 Y[6] =
811 (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
812 2.0f * (q1 * q2 + q0 * q3) * Be[1] + 2.0f * (q1 * q3 -
813 q0 * q2) * Be[2];
814 Y[7] =
815 2.0f * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
816 q2 * q2 - q3 * q3) * Be[1] +
817 2.0f * (q2 * q3 + q0 * q1) * Be[2];
818 Y[8] =
819 2.0f * (q1 * q3 + q0 * q2) * Be[0] + 2.0f * (q2 * q3 -
820 q0 * q1) * Be[1] +
821 (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
822
823 // Alt = -Pz
824 Y[9] = -1.0f * X[2];
825 }
826
827 void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
828 {
829 float q0, q1, q2, q3;
830
831 q0 = X[6];
832 q1 = X[7];
833 q2 = X[8];
834 q3 = X[9];
835
836 // dP/dP=I;
837 H[0][0] = H[1][1] = H[2][2] = 1.0f;
838 // dV/dV=I;
839 H[3][3] = H[4][4] = H[5][5] = 1.0f;
840
841 // dBb/dq
842 H[6][6] = 2.0f * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
843 H[6][7] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
844 H[6][8] = 2.0f * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
845 H[6][9] = 2.0f * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
846 H[7][6] = 2.0f * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
847 H[7][7] = 2.0f * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
848 H[7][8] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
849 H[7][9] = 2.0f * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
850 H[8][6] = 2.0f * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
851 H[8][7] = 2.0f * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
852 H[8][8] = 2.0f * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
853 H[8][9] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
854
855 // dAlt/dPz = -1
856 H[9][2] = -1.0f;
857 }
858
859 /**
860 * @}
861 * @}
862 */