2015-09-21 02:18:49 -03:00
/*
2017-01-30 02:30:59 -04:00
24 state EKF based on the derivation in https : //github.com/priseborough/
InertialNav / blob / master / derivations / RotationVectorAttitudeParameterisation /
GenerateNavFilterEquations . m
2015-09-21 02:18:49 -03:00
Converted from Matlab to C + + by Paul Riseborough
EKF Tuning parameters refactored by Tom Cauchois
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/>.
*/
2016-02-17 21:25:39 -04:00
# pragma once
2015-09-21 02:18:49 -03:00
2020-10-16 00:38:43 -03:00
# include <AP_Common/Location.h>
2015-09-21 02:18:49 -03:00
# include <AP_Math/AP_Math.h>
# include <AP_Param/AP_Param.h>
# include <GCS_MAVLink/GCS_MAVLink.h>
2015-09-22 21:27:56 -03:00
# include <AP_NavEKF/AP_Nav_Common.h>
class NavEKF2_core ;
2015-09-21 02:18:49 -03:00
2017-08-30 04:28:02 -03:00
class NavEKF2 {
2015-09-22 21:27:56 -03:00
friend class NavEKF2_core ;
2017-08-30 04:28:02 -03:00
public :
2019-12-10 05:33:22 -04:00
NavEKF2 ( ) ;
2017-08-30 04:28:02 -03:00
/* Do not allow copies */
NavEKF2 ( const NavEKF2 & other ) = delete ;
NavEKF2 & operator = ( const NavEKF2 & ) = delete ;
static const struct AP_Param : : GroupInfo var_info [ ] ;
2015-09-22 21:27:56 -03:00
2015-11-06 07:54:22 -04:00
// allow logging to determine the number of active cores
uint8_t activeCores ( void ) const {
return num_cores ;
2015-09-23 04:29:28 -03:00
}
2015-09-22 21:27:56 -03:00
// Initialise the filter
bool InitialiseFilter ( 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 ;
2020-08-11 02:03:41 -03:00
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool pre_arm_check ( char * failure_msg , uint8_t failure_msg_len ) const ;
2015-09-22 21:27:56 -03:00
2015-11-05 21:13:43 -04:00
// returns the index of the primary core
2015-11-05 23:04:22 -04:00
// return -1 if no primary core selected
int8_t getPrimaryCoreIndex ( void ) const ;
2015-11-05 21:13:43 -04:00
2016-09-03 03:50:26 -03:00
// returns the index of the IMU of the primary core
// return -1 if no primary core selected
int8_t getPrimaryCoreIMUIndex ( void ) const ;
2016-07-11 20:54:50 -03:00
// Write the last calculated NE position relative to the reference point (m) for the specified instance.
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2016-07-10 05:43:28 -03:00
// If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control
2018-02-22 06:40:14 -04:00
bool getPosNE ( int8_t instance , Vector2f & posNE ) const ;
2016-07-10 05:43:28 -03:00
2016-07-11 20:54:50 -03:00
// Write the last calculated D position relative to the reference point (m) for the specified instance.
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2016-07-10 05:43:28 -03:00
// If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control
2018-02-22 06:40:14 -04:00
bool getPosD ( int8_t instance , float & posD ) const ;
2016-07-10 05:43:28 -03:00
2015-11-07 08:00:29 -04:00
// return NED velocity in m/s for the specified instance
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2018-02-22 06:40:14 -04:00
void getVelNED ( int8_t instance , Vector3f & vel ) const ;
2015-09-22 21:27:56 -03:00
2020-11-20 17:39:55 -04:00
// return estimate of true airspeed vector in body frame in m/s for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
// returns false if estimate is unavailable
bool getAirSpdVec ( int8_t instance , Vector3f & vel ) const ;
2019-02-21 13:33:38 -04:00
// Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
2015-10-12 03:29:13 -03:00
// 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
// but will always be kinematically consistent with the z component of the EKF position state
2018-02-22 06:40:14 -04:00
float getPosDownDerivative ( int8_t instance ) const ;
2015-10-12 03:29:13 -03:00
2015-11-07 08:00:29 -04:00
// return body axis gyro bias estimates in rad/sec for the specified instance
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2018-02-22 06:40:14 -04:00
void getGyroBias ( int8_t instance , Vector3f & gyroBias ) const ;
2015-09-22 21:27:56 -03:00
2015-11-07 08:00:29 -04:00
// return body axis gyro scale factor error as a percentage for the specified instance
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2018-02-22 06:40:14 -04:00
void getGyroScaleErrorPercentage ( int8_t instance , Vector3f & gyroScale ) const ;
2015-09-22 21:27:56 -03:00
// 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 ) ;
// 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 ;
2015-11-07 08:00:29 -04:00
// return the Z-accel bias estimate in m/s^2 for the specified instance
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2018-02-22 06:40:14 -04:00
void getAccelZBias ( int8_t instance , float & zbias ) const ;
2015-09-22 21:27:56 -03:00
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2018-02-22 06:40:14 -04:00
void getWind ( int8_t instance , Vector3f & wind ) const ;
2015-09-22 21:27:56 -03:00
2015-11-07 08:00:29 -04:00
// return earth magnetic field estimates in measurement units / 1000 for the specified instance
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2018-02-22 06:40:14 -04:00
void getMagNED ( int8_t instance , Vector3f & magNED ) const ;
2015-09-22 21:27:56 -03:00
2015-11-07 08:00:29 -04:00
// return body magnetic field estimates in measurement units / 1000 for the specified instance
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2018-02-22 06:40:14 -04:00
void getMagXYZ ( int8_t instance , Vector3f & magXYZ ) const ;
2015-09-22 21:27:56 -03:00
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
2016-03-29 17:06:42 -03:00
bool getMagOffsets ( uint8_t mag_idx , Vector3f & magOffsets ) const ;
2015-09-22 21:27:56 -03:00
// 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 ;
2017-05-29 22:37:37 -03:00
// Return the latitude and longitude and height used to set the NED origin for the specified instance
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2015-09-22 21:27:56 -03:00
// All NED positions calculated by the filter are relative to this location
// Returns false if the origin has not been set
2017-05-29 22:37:37 -03:00
bool getOriginLLH ( int8_t instance , struct Location & loc ) const ;
2015-09-22 21:27:56 -03:00
// set the latitude and longitude and height used to set the NED origin
2017-05-01 07:14:33 -03:00
// All NED positions calculated by the filter will be relative to this location
2015-09-22 21:27:56 -03:00
// 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
2017-04-19 03:29:17 -03:00
bool setOriginLLH ( const Location & loc ) ;
2015-09-22 21:27:56 -03:00
// return estimated height above ground level
// return false if ground height is not being estimated.
bool getHAGL ( float & HAGL ) const ;
2015-11-07 08:00:29 -04:00
// return the Euler roll, pitch and yaw angle in radians for the specified instance
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2018-01-05 21:26:00 -04:00
void getEulerAngles ( int8_t instance , Vector3f & eulers ) const ;
2015-09-22 21:27:56 -03:00
// return the transformation matrix from XYZ (body) to NED axes
void getRotationBodyToNED ( Matrix3f & mat ) const ;
2019-03-20 14:45:18 -03:00
// return the transformation matrix from XYZ (body) to NED axes
void getQuaternionBodyToNED ( int8_t instance , Quaternion & quat ) const ;
// return the quaternions defining the rotation from NED to autopilot axes
2017-04-15 07:36:34 -03:00
void getQuaternion ( int8_t instance , Quaternion & quat ) const ;
2015-09-22 21:27:56 -03:00
2015-11-07 08:00:29 -04:00
// return the innovations for the specified instance
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2021-07-19 08:27:37 -03:00
bool getInnovations ( int8_t index , Vector3f & velInnov , Vector3f & posInnov , Vector3f & magInnov , float & tasInnov , float & yawInnov ) const ;
2015-09-22 21:27:56 -03:00
2015-11-07 08:00:29 -04:00
// return the innovation consistency test ratios for the specified instance
2019-02-21 13:33:38 -04:00
// An out of range instance (eg -1) returns data for the primary instance
2021-07-19 08:27:37 -03:00
bool getVariances ( int8_t instance , float & velVar , float & posVar , float & hgtVar , Vector3f & magVar , float & tasVar , Vector2f & offset ) const ;
2015-09-22 21:27:56 -03:00
// 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.
2016-10-11 18:23:39 -03:00
// posOffset is the XYZ flow sensor position in the body frame in m
2018-09-02 08:25:07 -03:00
void writeOptFlowMeas ( const uint8_t rawFlowQuality , const Vector2f & rawFlowRates , const Vector2f & rawGyroRates , const uint32_t msecFlowMeas , const Vector3f & posOffset ) ;
2015-09-22 21:27:56 -03:00
2016-07-12 05:56:58 -03:00
// Set to true if the terrain underneath is stable enough to be used as a height reference
// in combination with a range finder. Set to false if the terrain underneath the vehicle
2020-05-25 07:37:44 -03:00
// cannot be used as a height reference. Use to prevent range finder operation otherwise
// enabled by the combination of EK2_RNG_AID_HGT and EK2_RNG_USE_SPD parameters.
2016-07-12 05:56:58 -03:00
void setTerrainHgtStable ( bool val ) ;
2015-09-22 21:27:56 -03:00
/*
2015-11-07 08:00:29 -04:00
return the filter fault status as a bitmasked integer for the specified instance
2019-02-21 13:33:38 -04:00
An out of range instance ( eg - 1 ) returns data for the primary instance
2015-09-22 21:27:56 -03:00
0 = quaternions are NaN
1 = velocities are NaN
2 = badly conditioned X magnetometer fusion
3 = badly conditioned Y magnetometer fusion
2020-06-29 08:10:41 -03:00
4 = badly conditioned Z magnetometer fusion
5 = badly conditioned airspeed fusion
6 = badly conditioned synthetic sideslip fusion
2015-09-22 21:27:56 -03:00
7 = filter is not initialised
*/
2018-02-22 06:40:14 -04:00
void getFilterFaults ( int8_t instance , uint16_t & faults ) const ;
2015-09-22 21:27:56 -03:00
2015-10-07 21:38:26 -03:00
/*
2015-11-07 08:00:29 -04:00
return filter gps quality check status for the specified instance
2019-02-21 13:33:38 -04:00
An out of range instance ( eg - 1 ) returns data for the primary instance
2015-10-07 21:38:26 -03:00
*/
2018-02-22 06:40:14 -04:00
void getFilterGpsStatus ( int8_t instance , nav_gps_status & faults ) const ;
2015-10-07 21:38:26 -03:00
2015-09-22 21:27:56 -03:00
/*
2015-11-07 08:00:29 -04:00
return filter status flags for the specified instance
2019-02-21 13:33:38 -04:00
An out of range instance ( eg - 1 ) returns data for the primary instance
2015-09-22 21:27:56 -03:00
*/
2018-02-22 06:40:14 -04:00
void getFilterStatus ( int8_t instance , nav_filter_status & status ) const ;
2015-09-22 21:27:56 -03:00
// send an EKF_STATUS_REPORT message to GCS
2019-12-10 03:03:55 -04:00
void send_status_report ( mavlink_channel_t chan ) const ;
2015-09-22 21:27:56 -03:00
// 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 ;
2016-09-20 15:23:50 -03:00
// return the amount of yaw angle change (in radians) due to the last yaw angle reset or core selection switch
2015-09-24 03:50:14 -03:00
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
2016-08-25 09:10:22 -03:00
uint32_t getLastYawResetAngle ( float & yawAngDelta ) ;
2015-09-23 04:29:28 -03:00
2015-10-29 02:06:24 -03:00
// return the amount of NE position change due to the last position reset in metres
// returns the time of the last reset or 0 if no reset has ever occurred
2016-10-09 18:18:54 -03:00
uint32_t getLastPosNorthEastReset ( Vector2f & posDelta ) ;
2015-10-29 02:06:24 -03:00
// return the amount of NE velocity change due to the last velocity reset in metres/sec
// returns the time of the last reset or 0 if no reset has ever occurred
2015-10-29 05:36:44 -03:00
uint32_t getLastVelNorthEastReset ( Vector2f & vel ) const ;
2015-10-29 02:06:24 -03:00
2016-11-22 06:29:30 -04:00
// return the amount of vertical position change due to the last reset in metres
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosDownReset ( float & posDelta ) ;
2017-02-03 00:18:26 -04:00
// set and save the _baroAltNoise parameter
void set_baro_alt_noise ( float noise ) { _baroAltNoise . set_and_save ( noise ) ; } ;
2015-09-23 05:55:12 -03:00
// allow the enable flag to be set by Replay
2020-04-18 19:35:19 -03:00
void set_enable ( bool enable ) { _enable . set_enable ( enable ) ; }
2016-05-04 20:24:04 -03:00
2018-03-07 00:42:31 -04:00
/*
* Write position and quaternion data from an external navigation system
*
2020-04-17 22:41:02 -03:00
* pos : position in the RH navigation frame . Frame is assumed to be NED ( m )
2019-02-21 13:33:38 -04:00
* quat : quaternion desribing the rotation from navigation frame to body frame
2018-03-07 00:42:31 -04:00
* posErr : 1 - sigma spherical position error ( m )
* angErr : 1 - sigma spherical angle error ( rad )
* timeStamp_ms : system time the measurement was taken , not the time it was received ( mSec )
2020-04-13 02:02:14 -03:00
* delay_ms : average delay of external nav system measurements relative to inertial measurements
2018-03-07 00:42:31 -04:00
* resetTime_ms : system time of the last position reset request ( mSec )
*
2020-04-13 00:19:46 -03:00
* Sensor offsets are pulled directly from the AP_VisualOdom library
*
2018-03-07 00:42:31 -04:00
*/
2020-04-13 02:02:14 -03:00
void writeExtNavData ( const Vector3f & pos , const Quaternion & quat , float posErr , float angErr , uint32_t timeStamp_ms , uint16_t delay_ms , uint32_t resetTime_ms ) ;
2018-03-07 00:42:31 -04:00
2018-10-03 23:43:13 -03:00
/*
* Write velocity data from an external navigation system
* vel : velocity in NED ( m )
* err : velocity error ( m / s )
* timeStamp_ms : system time the measurement was taken , not the time it was received ( mSec )
* delay_ms : average delay of external nav system measurements relative to inertial measurements
*/
void writeExtNavVelData ( const Vector3f & vel , float err , uint32_t timeStamp_ms , uint16_t delay_ms ) ;
2019-06-07 20:25:23 -03:00
/*
check if switching lanes will reduce the normalised
innovations . This is called when the vehicle code is about to
trigger an EKF failsafe , and it would like to avoid that by
using a different EKF lane
*/
void checkLaneSwitch ( void ) ;
2020-03-25 00:53:13 -03:00
/*
Request a reset of the EKF yaw . This is called when the vehicle code is about to
trigger an EKF failsafe , and it would like to avoid that .
*/
void requestYawReset ( void ) ;
2019-07-03 00:58:36 -03:00
// write EKF information to on-board logs
void Log_Write ( ) ;
2019-07-24 06:48:07 -03:00
// check if external navigation is being used for yaw observation
bool isExtNavUsedForYaw ( void ) const ;
2020-11-24 01:53:51 -04:00
// check if configured to use GPS for horizontal position estimation
bool configuredToUseGPSForPosXY ( void ) const ;
2019-07-24 06:48:07 -03:00
2020-03-25 00:53:13 -03:00
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void writeDefaultAirSpeed ( float airspeed ) ;
2015-09-22 21:27:56 -03:00
private :
2015-11-04 21:00:57 -04:00
uint8_t num_cores ; // number of allocated cores
uint8_t primary ; // current primary core
2015-09-22 21:27:56 -03:00
NavEKF2_core * core = nullptr ;
2019-11-08 11:02:43 -04:00
bool core_malloc_failed ;
2015-09-21 02:18:49 -03:00
2017-04-28 21:48:49 -03:00
uint32_t _frameTimeUsec ; // time per IMU frame
uint8_t _framesPerPrediction ; // expected number of IMU frames per prediction
2017-07-24 22:35:03 -03:00
2015-09-21 02:18:49 -03:00
// EKF Mavlink Tuneable Parameters
2015-09-22 20:57:02 -03:00
AP_Int8 _enable ; // zero to disable EKF2
2015-09-21 02:18:49 -03:00
AP_Float _gpsHorizVelNoise ; // GPS horizontal velocity measurement noise : m/s
AP_Float _gpsVertVelNoise ; // GPS vertical velocity measurement noise : m/s
AP_Float _gpsHorizPosNoise ; // GPS horizontal position measurement noise m
2016-10-25 17:54:29 -03:00
AP_Float _baroAltNoise ; // Baro height measurement noise : m
2015-09-21 02:18:49 -03:00
AP_Float _magNoise ; // magnetometer measurement noise : gauss
AP_Float _easNoise ; // equivalent airspeed measurement noise : m/s
AP_Float _windVelProcessNoise ; // wind velocity state process noise : m/s^2
AP_Float _wndVarHgtRateScale ; // scale factor applied to wind process noise due to height rate
2016-06-13 05:58:28 -03:00
AP_Float _magEarthProcessNoise ; // Earth magnetic field process noise : gauss/sec
AP_Float _magBodyProcessNoise ; // Body magnetic field process noise : gauss/sec
2015-09-21 02:18:49 -03:00
AP_Float _gyrNoise ; // gyro process noise : rad/s
AP_Float _accNoise ; // accelerometer process noise : m/s^2
AP_Float _gyroBiasProcessNoise ; // gyro bias state process noise : rad/s
AP_Float _accelBiasProcessNoise ; // accel bias state process noise : m/s^2
2015-09-23 20:38:54 -03:00
AP_Int16 _hgtDelay_ms ; // effective average delay of Height measurements relative to inertial measurements (msec)
2020-04-17 23:08:50 -03:00
AP_Int8 _fusionModeGPS ; // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity, 3 = do not use GPS
2015-11-16 19:08:46 -04:00
AP_Int16 _gpsVelInnovGate ; // Percentage number of standard deviations applied to GPS velocity innovation consistency check
AP_Int16 _gpsPosInnovGate ; // Percentage number of standard deviations applied to GPS position innovation consistency check
AP_Int16 _hgtInnovGate ; // Percentage number of standard deviations applied to height innovation consistency check
AP_Int16 _magInnovGate ; // Percentage number of standard deviations applied to magnetometer innovation consistency check
AP_Int16 _tasInnovGate ; // Percentage number of standard deviations applied to true airspeed innovation consistency check
2015-09-21 02:18:49 -03:00
AP_Int8 _magCal ; // Sets activation condition for in-flight magnetometer calibration
AP_Int8 _gpsGlitchRadiusMax ; // Maximum allowed discrepancy between inertial and GPS Horizontal position before GPS glitch is declared : m
AP_Float _flowNoise ; // optical flow rate measurement noise
2015-11-16 19:08:46 -04:00
AP_Int16 _flowInnovGate ; // Percentage number of standard deviations applied to optical flow innovation consistency check
2015-09-23 09:19:48 -03:00
AP_Int8 _flowDelay_ms ; // effective average delay of optical flow measurements rel to IMU (msec)
2015-11-16 19:08:46 -04:00
AP_Int16 _rngInnovGate ; // Percentage number of standard deviations applied to range finder innovation consistency check
2015-09-21 02:18:49 -03:00
AP_Float _maxFlowRate ; // Maximum flow rate magnitude that will be accepted by the filter
2020-04-27 07:14:25 -03:00
AP_Int8 _altSource ; // Primary alt source during optical flow navigation. 0 = use Baro, 1 = use range finder, 2 = use GPS, 3 = use Range Beacon
2015-09-21 02:18:49 -03:00
AP_Float _gyroScaleProcessNoise ; // gyro scale factor state process noise : 1/s
2015-09-23 20:38:54 -03:00
AP_Float _rngNoise ; // Range finder noise : m
2015-10-09 14:59:47 -03:00
AP_Int8 _gpsCheck ; // Bitmask controlling which preflight GPS checks are bypassed
2015-11-04 21:00:57 -04:00
AP_Int8 _imuMask ; // Bitmask of IMUs to instantiate EKF2 for
2015-11-12 05:39:15 -04:00
AP_Int16 _gpsCheckScaler ; // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds
2016-01-19 23:07:10 -04:00
AP_Float _noaidHorizNoise ; // horizontal position measurement noise assumed when synthesised zero position measurements are used to constrain attitude drift : m
2016-06-13 07:51:45 -03:00
AP_Float _yawNoise ; // magnetic yaw measurement noise : rad
AP_Int16 _yawInnovGate ; // Percentage number of standard deviations applied to magnetic yaw innovation consistency check
2016-06-16 00:32:43 -03:00
AP_Int8 _tauVelPosOutput ; // Time constant of output complementary filter : csec (centi-seconds)
2016-10-05 08:09:36 -03:00
AP_Int8 _useRngSwHgt ; // Maximum valid range of the range finder as a percentage of the maximum range specified by the sensor driver
2016-09-09 19:52:37 -03:00
AP_Float _terrGradMax ; // Maximum terrain gradient below the vehicle
2016-10-25 17:54:29 -03:00
AP_Float _rngBcnNoise ; // Range beacon measurement noise (m)
AP_Int16 _rngBcnInnovGate ; // Percentage number of standard deviations applied to range beacon innovation consistency check
AP_Int8 _rngBcnDelay_ms ; // effective average delay of range beacon measurements rel to IMU (msec)
2016-10-05 08:09:36 -03:00
AP_Float _useRngSwSpd ; // Maximum horizontal ground speed to use range finder as the primary height source (m/s)
2017-02-14 23:15:01 -04:00
AP_Int8 _magMask ; // Bitmask forcng specific EKF core instances to use simple heading magnetometer fusion.
2017-05-09 03:30:58 -03:00
AP_Int8 _originHgtMode ; // Bitmask controlling post alignment correction and reporting of the EKF origin height.
2019-03-14 15:45:28 -03:00
AP_Int8 _flowUse ; // Controls if the optical flow data is fused into the main navigation estimator and/or the terrain estimator.
2019-06-01 08:03:16 -03:00
AP_Int16 _mag_ef_limit ; // limit on difference between WMM tables and learned earth field.
2019-09-22 02:09:58 -03:00
AP_Float _hrt_filt_freq ; // frequency of output observer height rate complementary filter in Hz
2020-04-22 22:25:09 -03:00
AP_Int8 _gsfRunMask ; // mask controlling which EKF2 instances run a separate EKF-GSF yaw estimator
AP_Int8 _gsfUseMask ; // mask controlling which EKF2 instances will use EKF-GSF yaw estimator data to assit with yaw resets
AP_Int8 _gsfResetMaxCount ; // maximum number of times the EKF2 is allowed to reset it's yaw to the EKF-GSF estimate
2019-03-14 15:45:28 -03:00
// Possible values for _flowUse
# define FLOW_USE_NONE 0
# define FLOW_USE_NAV 1
# define FLOW_USE_TERRAIN 2
2015-09-21 02:18:49 -03:00
// Tuning parameters
2018-02-22 05:06:03 -04:00
const float gpsNEVelVarAccScale = 0.05f ; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
const float gpsDVelVarAccScale = 0.07f ; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
2018-10-03 23:43:13 -03:00
const float extNavVelVarAccScale = 0.05f ; // Scale factor applied to ext nav velocity measurement variance due to manoeuvre acceleration
2018-02-22 05:06:03 -04:00
const float gpsPosVarAccScale = 0.05f ; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
2018-02-22 05:17:14 -04:00
const uint8_t magDelay_ms = 60 ; // Magnetometer measurement delay (msec)
const uint8_t tasDelay_ms = 240 ; // Airspeed measurement delay (msec)
2018-02-22 05:06:03 -04:00
const uint16_t tiltDriftTimeMax_ms = 15000 ; // Maximum number of ms allowed without any form of tilt aiding (GPS, flow, TAS, etc)
const uint16_t posRetryTimeUseVel_ms = 10000 ; // Position aiding retry time with velocity measurements (msec)
const uint16_t posRetryTimeNoVel_ms = 7000 ; // Position aiding retry time without velocity measurements (msec)
const uint16_t hgtRetryTimeMode0_ms = 10000 ; // Height retry time with vertical velocity measurement (msec)
const uint16_t hgtRetryTimeMode12_ms = 5000 ; // Height retry time without vertical velocity measurement (msec)
const uint16_t tasRetryTime_ms = 5000 ; // True airspeed timeout and retry interval (msec)
2018-02-22 05:17:14 -04:00
const uint16_t magFailTimeLimit_ms = 10000 ; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
2018-02-22 05:06:03 -04:00
const float magVarRateScale = 0.005f ; // scale factor applied to magnetometer variance due to angular rate
const float gyroBiasNoiseScaler = 2.0f ; // scale factor applied to gyro bias state process noise when on ground
2018-02-22 05:17:14 -04:00
const uint8_t hgtAvg_ms = 100 ; // average number of msec between height measurements
const uint8_t betaAvg_ms = 100 ; // average number of msec between synthetic sideslip measurements
2018-02-22 05:06:03 -04:00
const float covTimeStepMax = 0.1f ; // maximum time (sec) between covariance prediction updates
const float covDelAngMax = 0.05f ; // maximum delta angle between covariance prediction updates
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.
const float fScaleFactorPnoise = 1e-10 f ; // Process noise added to focal length scale factor state variance at each time step
const uint8_t flowTimeDeltaAvg_ms = 100 ; // average interval between optical flow measurements (msec)
2018-02-22 05:17:14 -04:00
const uint8_t flowIntervalMax_ms = 100 ; // maximum allowable time between flow fusion events
2018-02-22 05:06:03 -04:00
const float gndEffectBaroScaler = 4.0f ; // scaler applied to the barometer observation variance when ground effect mode is active
const uint8_t fusionTimeStep_ms = 10 ; // The minimum time interval between covariance predictions and measurement fusions in msec
2020-11-15 21:15:31 -04:00
const float maxYawEstVelInnov = 2.0f ; // Maximum acceptable length of the velocity innovation returned by the EKF-GSF yaw estimator (m/s)
2016-05-03 19:23:51 -03:00
2019-07-27 04:00:10 -03:00
// origin set by one of the cores
struct Location common_EKF_origin ;
bool common_origin_valid ;
2016-05-05 00:37:37 -03:00
// time at start of current filter update
uint64_t imuSampleTime_us ;
2020-11-05 20:02:55 -04:00
// last time of Log_Write
uint64_t lastLogWrite_us ;
2016-09-20 15:23:50 -03:00
struct {
2020-10-27 23:31:56 -03:00
uint32_t last_function_call ; // last time getLastYawResetAngle was called
2016-09-20 15:23:50 -03:00
bool core_changed ; // true when a core change happened and hasn't been consumed, false otherwise
uint32_t last_primary_change ; // last time a primary has changed
2016-10-09 18:18:54 -03:00
float core_delta ; // the amount of yaw change between cores when a change happened
2016-09-20 15:23:50 -03:00
} yaw_reset_data ;
2016-10-09 18:18:54 -03:00
struct {
uint32_t last_function_call ; // last time getLastPosNorthEastReset was called
bool core_changed ; // true when a core change happened and hasn't been consumed, false otherwise
uint32_t last_primary_change ; // last time a primary has changed
Vector2f core_delta ; // the amount of NE position change between cores when a change happened
} pos_reset_data ;
2016-11-22 06:29:30 -04:00
struct {
uint32_t last_function_call ; // last time getLastPosDownReset was called
bool core_changed ; // true when a core change happened and hasn't been consumed, false otherwise
uint32_t last_primary_change ; // last time a primary has changed
float core_delta ; // the amount of D position change between cores when a change happened
} pos_down_reset_data ;
2016-12-12 17:58:53 -04:00
bool runCoreSelection ; // true when the primary core has stabilised and the core selection logic can be started
2019-06-07 20:25:23 -03:00
// time of last lane switch
uint32_t lastLaneSwitch_ms ;
2019-10-30 12:25:23 -03:00
enum class InitFailures {
UNKNOWN ,
2019-10-29 14:39:19 -03:00
NO_ENABLE ,
NO_IMUS ,
NO_MASK ,
2019-10-30 12:25:23 -03:00
NO_MEM ,
2019-11-01 00:50:22 -03:00
NO_SETUP ,
2019-10-29 14:39:19 -03:00
NUM_INIT_FAILURES
} ;
// initialization failure reasons
2019-10-30 12:25:23 -03:00
const char * initFailureReason [ int ( InitFailures : : NUM_INIT_FAILURES ) ] {
" EKF2: unknown initialization failure " ,
" EKF2: EK2_enable is false " ,
" EKF2: no IMUs available " ,
" EKF2: EK2_IMU_MASK is zero " ,
" EKF2: insufficient memory available " ,
2019-11-01 00:50:22 -03:00
" EKF2: core setup failed "
2019-10-29 14:39:19 -03:00
} ;
2019-10-30 12:25:23 -03:00
InitFailures initFailure ;
2019-10-29 14:39:19 -03:00
2016-10-09 18:18:54 -03:00
// update the yaw reset data to capture changes due to a lane switch
// new_primary - index of the ekf instance that we are about to switch to as the primary
// old_primary - index of the ekf instance that we are currently using as the primary
2016-11-22 06:43:32 -04:00
void updateLaneSwitchYawResetData ( uint8_t new_primary , uint8_t old_primary ) ;
2016-10-09 18:18:54 -03:00
// update the position reset data to capture changes due to a lane switch
// new_primary - index of the ekf instance that we are about to switch to as the primary
// old_primary - index of the ekf instance that we are currently using as the primary
2016-11-22 06:43:32 -04:00
void updateLaneSwitchPosResetData ( uint8_t new_primary , uint8_t old_primary ) ;
2016-11-22 06:29:30 -04:00
2016-11-22 04:25:45 -04:00
// update the position down reset data to capture changes due to a lane switch
2016-11-22 06:29:30 -04:00
// new_primary - index of the ekf instance that we are about to switch to as the primary
// old_primary - index of the ekf instance that we are currently using as the primary
void updateLaneSwitchPosDownResetData ( uint8_t new_primary , uint8_t old_primary ) ;
2019-07-02 21:31:19 -03:00
2020-09-06 08:33:31 -03:00
// return true if a new core has a better score than an existing core, including
// checks for alignment
2020-10-29 22:48:32 -03:00
bool coreBetterScore ( uint8_t new_core , uint8_t current_core ) const ;
2015-09-21 02:18:49 -03:00
} ;