Ardupilot2/libraries/AP_NavEKF2/AP_NavEKF2_core.h

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
  24 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 <http://www.gnu.org/licenses/>.
 */

#ifndef AP_NavEKF2_core
#define AP_NavEKF2_core

#include <AP_Math/AP_Math.h>
#include "AP_NavEKF2.h"

// #define MATH_CHECK_INDEXES 1

#include <AP_Math/vectorN.h>

#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <systemlib/perf_counter.h>
#endif

class AP_AHRS;

class NavEKF2_core
{
public:
    // Constructor
    NavEKF2_core(NavEKF2 &frontend, const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng);

    // Initialise the states from accelerometer and magnetometer data (if present)
    // This method can only be used when the vehicle is static
    bool InitialiseFilterBootstrap(void);

    // Update Filter States - this should be called whenever new IMU data is available
    void UpdateFilter(void);

    // Check basic filter health metrics and return a consolidated health status
    bool healthy(void) const;

    // Return the last calculated NED position relative to the reference point (m).
    // If a calculated solution is not available, use the best available data and return false
    // If false returned, do not use for flight control
    bool getPosNED(Vector3f &pos) const;

    // return NED velocity in m/s
    void getVelNED(Vector3f &vel) const;

    // This returns the specific forces in the NED frame
    void getAccelNED(Vector3f &accelNED) const;

    // return body axis gyro bias estimates in rad/sec
    void getGyroBias(Vector3f &gyroBias) const;

    // return body axis gyro scale factor error as a percentage
    void getGyroScaleErrorPercentage(Vector3f &gyroScale) const;

    // return tilt error convergence metric
    void getTiltError(float &ang) const;

    // reset body axis gyro bias estimates
    void resetGyroBias(void);

    // Resets the baro so that it reads zero at the current height
    // Resets the EKF height to zero
    // Adjusts the EKf origin height so that the EKF height + origin height is the same as before
    // Returns true if the height datum reset has been performed
    // If using a range finder for height no reset is performed and it returns false
    bool resetHeightDatum(void);

    // Commands the EKF to not use GPS.
    // This command must be sent prior to arming as it will only be actioned when the filter is in static mode
    // This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms)
    // Returns 0 if command rejected
    // Returns 1 if attitude, vertical velocity and vertical position will be provided
    // Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
    uint8_t setInhibitGPS(void);

    // return the horizontal speed limit in m/s set by optical flow sensor limits
    // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
    void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;

    // return the Z-accel bias estimate in m/s^2
    void getAccelZBias(float &zbias) const;

    // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
    void getWind(Vector3f &wind) const;

    // return earth magnetic field estimates in measurement units / 1000
    void getMagNED(Vector3f &magNED) const;

    // return body magnetic field estimates in measurement units / 1000
    void getMagXYZ(Vector3f &magXYZ) const;

    // Return estimated magnetometer offsets
    // Return true if magnetometer offsets are valid
    bool getMagOffsets(Vector3f &magOffsets) const;

    // Return the last calculated latitude, longitude and height in WGS-84
    // If a calculated location isn't available, return a raw GPS measurement
    // The status will return true if a calculation or raw measurement is available
    // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
    bool getLLH(struct Location &loc) const;

    // return the latitude and longitude and height used to set the NED origin
    // All NED positions calculated by the filter are relative to this location
    // Returns false if the origin has not been set
    bool getOriginLLH(struct Location &loc) const;

    // set the latitude and longitude and height used to set the NED origin
    // All NED positions calcualted by the filter will be relative to this location
    // The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
    // Returns false if the filter has rejected the attempt to set the origin
    bool setOriginLLH(struct Location &loc);

    // return estimated height above ground level
    // return false if ground height is not being estimated.
    bool getHAGL(float &HAGL) const;

    // return the Euler roll, pitch and yaw angle in radians
    void getEulerAngles(Vector3f &eulers) const;

