Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-31 04:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Publish output observer tracking errors

Browse Source
This commit is contained in:
priseborough 2016-06-29 15:57:10 +10:00 committed by Andrew Tridgell
parent fa435d0323
commit 14bb4f4574
4 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -940,6 +940,15 @@ void NavEKF2::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posI
}
}
// publish output observer angular, velocity and position tracking error
void NavEKF2::getOutputTrackingError(int8_t instance, Vector3f &error) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getOutputTrackingError(error);
}
}
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void NavEKF2::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset)
{

3
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -174,6 +174,9 @@ public:
// An out of range instance (eg -1) returns data for the the primary instance
void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov);
// publish output observer angular, velocity and position tracking error
void getOutputTrackingError(int8_t instance, Vector3f &error) const;
// return the innovation consistency test ratios for the specified instance
// An out of range instance (eg -1) returns data for the the primary instance
void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset);

6
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -578,4 +578,10 @@ uint8_t NavEKF2_core::getFramesSincePredict(void) const
return framesSincePredict;
}
// publish output observer angular, velocity and position tracking error
void NavEKF2_core::getOutputTrackingError(Vector3f &error) const
{
error = outputTrackError;
}
#endif // HAL_CPU_CLASS

5
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -258,6 +258,9 @@ public:
// this is used by other instances to level load
uint8_t getFramesSincePredict(void) const;
// publish output observer angular, velocity and position tracking error
void getOutputTrackingError(Vector3f &error) const;
private:
// Reference to the global EKF frontend for parameters
NavEKF2 *frontend;
@ -790,6 +793,8 @@ private:
Vector3f delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)
Vector3f delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
Vector3f outputTrackError;
// variables used to calculate a vertical velocity that is kinematically consistent with the verical position
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
float posDown; // Down position state used in calculation of posDownRate