mbe: improve algorithm and fix unit test

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{};
 };

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);
 }