flight/libraries/insgps13state.c

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