aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Update parameters

Browse Source
This commit is contained in:
Paul Riseborough 2015-09-24 09:38:54 +10:00 committed by Andrew Tridgell
parent 9c5e48e7e9
commit 4acd6c129a
3 changed files with 224 additions and 208 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

410
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -89,7 +89,16 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Description: This enables EKF2. Enabling EKF2 only makes the maths run, it does not mean it will be used for flight control. To use it for flight control set AHRS_EKF_USE=3
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO("ENABLE", 0, NavEKF2, _enable, 0),
AP_GROUPINFO("ENABLE", 0, NavEKF2, _enable, 0),
// GPS measurement parameters
// @Param: GPS_TYPE
// @DisplayName: GPS mode control
// @Description: This parameter controls use of GPS measurements : 0 = use 3D velocity & 2D position, 1 = use 2D velocity and 2D position, 2 = use 2D position, 3 = use no GPS (optical flow will be used if available)
// @Values: 0:GPS 3D Vel and 2D Pos, 1:GPS 2D vel and 2D pos, 2:GPS 2D pos, 3:No GPS use optical flow
// @User: Advanced
AP_GROUPINFO("GPS_TYPE", 1, NavEKF2, _fusionModeGPS, 0),
// @Param: VELNE_NOISE
// @DisplayName: GPS horizontal velocity measurement noise scaler
@ -97,7 +106,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Range: 0.05 5.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("VELNE_NOISE", 1, NavEKF2, _gpsHorizVelNoise, VELNE_NOISE_DEFAULT),
AP_GROUPINFO("VELNE_NOISE", 2, NavEKF2, _gpsHorizVelNoise, VELNE_NOISE_DEFAULT),
// @Param: VELD_NOISE
// @DisplayName: GPS vertical velocity measurement noise scaler
@ -105,7 +114,15 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Range: 0.05 5.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("VELD_NOISE", 2, NavEKF2, _gpsVertVelNoise, VELD_NOISE_DEFAULT),
AP_GROUPINFO("VELD_NOISE", 3, NavEKF2, _gpsVertVelNoise, VELD_NOISE_DEFAULT),
// @Param: VEL_GATE
// @DisplayName: GPS velocity measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements willbe rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 1 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("VEL_GATE", 4, NavEKF2, _gpsVelInnovGate, VEL_GATE_DEFAULT),
// @Param: POSNE_NOISE
// @DisplayName: GPS horizontal position measurement noise (m)
@ -114,123 +131,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Increment: 0.1
// @User: Advanced
// @Units: meters
AP_GROUPINFO("POSNE_NOISE", 3, NavEKF2, _gpsHorizPosNoise, POSNE_NOISE_DEFAULT),
// @Param: ALT_NOISE
// @DisplayName: Altitude measurement noise (m)
// @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting on this measurement.
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: meters
AP_GROUPINFO("ALT_NOISE", 4, NavEKF2, _baroAltNoise, ALT_NOISE_DEFAULT),
// @Param: MAG_NOISE
// @DisplayName: Magnetometer measurement noise (Gauss)
// @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.01 0.5
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("MAG_NOISE", 5, NavEKF2, _magNoise, MAG_NOISE_DEFAULT),
// @Param: EAS_NOISE
// @DisplayName: Equivalent airspeed measurement noise (m/s)
// @Description: This is the RMS value of noise in equivalent airspeed measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("EAS_NOISE", 6, NavEKF2, _easNoise, 1.4f),
// @Param: WIND_PNOISE
// @DisplayName: Wind velocity process noise (m/s^2)
// @Description: This noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier.
// @Range: 0.01 1.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("WIND_PNOISE", 7, NavEKF2, _windVelProcessNoise, 0.1f),
// @Param: WIND_PSCALE
// @DisplayName: Height rate to wind procss noise scaler
// @Description: Increasing this parameter increases how rapidly the wind states adapt when changing altitude, but does make wind speed estimation noiser.
// @Range: 0.0 1.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("WIND_PSCALE", 8, NavEKF2, _wndVarHgtRateScale, 0.5f),
// @Param: GYRO_PNOISE
// @DisplayName: Rate gyro noise (rad/s)
// @Description: This noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
// @Range: 0.001 0.05
// @Increment: 0.001
// @User: Advanced
// @Units: rad/s
AP_GROUPINFO("GYRO_PNOISE", 9, NavEKF2, _gyrNoise, GYRO_PNOISE_DEFAULT),
// @Param: ACC_PNOISE
// @DisplayName: Accelerometer noise (m/s^2)
// @Description: This noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
// @Range: 0.05 1.0
// @Increment: 0.01
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("ACC_PNOISE", 10, NavEKF2, _accNoise, ACC_PNOISE_DEFAULT),
// @Param: GBIAS_PNOISE
// @DisplayName: Rate gyro bias process noise (rad/s)
// @Description: This noise controls the growth of gyro bias state error estimates. Increasing it makes rate gyro bias estimation faster and noisier.
// @Range: 0.0000001 0.00001
// @User: Advanced
// @Units: rad/s
AP_GROUPINFO("GBIAS_PNOISE", 11, NavEKF2, _gyroBiasProcessNoise, GBIAS_PNOISE_DEFAULT),
// @Param: ABIAS_PNOISE
// @DisplayName: Accelerometer bias process noise (m/s^2)
// @Description: This noise controls the growth of the vertical acelerometer bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier.
// @Range: 0.00001 0.001
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("ABIAS_PNOISE", 12, NavEKF2, _accelBiasProcessNoise, ABIAS_PNOISE_DEFAULT),
// @Param: MAG_PNOISE
// @DisplayName: Magnetic field process noise (gauss/s)
// @Description: This noise controls the growth of magnetic field state error estimates. Increasing it makes magnetic field bias estimation faster and noisier.
// @Range: 0.0001 0.01
// @User: Advanced
// @Units: gauss/s
AP_GROUPINFO("MAG_PNOISE", 13, NavEKF2, _magProcessNoise, MAG_PNOISE_DEFAULT),
// @Param: GSCL_PNOISE
// @DisplayName: Rate gyro scale factor process noise (1/s)
// @Description: This noise controls the rate of gyro scale factor learning. Increasing it makes rate gyro scale factor estimation faster and noisier.
// @Range: 0.0000001 0.00001
// @User: Advanced
// @Units: 1/s
AP_GROUPINFO("GSCL_PNOISE", 14, NavEKF2, _gyroScaleProcessNoise, 1e-6f),
// @Param: GPS_DELAY
// @DisplayName: GPS measurement delay (msec)
// @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements.
// @Range: 0 500
// @Increment: 10
// @User: Advanced
// @Units: milliseconds
AP_GROUPINFO("VEL_DELAY", 15, NavEKF2, _gpsDelay_ms, 220),
// @Param: GPS_TYPE
// @DisplayName: GPS mode control
// @Description: This parameter controls use of GPS measurements : 0 = use 3D velocity & 2D position, 1 = use 2D velocity and 2D position, 2 = use 2D position, 3 = use no GPS (optical flow will be used if available)
// @Values: 0:GPS 3D Vel and 2D Pos, 1:GPS 2D vel and 2D pos, 2:GPS 2D pos, 3:No GPS use optical flow
// @User: Advanced
AP_GROUPINFO("GPS_TYPE", 16, NavEKF2, _fusionModeGPS, 0),
// @Param: VEL_GATE
// @DisplayName: GPS velocity measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements willbe rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 1 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("VEL_GATE", 17, NavEKF2, _gpsVelInnovGate, VEL_GATE_DEFAULT),
AP_GROUPINFO("POSNE_NOISE", 5, NavEKF2, _gpsHorizPosNoise, POSNE_NOISE_DEFAULT),
// @Param: POS_GATE
// @DisplayName: GPS position measurement gate size
@ -238,38 +139,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Range: 1 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("POS_GATE", 18, NavEKF2, _gpsPosInnovGate, POS_GATE_DEFAULT),
// @Param: HGT_GATE
// @DisplayName: Height measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 1 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("HGT_GATE", 19, NavEKF2, _hgtInnovGate, HGT_GATE_DEFAULT),
// @Param: MAG_GATE
// @DisplayName: Magnetometer measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the magnetometer measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 1 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("MAG_GATE", 20, NavEKF2, _magInnovGate, MAG_GATE_DEFAULT),
// @Param: EAS_GATE
// @DisplayName: Airspeed measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the airspeed measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 1 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("EAS_GATE", 21, NavEKF2, _tasInnovGate, 10),
// @Param: MAG_CAL
// @DisplayName: Magnetometer calibration mode
// @Description: EKF_MAG_CAL = 0 enables calibration based on flying speed and altitude and is the default setting for Plane users. EKF_MAG_CAL = 1 enables calibration based on manoeuvre level and is the default setting for Copter and Rover users. EKF_MAG_CAL = 2 prevents magnetometer calibration regardless of flight condition and is recommended if in-flight magnetometer calibration is unreliable.
// @Values: 0:Speed and Height,1:Acceleration,2:Never,3:Always
// @User: Advanced
AP_GROUPINFO("MAG_CAL", 22, NavEKF2, _magCal, MAG_CAL_DEFAULT),
AP_GROUPINFO("POS_GATE", 6, NavEKF2, _gpsPosInnovGate, POS_GATE_DEFAULT),
// @Param: GLITCH_RAD
// @DisplayName: GPS glitch radius gate size (m)
@ -278,15 +148,124 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Increment: 5
// @User: Advanced
// @Units: meters
AP_GROUPINFO("GLITCH_RAD", 23, NavEKF2, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
AP_GROUPINFO("GLITCH_RAD", 7, NavEKF2, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
// @Param: GND_GRADIENT
// @DisplayName: Terrain Gradient % RMS
// @Description: This parameter sets the RMS terrain gradient percentage assumed by the terrain height estimation. Terrain height can be estimated using optical flow and/or range finder sensor data if fitted. Smaller values cause the terrain height estimate to be slower to respond to changes in measurement. Larger values casue the terrain height estimate to be faster to respond, but also more noisy. Generally this value can be reduced if operating over very flat terrain and increased if operating over uneven terrain.
// @Range: 1 - 50
// @Param: GPS_DELAY
// @DisplayName: GPS measurement delay (msec)
// @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements.
// @Range: 0 500
// @Increment: 10
// @User: Advanced
// @Units: milliseconds
AP_GROUPINFO("VEL_DELAY", 8, NavEKF2, _gpsDelay_ms, 220),
// Height measurement parameters
// @Param: ALT_SOURCE
// @DisplayName: Primary height source
// @Description: This parameter controls which height sensor is used by the EKF during optical flow navigation (when EKF_GPS_TYPE = 3). A value of will 0 cause it to always use baro altitude. A value of 1 will casue it to use range finder if available.
// @Values: 0:Use Baro, 1:Use Range Finder
// @User: Advanced
AP_GROUPINFO("ALT_SOURCE", 9, NavEKF2, _altSource, 1),
// @Param: ALT_NOISE
// @DisplayName: Altitude measurement noise (m)
// @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting on this measurement.
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: meters
AP_GROUPINFO("ALT_NOISE", 10, NavEKF2, _baroAltNoise, ALT_NOISE_DEFAULT),
// @Param: HGT_GATE
// @DisplayName: Height measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 1 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("GND_GRADIENT", 24, NavEKF2, _gndGradientSigma, 2),
AP_GROUPINFO("HGT_GATE", 11, NavEKF2, _hgtInnovGate, HGT_GATE_DEFAULT),
// @Param: HGT_DELAY
// @DisplayName: Height measurement delay (msec)
// @Description: This is the number of msec that the Height measurements lag behind the inertial measurements.
// @Range: 0 500
// @Increment: 10
// @User: Advanced
// @Units: milliseconds
AP_GROUPINFO("HGT_DELAY", 12, NavEKF2, _hgtDelay_ms, 60),
// Magnetometer measurement parameters
// @Param: MAG_NOISE
// @DisplayName: Magnetometer measurement noise (Gauss)
// @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.01 0.5
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("MAG_NOISE", 13, NavEKF2, _magNoise, MAG_NOISE_DEFAULT),
// @Param: MAG_CAL
// @DisplayName: Magnetometer calibration mode
// @Description: EKF_MAG_CAL = 0 enables calibration based on flying speed and altitude and is the default setting for Plane users. EKF_MAG_CAL = 1 enables calibration based on manoeuvre level and is the default setting for Copter and Rover users. EKF_MAG_CAL = 2 prevents magnetometer calibration regardless of flight condition and is recommended if in-flight magnetometer calibration is unreliable.
// @Values: 0:Speed and Height,1:Acceleration,2:Never,3:Always
// @User: Advanced
AP_GROUPINFO("MAG_CAL", 14, NavEKF2, _magCal, MAG_CAL_DEFAULT),
// @Param: MAG_GATE
// @DisplayName: Magnetometer measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the magnetometer measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 1 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("MAG_GATE", 15, NavEKF2, _magInnovGate, MAG_GATE_DEFAULT),
// Airspeed measurement parameters
// @Param: EAS_NOISE
// @DisplayName: Equivalent airspeed measurement noise (m/s)
// @Description: This is the RMS value of noise in equivalent airspeed measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("EAS_NOISE", 16, NavEKF2, _easNoise, 1.4f),
// @Param: EAS_GATE
// @DisplayName: Airspeed measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the airspeed measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 1 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("EAS_GATE", 17, NavEKF2, _tasInnovGate, 10),
// Rangefinder measurement parameters
// @Param: RNG_NOISE
// @DisplayName: Range finder measurement noise (m)
// @Description: This is the RMS value of noise in the range finder measurement. Increasing it reduces the weighting on this measurement.
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: meters
AP_GROUPINFO("RNG_NOISE", 18, NavEKF2, _rngNoise, 0.5f),
// @Param: RNG_GATE
// @DisplayName: Range finder measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the range finder innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 1 - 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("RNG_GATE", 19, NavEKF2, _rngInnovGate, 5),
// @Param: MAX_FLOW
// @DisplayName: Maximum valid optical flow rate
// @Description: This parameter sets the magnitude maximum optical flow rate in rad/sec that will be accepted by the filter
// @Range: 1.0 - 4.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("MAX_FLOW", 20, NavEKF2, _maxFlowRate, 2.5f),
// Optical flow measurement parameters
// @Param: FLOW_NOISE
// @DisplayName: Optical flow measurement noise (rad/s)
@ -295,7 +274,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Increment: 0.05
// @User: Advanced
// @Units: rad/s
AP_GROUPINFO("FLOW_NOISE", 25, NavEKF2, _flowNoise, FLOW_NOISE_DEFAULT),
AP_GROUPINFO("FLOW_NOISE", 21, NavEKF2, _flowNoise, FLOW_NOISE_DEFAULT),
// @Param: FLOW_GATE
// @DisplayName: Optical Flow measurement gate size
@ -303,7 +282,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Range: 1 - 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("FLOW_GATE", 26, NavEKF2, _flowInnovGate, FLOW_GATE_DEFAULT),
AP_GROUPINFO("FLOW_GATE", 22, NavEKF2, _flowInnovGate, FLOW_GATE_DEFAULT),
// @Param: FLOW_DELAY
// @DisplayName: Optical Flow measurement delay (msec)
@ -312,38 +291,75 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Increment: 10
// @User: Advanced
// @Units: milliseconds
AP_GROUPINFO("FLOW_DELAY", 27, NavEKF2, _flowDelay_ms, FLOW_MEAS_DELAY),
AP_GROUPINFO("FLOW_DELAY", 23, NavEKF2, _flowDelay_ms, FLOW_MEAS_DELAY),
// @Param: RNG_GATE
// @DisplayName: Range finder measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the range finder innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 1 - 100
// @Increment: 1
// State and Covariance Predition Parameters
// @Param: GYRO_PNOISE
// @DisplayName: Rate gyro noise (rad/s)
// @Description: This noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
// @Range: 0.001 0.05
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("RNG_GATE", 28, NavEKF2, _rngInnovGate, 5),
// @Units: rad/s
AP_GROUPINFO("GYRO_PNOISE", 24, NavEKF2, _gyrNoise, GYRO_PNOISE_DEFAULT),
// @Param: MAX_FLOW
// @DisplayName: Maximum valid optical flow rate
// @Description: This parameter sets the magnitude maximum optical flow rate in rad/sec that will be accepted by the filter
// @Range: 1.0 - 4.0
// @Param: ACC_PNOISE
// @DisplayName: Accelerometer noise (m/s^2)
// @Description: This noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
// @Range: 0.05 1.0
// @Increment: 0.01
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("ACC_PNOISE", 25, NavEKF2, _accNoise, ACC_PNOISE_DEFAULT),
// @Param: GBIAS_PNOISE
// @DisplayName: Rate gyro bias process noise (rad/s)
// @Description: This noise controls the growth of gyro bias state error estimates. Increasing it makes rate gyro bias estimation faster and noisier.
// @Range: 0.0000001 0.00001
// @User: Advanced
// @Units: rad/s
AP_GROUPINFO("GBIAS_PNOISE", 26, NavEKF2, _gyroBiasProcessNoise, GBIAS_PNOISE_DEFAULT),
// @Param: GSCL_PNOISE
// @DisplayName: Rate gyro scale factor process noise (1/s)
// @Description: This noise controls the rate of gyro scale factor learning. Increasing it makes rate gyro scale factor estimation faster and noisier.
// @Range: 0.0000001 0.00001
// @User: Advanced
// @Units: 1/s
AP_GROUPINFO("GSCL_PNOISE", 27, NavEKF2, _gyroScaleProcessNoise, 1e-6f),
// @Param: ABIAS_PNOISE
// @DisplayName: Accelerometer bias process noise (m/s^2)
// @Description: This noise controls the growth of the vertical acelerometer bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier.
// @Range: 0.00001 0.001
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("ABIAS_PNOISE", 28, NavEKF2, _accelBiasProcessNoise, ABIAS_PNOISE_DEFAULT),
// @Param: MAG_PNOISE
// @DisplayName: Magnetic field process noise (gauss/s)
// @Description: This noise controls the growth of magnetic field state error estimates. Increasing it makes magnetic field bias estimation faster and noisier.
// @Range: 0.0001 0.01
// @User: Advanced
// @Units: gauss/s
AP_GROUPINFO("MAG_PNOISE", 29, NavEKF2, _magProcessNoise, MAG_PNOISE_DEFAULT),
// @Param: WIND_PNOISE
// @DisplayName: Wind velocity process noise (m/s^2)
// @Description: This noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier.
// @Range: 0.01 1.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("MAX_FLOW", 29, NavEKF2, _maxFlowRate, 2.5f),
AP_GROUPINFO("WIND_PNOISE", 30, NavEKF2, _windVelProcessNoise, 0.1f),
// @Param: FALLBACK
// @DisplayName: Fallback strictness
// @Description: This parameter controls the conditions necessary to trigger a fallback to DCM and INAV. A value of 1 will cause fallbacks to occur on loss of GPS and other conditions. A value of 0 will trust the EKF more.
// @Values: 0:Trust EKF more, 1:Trust DCM more
// @Param: WIND_PSCALE
// @DisplayName: Height rate to wind procss noise scaler
// @Description: Increasing this parameter increases how rapidly the wind states adapt when changing altitude, but does make wind speed estimation noiser.
// @Range: 0.0 1.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("FALLBACK", 30, NavEKF2, _fallback, 1),
// @Param: ALT_SOURCE
// @DisplayName: Primary height source
// @Description: This parameter controls which height sensor is used by the EKF during optical flow navigation (when EKF_GPS_TYPE = 3). A value of will 0 cause it to always use baro altitude. A value of 1 will casue it to use range finder if available.
// @Values: 0:Use Baro, 1:Use Range Finder
// @User: Advanced
AP_GROUPINFO("ALT_SOURCE", 31, NavEKF2, _altSource, 1),
AP_GROUPINFO("WIND_PSCALE", 31, NavEKF2, _wndVarHgtRateScale, 0.5f),
AP_GROUPEND
};
@ -355,21 +371,20 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
gpsNEVelVarAccScale(0.05f), // Scale factor applied to horizontal velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error
gpsDVelVarAccScale(0.07f), // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error
gpsPosVarAccScale(0.05f), // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
hgtDelay_ms(60), // Height measurement delay (msec)
magDelay_ms(60), // Magnetometer measurement delay (msec)
tasDelay_ms(240), // Airspeed measurement delay (msec)
gpsRetryTimeUseTAS_ms(10000), // GPS retry time with airspeed measurements (msec)
gpsRetryTimeNoTAS_ms(7000), // GPS retry time without airspeed measurements (msec)
gpsFailTimeWithFlow_ms(1000), // If we have no GPS for longer than this and we have optical flow, then we will switch across to using optical flow (msec)
hgtRetryTimeMode0_ms(10000), // Height retry time with vertical velocity measurement (msec)
hgtRetryTimeMode12_ms(5000), // Height retry time without vertical velocity measurement (msec)
tasRetryTime_ms(5000), // True airspeed timeout and retry interval (msec)
magDelay_ms(60), // Magnetometer measurement delay (msec)
tasDelay_ms(240), // Airspeed measurement delay (msec)
gpsRetryTimeUseTAS_ms(10000), // GPS retry time with airspeed measurements (msec)
gpsRetryTimeNoTAS_ms(7000), // GPS retry time without airspeed measurements (msec)
gpsFailTimeWithFlow_ms(1000), // If we have no GPS for longer than this and we have optical flow, then we will switch across to using optical flow (msec)
hgtRetryTimeMode0_ms(10000), // Height retry time with vertical velocity measurement (msec)
hgtRetryTimeMode12_ms(5000), // Height retry time without vertical velocity measurement (msec)
tasRetryTime_ms(5000), // True airspeed timeout and retry interval (msec)
magFailTimeLimit_ms(10000), // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
magVarRateScale(0.05f), // scale factor applied to magnetometer variance due to angular rate
gyroBiasNoiseScaler(2.0f), // scale factor applied to imu gyro bias learning before the vehicle is armed
accelBiasNoiseScaler(1.0f), // scale factor applied to imu accel bias learning before the vehicle is armed
hgtAvg_ms(100), // average number of msec between height measurements
betaAvg_ms(100), // average number of msec between synthetic sideslip measurements
hgtAvg_ms(100), // average number of msec between height measurements
betaAvg_ms(100), // average number of msec between synthetic sideslip measurements
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
DCM33FlowMin(0.71f), // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
@ -377,7 +392,8 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
flowTimeDeltaAvg_ms(100), // average interval between optical flow measurements (msec)
flowIntervalMax_ms(100), // maximum allowable time between flow fusion events
gndEffectTimeout_ms(1000), // time in msec that baro ground effect compensation will timeout after initiation
gndEffectBaroScaler(4.0f) // scaler applied to the barometer observation variance when operating in ground effect
gndEffectBaroScaler(4.0f), // scaler applied to the barometer observation variance when operating in ground effect
gndGradientSigma(2) // RMS terrain gradient percentage assumed by the terrain height estimation
{
AP_Param::setup_object_defaults(this, var_info);
}

14
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -245,7 +245,8 @@ private:
AP_Float _accNoise; // accelerometer process noise : m/s^2
AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s
AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2
AP_Int16 _gpsDelay_ms; // effective average delay of GPS measurements relative to time of receipt (msec)
AP_Int16 _gpsDelay_ms; // effective average delay of GPS measurements relative to inertial measurement (msec)
AP_Int16 _hgtDelay_ms; // effective average delay of Height measurements relative to inertial measurements (msec)
AP_Int8 _fusionModeGPS; // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity
AP_Int8 _gpsVelInnovGate; // Number of standard deviations applied to GPS velocity innovation consistency check
AP_Int8 _gpsPosInnovGate; // Number of standard deviations applied to GPS position innovation consistency check
@ -254,21 +255,19 @@ private:
AP_Int8 _tasInnovGate; // Number of standard deviations applied to true airspeed innovation consistency check
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_Int8 _gndGradientSigma; // RMS terrain gradient percentage assumed by the terrain height estimation.
AP_Float _flowNoise; // optical flow rate measurement noise
AP_Int8 _flowInnovGate; // Number of standard deviations applied to optical flow innovation consistency check
AP_Int8 _flowDelay_ms; // effective average delay of optical flow measurements rel to IMU (msec)
AP_Int8 _rngInnovGate; // Number of standard deviations applied to range finder innovation consistency check
AP_Float _maxFlowRate; // Maximum flow rate magnitude that will be accepted by the filter
AP_Int8 _fallback; // EKF-to-DCM fallback strictness. 0 = trust EKF more, 1 = fallback more conservatively.
AP_Int8 _altSource; // Primary alt source during optical flow navigation. 0 = use Baro, 1 = use range finder.
AP_Float _gyroScaleProcessNoise;// gyro scale factor state process noise : 1/s
AP_Float _rngNoise; // Range finder noise : m
// Tuning parameters
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
const float gpsDVelVarAccScale; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
const float gpsPosVarAccScale; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
const uint16_t hgtDelay_ms; // Height measurement delay (msec)
const uint16_t magDelay_ms; // Magnetometer measurement delay (msec)
const uint16_t tasDelay_ms; // Airspeed measurement delay (msec)
const uint16_t gpsRetryTimeUseTAS_ms; // GPS retry time with airspeed measurements (msec)
@ -281,16 +280,17 @@ private:
const float magVarRateScale; // scale factor applied to magnetometer variance due to angular rate
const float gyroBiasNoiseScaler; // scale factor applied to gyro bias state process noise when on ground
const float accelBiasNoiseScaler; // scale factor applied to accel bias state process noise when on ground
const uint16_t hgtAvg_ms; // average number of msec between height measurements
const uint16_t betaAvg_ms; // average number of msec between synthetic sideslip measurements
const uint16_t hgtAvg_ms; // average number of msec between height measurements
const uint16_t betaAvg_ms; // average number of msec between synthetic sideslip measurements
const float covTimeStepMax; // maximum time (sec) between covariance prediction updates
const float covDelAngMax; // maximum delta angle between covariance prediction updates
const float DCM33FlowMin; // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
const float fScaleFactorPnoise; // Process noise added to focal length scale factor state variance at each time step
const uint8_t flowTimeDeltaAvg_ms; // average interval between optical flow measurements (msec)
const uint32_t flowIntervalMax_ms; // maximum allowable time between flow fusion events
const uint16_t gndEffectTimeout_ms; // time in msec that ground effect mode is active after being activated
const uint16_t gndEffectTimeout_ms; // time in msec that ground effect mode is active after being activated
const float gndEffectBaroScaler; // scaler applied to the barometer observation variance when ground effect mode is active
const uint8_t gndGradientSigma; // RMS terrain gradient percentage assumed by the terrain height estimation
};
#endif //AP_NavEKF2

