overhaul approach to work on unit tests and snow/blocking/normal flights

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

@@ -40,94 +40,114 @@
 
 using namespace matrix;
 
-void RangeFinderConsistencyCheck::init(float var_z, float var_terrain, float z, float dist_bottom)
+void RangeFinderConsistencyCheck::init(const float &z, const float &z_var, const float &dist_bottom,
+				       const float &dist_bottom_var)
 {
-	_R.setZero();
-	_A.setIdentity();
-	float p[4] = {var_z, 0.f, 0.f, var_terrain};
+	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);
 	_x(RangeFilter::z.idx) = z;
-	_x(RangeFilter::terrain.idx) = z + dist_bottom;
+	_x(RangeFilter::terrain.idx) = z - dist_bottom;
 	_initialized = true;
-	_sample_count = 0;
-	_state = KinematicState::UNKNOWN;
+	_state = _test_ratio_lpf.getState() < 1.f ? KinematicState::UNKNOWN : KinematicState::INCONSISTENT;
+	_test_ratio_lpf.reset(2.f);
+	_t_since_first_sample = 0.f;
+	_test_ratio_lpf.setAlpha(0.2f);
 }
 
-void RangeFinderConsistencyCheck::update(float z, float z_var, float vz, float vz_var, float dist_bottom,
-		float dist_bottom_var, 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;
 
 	if (_time_last_update_us == 0 || dt > 1.f) {
 		_time_last_update_us = time_us;
+		init(z, z_var, dist_bottom, dist_bottom_var);
 		return;
 	}
 
-	if (_min_nr_of_samples == 0) {
-		_min_nr_of_samples = (int)(1.f / dt);
-	}
-
 	_time_last_update_us = time_us;
 
-	_R(RangeFilter::z.idx, RangeFilter::z.idx) = z_var;
-	_R(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = dist_bottom_var;
-
-	SquareMatrix<float, 2> Q;
-	Q(RangeFilter::z.idx, RangeFilter::z.idx) = dt * dt * vz_var + 0.001f;
-	Q(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = terrain_process_noise;
-
-	_x(RangeFilter::z.idx) += dt * vz;
-	_P = _A * _P * _A.transpose() + Q;
+	_x(RangeFilter::z.idx) -= dt * vz;
+	_P(0, 0) += dt * dt * vz_var + 0.001f;
+	_P(1, 1) += terrain_process_noise;
 
 	const Vector2f measurements(z, dist_bottom);
-	const Vector2f y = measurements - _H * _x;
-	const Matrix2f S = _H * _P * _H.transpose() + _R;
-	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();
+	Vector2f Kv{1.f, 0.f};
+	Vector2f test_ratio{0.f, 0.f};
+	Vector2f R{z_var, dist_bottom_var};
+	Vector2f y;
 
-	K(RangeFilter::z.idx, RangeFilter::z.idx) = 1.f;
-	K(RangeFilter::z.idx, RangeFilter::terrain.idx) = 0.f;
+	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;
 
-	if (test_ratio > 1.f) {
-		K(RangeFilter::terrain.idx, RangeFilter::z.idx) = 0.f;
-		K(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = 0.f;
+		Vector2f PH = _P.row(i);
+
+		for (int u = 0; u < RangeFilter::size; u++) {
+			for (int v = 0; v < RangeFilter::size; v++) {
+				_P(u, v) -= Kv(u) * PH(v);
+			}
+		}
+
+		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);
+			}
+		}
+
+		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;
+		}
+
+		_x = _x + Kv * y;
 	}
 
-	_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);
+	_test_ratio_lpf.update(sign(_innov) * test_ratio(1));
 
-	_test_ratio_lpf.update(sign(_innov) * test_ratio);
-
-	if (_sample_count++ > _min_nr_of_samples) {
+	// start the consistency check after 1s
+	if (_t_since_first_sample + dt > 1.0f) {
+		_t_since_first_sample = 2.0f;
 
 		if (abs(_test_ratio_lpf.getState()) < 1.f) {
-			_state = KinematicState::CONSISTENT;
+			const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f);
+
+			if (!horizontal_motion && vertical_motion) {
+				_state = KinematicState::CONSISTENT;
+
+			} else {
+				_state = KinematicState::UNKNOWN;
+			}
 
 		} else {
-			_sample_count = 0;
+			_t_since_first_sample = 0.f;
 			_state = KinematicState::INCONSISTENT;
 		}
 
+	} else {
+		_t_since_first_sample += dt;
 	}
 }
 
-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 &vz,
+				      const matrix::SquareMatrix<float, estimator::State::size> &P,
+				      const float &dist_bottom, const float &dist_bottom_var, 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;
-		const float terrain_var = P(estimator::State::terrain.idx, estimator::State::terrain.idx);
-		init(z_var, terrain_var, z, dist_bottom);
+		init(z, z_var, dist_bottom, dist_bottom_var);
 	}
 
 	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

