zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF: Fix one frame delay in processing yaw estimator velocity data

c415-sdk
Paul Riseborough 5 years ago committed by Peter Barker
parent
ed14ab84ce
commit
0ce4dd457d
2 changed files with 52 additions and 56 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 101
      libraries/AP_NavEKF/EKFGSF_yaw.cpp
  2. 7
      libraries/AP_NavEKF/EKFGSF_yaw.h

101
libraries/AP_NavEKF/EKFGSF_yaw.cpp
Unescape View File

@ -98,47 +98,7 @@ void EKFGSF_yaw::update(const Vector3f &delAng, @@ -98,47 +98,7 @@ void EKFGSF_yaw::update(const Vector3f &delAng,
predict(mdl_idx);
}
// The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference
if (vel_data_updated && run_ekf_gsf) {
if (!vel_fuse_running) {
// Perform in-flight alignment
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].P[0][0] = sq(fmaxf(vel_accuracy, 0.5f));
EKF[mdl_idx].P[1][1] = EKF[mdl_idx].P[0][0];
}
alignYaw();
vel_fuse_running = true;
} else {
float total_w = 0.0f;
float newWeight[(uint8_t)N_MODELS_EKFGSF];
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)) {
state_update_failed = true;
}
}
if (!state_update_failed) {
// Calculate weighting for each model assuming a normal error distribution
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
newWeight[mdl_idx]= fmaxf(gaussianDensity(mdl_idx) * GSF.weights[mdl_idx], 0.0f);
total_w += newWeight[mdl_idx];
}
// Normalise the sum of weights to unity
if (vel_fuse_running && is_positive(total_w)) {
float total_w_inv = 1.0f / total_w;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
GSF.weights[mdl_idx] = newWeight[mdl_idx] * total_w_inv;
}
}
}
}
} else if (vel_fuse_running && !run_ekf_gsf) {
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();
@ -176,16 +136,55 @@ void EKFGSF_yaw::update(const Vector3f &delAng, @@ -176,16 +136,55 @@ void EKFGSF_yaw::update(const Vector3f &delAng,
float yawDelta = wrap_PI(EKF[mdl_idx].X[2] - GSF.yaw);
GSF.yaw_variance += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[2][2] + sq(yawDelta));
}
// prevent the same velocity data being used again
vel_data_updated = false;
}
void EKFGSF_yaw::pushVelData(Vector2f vel, float velAcc)
void EKFGSF_yaw::fuseVelData(Vector2f vel, float velAcc)
{
// set class variables
velObsVar = sq(fmaxf(velAcc, 0.5f));
vel_NE = vel;
vel_accuracy = velAcc;
vel_data_updated = true;
// 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
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].P[0][0] = velObsVar;
EKF[mdl_idx].P[1][1] = velObsVar;
}
alignYaw();
vel_fuse_running = true;
} else {
float total_w = 0.0f;
float newWeight[(uint8_t)N_MODELS_EKFGSF];
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)) {
state_update_failed = true;
}
}
if (!state_update_failed) {
// Calculate weighting for each model assuming a normal error distribution
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
newWeight[mdl_idx]= fmaxf(gaussianDensity(mdl_idx) * GSF.weights[mdl_idx], 0.0f);
total_w += newWeight[mdl_idx];
}
// Normalise the sum of weights to unity
if (vel_fuse_running && is_positive(total_w)) {
float total_w_inv = 1.0f / total_w;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
GSF.weights[mdl_idx] = newWeight[mdl_idx] * total_w_inv;
}
}
}
}
}
}
void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx)
@ -386,9 +385,6 @@ void EKFGSF_yaw::predict(const uint8_t mdl_idx) @@ -386,9 +385,6 @@ void EKFGSF_yaw::predict(const uint8_t mdl_idx)
// Returns false if the sttae and covariance correction failed
bool EKFGSF_yaw::correct(const uint8_t mdl_idx)
{
// set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum
const float velObsVar = sq(fmaxf(vel_accuracy, 0.5f));
// 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];
@ -539,7 +535,6 @@ void EKFGSF_yaw::initialise() @@ -539,7 +535,6 @@ void EKFGSF_yaw::initialise()
{
memset(&GSF, 0, sizeof(GSF));
vel_fuse_running = false;
vel_data_updated = false;
run_ekf_gsf = false;
memset(&EKF, 0, sizeof(EKF));
@ -554,8 +549,8 @@ void EKFGSF_yaw::initialise() @@ -554,8 +549,8 @@ void EKFGSF_yaw::initialise()
// 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] = sq(fmaxf(vel_accuracy, 0.5f));
EKF[mdl_idx].P[1][1] = EKF[mdl_idx].P[0][0];
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);

7
libraries/AP_NavEKF/EKFGSF_yaw.h
Unescape View File

@ -22,7 +22,9 @@ public: @@ -22,7 +22,9 @@ public:
bool runEKF, // set to true when flying or movement suitable for yaw estimation
float TAS); // true airspeed used for centripetal accel compensation - set to 0 when not required.
void pushVelData(Vector2f vel, // NE velocity measurement (m/s)
// Fuse NE velocty mesurements and update the EKF's and GSF state and covariance estimates
// Should be called after update(...) whenever new velocity data is available
void fuseVelData(Vector2f vel, // NE velocity measurement (m/s)
float velAcc); // 1-sigma accuracy of velocity measurement (m/s)
// get solution data for logging
@ -100,10 +102,9 @@ private: @@ -100,10 +102,9 @@ 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 vel_data_updated; // true when velocity data has been updated
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 vel_accuracy; // 1-sigma accuracy of 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();

Write Preview
Loading…
Cancel
Save