From 59d31cc7d5206c60a60b48836d4e886d85fd2343 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 17 Jul 2021 16:50:15 +1000 Subject: [PATCH] AP_NavEKF3: Rework non-airspeed wind estimation Faster wind estimation when not using airspeed with acceptable noise in wind velocity estimates. --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 22 +++++----- libraries/AP_NavEKF3/AP_NavEKF3.h | 2 +- .../AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 15 +++---- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 41 +++++++++++++------ .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 15 ++++--- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 3 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 8 +++- 7 files changed, 65 insertions(+), 41 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 641ba8bfde..17f496d8c3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -39,7 +39,7 @@ #define FLOW_I_GATE_DEFAULT 300 #define CHECK_SCALER_DEFAULT 100 #define FLOW_USE_DEFAULT 1 -#define WIND_P_NSE_DEFAULT 0.2 +#define WIND_P_NSE_DEFAULT 1.0 #elif APM_BUILD_TYPE(APM_BUILD_Rover) // rover defaults @@ -65,7 +65,7 @@ #define FLOW_I_GATE_DEFAULT 300 #define CHECK_SCALER_DEFAULT 100 #define FLOW_USE_DEFAULT 1 -#define WIND_P_NSE_DEFAULT 0.1 +#define WIND_P_NSE_DEFAULT 0.5 #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane) // plane defaults @@ -91,7 +91,7 @@ #define FLOW_I_GATE_DEFAULT 500 #define CHECK_SCALER_DEFAULT 150 #define FLOW_USE_DEFAULT 2 -#define WIND_P_NSE_DEFAULT 0.1 +#define WIND_P_NSE_DEFAULT 0.5 #else // build type not specified, use copter defaults @@ -117,7 +117,7 @@ #define FLOW_I_GATE_DEFAULT 300 #define CHECK_SCALER_DEFAULT 100 #define FLOW_USE_DEFAULT 1 -#define WIND_P_NSE_DEFAULT 0.1 +#define WIND_P_NSE_DEFAULT 0.5 #endif // APM_BUILD_DIRECTORY @@ -371,7 +371,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: WIND_P_NSE // @DisplayName: Wind velocity process noise (m/s^2) // @Description: This state process noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier. - // @Range: 0.01 1.0 + // @Range: 0.01 2.0 // @Increment: 0.1 // @User: Advanced // @Units: m/s/s @@ -380,10 +380,10 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: WIND_PSCALE // @DisplayName: Height rate to wind process noise scaler // @Description: This controls how much the process noise on the wind states is increased when gaining or losing altitude to take into account changes in wind speed and direction with altitude. Increasing this parameter increases how rapidly the wind states adapt when changing altitude, but does make wind velocity estimation noiser. - // @Range: 0.0 1.0 + // @Range: 0.0 2.0 // @Increment: 0.1 // @User: Advanced - AP_GROUPINFO("WIND_PSCALE", 31, NavEKF3, _wndVarHgtRateScale, 0.5f), + AP_GROUPINFO("WIND_PSCALE", 31, NavEKF3, _wndVarHgtRateScale, 1.0f), // @Param: GPS_CHECK // @DisplayName: GPS preflight check @@ -416,13 +416,13 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Units: m AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF3, _noaidHorizNoise, 10.0f), - // @Param: BETA_MASK + // @Param: ADATA_MASK // @DisplayName: Bitmask controlling sidelip angle fusion - // @Description: 1 byte bitmap controlling use of sideslip angle fusion for estimation of non wind states during operation of 'fly forward' vehicle types such as fixed wing planes. By assuming that the angle of sideslip is small, the wind velocity state estimates are corrected whenever the EKF is not dead reckoning (e.g. has an independent velocity or position sensor such as GPS). This behaviour is on by default and cannot be disabled. When the EKF is dead reckoning, the wind states are used as a reference, enabling use of the small angle of sideslip assumption to correct non wind velocity states (eg attitude, velocity, position, etc) and improve navigation accuracy. This behaviour is on by default and cannot be disabled. The behaviour controlled by this parameter is the use of the small angle of sideslip assumption to correct non wind velocity states when the EKF is NOT dead reckoning. This is primarily of benefit to reduce the buildup of yaw angle errors during straight and level flight without a yaw sensor (e.g. magnetometer or dual antenna GPS yaw) provided aerobatic flight maneuvers with large sideslip angles are not performed. The 'always' option might be used where the yaw sensor is intentionally not fitted or disabled. The 'WhenNoYawSensor' option might be used if a yaw sensor is fitted, but protection against in-flight failure and continual rejection by the EKF is desired. For vehicles operated within visual range of the operator performing frequent turning maneuvers, setting this parameter is unnecessary. - // @Bitmask: 0:Always,1:WhenNoYawSensor + // @Description: 1 byte bitmap controlling use of sideslip angle and estimated airspeed during operation of 'fly forward' vehicle types such as fixed wing planes. By assuming that the angle of sideslip is small, the wind velocity state estimates are corrected whenever the EKF is not dead reckoning (e.g. has an independent velocity or position sensor such as GPS). This behaviour is on by default and cannot be disabled. When the EKF is dead reckoning, the wind states are used as a reference, enabling use of the small angle of sideslip assumption to correct non wind velocity states (eg attitude, velocity, position, etc) and improve navigation accuracy. This behaviour is on by default and cannot be disabled. The behaviour controlled by this parameter is the use of the small angle of sideslip assumption and the use of the estimated airspeed (average of ARSPD_FBW_MIN and ARSPD_FBW_MAX) to correct non wind velocity states when the EKF is NOT dead reckoning. This is primarily of benefit to reduce the buildup of yaw angle errors during straight and level flight without a yaw sensor (e.g. magnetometer or dual antenna GPS yaw) provided aerobatic flight maneuvers with large sideslip angles are not performed. The 'SideslipAlways' option might be used where the yaw sensor is intentionally not fitted or disabled. The 'SideslipWhenFailedYawSensor' option might be used if a yaw sensor is fitted, but protection against in-flight failure and continual rejection by the EKF is desired. For vehicles operated within visual range of the operator performing frequent turning maneuvers, setting this parameter is unnecessary. The AirspeedAlways option might be used when operating without an airspeed sensor and when the TRIM_THROTTLE has been adjusted to fly close to the average of ARSPD_FBW_MIN and ARSPD_FBW_MAX. + // @Bitmask: 0:SideslipAlways,1:SideslipWhenFailedYawSensor,2:AirspeedAlways // @User: Advanced // @RebootRequired: True - AP_GROUPINFO("BETA_MASK", 36, NavEKF3, _betaMask, 0), + AP_GROUPINFO("ADATA_MASK", 36, NavEKF3, _airDataMask, 0), // control of magnetic yaw angle fusion diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index c35e9b7daa..508ce7262d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -425,7 +425,7 @@ private: AP_Float _ballisticCoef_x; // ballistic coefficient measured for flow in X body frame directions AP_Float _ballisticCoef_y; // ballistic coefficient measured for flow in Y body frame directions AP_Float _momentumDragCoef; // lift rotor momentum drag coefficient - AP_Int8 _betaMask; // Bitmask controlling when sideslip angle fusion is used to estimate non wind states + AP_Int8 _airDataMask; // Bitmask controlling when sideslip angle and default airspeed fusion is used to estimate non wind states AP_Float _ognmTestScaleFactor; // Scale factor applied to the thresholds used by the on ground not moving test AP_Float _baroGndEffectDeadZone;// Dead zone applied to positive baro height innovations when in ground effect (m) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 14c69c788a..6952165fa5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -55,8 +55,8 @@ void NavEKF3_core::FuseAirspeed() H_TAS[22] = -SH_TAS[2]; H_TAS[23] = -SH_TAS[1]; // calculate Kalman gains - ftype temp = (tasErrVar + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); - if (temp >= tasErrVar) { + ftype temp = (tasDataDelayed.tasVariance + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + if (temp >= tasDataDelayed.tasVariance) { SK_TAS[0] = 1.0f / temp; faultStatus.bad_airspeed = false; } else { @@ -237,7 +237,7 @@ void NavEKF3_core::SelectBetaDragFusion() // use of air data to constrain drift is necessary if we have limited sensor data or are doing inertial dead reckoning bool is_dead_reckoning = ((imuSampleTime_ms - lastPosPassTime_ms) > frontend->deadReckonDeclare_ms) && ((imuSampleTime_ms - lastVelPassTime_ms) > frontend->deadReckonDeclare_ms); const bool noYawSensor = !use_compass() && !using_external_yaw(); - const bool f_required = (noYawSensor && (frontend->_betaMask & (1<<1))) || is_dead_reckoning; + const bool f_required = (noYawSensor && (frontend->_airDataMask & (1<<1))) || is_dead_reckoning; // set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states) const bool f_beta_feasible = (assume_zero_sideslip() && !inhibitWindStates); @@ -245,7 +245,7 @@ void NavEKF3_core::SelectBetaDragFusion() // use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion if (f_beta_feasible && f_timeTrigger) { // unless air data is required to constrain drift, it is only used to update wind state estimates - if (f_required || (frontend->_betaMask & (1<<0))) { + if (f_required || (frontend->_airDataMask & (1<<0))) { // we are required to correct all states airDataFusionWindOnly = false; } else { @@ -428,12 +428,7 @@ void NavEKF3_core::FuseSideslip() } // calculate predicted sideslip angle and innovation using small angle approximation - innovBeta = vel_rel_wind.y / vel_rel_wind.x; - - // reject measurement if greater than 3-sigma inconsistency - if (innovBeta > 0.5f) { - return; - } + innovBeta = constrain_ftype(vel_rel_wind.y / vel_rel_wind.x, -0.5f, 0.5f); // correct the state vector for (uint8_t j= 0; j<=stateIndexLim; j++) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index aff6b2ff6e..690c691791 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -59,32 +59,49 @@ NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const // avoid unnecessary operations void NavEKF3_core::setWindMagStateLearningMode() { - // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states - bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip() && !(dragFusionEnabled && finalInflightYawInit)) || onGround || (PV_AidingMode == AID_NONE); - if (!inhibitWindStates && setWindInhibit) { + const bool canEstimateWind = ((finalInflightYawInit && dragFusionEnabled) || assume_zero_sideslip()) && + !onGround && + PV_AidingMode != AID_NONE; + if (!inhibitWindStates && !canEstimateWind) { inhibitWindStates = true; updateStateIndexLim(); - } else if (inhibitWindStates && !setWindInhibit) { + } else if (inhibitWindStates && canEstimateWind && + sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f)) { inhibitWindStates = false; updateStateIndexLim(); // set states and variances - if (yawAlignComplete && useAirspeed()) { - // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading + if (yawAlignComplete && assume_zero_sideslip()) { + // if we have a valid heading, set the wind states to the reciprocal of the vehicle heading // which assumes the vehicle has launched into the wind + // use airspeed if if recent data available Vector3F tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); - ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; - stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z); - stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z); + ftype trueAirspeedVariance; + const bool haveAirspeedMeasurement = usingDefaultAirspeed || imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500; + if (haveAirspeedMeasurement) { + trueAirspeedVariance = constrain_ftype(tasDataDelayed.tasVariance, WIND_VEL_VARIANCE_MIN, WIND_VEL_VARIANCE_MAX); + const ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; + stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z); + stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z); + } else { + trueAirspeedVariance = sq(WIND_VEL_VARIANCE_MAX); // use 2-sigma for faster initial convergence + } // set the wind state variances to the measurement uncertainty for (uint8_t index=22; index<=23; index++) { - P[index][index] = sq(constrain_ftype(frontend->_easNoise, 0.5f, 5.0f) * constrain_ftype(dal.get_EAS2TAS(), 0.9f, 10.0f)); + zeroCols(P, 22, 23); + zeroRows(P, 22, 23); + P[index][index] = trueAirspeedVariance; } + + windStatesAligned = true; + } else { - // set the variances using a typical wind speed + // set the variances using a typical max wind speed for small UAV operation + zeroCols(P, 22, 23); + zeroRows(P, 22, 23); for (uint8_t index=22; index<=23; index++) { - P[index][index] = sq(5.0f); + P[index][index] = sq(WIND_VEL_VARIANCE_MAX); } } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index b63bfcdcd2..dd0b1b4f3e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -842,6 +842,7 @@ void NavEKF3_core::readAirSpdData() tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS; timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed); tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; + tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f)); // Correct for the average intersampling delay due to the filter update rate tasDataNew.time_ms -= localFilterTimeStep_ms/2; @@ -849,18 +850,22 @@ void NavEKF3_core::readAirSpdData() // Save data into the buffer to be fused when the fusion time horizon catches up with it storedTAS.push(tasDataNew); } + // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms); + float easErrVar = sq(MAX(frontend->_easNoise, 0.5f)); - // Allow use of a default value if the measurement times out - if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 5000 && is_positive(defaultAirSpeed)) { + // Allow use of a default value if enabled + if (!useAirspeed() && + imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200 && + is_positive(defaultAirSpeed)) { tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; - easErrVar = MAX(defaultAirSpeedVariance, easErrVar); - usingDefaultAirspeed = true; + tasDataDelayed.tasVariance = sq(MAX(defaultAirSpeedVariance, easErrVar)); + tasDataDelayed.time_ms = 0; + usingDefaultAirspeed = frontend->_airDataMask & (1<<2); } else { usingDefaultAirspeed = false; } - tasErrVar = easErrVar * sq(EAS2TAS); } /******************************************************** diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 95c0ede9f3..dd68ff3cc1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -268,6 +268,7 @@ void NavEKF3_core::InitialiseVariables() prevInFlight = false; manoeuvring = false; inhibitWindStates = true; + windStatesAligned = false; inhibitDelVelBiasStates = true; inhibitDelAngBiasStates = true; gndOffsetValid = false; @@ -1946,7 +1947,7 @@ void NavEKF3_core::ConstrainVariances() } if (!inhibitWindStates) { - for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,1.0e3f); + for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,WIND_VEL_VARIANCE_MAX); } else { zeroCols(P,22,23); zeroRows(P,22,23); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index d0b8814ac7..0a57f2b099 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -105,6 +105,11 @@ // number of milliseconds the bad IMU data response settings will be held after the last bad IMU data is detected #define BAD_IMU_DATA_HOLD_MS 10000 +// wind state variance limits +#define WIND_VEL_VARIANCE_MAX 400.0f +#define WIND_VEL_VARIANCE_MIN 0.25f + + class NavEKF3_core : public NavEKF_core_common { public: @@ -541,6 +546,7 @@ private: struct tas_elements : EKF_obs_element_t { ftype tas; // true airspeed measurement (m/sec) + ftype tasVariance; // variance of true airspeed measurement (m/sec)^2 }; struct of_elements : EKF_obs_element_t { @@ -1036,6 +1042,7 @@ private: Vector3F magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold ftype tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold bool inhibitWindStates; // true when wind states and covariances are to remain constant + bool windStatesAligned; // true when wind states have been aligned bool inhibitMagStates; // true when magnetic field states are inactive bool lastInhibitMagStates; // previous inhibitMagStates bool needMagBodyVarReset; // we need to reset mag body variances at next CovariancePrediction @@ -1069,7 +1076,6 @@ private: range_elements rangeDataDelayed;// Range finder data at the fusion time horizon tas_elements tasDataNew; // TAS data at the current time horizon tas_elements tasDataDelayed; // TAS data at the fusion time horizon - ftype tasErrVar; // TAS error variance (m/s)**2 bool usingDefaultAirspeed; // true when a default airspeed is being used instead of a measured value mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon gps_elements gpsDataNew; // GPS data at the current time horizon