Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PrecLand: Initialize EKF before using its output

This commit is contained in:
Rishabh 2021-06-05 15:01:44 +05:30 committed by Randy Mackay
parent 55c07e24c0
commit f1e4f6dbc8
2 changed files with 44 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -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) {

9
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -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