diff options
Diffstat (limited to 'src/modules/ekf_att_pos_estimator')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 35 |
1 files changed, 25 insertions, 10 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5a6f879b8..068d665a5 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1268,7 +1268,7 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t i = 0; i < n_states; i++) { H_MAG[i] = 0.0f; } - uint8_t indexLimit; + unsigned indexLimit; // Perform sequential fusion of Magnetometer measurements. // This assumes that the errors in the different components are @@ -1281,11 +1281,11 @@ void AttPosEKF::FuseMagnetometer() // Limit range of states modified when on ground if(!onGround) { - indexLimit = 22; + indexLimit = n_states; } else { - indexLimit = 13; + indexLimit = 13 + 1; } // Sequential fusion of XYZ components to spread processing load across @@ -1483,7 +1483,7 @@ void AttPosEKF::FuseMagnetometer() if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) { // correct the state vector - for (uint8_t j= 0; j<=indexLimit; j++) + for (uint8_t j= 0; j < indexLimit; j++) { states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } @@ -1500,7 +1500,7 @@ void AttPosEKF::FuseMagnetometer() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations - for (uint8_t i = 0; i<=indexLimit; i++) + for (uint8_t i = 0; i < indexLimit; i++) { for (uint8_t j = 0; j <= 3; j++) { @@ -1522,9 +1522,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i<=indexLimit; i++) + for (uint8_t i = 0; i < indexLimit; i++) { - for (uint8_t j = 0; j<=indexLimit; j++) + for (uint8_t j = 0; j < indexLimit; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 3; k++) @@ -1541,9 +1541,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i <= indexLimit; i++) + for (uint8_t i = 0; i < indexLimit; i++) { - for (uint8_t j = 0; j <= indexLimit; j++) + for (uint8_t j = 0; j < indexLimit; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -2001,7 +2001,22 @@ void AttPosEKF::CovarianceInit() float AttPosEKF::ConstrainFloat(float val, float min, float max) { - return (val > max) ? max : ((val < min) ? min : val); + float ret; + if (val > max) { + ret = max; + ekf_debug("> max: %8.4f, val: %8.4f", min); + } else if (val < min) { + ret = min; + ekf_debug("< min: %8.4f, val: %8.4f", max); + } else { + ret = val; + } + + if (!isfinite) { + ekf_debug("constrain: non-finite!"); + } + + return ret; } void AttPosEKF::ConstrainVariances() |