Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Treat wind as truth when deadreckoning with no airspeed sensor

Browse Source
This commit is contained in:
Paul Riseborough 2023-09-27 08:18:07 +10:00 committed by Andrew Tridgell
parent 568972c0f0
commit 87bf8d9997
7 changed files with 23 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -120,7 +120,7 @@ void NavEKF3_core::FuseAirspeed()
zero_range(&Kfusion[0], 16, 21);
}
if (tasDataDelayed.allowFusion && !inhibitWindStates) {
if (tasDataDelayed.allowFusion && !inhibitWindStates && !treatWindStatesAsTruth) {
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 {
@ -420,7 +420,7 @@ void NavEKF3_core::FuseSideslip()
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]);
Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]);
} else {

8
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -642,7 +642,7 @@ void NavEKF3_core::FuseMagnetometer()
}
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]);
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]);
} else {
@ -725,7 +725,7 @@ void NavEKF3_core::FuseMagnetometer()
}
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]);
} else {
@ -809,7 +809,7 @@ void NavEKF3_core::FuseMagnetometer()
}
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]);
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]);
} else {
@ -1345,7 +1345,7 @@ void NavEKF3_core::FuseDeclination(ftype declErr)
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN);
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
} else {

4
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -491,7 +491,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
Kfusion[23] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
} else {
@ -668,7 +668,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
Kfusion[23] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
} else {

8
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -1046,7 +1046,7 @@ void NavEKF3_core::FuseVelPosNED()
}
// inhibit wind state estimation by setting Kalman gains to zero
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = P[22][stateIndex]*SK;
Kfusion[23] = P[23][stateIndex]*SK;
} else {
@ -1563,7 +1563,7 @@ void NavEKF3_core::FuseBodyVel()
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24);
Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24);
} else {
@ -1740,7 +1740,7 @@ void NavEKF3_core::FuseBodyVel()
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25);
Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25);
} else {
@ -1918,7 +1918,7 @@ void NavEKF3_core::FuseBodyVel()
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24);
Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24);
} else {

2
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -234,7 +234,7 @@ void NavEKF3_core::FuseRngBcn()
zero_range(&Kfusion[0], 16, 21);
}
if (!inhibitWindStates) {
if (!inhibitWindStates && !treatWindStatesAsTruth) {
Kfusion[22] = -t26*(P[22][7]*t4*t9+P[22][8]*t3*t9+P[22][9]*t2*t9);
Kfusion[23] = -t26*(P[23][7]*t4*t9+P[23][8]*t3*t9+P[23][9]*t2*t9);
} else {

11
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -270,6 +270,7 @@ void NavEKF3_core::InitialiseVariables()
inhibitWindStates = true;
windStateLastObs.zero();
windStateIsObservable = false;
treatWindStatesAsTruth = false;
windStateLastObsIsValid = false;
windStatesAligned = false;
inhibitDelVelBiasStates = true;
@ -1085,8 +1086,8 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
if (!inhibitWindStates) {
const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout;
if (isDragFusionDeadReckoning) {
// when dead reckoning using drag fusion stop learning wind states to provide a more stable velocity estimate
treatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable;
if (treatWindStatesAsTruth) {
P[23][23] = P[22][22] = 0.0f;
} else {
ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate)));
@ -1946,7 +1947,11 @@ void NavEKF3_core::ConstrainVariances()
}
if (!inhibitWindStates) {
for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,WIND_VEL_VARIANCE_MAX);
if (treatWindStatesAsTruth) {
P[23][23] = P[22][22] = 0.0f;
} else {
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);

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1109,10 +1109,11 @@ private:
ftype hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
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 inhibitWindStates; // true when wind states and covariances should not be used
Vector2F windStateLastObs; // wind states from the last time wind states were constrained using observations (m/s)
bool windStateLastObsIsValid;
bool windStateIsObservable; // true when wind states are observable from measurements.
bool treatWindStatesAsTruth; // true when wind states should be used as a truth reference
bool windStatesAligned; // true when wind states have been aligned
bool inhibitMagStates; // true when magnetic field states are inactive
bool lastInhibitMagStates; // previous inhibitMagStates