fix uncertainty initialization

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

@@ -43,19 +43,20 @@ using namespace matrix;
 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 terrain on initialization
+	// 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) = 0.f;
-	_P(RangeFilter::terrain.idx, RangeFilter::z.idx) = 0.f;
-	_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = z_var + dist_bottom_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>();
+	_Ht = sym::RangeValidationFilterH<float>().transpose();
 
 	_x(RangeFilter::z.idx) = z;
 	_x(RangeFilter::terrain.idx) = z - dist_bottom;
 	_initialized = true;
 	_state = KinematicState::UNKNOWN;
-	_test_ratio_lpf.reset(0);
+	_test_ratio_lpf.reset(0.f);
 	_t_since_first_sample = 0.f;
 }
 
@@ -71,82 +72,67 @@ void RangeFinderConsistencyCheck::update(const float &z, const float &z_var, con
 
 	} 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(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var + 0.001f;
-	_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += terrain_process_noise;
+	_P(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var;
+	_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += _terrain_process_noise;
 
 	// iterate through both measurements (z and dist_bottom)
 	const Vector2f measurements{z, dist_bottom};
 
 	for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) {
 
-		float hz; // Jacobian in respect to z
-		float ht; // Jacobian in respect to terrain
 		float R;
 		bool reject = false;
+		Vector2f H;
 
 		if (measurement_idx == 0) {
 			// direct state measurement
-			hz = 1.f;
-			ht = 0.f;
+			H(RangeFilter::z.idx) = 1.f;
+			H(RangeFilter::terrain.idx) = 0.f;
 			R = z_var;
 
 		} else if (measurement_idx == 1) {
-			hz = _Ht(0, 0);
-			ht = _Ht(0, 1);
+			H(RangeFilter::z.idx) = _Ht(0);
+			H(RangeFilter::terrain.idx) = _Ht(1);
 			R = dist_bottom_var;
 		}
 
 		// residual
-		const float measurement_pred = hz * _x(RangeFilter::z.idx) + ht * _x(RangeFilter::terrain.idx);
+		const float measurement_pred = H * _x;
 		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;
+		// for H as col-vector:
+		// innovation variance S = H^T * P * H + R
+		// kalman gain K = P * H / S
+		const float S = (H.transpose() * _P * H + R)(0, 0);
+		Vector2f K = (_P * H / S);
 
 		if (measurement_idx == 0) {
-			Kz = 1.f;
+			K(RangeFilter::z.idx) = 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);
+			_test_ratio_lpf.update(sign(_innov) * test_ratio, dt);
 			reject = test_ratio > 1.f;
 		}
 
 		if (!reject) {
 			// update step
-			_x(RangeFilter::z.idx)       += Kz * y;
-			_x(RangeFilter::terrain.idx) += Kt * y;
+			_x(RangeFilter::z.idx)       += K(RangeFilter::z.idx) * y;
+			_x(RangeFilter::terrain.idx) += K(RangeFilter::terrain.idx) * 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();
+			Matrix2f IKH = I - K.multiplyByTranspose(H);
+			_P = IKH * _P * IKH.transpose() + (K * R).multiplyByTranspose(K);
 		}
 	}
 

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

@@ -64,6 +64,7 @@ public:
 	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 reset()
 	{
 		_state = (_initialized && _state == KinematicState::CONSISTENT) ? KinematicState::UNKNOWN : _state;
@@ -71,7 +72,6 @@ public:
 	}
 
 	uint8_t current_posD_reset_count{0};
-	float terrain_process_noise{0.0f};
 	bool horizontal_motion{false};
 
 private:
@@ -80,14 +80,16 @@ private:
 		    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);
+	float _terrain_process_noise{0.0f};
 	matrix::SquareMatrix<float, 2> _P{};
-	matrix::Matrix<float, 1, 2> _Ht{};
+	matrix::Vector2f _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{};
+	static constexpr float time_constant{1.f};
+	AlphaFilter<float> _test_ratio_lpf{time_constant};
 	float _gate{1.0f};
 	KinematicState _state{KinematicState::UNKNOWN};
 	float _t_since_first_sample{0.f};

src/modules/ekf2/EKF/covariance.cpp

@@ -226,7 +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.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(
 						 1)));
-		_rng_consistency_check.terrain_process_noise = terrain_process_noise;
+		_rng_consistency_check.set_terrain_process_noise(terrain_process_noise);
 		P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
 	}