/// -*- 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 . */ #ifndef AP_NavEKF2_core #define AP_NavEKF2_core #include #include "AP_NavEKF2.h" // #define MATH_CHECK_INDEXES 1 #include #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #include #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 Vector2; typedef VectorN Vector3; typedef VectorN Vector4; typedef VectorN Vector5; typedef VectorN Vector6; typedef VectorN Vector7; typedef VectorN Vector8; typedef VectorN Vector9; typedef VectorN Vector10; typedef VectorN Vector11; typedef VectorN Vector13; typedef VectorN Vector14; typedef VectorN Vector15; typedef VectorN Vector22; typedef VectorN Vector23; typedef VectorN Vector24; typedef VectorN Vector25; typedef VectorN Vector31; typedef VectorN Vector28; typedef VectorN,3> Matrix3; typedef VectorN,24> Matrix24; typedef VectorN,50> Matrix34_50; typedef VectorN 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