fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 12:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: Enable tuning of range finder and optical flow fusion parameters

Browse Source
This commit is contained in:
Paul Riseborough
2016-03-13 17:47:56 +01:00
committed by Lorenz Meier
parent dcb47e425d
commit 735f2942c8
2 changed files with 125 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+28 -3
View File
@@ -186,8 +186,7 @@ private:
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
control::BlockParamInt
*_mag_decl_source; // bitmasked integer used to control the handling of magnetic declination
control::BlockParamInt *_mag_decl_source; // bitmasked integer used to control the handling of magnetic declination
control::BlockParamInt *_mag_fuse_type; // integer ued to control the type of magnetometer fusion used
control::BlockParamInt *_gps_check_mask; // bitmasked integer used to activate the different GPS quality checks
@@ -200,6 +199,22 @@ private:
control::BlockParamFloat *_requiredVdrift; // maximum acceptable vertical drift speed (m/s)
control::BlockParamInt *_param_record_replay_msg; // indicates if we want to record ekf2 replay messages
// measurement source control
control::BlockParamInt *_fusion_mode; // bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
control::BlockParamInt *_vdist_sensor_type; // selects the primary source for height data
// range finder fusion
control::BlockParamFloat *_range_noise; // observation noise for range finder measurements (m)
control::BlockParamFloat *_range_innov_gate; // range finder fusion innovation consistency gate size (STD)
control::BlockParamFloat *_rng_gnd_clearance; // minimum valid value for range when on ground (m)
// optical flow fusion
control::BlockParamFloat *_flow_noise; // best quality observation noise for optical flow LOS rate measurements (rad/sec)
control::BlockParamFloat *_flow_noise_qual_min; // worst quality observation noise for optical flow LOS rate measurements (rad/sec)
control::BlockParamInt *_flow_qual_min; // minimum acceptable quality integer from the flow sensor
control::BlockParamFloat *_flow_innov_gate; // optical flow fusion innovation consistency gate size (STD)
control::BlockParamFloat *_flow_rate_max; // maximum valid optical flow rate (rad/sec)
int update_subscriptions();
};
@@ -253,7 +268,17 @@ Ekf2::Ekf2():
_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)),
_param_record_replay_msg(new control::BlockParamInt(this, "EKF2_REC_RPL", false, &_publish_replay_mode))
_param_record_replay_msg(new control::BlockParamInt(this, "EKF2_REC_RPL", false, &_publish_replay_mode)),
_fusion_mode(new control::BlockParamInt(this, "EKF2_AID_MASK", false, &_params->fusion_mode)),
_vdist_sensor_type(new control::BlockParamInt(this, "EKF2_HGT_MODE", false, &_params->vdist_sensor_type)),
_range_noise(new control::BlockParamFloat(this, "EKF2_RNG_NOISE", false, &_params->range_noise)),
_range_innov_gate(new control::BlockParamFloat(this, "EKF2_RNG_GATE", false, &_params->range_innov_gate)),
_rng_gnd_clearance(new control::BlockParamFloat(this, "EKF2_MIN_RNG", false, &_params->rng_gnd_clearance)),
_flow_noise(new control::BlockParamFloat(this, "EKF2_OF_N_MIN", false, &_params->flow_noise)),
_flow_noise_qual_min(new control::BlockParamFloat(this, "EKF2_OF_N_MAX", false, &_params->flow_noise_qual_min)),
_flow_qual_min(new control::BlockParamInt(this, "EKF2_OF_QMIN", false, &_params->flow_qual_min)),
_flow_innov_gate(new control::BlockParamFloat(this, "EKF2_OF_GATE", false, &_params->flow_innov_gate)),
_flow_rate_max(new control::BlockParamFloat(this, "EKF2_OF_RMAX", false, &_params->flow_rate_max))
{
}
src/modules/ekf2/ekf2_params.c
+97 -1
View File
@@ -327,7 +327,7 @@ PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 3.0f);
PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f);
/**
* Integer bitmask controlling handling of magnetic declination. Set bits to in the following positions to enable functions.
* Integer bitmask controlling handling of magnetic declination. Set bits in the following positions to enable functions.
* 0 : Set to true to use the declination from the geo_lookup library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value.
* 1 : Set to true to save the EKF2_MAG_DECL parameter to the value returned by the EKF when the vehicle disarms.
* 2 : Set to true to always use the declination as an observaton when 3-axis magnetometer fusion is being used.
@@ -388,3 +388,99 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 3.0f);
* @max 1
*/
PARAM_DEFINE_INT32(EKF2_REC_RPL, 0);
/**
* Integer bitmask controlling which external aiding sources will be used. Set bits in the following positions to enable:
* 0 : Set to true to use GPS data if available
* 1 : Set to true to use optical flow data if available
*
* @group EKF2
* @min 0
* @max 3
*/
PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
/**
* Determines the primary source of height data used by the EKF.
* 0 : Barometric pressure
* 1 : GPS
* 2 : Range to ground
*
* @group EKF2
* @min 0
* @max 2
*/
PARAM_DEFINE_INT32(EKF2_HGT_MODE, 0);
/**
* Measurement noise for range finder fusion
*
* @group EKF2
* @min 0.01
* @unit m
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_NOISE, 0.1f);
/**
* Gate size for range finder fusion
*
* @group EKF2
* @min 1.0
* @unit SD
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f);
/**
* Minimum valid range for the range finder
*
* @group EKF2
* @min 0.01
* @unit m
*/
PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f);
/**
* Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum
*
* @group EKF2
* @min 0.05
* @unit rad/sec
*/
PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f);
/**
* Measurement noise for the optical flow sensor when it's reported quality metric is at the minimum set by EKF2_OF_QMIN
* The following condition must be met: EKF2_OF_N_MAXN >= EKF2_OF_N_MIN
*
* @group EKF2
* @min 0.05
* @unit rad/sec
*/
PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f);
/**
* Optical Flow data will only be used if the sensor reports a quality metric >= EKF2_OF_QMIN.
*
* @group EKF2
* @min 0
* @max 255
*/
PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1);
/**
* Gate size for optical flow fusion
*
* @group EKF2
* @min 1.0
* @unit SD
*/
PARAM_DEFINE_FLOAT(EKF2_OF_GATE, 3.0f);
/**
* Optical Flow data will not fused if the magnitude of the flow rate > EKF2_OF_RMAX
*
* @group EKF2
* @min 1.0
* @unit rad/sec
*/
PARAM_DEFINE_FLOAT(EKF2_OF_RMAX, 2.5f);