* clean up method calls

* move P-init to ekf-derivation script

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

@@ -37,20 +37,15 @@
 
 #include <aid_sources/range_finder/range_finder_consistency_check.hpp>
 #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<float>().transpose();
+	_P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
+	_Ht = sym::RangeValidationFilterH<float>();
 
 	_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<float>(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<float, estimator::State::size> &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;

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 <mathlib/math/filter/AlphaFilter.hpp>
-#include <ekf_derivation/generated/state.h>
 
 
 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<float, estimator::State::size> &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<float, 2> _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

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 {

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)

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 <matrix/math.hpp>
+
+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 <typename Scalar>
+matrix::Matrix<Scalar, 2, 2> RangeValidationFilterPInit(const Scalar z_var,
+                                                       const Scalar dist_bottom_var) {
+  // Total ops: 1
+
+  // Input arrays
+
+  // Intermediate terms (0)
+
+  // Output terms (1)
+  matrix::Matrix<Scalar, 2, 2> _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

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 <typename Scalar>
-matrix::Matrix<Scalar, 1, 2> RangeValidationFilterH() {
+matrix::Matrix<Scalar, 2, 1> RangeValidationFilterH() {
   // Total ops: 0
 
   // Input arrays
@@ -29,10 +29,10 @@ matrix::Matrix<Scalar, 1, 2> RangeValidationFilterH() {
   // Intermediate terms (0)
 
   // Output terms (1)
-  matrix::Matrix<Scalar, 1, 2> _res;
+  matrix::Matrix<Scalar, 2, 1> _res;
 
   _res(0, 0) = 1;
-  _res(0, 1) = -1;
+  _res(1, 0) = -1;
 
   return _res;
 }  // NOLINT(readability/fn_size)