From 42ea74c785879492390da1ce729004e786d111cd 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 ++++----
 .../EKF/python/ekf_derivation/derivation.py    |  2 +-
 .../generated/range_validation_filter.h        | 18 +++++++++---------
 3 files changed, 14 insertions(+), 14 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 b1505190c30b..a57d8aeed61e 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/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
index 0e682239194b..040cab0811be 100755
--- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
@@ -770,6 +770,6 @@ def range_validation_filter() -> sf.Matrix:
 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 9e8028e02f7b..3eed6284437d 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)