dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Reduce EKF prediction delta time jitter 

Make the target EKF rate an integer multiple of the IMU rate. This slightly increases the average prediction time step for the EKF from just over 10msec to 12msec, but the variation reduces significantly which makes filter tuning more deterministic.
Improve the algorithm used to adjust the collection time criteria to reduce jitter in the correction.
This commit is contained in:
Paul Riseborough 2017-03-02 10:31:48 +11:00 committed by Lorenz Meier
parent 324fe3b0c7
commit 5fb24c3032
3 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
EKF/ekf.cpp
View File

@ -84,7 +84,7 @@ Ekf::Ekf():
_time_last_arsp_fuse(0),
_time_last_beta_fuse(0),
_last_disarmed_posD(0.0f),
_last_dt_overrun(0.0f),
_imu_collection_time_adj(0.0f),
_time_acc_bias_check(0.0f),
_airspeed_innov(0.0f),
_airspeed_innov_var(0.0f),
@ -507,10 +507,12 @@ bool Ekf::collect_imu(imuSample &imu)
// if the target time delta between filter prediction steps has been exceeded
// write the accumulated IMU data to the ring buffer
float target_dt = (float)(FILTER_UPDATE_PERIOD_MS) / 1000;
if (_imu_down_sampled.delta_ang_dt >= target_dt - _last_dt_overrun) {
if (_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj) {
// store the amount we have over-run the target update rate by
_last_dt_overrun = _imu_down_sampled.delta_ang_dt - target_dt;
// accumulate the amount of time to advance the IMU collection time so that we meet the
// average EKF update rate requirement
_imu_collection_time_adj += 0.01f * (_imu_down_sampled.delta_ang_dt - target_dt);
_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * target_dt, 0.5f * target_dt);
imu.delta_ang = _q_down_sampled.to_axis_angle();
imu.delta_vel = _imu_down_sampled.delta_vel;

2
EKF/ekf.h
View File

@ -250,7 +250,7 @@ private:
uint64_t _time_last_beta_fuse; // time the last fusion of synthetic sideslip measurements were performed (usec)
Vector2f _last_known_posNE; // last known local NE position vector (m)
float _last_disarmed_posD; // vertical position recorded at arming (m)
float _last_dt_overrun; // the amount of time the last IMU collection over-ran the target set by FILTER_UPDATE_PERIOD_MS (sec)
float _imu_collection_time_adj; // the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
uint64_t _time_acc_bias_check; // last time the accel bias check passed (usec)

2
EKF/estimator_interface.h
View File

@ -297,7 +297,7 @@ protected:
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
static const unsigned FILTER_UPDATE_PERIOD_MS = 12; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
unsigned _min_obs_interval_us; // minimum time interval between observations that will guarantee data is not lost (usec)