    // return the transformation matrix from XYZ (body) to NED axes
    void getRotationBodyToNED(Matrix3f &mat) const;

    // return 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, float &yawInnov) const;

    // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
    void  getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;

    // should we use the compass? This is public so it can be used for
    // reporting via ahrs.use_compass()
    bool use_compass(void) const;

    // write the raw optical flow measurements
    // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
    // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
    // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro
    // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
    // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
    void  writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas);

    // return data for debugging optical flow fusion
    void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;

    // called by vehicle code to specify that a takeoff is happening
    // causes the EKF to compensate for expected barometer errors due to ground effect
    void setTakeoffExpected(bool val);

    // called by vehicle code to specify that a touchdown is expected to happen
    // causes the EKF to compensate for expected barometer errors due to ground effect
    void setTouchdownExpected(bool val);

    /*
    return the filter fault status as a bitmasked integer
     0 = quaternions are NaN
     1 = velocities are NaN
     2 = badly conditioned X magnetometer fusion
     3 = badly conditioned Y magnetometer fusion
     5 = badly conditioned Z magnetometer fusion
     6 = badly conditioned airspeed fusion
     7 = badly conditioned synthetic sideslip fusion
     7 = filter is not initialised
    */
    void  getFilterFaults(uint8_t &faults) const;

    /*
    return filter timeout status as a bitmasked integer
     0 = position measurement timeout
     1 = velocity measurement timeout
     2 = height measurement timeout
     3 = magnetometer measurement timeout
     5 = unassigned
     6 = unassigned
     7 = unassigned
     7 = unassigned
    */
    void  getFilterTimeouts(uint8_t &timeouts) const;

    /*
    return filter status flags
    */
    void  getFilterStatus(nav_filter_status &status) const;

    // send an EKF_STATUS_REPORT message to GCS
    void send_status_report(mavlink_channel_t chan);

    // provides the height limit to be observed by the control loops
    // returns false if no height limiting is required
    // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
    bool getHeightControlLimit(float &height) const;

    // return the amount of yaw angle change due to the last yaw angle reset in radians
    // returns true if a reset yaw angle has been updated and not queried
    // this function should not have more than one client
    bool getLastYawResetAngle(float &yawAng);

private:
    // Reference to the global EKF frontend for parameters
    NavEKF2 &frontend;

    typedef float ftype;
#if defined(MATH_CHECK_INDEXES) && (MATH_CHECK_INDEXES == 1)
    typedef VectorN<ftype,2> Vector2;
    typedef VectorN<ftype,3> Vector3;
    typedef VectorN<ftype,4> Vector4;
    typedef VectorN<ftype,5> Vector5;
    typedef VectorN<ftype,6> Vector6;
    typedef VectorN<ftype,7> Vector7;
    typedef VectorN<ftype,8> Vector8;
    typedef VectorN<ftype,9> Vector9;
    typedef VectorN<ftype,10> Vector10;
    typedef VectorN<ftype,11> Vector11;
    typedef VectorN<ftype,13> Vector13;
    typedef VectorN<ftype,14> Vector14;
    typedef VectorN<ftype,15> Vector15;
    typedef VectorN<ftype,22> Vector22;
    typedef VectorN<ftype,23> Vector23;
    typedef VectorN<ftype,24> Vector24;
    typedef VectorN<ftype,25> Vector25;
    typedef VectorN<ftype,31> Vector31;
    typedef VectorN<ftype,28> Vector28;
    typedef VectorN<VectorN<ftype,3>,3> Matrix3;
    typedef VectorN<VectorN<ftype,24>,24> Matrix24;
    typedef VectorN<VectorN<ftype,34>,50> Matrix34_50;
    typedef VectorN<uint32_t,50> Vector_u32_50;
