diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 57c4d45a30..53f5d1e77a 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1215,8 +1215,8 @@ void Ekf::update_deadreckoning_status() _time_last_aiding = _time_last_imu - _params.no_aid_timeout_max; } - // report if we have been deadreckoning for too long - _deadreckon_time_exceeded = isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max); + // report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present + _deadreckon_time_exceeded = (_time_last_aiding == 0) || isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max); } // calculate the inverse rotation matrix from a quaternion rotation diff --git a/test/test_EKF_fusionLogic.cpp b/test/test_EKF_fusionLogic.cpp index 2e69c53d6c..5db60989ee 100644 --- a/test/test_EKF_fusionLogic.cpp +++ b/test/test_EKF_fusionLogic.cpp @@ -71,11 +71,9 @@ class EkfFusionLogicTest : public ::testing::Test { TEST_F(EkfFusionLogicTest, doNoFusion) { // GIVEN: a tilt and heading aligned filter - // WHEN: having no aiding source EKF should not have a valid position estimate - - // TODO: for the first 5 second it still has some valid local position - // that needs to change - EXPECT_TRUE(_ekf->local_position_is_valid()); + // WHEN: having no aiding source + // THEN: EKF should not have a valid position estimate + EXPECT_FALSE(_ekf->local_position_is_valid()); _sensor_simulator.runSeconds(4);