dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FieldSensorBiasEst: fix bias learning equation 

Also compensate the tuned learning gains for the new equation to keep
the same tuning
This commit is contained in:
bresch 2021-03-11 18:52:44 +01:00 committed by Daniel Agar
parent e0bcc17f5b
commit 07e307b074
3 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp
View File

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

2
src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimatorTest.cpp
View File

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

2
src/modules/mag_bias_estimator/params.c
View File

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