diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index 780a38b88b..2a455185e1 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -52,6 +52,7 @@ void RangeFinderConsistencyCheck::init(const float z, const float z_var, const f _initialized = true; _test_ratio_lpf.reset(0.f); _t_since_first_sample = 0.f; + _state = KinematicState::kUnknown; } void RangeFinderConsistencyCheck::update(const float z, const float z_var, const float vz, const float vz_var, diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp index 72bd904e66..4c948e3740 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp @@ -57,7 +57,7 @@ public: float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; } float getInnov() const { return _initialized ? _innov : 0.f; } float getInnovVar() const { return _initialized ? _innov_var : 0.f; } - bool isKinematicallyConsistent() const { return _state == KinematicState::kConsistent && _t_since_first_sample > _kTestRatioLpfTimeConstant; } + bool isKinematicallyConsistent() const { return _state == KinematicState::kConsistent; } bool isNotKinematicallyInconsistent() const { return _state != KinematicState::kInconsistent && (_t_since_first_sample > _kTestRatioLpfTimeConstant || _landed); } void setGate(const float gate) { _gate = gate; } void run(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var,