Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Rework non-airspeed wind estimation 

Faster wind estimation when not using airspeed with acceptable noise in wind velocity estimates.
This commit is contained in:
Paul Riseborough 2021-07-17 16:50:15 +10:00 committed by Randy Mackay
parent 51a1b9ae9a
commit a98583e0ca
7 changed files with 65 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -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

2
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -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)

15
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -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++) {

37
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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;
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);
}
}
}

15
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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);
}
/********************************************************

3
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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);

8
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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