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 6fcee9f00f..eb04810e24 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 @@ -37,20 +37,15 @@ #include #include "ekf_derivation/generated/range_validation_filter_h.h" +#include "ekf_derivation/generated/range_validation_filter_P_init.h" using namespace matrix; -void RangeFinderConsistencyCheck::init(const float &z, const float &z_var, const float &dist_bottom, - const float &dist_bottom_var) +void RangeFinderConsistencyCheck::init(const float z, const float z_var, const float dist_bottom, + const float dist_bottom_var) { - // assume no correlation between z and dist_bottom - // values defined with symforce for H[1, -1] , P = [z_var, 0; 0, z_var + dist_bottom_var], K = [0,1/H(1)] - _P(RangeFilter::z.idx, RangeFilter::z.idx) = z_var; - _P(RangeFilter::z.idx, RangeFilter::terrain.idx) = z_var; - _P(RangeFilter::terrain.idx, RangeFilter::z.idx) = z_var; - _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = dist_bottom_var + z_var; - - _Ht = sym::RangeValidationFilterH().transpose(); + _P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var); + _Ht = sym::RangeValidationFilterH(); _x(RangeFilter::z.idx) = z; _x(RangeFilter::terrain.idx) = z - dist_bottom; @@ -60,8 +55,8 @@ void RangeFinderConsistencyCheck::init(const float &z, const float &z_var, const _t_since_first_sample = 0.f; } -void RangeFinderConsistencyCheck::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 RangeFinderConsistencyCheck::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) { const float dt = static_cast(time_us - _time_last_update_us) * 1e-6f; @@ -139,7 +134,7 @@ void RangeFinderConsistencyCheck::update(const float &z, const float &z_var, con evaluateState(dt, vz, vz_var); } -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 if (_t_since_first_sample + dt > 1.0f) { @@ -165,13 +160,9 @@ void RangeFinderConsistencyCheck::evaluateState(const float &dt, const float &vz } } -void RangeFinderConsistencyCheck::run(const float &z, const float &vz, - const matrix::SquareMatrix &P, - const float &dist_bottom, const float &dist_bottom_var, uint64_t time_us) +void RangeFinderConsistencyCheck::run(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) { - const float z_var = P(estimator::State::pos.idx + 2, estimator::State::pos.idx + 2); - const float vz_var = P(estimator::State::vel.idx + 2, estimator::State::vel.idx + 2); - if (!_initialized || current_posD_reset_count != _last_posD_reset_count) { _last_posD_reset_count = current_posD_reset_count; _initialized = false; 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 4275510933..a8334c9e87 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 @@ -41,7 +41,6 @@ #define EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP #include -#include class RangeFinderConsistencyCheck final @@ -62,9 +61,9 @@ public: bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; } bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT; } void setGate(const float gate) { _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); - void set_terrain_process_noise(const float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; } + void run(float z, float z_var, float vz, float vz_var, + float dist_bottom, float dist_bottom_var, uint64_t time_us); + void set_terrain_process_noise(float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; } void reset() { _state = (_initialized && _state == KinematicState::CONSISTENT) ? KinematicState::UNKNOWN : _state; @@ -76,10 +75,10 @@ public: 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); + 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 z, float z_var, float dist_bottom, float dist_bottom_var); + void evaluateState(float dt, float vz, float vz_var); float _terrain_process_noise{0.0f}; matrix::SquareMatrix _P{}; matrix::Vector2f _Ht{}; @@ -98,9 +97,10 @@ private: namespace RangeFilter { -static constexpr estimator::IdxDof z{0, 1}; -static constexpr estimator::IdxDof terrain{1, 1}; +struct IdxDof { unsigned idx; unsigned dof; }; +static constexpr IdxDof z{0, 1}; +static constexpr 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 0de9e64d9e..148b598c90 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 @@ -77,7 +77,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } _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); + const float z_var = P(State::pos.idx + 2, State::pos.idx + 2); + const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2); + _rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(), + dist_var, imu_sample.time_us); } } else { diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 0567f9616d..7c8baf1fec 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -729,11 +729,29 @@ def range_validation_filter_h() -> sf.Matrix: ) dist_bottom = state["z"] - state["terrain"] - H = sf.M12() - H[0, :] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False) + H = sf.M21() + H[:, 0] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False).transpose() return H +def range_validation_filter_P_init( + z_var: sf.Scalar, + dist_bottom_var: sf.Scalar + ) -> sf.Matrix: + + H = range_validation_filter_h().T + # enforce terrain to match the measurement + K = sf.V2(0, 1/H[1]) + R = sf.V1(dist_bottom_var) + + # dist_bottom = z - terrain + P = sf.M22.diag([z_var, z_var + dist_bottom_var]) + I = sf.M22.eye() + # Joseph form + P = (I - K * H) * P * (I - K * H).T + K * R * K.T + + return P + print("Derive EKF2 equations...") generate_px4_function(predict_covariance, output_names=None) @@ -766,5 +784,6 @@ generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", " 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_h, output_names=None) +generate_px4_function(range_validation_filter_P_init, output_names=None) generate_px4_state(State, tangent_idx) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_P_init.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_P_init.h new file mode 100644 index 0000000000..b7e4e62e73 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_P_init.h @@ -0,0 +1,46 @@ +// ----------------------------------------------------------------------------- +// 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_p_init + * + * Args: + * z_var: Scalar + * dist_bottom_var: Scalar + * + * Outputs: + * res: Matrix22 + */ +template +matrix::Matrix RangeValidationFilterPInit(const Scalar z_var, + const Scalar dist_bottom_var) { + // Total ops: 1 + + // Input arrays + + // Intermediate terms (0) + + // Output terms (1) + matrix::Matrix _res; + + _res(0, 0) = z_var; + _res(1, 0) = z_var; + _res(0, 1) = z_var; + _res(1, 1) = dist_bottom_var + z_var; + + return _res; +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h index ba2a9abe4b..91deb51ed9 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h @@ -18,10 +18,10 @@ namespace sym { * Args: * * Outputs: - * res: Matrix12 + * res: Matrix21 */ template -matrix::Matrix RangeValidationFilterH() { +matrix::Matrix RangeValidationFilterH() { // Total ops: 0 // Input arrays @@ -29,10 +29,10 @@ matrix::Matrix RangeValidationFilterH() { // Intermediate terms (0) // Output terms (1) - matrix::Matrix _res; + matrix::Matrix _res; _res(0, 0) = 1; - _res(0, 1) = -1; + _res(1, 0) = -1; return _res; } // NOLINT(readability/fn_size)