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

change consistency to only be valid if running for a second

Browse Source
This commit is contained in:
Marco Hauswirth 2025-03-04 09:46:26 +01:00 committed by Jacob Dahl
parent 564b517225
commit 7cfea9915b
2 changed files with 6 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
View File

@ -50,7 +50,6 @@ void RangeFinderConsistencyCheck::init(const float z, const float z_var, const f
_x(RangeFilter::z.idx) = z;
_x(RangeFilter::terrain.idx) = z - dist_bottom;
_initialized = true;
_state = KinematicState::UNKNOWN;
_test_ratio_lpf.reset(0.f);
_t_since_first_sample = 0.f;
}
@ -92,8 +91,7 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
R = z_var;
} else if (measurement_idx == 1) {
H(RangeFilter::z.idx) = _Ht(0);
H(RangeFilter::terrain.idx) = _Ht(1);
H = _Ht;
R = dist_bottom_var;
}
@ -137,10 +135,8 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var)
{
// start the consistency check after 1s
if (_t_since_first_sample + dt > 1.0f) {
_t_since_first_sample = 2.0f;
if (abs(_test_ratio_lpf.getState()) < 1.f) {
if (_t_since_first_sample > _t_to_init) {
if (fabsf(_test_ratio_lpf.getState()) < 1.f) {
const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f);
if (!horizontal_motion && vertical_motion) {

5
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
View File

@ -58,8 +58,8 @@ 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::CONSISTENT; }
bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT; }
bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT && _t_since_first_sample > _t_to_init; }
bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT && _t_since_first_sample > _t_to_init; }
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, uint64_t time_us);
@ -93,6 +93,7 @@ private:
KinematicState _state{KinematicState::UNKNOWN};
float _t_since_first_sample{0.f};
uint8_t _last_posD_reset_count{0};
static constexpr float _t_to_init{1.f};
};
namespace RangeFilter