@@ -56,26 +56,19 @@ public:
 		UNKNOWN = 2
 	};
 
-	float getTestRatioLpf() const { return _test_ratio_lpf.getState(); }
-	float getInnov() const { return _innov; }
-	float getInnovVar() const { return _innov_var; }
-
+	float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; }
+	float getInnov() const { return _initialized ? _innov : 0.f; }
+	float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
 	bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; }
-	bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT || _fixed_wing; }
+	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 reset()
 	{
-		if (_initialized) {
-			if (_state == KinematicState::CONSISTENT) {
-				_state = KinematicState::UNKNOWN;
-			}
-
-			_initialized = false;
-		}
+		_state = (_initialized && _state == KinematicState::CONSISTENT) ? KinematicState::UNKNOWN : _state;
+		_initialized = false;
 	}
-	int getConsistencyState() const { return static_cast<int>(_state); }
-
-	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};
@@ -83,25 +76,20 @@ public:
 
 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{};
+	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);
 	matrix::SquareMatrix<float, 2> _P{};
-	matrix::SquareMatrix<float, 2> _A{};
 	matrix::SquareMatrix<float, 2> _H{};
 	matrix::Vector2f _x{};
 	bool _initialized{false};
-	float _innov{};
-	float _innov_var{};
+	float _innov{0.f};
+	float _innov_var{0.f};
 	uint64_t _time_last_update_us{0};
-	float _dist_bottom_prev{};
-	AlphaFilter<float> _test_ratio_lpf{0.3};
+	AlphaFilter<float> _test_ratio_lpf{};
 	float _gate{1.f};
-	int _sample_count{0};
 	KinematicState _state{KinematicState::UNKNOWN};
-	int _min_nr_of_samples{0};
-	bool _fixed_wing{false};
+	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

@@ -56,6 +56,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
 		_range_sensor.setMaxFogDistance(_params.rng_fog);
+		_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
 
 		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
 
@@ -69,11 +70,13 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				const float dist_var = getRngVar();
 				_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
 
-				const bool updated_horizontal_motion = (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
+				const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f);
 
-				if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
+				if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion
+				    && _rng_consistency_check.isNotKinematicallyInconsistent()) {
 					_rng_consistency_check.reset();
 				}
+
 				_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);
 			}
@@ -94,8 +97,9 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 			}
 		}
 
-		_control_status.flags.rng_kin_consistent = _rng_consistency_check.getConsistencyState() == 1;
-		_control_status.flags.rng_kin_unknown = _rng_consistency_check.getConsistencyState() == 2;
+		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+		_control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent()
+							&& _rng_consistency_check.isNotKinematicallyInconsistent();
 
 	} else {
 		return;
@@ -228,7 +232,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					}
 
 				} else {
-					if (aid_src.innovation_rejected) {
+					if (aid_src.innovation_rejected && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}

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

@@ -164,6 +164,9 @@ void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t t
 		}
 
 		_prev_median_dist = median_dist;
+
+	} else {
+		_prev_median_dist = dist_bottom;
 	}
 }
 

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

@@ -721,19 +721,17 @@ def compute_gravity_z_innov_var_and_h(
 
     return (innov_var, H.T)
 
-def range_validation_filter() -> sf.matrix:
+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
+    state = Values(
+        z=sf.Symbol("z"),
+        terrain=sf.Symbol("terrain")
+    )
+    dist_bottom = state["z"] - state["terrain"]
 
     H = sf.M22()
-    H[0, :] = sf.V1(z).jacobian(state, tangent_space=False)
-    H[1, :] = sf.V1(terrain).jacobian(state, tangent_space=False)
+    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)
 
     return H
 

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

@@ -35,8 +35,8 @@ void RangeValidationFilter(matrix::Matrix<Scalar, 2, 2>* const H = nullptr) {
     _h.setZero();
 
     _h(0, 0) = 1;
-    _h(1, 0) = -1;
-    _h(1, 1) = 1;
+    _h(1, 0) = 1;
+    _h(1, 1) = -1;
   }
 }  // NOLINT(readability/fn_size)
 

src/modules/ekf2/test/test_EKF_height_fusion.cpp

@@ -103,7 +103,7 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
 	/* _ekf_wrapper.enableExternalVisionHeightFusion(); */ //TODO: this currently sets the reference to EV
-	_sensor_simulator.runSeconds(1);
+	_sensor_simulator.runSeconds(10);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -131,8 +131,8 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	const BiasEstimator::status &gps_status = _ekf->getGpsHgtBiasEstimatorStatus();
 	EXPECT_NEAR(gps_status.bias, _sensor_simulator._gps.getData().alt - _sensor_simulator._baro.getData(), 0.2f);
 
