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

ekf2: set horizon using specific parameter

Browse Source 
Some sensors can have their delay included in the timestamp. In this
case, the buffer cannot be sized using the _DELAY parameter.
This commit is contained in:
bresch 2024-04-05 13:49:03 +02:00 committed by Daniel Agar
parent 6796945d0b
commit a8a67fbf8f
5 changed files with 93 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/EKF/common.h
View File

@ -265,7 +265,7 @@ struct parameters {
int32_t height_sensor_ref{static_cast<int32_t>(HeightSensor::BARO)};
int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
int32_t sensor_interval_max_ms{10}; ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
float delay_max_ms{110.f}; ///< maximum time delay of all the aiding sensors. Sets the size of the observation buffers. (mSec)
// input noise
float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec)

60
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -516,71 +516,15 @@ void EstimatorInterface::setDragData(const imuSample &imu)
bool EstimatorInterface::initialise_interface(uint64_t timestamp)
{
// find the maximum time delay the buffers are required to handle
// it's reasonable to assume that aux velocity device has low delay. TODO: check the delay only if the aux device is used
float max_time_delay_ms = _params.sensor_interval_max_ms;
// aux vel
#if defined(CONFIG_EKF2_AUXVEL)
max_time_delay_ms = math::max(_params.auxvel_delay_ms, max_time_delay_ms);
#endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_BAROMETER)
// using baro
if (_params.baro_ctrl > 0) {
max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_AIRSPEED)
// using airspeed
if (_params.arsp_thr > FLT_EPSILON) {
max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_MAGNETOMETER)
// mag mode
if (_params.mag_fusion_type != MagFuseType::NONE) {
max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_RANGE_FINDER)
// using range finder
if ((_params.rng_ctrl != static_cast<int32_t>(RngCtrl::DISABLED))) {
max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_GNSS)
if (_params.gnss_ctrl > 0) {
max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
if (_params.flow_ctrl > 0) {
max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_params.ev_ctrl > 0) {
max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_EXTERNAL_VISION
const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
_imu_buffer_length = math::max(2, (int)ceilf(max_time_delay_ms / filter_update_period_ms));
_imu_buffer_length = math::max(2, (int)ceilf(_params.delay_max_ms / filter_update_period_ms));
// set the observation buffer length to handle the minimum time of arrival between observations in combination
// with the worst case delay from current time to ekf fusion time
// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
const float ekf_delay_ms = max_time_delay_ms * 1.5f;
const float ekf_delay_ms = _params.delay_max_ms * 1.5f;
_obs_buffer_length = roundf(ekf_delay_ms / filter_update_period_ms);
// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)

117
src/modules/ekf2/EKF2.cpp
View File

@ -64,6 +64,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
#endif // CONFIG_EKF2_WIND
_params(_ekf.getParamHandle()),
_param_ekf2_predict_us(_params->filter_update_interval_us),
_param_ekf2_delay_max(_params->delay_max_ms),
_param_ekf2_imu_ctrl(_params->imu_ctrl),
#if defined(CONFIG_EKF2_AUXVEL)
_param_ekf2_avel_delay(_params->auxvel_delay_ms),
@ -437,46 +438,6 @@ void EKF2::Run()
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_BAROMETER)
// if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output
if (_params->baro_ctrl == 1) {
float sens_baro_rate = 0.f;
if (param_get(param_find("SENS_BARO_RATE"), &sens_baro_rate) == PX4_OK) {
if (sens_baro_rate > 0) {
float interval_ms = roundf(1000.f / sens_baro_rate);
if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) {
PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms);
_params->sensor_interval_max_ms = interval_ms;
}
}
}
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_MAGNETOMETER)
// if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output
if (_params->mag_fusion_type != MagFuseType::NONE) {
float sens_mag_rate = 0.f;
if (param_get(param_find("SENS_MAG_RATE"), &sens_mag_rate) == PX4_OK) {
if (sens_mag_rate > 0) {
float interval_ms = roundf(1000.f / sens_mag_rate);
if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) {
PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms);
_params->sensor_interval_max_ms = interval_ms;
}
}
}
}
#endif // CONFIG_EKF2_MAGNETOMETER
_ekf.updateParameters();
}
@ -822,6 +783,82 @@ void EKF2::VerifyParams()
}
#endif // CONFIG_EKF2_MAGNETOMETER
float delay_max = _param_ekf2_delay_max.get();
#if defined(CONFIG_EKF2_AUXVEL)
if (_param_ekf2_avel_delay.get() > delay_max) {
delay_max = _param_ekf2_avel_delay.get();
}
#endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_BAROMETER)
if (_param_ekf2_baro_delay.get() > delay_max) {
delay_max = _param_ekf2_baro_delay.get();
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_AIRSPEED)
if (_param_ekf2_asp_delay.get() > delay_max) {
delay_max = _param_ekf2_asp_delay.get();
}
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_param_ekf2_mag_delay.get() > delay_max) {
delay_max = _param_ekf2_mag_delay.get();
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_param_ekf2_rng_delay.get() > delay_max) {
delay_max = _param_ekf2_rng_delay.get();
}
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_GNSS)
if (_param_ekf2_gps_delay.get() > delay_max) {
delay_max = _param_ekf2_gps_delay.get();
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
if (_param_ekf2_of_delay.get() > delay_max) {
delay_max = _param_ekf2_of_delay.get();
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_param_ekf2_ev_delay.get() > delay_max) {
delay_max = _param_ekf2_ev_delay.get();
}
#endif // CONFIG_EKF2_EXTERNAL_VISION
if (delay_max > _param_ekf2_delay_max.get()) {
/* EVENT
* @description EKF2_DELAY_MAX({1}ms) is too small compared to the maximum sensor delay ({2})
*/
events::send<float, float>(events::ID("nf_delay_max_too_small"), events::Log::Warning,
"EKF2_DELAY_MAX increased to {2}ms, please reboot", _param_ekf2_delay_max.get(),
delay_max);
_param_ekf2_delay_max.commit_no_notification(delay_max);
}
}
void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)

1
src/modules/ekf2/EKF2.hpp
View File

@ -498,6 +498,7 @@ private:
DEFINE_PARAMETERS(
(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
(ParamExtFloat<px4::params::EKF2_DELAY_MAX>) _param_ekf2_delay_max,
(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
#if defined(CONFIG_EKF2_AUXVEL)

12
src/modules/ekf2/module.yaml
View File

@ -29,6 +29,18 @@ parameters:
default: 7
min: 0
max: 7
EKF2_DELAY_MAX:
description:
short: Maximum delay of all the aiding sensors
long: Defines the delay between the current time and the delayed-time horizon.
This value should be at least as large as the largest EKF2_XXX_DELAY parameter.
type: float
default: 200
min: 0
max: 1000
unit: ms
reboot_required: true
decimal: 1
EKF2_MAG_DELAY:
description:
short: Magnetometer measurement delay relative to IMU measurements