diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index c491131994..d0c3fe8733 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -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), diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 4168093857..019bd1fc34 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index 7d13b4f7de..52bf29a0cf 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp @@ -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); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 48569dad14..58cabf0e15 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -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) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index f78717e94a..25dd397115 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -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)) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 617e84d4c2..e517882617 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -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