-	const float terrain = _ekf->getTerrainVertPos();
-	EXPECT_NEAR(terrain, -baro_increment, 1.2f);
+	const float hagl  = _ekf->output_predictor().getLatLonAlt().altitude() + _ekf->state().terrain;
+	EXPECT_NEAR(hagl, baro_increment, 1.2f);
 
 	const BiasEstimator::status &ev_status = _ekf->getEvHgtBiasEstimatorStatus();
 	EXPECT_EQ(ev_status.bias, 0.f);
@@ -145,7 +145,6 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 	EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
-	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
 
 	// AND WHEN: the gps height increases
 	const float gps_increment = 1.f;
@@ -157,8 +156,8 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	// the estimated height follows the GPS height
 	EXPECT_NEAR(_ekf->getPosition()(2), -(baro_increment + gps_increment), 0.3f);
 	// and the range finder bias is adjusted to follow the new reference
-	const float terrain2 = _ekf->getTerrainVertPos();
-	EXPECT_NEAR(terrain2, -(baro_increment + gps_increment), 1.3f);
+	const float hagl2  = _ekf->output_predictor().getLatLonAlt().altitude() + _ekf->state().terrain;
+	EXPECT_NEAR(hagl2, (baro_increment + gps_increment), 1.3f);
 }
 
 TEST_F(EkfHeightFusionTest, gpsRef)
@@ -168,7 +167,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
 	_ekf_wrapper.enableBaroHeightFusion();
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
-	_sensor_simulator.runSeconds(1);
+	_sensor_simulator.runSeconds(10);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -221,7 +220,7 @@ TEST_F(EkfHeightFusionTest, gpsRefNoAltFusion)
 	_ekf_wrapper.enableBaroHeightFusion();
 	_ekf_wrapper.disableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
-	_sensor_simulator.runSeconds(1);
+	_sensor_simulator.runSeconds(10);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); // Fallback to baro as GNSS alt is disabled
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -248,7 +247,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
 
-	_sensor_simulator.runSeconds(0.1);
+	_sensor_simulator.runSeconds(10);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO);
 

src/modules/ekf2/test/test_EKF_terrain.cpp

@@ -207,7 +207,7 @@ TEST_F(EkfTerrainTest, testHeightReset)
 	const float new_baro_height = _sensor_simulator._baro.getData() + 50.f;
 	_sensor_simulator._baro.setData(new_baro_height);
 	_sensor_simulator.stopGps(); // prevent from switching to GNSS height
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(100);
 
 	// THEN: a height reset occurred and the estimated distance to the ground remains constant
 	reset_logging_checker.capturePostResetState();
@@ -219,11 +219,36 @@ TEST_F(EkfTerrainTest, testRngStartInAir)
 {
 	// GIVEN: rng for terrain but not flow
 	_ekf_wrapper.disableFlowFusion();
-	_ekf_wrapper.enableRangeHeightFusion();
+	const float rng_height = 16.f;
 
-	const float rng_height = 18;
-	const float flow_height = 1.f;
-	runFlowAndRngScenario(rng_height, flow_height);
+	_sensor_simulator.startGps();
+	_ekf->set_min_required_gps_health_time(1e6);
+	_ekf->set_in_air_status(false);
+	_ekf->set_vehicle_at_rest(true);
+	_ekf_wrapper.enableGpsFusion();
+	_sensor_simulator.runSeconds(1.5); // Run to pass the GPS checks
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+
+	const Vector3f simulated_velocity(0.5f, -1.0f, 0.0f);
+
+	// Configure GPS simulator data
+	_sensor_simulator._gps.setVelocity(simulated_velocity);
+	_sensor_simulator._gps.setPositionRateNED(simulated_velocity);
+
+	// Simulate flight above max range finder distance
+	// run for a while to let terrain uncertainty increase
+	_sensor_simulator._rng.setData(30.f, 100);
+	_sensor_simulator._rng.setLimits(0.1f, 20.f);
+	_sensor_simulator.startRangeFinder();
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_sensor_simulator.runSeconds(40);
+
+	// quick range finder change to bypass stuck-check
+	_sensor_simulator._rng.setData(rng_height - 1.f, 100);
+	_sensor_simulator.runSeconds(1);
+	_sensor_simulator._rng.setData(rng_height, 100);
+	_sensor_simulator.runSeconds(10);
 
 	// THEN: the terrain should reset using rng
 	EXPECT_NEAR(rng_height, _ekf->getHagl(), 1e-3f);
@@ -242,5 +267,5 @@ TEST_F(EkfTerrainTest, testRngStartInAir)
 
 	const float corr_terrain_abias_z = P(State::accel_bias.idx + 2,
 					     State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain);
-	EXPECT_TRUE(corr_terrain_abias_z < -0.4f);
+	EXPECT_TRUE(corr_terrain_abias_z < -0.1f);
 }