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

EKF: Improve height reset for external vision

Browse Source 
reset to measurement closest in time to fusion time horizon
This commit is contained in:
Paul Riseborough
2016-05-27 14:00:50 +10:00
parent 98c0b74a71
commit ad02818b3d
1 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+6 -3
View File
@@ -208,12 +208,15 @@ void Ekf::resetHeight()
// initialize vertical position with newest measurement
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
if (_time_last_imu - ev_newest.time_us < 2 * EV_MAX_INTERVAL) {
// use the most recent data if it's time offset from the fusion time horizon is smaller
int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
if (abs(dt_newest) < abs(dt_delayed)) {
_state.pos(2) = ev_newest.posNED(2);
} else {
// TODO: reset to last known baro based estimate
_state.pos(2) = _ev_sample_delayed.posNED(2);
}
}
// reset the vertical velocity covariance values