flight/modules/StateEstimation/filterekf.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup State Estimation
   6  * @brief Acquires sensor data and computes state estimate
   7  * @{
   8  *
   9  * @file       filterekf.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  11  * @brief      Extended Kalman Filter. Calculates complete system state except
  12  *             accelerometer drift.
  13  *
  14  * @see        The GNU Public License (GPL) Version 3
  15  *
  16  ******************************************************************************/
  17 /*
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License as published by
  20  * the Free Software Foundation; either version 3 of the License, or
  21  * (at your option) any later version.
  22  *
  23  * This program is distributed in the hope that it will be useful, but
  24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  26  * for more details.
  27  *
  28  * You should have received a copy of the GNU General Public License along
  29  * with this program; if not, write to the Free Software Foundation, Inc.,
  30  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  31  */
  32
  33 #include "inc/stateestimation.h"
  34
  35 #include <ekfconfiguration.h>
  36 #include <ekfstatevariance.h>
  37 #include <attitudestate.h>
  38 #include <systemalarms.h>
  39 #include <homelocation.h>
  40
  41 #include <insgps.h>
  42 #include <CoordinateConversions.h>
  43
  44 // Private constants
  45
  46 #define STACK_REQUIRED 2048
  47 #define DT_ALPHA       1e-3f
  48 #define DT_MIN         1e-6f
  49 #define DT_MAX         1.0f
  50 #define DT_INIT        (1.0f / PIOS_SENSOR_RATE) // initialize with board sensor rate
  51
  52 #define IMPORT_SENSOR_IF_UPDATED(shortname, num) \
  53     if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \
  54         uint8_t t; \
  55         for (t = 0; t < num; t++) { \
  56             this->work.shortname[t] = state->shortname[t]; \
  57         } \
  58     }
  59
  60 // Private types
  61 struct data {
  62     EKFConfigurationData ekfConfiguration;
  63     HomeLocationData     homeLocation;
  64
  65     bool    usePos;
  66
  67     int32_t init_stage;
  68
  69     stateEstimation work;
  70
  71     bool  inited;
  72
  73     PiOSDeltatimeConfig dtconfig;
  74     bool  navOnly;
  75     float magLockAlpha;
  76 };
  77
  78
  79 // Private functions
  80
  81 static int32_t init(stateFilter *self);
  82 static filterResult filter(stateFilter *self, stateEstimation *state);
  83 static inline bool invalid_var(float data);
  84
  85 static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly);
  86
  87
  88 static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly)
  89 {
  90     handle->init      = &init;
  91     handle->filter    = &filter;
  92     handle->localdata = pios_malloc(sizeof(struct data));
  93     struct data *this = (struct data *)handle->localdata;
  94     this->usePos      = usePos;
  95     this->navOnly     = navOnly;
  96     EKFConfigurationInitialize();
  97     EKFStateVarianceInitialize();
  98     HomeLocationInitialize();
  99     return STACK_REQUIRED;
 100 }
 101
 102 int32_t filterEKF13iInitialize(stateFilter *handle)
 103 {
 104     return globalInit(handle, false, false);
 105 }
 106
 107 int32_t filterEKF13Initialize(stateFilter *handle)
 108 {
 109     return globalInit(handle, true, false);
 110 }
 111
 112 int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle)
 113 {
 114     return globalInit(handle, false, true);
 115 }
 116
 117 int32_t filterEKF13NavOnlyInitialize(stateFilter *handle)
 118 {
 119     return globalInit(handle, true, true);
 120 }
 121
 122 #ifdef FALSE
 123 // XXX
 124 // TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through
 125 // XXX
 126 int32_t filterEKF16iInitialize(stateFilter *handle)
 127 {
 128     return filterEKFi13Initialize(handle);
 129 }
 130 int32_t filterEKF16Initialize(stateFilter *handle)
 131 {
 132     return filterEKF13Initialize(handle);
 133 }
 134 #endif
 135
 136 static int32_t init(stateFilter *self)
 137 {
 138     struct data *this = (struct data *)self->localdata;
 139
 140     this->inited       = false;
 141     this->init_stage   = 0;
 142     this->work.updated = 0;
 143     PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA);
 144
 145     EKFConfigurationGet(&this->ekfConfiguration);
 146     int t;
 147     // plausibility check
 148     for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
 149         if (invalid_var(EKFConfigurationPToArray(this->ekfConfiguration.P)[t])) {
 150             return 2;
 151         }
 152     }
 153     for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
 154         if (invalid_var(EKFConfigurationQToArray(this->ekfConfiguration.Q)[t])) {
 155             return 2;
 156         }
 157     }
 158     for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
 159         if (invalid_var(EKFConfigurationRToArray(this->ekfConfiguration.R)[t])) {
 160             return 2;
 161         }
 162     }
 163     HomeLocationGet(&this->homeLocation);
 164     // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
 165     // switching between indoor and outdoor mode with Set = false)
 166     if ((this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2] < 1e-5f)) {
 167         return 2;
 168     }
 169
 170     // calculate Angle between Down vector and homeLocation.Be
 171     float cross[3];
 172     float magnorm[3]    = { this->homeLocation.Be[0], this->homeLocation.Be[1], this->homeLocation.Be[2] };
 173     vector_normalizef(magnorm, 3);
 174     const float down[3] = { 0, 0, 1 };
 175     CrossProduct(down, magnorm, cross);
 176     // VectorMagnitude(cross) = sin(Alpha)
 177     // [0,0,1] dot magnorm = magnorm[2] = cos(Alpha)
 178     this->magLockAlpha = atan2f(VectorMagnitude(cross), magnorm[2]);
 179
 180     return 0;
 181 }
 182
 183 /**
 184  * Collect all required state variables, then run complementary filter
 185  */
 186 static filterResult filter(stateFilter *self, stateEstimation *state)
 187 {
 188     struct data *this    = (struct data *)self->localdata;
 189
 190     const float zeros[3] = { 0.0f, 0.0f, 0.0f };
 191
 192     // Perform the update
 193     float dT;
 194     uint16_t sensors = 0;
 195
 196     INSSetArmed(state->armed);
 197     INSSetMagNorth(this->homeLocation.Be);
 198     state->navUsed      = (this->usePos || this->navOnly);
 199     this->work.updated |= state->updated;
 200     // check magnetometer alarm, discard any magnetometer readings if not OK
 201     // during initialization phase (but let them through afterwards)
 202     SystemAlarmsAlarmData alarms;
 203     SystemAlarmsAlarmGet(&alarms);
 204     if (alarms.Magnetometer != SYSTEMALARMS_ALARM_OK && !this->inited) {
 205         UNSET_MASK(state->updated, SENSORUPDATES_mag);
 206         UNSET_MASK(this->work.updated, SENSORUPDATES_mag);
 207     }
 208
 209     // Get most recent data
 210     IMPORT_SENSOR_IF_UPDATED(gyro, 3);
 211     IMPORT_SENSOR_IF_UPDATED(accel, 3);
 212     IMPORT_SENSOR_IF_UPDATED(mag, 3);
 213     IMPORT_SENSOR_IF_UPDATED(baro, 1);
 214     IMPORT_SENSOR_IF_UPDATED(pos, 3);
 215     IMPORT_SENSOR_IF_UPDATED(vel, 3);
 216     IMPORT_SENSOR_IF_UPDATED(airspeed, 2);
 217
 218     // check whether mandatory updates are present accels must have been supplied already,
 219     // and gyros must be supplied just now for a prediction step to take place
 220     // ("gyros last" rule for multi object synchronization)
 221     if (!(IS_SET(this->work.updated, SENSORUPDATES_accel) && IS_SET(state->updated, SENSORUPDATES_gyro))) {
 222         UNSET_MASK(state->updated, SENSORUPDATES_pos);
 223         UNSET_MASK(state->updated, SENSORUPDATES_vel);
 224         UNSET_MASK(state->updated, SENSORUPDATES_attitude);
 225         UNSET_MASK(state->updated, SENSORUPDATES_gyro);
 226         UNSET_MASK(state->updated, SENSORUPDATES_mag);
 227         return FILTERRESULT_OK;
 228     }
 229
 230     dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig);
 231
 232     if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) {
 233         // Don't initialize until all sensors are read
 234         if (this->init_stage == 0) {
 235             // Reset the INS algorithm
 236             INSGPSInit();
 237             // variance is measured in mGaus, but internally the EKF works with a normalized  vector. Scale down by Be^2
 238             INSSetMagVar((float[3]) { this->ekfConfiguration.R.MagX,
 239                                       this->ekfConfiguration.R.MagY,
 240                                       this->ekfConfiguration.R.MagZ }
 241                          );
 242             INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.AccelX,
 243                                         this->ekfConfiguration.Q.AccelY,
 244                                         this->ekfConfiguration.Q.AccelZ }
 245                            );
 246             INSSetGyroVar((float[3]) { this->ekfConfiguration.Q.GyroX,
 247                                        this->ekfConfiguration.Q.GyroY,
 248                                        this->ekfConfiguration.Q.GyroZ }
 249                           );
 250             INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q.GyroDriftX,
 251                                            this->ekfConfiguration.Q.GyroDriftY,
 252                                            this->ekfConfiguration.Q.GyroDriftZ }
 253                               );
 254             INSSetBaroVar(this->ekfConfiguration.R.BaroZ);
 255
 256             // Initialize the gyro bias
 257             float gyro_bias[3] = { 0.0f, 0.0f, 0.0f };
 258             INSSetGyroBias(gyro_bias);
 259
 260             AttitudeStateData attitudeState;
 261             AttitudeStateGet(&attitudeState);
 262
 263             // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
 264             // so pseudo "north" vector can be estimated even if the board is not level
 265             attitudeState.Roll = atan2f(-this->work.accel[1], -this->work.accel[2]);
 266             float zn  = cosf(attitudeState.Roll) * this->work.mag[2] + sinf(attitudeState.Roll) * this->work.mag[1];
 267             float yn  = cosf(attitudeState.Roll) * this->work.mag[1] - sinf(attitudeState.Roll) * this->work.mag[2];
 268
 269             // rotate accels z vector according to roll
 270             float azn = cosf(attitudeState.Roll) * this->work.accel[2] + sinf(attitudeState.Roll) * this->work.accel[1];
 271             attitudeState.Pitch = atan2f(this->work.accel[0], -azn);
 272
 273             float xn  = cosf(attitudeState.Pitch) * this->work.mag[0] + sinf(attitudeState.Pitch) * zn;
 274
 275             attitudeState.Yaw = atan2f(-yn, xn);
 276             // TODO: This is still a hack
 277             // Put this in a proper generic function in CoordinateConversion.c
 278             // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
 279             // should calculate the rotation in 3d space using proper cross product math
 280             // SUBTODO: formulate the math required
 281
 282             attitudeState.Roll  = RAD2DEG(attitudeState.Roll);
 283             attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
 284             attitudeState.Yaw   = RAD2DEG(attitudeState.Yaw);
 285
 286             RPY2Quaternion(&attitudeState.Roll, this->work.attitude);
 287
 288             INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros);
 289
 290             INSResetP(EKFConfigurationPToArray(this->ekfConfiguration.P));
 291         } else {
 292             // Run prediction a bit before any corrections
 293
 294             float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
 295             INSStatePrediction(gyros, this->work.accel, dT);
 296
 297             // Copy the attitude into the state
 298             // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
 299             if (!this->navOnly) {
 300                 state->attitude[0] = Nav.q[0];
 301                 state->attitude[1] = Nav.q[1];
 302                 state->attitude[2] = Nav.q[2];
 303                 state->attitude[3] = Nav.q[3];
 304
 305                 state->gyro[0]    -= RAD2DEG(Nav.gyro_bias[0]);
 306                 state->gyro[1]    -= RAD2DEG(Nav.gyro_bias[1]);
 307                 state->gyro[2]    -= RAD2DEG(Nav.gyro_bias[2]);
 308             }
 309             state->pos[0]   = Nav.Pos[0];
 310             state->pos[1]   = Nav.Pos[1];
 311             state->pos[2]   = Nav.Pos[2];
 312             state->vel[0]   = Nav.Vel[0];
 313             state->vel[1]   = Nav.Vel[1];
 314             state->vel[2]   = Nav.Vel[2];
 315             state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
 316         }
 317
 318         this->init_stage++;
 319         if (this->init_stage > 10) {
 320             state->navOk = true;
 321             this->inited = true;
 322         }
 323
 324         return FILTERRESULT_OK;
 325     }
 326
 327     if (!this->inited) {
 328         return this->navOnly ? FILTERRESULT_OK : FILTERRESULT_CRITICAL;
 329     }
 330
 331     float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
 332
 333     // Advance the state estimate
 334     INSStatePrediction(gyros, this->work.accel, dT);
 335
 336     // Copy the attitude into the state
 337     // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
 338     if (!this->navOnly) {
 339         state->attitude[0] = Nav.q[0];
 340         state->attitude[1] = Nav.q[1];
 341         state->attitude[2] = Nav.q[2];
 342         state->attitude[3] = Nav.q[3];
 343         state->gyro[0]    -= RAD2DEG(Nav.gyro_bias[0]);
 344         state->gyro[1]    -= RAD2DEG(Nav.gyro_bias[1]);
 345         state->gyro[2]    -= RAD2DEG(Nav.gyro_bias[2]);
 346     }
 347     {
 348         float tmp[3];
 349         Quaternion2RPY(Nav.q, tmp);
 350         state->debugNavYaw = tmp[2];
 351     }
 352     state->pos[0]   = Nav.Pos[0];
 353     state->pos[1]   = Nav.Pos[1];
 354     state->pos[2]   = Nav.Pos[2];
 355     state->vel[0]   = Nav.Vel[0];
 356     state->vel[1]   = Nav.Vel[1];
 357     state->vel[2]   = Nav.Vel[2];
 358     state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
 359
 360     // Advance the covariance estimate
 361     INSCovariancePrediction(dT);
 362
 363     if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
 364         sensors |= MAG_SENSORS;
 365         if (this->ekfConfiguration.MapMagnetometerToHorizontalPlane == EKFCONFIGURATION_MAPMAGNETOMETERTOHORIZONTALPLANE_TRUE) {
 366             // Map Magnetometer vector to correspond to the Roll+Pitch of the current Attitude State Estimate (no conflicting gravity)
 367             // Idea: Alpha between Local Down and Mag is invariant of orientation, and identical to Alpha between [0,0,1] and HomeLocation.Be
 368             // which is measured in init()
 369             float R[3][3];
 370
 371             // 1. rotate down vector into body frame
 372             Quaternion2R(Nav.q, R);
 373             float local_down[3];
 374             rot_mult(R, (float[3]) { 0, 0, 1 }, local_down);
 375             // 2. create a rotation vector that is perpendicular to rotated down vector, magnetic field vector and of size magLockAlpha
 376             float rotvec[3];
 377             CrossProduct(local_down, this->work.mag, rotvec);
 378             vector_normalizef(rotvec, 3);
 379             rotvec[0] *= -this->magLockAlpha;
 380             rotvec[1] *= -this->magLockAlpha;
 381             rotvec[2] *= -this->magLockAlpha;
 382             // 3. rotate artificial magnetometer reading from straight down to correct roll+pitch
 383             Rv2Rot(rotvec, R);
 384             float MagStrength = VectorMagnitude(this->homeLocation.Be);
 385             local_down[0] *= MagStrength;
 386             local_down[1] *= MagStrength;
 387             local_down[2] *= MagStrength;
 388             rot_mult(R, local_down, this->work.mag);
 389         }
 390         // debug rotated mags
 391         state->mag[0]   = this->work.mag[0];
 392         state->mag[1]   = this->work.mag[1];
 393         state->mag[2]   = this->work.mag[2];
 394         state->updated |= SENSORUPDATES_mag;
 395     } else {
 396         // mag state is delayed until EKF processed it, allows overriding/debugging magnetometer estimate
 397         UNSET_MASK(state->updated, SENSORUPDATES_mag);
 398     }
 399
 400     if (IS_SET(this->work.updated, SENSORUPDATES_baro)) {
 401         sensors |= BARO_SENSOR;
 402     }
 403
 404     if (!this->usePos) {
 405         // position and velocity variance used in indoor mode
 406         INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.FakeGPSPosIndoor,
 407                                      this->ekfConfiguration.FakeR.FakeGPSPosIndoor,
 408                                      this->ekfConfiguration.FakeR.FakeGPSPosIndoor },
 409                         (float[3]) { this->ekfConfiguration.FakeR.FakeGPSVelIndoor,
 410                                      this->ekfConfiguration.FakeR.FakeGPSVelIndoor,
 411                                      this->ekfConfiguration.FakeR.FakeGPSVelIndoor }
 412                         );
 413     } else {
 414         // position and velocity variance used in outdoor mode
 415         INSSetPosVelVar((float[3]) { this->ekfConfiguration.R.GPSPosNorth,
 416                                      this->ekfConfiguration.R.GPSPosEast,
 417                                      this->ekfConfiguration.R.GPSPosDown },
 418                         (float[3]) { this->ekfConfiguration.R.GPSVelNorth,
 419                                      this->ekfConfiguration.R.GPSVelEast,
 420                                      this->ekfConfiguration.R.GPSVelDown }
 421                         );
 422     }
 423
 424     if (IS_SET(this->work.updated, SENSORUPDATES_pos)) {
 425         sensors |= POS_SENSORS;
 426     }
 427
 428     if (IS_SET(this->work.updated, SENSORUPDATES_vel)) {
 429         sensors |= HORIZ_SENSORS | VERT_SENSORS;
 430     }
 431
 432     if (IS_SET(this->work.updated, SENSORUPDATES_airspeed) && ((!IS_SET(this->work.updated, SENSORUPDATES_vel) && !IS_SET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) {
 433         // HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance
 434         sensors |= HORIZ_SENSORS | VERT_SENSORS;
 435         INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.FakeGPSPosIndoor,
 436                                      this->ekfConfiguration.FakeR.FakeGPSPosIndoor,
 437                                      this->ekfConfiguration.FakeR.FakeGPSPosIndoor },
 438                         (float[3]) { this->ekfConfiguration.FakeR.FakeGPSVelAirspeed,
 439                                      this->ekfConfiguration.FakeR.FakeGPSVelAirspeed,
 440                                      this->ekfConfiguration.FakeR.FakeGPSVelAirspeed }
 441                         );
 442         // rotate airspeed vector into NED frame - airspeed is measured in X axis only
 443         float R[3][3];
 444         Quaternion2R(Nav.q, R);
 445         float vtas[3] = { this->work.airspeed[1], 0.0f, 0.0f };
 446         rot_mult(R, vtas, this->work.vel);
 447     }
 448
 449     /*
 450      * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
 451      * although probably should occur within INS itself
 452      */
 453     if (sensors) {
 454         INSCorrection(this->work.mag, this->work.pos, this->work.vel, this->work.baro[0], sensors);
 455     }
 456
 457     EKFStateVarianceData vardata;
 458     EKFStateVarianceGet(&vardata);
 459     INSGetVariance(EKFStateVariancePToArray(vardata.P));
 460     EKFStateVarianceSet(&vardata);
 461     int t;
 462     for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) {
 463         if (!IS_REAL(EKFStateVariancePToArray(vardata.P)[t]) || EKFStateVariancePToArray(vardata.P)[t] <= 0.0f) {
 464             INSResetP(EKFConfigurationPToArray(this->ekfConfiguration.P));
 465             this->init_stage = -1;
 466             break;
 467         }
 468     }
 469
 470     // all sensor data has been used, reset!
 471     this->work.updated = 0;
 472
 473     if (this->init_stage < 0) {
 474         return this->navOnly ? FILTERRESULT_OK : FILTERRESULT_WARNING;
 475     } else {
 476         return FILTERRESULT_OK;
 477     }
 478 }
 479
 480 // check for invalid variance values
 481 static inline bool invalid_var(float data)
 482 {
 483     if (isnan(data) || isinf(data)) {
 484         return true;
 485     }
 486     if (data < 1e-15f) { // var should not be close to zero. And not negative either.
 487         return true;
 488     }
 489     return false;
 490 }
 491
 492 /**
 493  * @}
 494  * @}
 495  */