Line data    Source code

       1             : /*
       2             :   24 state EKF based on the derivation in https://github.com/priseborough/
       3             :   InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/
       4             :   GenerateNavFilterEquations.m
       5             : 
       6             :   Converted from Matlab to C++ by Paul Riseborough
       7             : 
       8             :   EKF Tuning parameters refactored by Tom Cauchois
       9             : 
      10             :   This program is free software: you can redistribute it and/or modify
      11             :   it under the terms of the GNU General Public License as published by
      12             :   the Free Software Foundation, either version 3 of the License, or
      13             :   (at your option) any later version.
      14             : 
      15             :   This program is distributed in the hope that it will be useful,
      16             :   but WITHOUT ANY WARRANTY; without even the implied warranty of
      17             :   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      18             :   GNU General Public License for more details.
      19             : 
      20             :   You should have received a copy of the GNU General Public License
      21             :   along with this program.  If not, see <http://www.gnu.org/licenses/>.
      22             :  */
      23             : #pragma once
      24             : 
      25             : #include <AP_Common/Location.h>
      26             : #include <AP_Math/AP_Math.h>
      27             : #include <AP_Param/AP_Param.h>
      28             : #include <AP_NavEKF/AP_Nav_Common.h>
      29             : 
      30             : class NavEKF2_core;
      31             : class EKFGSF_yaw;
      32             : 
      33             : class NavEKF2 {
      34             :     friend class NavEKF2_core;
      35             : 
      36             : public:
      37             :     NavEKF2();
      38             : 
      39             :     /* Do not allow copies */
      40             :     CLASS_NO_COPY(NavEKF2);
      41             : 
      42             :     static const struct AP_Param::GroupInfo var_info[];
      43             : 
      44             :     // allow logging to determine the number of active cores
      45     4812879 :     uint8_t activeCores(void) const {
      46     4812879 :         return num_cores;
      47             :     }
      48             : 
      49             :     // Initialise the filter
      50             :     bool InitialiseFilter(void);
      51             : 
      52             :     // Update Filter States - this should be called whenever new IMU data is available
      53             :     void UpdateFilter(void);
      54             : 
      55             :     // Check basic filter health metrics and return a consolidated health status
      56             :     bool healthy(void) const;
      57             : 
      58             :     // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
      59             :     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
      60             : 
      61             :     // returns the index of the primary core
      62             :     // return -1 if no primary core selected
      63             :     int8_t getPrimaryCoreIndex(void) const;
      64             : 
      65             :     // returns the index of the IMU of the primary core
      66             :     // return -1 if no primary core selected
      67             :     int8_t getPrimaryCoreIMUIndex(void) const;
      68             :     
      69             :     // Write the last calculated NE position relative to the reference point (m)
      70             :     // If a calculated solution is not available, use the best available data and return false
      71             :     // If false returned, do not use for flight control
      72             :     bool getPosNE(Vector2p &posNE) const;
      73             : 
      74             :     // Write the last calculated D position relative to the reference point (m)
      75             :     // If a calculated solution is not available, use the best available data and return false
      76             :     // If false returned, do not use for flight control
      77             :     bool getPosD(postype_t &posD) const;
      78             : 
      79             :     // return NED velocity in m/s
      80             :     void getVelNED(Vector3f &vel) const;
      81             : 
      82             :     // return estimate of true airspeed vector in body frame in m/s
      83             :     // returns false if estimate is unavailable
      84             :     bool getAirSpdVec(Vector3f &vel) const;
      85             : 
      86             :     // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
      87             :     // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF
      88             :     // but will always be kinematically consistent with the z component of the EKF position state
      89             :     float getPosDownDerivative() const;
      90             : 
      91             :     // return body axis gyro bias estimates in rad/sec
      92             :     void getGyroBias(Vector3f &gyroBias) const;
      93             : 
      94             :     // reset body axis gyro bias estimates
      95             :     void resetGyroBias(void);
      96             : 
      97             :     // Resets the baro so that it reads zero at the current height
      98             :     // Resets the EKF height to zero
      99             :     // Adjusts the EKf origin height so that the EKF height + origin height is the same as before
     100             :     // Returns true if the height datum reset has been performed
     101             :     // If using a range finder for height no reset is performed and it returns false
     102             :     bool resetHeightDatum(void);
     103             : 
     104             :     // return the horizontal speed limit in m/s set by optical flow sensor limits
     105             :     // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
     106             :     void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
     107             : 
     108             :     // return the Z-accel bias estimate in m/s^2 for the specified instance
     109             :     // An out of range instance (eg -1) returns data for the primary instance
     110             :     void getAccelZBias(float &zbias) const;
     111             : 
     112             :     // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
     113             :     // An out of range instance (eg -1) returns data for the primary instance
     114             :     void getWind(Vector3f &wind) const;
     115             : 
     116             :     // return earth magnetic field estimates in measurement units / 1000 for the specified instance
     117             :     // An out of range instance (eg -1) returns data for the primary instance
     118             :     void getMagNED(Vector3f &magNED) const;
     119             : 
     120             :     // return body magnetic field estimates in measurement units / 1000 for the specified instance
     121             :     // An out of range instance (eg -1) returns data for the primary instance
     122             :     void getMagXYZ(Vector3f &magXYZ) const;
     123             : 
     124             :     // Return estimated magnetometer offsets
     125             :     // Return true if magnetometer offsets are valid
     126             :     bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;
     127             : 
     128             :     // Return the last calculated latitude, longitude and height in WGS-84
     129             :     // If a calculated location isn't available, return a raw GPS measurement
     130             :     // The status will return true if a calculation or raw measurement is available
     131             :     // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
     132             :     bool getLLH(Location &loc) const;
     133             : 
     134             :     // Return the latitude and longitude and height used to set the NED origin for the specified instance
     135             :     // An out of range instance (eg -1) returns data for the primary instance
     136             :     // All NED positions calculated by the filter are relative to this location
     137             :     // Returns false if the origin has not been set
     138             :     bool getOriginLLH(Location &loc) const;
     139             : 
     140             :     // set the latitude and longitude and height used to set the NED origin
     141             :     // All NED positions calculated by the filter will be relative to this location
     142             :     // The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
     143             :     // Returns false if the filter has rejected the attempt to set the origin
     144             :     bool setOriginLLH(const Location &loc);
     145             : 
     146             :     // return estimated height above ground level
     147             :     // return false if ground height is not being estimated.
     148             :     bool getHAGL(float &HAGL) const;
     149             : 
     150             :     // return the Euler roll, pitch and yaw angle in radians for the specified instance
     151             :     // An out of range instance (eg -1) returns data for the primary instance
     152             :     void getEulerAngles(Vector3f &eulers) const;
     153             : 
     154             :     // return the transformation matrix from XYZ (body) to NED axes
     155             :     void getRotationBodyToNED(Matrix3f &mat) const;
     156             : 
     157             :     // return the transformation matrix from XYZ (body) to NED axes
     158             :     void getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const;
     159             : 
     160             :     // return the quaternions defining the rotation from NED to autopilot axes
     161             :     void getQuaternion(Quaternion &quat) const;
     162             : 
     163             :     // return the innovations for the specified instance
     164             :     // An out of range instance (eg -1) returns data for the primary instance
     165             :     bool getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
     166             : 
     167             :     // return the innovation consistency test ratios for the specified instance
     168             :     // An out of range instance (eg -1) returns data for the primary instance
     169             :     bool getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
     170             : 
     171             :     // should we use the compass? This is public so it can be used for
     172             :     // reporting via ahrs.use_compass()
     173             :     bool use_compass(void) const;
     174             : 
     175             :     // are we using a gps?
     176             :     bool using_gps(void) const;
     177             : 
     178             :     // write the raw optical flow measurements
     179             :     // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
     180             :     // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
     181             :     // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro
     182             :     // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
     183             :     // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
     184             :     // posOffset is the XYZ flow sensor position in the body frame in m
     185             :     // heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
     186             :     void  writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
     187             : 
     188             :     // Set to true if the terrain underneath is stable enough to be used as a height reference
     189             :     // in combination with a range finder. Set to false if the terrain underneath the vehicle
     190             :     // cannot be used as a height reference. Use to prevent range finder operation otherwise
     191             :     // enabled by the combination of EK2_RNG_AID_HGT and EK2_RNG_USE_SPD parameters.
     192             :     void setTerrainHgtStable(bool val);
     193             : 
     194             :     /*
     195             :     return the filter fault status as a bitmasked integer for the specified instance
     196             :     An out of range instance (eg -1) returns data for the primary instance
     197             :      0 = quaternions are NaN
     198             :      1 = velocities are NaN
     199             :      2 = badly conditioned X magnetometer fusion
     200             :      3 = badly conditioned Y magnetometer fusion
     201             :      4 = badly conditioned Z magnetometer fusion
     202             :      5 = badly conditioned airspeed fusion
     203             :      6 = badly conditioned synthetic sideslip fusion
     204             :      7 = filter is not initialised
     205             :     */
     206             :     void  getFilterFaults(uint16_t &faults) const;
     207             : 
     208             :     /*
     209             :     return filter gps quality check status for the specified instance
     210             :     An out of range instance (eg -1) returns data for the primary instance
     211             :     */
     212             :     void  getFilterGpsStatus(nav_gps_status &faults) const;
     213             : 
     214             :     /*
     215             :     return filter status flags for the specified instance
     216             :     An out of range instance (eg -1) returns data for the primary instance
     217             :     */
     218             :     void  getFilterStatus(nav_filter_status &status) const;
     219             : 
     220             :     // return a terrain altitude variance
     221             :     bool getTerrainAltVariance(float &terrain_alt_variance) const;
     222             : 
     223             :     // provides the height limit to be observed by the control loops
     224             :     // returns false if no height limiting is required
     225             :     // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
     226             :     bool getHeightControlLimit(float &height) const;
     227             : 
     228             :     // return a count of yaw reset events; incremented when the
     229             :     // primary core changes and when the primary core resets its yaw
     230             :     uint16_t getYawResetCount(void);
     231             : 
     232             :     // return a count of NE position reset events; incremented when the
     233             :     // primary core changes and when the primary core resets its NE position
     234             :     uint16_t getPosNorthEastResetCount(void);
     235             : 
     236             :     // return a count of D position reset events; incremented when the
     237             :     // primary core changes and when the primary core resets its D position
     238             :     uint16_t getPosDownResetCount(void);
     239             : 
     240             :     // set and save the _baroAltNoise parameter
     241          82 :     void set_baro_alt_noise(float noise) { _baroAltNoise.set_and_save(noise); };
     242             : 
     243             :     // allow the enable flag to be set by Replay
     244             :     void set_enable(bool enable) { _enable.set_enable(enable); }
     245             : 
     246             :     // get the enable parameter
     247          13 :     bool get_enable(void) const { return bool(_enable.get()); }
     248             :     
     249             :     /*
     250             :      * Write position and quaternion data from an external navigation system
     251             :      *
     252             :      * pos        : position in the RH navigation frame. Frame is assumed to be NED (m)
     253             :      * quat       : quaternion desribing the rotation from navigation frame to body frame
     254             :      * posErr     : 1-sigma spherical position error (m)
     255             :      * angErr     : 1-sigma spherical angle error (rad)
     256             :      * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
     257             :      * delay_ms   : average delay of external nav system measurements relative to inertial measurements
     258             :      * resetTime_ms : system time of the last position reset request (mSec)
     259             :      *
     260             :      * Sensor offsets are pulled directly from the AP_VisualOdom library
     261             :      *
     262             :     */
     263             :     void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
     264             : 
     265             :     /*
     266             :      * Write velocity data from an external navigation system
     267             :      * vel : velocity in NED (m)
     268             :      * err : velocity error (m/s)
     269             :      * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
     270             :      * delay_ms   : average delay of external nav system measurements relative to inertial measurements
     271             :      */
     272             :     void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
     273             : 
     274             :     /*
     275             :       check if switching lanes will reduce the normalised
     276             :       innovations. This is called when the vehicle code is about to
     277             :       trigger an EKF failsafe, and it would like to avoid that by
     278             :       using a different EKF lane
     279             :      */
     280             :     void checkLaneSwitch(void);
     281             : 
     282             :     /*
     283             :       Request a reset of the EKF yaw. This is called when the vehicle code is about to
     284             :       trigger an EKF failsafe, and it would like to avoid that.
     285             :      */
     286             :     void requestYawReset(void);
     287             : 
     288             :     // write EKF information to on-board logs
     289             :     void Log_Write();
     290             : 
     291             :     // check if external navigation is being used for yaw observation
     292             :     bool isExtNavUsedForYaw(void) const;
     293             : 
     294             :     // check if configured to use GPS for horizontal position estimation
     295             :     bool configuredToUseGPSForPosXY(void) const;
     296             : 
     297             :     // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
     298             :     void writeDefaultAirSpeed(float airspeed);
     299             : 
     300             :     // get a yaw estimator instance
     301             :     const EKFGSF_yaw *get_yawEstimator(void) const;
     302             :     
     303             : private:
     304             :     uint8_t num_cores; // number of allocated cores
     305             :     uint8_t primary;   // current primary core
     306             :     NavEKF2_core *core = nullptr;
     307             :     bool core_malloc_failed;
     308             : 
     309             :     uint32_t _frameTimeUsec;        // time per IMU frame
     310             :     uint8_t  _framesPerPrediction;  // expected number of IMU frames per prediction
     311             : 
     312             :     // EKF Mavlink Tuneable Parameters
     313             :     AP_Int8  _enable;               // zero to disable EKF2
     314             :     AP_Float _gpsHorizVelNoise;     // GPS horizontal velocity measurement noise : m/s
     315             :     AP_Float _gpsVertVelNoise;      // GPS vertical velocity measurement noise : m/s
     316             :     AP_Float _gpsHorizPosNoise;     // GPS horizontal position measurement noise m
     317             :     AP_Float _baroAltNoise;         // Baro height measurement noise : m
     318             :     AP_Float _magNoise;             // magnetometer measurement noise : gauss
     319             :     AP_Float _easNoise;             // equivalent airspeed measurement noise : m/s
     320             :     AP_Float _windVelProcessNoise;  // wind velocity state process noise : m/s^2
     321             :     AP_Float _wndVarHgtRateScale;   // scale factor applied to wind process noise due to height rate
     322             :     AP_Float _magEarthProcessNoise; // Earth magnetic field process noise : gauss/sec
     323             :     AP_Float _magBodyProcessNoise;  // Body magnetic field process noise : gauss/sec
     324             :     AP_Float _gyrNoise;             // gyro process noise : rad/s
     325             :     AP_Float _accNoise;             // accelerometer process noise : m/s^2
     326             :     AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s
     327             :     AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2
     328             :     AP_Int16 _hgtDelay_ms;          // effective average delay of Height measurements relative to inertial measurements (msec)
     329             :     AP_Int8  _fusionModeGPS;        // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity, 3 = do not use GPS
     330             :     AP_Int16  _gpsVelInnovGate;     // Percentage number of standard deviations applied to GPS velocity innovation consistency check
     331             :     AP_Int16  _gpsPosInnovGate;     // Percentage number of standard deviations applied to GPS position innovation consistency check
     332             :     AP_Int16  _hgtInnovGate;        // Percentage number of standard deviations applied to height innovation consistency check
     333             :     AP_Int16  _magInnovGate;        // Percentage number of standard deviations applied to magnetometer innovation consistency check
     334             :     AP_Int16  _tasInnovGate;        // Percentage number of standard deviations applied to true airspeed innovation consistency check
     335             :     AP_Int8  _magCal;               // Sets activation condition for in-flight magnetometer calibration
     336             :     AP_Int8 _gpsGlitchRadiusMax;    // Maximum allowed discrepancy between inertial and GPS Horizontal position before GPS glitch is declared : m
     337             :     AP_Float _flowNoise;            // optical flow rate measurement noise
     338             :     AP_Int16  _flowInnovGate;       // Percentage number of standard deviations applied to optical flow innovation consistency check
     339             :     AP_Int8  _flowDelay_ms;         // effective average delay of optical flow measurements rel to IMU (msec)
     340             :     AP_Int16  _rngInnovGate;        // Percentage number of standard deviations applied to range finder innovation consistency check
     341             :     AP_Float _maxFlowRate;          // Maximum flow rate magnitude that will be accepted by the filter
     342             :     AP_Int8 _altSource;             // Primary alt source during optical flow navigation. 0 = use Baro, 1 = use range finder, 2 = use GPS, 3 = use Range Beacon
     343             :     AP_Float _gyroScaleProcessNoise;// gyro scale factor state process noise : 1/s
     344             :     AP_Float _rngNoise;             // Range finder noise : m
     345             :     AP_Int8 _gpsCheck;              // Bitmask controlling which preflight GPS checks are bypassed
     346             :     AP_Int8 _imuMask;               // Bitmask of IMUs to instantiate EKF2 for
     347             :     AP_Int16 _gpsCheckScaler;       // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds
     348             :     AP_Float _noaidHorizNoise;      // horizontal position measurement noise assumed when synthesised zero position measurements are used to constrain attitude drift : m
     349             :     AP_Float _yawNoise;             // magnetic yaw measurement noise : rad
     350             :     AP_Int16 _yawInnovGate;         // Percentage number of standard deviations applied to magnetic yaw innovation consistency check
     351             :     AP_Int8 _tauVelPosOutput;       // Time constant of output complementary filter : csec (centi-seconds)
     352             :     AP_Int8 _useRngSwHgt;           // Maximum valid range of the range finder as a percentage of the maximum range specified by the sensor driver
     353             :     AP_Float _terrGradMax;          // Maximum terrain gradient below the vehicle
     354             :     AP_Float _rngBcnNoise;          // Range beacon measurement noise (m)
     355             :     AP_Int16 _rngBcnInnovGate;      // Percentage number of standard deviations applied to range beacon innovation consistency check
     356             :     AP_Int8  _rngBcnDelay_ms;       // effective average delay of range beacon measurements rel to IMU (msec)
     357             :     AP_Float _useRngSwSpd;          // Maximum horizontal ground speed to use range finder as the primary height source (m/s)
     358             :     AP_Int8 _magMask;               // Bitmask forcng specific EKF core instances to use simple heading magnetometer fusion.
     359             :     AP_Int8 _originHgtMode;         // Bitmask controlling post alignment correction and reporting of the EKF origin height.
     360             :     AP_Int8 _flowUse;               // Controls if the optical flow data is fused into the main navigation estimator and/or the terrain estimator.
     361             :     AP_Int16 _mag_ef_limit;         // limit on difference between WMM tables and learned earth field.
     362             :     AP_Float _hrt_filt_freq;        // frequency of output observer height rate complementary filter in Hz
     363             :     AP_Int8 _gsfRunMask;            // mask controlling which EKF2 instances run a separate EKF-GSF yaw estimator
     364             :     AP_Int8 _gsfUseMask;            // mask controlling which EKF2 instances will use EKF-GSF yaw estimator data to assit with yaw resets
     365             :     AP_Int8 _gsfResetMaxCount;      // maximum number of times the EKF2 is allowed to reset it's yaw to the EKF-GSF estimate
     366             :     AP_Int32 _options;              // optional behaviour bitmask
     367             : 
     368             :     // enum for processing options
     369             :     enum class Option {
     370             :         DisableExternalNav     = (1U<<0),
     371             :     };
     372             : 
     373             :     // return true if an option is set
     374       14347 :     bool option_is_set(Option option) const {
     375       14347 :         return (uint32_t(option) & uint32_t(_options)) != 0;
     376             :     }
     377             :     
     378             : // Possible values for _flowUse
     379             : #define FLOW_USE_NONE    0
     380             : #define FLOW_USE_NAV     1
     381             : #define FLOW_USE_TERRAIN 2
     382             : 
     383             :     // Tuning parameters
     384             :     const float gpsNEVelVarAccScale = 0.05f;       // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
     385             :     const float gpsDVelVarAccScale = 0.07f;        // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
     386             :     const float extNavVelVarAccScale = 0.05f;      // Scale factor applied to ext nav velocity measurement variance due to manoeuvre acceleration
     387             :     const float gpsPosVarAccScale = 0.05f;         // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
     388             :     const uint8_t magDelay_ms = 60;               // Magnetometer measurement delay (msec)
     389             :     const uint8_t tasDelay_ms = 240;              // Airspeed measurement delay (msec)
     390             :     const uint16_t tiltDriftTimeMax_ms = 15000;    // Maximum number of ms allowed without any form of tilt aiding (GPS, flow, TAS, etc)
     391             :     const uint16_t posRetryTimeUseVel_ms = 10000;  // Position aiding retry time with velocity measurements (msec)
     392             :     const uint16_t posRetryTimeNoVel_ms = 7000;    // Position aiding retry time without velocity measurements (msec)
     393             :     const uint16_t hgtRetryTimeMode0_ms = 10000;   // Height retry time with vertical velocity measurement (msec)
     394             :     const uint16_t hgtRetryTimeMode12_ms = 5000;   // Height retry time without vertical velocity measurement (msec)
     395             :     const uint16_t tasRetryTime_ms = 5000;         // True airspeed timeout and retry interval (msec)
     396             :     const uint16_t magFailTimeLimit_ms = 10000;    // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
     397             :     const float magVarRateScale = 0.005f;          // scale factor applied to magnetometer variance due to angular rate
     398             :     const float gyroBiasNoiseScaler = 2.0f;        // scale factor applied to gyro bias state process noise when on ground
     399             :     const uint8_t hgtAvg_ms = 100;                // average number of msec between height measurements
     400             :     const uint8_t betaAvg_ms = 100;               // average number of msec between synthetic sideslip measurements
     401             :     const float covTimeStepMax = 0.1f;             // maximum time (sec) between covariance prediction updates
     402             :     const float covDelAngMax = 0.05f;              // maximum delta angle between covariance prediction updates
     403             :     const float DCM33FlowMin = 0.71f;              // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
     404             :     const float fScaleFactorPnoise = 1e-10f;       // Process noise added to focal length scale factor state variance at each time step
     405             :     const uint8_t flowTimeDeltaAvg_ms = 100;       // average interval between optical flow measurements (msec)
     406             :     const uint8_t flowIntervalMax_ms = 100;       // maximum allowable time between flow fusion events
     407             :     const float gndEffectBaroScaler = 4.0f;        // scaler applied to the barometer observation variance when ground effect mode is active
     408             :     const uint8_t fusionTimeStep_ms = 10;          // The minimum time interval between covariance predictions and measurement fusions in msec
     409             :     const float maxYawEstVelInnov = 2.0f;          // Maximum acceptable length of the velocity innovation returned by the EKF-GSF yaw estimator (m/s)
     410             : 
     411             :     // origin set by one of the cores
     412             :     Location common_EKF_origin;
     413             :     bool common_origin_valid;
     414             : 
     415             :     // time at start of current filter update
     416             :     uint64_t imuSampleTime_us;
     417             : 
     418             :     // last time of Log_Write
     419             :     uint64_t lastLogWrite_us;
     420             :     
     421             :     struct {
     422             :         uint16_t count;               // count of yaw reset events passed to consumers
     423             :         uint16_t last_core_count;     // primary core's yaw reset count when count last changed
     424             :     } yaw_reset_data;
     425             : 
     426             :     struct {
     427             :         uint16_t count;               // count of NE position reset events passed to consumers
     428             :         uint16_t last_core_count;     // primary core's NE position reset count when count last changed
     429             :     } pos_reset_data;
     430             : 
     431             :     struct {
     432             :         uint16_t count;               // count of D position reset events passed to consumers
     433             :         uint16_t last_core_count;     // primary core's D position reset count when count last changed
     434             :     } pos_down_reset_data;
     435             : 
     436             :     bool runCoreSelection; // true when the primary core has stabilised and the core selection logic can be started
     437             : 
     438             :     // time of last lane switch
     439             :     uint32_t lastLaneSwitch_ms;
     440             : 
     441             :     enum class InitFailures {
     442             :         UNKNOWN,
     443             :         NO_ENABLE, 
     444             :         NO_IMUS, 
     445             :         NO_MASK, 
     446             :         NO_MEM, 
     447             :         NO_SETUP,
     448             :         NUM_INIT_FAILURES
     449             :     };
     450             :     InitFailures initFailure;
     451             : 
     452             :     // update the yaw reset data to capture changes due to a lane switch
     453             :     // new_primary - index of the ekf instance that we are about to switch to as the primary
     454             :     // old_primary - index of the ekf instance that we are currently using as the primary
     455             :     void updateLaneSwitchYawResetData(uint8_t new_primary);
     456             : 
     457             :     // update the position reset data to capture changes due to a lane switch
     458             :     // new_primary - index of the ekf instance that we are about to switch to as the primary
     459             :     // old_primary - index of the ekf instance that we are currently using as the primary
     460             :     void updateLaneSwitchPosResetData(uint8_t new_primary);
     461             : 
     462             :     // update the position down reset data to capture changes due to a lane switch
     463             :     // new_primary - index of the ekf instance that we are about to switch to as the primary
     464             :     // old_primary - index of the ekf instance that we are currently using as the primary
     465             :     void updateLaneSwitchPosDownResetData(uint8_t new_primary);
     466             : 
     467             :     // return true if a new core has a better score than an existing core, including
     468             :     // checks for alignment
     469             :     bool coreBetterScore(uint8_t new_core, uint8_t current_core) const;
     470             : };