dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Add method to zero covariance terms

This commit is contained in:
Paul Riseborough 2017-11-09 17:37:45 +11:00
parent f3e34eddc9
commit 32de90b9ef
2 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/ekf.h
View File

@ -596,6 +596,9 @@ private:
// zero the specified range of columns in the state covariance matrix
void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
// zero the specified range of off diagonals the state covariance matrix
void zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
// calculate the measurement variance for the optical flow sensor
float calcOptFlowMeasVar();

20
EKF/ekf_helper.cpp
View File

@ -1207,6 +1207,26 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first
}
}
void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
{
// store diagonals
uint8_t row;
float variances[last-first+1];
for (row = first; row <= last; row++) {
variances[row-first] = cov_mat[row][row];
}
// zero rows and columns
zeroRows(cov_mat, first, last);
zeroCols(cov_mat, first, last);
// restore diagonals
for (row = first; row <= last; row++) {
cov_mat[row][row] = variances[row-first];
}
}
bool Ekf::global_position_is_valid()
{
// return true if we are not doing unconstrained free inertial navigation and the origin is set