From 07e307b074176dfeff72659587c8df915896c9b7 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 11 Mar 2021 18:52:44 +0100 Subject: [PATCH] FieldSensorBiasEst: fix bias learning equation Also compensate the tuned learning gains for the new equation to keep the same tuning --- .../field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp | 2 +- .../FieldSensorBiasEstimatorTest.cpp | 2 +- src/modules/mag_bias_estimator/params.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp b/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp index c7e23cae89..4ebf5a9aaf 100644 --- a/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp +++ b/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp @@ -71,7 +71,7 @@ public: { const matrix::Vector3f field_pred = _field_prev + (-gyro % (_field_prev - _state_bias)) * dt; const matrix::Vector3f field_innov = field - field_pred; - _state_bias += _learning_gain * (-gyro % field_innov); + _state_bias += _learning_gain * (-gyro % field_innov) * dt; _field_prev = field; } diff --git a/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimatorTest.cpp b/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimatorTest.cpp index d53ebdbf70..49188bd390 100644 --- a/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimatorTest.cpp +++ b/src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimatorTest.cpp @@ -44,7 +44,7 @@ using namespace matrix; TEST(MagnetometerBiasEstimatorTest, constantZRotation) { FieldSensorBiasEstimator field_sensor_bias_estimator; - field_sensor_bias_estimator.setLearningGain(100.f); + field_sensor_bias_estimator.setLearningGain(10000.f); const Vector3f virtual_gyro = Vector3f(0.f, 0.f, 0.1f); Vector3f virtual_unbiased_mag = Vector3f(0.9f, 0.f, 1.79f); // taken from SITL jmavsim initialization const Vector3f virtual_bias(0.2f, -0.4f, 0.5f); diff --git a/src/modules/mag_bias_estimator/params.c b/src/modules/mag_bias_estimator/params.c index 9df4e565a3..8fb3e7bca3 100644 --- a/src/modules/mag_bias_estimator/params.c +++ b/src/modules/mag_bias_estimator/params.c @@ -55,4 +55,4 @@ PARAM_DEFINE_INT32(MBE_ENABLE, 1); * @decimal 1 * @group Magnetometer Bias Estimator */ -PARAM_DEFINE_FLOAT(MBE_LEARN_GAIN, 0.4f); +PARAM_DEFINE_FLOAT(MBE_LEARN_GAIN, 18.f);