From ba72d37f9bbcdbd60d1124391febcd55a913a0d1 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Mon, 9 Dec 2024 15:41:50 +0100 Subject: [PATCH] add symforce, adjust smaller things --- .../range_finder_consistency_check.cpp | 42 +++++++++--------- .../range_finder_consistency_check.hpp | 32 ++++++-------- .../range_finder/range_height_control.cpp | 28 +++--------- src/modules/ekf2/EKF/covariance.cpp | 1 + .../EKF/python/ekf_derivation/derivation.py | 17 +++++++ .../generated/range_validation_filter.h | 44 +++++++++++++++++++ 6 files changed, 103 insertions(+), 61 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h 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 45d917cb7e..533005f563 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 @@ -36,6 +36,7 @@ */ #include +#include "ekf_derivation/generated/range_validation_filter.h" using namespace matrix; @@ -44,11 +45,10 @@ void RangeFinderConsistencyCheck::init(float var_z, float var_terrain, float z, _R.setZero(); _A.setIdentity(); float p[4] = {var_z, 0.f, 0.f, var_terrain}; - _P = Matrix(p); - float h[4] = {1.f, 0.f, -1.f, 1.f}; - _H = Matrix(h); - _x(0) = z; - _x(1) = z + dist_bottom; + _P = Matrix(p); + sym::RangeValidationFilter(&_H); + _x(RangeFilter::z.idx) = z; + _x(RangeFilter::terrain.idx) = z + dist_bottom; _initialized = true; _sample_count = 0; _state = KinematicState::UNKNOWN; @@ -70,41 +70,43 @@ void RangeFinderConsistencyCheck::update(float z, float z_var, float vz, float v _time_last_update_us = time_us; - _R(0, 0) = z_var; - _R(1, 1) = dist_bottom_var; + _R(RangeFilter::z.idx, RangeFilter::z.idx) = z_var; + _R(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = dist_bottom_var; SquareMatrix Q; - const float process_noise = 0.001f; - Q(0, 0) = dt * dt * vz_var + process_noise; - Q(1, 1) = process_noise; - _x(0) += dt * vz; + Q(RangeFilter::z.idx, RangeFilter::z.idx) = dt * dt * vz_var + 0.001f; + Q(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = terrain_process_noise; + + _x(RangeFilter::z.idx) += 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 = fminf(abs(sq(y(1)) / (sq(_gate) * S(1, 1))), 2.f); + const float normalized_residual = (y.transpose() * S.I() * y)(0, 0); + float test_ratio = fminf(normalized_residual / sq(_gate), 2.f); Matrix2f K = _P * _H.transpose() * S.I(); - K(0, 0) = 1.f; - K(0, 1) = 0.f; + K(RangeFilter::z.idx, RangeFilter::z.idx) = 1.f; + K(RangeFilter::z.idx, RangeFilter::terrain.idx) = 0.f; if (test_ratio > 1.f) { - K(1, 0) = 0.f; - K(1, 1) = 0.f; + K(RangeFilter::terrain.idx, RangeFilter::z.idx) = 0.f; + K(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = 0.f; } _x = _x + K * y; _P = _P - K * _H * _P; _P = 0.5f * (_P + _P.transpose()); // Ensure symmetry + _innov = y(RangeFilter::terrain.idx); + _innov_var = S(RangeFilter::terrain.idx, RangeFilter::terrain.idx); - _innov = y(1); - _innov_var = S(1, 1); + _test_ratio_lpf.update(sign(_innov) * test_ratio); if (_sample_count++ > _min_nr_of_samples) { - if (_test_ratio_lpf.update(test_ratio) < 1.f) { + if (abs(_test_ratio_lpf.getState()) < 1.f) { _state = KinematicState::CONSISTENT; } else { @@ -112,8 +114,6 @@ void RangeFinderConsistencyCheck::update(float z, float z_var, float vz, float v _state = KinematicState::INCONSISTENT; } - } else { - _test_ratio_lpf.update(test_ratio); } } 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 c1ffb4eadd..fe2a7bb7c4 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 @@ -62,10 +62,7 @@ public: bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; } bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT || _fixed_wing; } - void update(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var, - uint64_t time_us); - void init(float var_z, float var_terrain, float z, float dist_bottom); - void stop() + void reset() { if (_initialized) { if (_state == KinematicState::CONSISTENT) { @@ -77,28 +74,18 @@ public: } int getConsistencyState() const { return static_cast(_state); } - bool isRunning() { return _initialized; } - - void setNotMoving() - { - if (_state == KinematicState::CONSISTENT) { - _state = KinematicState::UNKNOWN; - } - } - - void setFixedWing(bool is_fixed_wing, float gate) - { - _fixed_wing = is_fixed_wing; - _gate = gate; - } - void run(const float z, const float vz, const matrix::SquareMatrix P, const float dist_bottom, const float dist_bottom_var, uint64_t time_us); uint8_t current_posD_reset_count{0}; + float terrain_process_noise{0.0f}; + bool horizontal_motion{false}; private: + void update(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var, + uint64_t time_us); + void init(float var_z, float var_terrain, float z, float dist_bottom); matrix::SquareMatrix _R{}; matrix::SquareMatrix _P{}; matrix::SquareMatrix _A{}; @@ -118,4 +105,11 @@ private: uint8_t _last_posD_reset_count{0}; }; +namespace RangeFilter +{ +static constexpr estimator::IdxDof z{0, 1}; +static constexpr estimator::IdxDof terrain{1, 1}; +static constexpr uint8_t size{2}; +}; + #endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 66c03f24b5..3769dc50f8 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -69,27 +69,13 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) const float dist_var = getRngVar(); _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); - if (_control_status.flags.fixed_wing) { - _rng_consistency_check.setFixedWing(true, 2.0f * _params.range_kin_consistency_gate); - _rng_consistency_check.run(_state.pos(2), _state.vel(2), P, _range_sensor.getRange(), dist_var, imu_sample.time_us); + const bool updated_horizontal_motion = (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f)); - } else { - _rng_consistency_check.setFixedWing(false, _params.range_kin_consistency_gate); - const bool horizontal_motion = (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); - - if (horizontal_motion) { - if (_rng_consistency_check.isRunning()) { - _rng_consistency_check.stop(); - } - - } else if (vertical_motion) { - _rng_consistency_check.run(_state.pos(2), _state.vel(2), P, _range_sensor.getRange(), dist_var, imu_sample.time_us); - - } else { - _rng_consistency_check.setNotMoving(); - } + if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) { + _rng_consistency_check.reset(); } + _rng_consistency_check.horizontal_motion = updated_horizontal_motion; + _rng_consistency_check.run(_gpos.altitude(), _state.vel(2), P, _range_sensor.getRange(), dist_var, imu_sample.time_us); } } else { @@ -103,7 +89,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _range_sensor.setValidity(true); // bypass the checks } else { - _rng_consistency_check.stop(); + _rng_consistency_check.reset(); } } } @@ -228,7 +214,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } else { - ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME); + ECL_WARN("stoppPing %s fusion, continuing conditions failing", HGT_SRC_NAME); stopRngHgtFusion(); stopRngTerrFusion(); } diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 85ce9b9c17..a48dcbfd6c 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -226,6 +226,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // process noise due to terrain gradient terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.ekf2_terr_grad) * (sq(_state.vel(0)) + sq(_state.vel( 1))); + _rng_consistency_check.terrain_process_noise = terrain_process_noise; P(State::terrain.idx, State::terrain.idx) += terrain_process_noise; } diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 91c3fd734e..6130fc25c4 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -721,6 +721,22 @@ def compute_gravity_z_innov_var_and_h( return (innov_var, H.T) +def range_validation_filter() -> sf.matrix: + + z = sf.Symbol("z") + dist_bottom = sf.Symbol("dist_bottom") + terrain = dist_bottom - z + state = sf.V2.symbolic("state") + state_indices = {"z": 0, "dist_bottom": 1} + state[state_indices["z"]] = z + state[state_indices["dist_bottom"]] = dist_bottom + + H = sf.M22() + H[0, :] = sf.V1(z).jacobian(state, tangent_space=False) + H[1, :] = sf.V1(terrain).jacobian(state, tangent_space=False) + + return H + print("Derive EKF2 equations...") generate_px4_function(predict_covariance, output_names=None) @@ -752,5 +768,6 @@ generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_va generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"]) generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"]) generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"]) +generate_px4_function(range_validation_filter, output_names=["H"]) generate_px4_state(State, tangent_idx) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h new file mode 100644 index 0000000000..de8fed34ac --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h @@ -0,0 +1,44 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: range_validation_filter + * + * Args: + * + * Outputs: + * H: Matrix22 + */ +template +void RangeValidationFilter(matrix::Matrix* const H = nullptr) { + // Total ops: 0 + + // Input arrays + + // Intermediate terms (0) + + // Output terms (1) + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = 1; + _h(1, 0) = -1; + _h(1, 1) = 1; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym