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

EKF: Use last known position when resuming OF aiding

Browse Source
This commit is contained in:
Paul Riseborough
2017-01-17 08:58:01 +11:00
parent 453bde73f8
commit 21270bb06a
1 changed files with 13 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+13 -7
View File
@@ -279,15 +279,21 @@ void Ekf::controlOpticalFlowFusion()
_state.pos(0) = 0.0f;
_state.pos(1) = 0.0f;
// reset the corresponding covariances
// we are by definition at the origin at commencement so variances are also zeroed
zeroRows(P,7,8);
zeroCols(P,7,8);
// align the output observer to the EKF states
alignOutputFilter();
} else {
// set to the last known position
_state.pos(0) = _last_known_posNE(0);
_state.pos(1) = _last_known_posNE(1);
}
// reset the corresponding covariances
// we are by definition at the origin at commencement so variances are also zeroed
zeroRows(P,7,8);
zeroCols(P,7,8);
// align the output observer to the EKF states
alignOutputFilter();
}
}