diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 8599b00277..7fed72cc28 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -49,7 +49,7 @@ bool cs_valid_fake_pos # 41 - true if a valid constant position is bein bool cs_constant_pos # 42 - true if the vehicle is at a constant position bool cs_baro_fault # 43 - true when the current baro has been declared faulty and is no longer being used bool cs_gnss_vel # 44 - true if GNSS velocity measurement fusion is intended -bool cs_rng_kin_unknown # 43 - true when the range finder kinematic consistency check is not running +bool cs_rng_kin_unknown # 45 - true when the range finder kinematic consistency check is not running # fault status uint32 fault_status_changes # number of filter fault status (fs) changes 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 a57d8aeed6..a6546a6f0c 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,23 +36,27 @@ */ #include -#include "ekf_derivation/generated/range_validation_filter.h" +#include "ekf_derivation/generated/range_validation_filter_h.h" using namespace matrix; void RangeFinderConsistencyCheck::init(const float &z, const float &z_var, const float &dist_bottom, const float &dist_bottom_var) { - float p[4] = {z_var, z_var, z_var, z_var + dist_bottom_var}; - _P = SquareMatrix(p); - _H = sym::RangeValidationFilter(); + // assume no correlation between z and terrain on initialization + _P(RangeFilter::z.idx, RangeFilter::z.idx) = z_var; + _P(RangeFilter::z.idx, RangeFilter::terrain.idx) = 0.f; + _P(RangeFilter::terrain.idx, RangeFilter::z.idx) = 0.f; + _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = z_var + dist_bottom_var; + + _Ht = sym::RangeValidationFilterH(); + _x(RangeFilter::z.idx) = z; _x(RangeFilter::terrain.idx) = z - dist_bottom; _initialized = true; _state = KinematicState::UNKNOWN; - _test_ratio_lpf.reset(2.f); + _test_ratio_lpf.reset(0); _t_since_first_sample = 0.f; - _test_ratio_lpf.setAlpha(0.2f); } void RangeFinderConsistencyCheck::update(const float &z, const float &z_var, const float &vz, const float &vz_var, @@ -60,60 +64,97 @@ void RangeFinderConsistencyCheck::update(const float &z, const float &z_var, con { const float dt = static_cast(time_us - _time_last_update_us) * 1e-6f; - if (_time_last_update_us == 0 || dt > 1.f) { + if (dt > 1.f) { _time_last_update_us = time_us; + _initialized = false; + return; + + } else if (!_initialized) { init(z, z_var, dist_bottom, dist_bottom_var); + _test_ratio_lpf.setParameters(dt, 1.f); return; } + // prediction step _time_last_update_us = time_us; - _x(RangeFilter::z.idx) -= dt * vz; - _P(0, 0) += dt * dt * vz_var + 0.001f; - _P(1, 1) += terrain_process_noise; + _P(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var + 0.001f; + _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += terrain_process_noise; - const Vector2f measurements(z, dist_bottom); + // iterate through both measurements (z and dist_bottom) + const Vector2f measurements{z, dist_bottom}; - Vector2f Kv{1.f, 0.f}; - Vector2f test_ratio{0.f, 0.f}; - Vector2f R{z_var, dist_bottom_var}; - Vector2f y; + for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) { - for (int i = 0; i < 2 ; i++) { - y = measurements - _H * _x; - Vector2f H = _H.row(i); - _innov_var = (H.transpose() * _P * H + R(i))(0, 0); - Kv(RangeFilter::terrain.idx) = _P(RangeFilter::terrain.idx, i) / _innov_var; + float hz; // Jacobian in respect to z + float ht; // Jacobian in respect to terrain + float R; + bool reject = false; - Vector2f PH = _P.row(i); + if (measurement_idx == 0) { + // direct state measurement + hz = 1.f; + ht = 0.f; + R = z_var; - for (int u = 0; u < RangeFilter::size; u++) { - for (int v = 0; v < RangeFilter::size; v++) { - _P(u, v) -= Kv(u) * PH(v); - } + } else if (measurement_idx == 1) { + hz = _Ht(0, 0); + ht = _Ht(0, 1); + R = dist_bottom_var; } - PH = _P.col(i); + // residual + const float measurement_pred = hz * _x(RangeFilter::z.idx) + ht * _x(RangeFilter::terrain.idx); + const float y = measurements(measurement_idx) - measurement_pred; - for (int u = 0; u < RangeFilter::size; u++) { - for (int v = 0; v <= u; v++) { - _P(u, v) = _P(u, v) - PH(u) * Kv(v) + Kv(u) * R(i) * Kv(v); - _P(v, u) = _P(u, v); - } + // innovation variance H * P * H^T + R + const float S = hz * (hz * _P(RangeFilter::z.idx, RangeFilter::z.idx) + + ht * _P(RangeFilter::terrain.idx, RangeFilter::z.idx)) + + ht * (hz * _P(RangeFilter::z.idx, RangeFilter::terrain.idx) + + ht * _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx)) + + R; + + // kalman gain K = P * H^T / S + float Kz = (hz * _P(RangeFilter::z.idx, RangeFilter::z.idx) + + ht * _P(RangeFilter::z.idx, RangeFilter::terrain.idx)) / S; + const float Kt = (hz * _P(RangeFilter::terrain.idx, RangeFilter::z.idx) + + ht * _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx)) / S; + + if (measurement_idx == 0) { + Kz = 1.f; + + } else if (measurement_idx == 1) { + _innov = y; + const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers + _test_ratio_lpf.update(sign(_innov) * test_ratio); + reject = test_ratio > 1.f; } - test_ratio(i) = fminf(sq(y(i)) / sq(_gate), 4.f); + if (!reject) { + // update step + _x(RangeFilter::z.idx) += Kz * y; + _x(RangeFilter::terrain.idx) += Kt * y; - if (i == (int)RangeFilter::z.idx && test_ratio(1) > 1.f) { - Kv(1) = 0.f; + // covariance update with Joseph form: + // P = (I - K H) P (I - K H)^T + K R K^T + Matrix2f I; + I.setIdentity(); + Matrix K; + K(0, 0) = Kz; + K(1, 0) = Kt; + Vector2f H0; + H0(0) = hz; + H0(1) = ht; + Matrix2f IKH0 = I - K * H0.transpose(); + _P = IKH0 * _P * IKH0.transpose() + K * R * K.transpose(); } - - _x = _x + Kv * y; } - _innov = y(RangeFilter::terrain.idx); - _test_ratio_lpf.update(sign(_innov) * test_ratio(1)); + evaluateState(dt, vz, vz_var); +} +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; @@ -147,7 +188,7 @@ void RangeFinderConsistencyCheck::run(const float &z, const float &vz, if (!_initialized || current_posD_reset_count != _last_posD_reset_count) { _last_posD_reset_count = current_posD_reset_count; - init(z, z_var, dist_bottom, dist_bottom_var); + _initialized = false; } update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us); 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 1efcc1067a..85716474c7 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 @@ -79,15 +79,16 @@ private: void update(const float &z, const float &z_var, const float &vz, const float &vz_var, const float &dist_bottom, const float &dist_bottom_var, const uint64_t &time_us); void init(const float &z, const float &z_var, const float &dist_bottom, const float &dist_bottom_var); + void evaluateState(const float &dt, const float &vz, const float &vz_var); matrix::SquareMatrix _P{}; - matrix::SquareMatrix _H{}; + matrix::Matrix _Ht{}; matrix::Vector2f _x{}; bool _initialized{false}; float _innov{0.f}; float _innov_var{0.f}; uint64_t _time_last_update_us{0}; AlphaFilter _test_ratio_lpf{}; - float _gate{1.f}; + float _gate{1.0f}; KinematicState _state{KinematicState::UNKNOWN}; float _t_since_first_sample{0.f}; uint8_t _last_posD_reset_count{0}; 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 11610763db..0de9e64d9e 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 @@ -217,7 +217,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } else { - ECL_WARN("stoppPing %s fusion, continuing conditions failing", HGT_SRC_NAME); + ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME); stopRngHgtFusion(); stopRngTerrFusion(); } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 971c63c3ae..23ef040073 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -422,7 +422,7 @@ struct parameters { float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion float range_valid_quality_s{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data - float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check + float range_kin_consistency_gate{0.5f}; ///< gate size used by the range finder kinematic consistency check float rng_fog{0.f}; ///< max distance which a blocked range sensor measures (fog, dirt) [m] Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m) @@ -622,8 +622,7 @@ uint64_t mag_heading_consistent : uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used uint64_t gnss_vel : 1; ///< 44 - true if GNSS velocity measurement fusion is intended - uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used - uint64_t rng_kin_unknown : 1; ///< xx - true when the range finder kinematic consistency check is not running + uint64_t rng_kin_unknown : 1; ///< 45 - true when the range finder kinematic consistency check is not running } flags; uint64_t value; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 038f033080..0567f9616d 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -721,7 +721,7 @@ def compute_gravity_z_innov_var_and_h( return (innov_var, H.T) -def range_validation_filter() -> sf.Matrix: +def range_validation_filter_h() -> sf.Matrix: state = Values( z=sf.Symbol("z"), @@ -729,9 +729,8 @@ def range_validation_filter() -> sf.Matrix: ) dist_bottom = state["z"] - state["terrain"] - H = sf.M22() - H[0, :] = sf.V1(state["z"]).jacobian(state.to_storage(), tangent_space=False) - H[1, :] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False) + H = sf.M12() + H[0, :] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False) return H @@ -766,6 +765,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=None) +generate_px4_function(range_validation_filter_h, output_names=None) 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.h similarity index 76% rename from src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h rename to src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h index 3eed628443..ba2a9abe4b 100644 --- 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.h @@ -13,15 +13,15 @@ namespace sym { /** * This function was autogenerated from a symbolic function. Do not modify by hand. * - * Symbolic function: range_validation_filter + * Symbolic function: range_validation_filter_h * * Args: * * Outputs: - * res: Matrix22 + * res: Matrix12 */ template -matrix::Matrix RangeValidationFilter() { +matrix::Matrix RangeValidationFilterH() { // Total ops: 0 // Input arrays @@ -29,13 +29,10 @@ matrix::Matrix RangeValidationFilter() { // Intermediate terms (0) // Output terms (1) - matrix::Matrix _res; - - _res.setZero(); + matrix::Matrix _res; _res(0, 0) = 1; - _res(1, 0) = 1; - _res(1, 1) = -1; + _res(0, 1) = -1; return _res; } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/params_range_finder.yaml b/src/modules/ekf2/params_range_finder.yaml index 97f60bdff4..90525178af 100644 --- a/src/modules/ekf2/params_range_finder.yaml +++ b/src/modules/ekf2/params_range_finder.yaml @@ -113,7 +113,7 @@ parameters: ...). Note: tune the range finder noise parameters (EKF2_RNG_NOISE and EKF2_RNG_SFE) before tuning this gate.' type: float - default: 1.0 + default: 0.5 unit: SD min: 0.1 max: 5.0 diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index 2fb59f0949..d39dc72a99 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -113,14 +113,8 @@ TEST_F(EkfHeightFusionTest, baroRef) // AND WHEN: the baro data increases const float baro_increment = 4.f; - const float baro_initial = _sensor_simulator._baro.getData(); - const float baro_inc = 0.2f; - - // increase slowly, height jump leads to invalid terrain estimate - while (_sensor_simulator._baro.getData() + baro_inc < baro_initial + baro_increment + 0.01f) { - _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_inc); - _sensor_simulator.runSeconds(3); - } + _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_increment); + _sensor_simulator.runSeconds(60); // THEN: the height estimate converges to the baro value // and the other height sources are getting their bias estimated diff --git a/src/modules/ekf2/test/test_EKF_terrain.cpp b/src/modules/ekf2/test/test_EKF_terrain.cpp index 327acd2c32..ba50c790ae 100644 --- a/src/modules/ekf2/test/test_EKF_terrain.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain.cpp @@ -259,11 +259,11 @@ TEST_F(EkfTerrainTest, testRngStartInAir) const float corr_terrain_vz = P(State::vel.idx + 2, State::terrain.idx) / sqrtf(_ekf->getVelocityVariance()(2) * var_terrain); - EXPECT_TRUE(corr_terrain_vz > 0.6f); + EXPECT_TRUE(fabsf(corr_terrain_vz) > 0.6f && fabsf(corr_terrain_vz) < 1.f); const float corr_terrain_z = P(State::pos.idx + 2, State::terrain.idx) / sqrtf(_ekf->getPositionVariance()(2) * var_terrain); - EXPECT_TRUE(corr_terrain_z > 0.8f); + EXPECT_TRUE(fabsf(corr_terrain_z) > 0.8f && fabsf(corr_terrain_z) < 1.f); const float corr_terrain_abias_z = P(State::accel_bias.idx + 2, State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain);