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