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     EKFStateVarianceInitialize();
  97     return STACK_REQUIRED;
  98 }
  99
 100 int32_t filterEKF13iInitialize(stateFilter *handle)
 101 {
 102     return globalInit(handle, false, false);
 103 }
 104
 105 int32_t filterEKF13Initialize(stateFilter *handle)
 106 {
 107     return globalInit(handle, true, false);
 108 }
 109
 110 int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle)
 111 {
 112     return globalInit(handle, false, true);
 113 }
 114
 115 int32_t filterEKF13NavOnlyInitialize(stateFilter *handle)
 116 {
 117     return globalInit(handle, true, true);
 118 }
 119
 120 #ifdef FALSE
 121 // XXX
 122 // TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through
 123 // XXX
 124 int32_t filterEKF16iInitialize(stateFilter *handle)
 125 {
 126     return filterEKFi13Initialize(handle);
 127 }
 128 int32_t filterEKF16Initialize(stateFilter *handle)
 129 {
 130     return filterEKF13Initialize(handle);
 131 }
 132 #endif
 133
 134 static int32_t init(stateFilter *self)
 135 {
 136     struct data *this = (struct data *)self->localdata;
 137
 138     this->inited       = false;
 139     this->init_stage   = 0;
 140     this->work.updated = 0;
 141     PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA);
 142
 143     EKFConfigurationGet(&this->ekfConfiguration);
 144     int t;
 145     // plausibility check
 146     for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
 147         if (invalid_var(EKFConfigurationPToArray(this->ekfConfiguration.P)[t])) {
 148             return 2;
 149         }
 150     }
 151     for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
 152         if (invalid_var(EKFConfigurationQToArray(this->ekfConfiguration.Q)[t])) {
 153             return 2;
 154         }
 155     }
 156     for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
 157         if (invalid_var(EKFConfigurationRToArray(this->ekfConfiguration.R)[t])) {
 158             return 2;
 159         }
 160     }
 161     HomeLocationGet(&this->homeLocation);
 162     // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
 163     // switching between indoor and outdoor mode with Set = false)
 164     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)) {
 165         return 2;
 166     }
 167
 168     // calculate Angle between Down vector and homeLocation.Be
 169     float cross[3];
 170     float magnorm[3]    = { this->homeLocation.Be[0], this->homeLocation.Be[1], this->homeLocation.Be[2] };
 171     vector_normalizef(magnorm, 3);
 172     const float down[3] = { 0, 0, 1 };
 173     CrossProduct(down, magnorm, cross);
 174     // VectorMagnitude(cross) = sin(Alpha)
 175     // [0,0,1] dot magnorm = magnorm[2] = cos(Alpha)
 176     this->magLockAlpha = atan2f(VectorMagnitude(cross), magnorm[2]);
 177
 178     return 0;
 179 }
 180
 181 /**
 182  * Collect all required state variables, then run complementary filter
 183  */
 184 static filterResult filter(stateFilter *self, stateEstimation *state)
 185 {
 186     struct data *this    = (struct data *)self->localdata;
 187
 188     const float zeros[3] = { 0.0f, 0.0f, 0.0f };
 189
 190     // Perform the update
 191     float dT;
 192     uint16_t sensors = 0;
 193
 194     INSSetArmed(state->armed);
 195     INSSetMagNorth(this->homeLocation.Be);
 196     state->navUsed      = (this->usePos || this->navOnly);
 197     this->work.updated |= state->updated;
 198     // check magnetometer alarm, discard any magnetometer readings if not OK
 199     // during initialization phase (but let them through afterwards)
 200     SystemAlarmsAlarmData alarms;
 201     SystemAlarmsAlarmGet(&alarms);
 202     if (alarms.Magnetometer != SYSTEMALARMS_ALARM_OK && !this->inited) {
 203         UNSET_MASK(state->updated, SENSORUPDATES_mag);
 204         UNSET_MASK(this->work.updated, SENSORUPDATES_mag);
 205     }
 206
 207     // Get most recent data
 208     IMPORT_SENSOR_IF_UPDATED(gyro, 3);
 209     IMPORT_SENSOR_IF_UPDATED(accel, 3);
 210     IMPORT_SENSOR_IF_UPDATED(mag, 3);
 211     IMPORT_SENSOR_IF_UPDATED(baro, 1);
 212     IMPORT_SENSOR_IF_UPDATED(pos, 3);
 213     IMPORT_SENSOR_IF_UPDATED(vel, 3);
 214     IMPORT_SENSOR_IF_UPDATED(airspeed, 2);
 215
 216     // check whether mandatory updates are present accels must have been supplied already,
 217     // and gyros must be supplied just now for a prediction step to take place
 218     // ("gyros last" rule for multi object synchronization)
 219     if (!(IS_SET(this->work.updated, SENSORUPDATES_accel) && IS_SET(state->updated, SENSORUPDATES_gyro))) {
 220         UNSET_MASK(state->updated, SENSORUPDATES_pos);
 221         UNSET_MASK(state->updated, SENSORUPDATES_vel);
 222         UNSET_MASK(state->updated, SENSORUPDATES_attitude);
 223         UNSET_MASK(state->updated, SENSORUPDATES_gyro);
 224         return FILTERRESULT_OK;
 225     }
 226
 227     dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig);
 228
 229     if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) {
 230         // Don't initialize until all sensors are read
 231         if (this->init_stage == 0) {
 232             // Reset the INS algorithm
 233             INSGPSInit();
 234             // variance is measured in mGaus, but internally the EKF works with a normalized  vector. Scale down by Be^2
 235             float Be2 = this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2];
 236             INSSetMagVar((float[3]) { this->ekfConfiguration.R.MagX / Be2,
 237                                       this->ekfConfiguration.R.MagY / Be2,
 238                                       this->ekfConfiguration.R.MagZ / Be2 }
 239                          );
 240             INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.AccelX,
 241                                         this->ekfConfiguration.Q.AccelY,
 242                                         this->ekfConfiguration.Q.AccelZ }
 243                            );
 244             INSSetGyroVar((float[3]) { this->ekfConfiguration.Q.GyroX,
 245                                        this->ekfConfiguration.Q.GyroY,
 246                                        this->ekfConfiguration.Q.GyroZ }
 247                           );
 248             INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q.GyroDriftX,
 249                                            this->ekfConfiguration.Q.GyroDriftY,
 250                                            this->ekfConfiguration.Q.GyroDriftZ }
 251                               );
 252             INSSetBaroVar(this->ekfConfiguration.R.BaroZ);
 253
 254             // Initialize the gyro bias
 255             float gyro_bias[3] = { 0.0f, 0.0f, 0.0f };
 256             INSSetGyroBias(gyro_bias);
 257
 258             AttitudeStateData attitudeState;
 259             AttitudeStateGet(&attitudeState);
 260
 261             // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
 262             // so pseudo "north" vector can be estimated even if the board is not level
 263             attitudeState.Roll = atan2f(-this->work.accel[1], -this->work.accel[2]);
 264             float zn  = cosf(attitudeState.Roll) * this->work.mag[2] + sinf(attitudeState.Roll) * this->work.mag[1];
 265             float yn  = cosf(attitudeState.Roll) * this->work.mag[1] - sinf(attitudeState.Roll) * this->work.mag[2];
 266
 267             // rotate accels z vector according to roll
 268             float azn = cosf(attitudeState.Roll) * this->work.accel[2] + sinf(attitudeState.Roll) * this->work.accel[1];
 269             attitudeState.Pitch = atan2f(this->work.accel[0], -azn);
 270
 271             float xn  = cosf(attitudeState.Pitch) * this->work.mag[0] + sinf(attitudeState.Pitch) * zn;
 272
 273             attitudeState.Yaw = atan2f(-yn, xn);
 274             // TODO: This is still a hack
 275             // Put this in a proper generic function in CoordinateConversion.c
 276             // 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)
 277             // should calculate the rotation in 3d space using proper cross product math
 278             // SUBTODO: formulate the math required
 279
 280             attitudeState.Roll  = RAD2DEG(attitudeState.Roll);
 281             attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
 282             attitudeState.Yaw   = RAD2DEG(attitudeState.Yaw);
 283
 284             RPY2Quaternion(&attitudeState.Roll, this->work.attitude);
 285
 286             INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros);
 287
 288             INSResetP(EKFConfigurationPToArray(this->ekfConfiguration.P));
 289         } else {
 290             // Run prediction a bit before any corrections
 291
 292             float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
 293             INSStatePrediction(gyros, this->work.accel, dT);
 294
 295             // Copy the attitude into the state
 296             // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
 297             if (!this->navOnly) {
 298                 state->attitude[0] = Nav.q[0];
 299                 state->attitude[1] = Nav.q[1];
 300                 state->attitude[2] = Nav.q[2];
 301                 state->attitude[3] = Nav.q[3];
 302
 303                 state->gyro[0]    -= RAD2DEG(Nav.gyro_bias[0]);
 304                 state->gyro[1]    -= RAD2DEG(Nav.gyro_bias[1]);
 305                 state->gyro[2]    -= RAD2DEG(Nav.gyro_bias[2]);
 306             }
 307             state->pos[0]   = Nav.Pos[0];
 308             state->pos[1]   = Nav.Pos[1];
 309             state->pos[2]   = Nav.Pos[2];
 310             state->vel[0]   = Nav.Vel[0];
 311             state->vel[1]   = Nav.Vel[1];
 312             state->vel[2]   = Nav.Vel[2];
 313             state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
 314         }
 315
 316         this->init_stage++;
 317         if (this->init_stage > 10) {
 318             state->navOk = true;
 319             this->inited = true;
 320         }
 321
 322         return FILTERRESULT_OK;
 323     }
 324
 325     if (!this->inited) {
 326         return this->navOnly ? FILTERRESULT_OK : FILTERRESULT_CRITICAL;
 327     }
 328
 329     float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
 330
 331     // Advance the state estimate
 332     INSStatePrediction(gyros, this->work.accel, dT);
 333
 334     // Copy the attitude into the state
 335     // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
 336     if (!this->navOnly) {
 337         state->attitude[0] = Nav.q[0];
 338         state->attitude[1] = Nav.q[1];
 339         state->attitude[2] = Nav.q[2];
 340         state->attitude[3] = Nav.q[3];
 341         state->gyro[0]    -= RAD2DEG(Nav.gyro_bias[0]);
 342         state->gyro[1]    -= RAD2DEG(Nav.gyro_bias[1]);
 343         state->gyro[2]    -= RAD2DEG(Nav.gyro_bias[2]);
 344     }
 345     {
 346         float tmp[3];
 347         Quaternion2RPY(Nav.q, tmp);
 348         state->debugNavYaw = tmp[2];
 349     }
 350     state->pos[0]   = Nav.Pos[0];
 351     state->pos[1]   = Nav.Pos[1];
 352     state->pos[2]   = Nav.Pos[2];
 353     state->vel[0]   = Nav.Vel[0];
 354     state->vel[1]   = Nav.Vel[1];
 355     state->vel[2]   = Nav.Vel[2];
 356     state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
 357
 358     // Advance the covariance estimate
 359     INSCovariancePrediction(dT);
 360
 361     if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
 362         sensors |= MAG_SENSORS;
 363         if (this->ekfConfiguration.MapMagnetometerToHorizontalPlane == EKFCONFIGURATION_MAPMAGNETOMETERTOHORIZONTALPLANE_TRUE) {
 364             // Map Magnetometer vector to correspond to the Roll+Pitch of the current Attitude State Estimate (no conflicting gravity)
 365             // Idea: Alpha between Local Down and Mag is invariant of orientation, and identical to Alpha between [0,0,1] and HomeLocation.Be
 366             // which is measured in init()
 367             float R[3][3];
 368
 369             // 1. rotate down vector into body frame
 370             Quaternion2R(Nav.q, R);
 371             float local_down[3];
 372             rot_mult(R, (float[3]) { 0, 0, 1 }, local_down);
 373             // 2. create a rotation vector that is perpendicular to rotated down vector, magnetic field vector and of size magLockAlpha
 374             float rotvec[3];
 375             CrossProduct(local_down, this->work.mag, rotvec);
 376             vector_normalizef(rotvec, 3);
 377             rotvec[0] *= -this->magLockAlpha;
 378             rotvec[1] *= -this->magLockAlpha;
 379             rotvec[2] *= -this->magLockAlpha;
 380             // 3. rotate artificial magnetometer reading from straight down to correct roll+pitch
 381             Rv2Rot(rotvec, R);
 382             float MagStrength = VectorMagnitude(this->homeLocation.Be);
 383             local_down[0] *= MagStrength;
 384             local_down[1] *= MagStrength;
 385             local_down[2] *= MagStrength;
 386             rot_mult(R, local_down, this->work.mag);
 387         }
 388         // From Eric: "exporting it in MagState was meant for debugging, but I think it makes a
 389         // lot of sense to have a "corrected" magnetometer reading available in the system."
 390         // TODO: Should move above calc to filtermag, updating from here cause trouble with the state->MagStatus (LP-534)
 391         // debug rotated mags
 392         // state->mag[0]   = this->work.mag[0];
 393         // state->mag[1]   = this->work.mag[1];
 394         // state->mag[2]   = this->work.mag[2];
 395         // state->updated |= SENSORUPDATES_mag;
 396     } // else {
 397       // mag state is delayed until EKF processed it, allows overriding/debugging magnetometer estimate
 398       // UNSET_MASK(state->updated, SENSORUPDATES_mag);
 399       // }
 400
 401     if (IS_SET(this->work.updated, SENSORUPDATES_baro)) {
 402         sensors |= BARO_SENSOR;
 403     }
 404
 405     if (!this->usePos) {
 406         // position and velocity variance used in indoor mode
 407         INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.FakeGPSPosIndoor,
 408                                      this->ekfConfiguration.FakeR.FakeGPSPosIndoor,
 409                                      this->ekfConfiguration.FakeR.FakeGPSPosIndoor },
 410                         (float[3]) { this->ekfConfiguration.FakeR.FakeGPSVelIndoor,
 411                                      this->ekfConfiguration.FakeR.FakeGPSVelIndoor,
 412                                      this->ekfConfiguration.FakeR.FakeGPSVelIndoor }
 413                         );
 414     } else {
 415         // position and velocity variance used in outdoor mode
 416         INSSetPosVelVar((float[3]) { this->ekfConfiguration.R.GPSPosNorth,
 417                                      this->ekfConfiguration.R.GPSPosEast,
 418                                      this->ekfConfiguration.R.GPSPosDown },
 419                         (float[3]) { this->ekfConfiguration.R.GPSVelNorth,
 420                                      this->ekfConfiguration.R.GPSVelEast,
 421                                      this->ekfConfiguration.R.GPSVelDown }
 422                         );
 423     }
 424
 425     if (IS_SET(this->work.updated, SENSORUPDATES_pos)) {
 426         sensors |= POS_SENSORS;
 427     }
 428
 429     if (IS_SET(this->work.updated, SENSORUPDATES_vel)) {
 430         sensors |= HORIZ_SENSORS | VERT_SENSORS;
 431     }
 432
 433     if (IS_SET(this->work.updated, SENSORUPDATES_airspeed) && ((!IS_SET(this->work.updated, SENSORUPDATES_vel) && !IS_SET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) {
 434         // HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance
 435         sensors |= HORIZ_SENSORS | VERT_SENSORS;
 436         INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.FakeGPSPosIndoor,
 437                                      this->ekfConfiguration.FakeR.FakeGPSPosIndoor,
 438                                      this->ekfConfiguration.FakeR.FakeGPSPosIndoor },
 439                         (float[3]) { this->ekfConfiguration.FakeR.FakeGPSVelAirspeed,
 440                                      this->ekfConfiguration.FakeR.FakeGPSVelAirspeed,
 441                                      this->ekfConfiguration.FakeR.FakeGPSVelAirspeed }
 442                         );
 443         // rotate airspeed vector into NED frame - airspeed is measured in X axis only
 444         float R[3][3];
 445         Quaternion2R(Nav.q, R);
 446         float vtas[3] = { this->work.airspeed[1], 0.0f, 0.0f };
 447         rot_mult(R, vtas, this->work.vel);
 448     }
 449
 450     /*
 451      * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
 452      * although probably should occur within INS itself
 453      */
 454     if (sensors) {
 455         INSCorrection(this->work.mag, this->work.pos, this->work.vel, this->work.baro[0], sensors);
 456     }
 457
 458     EKFStateVarianceData vardata;
 459     EKFStateVarianceGet(&vardata);
 460     INSGetVariance(EKFStateVariancePToArray(vardata.P));
 461     EKFStateVarianceSet(&vardata);
 462     int t;
 463     for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) {
 464         if (!IS_REAL(EKFStateVariancePToArray(vardata.P)[t]) || EKFStateVariancePToArray(vardata.P)[t] <= 0.0f) {
 465             INSResetP(EKFConfigurationPToArray(this->ekfConfiguration.P));
 466             this->init_stage = -1;
 467             break;
 468         }
 469     }
 470
 471     // all sensor data has been used, reset!
 472     this->work.updated = 0;
 473
 474     if (this->init_stage < 0) {
 475         return this->navOnly ? FILTERRESULT_OK : FILTERRESULT_WARNING;
 476     } else {
 477         return FILTERRESULT_OK;
 478     }
 479 }
 480
 481 // check for invalid variance values
 482 static inline bool invalid_var(float data)
 483 {
 484     if (isnan(data) || isinf(data)) {
 485         return true;
 486     }
 487     if (data < 1e-15f) { // var should not be close to zero. And not negative either.
 488         return true;
 489     }
 490     return false;
 491 }
 492
 493 /**
 494  * @}
 495  * @}
 496  */