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

AP_AHRS: add get_innovations

This commit is contained in:
Randy Mackay 2019-10-05 10:04:00 +09:00 committed by Andrew Tridgell
parent a2eb1eddc2
commit 27bde0f2db
3 changed files with 42 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_AHRS/AP_AHRS.h
View File

@ -528,6 +528,12 @@ public:
return false;
}
// return the innovations for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
virtual bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const {
return false;
}
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
// indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accepted by the filter

32
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1643,6 +1643,38 @@ bool AP_AHRS_NavEKF::get_location(struct Location &loc) const
}
}
// return the innovations for the primariy EKF
// boolean false is returned if innovations are not available
bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
// We are not using an EKF so no data
return false;
case EKF_TYPE2:
default:
// use EKF to get innovations
EKF2.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov);
return true;
case EKF_TYPE3:
// use EKF to get innovations
EKF3.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov);
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
velInnov.zero();
posInnov.zero();
magInnov.zero();
tasInnov = 0.0f;
yawInnov = 0.0f;
return true;
#endif
}
}
// 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

4
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -230,6 +230,10 @@ public:
// position), false on failure
bool get_location(struct Location &loc) const;
// return the innovations for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override;
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
// indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accepted by the filter