fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-12 23:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Minimise data buffer lengths

Browse Source 
Saves RAM and reduces output predictor errors by using the smallest data buffer length that meets time delay and update rate requirements.
This commit is contained in:
Paul Riseborough
2016-11-05 23:44:45 +11:00
parent 750a8874c1
commit d5b398ddb1
3 changed files with 35 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+2
View File
@@ -173,6 +173,7 @@ struct parameters {
// measurement source control
int fusion_mode; // bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
int vdist_sensor_type; // selects the primary source for height data
int sensor_interval_min_ms; // minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers.
// measurement time delays
float mag_delay_ms; // magnetometer measurement delay relative to the IMU (msec)
@@ -275,6 +276,7 @@ struct parameters {
// measurement source control
fusion_mode = MASK_USE_GPS;
vdist_sensor_type = VDIST_SENSOR_BARO;
sensor_interval_min_ms = 20;
// measurement time delays
mag_delay_ms = 0.0f;
EKF/estimator_interface.cpp
+18
View File
@@ -48,6 +48,8 @@
EstimatorInterface::EstimatorInterface():
OBS_BUFFER_LENGTH(10),
IMU_BUFFER_LENGTH(30),
_min_obs_interval_us(0),
_dt_imu_avg(0.0f),
_imu_ticks(0),
@@ -340,6 +342,22 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
bool EstimatorInterface::initialise_interface(uint64_t timestamp)
{
// find the maximum time delay required to compensate for
uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms,
math::max(_params.range_delay_ms,
math::max(_params.gps_delay_ms,
math::max(_params.flow_delay_ms,
math::max(_params.ev_delay_ms,
math::max(_params.airspeed_delay_ms, _params.baro_delay_ms))))));
// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
IMU_BUFFER_LENGTH = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
// set the observaton buffer length to handle the minimum time of arrival between observations
OBS_BUFFER_LENGTH = (max_time_delay_ms / _params.sensor_interval_min_ms) + 1;
ECL_INFO("EKF IMU buffer length = %i",(int)IMU_BUFFER_LENGTH);
ECL_INFO("EKF observation buffer length = %i",(int)OBS_BUFFER_LENGTH);
if (!(_imu_buffer.allocate(IMU_BUFFER_LENGTH) &&
_gps_buffer.allocate(OBS_BUFFER_LENGTH) &&
EKF/estimator_interface.h
+15 -2
View File
@@ -273,8 +273,21 @@ protected:
parameters _params; // filter parameters
static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer
static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer
/*
OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer
which sets the maximum frequency at which we can process non-IMU measurements. Measurements that
arrive too soon after the previous measurement will not be processed.
max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001)
This can be adjusted to match the max sensor data rate plus some margin for jitter.
*/
uint8_t OBS_BUFFER_LENGTH;
/*
IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the
EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for.
max sensor time offet (msec) = IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS
This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay.
*/
uint8_t IMU_BUFFER_LENGTH;
static const unsigned FILTER_UPDATE_PERIOD_MS = 10; // ekf prediction period in milliseconds
unsigned _min_obs_interval_us; // minimum time interval between observations that will guarantee data is not lost (usec)