Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Add public accessor for state variances

Browse Source
This commit is contained in:
priseborough 2017-05-21 11:25:06 +10:00 committed by Francisco Ferreira
parent 9f97cbfc3b
commit 2b97d0f5c9
4 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1087,6 +1087,15 @@ void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float
}
}
// return the diagonals from the covariance matrix for the specified instance
void NavEKF3::getStateVariances(int8_t instance, float stateVar[24])
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getStateVariances(stateVar);
}
}
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()
bool NavEKF3::use_compass(void) const

3
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -183,6 +183,9 @@ public:
// 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);
// return the diagonals from the covariance matrix for the specified instance
void getStateVariances(int8_t instance, float stateVar[24]);
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()
bool use_compass(void) const;

7
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -451,6 +451,13 @@ void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve
offset = posResetNE;
}
// return the diagonals from the covariance matrix
void NavEKF3_core::getStateVariances(float stateVar[24])
{
for (uint8_t i=0; i<24; i++) {
stateVar[i] = P[i][i];
}
}
/*
return the filter fault status as a bitmasked integer

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -189,6 +189,9 @@ public:
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
// return the diagonals from the covariance matrix
void getStateVariances(float stateVar[24]);
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()
bool use_compass(void) const;