fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Add method to set diagonals in covariance matrix

Browse Source
This commit is contained in:
Paul Riseborough 2017-11-09 17:49:32 +11:00
parent 32de90b9ef
commit 141264fe63
2 changed files with 19 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/ekf.h
View File

@ -596,9 +596,13 @@ 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
// zero the specified range of off diagonals in the state covariance matrix
void zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
// zero the specified range of off diagonals in the state covariance matrix
// set the diagonals to the supplied value
void setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance);
// calculate the measurement variance for the optical flow sensor
float calcOptFlowMeasVar();

14
EKF/ekf_helper.cpp
View File

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