From 2b3241470996d333d78fa9e78fea9b35fee7e18c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 12:13:40 +0200 Subject: [PATCH] estimator: Move symmetry force outside of non-related loop, improving efficiency and resolving a locals warning. --- .../estimator_23states.cpp | 22 +++++++++---------- 1 file 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 0894d60ae3..7f59683633 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]; } }