FieldSensorBiasEst: fix bias learning equation

Also compensate the tuned learning gains for the new equation to keep
the same tuning

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

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

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