aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Enable monitoring of unhealthy airspeed sensors

Browse Source 
Innovations and innovation test ratios will still be calculated and reported for an unhealthy sensor, but the EKF states wnd covariance matrix will not be modified.
This commit is contained in:
Paul Riseborough 2022-06-26 08:09:09 +10:00 committed by Randy Mackay
parent 6a60f1a421
commit 47189d2c73
4 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -68,7 +68,7 @@ void NavEKF3_core::FuseAirspeed()
}
SK_TAS[1] = SH_TAS[1];
if (!airDataFusionWindOnly) {
if (tasDataDelayed.healthy && !airDataFusionWindOnly) {
Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]);
Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]);
Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]);
@ -84,7 +84,7 @@ void NavEKF3_core::FuseAirspeed()
zero_range(&Kfusion[0], 0, 9);
}
if (!inhibitDelAngBiasStates && !airDataFusionWindOnly) {
if (tasDataDelayed.healthy && !inhibitDelAngBiasStates && !airDataFusionWindOnly) {
Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]);
Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]);
Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]);
@ -93,7 +93,7 @@ void NavEKF3_core::FuseAirspeed()
zero_range(&Kfusion[0], 10, 12);
}
if (!inhibitDelVelBiasStates && !airDataFusionWindOnly) {
if (tasDataDelayed.healthy && !inhibitDelVelBiasStates && !airDataFusionWindOnly) {
for (uint8_t index = 0; index < 3; index++) {
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
@ -108,7 +108,7 @@ void NavEKF3_core::FuseAirspeed()
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates && !airDataFusionWindOnly) {
if (tasDataDelayed.healthy && !inhibitMagStates && !airDataFusionWindOnly) {
Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]);
Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]);
Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]);
@ -120,7 +120,7 @@ void NavEKF3_core::FuseAirspeed()
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
if (tasDataDelayed.healthy && !inhibitWindStates) {
Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]);
Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]);
} else {
@ -135,7 +135,7 @@ void NavEKF3_core::FuseAirspeed()
tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas);
// fail if the ratio is > 1, but don't fail if bad IMU data
bool tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
const bool tasHealth = tasDataDelayed.healthy && ((tasTestRatio < 1.0f) || badIMUdata);
tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms;
if (!tasHealth) {
lastTasFailTime_ms = imuSampleTime_ms;
@ -189,11 +189,10 @@ void NavEKF3_core::FuseAirspeed()
}
}
}
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
ForceSymmetry();
ConstrainVariances();
}
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
ForceSymmetry();
ConstrainVariances();
}
// select fusion of true airspeed measurements

4
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -77,7 +77,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
Vector3F tempEuler;
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
ftype trueAirspeedVariance;
const bool haveAirspeedMeasurement = usingDefaultAirspeed || (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500 && useAirspeed());
const bool haveAirspeedMeasurement = usingDefaultAirspeed || (tasDataDelayed.healthy && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed());
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;
@ -738,7 +738,7 @@ void NavEKF3_core::runYawEstimatorPrediction()
}
ftype trueAirspeed;
if (assume_zero_sideslip()) {
if (tasDataDelayed.healthy && assume_zero_sideslip()) {
trueAirspeed = MAX(tasDataDelayed.tas, 0.0f);
} else {
trueAirspeed = 0.0f;

3
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -832,12 +832,12 @@ void NavEKF3_core::readAirSpdData()
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) * 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));
tasDataNew.healthy = airspeed->healthy(selected_airspeed);
// Correct for the average intersampling delay due to the filter update rate
tasDataNew.time_ms -= localFilterTimeStep_ms/2;
@ -856,6 +856,7 @@ void NavEKF3_core::readAirSpdData()
is_positive(defaultAirSpeed)) {
tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
tasDataDelayed.tasVariance = sq(MAX(defaultAirSpeedVariance, easErrVar));
tasDataDelayed.healthy = true;
tasDataDelayed.time_ms = 0;
usingDefaultAirspeed = true;
} else {

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -555,6 +555,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
bool healthy; // true if measurement can be allowed to modify EKF states.
};
struct of_elements : EKF_obs_element_t {