From 5693a9d3b6e923b2a2c05594f10cb5366ee6c495 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Mar 2014 18:28:13 +0100 Subject: new fixed wing estimator: Fix the symmetry force step of the covariance prediction. --- src/modules/fw_att_pos_estimator/estimator.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) (limited to 'src/modules/fw_att_pos_estimator/estimator.cpp') diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 36b9f4491..e6f1fee87 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -876,7 +876,7 @@ void CovariancePrediction(float dt) nextP[20][19] = P[20][19]; nextP[20][20] = P[20][20]; - for (uint8_t i=0; i<= 20; i++) + for (unsigned i = 0; i < n_states; i++) { nextP[i][i] = nextP[i][i] + processNoise[i]; } @@ -942,7 +942,16 @@ void CovariancePrediction(float dt) P[i][i] = nextP[i][i]; } - ForceSymmetry(); + // force symmetry for observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; + } + } + } ConstrainVariances(); -- cgit v1.2.3