mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_NavEKF2: Increase resolution of innovation consistency gate parameters
Also adds protection against setting the gate to a number that would casue numerical errors.
This commit is contained in:
parent
08ef00b431
commit
f9cadaf15c
@ -24,15 +24,15 @@
|
||||
#define GBIAS_PNOISE_DEFAULT 7.0E-05f
|
||||
#define ABIAS_PNOISE_DEFAULT 1.0E-04f
|
||||
#define MAG_PNOISE_DEFAULT 2.5E-02f
|
||||
#define VEL_GATE_DEFAULT 2
|
||||
#define POS_GATE_DEFAULT 3
|
||||
#define HGT_GATE_DEFAULT 3
|
||||
#define MAG_GATE_DEFAULT 3
|
||||
#define VEL_GATE_DEFAULT 200
|
||||
#define POS_GATE_DEFAULT 300
|
||||
#define HGT_GATE_DEFAULT 300
|
||||
#define MAG_GATE_DEFAULT 300
|
||||
#define MAG_CAL_DEFAULT 3
|
||||
#define GLITCH_RADIUS_DEFAULT 25
|
||||
#define FLOW_MEAS_DELAY 10
|
||||
#define FLOW_NOISE_DEFAULT 0.25f
|
||||
#define FLOW_GATE_DEFAULT 3
|
||||
#define FLOW_GATE_DEFAULT 300
|
||||
#define GSCALE_PNOISE_DEFAULT 3.0E-03f
|
||||
#define CHECK_SCALER_DEFAULT 100
|
||||
|
||||
@ -48,15 +48,15 @@
|
||||
#define GBIAS_PNOISE_DEFAULT 7.0E-05f
|
||||
#define ABIAS_PNOISE_DEFAULT 1.0E-04f
|
||||
#define MAG_PNOISE_DEFAULT 2.5E-02f
|
||||
#define VEL_GATE_DEFAULT 2
|
||||
#define POS_GATE_DEFAULT 3
|
||||
#define HGT_GATE_DEFAULT 3
|
||||
#define MAG_GATE_DEFAULT 3
|
||||
#define VEL_GATE_DEFAULT 200
|
||||
#define POS_GATE_DEFAULT 300
|
||||
#define HGT_GATE_DEFAULT 300
|
||||
#define MAG_GATE_DEFAULT 300
|
||||
#define MAG_CAL_DEFAULT 2
|
||||
#define GLITCH_RADIUS_DEFAULT 25
|
||||
#define FLOW_MEAS_DELAY 10
|
||||
#define FLOW_NOISE_DEFAULT 0.25f
|
||||
#define FLOW_GATE_DEFAULT 3
|
||||
#define FLOW_GATE_DEFAULT 300
|
||||
#define GSCALE_PNOISE_DEFAULT 3.0E-03f
|
||||
#define CHECK_SCALER_DEFAULT 100
|
||||
|
||||
@ -72,15 +72,15 @@
|
||||
#define GBIAS_PNOISE_DEFAULT 7.0E-05f
|
||||
#define ABIAS_PNOISE_DEFAULT 1.0E-04f
|
||||
#define MAG_PNOISE_DEFAULT 2.5E-02f
|
||||
#define VEL_GATE_DEFAULT 2
|
||||
#define POS_GATE_DEFAULT 3
|
||||
#define HGT_GATE_DEFAULT 4
|
||||
#define MAG_GATE_DEFAULT 2
|
||||
#define VEL_GATE_DEFAULT 200
|
||||
#define POS_GATE_DEFAULT 300
|
||||
#define HGT_GATE_DEFAULT 400
|
||||
#define MAG_GATE_DEFAULT 200
|
||||
#define MAG_CAL_DEFAULT 0
|
||||
#define GLITCH_RADIUS_DEFAULT 25
|
||||
#define FLOW_MEAS_DELAY 10
|
||||
#define FLOW_NOISE_DEFAULT 0.25f
|
||||
#define FLOW_GATE_DEFAULT 3
|
||||
#define FLOW_GATE_DEFAULT 300
|
||||
#define GSCALE_PNOISE_DEFAULT 3.0E-03f
|
||||
#define CHECK_SCALER_DEFAULT 150
|
||||
|
||||
@ -96,15 +96,15 @@
|
||||
#define GBIAS_PNOISE_DEFAULT 7.0E-05f
|
||||
#define ABIAS_PNOISE_DEFAULT 1.0E-04f
|
||||
#define MAG_PNOISE_DEFAULT 2.5E-02f
|
||||
#define VEL_GATE_DEFAULT 2
|
||||
#define POS_GATE_DEFAULT 3
|
||||
#define HGT_GATE_DEFAULT 3
|
||||
#define MAG_GATE_DEFAULT 3
|
||||
#define VEL_GATE_DEFAULT 200
|
||||
#define POS_GATE_DEFAULT 300
|
||||
#define HGT_GATE_DEFAULT 300
|
||||
#define MAG_GATE_DEFAULT 300
|
||||
#define MAG_CAL_DEFAULT 3
|
||||
#define GLITCH_RADIUS_DEFAULT 25
|
||||
#define FLOW_MEAS_DELAY 10
|
||||
#define FLOW_NOISE_DEFAULT 0.25f
|
||||
#define FLOW_GATE_DEFAULT 3
|
||||
#define FLOW_GATE_DEFAULT 300
|
||||
#define GSCALE_PNOISE_DEFAULT 3.0E-03f
|
||||
#define CHECK_SCALER_DEFAULT 100
|
||||
|
||||
@ -151,9 +151,9 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||
|
||||
// @Param: VEL_GATE
|
||||
// @DisplayName: GPS velocity innovation gate size
|
||||
// @Description: This 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
|
||||
// @Description: This sets the percentage 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: 100 1000
|
||||
// @Increment: 25
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("VEL_GATE", 4, NavEKF2, _gpsVelInnovGate, VEL_GATE_DEFAULT),
|
||||
|
||||
@ -168,9 +168,9 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||
|
||||
// @Param: POS_GATE
|
||||
// @DisplayName: GPS position measurement gate size
|
||||
// @Description: This sets the number of standard deviations applied to the GPS position 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
|
||||
// @Description: This sets the percentage number of standard deviations applied to the GPS position 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: 100 1000
|
||||
// @Increment: 25
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("POS_GATE", 6, NavEKF2, _gpsPosInnovGate, POS_GATE_DEFAULT),
|
||||
|
||||
@ -212,9 +212,9 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||
|
||||
// @Param: HGT_GATE
|
||||
// @DisplayName: Height measurement gate size
|
||||
// @Description: This 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
|
||||
// @Description: This sets the percentage 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: 100 1000
|
||||
// @Increment: 25
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("HGT_GATE", 11, NavEKF2, _hgtInnovGate, HGT_GATE_DEFAULT),
|
||||
|
||||
@ -247,9 +247,9 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||
|
||||
// @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
|
||||
// @Description: This sets the percentage 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: 100 1000
|
||||
// @Increment: 25
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("MAG_GATE", 15, NavEKF2, _magInnovGate, MAG_GATE_DEFAULT),
|
||||
|
||||
@ -266,11 +266,11 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||
|
||||
// @Param: EAS_GATE
|
||||
// @DisplayName: Airspeed measurement gate size
|
||||
// @Description: This is the RMS value of noise in equivalent airspeed measurements used by planes. Increasing it reduces the weighting of airspeed measurements and will make wind speed estimates less noisy and slower to converge. Increasing also increases navigation errors when dead-reckoning without GPS measurements.
|
||||
// @Range: 1 100
|
||||
// @Increment: 1
|
||||
// @Description: This sets the percentage 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: 100 1000
|
||||
// @Increment: 25
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("EAS_GATE", 17, NavEKF2, _tasInnovGate, 10),
|
||||
AP_GROUPINFO("EAS_GATE", 17, NavEKF2, _tasInnovGate, 1000),
|
||||
|
||||
// Rangefinder measurement parameters
|
||||
|
||||
@ -285,11 +285,11 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||
|
||||
// @Param: RNG_GATE
|
||||
// @DisplayName: Range finder measurement gate size
|
||||
// @Description: This 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
|
||||
// @Description: This sets the percentage 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: 100 - 1000
|
||||
// @Increment: 25
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("RNG_GATE", 19, NavEKF2, _rngInnovGate, 5),
|
||||
AP_GROUPINFO("RNG_GATE", 19, NavEKF2, _rngInnovGate, 500),
|
||||
|
||||
// Optical flow measurement parameters
|
||||
|
||||
@ -313,9 +313,9 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||
|
||||
// @Param: FLOW_GATE
|
||||
// @DisplayName: Optical Flow measurement gate size
|
||||
// @Description: This sets the number of standard deviations applied to the optical flow 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
|
||||
// @Description: This sets the percentage number of standard deviations applied to the optical flow 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: 100 - 1000
|
||||
// @Increment: 25
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("FLOW_GATE", 22, NavEKF2, _flowInnovGate, FLOW_GATE_DEFAULT),
|
||||
|
||||
|
@ -293,17 +293,17 @@ private:
|
||||
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
|
||||
AP_Int8 _hgtInnovGate; // Number of standard deviations applied to height innovation consistency check
|
||||
AP_Int8 _magInnovGate; // Number of standard deviations applied to magnetometer innovation consistency check
|
||||
AP_Int8 _tasInnovGate; // Number of standard deviations applied to true airspeed innovation consistency check
|
||||
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
|
||||
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
|
||||
AP_Int8 _flowInnovGate; // Number of standard deviations applied to optical flow innovation consistency check
|
||||
AP_Int16 _flowInnovGate; // Percentage 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_Int16 _rngInnovGate; // Percentage 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 _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
|
||||
|
@ -121,7 +121,7 @@ void NavEKF2_core::FuseAirspeed()
|
||||
innovVtas = VtasPred - tasDataDelayed.tas;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
tasTestRatio = sq(innovVtas) / (sq(frontend->_tasInnovGate) * varInnovVtas);
|
||||
tasTestRatio = sq(innovVtas) / (sq(max(0.01f * (float)frontend->_tasInnovGate, 1.0f)) * varInnovVtas);
|
||||
|
||||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||||
tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
|
||||
|
@ -321,7 +321,7 @@ void NavEKF2_core::FuseMagnetometer()
|
||||
|
||||
// calculate the innovation test ratios
|
||||
for (uint8_t i = 0; i<=2; i++) {
|
||||
magTestRatio[i] = sq(innovMag[i]) / (sq(max(frontend->_magInnovGate,1)) * varInnovMag[i]);
|
||||
magTestRatio[i] = sq(innovMag[i]) / (sq(max(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
|
||||
}
|
||||
|
||||
// check the last values from all components and set magnetometer health accordingly
|
||||
@ -701,7 +701,7 @@ void NavEKF2_core::fuseCompass()
|
||||
}
|
||||
|
||||
// calculate the innovation test ratio
|
||||
yawTestRatio = sq(innovation) / (sq(max(frontend->_magInnovGate,1)) * varInnov);
|
||||
yawTestRatio = sq(innovation) / (sq(max(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnov);
|
||||
|
||||
// Declare the magnetometer unhealthy if the innovation test fails
|
||||
if (yawTestRatio > 1.0f) {
|
||||
|
@ -155,7 +155,7 @@ void NavEKF2_core::EstimateTerrainOffset()
|
||||
innovRng = predRngMeas - rangeDataDelayed.rng;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(frontend->_rngInnovGate) * varInnovRng);
|
||||
auxRngTestRatio = sq(innovRng) / (sq(max(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||||
if ((sq(innovRng)*SK_RNG) < 25.0f)
|
||||
@ -243,7 +243,7 @@ void NavEKF2_core::EstimateTerrainOffset()
|
||||
K_OPT = Popt*H_OPT/auxFlowObsInnovVar;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(frontend->_flowInnovGate) * auxFlowObsInnovVar);
|
||||
auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(max(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar);
|
||||
|
||||
// don't fuse if optical flow data is outside valid range
|
||||
if (max(flowRadXY[0],flowRadXY[1]) < frontend->_maxFlowRate) {
|
||||
@ -627,7 +627,7 @@ void NavEKF2_core::FuseOptFlow()
|
||||
}
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(frontend->_flowInnovGate) * varInnovOptFlow[obsIndex]);
|
||||
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(max(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
|
||||
if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {
|
||||
|
@ -304,7 +304,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
|
||||
varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
|
||||
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
||||
float maxPosInnov2 = sq(frontend->_gpsPosInnovGate)*(varInnovVelPos[3] + varInnovVelPos[4]);
|
||||
float maxPosInnov2 = sq(max(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
|
||||
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
|
||||
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
|
||||
// declare a timeout condition if we have been too long without data or not aiding
|
||||
@ -362,7 +362,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
}
|
||||
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
||||
// calculate the test ratio
|
||||
velTestRatio = innovVelSumSq / (varVelSum * sq(frontend->_gpsVelInnovGate));
|
||||
velTestRatio = innovVelSumSq / (varVelSum * sq(max(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f)));
|
||||
// fail if the ratio is greater than 1
|
||||
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
|
||||
// declare a timeout if we have not fused velocity data for too long or not aiding
|
||||
@ -392,7 +392,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
innovVelPos[5] = stateStruct.position.z - observation[5];
|
||||
varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
|
||||
// calculate the innovation consistency test ratio
|
||||
hgtTestRatio = sq(innovVelPos[5]) / (sq(frontend->_hgtInnovGate) * varInnovVelPos[5]);
|
||||
hgtTestRatio = sq(innovVelPos[5]) / (sq(max(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
|
||||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||||
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
|
||||
// Fuse height data if healthy or timed out or in constant position mode
|
||||
|
Loading…
Reference in New Issue
Block a user