clean up range-filter fusion step

Author: haumarco

msg/EstimatorStatusFlags.msg

@@ -48,7 +48,7 @@ bool cs_opt_flow_terrain        # 40 - true if we are fusing flow data for terra
 bool cs_valid_fake_pos          # 41 - true if a valid constant position is being fused
 bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
 bool cs_baro_fault	        # 43 - true when the current baro has been declared faulty and is no longer being used
-bool cs_rng_kin_unknown      	# 43 - true when the range finder kinematic consistency check is not running
+bool cs_rng_kin_unknown      	# 44 - true when the range finder kinematic consistency check is not running
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

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

@@ -36,84 +36,125 @@
  */
 
 #include <aid_sources/range_finder/range_finder_consistency_check.hpp>
-#include "ekf_derivation/generated/range_validation_filter.h"
+#include "ekf_derivation/generated/range_validation_filter_h.h"
 
 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, z_var, z_var, z_var + dist_bottom_var};
-	_P = SquareMatrix<float, RangeFilter::size>(p);
-	_H = sym::RangeValidationFilter<float>();
+	// assume no correlation between z and terrain on initialization
+	_P(RangeFilter::z.idx, RangeFilter::z.idx) = z_var;
+	_P(RangeFilter::z.idx, RangeFilter::terrain.idx) = 0.f;
+	_P(RangeFilter::terrain.idx, RangeFilter::z.idx) = 0.f;
+	_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = z_var + dist_bottom_var;
+
+	_Ht = sym::RangeValidationFilterH<float>();
+
 	_x(RangeFilter::z.idx) = z;
 	_x(RangeFilter::terrain.idx) = z - dist_bottom;
 	_initialized = true;
 	_state = KinematicState::UNKNOWN;
-	_test_ratio_lpf.reset(2.f);
+	_test_ratio_lpf.reset(0);
 	_t_since_first_sample = 0.f;
-	_test_ratio_lpf.setAlpha(0.2f);
 }
 
 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;
 
