fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 00:29:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: update to latest ecl version [continued] (#13023)

Browse Source 
* ekf2: update to latest ecl version

Includes compatibility changes for renamed variables.

* ekf2: Add write of odometry velocity data

Co-Authored-By: kritz <kritz@ethz.ch>

* ekf2: Modify parameters to enable control of vision velocity fusion

* ekf2: Update to latest ecl with timestamp messages
This commit is contained in:
kritz 2019-09-26 10:21:53 +02:00 committed by Julian Kent
parent dedd257433
commit 190c817a9e
3 changed files with 65 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/ecl

@ -1 +1 @@
Subproject commit 62a1e0751206e76ccd482a8b2a0f77550649f39d
Subproject commit b78429aa60257157c53a8a43c1c29646560de124

39
src/modules/ekf2/ekf2_main.cpp
View File

@ -421,10 +421,14 @@ private:
// vision estimate fusion
(ParamFloat<px4::params::EKF2_EVP_NOISE>)
_param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m)
(ParamFloat<px4::params::EKF2_EVV_NOISE>)
_param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s)
(ParamFloat<px4::params::EKF2_EVA_NOISE>)
_param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad)
(ParamExtFloat<px4::params::EKF2_EV_GATE>)
_param_ekf2_ev_gate, ///< external vision position innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_EVV_GATE>)
_param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_EVP_GATE>)
_param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD)
// optical flow fusion
(ParamExtFloat<px4::params::EKF2_OF_N_MIN>)
@ -566,8 +570,8 @@ Ekf2::Ekf2():
_param_ekf2_baro_gate(_params->baro_innov_gate),
_param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone),
_param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt),
_param_ekf2_gps_p_gate(_params->posNE_innov_gate),
_param_ekf2_gps_v_gate(_params->vel_innov_gate),
_param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
_param_ekf2_gps_v_gate(_params->gps_vel_innov_gate),
_param_ekf2_tas_gate(_params->tas_innov_gate),
_param_ekf2_head_noise(_params->mag_heading_noise),
_param_ekf2_mag_noise(_params->mag_noise),
@ -601,7 +605,8 @@ Ekf2::Ekf2():
_param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
_param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
_param_ekf2_ev_gate(_params->ev_innov_gate),
_param_ekf2_evv_gate(_params->ev_vel_innov_gate),
_param_ekf2_evp_gate(_params->ev_pos_innov_gate),
_param_ekf2_of_n_min(_params->flow_noise),
_param_ekf2_of_n_max(_params->flow_noise_qual_min),
_param_ekf2_of_qmin(_params->flow_qual_min),
@ -1154,11 +1159,29 @@ void Ekf2::run()
ext_vision_message ev_data;
// check for valid velocity data
if (PX4_ISFINITE(_ev_odom.vx) && PX4_ISFINITE(_ev_odom.vy) && PX4_ISFINITE(_ev_odom.vz)) {
ev_data.vel(0) = _ev_odom.vx;
ev_data.vel(1) = _ev_odom.vy;
ev_data.vel(2) = _ev_odom.vz;
// velocity measurement error from parameters
if (PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE])) {
ev_data.velErr = fmaxf(_param_ekf2_evv_noise.get(),
sqrtf(fmaxf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE],
fmaxf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE],
_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE]))));
} else {
ev_data.velErr = _param_ekf2_evv_noise.get();
}
}
// check for valid position data
if (PX4_ISFINITE(_ev_odom.x) && PX4_ISFINITE(_ev_odom.y) && PX4_ISFINITE(_ev_odom.z)) {
ev_data.posNED(0) = _ev_odom.x;
ev_data.posNED(1) = _ev_odom.y;
ev_data.posNED(2) = _ev_odom.z;
ev_data.pos(0) = _ev_odom.x;
ev_data.pos(1) = _ev_odom.y;
ev_data.pos(2) = _ev_odom.z;
// position measurement error from parameter
if (PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE])) {

47
src/modules/ekf2/ekf2_params.c
View File

@ -700,7 +700,6 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f);
*/
PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f);
/**
* Measurement noise for vision position observations used when the vision system does not supply error estimates
*
@ -709,7 +708,17 @@ PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f);
* @unit m
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_EVP_NOISE, 0.05f);
PARAM_DEFINE_FLOAT(EKF2_EVP_NOISE, 0.1f);
/**
* Measurement noise for vision velocity observations used when the vision system does not supply error estimates
*
* @group EKF2
* @min 0.01
* @unit m/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f);
/**
* Measurement noise for vision angle observations used when the vision system does not supply error estimates
@ -721,18 +730,6 @@ PARAM_DEFINE_FLOAT(EKF2_EVP_NOISE, 0.05f);
*/
PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f);
/**
* Gate size for vision estimate fusion
*
* Sets the number of standard deviations used by the innovation consistency test.
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_EV_GATE, 5.0f);
/**
* Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum
*
@ -1161,6 +1158,28 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_A_HMAX, 5.0f);
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f);
/**
* Gate size for vision velocity estimate fusion
*
* Sets the number of standard deviations used by the innovation consistency test.
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_EVV_GATE, 3.0f);
/**
* Gate size for vision position fusion
* Sets the number of standard deviations used by the innovation consistency test.
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_EVP_GATE, 5.0f);
/**
* Specific drag force observation noise variance used by the multi-rotor specific drag force model.
* Increasing it makes the multi-rotor wind estimates adjust more slowly.