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