Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Robustify range finder kinematic consistency check #22117

Merged
merged 1 commit into from
Sep 22, 2023
Merged

Robustify range finder kinematic consistency check #22117

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 16 additions & 4 deletions src/modules/ekf2/EKF/range_finder_consistency_check.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,12 @@

#include "range_finder_consistency_check.hpp"

void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us)
void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us)
{
if (horizontal_motion) {
_time_last_horizontal_motion = time_us;
}

const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;

if ((_time_last_update_us == 0)
Expand Down Expand Up @@ -68,12 +72,20 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va

void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
{
if (fabsf(vz) < _min_vz_for_valid_consistency) {
// We can only check consistency if there is vertical motion
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Wouldn't you want to still invalidate if there's horizontal motion?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No, we can validate regardless of the horizontal motion, but we can only invalidate if we're only moving vertically, otherwise a change in terrain height would invalidate too.

return;
}

if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
_is_kinematically_consistent = false;
_time_last_inconsistent_us = time_us;
if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
_is_kinematically_consistent = false;
_time_last_inconsistent_us = time_us;
}

} else {
if (fabsf(vz) > _min_vz_for_valid_consistency && _test_ratio < 1.f && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
if (_test_ratio < 1.f
&& ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
_is_kinematically_consistent = true;
}
}
Expand Down
3 changes: 2 additions & 1 deletion src/modules/ekf2/EKF/range_finder_consistency_check.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ class RangeFinderConsistencyCheck final
RangeFinderConsistencyCheck() = default;
~RangeFinderConsistencyCheck() = default;

void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us);
void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us);

void setGate(float gate) { _gate = gate; }

Expand All @@ -72,6 +72,7 @@ class RangeFinderConsistencyCheck final

bool _is_kinematically_consistent{true};
uint64_t _time_last_inconsistent_us{};
uint64_t _time_last_horizontal_motion{};

static constexpr float _signed_test_ratio_tau = 2.f;

Expand Down
8 changes: 4 additions & 4 deletions src/modules/ekf2/EKF/range_height_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,15 +62,15 @@ void Ekf::controlRangeHeightFusion()
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());

// Run the kinematic consistency check when not moving horizontally
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
&& (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P.trace<2>(State::vel.idx), 0.1f))) {
if (_control_status.flags.in_air) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What's the harm in letting this always run (!in_air)? The actual usage in the terrain estimator is still going to require in_air && rng_kin_consistent.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

An issue I see is when the operator is powering the drone but is still manipulating it before takeoff. There is then a high chance to fail the consistency check for no real reason. It should then be validated again after takeoff, so in theory it's not a big problem, the terrain estimator will just start a bit later and the 5s of no terrain aiding timeout should be enough.

const bool horizontal_motion = _control_status.flags.fixed_wing
|| (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));

const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float var = sq(_params.range_noise) + dist_dependant_var;

_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), _time_delayed_us);
_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us);
}

} else {
Expand Down
Loading