From bb17333bf4b98502bd26965ba0db3d5bbf0ef733 Mon Sep 17 00:00:00 2001
From: Marco Hauswirth <marco.hauswirth@auterion.com>
Date: Mon, 30 Dec 2024 10:18:24 +0100
Subject: [PATCH] reset state to unknown on init

---
 .../range_finder_consistency_check.cpp         |  8 ++++----
 .../range_finder/range_height_control.cpp      |  3 +--
 .../EKF/python/ekf_derivation/derivation.py    |  2 +-
 .../generated/range_validation_filter.h        | 18 +++++++++---------
 4 files changed, 15 insertions(+), 16 deletions(-)

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 b1505190c3..a57d8aeed6 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
@@ -43,13 +43,13 @@ 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, 0.f, 0.f, z_var + dist_bottom_var};
-	_P = Matrix<float, RangeFilter::size, RangeFilter::size>(p);
-	sym::RangeValidationFilter(&_H);
+	float p[4] = {z_var, z_var, z_var, z_var + dist_bottom_var};
+	_P = SquareMatrix<float, RangeFilter::size>(p);
+	_H = sym::RangeValidationFilter<float>();
 	_x(RangeFilter::z.idx) = z;
 	_x(RangeFilter::terrain.idx) = z - dist_bottom;
 	_initialized = true;
-	_state = _test_ratio_lpf.getState() < 1.f ? KinematicState::UNKNOWN : KinematicState::INCONSISTENT;
+	_state = KinematicState::UNKNOWN;
 	_test_ratio_lpf.reset(2.f);
 	_t_since_first_sample = 0.f;
 	_test_ratio_lpf.setAlpha(0.2f);
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 1647330b54..b7d8a3b3de 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
@@ -72,8 +72,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 
 				const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f);
 
-				if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion
-				    && _rng_consistency_check.isNotKinematicallyInconsistent()) {
+				if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
 					_rng_consistency_check.reset();
 				}
 
diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
index 3cfcf095ba..038f033080 100755
--- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
@@ -766,6 +766,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_function(range_validation_filter, 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
index 9e8028e02f..3eed628443 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
@@ -18,10 +18,10 @@ namespace sym {
  * Args:
  *
  * Outputs:
- *     H: Matrix22
+ *     res: Matrix22
  */
 template <typename Scalar>
-void RangeValidationFilter(matrix::Matrix<Scalar, 2, 2>* const H = nullptr) {
+matrix::Matrix<Scalar, 2, 2> RangeValidationFilter() {
   // Total ops: 0
 
   // Input arrays
@@ -29,15 +29,15 @@ void RangeValidationFilter(matrix::Matrix<Scalar, 2, 2>* const H = nullptr) {
   // Intermediate terms (0)
 
   // Output terms (1)
-  if (H != nullptr) {
-    matrix::Matrix<Scalar, 2, 2>& _h = (*H);
+  matrix::Matrix<Scalar, 2, 2> _res;
 
-    _h.setZero();
+  _res.setZero();
 
-    _h(0, 0) = 1;
-    _h(1, 0) = 1;
-    _h(1, 1) = -1;
-  }
+  _res(0, 0) = 1;
+  _res(1, 0) = 1;
+  _res(1, 1) = -1;
+
+  return _res;
 }  // NOLINT(readability/fn_size)
 
 // NOLINTNEXTLINE(readability/fn_size)