EKF: consolidate covariance corrections

Combines the forced symmetry, variance limiting and zeroing of covariances for unwanted states in the one function.
This ensures a consistent correction is applied after every covariance prediction or correction.
This commit is contained in:
Paul Riseborough
2016-05-07 20:29:50 +10:00
parent 0c6a367e1b
commit 7f5669fb2d
7 changed files with 62 additions and 59 deletions
+3 -5
View File
@@ -307,13 +307,11 @@ private:
// reset height state of the ekf
void resetHeight();
void makeCovSymetrical();
// limit the diagonal of the covariance matrix
void limitCov();
void fixCovarianceErrors();
// make ekf covariance matrix symmetric
void makeSymmetrical();
// make ekf covariance matrix symmetric between a nominated state indexe range
void makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
// constrain the ekf states
void constrainStates();