fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 03:01:36 +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-05-19 14:45:34 +02:00
parent 3d14318cc2
commit adad0282bc
2 changed files with 6 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
+3 -7
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::z.idx) = z;
_x(RangeFilter::terrain.idx) = z - dist_bottom; _x(RangeFilter::terrain.idx) = z - dist_bottom;
_initialized = true; _initialized = true;
_state = KinematicState::UNKNOWN;
_test_ratio_lpf.reset(0.f); _test_ratio_lpf.reset(0.f);
_t_since_first_sample = 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; R = z_var;
} else if (measurement_idx == 1) { } else if (measurement_idx == 1) {
H(RangeFilter::z.idx) = _Ht(0); H = _Ht;
H(RangeFilter::terrain.idx) = _Ht(1);
R = dist_bottom_var; 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) void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var)
{ {
// start the consistency check after 1s // start the consistency check after 1s
if (_t_since_first_sample + dt > 1.0f) { if (_t_since_first_sample > _t_to_init) {
_t_since_first_sample = 2.0f; if (fabsf(_test_ratio_lpf.getState()) < 1.f) {
if (abs(_test_ratio_lpf.getState()) < 1.f) {
const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f); const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f);
if (!horizontal_motion && vertical_motion) { if (!horizontal_motion && vertical_motion) {
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
+3 -2
View File
@@ -58,8 +58,8 @@ public:
float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; } float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; }
float getInnov() const { return _initialized ? _innov : 0.f; } float getInnov() const { return _initialized ? _innov : 0.f; }
float getInnovVar() const { return _initialized ? _innov_var : 0.f; } float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; } bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT && _t_since_first_sample > _t_to_init; }
bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT; } bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT && _t_since_first_sample > _t_to_init; }
void setGate(const float gate) { _gate = gate; } void setGate(const float gate) { _gate = gate; }
void run(float z, float z_var, float vz, float vz_var, void run(float z, float z_var, float vz, float vz_var,
float dist_bottom, float dist_bottom_var, uint64_t time_us); float dist_bottom, float dist_bottom_var, uint64_t time_us);
@@ -93,6 +93,7 @@ private:
KinematicState _state{KinematicState::UNKNOWN}; KinematicState _state{KinematicState::UNKNOWN};
float _t_since_first_sample{0.f}; float _t_since_first_sample{0.f};
uint8_t _last_posD_reset_count{0}; uint8_t _last_posD_reset_count{0};
static constexpr float _t_to_init{1.f};
}; };
namespace RangeFilter namespace RangeFilter