EKF: Don't start the output observer before the main filter has initialised

This prevents the possibility of output transients if alignment is delayed.
This commit is contained in:
Paul Riseborough
2016-03-13 20:35:51 +11:00
parent d2407c3463
commit 7677a162aa
+3 -2
View File
@@ -160,8 +160,6 @@ bool Ekf::init(uint64_t timestamp)
bool Ekf::update()
{
// Only run the filter if IMU data in the buffer has been updated
if (_imu_updated) {
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
@@ -170,6 +168,9 @@ bool Ekf::update()
}
}
// Only run the filter if IMU data in the buffer has been updated
if (_imu_updated) {
// perform state and covariance prediction for the main filter
predictState();
predictCovariance();