-	if (_time_last_update_us == 0 || dt > 1.f) {
+	if (dt > 1.f) {
 		_time_last_update_us = time_us;
+		_initialized = false;
+		return;
+
+	} else if (!_initialized) {
 		init(z, z_var, dist_bottom, dist_bottom_var);
+		_test_ratio_lpf.setParameters(dt, 1.f);
 		return;
 	}
 
+	// prediction step
 	_time_last_update_us = time_us;
-
 	_x(RangeFilter::z.idx) -= dt * vz;
-	_P(0, 0) += dt * dt * vz_var + 0.001f;
-	_P(1, 1) += terrain_process_noise;
+	_P(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var + 0.001f;
+	_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += terrain_process_noise;
 
-	const Vector2f measurements(z, dist_bottom);
+	// iterate through both measurements (z and dist_bottom)
+	const Vector2f measurements{z, dist_bottom};
 
-	Vector2f Kv{1.f, 0.f};
-	Vector2f test_ratio{0.f, 0.f};
-	Vector2f R{z_var, dist_bottom_var};
-	Vector2f y;
+	for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) {
 
-	for (int i = 0; i < 2 ; i++) {
-		y = measurements - _H * _x;
-		Vector2f H = _H.row(i);
-		_innov_var = (H.transpose() * _P * H + R(i))(0, 0);
-		Kv(RangeFilter::terrain.idx) = _P(RangeFilter::terrain.idx, i) / _innov_var;
+		float hz; // Jacobian in respect to z
+		float ht; // Jacobian in respect to terrain
+		float R;
+		bool reject = false;
 
-		Vector2f PH = _P.row(i);
+		if (measurement_idx == 0) {
+			// direct state measurement
+			hz = 1.f;
+			ht = 0.f;
+			R = z_var;
 
-		for (int u = 0; u < RangeFilter::size; u++) {
-			for (int v = 0; v < RangeFilter::size; v++) {
-				_P(u, v) -= Kv(u) * PH(v);
-			}
+		} else if (measurement_idx == 1) {
+			hz = _Ht(0, 0);
+			ht = _Ht(0, 1);
+			R = dist_bottom_var;
 		}
 
-		PH = _P.col(i);
-
-		for (int u = 0; u < RangeFilter::size; u++) {
-			for (int v = 0; v <= u; v++) {
-				_P(u, v) = _P(u, v) - PH(u) * Kv(v) + Kv(u) * R(i) * Kv(v);
-				_P(v, u) = _P(u, v);
-			}
+		// residual
+		const float measurement_pred = hz * _x(RangeFilter::z.idx) + ht * _x(RangeFilter::terrain.idx);
+		const float y = measurements(measurement_idx) - measurement_pred;
+
+		// innovation variance H * P * H^T + R
+		const float S = hz * (hz * _P(RangeFilter::z.idx, RangeFilter::z.idx)
+				      + ht * _P(RangeFilter::terrain.idx, RangeFilter::z.idx))
+				+ ht * (hz * _P(RangeFilter::z.idx, RangeFilter::terrain.idx)
+					+ ht * _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx))
+				+ R;
+
+		// kalman gain K = P * H^T / S
+		float Kz = (hz * _P(RangeFilter::z.idx, RangeFilter::z.idx)
+			    + ht * _P(RangeFilter::z.idx, RangeFilter::terrain.idx)) / S;
+		const float Kt = (hz * _P(RangeFilter::terrain.idx, RangeFilter::z.idx)
+				  + ht * _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx)) / S;
+
+		if (measurement_idx == 0) {
+			Kz = 1.f;
+
+		} else if (measurement_idx == 1) {
+			_innov = y;
+			const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers
+			_test_ratio_lpf.update(sign(_innov) * test_ratio);
+			reject = test_ratio > 1.f;
 		}
 
-		test_ratio(i) = fminf(sq(y(i)) / sq(_gate), 4.f);
-
-		if (i == (int)RangeFilter::z.idx && test_ratio(1) > 1.f) {
-			Kv(1) = 0.f;
+		if (!reject) {
+			// update step
+			_x(RangeFilter::z.idx)       += Kz * y;
+			_x(RangeFilter::terrain.idx) += Kt * y;
+
+			// covariance update with Joseph form:
+			// P = (I - K H) P (I - K H)^T + K R K^T
+			Matrix2f I;
+			I.setIdentity();
+			Matrix<float, 2, 1> K;
+			K(0, 0) = Kz;
+			K(1, 0) = Kt;
+			Vector2f H0;
+			H0(0) = hz;
+			H0(1) = ht;
+			Matrix2f IKH0 = I - K * H0.transpose();
+			_P = IKH0 * _P * IKH0.transpose() + K * R * K.transpose();
 		}
-
-		_x = _x + Kv * y;
 	}
 
-	_innov = y(RangeFilter::terrain.idx);
-	_test_ratio_lpf.update(sign(_innov) * test_ratio(1));
+	evaluateState(dt, vz, 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) {
 		_t_since_first_sample = 2.0f;
@@ -147,7 +188,7 @@ void RangeFinderConsistencyCheck::run(const float &z, const float &vz,
 
 	if (!_initialized || current_posD_reset_count != _last_posD_reset_count) {
 		_last_posD_reset_count = current_posD_reset_count;
-		init(z, z_var, dist_bottom, dist_bottom_var);
+		_initialized = false;
 	}
 
 	update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us);

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

@@ -79,15 +79,16 @@ class RangeFinderConsistencyCheck final
 	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);
 	matrix::SquareMatrix<float, 2> _P{};
-	matrix::SquareMatrix<float, 2> _H{};
+	matrix::Matrix<float, 1, 2> _Ht{};
 	matrix::Vector2f _x{};
 	bool _initialized{false};
 	float _innov{0.f};
 	float _innov_var{0.f};
 	uint64_t _time_last_update_us{0};
 	AlphaFilter<float> _test_ratio_lpf{};
-	float _gate{1.f};
+	float _gate{1.0f};
 	KinematicState _state{KinematicState::UNKNOWN};
 	float _t_since_first_sample{0.f};
 	uint8_t _last_posD_reset_count{0};

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

@@ -217,7 +217,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				}
 
 			} else {
-				ECL_WARN("stoppPing %s fusion, continuing conditions failing", HGT_SRC_NAME);
+				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
 				stopRngHgtFusion();
 				stopRngTerrFusion();
 			}

src/modules/ekf2/EKF/common.h

@@ -422,7 +422,7 @@ struct parameters {
 	float range_aid_innov_gate{1.0f};       ///< gate size used for innovation consistency checks for range aid fusion
 	float range_valid_quality_s{1.0f};      ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
 	float range_cos_max_tilt{0.7071f};      ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
-	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
+	float range_kin_consistency_gate{0.5f}; ///< gate size used by the range finder kinematic consistency check
 	float rng_fog{0.f};                 	///< max distance which a blocked range sensor measures (fog, dirt) [m]
 
 	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
@@ -620,8 +620,8 @@ uint64_t mag_heading_consistent  :
 		uint64_t opt_flow_terrain        : 1; ///< 40 - true if we are fusing flow data for terrain
 		uint64_t valid_fake_pos          : 1; ///< 41 - true if a valid constant position is being fused
 		uint64_t constant_pos            : 1; ///< 42 - true if the vehicle is at a constant position
-		uint64_t baro_fault	      : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
-		uint64_t rng_kin_unknown	 : 1; ///< xx - true when the range finder kinematic consistency check is not running
+		uint64_t baro_fault	      	 : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
+		uint64_t rng_kin_unknown	 : 1; ///< 44 - true when the range finder kinematic consistency check is not running
 
 	} flags;
 	uint64_t value;

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

@@ -721,17 +721,16 @@ def compute_gravity_z_innov_var_and_h(
 
     return (innov_var, H.T)
 
-def range_validation_filter() -> sf.Matrix:
+def range_validation_filter_h() -> sf.Matrix:
 
     state = Values(
         z=sf.Symbol("z"),
         terrain=sf.Symbol("terrain")
     )
     dist_bottom = state["z"] - state["terrain"]
 
-    H = sf.M22()
-    H[0, :] = sf.V1(state["z"]).jacobian(state.to_storage(), tangent_space=False)
-    H[1, :] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False)
+    H = sf.M12()
+    H[0, :] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False)
 
     return H
 
