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

add mini KF to validate range sensor measurements

Browse Source
This commit is contained in:
Marco Hauswirth 2024-09-18 18:12:36 +02:00
parent 6ec8dec63a
commit f1f1e6f059
4 changed files with 83 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -37,55 +37,63 @@
#include <aid_sources/range_finder/range_finder_consistency_check.hpp>
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;
}
using namespace matrix;
void RangeFinderConsistencyCheck::initMiniKF(float p1, float p2, float x1, float x2)
{
_R.setZero();
_A.setIdentity();
float p[4] = {p1, 0.f, 0.f, p2};
_P = Matrix<float, 2, 2>(p);
float h[4] = {1.f, 0.f, -1.f, 1.f};
_H = Matrix<float, 2, 2>(h);
_x(0) = x1;
_x(1) = x2;
_initialized = true;
_sample_count = 0;
}
void RangeFinderConsistencyCheck::UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom,
float dist_bottom_var, uint64_t time_us)
{
const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
if ((_time_last_update_us == 0)
|| (dt < 0.001f) || (dt > 0.5f)) {
if (_time_last_update_us == 0 || dt > 1.f) {
_time_last_update_us = time_us;
_dist_bottom_prev = dist_bottom;
return;
}
const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down
// Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
const float var = 2.f * dist_bottom_var / (dt * dt);
_innov_var = var + vz_var;
const float normalized_innov_sq = (_innov * _innov) / _innov_var;
_test_ratio = normalized_innov_sq / (_gate * _gate);
_signed_test_ratio_lpf.setParameters(dt, _signed_test_ratio_tau);
const float signed_test_ratio = matrix::sign(_innov) * _test_ratio;
_signed_test_ratio_lpf.update(signed_test_ratio);
updateConsistency(vz, time_us);
_time_last_update_us = time_us;
_dist_bottom_prev = dist_bottom;
}
void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
{
if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
_is_kinematically_consistent = false;
_time_last_inconsistent_us = time_us;
}
_R(0, 0) = z_var;
_R(1, 1) = dist_bottom_var;
SquareMatrix<float, 2> Q;
const float process_noise = 0.01f;
Q(0, 0) = dt * dt * vz_var + process_noise;
Q(1, 1) = process_noise * 0.5f;
_x(0) += dt * vz;
_P = _A * _P * _A.transpose() + Q;
const Vector2f measurements(z, dist_bottom);
const Vector2f y = measurements - _H * _x;
const Matrix2f S = _H * _P * _H.transpose() + _R;
float test_ratio = math::min(sq(y(1)) / (sq(_gate) * S(1, 1)), 2.f);
if (test_ratio < 1.f) {
Matrix2f K = _P * _H.transpose() * S.I();
_x = _x + K * y;
_P = _P - K * _H * _P;
_P = 0.5f * (_P + _P.transpose()); // Ensure symmetry
}
_innov = y(1);
_innov_var = S(1, 1);
if (_sample_count++ > 10) {
_is_kinematically_consistent = _test_ratio_lpf.update(test_ratio) < 1.f;
} else {
if ((fabsf(vz) > _min_vz_for_valid_consistency)
&& (_test_ratio < 1.f)
&& ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)
) {
_is_kinematically_consistent = true;
}
_test_ratio_lpf.update(test_ratio);
}
}

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

@ -48,36 +48,33 @@ public:
RangeFinderConsistencyCheck() = default;
~RangeFinderConsistencyCheck() = default;
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; }
float getTestRatio() const { return _test_ratio; }
float getSignedTestRatioLpf() const { return _signed_test_ratio_lpf.getState(); }
float getTestRatioLpf() const { return _test_ratio_lpf.getState(); }
float getInnov() const { return _innov; }
float getInnovVar() const { return _innov_var; }
bool isKinematicallyConsistent() const { return _is_kinematically_consistent; }
void UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var,
uint64_t time_us);
void initMiniKF(float p1, float p2, float x1, float x2);
void stopMiniKF() { _initialized = false; }
bool isRunning() { return _initialized; }
matrix::SquareMatrix<float, 2> _R{};
matrix::SquareMatrix<float, 2> _P{};
matrix::SquareMatrix<float, 2> _A{};
matrix::SquareMatrix<float, 2> _H{};
matrix::Vector2f _x{};
bool _initialized{false};
private:
void updateConsistency(float vz, uint64_t time_us);
uint64_t _time_last_update_us{};
float _dist_bottom_prev{};
float _test_ratio{};
AlphaFilter<float> _signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data
float _gate{.2f};
float _innov{};
float _innov_var{};
uint64_t _time_last_update_us{0};
float _dist_bottom_prev{};
AlphaFilter<float> _test_ratio_lpf{0.3}; // average signed test ratio used to detect a bias in the data
float _gate{1.f};
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;
static constexpr float _min_vz_for_valid_consistency = .5f;
static constexpr uint64_t _consistency_hyst_time_us = 1e6;
int _sample_count{0};
};
#endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP

21
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -68,13 +68,24 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
if (_control_status.flags.in_air) {
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 bool vertical_motion = sq(_state.vel(2)) > fmaxf(P(State::vel.idx + 2, State::vel.idx + 2), 0.1f);
const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
const float var = sq(_params.ekf2_rng_noise) + dist_dependant_var;
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float dist_var = sq(_params.range_noise) + dist_dependant_var;
if (horizontal_motion) {
_rng_consistency_check.stopMiniKF();
} else if (vertical_motion) {
if (!_rng_consistency_check.isRunning()) {
_rng_consistency_check.initMiniKF(P(State::pos.idx + 2, State::pos.idx + 2), P(State::terrain.idx, State::terrain.idx),
_state.pos(2), _state.terrain);
}
_rng_consistency_check.UpdateMiniKF(_state.pos(2), P(State::pos.idx + 2, State::pos.idx + 2), _state.vel(2),
P(State::vel.idx + 2, State::vel.idx + 2), _range_sensor.getRange(), dist_var, imu_sample.time_us);
}
_rng_consistency_check.setGate(_params.ekf2_rng_k_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), horizontal_motion, imu_sample.time_us);
}
} else {

2
src/modules/ekf2/EKF/ekf.h
View File

@ -117,7 +117,7 @@ public:
float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
float getHaglRateInnovVar() const { return _rng_consistency_check.getInnovVar(); }
float getHaglRateInnovRatio() const { return _rng_consistency_check.getSignedTestRatioLpf(); }
float getHaglRateInnovRatio() const { return _rng_consistency_check.getTestRatioLpf(); }
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)