From 8f020d5a8f940f716bf5af8728f20cd678719722 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 3 Feb 2016 17:07:42 +1100 Subject: [PATCH] ekf2: Update tuning parameters Set conservative defaults as a baseline for tuning Add a missing parameter for magnetometer observation noise. Correct error in definition of magnetic heading observations noise (previous parameter defined the variance directly, not the noise). Update documentation and display names for consistency. --- src/modules/ekf2/ekf2_main.cpp | 23 +++--- src/modules/ekf2/ekf2_params.c | 127 +++++++++++++++++++-------------- 2 files changed, 86 insertions(+), 64 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 8e6d17da5f..bc36e01769 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -170,7 +170,8 @@ private: control::BlockParamFloat *_vel_innov_gate; // innovation gate for GPS velocity innovation test (std dev) control::BlockParamFloat *_mag_heading_noise; // measurement noise used for simple heading fusion - control::BlockParamFloat *_mag_declination_deg; // magnetic declination in degrees + control::BlockParamFloat *_mag_noise; // measurement noise used for 3-axis magnetoemter fusion (Gauss) + control::BlockParamFloat *_mag_declination_deg; // magnetic declination in degrees control::BlockParamFloat *_heading_innov_gate; // innovation gate for heading innovation test control::BlockParamFloat *_mag_innov_gate; // innovation gate for magnetometer innovation test @@ -201,15 +202,15 @@ Ekf2::Ekf2(): _ekf(new Ekf()), _params(_ekf->getParamHandle()), _mag_delay_ms(new control::BlockParamFloat(this, "EKF2_MAG_DELAY", false, &_params->mag_delay_ms)), - _baro_delay_ms(new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, &_params->baro_delay_ms)), + _baro_delay_ms(new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, &_params->baro_delay_ms)), _gps_delay_ms(new control::BlockParamFloat(this, "EKF2_GPS_DELAY", false, &_params->gps_delay_ms)), _airspeed_delay_ms(new control::BlockParamFloat(this, "EKF2_ASP_DELAY", false, &_params->airspeed_delay_ms)), - _gyro_noise(new control::BlockParamFloat(this, "EKF2_G_NOISE", false, &_params->gyro_noise)), - _accel_noise(new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, &_params->accel_noise)), - _gyro_bias_p_noise(new control::BlockParamFloat(this, "EKF2_GB_NOISE", false, &_params->gyro_bias_p_noise)), - _accel_bias_p_noise(new control::BlockParamFloat(this, "EKF2_ACCB_NOISE", false, &_params->accel_bias_p_noise)), - _gyro_scale_p_noise(new control::BlockParamFloat(this, "EKF2_GS_NOISE", false, &_params->gyro_scale_p_noise)), - _mag_p_noise(new control::BlockParamFloat(this, "EKF2_MAG_NOISE", false, &_params->mag_p_noise)), + _gyro_noise(new control::BlockParamFloat(this, "EKF2_GYR_NOISE", false, &_params->gyro_noise)), + _accel_noise(new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, &_params->accel_noise)), + _gyro_bias_p_noise(new control::BlockParamFloat(this, "EKF2_GYR_B_NOISE", false, &_params->gyro_bias_p_noise)), + _accel_bias_p_noise(new control::BlockParamFloat(this, "EKF2_ACC_B_NOISE", false, &_params->accel_bias_p_noise)), + _gyro_scale_p_noise(new control::BlockParamFloat(this, "EKF2_GYR_S_NOISE", false, &_params->gyro_scale_p_noise)), + _mag_p_noise(new control::BlockParamFloat(this, "EKF2_MAG_B_NOISE", false, &_params->mag_p_noise)), _wind_vel_p_noise(new control::BlockParamFloat(this, "EKF2_WIND_NOISE", false, &_params->wind_vel_p_noise)), _gps_vel_noise(new control::BlockParamFloat(this, "EKF2_GPS_V_NOISE", false, &_params->gps_vel_noise)), _gps_pos_noise(new control::BlockParamFloat(this, "EKF2_GPS_P_NOISE", false, &_params->gps_pos_noise)), @@ -218,7 +219,8 @@ Ekf2::Ekf2(): _posNE_innov_gate(new control::BlockParamFloat(this, "EKF2_GPS_P_GATE", false, &_params->posNE_innov_gate)), _vel_innov_gate(new control::BlockParamFloat(this, "EKF2_GPS_V_GATE", false, &_params->vel_innov_gate)), _mag_heading_noise(new control::BlockParamFloat(this, "EKF2_HEAD_NOISE", false, &_params->mag_heading_noise)), - _mag_declination_deg(new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, &_params->mag_declination_deg)), + _mag_noise(new control::BlockParamFloat(this, "EKF2_MAG_NOISE", false, &_params->mag_noise)), + _mag_declination_deg(new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, &_params->mag_declination_deg)), _heading_innov_gate(new control::BlockParamFloat(this, "EKF2_HDG_GATE", false, &_params->heading_innov_gate)), _mag_innov_gate(new control::BlockParamFloat(this, "EKF2_MAG_GATE", false, &_params->mag_innov_gate)), _gps_check_mask(new control::BlockParamInt(this, "EKF2_GPS_CHECK", false, &_params->gps_check_mask)), @@ -228,8 +230,7 @@ Ekf2::Ekf2(): _requiredNsats(new control::BlockParamInt(this, "EKF2_REQ_NSATS", false, &_params->req_nsats)), _requiredGDoP(new control::BlockParamFloat(this, "EKF2_REQ_GDOP", false, &_params->req_gdop)), _requiredHdrift(new control::BlockParamFloat(this, "EKF2_REQ_HDRIFT", false, &_params->req_hdrift)), - _requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift)) - + _requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift)) { } diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 30912b055e..bd033980ed 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -42,7 +42,7 @@ #include /** -* Magnetometer measurement delay +* Magnetometer measurement delay relative to IMU measurements * * @group EKF2 * @min 0 @@ -52,7 +52,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_DELAY, 0); /** - * Barometer measurement delay + * Barometer measurement delay relative to IMU measurements * * @group EKF2 * @min 0 @@ -62,7 +62,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_DELAY, 0); PARAM_DEFINE_FLOAT(EKF2_BARO_DELAY, 0); /** - * GPS measurement delay + * GPS measurement delay relative to IMU measurements * * @group EKF2 * @min 0 @@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(EKF2_BARO_DELAY, 0); PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 200); /** - * Airspeed measurement delay + * Airspeed measurement delay relative to IMU measurements * * @group EKF2 * @min 0 @@ -171,113 +171,124 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_HDRIFT, 0.3f); PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.5f); /** - * Gyro noise. + * Rate gyro noise for covariance prediction. * * @group EKF2 * @min 0.0001 - * @max 0.05 + * @max 0.01 + * @unit rad/s */ -PARAM_DEFINE_FLOAT(EKF2_G_NOISE, 0.001f); +PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 1.0e-3f); /** - * Process noise for delta velocity prediction. + * Accelerometer noise for covariance prediction. * * @group EKF2 * @min 0.01 - * @max 1 - * @unit + * @max 1.0 + * @unit m/s/s */ -PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 0.1f); +PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 0.25f); /** * Process noise for delta angle bias prediction. * * @group EKF2 - * @min 0 + * @min 0.0 * @max 0.0001 - * @unit + * @unit rad/s */ -PARAM_DEFINE_FLOAT(EKF2_GB_NOISE, 1e-5f); +PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 7.0e-5f); /** * Process noise for delta velocity z bias prediction. * * @group EKF2 - * @min 0.000001 + * @min 0.0 * @max 0.01 - * @unit + * @unit m/s/s */ -PARAM_DEFINE_FLOAT(EKF2_ACCB_NOISE, 1e-3f); +PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 1.0e-4f); /** - * Process noise for delta angle scale prediction. + * Process noise for delta angle scale factor prediction. * * @group EKF2 - * @min 0.000001 + * @min 0.0 * @max 0.01 - * @unit + * @unit None */ -PARAM_DEFINE_FLOAT(EKF2_GS_NOISE, 1e-4f); +PARAM_DEFINE_FLOAT(EKF2_GYR_S_NOISE, 3.0e-3f); /** - * Process noise for earth magnetic field and bias prediction. + * Process noise for sensor bias and earth magnetic field prediction. * * @group EKF2 - * @min 0.0001 + * @min 0.0 * @max 0.1 - * @unit + * @unit Gauss/s */ -PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 1e-2f); +PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 2.5e-2f); /** * Process noise for wind velocity prediction. * * @group EKF2 - * @min 0.01 - * @max 1 - * @unit + * @min 0.0 + * @max 1.0 + * @unit m/s/s */ -PARAM_DEFINE_FLOAT(EKF2_WIND_NOISE, 0.05f); +PARAM_DEFINE_FLOAT(EKF2_WIND_NOISE, 1.0e-1f); /** - * Measurement noise for gps velocity. + * Measurement noise for gps horizontal velocity. * * @group EKF2 - * @min 0.001 - * @max 0.5 - * @unit + * @min 0.01 + * @max 5.0 + * @unit m/s */ -PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.05f); +PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.5f); /** * Measurement noise for gps position. * * @group EKF2 * @min 0.01 - * @max 5 - * @unit + * @max 10.0 + * @unit m */ PARAM_DEFINE_FLOAT(EKF2_GPS_P_NOISE, 1.0f); /** - * Measurement noise for barometer. + * Measurement noise for barometric altitude. * * @group EKF2 - * @min 0.001 - * @max 1 - * @unit + * @min 0.01 + * @max 15.0 + * @unit m */ -PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 0.1f); +PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 3.0f); /** - * Measurement noise for mag heading fusion. + * Measurement noise for magnetic heading fusion. * * @group EKF2 - * @min 0.0001 - * @max 0.1 - * @unit + * @min 0.01 + * @max 1.0 + * @unit rad + */ +PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 0.17f); + +/** + * Measurement noise for magnetometer 3-axis fusion. + * + * @group EKF2 + * @min 0.001 + * @max 1.0 + * @unit Gauss */ -PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 3e-2f); +PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 5.0e-2f); /** * Magnetic declination @@ -288,36 +299,46 @@ PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 3e-2f); PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0); /** - * Gate size for magnetic heading fusion (standard deviations) + * Gate size for magnetic heading fusion * * @group EKF2 + * @min 1.0 + * @unit SD */ PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 3.0f); /** - * Gate size for magnetometer XYZ component fusion (standard deviations) + * Gate size for magnetometer XYZ component fusion * * @group EKF2 + * @min 1.0 + * @unit SD */ PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f); /** - * Gate size for barometric height fusion (standard deviations) + * Gate size for barometric height fusion * * @group EKF2 + * @min 1.0 + * @unit SD */ -PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 5.0f); +PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 3.0f); /** - * Gate size for GPS horizontal position fusion (standard deviations) + * Gate size for GPS horizontal position fusion * * @group EKF2 + * @min 1.0 + * @unit SD */ -PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 5.0f); +PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 3.0f); /** - * Gate size for GPS velocity fusion (standard deviations) + * Gate size for GPS velocity fusion * * @group EKF2 + * @min 1.0 + * @unit SD */ PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 3.0f);