diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-23 18:28:13 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-23 18:28:13 +0100 |
commit | 5693a9d3b6e923b2a2c05594f10cb5366ee6c495 (patch) | |
tree | ca81d3ec4e570d2823ba256a4d0419fb518b0d24 /src | |
parent | a0ceeee9ef9988fa470524462e3097e52c63ed8f (diff) | |
download | px4-firmware-5693a9d3b6e923b2a2c05594f10cb5366ee6c495.tar.gz px4-firmware-5693a9d3b6e923b2a2c05594f10cb5366ee6c495.tar.bz2 px4-firmware-5693a9d3b6e923b2a2c05594f10cb5366ee6c495.zip |
new fixed wing estimator: Fix the symmetry force step of the covariance prediction.
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.cpp | 13 |
1 files changed, 11 insertions, 2 deletions
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(); |