/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* 21 state EKF based on https://github.com/priseborough/InertialNav Converted from Matlab to C++ by Paul Riseborough This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ #ifndef AP_NavEKF #define AP_NavEKF #include #include #include #include #include #include // #define MATH_CHECK_INDEXES 1 #include #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #include #endif class AP_AHRS; class NavEKF { public: typedef float ftype; #if MATH_CHECK_INDEXES typedef VectorN Vector2; typedef VectorN Vector3; typedef VectorN Vector6; typedef VectorN Vector8; typedef VectorN Vector11; typedef VectorN Vector13; typedef VectorN Vector14; typedef VectorN Vector15; typedef VectorN Vector22; typedef VectorN,3> Matrix3; typedef VectorN,22> Matrix22; typedef VectorN,22> Matrix22_50; #else typedef ftype Vector2[2]; typedef ftype Vector3[3]; typedef ftype Vector6[6]; typedef ftype Vector8[8]; typedef ftype Vector11[11]; typedef ftype Vector13[13]; typedef ftype Vector14[14]; typedef ftype Vector15[15]; typedef ftype Vector22[22]; typedef ftype Matrix3[3][3]; typedef ftype Matrix22[22][22]; typedef ftype Matrix22_50[22][50]; #endif // Constructor NavEKF(const AP_AHRS *ahrs, AP_Baro &baro); // Initialise the filter states from the AHRS and magnetometer data (if present) // This method can be used when the vehicle is moving void InitialiseFilterDynamic(void); // Initialise the states from accelerometer and magnetometer data (if present) // This method can only be used when the vehicle is static void InitialiseFilterBootstrap(void); // inhibits position and velocity attitude corrections when set to true // setting to true has same effect as ahrs.set_correct_centrifugal(false) void SetStaticMode(bool setting); // Update Filter States - this should be called whenever new IMU data is available void UpdateFilter(void); // return true if the filter is healthy bool healthy(void) const; // return true if filter is dead-reckoning height bool HeightDrifting(void) const; // return true if filter is dead-reckoning position bool PositionDrifting(void) const; // fill in latitude, longitude and height of the reference point void getRefLLH(struct Location &loc) const; // set latitude, longitude and height of the reference point void setRefLLH(int32_t lat, int32_t lng, int32_t alt_cm); // return the last calculated NED position relative to the // reference point (m). Return false if no position is available bool getPosNED(Vector3f &pos) const; // return NED velocity in m/s void getVelNED(Vector3f &vel) const; // return bodyaxis gyro bias estimates in deg/hr void getGyroBias(Vector3f &gyroBias) const; // return body axis accelerometer bias estimates in m/s^2 void getAccelBias(Vector3f &accelBias) const; // return the NED wind speed estimates in m/s // positive is air moving in the direction of the corresponding axis void getWind(Vector3f &wind) const; // return earth magnetic field estimates in measurement units void getMagNED(Vector3f &magNED) const; // return body magnetic field estimates in measurement units void getMagXYZ(Vector3f &magXYZ) const; // return the last calculated latitude, longitude and height bool getLLH(struct Location &loc) const; // return the Euler roll, pitch and yaw angle in radians void getEulerAngles(Vector3f &eulers) const; // get the transformation matrix from NED to XYD (body) axes void getRotationNEDToBody(Matrix3f &mat) const; // get the transformation matrix from XYZ (body) to NED axes void getRotationBodyToNED(Matrix3f &mat) const; // get the quaternions defining the rotation from NED to XYZ (body) axes void getQuaternion(Quaternion &quat) const; // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements void getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov) const; // return the innovation variances for the NED Pos, NED Vel, XYZ Mag and Vtas measurements void getVariances(Vector3f &velVar, Vector3f &posVar, Vector3f &magVar, float &tasVar) const; static const struct AP_Param::GroupInfo var_info[]; private: const AP_AHRS *_ahrs; AP_Baro &_baro; // update the quaternion, velocity and position states using IMU measurements void UpdateStrapdownEquationsNED(); // calculate the predicted state covariance matrix void CovariancePrediction(); // force symmetry on the state covariance matrix void ForceSymmetry(); // constrain variances (diagonal terms) on the state covariance matrix void ConstrainVariances(); // constrain states void ConstrainStates(); // fuse selected position, velocity and height measurements void FuseVelPosNED(); // fuse magnetometer measurements void FuseMagnetometer(); // fuse true airspeed measurements void FuseAirspeed(); // zero specified range of rows in the state covariance matrix void zeroRows(Matrix22 &covMat, uint8_t first, uint8_t last); // zero specified range of columns in the state covariance matrix void zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last); // normalise the quaternion states void quatNorm(Quaternion &quatOut, const Quaternion &quatIn) const; // store states along with system time stamp in msces void StoreStates(void); // recall state vector stored at closest time to the one specified by msec void RecallStates(Vector22 &statesForFusion, uint32_t msec); // calculate nav to body quaternions from body to nav rotation matrix void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const; // calculate the earth spin vector in NED axes void calcEarthRateNED(Vector3f &omega, int32_t latitude) const; // calculate a NED velocity vector from GPS speed, course and down velocity void calcvelNED(Vector3f &velNED, float gpsCourse, float gpsGndSpd, float gpsVelD) const; // calculate from height, airspeed and ground speed whether the flight vehicle is on the ground or flying void OnGroundCheck(); // initialise the covariance matrix void CovarianceInit(float roll, float pitch, float yaw); // update IMU delta angle and delta velocity measurements void readIMUData(); // check for new valid GPS data and update stored measurement if available void readGpsData(); // check for new altitude measurement data and update stored measurement if available void readHgtData(); // check for new magnetometer data and update store measurements if available void readMagData(); // check for new airspeed data and update stored measurements if available void readAirSpdData(); // determine when to perform fusion of GPS position and velocity measurements void SelectVelPosFusion(); // determine when to perform fusion of height measurements void SelectHgtFusion(); // determine when to perform fusion of true airspeed measurements void SelectTasFusion(); // determine when to perform fusion of magnetometer measurements void SelectMagFusion(); // force alignment of the yaw angle using GPS velocity data void ForceYawAlignment(); // zero stored variables void ZeroVariables(); // reset the horizontal position states uing the last GPS measurement void ResetPosition(void); // reset velocity states using the last GPS measurement void ResetVelocity(void); // reset the vertical position state using the last height measurement void ResetHeight(void); private: // EKF Mavlink Tuneable Parameters AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s AP_Float _gpsHorizPosNoise; // GPS horizontal position measurement noise m AP_Float _baroAltNoise; // Baro height measurement noise : m^2 AP_Float _magNoise; // magnetometer measurement noise : gauss AP_Float _easNoise; // equivalent airspeed measurement noise : m/s AP_Float _windVelProcessNoise; // wind velocity state process noise : m/s^2 AP_Float _wndVarHgtRateScale; // scale factor applied to wind process noise due to height rate AP_Float _magEarthProcessNoise; // earth magnetic field process noise : gauss/sec AP_Float _magBodyProcessNoise; // earth magnetic field process noise : gauss/sec AP_Float _gyrNoise; // gyro process noise : rad/s AP_Float _accNoise; // accelerometer process noise : m/s^2 AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2 AP_Int16 _msecVelDelay; // effective average delay of GPS velocity measurements rel to IMU (msec) AP_Int16 _msecPosDelay; // effective average delay of GPS position measurements rel to (msec) AP_Int8 _fusionModeGPS; // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity AP_Int8 _gpsVelInnovGate; // Number of standard deviations applied to GPS velocity innovation consistency check AP_Int8 _gpsPosInnovGate; // Number of standard deviations applied to GPS position innovation consistency check AP_Int8 _hgtInnovGate; // Number of standard deviations applied to height innovation consistency check AP_Int8 _magInnovGate; // Number of standard deviations applied to magnetometer innovation consistency check AP_Int8 _tasInnovGate; // Number of standard deviations applied to true airspeed innovation consistency check // Tuning parameters AP_Float _gpsNEVelVarAccScale; // scale factor applied to NE velocity measurement variance due to Vdot AP_Float _gpsDVelVarAccScale; // scale factor applied to D velocity measurement variance due to Vdot AP_Float _gpsPosVarAccScale; // scale factor applied to position measurement variance due to Vdot AP_Int16 _msecHgtDelay; // effective average delay of height measurements rel to (msec) AP_Int16 _msecMagDelay; // effective average delay of magnetometer measurements rel to IMU (msec) AP_Int16 _msecTasDelay; // effective average delay of airspeed measurements rel to IMU (msec) AP_Int16 _gpsRetryTimeUseTAS; // GPS retry time following innovation consistency fail if TAS measurements are used (msec) AP_Int16 _gpsRetryTimeNoTAS; // GPS retry time following innovation consistency fail if no TAS measurements are used (msec) AP_Int16 _hgtRetryTimeMode0; // height measurement retry time following innovation consistency fail if GPS fusion mode is = 0 (msec) AP_Int16 _hgtRetryTimeMode12; // height measurement retry time following innovation consistency fail if GPS fusion mode is > 0 (msec) float _gyroBiasNoiseScaler; // scale factor applied to gyro bias state process variance when on ground float _magVarRateScale; // scale factor applied to magnetometer variance due to angular rate uint16_t _msecGpsAvg; // average number of msec between GPS measurements uint16_t _msecHgtAvg; // average number of msec between height measurements float dtVelPos; // number of seconds between position and velocity corrections // Variables uint8_t skipCounter; // counter used to skip position and height corrections to achieve _skipRatio bool statesInitialised; // boolean true when filter states have been initialised bool staticModeDemanded; // boolean true when staticMode has been demanded externally. bool velHealth; // boolean true if velocity measurements have failed innovation consistency check bool posHealth; // boolean true if position measurements have failed innovation consistency check bool hgtHealth; // boolean true if height measurements have failed innovation consistency check bool velTimeout; // boolean true if velocity measurements have failed innovation consistency check and timed out bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out Vector22 states; // state matrix - 4 x quaternions, 3 x Vel, 3 x Pos, 3 x gyro bias, 3 x accel bias, 2 x wind vel, 3 x earth mag field, 3 x body mag field Vector22 Kfusion; // Kalman gain vector Matrix22 KH; // intermediate result used for covariance updates Matrix22 KHP; // intermediate result used for covariance updates Matrix22 P; // covariance matrix Matrix22_50 storedStates; // state vectors stored for the last 50 time steps uint32_t statetimeStamp[50]; // time stamp for each state vector stored Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) Vector3f summedDelAng; // corrected & summed delta angles about the xyz body axes (rad) Vector3f summedDelVel; // corrected & summed delta velocities along the XYZ body axes (m/s) Vector3f prevDelAng; // previous delta angle use for INS coning error compensation Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f dVelIMU; // delta velocity vector in XYZ body axes measured by the IMU (m/s) Vector3f dAngIMU; // delta angle vector in XYZ body axes measured by the IMU (rad) ftype dtIMU; // time lapsed since the last IMU measurement (sec) ftype dt; // time lapsed since the last covariance prediction (sec) ftype hgtRate; // state for rate of change of height filter bool onGround; // boolean true when the flight vehicle is on the ground (not flying) const bool useAirspeed; // boolean true if airspeed data is being used const bool useCompass; // boolean true if magnetometer data is being used Vector6 innovVelPos; // innovation output for a group of measurements Vector6 varInnovVelPos; // innovation variance output for a group of measurements bool fuseVelData; // this boolean causes the velNED measurements to be fused bool fusePosData; // this boolean causes the posNE measurements to be fused bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused Vector3f velNED; // North, East, Down velocity measurements (m/s) Vector2 posNE; // North, East position measurements (m) ftype hgtMea; // height measurement relative to reference point (m) Vector22 statesAtVelTime; // States at the effective time of velNED measurements Vector22 statesAtPosTime; // States at the effective time of posNE measurements Vector22 statesAtHgtTime; // States at the effective time of hgtMea measurement Vector3f innovMag; // innovation output from fusion of X,Y,Z compass measurements Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements bool fuseMagData; // boolean true when magnetometer data is to be fused Vector3f magData; // magnetometer flux readings in X,Y,Z body axes Vector22 statesAtMagMeasTime; // filter states at the effective time of compass measurements ftype innovVtas; // innovation output from fusion of airspeed measurements ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements bool fuseVtasData; // boolean true when airspeed data is to be fused float VtasMeas; // true airspeed measurement (m/s) Vector22 statesAtVtasMeasTime; // filter states at the effective measurement time Vector3f magBias; // magnetometer bias vector in XYZ body axes const ftype covTimeStepMax; // maximum time allowed between covariance predictions const ftype covDelAngMax; // maximum delta angle between covariance predictions bool covPredStep; // boolean set to true when a covariance prediction step has been performed bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step bool posVelFuseStep; // boolean set to true when position and velocity fusion is being performed bool tasFuseStep; // boolean set to true when airspeed fusion is being performed uint32_t TASmsecPrev; // time stamp of last TAS fusion step const uint32_t TASmsecMax; // maximum allowed interval between TAS fusion steps uint32_t MAGmsecPrev; // time stamp of last compass fusion step uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step const bool fuseMeNow; // boolean to force fusion whenever data arrives bool staticMode; // boolean to force position and velocity measurements to zero for pre-arm or bench testing uint32_t lastMagUpdate; // last time compass was updated Vector3f velDotNED; // rate of change of velocity in NED frame Vector3f velDotNEDfilt; // low pass filtered velDotNED Vector3f lastVelDotNED; // velDotNED filter state uint32_t lastAirspeedUpdate; // last time airspeed was updated uint32_t IMUmsec; // time that the last IMU value was taken ftype gpsCourse; // GPS ground course angle(rad) ftype gpsGndSpd; // GPS ground speed (m/s) bool newDataGps; // true when new GPS data has arrived bool newDataMag; // true when new magnetometer data has arrived float gpsVarScaler; // scaler applied to gps measurement variance to allow for oversampling bool newDataTas; // true when new airspeed data has arrived bool tasDataWaiting; // true when new airspeed data is waiting to be fused bool newDataHgt; // true when new height data has arrived uint32_t lastHgtUpdate; // time of last height measurement received (msec) float hgtVarScaler; // scaler applied to height measurement variance to allow for oversampling uint32_t velFailTime; // time stamp when GPS velocity measurement last failed covaraiance consistency check (msec) uint32_t posFailTime; // time stamp when GPS position measurement last failed covaraiance consistency check (msec) uint32_t hgtFailTime; // time stamp when height measurement last failed covaraiance consistency check (msec) uint8_t storeIndex; // State vector storage index uint32_t lastFixTime_ms; // time of last GPS fix used to determine if new data has arrived uint32_t secondLastFixTime_ms; // time of second last GPS fix used to determine how long since last update Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator Vector3f lastAccel; // acceleration from previous IMU sample used for trapezoidal integrator Matrix22 nextP; // Predicted covariance matrix before addition of process noise to diagonals Vector22 processNoise; // process noise added to diagonals of predicted covariance matrix Vector15 SF; // intermediate variables used to calculate predicted covariance matrix Vector8 SG; // intermediate variables used to calculate predicted covariance matrix Vector11 SQ; // intermediate variables used to calculate predicted covariance matrix Vector8 SPP; // intermediate variables used to calculate predicted covariance matrix // states held by magnetomter fusion across time steps // magnetometer X,Y,Z measurements are fused across three time steps // to level computational load as this is an expensive operation struct { ftype q0; ftype q1; ftype q2; ftype q3; ftype magN; ftype magE; ftype magD; ftype magXbias; ftype magYbias; ftype magZbias; uint8_t obsIndex; Matrix3f DCM; Vector3f MagPred; ftype R_MAG; ftype SH_MAG[9]; } mag_state; #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 // performance counters perf_counter_t _perf_UpdateFilter; perf_counter_t _perf_CovariancePrediction; perf_counter_t _perf_FuseVelPosNED; perf_counter_t _perf_FuseMagnetometer; perf_counter_t _perf_FuseAirspeed; #endif }; #if CONFIG_HAL_BOARD != HAL_BOARD_PX4 #define perf_begin(x) #define perf_end(x) #endif #endif // AP_NavEKF