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

EKF: Make average update rate of EKF closer to target

Browse Source 
With the EKF, the average update rate is more important than the instantaneous value as it affects tuning. This patch ensures that the EKF prediction cycle will be performed early if the previous one was late in an attempt to maintain the target update rate.
This commit is contained in:
Paul Riseborough 2016-05-05 19:49:28 +10:00
parent 1daa6c7866
commit f1b7e7714e
2 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
EKF/ekf.cpp
View File

@ -73,6 +73,7 @@ Ekf::Ekf():
_time_last_of_fuse(0),
_time_last_arsp_fuse(0),
_last_disarmed_posD(0.0f),
_last_dt_overrun(0.0f),
_airspeed_innov(0.0f),
_airspeed_innov_var(0.0f),
_heading_innov(0.0f),
@ -584,8 +585,12 @@ bool Ekf::collect_imu(imuSample &imu)
_imu_down_sampled.delta_vel += (_imu_sample_new.delta_vel + delta_R * _imu_sample_new.delta_vel) * 0.5f;
// if the target time delta between filter prediction steps has been exceeded
// write the accumulated IMu data to the ring buffer
if (_imu_down_sampled.delta_ang_dt >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) {
// write the accumulated IMU data to the ring buffer
float target_dt = (float)(FILTER_UPDATE_PERRIOD_MS) / 1000;
if (_imu_down_sampled.delta_ang_dt >= target_dt - _last_dt_overrun) {
// store the amount we have over-run the target update rate by
_last_dt_overrun = _imu_down_sampled.delta_ang_dt - target_dt;
imu.delta_ang = _q_down_sampled.to_axis_angle();
imu.delta_vel = _imu_down_sampled.delta_vel;

1
EKF/ekf.h
View File

@ -159,6 +159,7 @@ private:
uint64_t _time_last_arsp_fuse; // time the last fusion of airspeed 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_PERRIOD_MS (sec)
Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s