add symforce, adjust smaller things

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

@@ -36,6 +36,7 @@
  */
 
 #include <aid_sources/range_finder/range_finder_consistency_check.hpp>
+#include "ekf_derivation/generated/range_validation_filter.h"
 
 using namespace matrix;
 
@@ -44,11 +45,10 @@ void RangeFinderConsistencyCheck::init(float var_z, float var_terrain, float z,
 	_R.setZero();
 	_A.setIdentity();
 	float p[4] = {var_z, 0.f, 0.f, var_terrain};
-	_P = Matrix<float, 2, 2>(p);
+	_P = Matrix<float, RangeFilter::size, RangeFilter::size>(p);
-	float h[4] = {1.f, 0.f, -1.f, 1.f};
+	sym::RangeValidationFilter(&_H);
-	_H = Matrix<float, 2, 2>(h);
+	_x(RangeFilter::z.idx) = z;
-	_x(0) = z;
+	_x(RangeFilter::terrain.idx) = z + dist_bottom;
-	_x(1) = z + dist_bottom;
 	_initialized = true;
 	_sample_count = 0;
 	_state = KinematicState::UNKNOWN;
@@ -70,41 +70,43 @@ void RangeFinderConsistencyCheck::update(float z, float z_var, float vz, float v
 
 	_time_last_update_us = time_us;
 
-	_R(0, 0) = z_var;
+	_R(RangeFilter::z.idx, RangeFilter::z.idx) = z_var;
-	_R(1, 1) = dist_bottom_var;
+	_R(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = dist_bottom_var;
 
 	SquareMatrix<float, 2> Q;
-	const float process_noise = 0.001f;
+	Q(RangeFilter::z.idx, RangeFilter::z.idx) = dt * dt * vz_var + 0.001f;
-	Q(0, 0) = dt * dt * vz_var + process_noise;
+	Q(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = terrain_process_noise;
-	Q(1, 1) = process_noise;
+
-	_x(0) += dt * vz;
+	_x(RangeFilter::z.idx) += dt * vz;
 	_P = _A * _P * _A.transpose() + Q;
 
 	const Vector2f measurements(z, dist_bottom);
 	const Vector2f y = measurements - _H * _x;
 	const Matrix2f S = _H * _P * _H.transpose() + _R;
-	float test_ratio = fminf(abs(sq(y(1)) / (sq(_gate) * S(1, 1))), 2.f);
+	const float normalized_residual = (y.transpose() * S.I() * y)(0, 0);
+	float test_ratio = fminf(normalized_residual / sq(_gate), 2.f);
 
 	Matrix2f K = _P * _H.transpose() * S.I();
 
-	K(0, 0) = 1.f;
+	K(RangeFilter::z.idx, RangeFilter::z.idx) = 1.f;
-	K(0, 1) = 0.f;
+	K(RangeFilter::z.idx, RangeFilter::terrain.idx) = 0.f;
 
 	if (test_ratio > 1.f) {
-		K(1, 0) = 0.f;
+		K(RangeFilter::terrain.idx, RangeFilter::z.idx) = 0.f;
-		K(1, 1) = 0.f;
+		K(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = 0.f;
 	}
 
 	_x = _x + K * y;
 	_P = _P - K * _H * _P;
 	_P = 0.5f * (_P + _P.transpose()); // Ensure symmetry
+	_innov = y(RangeFilter::terrain.idx);
+	_innov_var = S(RangeFilter::terrain.idx, RangeFilter::terrain.idx);
 
-	_innov = y(1);
+	_test_ratio_lpf.update(sign(_innov) * test_ratio);
-	_innov_var = S(1, 1);
 
 	if (_sample_count++ > _min_nr_of_samples) {
 
-		if (_test_ratio_lpf.update(test_ratio) < 1.f) {
+		if (abs(_test_ratio_lpf.getState()) < 1.f) {
 			_state = KinematicState::CONSISTENT;
 
 		} else {
@@ -112,8 +114,6 @@ void RangeFinderConsistencyCheck::update(float z, float z_var, float vz, float v
 			_state = KinematicState::INCONSISTENT;
 		}
 
-	} else {
-		_test_ratio_lpf.update(test_ratio);
 	}
 }
 

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

@@ -62,10 +62,7 @@ public:
 
 	bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; }
 	bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT || _fixed_wing; }
-	void update(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var,
+	void reset()
-			  uint64_t time_us);
-	void init(float var_z, float var_terrain, float z, float dist_bottom);
-	void stop()
 	{
 		if (_initialized) {
 			if (_state == KinematicState::CONSISTENT) {
@@ -77,28 +74,18 @@ public:
 	}
 	int getConsistencyState() const { return static_cast<int>(_state); }
 
-	bool isRunning() { return _initialized; }
-
-	void setNotMoving()
-	{
-		if (_state == KinematicState::CONSISTENT) {
-			_state = KinematicState::UNKNOWN;
-		}
-	}
-
-	void setFixedWing(bool is_fixed_wing, float gate)
-	{
-		_fixed_wing = is_fixed_wing;
-		_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);
 
 	uint8_t current_posD_reset_count{0};
+	float terrain_process_noise{0.0f};
+	bool horizontal_motion{false};
 
 private:
 
+	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 var_z, float var_terrain, float z, float dist_bottom);
 	matrix::SquareMatrix<float, 2> _R{};
 	matrix::SquareMatrix<float, 2> _P{};
 	matrix::SquareMatrix<float, 2> _A{};
@@ -118,4 +105,11 @@ private:
 	uint8_t _last_posD_reset_count{0};
 };
 
+namespace RangeFilter
+{
+static constexpr estimator::IdxDof z{0, 1};
+static constexpr estimator::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

@@ -69,27 +69,13 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				const float dist_var = getRngVar();
 				_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
 
-				if (_control_status.flags.fixed_wing) {
+				const bool updated_horizontal_motion = (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
-					_rng_consistency_check.setFixedWing(true, 2.0f * _params.range_kin_consistency_gate);
-					_rng_consistency_check.run(_state.pos(2), _state.vel(2), P, _range_sensor.getRange(), dist_var, imu_sample.time_us);
 
-				} else {
+				if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
-					_rng_consistency_check.setFixedWing(false, _params.range_kin_consistency_gate);
+					_rng_consistency_check.reset();
-					const bool horizontal_motion = (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
-					const bool vertical_motion = sq(_state.vel(2)) > fmaxf(P(State::vel.idx + 2, State::vel.idx + 2), 0.1f);
-
-					if (horizontal_motion) {
-						if (_rng_consistency_check.isRunning()) {
-							_rng_consistency_check.stop();
-						}
-
-					} else if (vertical_motion) {
-						_rng_consistency_check.run(_state.pos(2), _state.vel(2), P, _range_sensor.getRange(), dist_var, imu_sample.time_us);
-
-					} else {
-						_rng_consistency_check.setNotMoving();
-					}
 				}
+				_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);
 			}
 
 		} else {
@@ -103,7 +89,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					_range_sensor.setValidity(true); // bypass the checks
 
 				} else {
-					_rng_consistency_check.stop();
+					_rng_consistency_check.reset();
 				}
 			}
 		}
@@ -228,7 +214,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				}
 
 			} else {
-				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
+				ECL_WARN("stoppPing %s fusion, continuing conditions failing", HGT_SRC_NAME);
 				stopRngHgtFusion();
 				stopRngTerrFusion();
 			}

