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

AP_NavEKF: getYawData also provides number of clipping models 

In the case of the compass calibrator we do not want to use the GSF
result if any model is degenerate.  We've had a compass calibrate in
flight 180-degrees out from what it should have.
This commit is contained in:
Peter Barker 2022-05-10 11:31:36 +10:00 committed by Andrew Tridgell
parent 437b313544
commit ad4c29748e
2 changed files with 17 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -167,7 +167,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc)
if (!state_update_failed) {
// Calculate weighting for each model assuming a normal error distribution
const ftype min_weight = 1e-5f;
uint8_t n_clips = 0;
n_clips = 0;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
newWeight[mdl_idx] = gaussianDensity(mdl_idx) * GSF.weights[mdl_idx];
if (newWeight[mdl_idx] < min_weight) {
@ -625,13 +625,19 @@ Matrix3F EKFGSF_yaw::updateRotMat(const Matrix3F &R, const Vector3F &g) const
return ret;
}
bool EKFGSF_yaw::getYawData(ftype &yaw, ftype &yawVariance) const
// returns true if a yaw estimate is available. yaw and its variance
// is returned, as well as the number of models which are *not* being
// used to snthesise the yaw.
bool EKFGSF_yaw::getYawData(ftype &yaw, ftype &yawVariance, uint8_t *_n_clips) const
{
if (!vel_fuse_running) {
return false;
}
yaw = GSF.yaw;
yawVariance = GSF.yaw_variance;
if (_n_clips != nullptr) {
*_n_clips = n_clips;
}
return true;
}

12
libraries/AP_NavEKF/EKFGSF_yaw.h
View File

@ -31,9 +31,11 @@ public:
// set the gyro bias in rad/sec
void setGyroBias(Vector3f &gyroBias);
// get yaw estimated and corresponding variance
// return false if yaw estimation is inactive
bool getYawData(ftype &yaw, ftype &yawVariance) const;
// get yaw estimated and corresponding variance return false if
// yaw estimation is inactive. n_clips will contain the number of
// models which were *not* used to create the yaw and yawVariance
// return values.
bool getYawData(ftype &yaw, ftype &yawVariance, uint8_t *n_clips=nullptr) const;
// get the length of the weighted average velocity innovation vector
// return false if not available
@ -136,4 +138,8 @@ private:
// Returns the probability for a selected model assuming a Gaussian error distribution
// Used by the Guassian Sum Filter to calculate the weightings when combining the outputs from the bank of EKF's
ftype gaussianDensity(const uint8_t mdl_idx) const;
// number of models whose weights underflowed due to excessive
// innovation variances:
uint8_t n_clips;
};