From f1e4f6dbc8877bae7e5d9617b477e94797d76770 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Sat, 5 Jun 2021 15:01:44 +0530 Subject: [PATCH] AC_PrecLand: Initialize EKF before using its output --- libraries/AC_PrecLand/AC_PrecLand.cpp | 40 ++++++++++++++++++++++++--- libraries/AC_PrecLand/AC_PrecLand.h | 9 +++++- 2 files changed, 44 insertions(+), 5 deletions(-) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index ebc4851174..1c66d7700a 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -12,6 +12,10 @@ extern const AP_HAL::HAL& hal; +static const uint32_t EKF_INIT_TIME_MS = 2000; // EKF initialisation requires this many milliseconds of good sensor data +static const uint32_t EKF_INIT_SENSOR_MIN_UPDATE_MS = 500; // Sensor must update within this many ms during EKF init, else init will fail +static const uint32_t LANDING_TARGET_TIMEOUT_MS = 2000; // Sensor must update within this many ms, else prec landing will be switched off + const AP_Param::GroupInfo AC_PrecLand::var_info[] = { // @Param: ENABLED // @DisplayName: Precision Land enabled/disabled @@ -220,7 +224,12 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) bool AC_PrecLand::target_acquired() { - _target_acquired = _target_acquired && (AP_HAL::millis()-_last_update_ms) < 2000; + if ((AP_HAL::millis()-_last_update_ms) > LANDING_TARGET_TIMEOUT_MS) { + // not had a sensor update since a long time + // probably lost the target + _estimator_initialized = false; + _target_acquired = false; + } return _target_acquired; } @@ -328,7 +337,8 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va // Update if a new Line-Of-Sight measurement is available if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) { float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f); - if (!target_acquired()) { + if (!_estimator_initialized) { + // start init of EKF. We will let the filter consume the data for a while before it available for consumption // reset filter state if (inertial_data_delayed->inertialNavVelocityValid) { _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.x, sq(2.0f)); @@ -338,7 +348,9 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, 0.0f, sq(10.0f)); } _last_update_ms = AP_HAL::millis(); - _target_acquired = true; + _estimator_init_ms = AP_HAL::millis(); + // we have initialized the estimator but will not use the values for sometime so that EKF settles down + _estimator_initialized = true; } else { float NIS_x = _ekf_x.getPosNIS(_target_pos_rel_meas_NED.x, xy_pos_var); float NIS_y = _ekf_y.getPosNIS(_target_pos_rel_meas_NED.y, xy_pos_var); @@ -347,13 +359,15 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va _ekf_x.fusePos(_target_pos_rel_meas_NED.x, xy_pos_var); _ekf_y.fusePos(_target_pos_rel_meas_NED.y, xy_pos_var); _last_update_ms = AP_HAL::millis(); - _target_acquired = true; } else { _outlier_reject_count++; } } } + // check EKF was properly initialized when the sensor detected a landing target + check_ekf_init_timeout(); + // Output prediction if (target_acquired()) { _target_pos_rel_est_NE.x = _ekf_x.getPos(); @@ -368,6 +382,24 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va } } + +// check if EKF got the time to initialize when the landing target was first detected +// Expects sensor to update within EKF_INIT_SENSOR_MIN_UPDATE_MS milliseconds till EKF_INIT_TIME_MS milliseconds have passed +// after this period landing target estimates can be used by vehicle +void AC_PrecLand::check_ekf_init_timeout() +{ + if (!target_acquired() && _estimator_initialized) { + // we have just got the target in sight + if (AP_HAL::millis()-_last_update_ms > EKF_INIT_SENSOR_MIN_UPDATE_MS) { + // we have lost the target, not enough readings to initialize the EKF + _estimator_initialized = false; + } else if (AP_HAL::millis()-_estimator_init_ms > EKF_INIT_TIME_MS) { + // the target has been visible for a while, EKF should now have initialized to a good value + _target_acquired = true; + } + } +} + bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body) { if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) { diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 2681756842..aa6cf6b854 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -87,6 +87,11 @@ private: SITL = 4, }; + // check if EKF got the time to initialize when the landing target was first detected + // Expects sensor to update within EKF_INIT_SENSOR_MIN_UPDATE_MS milliseconds till EKF_INIT_TIME_MS milliseconds have passed + // after this period landing target estimates can be used by vehicle + void check_ekf_init_timeout(); + // run target position estimator void run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid); @@ -113,7 +118,9 @@ private: AP_Vector3f _cam_offset; // Position of the camera relative to the CG uint32_t _last_update_ms; // system time in millisecond when update was last called - bool _target_acquired; // true if target has been seen recently + bool _target_acquired; // true if target has been seen recently after estimator is initialized + bool _estimator_initialized; // true if estimator has been initialized after few seconds of the target being detected by sensor + uint32_t _estimator_init_ms; // system time in millisecond when EKF was init uint32_t _last_backend_los_meas_ms; // system time target was last seen PosVelEKF _ekf_x, _ekf_y; // Kalman Filter for x and y axis