diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-29 12:13:40 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-29 12:13:40 +0200 |
commit | 2b3241470996d333d78fa9e78fea9b35fee7e18c (patch) | |
tree | cd4d0c520c7bf43e66a12930ecf8eec2ac371eea | |
parent | d93a64dd09a34515e4d9812408a530f3c9721a46 (diff) | |
download | px4-firmware-2b3241470996d333d78fa9e78fea9b35fee7e18c.tar.gz px4-firmware-2b3241470996d333d78fa9e78fea9b35fee7e18c.tar.bz2 px4-firmware-2b3241470996d333d78fa9e78fea9b35fee7e18c.zip |
estimator: Move symmetry force outside of non-related loop, improving efficiency and resolving a locals warning.
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 22 |
1 files changed, 11 insertions, 11 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 0894d60ae..7f5968363 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -887,20 +887,20 @@ void AttPosEKF::CovariancePrediction(float dt) // propagate for (unsigned i = 0; i <= 13; i++) { P[i][i] = nextP[i][i]; + } - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 13) || (j > 13)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; + if ((i > 13) || (j > 13)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); } + P[j][i] = P[i][j]; } } |