ekf2-rng: consider height covariance for terrain reset to range

Range does not provide a direct terrain observation but a measurement
relative to the height state. Correlation between height and terrain
must be set properly.

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

@@ -37,6 +37,7 @@
  */
 
 #include "ekf.h"
+#include "ekf_derivation/generated/compute_hagl_h.h"
 #include "ekf_derivation/generated/compute_hagl_innov_var.h"
 
 void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
@@ -270,13 +271,25 @@ float Ekf::getRngVar() const
 
 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
 {
-	const float new_terrain = _state.pos(2) + aid_src.observation;
+	// Since the distance is not a direct observation of the terrain state but is based
-	const float delta_terrain = new_terrain - _state.terrain;
+	// on the height state, a reset should consider the height uncertainty. This can be
+	// done by manipulating the Kalman gain to inject all the innovation in the terrain state
+	// and create the correct correlation with the terrain state with a covariance update.
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 0.f);
 
-	_state.terrain = new_terrain;
+	const float old_terrain = _state.terrain;
-	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, aid_src.observation_variance);
+
+	VectorState H;
+	sym::ComputeHaglH(&H);
+
+	VectorState K;
+	K(State::terrain.idx) = 1.f; // innovation is forced into the terrain state to create a "reset"
+
+	measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
 
 	// record the state change
+	const float delta_terrain = _state.terrain - old_terrain;
+
 	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
 		_state_reset_status.hagl_change = delta_terrain;
 
@@ -287,7 +300,6 @@ void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
 
 	_state_reset_status.reset_count.hagl++;
 
-
 	aid_src.time_last_fuse = _time_delayed_us;
 }
 

src/modules/ekf2/test/test_EKF_terrain.cpp

@@ -214,3 +214,33 @@ TEST_F(EkfTerrainTest, testHeightReset)
 	EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
 	EXPECT_NEAR(estimated_distance_to_ground, _ekf->getHagl(), 1e-3f);
 }
+
+TEST_F(EkfTerrainTest, testRngStartInAir)
+{
+	// GIVEN: rng for terrain but not flow
+	_ekf_wrapper.disableFlowFusion();
+	_ekf_wrapper.enableRangeHeightFusion();
+
+	const float rng_height = 18;
+	const float flow_height = 1.f;
+	runFlowAndRngScenario(rng_height, flow_height);
+
+	// THEN: the terrain should reset using rng
+	EXPECT_NEAR(rng_height, _ekf->getHagl(), 1e-3f);
+
+	// AND: the terrain state should be highly correlated with the other height states
+	auto P = _ekf->covariances();
+	const float var_terrain = _ekf->getTerrainVariance();
+
+	const float corr_terrain_vz = P(State::vel.idx + 2,
+					State::terrain.idx) / sqrtf(_ekf->getVelocityVariance()(2) * var_terrain);
+	EXPECT_NEAR(corr_terrain_vz, 0.6f, 0.03f);
+
+	const float corr_terrain_z = P(State::pos.idx + 2,
+				       State::terrain.idx) / sqrtf(_ekf->getPositionVariance()(2) * var_terrain);
+	EXPECT_NEAR(corr_terrain_z, 0.8f, 0.03f);
+
+	const float corr_terrain_abias_z = P(State::accel_bias.idx + 2,
+					     State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain);
+	EXPECT_NEAR(corr_terrain_abias_z, -0.4f, 0.03f);
+}