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

Add param to choose vision observation noise source

Browse Source
This commit is contained in:
kamilritz 2019-10-10 08:20:16 +02:00 committed by Nuno Marques
parent 6bfb50df8a
commit a4e035d338
2 changed files with 19 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -422,6 +422,8 @@ private:
_param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD)
// vision estimate fusion
(ParamInt<px4::params::EKF2_EV_NOISE_MD>)
_param_ekf2_ev_noise_md, ///< determine source of vision observation noise
(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>)
@ -1162,8 +1164,8 @@ void Ekf2::Run()
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])) {
// velocity measurement error from ev_data or parameters
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VX_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],
@ -1180,8 +1182,8 @@ void Ekf2::Run()
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])) {
// position measurement error from ev_data or parameters
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE])) {
ev_data.posErr = fmaxf(_param_ekf2_evp_noise.get(),
sqrtf(fmaxf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE],
_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])));
@ -1198,8 +1200,9 @@ void Ekf2::Run()
if (PX4_ISFINITE(_ev_odom.q[0])) {
ev_data.quat = matrix::Quatf(_ev_odom.q);
// orientation measurement error from parameters
if (PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE])) {
// orientation measurement error from ev_data or parameters
if (!_param_ekf2_ev_noise_md.get()
&& PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) {
ev_data.angErr = fmaxf(_param_ekf2_eva_noise.get(),
sqrtf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]));

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

@ -701,6 +701,16 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f);
*/
PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f);
/**
* Whether to set the external vision observation noise from the parameter or from vision message
*
* If set to true the observation noise is set from the parameters directly, if set to false the measurement noise is taken from the vision message and the parameter are used as a lower bound.
*
* @boolean
* @group EKF2
*/
PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0);
/**
* Measurement noise for vision position observations used when the vision system does not supply error estimates
*