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