#else
    typedef ftype Vector2[2];
    typedef ftype Vector3[3];
    typedef ftype Vector4[4];
    typedef ftype Vector5[5];
    typedef ftype Vector6[6];
    typedef ftype Vector7[7];
    typedef ftype Vector8[8];
    typedef ftype Vector9[9];
    typedef ftype Vector10[10];
    typedef ftype Vector11[11];
    typedef ftype Vector13[13];
    typedef ftype Vector14[14];
    typedef ftype Vector15[15];
    typedef ftype Vector22[22];
    typedef ftype Vector23[23];
    typedef ftype Vector24[24];
    typedef ftype Vector25[25];
    typedef ftype Vector28[28];
    typedef ftype Matrix3[3][3];
    typedef ftype Matrix24[24][24];
    typedef ftype Matrix34_50[34][50];
    typedef uint32_t Vector_u32_50[50];
#endif

    const AP_AHRS *_ahrs;
    AP_Baro &_baro;
    const RangeFinder &_rng;

    // the states are available in two forms, either as a Vector31, or
    // broken down as individual elements. Both are equivalent (same
    // memory)
    Vector28 statesArray;
    struct state_elements {
        Vector3f    angErr;         // 0..2
        Vector3f    velocity;       // 3..5
        Vector3f    position;       // 6..8
        Vector3f    gyro_bias;      // 9..11
        Vector3f    gyro_scale;     // 12..14
        float       accel_zbias;    // 15
        Vector3f    earth_magfield; // 16..18
        Vector3f    body_magfield;  // 19..21
        Vector2f    wind_vel;       // 22..23
        Quaternion  quat;           // 24..27
    } &stateStruct;

    struct output_elements {
        Quaternion  quat;           // 0..3
        Vector3f    velocity;       // 4..6
        Vector3f    position;       // 7..9
    };

    struct imu_elements {
        Vector3f    delAng;         // 0..2
        Vector3f    delVel;         // 3..5
        float       delAngDT;       // 6
        float       delVelDT;       // 7
        uint32_t    frame;          // 8
        uint32_t    time_ms;        // 9
    };

    struct gps_elements {
        Vector2f    pos;         // 0..1
        float       hgt;         // 2
        Vector3f    vel;         // 3..5
        uint32_t    time_ms;     // 6
    };

    struct mag_elements {
        Vector3f    mag;         // 0..2
        uint32_t    time_ms;     // 3
    };

    struct baro_elements {
        float       hgt;         // 0
        uint32_t    time_ms;     // 1
    };

    struct tas_elements {
        float       tas;         // 0
        uint32_t    time_ms;     // 1
    };

    struct of_elements {
        Vector2f    flowRadXY;      // 0..1
        Vector2f    flowRadXYcomp;  // 2..3
        uint32_t    time_ms;        // 4
    };

    // 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();

    // copy covariances across from covariance prediction calculation and fix numerical errors
    void CopyCovariances();

    // constrain variances (diagonal terms) in 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();

    // fuse sythetic sideslip measurement of zero
    void FuseSideslip();

    // zero specified range of rows in the state covariance matrix
    void zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last);

    // zero specified range of columns in the state covariance matrix
    void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last);

    // store imu data in the FIFO
    void StoreIMU(void);

    // Reset the stored IMU history to current data
    void StoreIMU_reset(void);

    // recall IMU data from the FIFO
    void RecallIMU();

    // store output data in the FIFO
    void StoreOutput(void);

    // Reset the stored output history to current data
    void StoreOutputReset(void);

    // Reset the stored output quaternion history to current EKF state
    void StoreQuatReset(void);

    // recall output data from the FIFO
    void RecallOutput();

    // store altimeter data
    void StoreBaro();

    // recall altimeter data at the fusion time horizon
    // return true if data found
    bool RecallBaro();

    // store magnetometer data
    void StoreMag();

    // recall magetometer data at the fusion time horizon
    // return true if data found
    bool RecallMag();

    // store GPS data
    void StoreGPS();

    // recall GPS data at the fusion time horizon
    // return true if data found
    bool RecallGPS();

    // store true airspeed data
    void StoreTAS();

    // recall true airspeed data at the fusion time horizon
    // return true if data found
    bool RecallTAS();

    // store optical flow data
    void StoreOF();

    // recall optical flow data at the fusion time horizon
    // return true if data found
    bool RecallOF();

    // calculate nav to body quaternions from body to nav rotation matrix
    void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const;

    // calculate the NED earth spin vector in rad/sec
    void calcEarthRateNED(Vector3f &omega, int32_t latitude) const;

    // calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
    void SetFlightAndFusionModes();

    // initialise the covariance matrix
    void CovarianceInit();

    // helper functions for readIMUData
    bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt);
    bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng);

    // 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 magnetometer measurements
    void SelectMagFusion();

    // determine when to perform fusion of true airspeed measurements
    void SelectTasFusion();

    // determine when to perform fusion of synthetic sideslp measurements
    void SelectBetaFusion();

    // force alignment of the yaw angle using GPS velocity data
    void alignYawGPS();

    // initialise the earth magnetic field states using declination and current attitude and magnetometer meaasurements
    // and return attitude quaternion
    Quaternion calcQuatAndFieldStates(float roll, float pitch);

    // zero stored variables
    void InitialiseVariables();

    // 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);

    // return true if we should use the airspeed sensor
    bool useAirspeed(void) const;

    // return true if the vehicle code has requested the filter to be ready for flight
    bool readyToUseGPS(void) const;

    // decay GPS horizontal position offset to close to zero at a rate of 1 m/s
    // this allows large GPS position jumps to be accomodated gradually
    void decayGpsOffset(void);

    // Check for filter divergence
    void checkDivergence(void);

    // Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
    void calcIMU_Weighting(float K1, float K2);

    // return true if optical flow data is available
    bool optFlowDataPresent(void) const;

    // return true if we should use the range finder sensor
    bool useRngFinder(void) const;

    // determine when to perform fusion of optical flow measurements
    void SelectFlowFusion();

    // Estimate terrain offset using a single state EKF
    void EstimateTerrainOffset();

    // fuse optical flow measurements into the main filter
    void FuseOptFlow();

    // Check arm status and perform required checks and mode changes
    void performArmingChecks();

    // Set the NED origin to be used until the next filter reset
    void setOrigin();

    // determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect
    bool getTakeoffExpected();

    // determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
    bool getTouchdownExpected();

    // Assess GPS data quality and return true if good enough to align the EKF
    bool calcGpsGoodToAlign(void);

    // Read the range finder and take new measurements if available
    // Apply a median filter to range finder data
    void readRangeFinder();

    // check if the vehicle has taken off during optical flow navigation by looking at inertial and range finder data
    void detectOptFlowTakeoff(void);

    // align the NE earth magnetic field states with the published declination
    void alignMagStateDeclination();

    // Fuse compass measurements using a simple declination observation (doesn't require magnetic field states)
    void fuseCompass();

    // Calculate compass heading innovation
    float calcMagHeadingInnov();

    // Propagate PVA solution forward from the fusion time horizon to the current time horizon
    // using buffered IMU data
    void calcOutputStates();

    // Propagate PVA solution forward from the fusion time horizon to the current time horizon
    // using a simple observer
    void calcOutputStatesFast();

    // measurement buffer sizes
    static const uint32_t IMU_BUFFER_LENGTH = 100;  // number of IMU samples stored in the buffer. Samples*delta_time must be > max sensor delay
    static const uint32_t OBS_BUFFER_LENGTH = 5;    // number of non-IMU sensor samples stored in the buffer.

    // Variables
    bool statesInitialised;         // boolean true when filter states have been initialised
    bool velHealth;                 // boolean true if velocity measurements have passed innovation consistency check
    bool posHealth;                 // boolean true if position measurements have passed innovation consistency check
    bool hgtHealth;                 // boolean true if height measurements have passed innovation consistency check
    bool magHealth;                 // boolean true if magnetometer has passed innovation consistency check
    bool tasHealth;                 // boolean true if true airspeed has passed 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
    bool magTimeout;                // boolean true if magnetometer measurements have failed for too long and have timed out
    bool tasTimeout;                // boolean true if true airspeed measurements have failed for too long and have timed out
    bool badMag;                    // boolean true if the magnetometer is declared to be producing bad data
    bool badIMUdata;                // boolean true if the bad IMU data is detected

    float gpsNoiseScaler;           // Used to scale the  GPS measurement noise and consistency gates to compensate for operation with small satellite counts
    Vector28 Kfusion;               // Kalman gain vector
    Matrix24 KH;                    // intermediate result used for covariance updates
    Matrix24 KHP;                   // intermediate result used for covariance updates
    Matrix24 P;                     // covariance matrix
    imu_elements storedIMU[IMU_BUFFER_LENGTH];      // IMU data buffer
    gps_elements storedGPS[OBS_BUFFER_LENGTH];      // GPS data buffer
    mag_elements storedMag[OBS_BUFFER_LENGTH];      // Magnetometer data buffer
    baro_elements storedBaro[OBS_BUFFER_LENGTH];    // Baro data buffer
    tas_elements storedTAS[OBS_BUFFER_LENGTH];      // TAS data buffer
    output_elements storedOutput[IMU_BUFFER_LENGTH];// output state buffer
    Vector3f correctedDelAng;       // delta angles about the xyz body axes corrected for errors (rad)
    Quaternion correctedDelAngQuat; // quaternion representation of correctedDelAng
    Vector3f correctedDelVel;       // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 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)
    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)
    ftype accNavMagHoriz;           // magnitude of navigation accel in horizontal plane (m/s^2)
    Vector3f earthRateNED;          // earths angular rate vector in NED (rad/s)
    ftype dtIMUavg;                 // expected time between IMU measurements (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)
    bool prevOnGround;              // value of onGround from previous update
    bool manoeuvring;               // boolean true when the flight vehicle is performing horizontal changes in velocity
    uint32_t airborneDetectTime_ms; // last time flight movement was detected
    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 innovMag;              // innovation output from fusion of X,Y,Z compass measurements
    Vector3f varInnovMag;           // innovation variance output from fusion of X,Y,Z compass measurements
    ftype innovVtas;                // innovation output from fusion of airspeed measurements
    ftype varInnovVtas;             // innovation variance output from fusion of airspeed measurements
    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
    uint32_t prevTasStep_ms;        // time stamp of last TAS fusion step
    uint32_t prevBetaStep_ms;       // time stamp of last synthetic sideslip fusion step
    bool constPosMode;              // true when fusing a constant position to maintain attitude reference for planned operation without GPS or optical flow data
    uint32_t lastMagUpdate_ms;      // last time compass was updated
    Vector3f velDotNED;             // rate of change of velocity in NED frame
    Vector3f velDotNEDfilt;         // low pass filtered velDotNED
    uint32_t imuSampleTime_ms;      // time that the last IMU value was taken
    bool newDataTas;                // true when new airspeed data has arrived
    bool tasDataWaiting;            // true when new airspeed data is waiting to be fused
    uint32_t lastHgtReceived_ms;    // time last time we received height data
    uint16_t hgtRetryTime_ms;       // time allowed without use of height measurements before a height timeout is declared
    uint32_t lastVelPassTime_ms;    // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
    uint32_t lastPosPassTime_ms;    // time stamp when GPS position measurement last passed innovation consistency check (msec)
    uint32_t lastPosFailTime_ms;    // time stamp when GPS position measurement last failed innovation consistency check (msec)
    uint32_t lastHgtPassTime_ms;    // time stamp when height measurement last passed innovation consistency check (msec)
    uint32_t lastTasPassTime_ms;    // time stamp when airspeed measurement last passed innovation consistency check (msec)
    uint32_t lastTimeGpsReceived_ms;// last time we recieved GPS data
    uint32_t timeAtLastAuxEKF_ms;   // last time the auxilliary filter was run to fuse range or optical flow measurements
    uint32_t secondLastGpsTime_ms;  // time of second last GPS fix used to determine how long since last update
    uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
    uint32_t ekfStartTime_ms;       // time the EKF was started (msec)
    Matrix24 nextP;                 // Predicted covariance matrix before addition of process noise to diagonals
    Vector24 processNoise;          // process noise added to diagonals of predicted covariance matrix
    Vector25 SF;                    // intermediate variables used to calculate predicted covariance matrix
    Vector5 SG;                     // intermediate variables used to calculate predicted covariance matrix
    Vector8 SQ;                     // intermediate variables used to calculate predicted covariance matrix
    Vector23 SPP;                   // intermediate variables used to calculate predicted covariance matrix
    bool yawAligned;                // true when the yaw angle has been aligned
    Vector2f gpsPosGlitchOffsetNE;  // offset applied to GPS data in the NE direction to compensate for rapid changes in GPS solution
    Vector2f lastKnownPositionNE;   // last known position
    uint32_t lastDecayTime_ms;      // time of last decay of GPS position offset
    float velTestRatio;             // sum of squares of GPS velocity innovation divided by fail threshold
    float posTestRatio;             // sum of squares of GPS position innovation divided by fail threshold
    float hgtTestRatio;             // sum of squares of baro height innovation divided by fail threshold
    Vector3f magTestRatio;          // sum of squares of magnetometer innovations divided by fail threshold
    float tasTestRatio;             // sum of squares of true airspeed innovation divided by fail threshold
    bool inhibitWindStates;         // true when wind states and covariances are to remain constant
    bool inhibitMagStates;          // true when magnetic field states and covariances are to remain constant
    bool firstArmComplete;          // true when first transition out of static mode has been performed after start up
    bool firstMagYawInit;           // true when the first post takeoff initialisation of earth field and yaw angle has been performed
    bool secondMagYawInit;          // true when the second post takeoff initialisation of earth field and yaw angle has been performed
    Vector2f gpsVelGlitchOffset;    // Offset applied to the GPS velocity when the gltch radius is being  decayed back to zero
    bool gpsNotAvailable;           // bool true when valid GPS data is not available
    bool filterArmed;               // true when the vehicle is disarmed
    bool prevFilterArmed;           // vehicleArmed from previous frame
    struct Location EKF_origin;     // LLH origin of the NED axis system - do not change unless filter is reset
    bool validOrigin;               // true when the EKF origin is valid
    float gpsSpdAccuracy;           // estimated speed accuracy in m/s returned by the UBlox GPS receiver
    uint32_t lastGpsVelFail_ms;     // time of last GPS vertical velocity consistency check fail
    Vector3f lastMagOffsets;        // magnetometer offsets returned by compass object from previous update
    bool gpsAidingBad;              // true when GPS position measurements have been consistently rejected by the filter
    uint32_t lastGpsAidBadTime_ms;  // time in msec gps aiding was last detected to be bad
    float posDownAtArming;          // flight vehicle vertical position at arming used as a reference point
    bool highYawRate;               // true when the vehicle is doing rapid yaw rotation where gyro scale factor errors could cause loss of heading reference
    float yawRateFilt;              // filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
    bool useGpsVertVel;             // true if GPS vertical velocity should be used
    float yawResetAngle;            // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
    bool yawResetAngleWaiting;      // true when the yaw reset angle has been updated and has not been retrieved via the getLastYawResetAngle() function
    Vector3f tiltErrVec;            // Vector of most recent attitude error correction from Vel,Pos fusion
    float tiltErrFilt;              // Filtered tilt error metric
    bool tiltAlignComplete;         // true when tilt alignment is complete
    bool yawAlignComplete;          // true when yaw alignment is complete
    uint8_t stateIndexLim;          // Max state index used during matrix and array operations
    imu_elements imuDataDelayed;    // IMU data at the fusion time horizon
    imu_elements imuDataNew;        // IMU data at the current time horizon
    uint8_t fifoIndexNow;           // Global index for inertial and output solution at current time horizon
    uint8_t fifoIndexDelayed;       // Global index for inertial and output solution at delayed/fusion time horizon
    uint32_t hgtMeasTime_ms;        // Effective measurement time of last received height measurement
    uint32_t magMeasTime_ms;        // Effective measurement time of last received magnetometer measurement
    baro_elements baroDataNew;      // Baro data at the current time horizon
    baro_elements baroDataDelayed;  // Baro data at the fusion time horizon
    uint8_t baroStoreIndex;         // Baro data storage index
    tas_elements tasDataNew;        // TAS data at the current time horizon
    tas_elements tasDataDelayed;    // TAS data at the fusion time horizon
    uint8_t tasStoreIndex;          // TAS data storage index
    mag_elements magDataNew;        // Magnetometer data at the current time horizon
    mag_elements magDataDelayed;    // Magnetometer data at the fusion time horizon
    uint8_t magStoreIndex;          // Magnetometer data storage index
    gps_elements gpsDataNew;        // GPS data at the current time horizon
    gps_elements gpsDataDelayed;    // GPS data at the fusion time horizon
    uint8_t gpsStoreIndex;          // GPS data storage index
    output_elements outputDataNew;  // output state data at the current time step
    output_elements outputDataDelayed; // output state data at the current time step
    Vector3f delAngCorrection;      // correction applied to delta angles used by output observer to track the EKF
    Vector3f delVelCorrection;      // correction applied to earth frame delta velocities used by output observer to track the EKF
    Vector3f velCorrection;         // correction applied to velocities used by the output observer to track the EKF
    float innovYaw;                 // compass yaw angle innovation (rad)
    uint32_t timeTasReceived_ms;    // tie last TAS data was received (msec)
    bool gpsQualGood;               // true when the GPS quality can be used to initialise the navigation system

    // variables added for optical flow fusion
    of_elements storedOF[OBS_BUFFER_LENGTH];    // OF data buffer
    of_elements ofDataNew;          // OF data at the current time horizon
    of_elements ofDataDelayed;      // OF data at the fusion time horizon
    uint8_t ofStoreIndex;           // OF data storage index
    bool newDataFlow;               // true when new optical flow data has arrived
    bool flowDataValid;             // true while optical flow data is still fresh
    bool fuseOptFlowData;           // this boolean causes the last optical flow measurement to be fused
    float auxFlowObsInnov;          // optical flow rate innovation from 1-state terrain offset estimator
    float auxFlowObsInnovVar;       // innovation variance for optical flow observations from 1-state terrain offset estimator
    Vector2 flowRadXYcomp;          // motion compensated optical flow angular rates(rad/sec)
    Vector2 flowRadXY;              // raw (non motion compensated) optical flow angular rates (rad/sec)
    uint32_t flowValidMeaTime_ms;   // time stamp from latest valid flow measurement (msec)
    uint32_t rngValidMeaTime_ms;    // time stamp from latest valid range measurement (msec)
    uint32_t flowMeaTime_ms;        // time stamp from latest flow measurement (msec)
    uint32_t gndHgtValidTime_ms;    // time stamp from last terrain offset state update (msec)
    Vector3f omegaAcrossFlowTime;   // body angular rates averaged across the optical flow sample period
    Matrix3f Tnb_flow;              // transformation matrix from nav to body axes at the middle of the optical flow sample period
    Matrix3f Tbn_flow;              // transformation matrix from body to nav axes at the middle of the optical flow sample period
    Vector2 varInnovOptFlow;        // optical flow innovations variances (rad/sec)^2
    Vector2 innovOptFlow;           // optical flow LOS innovations (rad/sec)
    float Popt;                     // Optical flow terrain height state covariance (m^2)
    float terrainState;             // terrain position state (m)
    float prevPosN;                 // north position at last measurement
    float prevPosE;                 // east position at last measurement
    bool fuseRngData;               // true when fusion of range data is demanded
    float varInnovRng;              // range finder observation innovation variance (m^2)
    float innovRng;                 // range finder observation innovation (m)
    float rngMea;                   // range finder measurement (m)
    bool inhibitGndState;           // true when the terrain position state is to remain constant
    uint32_t prevFlowFuseTime_ms;   // time both flow measurement components passed their innovation consistency checks
    Vector2 flowTestRatio;          // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
    float auxFlowTestRatio;         // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
    float R_LOS;                    // variance of optical flow rate measurements (rad/sec)^2
    float auxRngTestRatio;          // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
    Vector2f flowGyroBias;          // bias error of optical flow sensor gyro output
    bool newDataRng;                // true when new valid range finder data has arrived.
    bool constVelMode;              // true when fusing a constant velocity to maintain attitude reference when either optical flow or GPS measurements are lost after arming
    bool lastConstVelMode;          // last value of holdVelocity
    Vector2f heldVelNE;             // velocity held when no aiding is available
    enum AidingMode {AID_ABSOLUTE=0,    // GPS aiding is being used (optical flow may also be used) so position estimates are absolute.
                     AID_NONE=1,       // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.
                     AID_RELATIVE=2    // only optical flow aiding is being used so position estimates will be relative
                    };
    AidingMode PV_AidingMode;           // Defines the preferred mode for aiding of velocity and position estimates from the INS
    bool gndOffsetValid;            // true when the ground offset state can still be considered valid
    Vector3f delAngBodyOF;          // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
    float delTimeOF;                // time that delAngBodyOF is summed across

    // Range finder
    float baroHgtOffset;            // offset applied when baro height used as a backup height reference if range-finder fails
    float rngOnGnd;                 // Expected range finder reading in metres when vehicle is on ground
    float storedRngMeas[3];             // Ringbuffer of stored range measurements
    uint32_t storedRngMeasTime_ms[3];   // Ringbuffer of stored range measurement times
    uint32_t lastRngMeasTime_ms;        // Timestamp of last range measurement
    uint8_t rngMeasIndex;               // Current range measurement ringbuffer index

    // Movement detector
    bool takeOffDetected;           // true when takeoff for optical flow navigation has been detected
    float rangeAtArming;            // range finder measurement when armed
    uint32_t timeAtArming_ms;       // time in msec that the vehicle armed

    // baro ground effect
    bool expectGndEffectTakeoff;      // external state from ArduCopter - takeoff expected
    uint32_t takeoffExpectedSet_ms;   // system time at which expectGndEffectTakeoff was set
    bool expectGndEffectTouchdown;    // external state from ArduCopter - touchdown expected
    uint32_t touchdownExpectedSet_ms; // system time at which expectGndEffectTouchdown was set
    float meaHgtAtTakeOff;            // height measured at commencement of takeoff

    struct {
        bool bad_xmag:1;
        bool bad_ymag:1;
        bool bad_zmag:1;
        bool bad_airspeed:1;
        bool bad_sideslip:1;
    } faultStatus;

    // 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;
        Vector9 SH_MAG;
    } mag_state;


#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
    // 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;
    perf_counter_t  _perf_FuseSideslip;
    perf_counter_t  _perf_OpticalFlowEKF;
    perf_counter_t  _perf_FuseOptFlow;
#endif

    // should we assume zero sideslip?
    bool assume_zero_sideslip(void) const;

    // vehicle specific initial gyro bias uncertainty
    float InitialGyroBiasUncertainty(void) const;
};

#if CONFIG_HAL_BOARD != HAL_BOARD_PX4 && CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN
#define perf_begin(x)
#define perf_end(x)
#endif

#endif // AP_NavEKF2_core