reset state to unknown on init

PX4 · Dec 30, 2024 · 42ea74c · 42ea74c
1 parent debb615
commit 42ea74c
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 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
@@ -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
@@ -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
@@ -18,26 +18,26 @@ 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
 
   // 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)