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

AP_AHRS: Add EKF variance checks

This commit is contained in:
Paul Riseborough 2015-10-17 16:49:50 +11:00 committed by Andrew Tridgell
parent 242b9641c8
commit 0722ebe8a0
2 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -839,5 +839,30 @@ bool AP_AHRS_NavEKF::get_location(struct Location &loc) const
}
}
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
// indicates prefect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accpeted by the filter
// boolean false is returned if variances are not available
bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
// We are not using an EKF so no data
return false;
case EKF_TYPE1:
default:
// use EKF to get variance
EKF1.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
return true;
case EKF_TYPE2:
// use EKF to get variance
EKF2.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
return true;
}
}
#endif // AP_AHRS_NAVEKF_AVAILABLE

6
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -179,6 +179,12 @@ public:
// returns true on success (i.e. the EKF knows it's latest position), false on failure
bool get_location(struct Location &loc) const;
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
// indicates prefect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accpeted by the filter
// boolean false is returned if variances are not available
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
private:
enum EKF_TYPE {EKF_TYPE_NONE, EKF_TYPE1, EKF_TYPE2};
EKF_TYPE active_EKF_type(void) const;