From 76c2d92b7e9cc0291926baeb4ae6b5e198a04de0 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 7 Sep 2016 23:53:50 +0000 Subject: [PATCH] Fix for lpe accel bias saturation. (#5466) * Fix for lpe accel bias saturation. * Formatting. --- .../BlockLocalPositionEstimator.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index cdbf7e99f8..66556eae57 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -607,11 +607,20 @@ void BlockLocalPositionEstimator::correctionLogic(Vector &dx) float by = dx(X_by) + _x(X_by); float bz = dx(X_bz) + _x(X_bz); - if (std::abs(bx) > BIAS_MAX) { bx = BIAS_MAX * bx / std::abs(bx); } + if (std::abs(bx) > BIAS_MAX) { + bx = BIAS_MAX * bx / std::abs(bx); + dx(X_bx) = bx - _x(X_bx); + } - if (std::abs(by) > BIAS_MAX) { by = BIAS_MAX * by / std::abs(by); } + if (std::abs(by) > BIAS_MAX) { + by = BIAS_MAX * by / std::abs(by); + dx(X_by) = by - _x(X_by); + } - if (std::abs(bz) > BIAS_MAX) { bz = BIAS_MAX * bz / std::abs(bz); } + if (std::abs(bz) > BIAS_MAX) { + bz = BIAS_MAX * bz / std::abs(bz); + dx(X_bz) = bz - _x(X_bz); + } }