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

AP_NavEKF: Add accessor for yaw estimator velocity innovation length

Browse Source
This commit is contained in:
Paul Riseborough 2020-11-16 11:33:58 +11:00 committed by Andrew Tridgell
parent db86a5acc8
commit 78e10e99f5
2 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -642,6 +642,18 @@ bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance)
return true;
}
bool EKFGSF_yaw::getVelInnovLength(float &velInnovLength)
{
if (!vel_fuse_running) {
return false;
}
velInnovLength = 0.0f;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
velInnovLength += GSF.weights[mdl_idx] * sqrtf((sq(EKF[mdl_idx].innov[0]) + sq(EKF[mdl_idx].innov[1])));
}
return true;
}
void EKFGSF_yaw::setGyroBias(Vector3f &gyroBias)
{
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {

4
libraries/AP_NavEKF/EKFGSF_yaw.h
View File

@ -38,6 +38,10 @@ public:
// return false if yaw estimation is inactive
bool getYawData(float &yaw, float &yawVariance);
// get the length of the weighted average velocity innovation vector
// return false if not available
bool getVelInnovLength(float &velInnovLength);
private:
typedef float ftype;