@@ -766,6 +765,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=None)
+generate_px4_function(range_validation_filter_h, output_names=None)
 
 generate_px4_state(State, tangent_idx)

src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h renamed to src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h

@@ -13,29 +13,26 @@ namespace sym {
 /**
  * This function was autogenerated from a symbolic function. Do not modify by hand.
  *
- * Symbolic function: range_validation_filter
+ * Symbolic function: range_validation_filter_h
  *
  * Args:
  *
  * Outputs:
- *     res: Matrix22
+ *     res: Matrix12
  */
 template <typename Scalar>
-matrix::Matrix<Scalar, 2, 2> RangeValidationFilter() {
+matrix::Matrix<Scalar, 1, 2> RangeValidationFilterH() {
   // Total ops: 0
 
   // Input arrays
 
   // Intermediate terms (0)
 
   // Output terms (1)
-  matrix::Matrix<Scalar, 2, 2> _res;
-
-  _res.setZero();
+  matrix::Matrix<Scalar, 1, 2> _res;
 
   _res(0, 0) = 1;
-  _res(1, 0) = 1;
-  _res(1, 1) = -1;
+  _res(0, 1) = -1;
 
   return _res;
 }  // NOLINT(readability/fn_size)

src/modules/ekf2/params_range_finder.yaml

@@ -113,7 +113,7 @@ parameters:
           ...). Note: tune the range finder noise parameters (EKF2_RNG_NOISE and EKF2_RNG_SFE)
           before tuning this gate.'
       type: float
-      default: 1.0
+      default: 0.5
       unit: SD
       min: 0.1
       max: 5.0

src/modules/ekf2/test/test_EKF_height_fusion.cpp

@@ -113,14 +113,8 @@ TEST_F(EkfHeightFusionTest, baroRef)
 
 	// AND WHEN: the baro data increases
 	const float baro_increment = 4.f;
-	const float baro_initial = _sensor_simulator._baro.getData();
-	const float baro_inc = 0.2f;
-
-	// increase slowly, height jump leads to invalid terrain estimate
-	while (_sensor_simulator._baro.getData() + baro_inc < baro_initial + baro_increment + 0.01f) {
-		_sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_inc);
-		_sensor_simulator.runSeconds(3);
-	}
+	_sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_increment);
+	_sensor_simulator.runSeconds(60);
 
 	// THEN: the height estimate converges to the baro value
 	// and the other height sources are getting their bias estimated

src/modules/ekf2/test/test_EKF_terrain.cpp

@@ -259,11 +259,11 @@ TEST_F(EkfTerrainTest, testRngStartInAir)
 
 	const float corr_terrain_vz = P(State::vel.idx + 2,
 					State::terrain.idx) / sqrtf(_ekf->getVelocityVariance()(2) * var_terrain);
-	EXPECT_TRUE(corr_terrain_vz > 0.6f);
+	EXPECT_TRUE(fabsf(corr_terrain_vz) > 0.6f && fabsf(corr_terrain_vz) < 1.f);
 
 	const float corr_terrain_z = P(State::pos.idx + 2,
 				       State::terrain.idx) / sqrtf(_ekf->getPositionVariance()(2) * var_terrain);
-	EXPECT_TRUE(corr_terrain_z > 0.8f);
+	EXPECT_TRUE(fabsf(corr_terrain_z) > 0.8f && fabsf(corr_terrain_z) < 1.f);
 
 	const float corr_terrain_abias_z = P(State::accel_bias.idx + 2,
 					     State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain);