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

AP_NavEKF: Remove unnecessary reset operations and class variables 

The setting of EKF state variances is only required when commencing or recommencing velocity fusion.
The function that resets the EKF and GSF class variables has been renamed to be more consistent with its function.
This commit is contained in:
Paul Riseborough 2020-04-25 09:45:28 +10:00 committed by Peter Barker
parent b6d9b6f3c7
commit 4a743a3827
2 changed files with 13 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -58,7 +58,6 @@ void EKFGSF_yaw::update(const Vector3f &delAng,
const bool ok_to_align = ((accel_norm_sq > lower_accel_limit * lower_accel_limit &&
accel_norm_sq < upper_accel_limit * upper_accel_limit));
if (ok_to_align) {
initialise();
alignTilt();
ahrs_tilt_aligned = true;
ahrs_accel = accel;
@ -99,9 +98,6 @@ void EKFGSF_yaw::update(const Vector3f &delAng,
}
if (vel_fuse_running && !run_ekf_gsf) {
// We have landed so reset EKF-GSF states and wait to fly again
// Do not reset the AHRS as it continues to run when on ground
initialise();
vel_fuse_running = false;
}
@ -140,18 +136,18 @@ void EKFGSF_yaw::update(const Vector3f &delAng,
void EKFGSF_yaw::fuseVelData(Vector2f vel, float velAcc)
{
// set class variables
velObsVar = sq(fmaxf(velAcc, 0.5f));
vel_NE = vel;
// convert reported accuracy to a variance, but limit lower value to protect algorithm stability
const float velObsVar = sq(fmaxf(velAcc, 0.5f));
// The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference
if (run_ekf_gsf) {
if (!vel_fuse_running) {
// Perform in-flight alignment
resetEKFGSF();
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
// Use the firstGPS measurement to set the velocities and corresponding variances
EKF[mdl_idx].X[0] = vel_NE[0];
EKF[mdl_idx].X[1] = vel_NE[1];
EKF[mdl_idx].X[0] = vel[0];
EKF[mdl_idx].X[1] = vel[1];
EKF[mdl_idx].P[0][0] = velObsVar;
EKF[mdl_idx].P[1][1] = velObsVar;
}
@ -163,7 +159,7 @@ void EKFGSF_yaw::fuseVelData(Vector2f vel, float velAcc)
bool state_update_failed = false;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
// Update states and covariances using GPS NE velocity measurements fused as direct state observations
if (!correct(mdl_idx)) {
if (!correct(mdl_idx, vel, velObsVar)) {
state_update_failed = true;
}
}
@ -383,11 +379,11 @@ void EKFGSF_yaw::predict(const uint8_t mdl_idx)
// Update EKF states and covariance for specified model index using velocity measurement
// Returns false if the sttae and covariance correction failed
bool EKFGSF_yaw::correct(const uint8_t mdl_idx)
bool EKFGSF_yaw::correct(const uint8_t mdl_idx, const Vector2f &vel, const float velObsVar)
{
// calculate velocity observation innovations
EKF[mdl_idx].innov[0] = EKF[mdl_idx].X[0] - vel_NE[0];
EKF[mdl_idx].innov[1] = EKF[mdl_idx].X[1] - vel_NE[1];
EKF[mdl_idx].innov[0] = EKF[mdl_idx].X[0] - vel[0];
EKF[mdl_idx].innov[1] = EKF[mdl_idx].X[1] - vel[1];
// copy covariance matrix to temporary variables
const float P00 = EKF[mdl_idx].P[0][0];
@ -531,7 +527,7 @@ bool EKFGSF_yaw::correct(const uint8_t mdl_idx)
return true;
}
void EKFGSF_yaw::initialise()
void EKFGSF_yaw::resetEKFGSF()
{
memset(&GSF, 0, sizeof(GSF));
vel_fuse_running = false;
@ -546,12 +542,6 @@ void EKFGSF_yaw::initialise()
// All filter models start with the same weight
GSF.weights[mdl_idx] = 1.0f / (float)N_MODELS_EKFGSF;
// Take state and variance estimates direct from velocity sensor
EKF[mdl_idx].X[0] = vel_NE[0];
EKF[mdl_idx].X[1] = vel_NE[1];
EKF[mdl_idx].P[0][0] = velObsVar;
EKF[mdl_idx].P[1][1] = velObsVar;
// Use half yaw interval for yaw uncertainty as that is the maximum that the best model can be away from truth
GSF.yaw_variance = sq(0.5f * yaw_increment);
EKF[mdl_idx].P[2][2] = GSF.yaw_variance;

8
libraries/AP_NavEKF/EKFGSF_yaw.h
View File

@ -103,18 +103,16 @@ private:
EKF_struct EKF[N_MODELS_EKFGSF];
bool vel_fuse_running; // true when the bank of EKF's has started fusing GPS velocity data
bool run_ekf_gsf; // true when operating condition is suitable for to run the GSF and EKF models and fuse velocity data
Vector2f vel_NE; // NE velocity observations (m/s)
float velObsVar; // variance of velocity observations (m/s)^2
// Initialises the EKF's and GSF states, but not the AHRS complementary filters
void initialise();
// Resets states and covariances for the EKF's and GSF including GSF weights, but not the AHRS complementary filters
void resetEKFGSF();
// Runs the state and covariance prediction for the selected EKF
void predict(const uint8_t mdl_idx);
// Runs the state and covariance update for the selected EKF using the GPS NE velocity measurement
// Returns false if the sttae and covariance correction failed
bool correct(const uint8_t mdl_idx);
bool correct(const uint8_t mdl_idx, const Vector2f &vel, const float velObsVar);
// Forces symmetry on the covariance matrix for the selected EKF
void forceSymmetry(const uint8_t mdl_idx);