src/modules/ekf2/EKF/covariance.cpp

@@ -226,6 +226,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		// process noise due to terrain gradient
 		terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.ekf2_terr_grad) * (sq(_state.vel(0)) + sq(_state.vel(
 						 1)));
+		_rng_consistency_check.terrain_process_noise = terrain_process_noise;
 		P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
 	}
 

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -721,6 +721,22 @@ def compute_gravity_z_innov_var_and_h(
 
     return (innov_var, H.T)
 
+def range_validation_filter() -> sf.matrix:
+
+    z = sf.Symbol("z")
+    dist_bottom = sf.Symbol("dist_bottom")
+    terrain = dist_bottom - z
+    state = sf.V2.symbolic("state")
+    state_indices = {"z": 0, "dist_bottom": 1}
+    state[state_indices["z"]] = z
+    state[state_indices["dist_bottom"]] = dist_bottom
+
+    H = sf.M22()
+    H[0, :] = sf.V1(z).jacobian(state, tangent_space=False)
+    H[1, :] = sf.V1(terrain).jacobian(state, tangent_space=False)
+
+    return H
+
 print("Derive EKF2 equations...")
 generate_px4_function(predict_covariance, output_names=None)
 
@@ -752,5 +768,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_state(State, tangent_idx)

src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h

@@ -0,0 +1,44 @@
+// -----------------------------------------------------------------------------
+// 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
+ *
+ * Args:
+ *
+ * Outputs:
+ *     H: Matrix22
+ */
+template <typename Scalar>
+void RangeValidationFilter(matrix::Matrix<Scalar, 2, 2>* const H = nullptr) {
+  // Total ops: 0
+
+  // Input arrays
+
+  // Intermediate terms (0)
+
+  // Output terms (1)
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 2, 2>& _h = (*H);
+
+    _h.setZero();
+
+    _h(0, 0) = 1;
+    _h(1, 0) = -1;
+    _h(1, 1) = 1;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym