diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index 8c33ab91ae..f533a9375e 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -162,8 +162,10 @@ void LandingTargetEstimator::update() if (!_estimator_initialized) { PX4_INFO("Init"); - _kalman_filter_x.init(_rel_pos(0), 0, _params.pos_unc_init, _params.vel_unc_init); - _kalman_filter_y.init(_rel_pos(1), 0, _params.pos_unc_init, _params.vel_unc_init); + float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f; + float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f; + _kalman_filter_x.init(_rel_pos(0), vx_init, _params.pos_unc_init, _params.vel_unc_init); + _kalman_filter_y.init(_rel_pos(1), vy_init, _params.pos_unc_init, _params.vel_unc_init); _estimator_initialized = true; _last_update = hrt_absolute_time(); diff --git a/src/modules/landing_target_estimator/landing_target_estimator_params.c b/src/modules/landing_target_estimator/landing_target_estimator_params.c index 2b68695de9..252fc2fdb9 100644 --- a/src/modules/landing_target_estimator/landing_target_estimator_params.c +++ b/src/modules/landing_target_estimator/landing_target_estimator_params.c @@ -107,7 +107,7 @@ PARAM_DEFINE_FLOAT(LTEST_POS_UNC_IN, 0.1f); * * @group Landing target Estimator */ -PARAM_DEFINE_FLOAT(LTEST_VEL_UNC_IN, 1.0f); +PARAM_DEFINE_FLOAT(LTEST_VEL_UNC_IN, 0.1f); /** * Scale factor for sensor measurements in sensor x axis