|
|
|
@ -160,8 +160,6 @@ bool Ekf::init(uint64_t timestamp)
@@ -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()
@@ -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(); |
|
|
|
|