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

AP_NavEKF3: Fix bug causing unwanted use of default airspeed

This commit is contained in:
Paul Riseborough 2021-01-06 11:42:34 +11:00 committed by Andrew Tridgell
parent a0faa55ef5
commit 3634a942a1
4 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -25,7 +25,6 @@ void NavEKF3_core::FuseAirspeed()
float vd;
float vwn;
float vwe;
float EAS2TAS = dal.get_EAS2TAS();
float SH_TAS[3];
float SK_TAS[2];
Vector24 H_TAS = {};
@ -44,14 +43,7 @@ void NavEKF3_core::FuseAirspeed()
if (VtasPred > 1.0f)
{
// calculate observation innovation and variance
float specifiedVariance = sq(MAX(frontend->_easNoise, 0.5f));
if (is_positive(defaultAirSpeed)) {
innovVtas = VtasPred - defaultAirSpeed;
specifiedVariance = MAX(specifiedVariance, defaultAirSpeedVariance);
} else {
innovVtas = VtasPred - tasDataDelayed.tas;
}
const float R_TAS = specifiedVariance * sq(constrain_float(EAS2TAS, 0.9f, 10.0f));
innovVtas = VtasPred - tasDataDelayed.tas;
// calculate observation jacobians
SH_TAS[0] = 1.0f/VtasPred;
@ -63,8 +55,8 @@ void NavEKF3_core::FuseAirspeed()
H_TAS[22] = -SH_TAS[2];
H_TAS[23] = -SH_TAS[1];
// calculate Kalman gains
float temp = (R_TAS + 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 >= R_TAS) {
float 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) {
SK_TAS[0] = 1.0f / temp;
faultStatus.bad_airspeed = false;
} else {
@ -256,8 +248,7 @@ void NavEKF3_core::SelectBetaDragFusion()
airDataFusionWindOnly = true;
}
// Fuse estimated airspeed to aid wind estimation
if (is_positive(defaultAirSpeed)) {
frontend->_easNoise = 0.33f * defaultAirSpeed;
if (usingDefaultAirspeed) {
FuseAirspeed();
}
FuseSideslip();

9
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -700,16 +700,11 @@ void NavEKF3_core::runYawEstimatorPrediction()
}
float trueAirspeed;
if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) {
if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) {
trueAirspeed = tasDataDelayed.tas;
} else {
trueAirspeed = defaultAirSpeed * dal.get_EAS2TAS();
}
if (assume_zero_sideslip()) {
trueAirspeed = MAX(tasDataDelayed.tas, 0.0f);
} else {
trueAirspeed = 0.0f;
}
yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed);
}

14
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -815,15 +815,17 @@ void NavEKF3_core::correctEkfOriginHeight()
// check for new airspeed data and update stored measurements if available
void NavEKF3_core::readAirSpdData()
{
const float EAS2TAS = dal.get_EAS2TAS();
// if airspeed reading is valid and is set by the user to be used and has been updated then
// we take a new reading, convert from EAS to TAS and set the flag letting other functions
// know a new measurement is available
const auto *airspeed = dal.airspeed();
if (airspeed &&
airspeed->use(selected_airspeed) &&
airspeed->healthy(selected_airspeed) &&
(airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * dal.get_EAS2TAS();
tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS;
timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed);
tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
@ -835,6 +837,16 @@ void NavEKF3_core::readAirSpdData()
}
// 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)) {
tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
easErrVar = MAX(defaultAirSpeedVariance, easErrVar);
usingDefaultAirspeed = true;
} else {
usingDefaultAirspeed = false;
}
tasErrVar = easErrVar * sq(EAS2TAS);
}
/********************************************************

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1056,6 +1056,8 @@ 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
float 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
gps_elements gpsDataDelayed; // GPS data at the fusion time horizon