8
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -58,7 +58,7 @@ bool NavEKF2_core::healthy(void) const
if (faultInt > 0) {
return false;
}
if (frontend._fallback && velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) {
if (velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) {
// all three metrics being above 1 means the filter is
// extremely unhealthy.
return false;
@ -2768,7 +2768,7 @@ void NavEKF2_core::EstimateTerrainOffset()
// in addition to a terrain gradient error model, we also have a time based error growth that is scaled using the gradient parameter
float timeLapsed = min(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend._gndGradientSigma))) + sq(float(frontend._gndGradientSigma) * timeLapsed);
float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend.gndGradientSigma))) + sq(float(frontend.gndGradientSigma) * timeLapsed);
Popt += Pincrement;
timeAtLastAuxEKF_ms = imuSampleTime_ms;
@ -2784,7 +2784,7 @@ void NavEKF2_core::EstimateTerrainOffset()
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
// Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
float R_RNG = 0.5f;
float R_RNG = frontend._rngNoise;
// calculate Kalman gain
float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
@ -3868,7 +3868,7 @@ void NavEKF2_core::readHgtData()
lastHgtReceived_ms = _baro.get_last_update();
// estimate of time height measurement was taken, allowing for delays
hgtMeasTime_ms = lastHgtReceived_ms - frontend.hgtDelay_ms;
hgtMeasTime_ms = lastHgtReceived_ms - frontend._hgtDelay_ms;
// save baro measurement to buffer to be fused later
baroDataNew.time_ms = hgtMeasTime_ms;