From 9067d9cae984c571b93310e322e94ee55fcc9889 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 17 Dec 2020 15:19:12 +0100 Subject: [PATCH] mbe: improve algorithm and fix unit test --- .../FieldSensorBiasEstimator.hpp | 16 ++++++++-------- .../FieldSensorBiasEstimatorTest.cpp | 13 +++++++------ 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimator.hpp b/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimator.hpp index a054deabd5..fb75453a7f 100644 --- a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimator.hpp +++ b/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimator.hpp @@ -57,7 +57,7 @@ public: ~FieldSensorBiasEstimator() = default; // Set initial states - void setField(const matrix::Vector3f &field) { _state_field = field; } + void setField(const matrix::Vector3f &field) { _field_prev = field; } void setBias(const matrix::Vector3f &bias) { _state_bias = bias; } /** @@ -68,20 +68,20 @@ public: */ void updateEstimate(const matrix::Vector3f &gyro, const matrix::Vector3f &field, const float dt) { - const matrix::Vector3f state_field_prev = _state_field; - _state_field += (-gyro % (_state_field - _state_bias)) * dt + GAIN_FIELD * (field - _state_field); - _state_bias += GAIN_BIAS * (gyro % (state_field_prev - field)); + const matrix::Vector3f field_pred = _field_prev + (-gyro % (_field_prev - _state_bias)) * dt; + const matrix::Vector3f field_innov = field - field_pred; + _state_bias += GAIN_BIAS * (-gyro % field_innov); + _field_prev = field; } - const matrix::Vector3f &getField() { return _state_field; } + const matrix::Vector3f &getField() { return _field_prev; } const matrix::Vector3f &getBias() { return _state_bias; } private: // gains - static constexpr float GAIN_FIELD = 1.f; - static constexpr float GAIN_BIAS = 0.05f; + static constexpr float GAIN_BIAS = 100.f; // states - matrix::Vector3f _state_field{}; + matrix::Vector3f _field_prev{}; matrix::Vector3f _state_bias{}; }; diff --git a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimatorTest.cpp b/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimatorTest.cpp index 048fa3a734..5f916dcef2 100644 --- a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimatorTest.cpp +++ b/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimatorTest.cpp @@ -41,13 +41,13 @@ TEST(MagnetometerBiasEstimatorTest, AllZeroCase) FieldSensorBiasEstimator field_sensor_bias_estimator; 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); + const Vector3f virtual_bias(0.2f, -0.4f, 0.5f); Vector3f virtual_mag = virtual_unbiased_mag + virtual_bias; // field_sensor_bias_estimator.setField(virtual_mag); // field_sensor_bias_estimator.setBias(virtual_bias); - for (int i = 0; i <= 10000000; i++) { + for (int i = 0; i <= 1000; i++) { float dt = .01f; // turn the mag values according to the gyro @@ -61,8 +61,9 @@ TEST(MagnetometerBiasEstimatorTest, AllZeroCase) virtual_unbiased_mag = Dcmf(AxisAnglef(-virtual_gyro * dt)) * virtual_unbiased_mag; } - field_sensor_bias_estimator.getBias().print(); - // (magnetometer_bias_estimator._state_bias - virtual_bias).print(); - //EXPECT_EQ(field_sensor_bias_estimator.getBias(), virtual_bias); - //EXPECT_TRUE(false); + const Vector3f bias_est = field_sensor_bias_estimator.getBias(); + EXPECT_NEAR(bias_est(0), virtual_bias(0), 1e-3f) << "Estimated X bias " << bias_est(0); + EXPECT_NEAR(bias_est(1), virtual_bias(1), 1e-3f) << "Estimated Y bias " << bias_est(1); + // The Z bias is not observable due to pure yaw rotation + EXPECT_NEAR(bias_est(2), 0.f, 1e-3f) << "Estimated Z bias " << bias_est(2); }