ekf2: add terrain state

msg/EstimatorAidSource1d.msg

@@ -25,4 +25,3 @@ bool fused                   # true if the sample was successfully fused
 # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
 # TOPICS estimator_aid_src_fake_hgt
 # TOPICS estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
-# TOPICS estimator_aid_src_terrain_range_finder

msg/EstimatorAidSource2d.msg

@@ -22,5 +22,5 @@ bool innovation_rejected        # true if the observation has been rejected
 bool fused                      # true if the sample was successfully fused
 
 # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position
-# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
+# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow
 # TOPICS estimator_aid_src_drag

msg/EstimatorBias.msg

@@ -9,4 +9,4 @@ float32 innov			# innovation of the last measurement fusion (m)
 float32 innov_var		# innovation variance of the last measurement fusion (m^2)
 float32 innov_test_ratio	# normalized innovation squared test ratio
 
-# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias
+# TOPICS estimator_baro_bias estimator_gnss_hgt_bias

msg/EstimatorInnovations.msg

@@ -22,7 +22,6 @@ float32[2] aux_hvel	# horizontal auxiliary velocity innovation from landing targ
 
 # Optical flow
 float32[2] flow		# flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)
-float32[2] terr_flow	# flow innvoation (rad/sec) and innovation variance computed by the terrain estimator ((rad/sec)**2)
 
 # Various
 float32 heading		# heading innovation (rad) and innovation variance (rad**2)

msg/EstimatorStates.msg

@@ -1,7 +1,7 @@
 uint64 timestamp		# time since system start (microseconds)
 uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
-float32[24] states		# Internal filter states
+float32[25] states		# Internal filter states
 uint8 n_states		# Number of states effectively used
 
-float32[23] covariances	# Diagonal Elements of Covariance Matrix
+float32[24] covariances	# Diagonal Elements of Covariance Matrix

src/modules/ekf2/CMakeLists.txt

@@ -203,6 +203,7 @@ if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
 		EKF/aid_sources/range_finder/range_height_control.cpp
+		EKF/aid_sources/range_finder/range_height_fusion.cpp
 		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()

src/modules/ekf2/EKF/CMakeLists.txt

@@ -125,6 +125,7 @@ if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		aid_sources/range_finder/range_finder_consistency_check.cpp
 		aid_sources/range_finder/range_height_control.cpp
+		aid_sources/range_finder/range_height_fusion.cpp
 		aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -55,13 +55,9 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 					       && !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate);
 		const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
 
-		// don't enforce this condition if terrain estimate is not valid as we have logic below to coast through bad range finder data
-		const bool is_within_max_sensor_dist = isTerrainEstimateValid() ? (_terrain_vpos - _state.pos(2) <= _flow_max_distance) : true;
-
 		if (is_quality_good
 		    && is_magnitude_good
-		    && is_tilt_good
+		    && is_tilt_good) {
-		    && is_within_max_sensor_dist) {
 			// compensate for body motion to give a LOS rate
 			calcOptFlowBodyRateComp(imu_delayed);
 			_flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy();
@@ -81,20 +77,21 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 					&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
 					|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
 
-		// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
+		const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance;
-		// use a relaxed time criteria to enable it to coast through bad range finder data
-		const bool terrain_available = isTerrainEstimateValid() || isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6);
 
 		const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
 							   && _control_status.flags.tilt_align
-							   && (terrain_available || is_flow_required);
+							   && is_within_max_sensor_dist;
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 							 && isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
 
+		// If the height is relative to the ground, terrain height cannot be observed.
+		_hagl_sensor_status.flags.flow = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE);
+
 		if (_control_status.flags.opt_flow) {
 			if (continuing_conditions_passing) {
-				fuseOptFlow();
+				fuseOptFlow(_hagl_sensor_status.flags.flow);
 
 				// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
 				if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) {
@@ -125,19 +122,35 @@ void Ekf::startFlowFusion()
 {
 	ECL_INFO("starting optical flow fusion");
 
-	if (!_aid_src_optical_flow.innovation_rejected && isHorizontalAidingActive()) {
+	if (_height_sensor_ref != HeightSensor::RANGE) {
-		// Consistent with the current velocity state, simply fuse the data without reset
+		// If the height is relative to the ground, terrain height cannot be observed.
-		fuseOptFlow();
+		_hagl_sensor_status.flags.flow = true;
-		_control_status.flags.opt_flow = true;
+	}
 
-	} else if (!isHorizontalAidingActive()) {
+	if (isHorizontalAidingActive()) {
-		resetFlowFusion();
+		if (!_aid_src_optical_flow.innovation_rejected) {
-		_control_status.flags.opt_flow = true;
+			ECL_INFO("starting optical flow no reset");
+			fuseOptFlow(_hagl_sensor_status.flags.flow);
+
+		} else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) {
+			resetHaglFlow();
+
+		} else {
+			ECL_INFO("optical flow fusion failed to start");
+			_control_status.flags.opt_flow = false;
+			_hagl_sensor_status.flags.flow = false;
+		}
 
 	} else {
-		ECL_INFO("optical flow fusion failed to start");
+		if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
-		_control_status.flags.opt_flow = false;
+			resetFlowFusion();
+
+		} else if (_hagl_sensor_status.flags.flow) {
+			resetHaglFlow();
+		}
 	}
+
+	_control_status.flags.opt_flow = true; // needs to be here because of isHorizontalAidingActive
 }
 
 void Ekf::resetFlowFusion()
@@ -159,11 +172,26 @@ void Ekf::resetFlowFusion()
 	_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
 }
 
+void Ekf::resetHaglFlow()
+{
+	ECL_INFO("reset hagl to flow");
+	// TODO: use the flow data
+	_state.terrain = fmaxf(0.0f, _state.pos(2));
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);
+	_terrain_vpos_reset_counter++;
+
+	_innov_check_fail_status.flags.reject_optflow_X = false;
+	_innov_check_fail_status.flags.reject_optflow_Y = false;
+
+	_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
+}
+
 void Ekf::stopFlowFusion()
 {
 	if (_control_status.flags.opt_flow) {
 		ECL_INFO("stopping optical flow fusion");
 		_control_status.flags.opt_flow = false;
+		_hagl_sensor_status.flags.flow = false;
 	}
 }
 

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp

@@ -73,7 +73,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 
 	Vector2f innov_var;
 	VectorState H;
-	sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
+	sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, FLT_EPSILON, &innov_var, &H);
 
 	// run the innovation consistency check and record result
 	updateAidSourceStatus(aid_src,
@@ -85,7 +85,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 		math::max(_params.flow_innov_gate, 1.f));      // innovation gate
 }
 
-void Ekf::fuseOptFlow()
+void Ekf::fuseOptFlow(const bool update_terrain)
 {
 	const float R_LOS = _aid_src_optical_flow.observation_variance[0];
 
@@ -97,7 +97,7 @@ void Ekf::fuseOptFlow()
 
 	Vector2f innov_var;
 	VectorState H;
-	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
+	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, R_LOS, FLT_EPSILON, &innov_var, &H);
 	innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
 
 	if ((innov_var(0) < R_LOS) || (innov_var(1) < R_LOS)) {
@@ -124,7 +124,7 @@ void Ekf::fuseOptFlow()
 
 		} else if (index == 1) {
 			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
-			sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
+			sym::ComputeFlowYInnovVarAndH(state_vector, P, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
 
 			// recalculate the innovation using the updated state
 			const Vector2f vel_body = predictFlowVelBody();
@@ -141,6 +141,10 @@ void Ekf::fuseOptFlow()
 
 		VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index];
 
+		if (!update_terrain) {
+			Kfusion(State::terrain.idx) = 0.f;
+		}
+
 		if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) {
 			fused[index] = true;
 		}
@@ -165,10 +169,17 @@ float Ekf::predictFlowRange()
 
 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
 	// a positive offset in earth frame leads to a smaller height above the ground.
-	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
+	const float height_above_gnd_est = getHagl() - pos_offset_earth(2);
 
 	// calculate range from focal point to centre of image
-	return height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
+	float flow_range = height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
+
+	// avoid the flow prediction singularity at range = 0
+	if (fabsf(flow_range) < FLT_EPSILON) {
+		flow_range = signNoZero(flow_range) * FLT_EPSILON;
+	}
+
+	return flow_range;
 }
 
 Vector2f Ekf::predictFlowVelBody()

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_innov_var.h"
 
 void Ekf::controlRangeHeightFusion()
 {
@@ -93,58 +94,91 @@ void Ekf::controlRangeHeightFusion()
 	}
 
 	auto &aid_src = _aid_src_rng_hgt;
-	HeightBiasEstimator &bias_est = _rng_hgt_b_est;
-
-	bias_est.predict(_dt_ekf_avg);
 
 	if (rng_data_ready && _range_sensor.getSampleAddress()) {
 
-		const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
+		updateRangeHeight(aid_src);
-		const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
 
-		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
+		const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
-
+							    || (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
-		// vertical position innovation - baro measurement has opposite sign to earth z axis
+							   && _control_status.flags.tilt_align
-		updateVerticalPositionAidStatus(aid_src,
+							   && measurement_valid
-						_range_sensor.getSampleAddress()->time_us, // sample timestamp
+							   && _range_sensor.isDataHealthy()
-						-(measurement - bias_est.getBias()),       // observation
+						           && _rng_consistency_check.isKinematicallyConsistent();
-						measurement_var + bias_est.getBiasVar(),   // observation variance
-						math::max(_params.range_innov_gate, 1.f)); // innovation gate
-
-		// update the bias estimator before updating the main filter but after
-		// using its current state to compute the vertical position innovation
-		if (measurement_valid && _range_sensor.isDataHealthy()) {
-			bias_est.setMaxStateNoise(sqrtf(measurement_var));
-			bias_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd);
-			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
-		}
-
-		// determine if we should use height aiding
-		const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
-						      && isConditionalRangeAidSuitable();
-
-		const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED)) || do_conditional_range_aid)
-				&& measurement_valid
-				&& _range_sensor.isDataHealthy();
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();
 
+
+		const bool do_conditional_range_aid = (_hagl_sensor_status.flags.range_finder || _control_status.flags.rng_hgt)
+						      && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
+						      && isConditionalRangeAidSuitable();
+
+		const bool do_range_aid = (_hagl_sensor_status.flags.range_finder || _control_status.flags.rng_hgt)
+					  && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
+
 		if (_control_status.flags.rng_hgt) {
+			if (!(do_conditional_range_aid || do_range_aid)) {
+				ECL_INFO("stopping %s fusion", HGT_SRC_NAME);
+				stopRngHgtFusion();
+			}
+
+		} else {
+			if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
+				if (do_conditional_range_aid) {
+					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
+					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
+					_height_sensor_ref = HeightSensor::RANGE;
+
+					_control_status.flags.rng_hgt = true;
+					stopRngTerrFusion();
+
+					if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) {
+						resetHaglRng(aid_src);
+					}
+
+				} else if (do_range_aid) {
+					// Range finder is the primary height source, the ground is now the datum used
+					// to compute the local vertical position
+					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
+					_height_sensor_ref = HeightSensor::RANGE;
+
+					_information_events.flags.reset_hgt_to_rng = true;
+					resetVerticalPositionTo(-aid_src.observation, aid_src.observation_variance);
+					_state.terrain = 0.f;
+					_control_status.flags.rng_hgt = true;
+					stopRngTerrFusion();
+
+					aid_src.time_last_fuse = _time_delayed_us;
+				}
+
+			} else {
+				if (do_conditional_range_aid || do_range_aid) {
+					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
+					_control_status.flags.rng_hgt = true;
+
+					if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) {
+						resetHaglRng(aid_src);
+					}
+				}
+			}
+		}
+
+		if (_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder) {
 			if (continuing_conditions_passing) {
 
-				fuseVerticalPosition(aid_src);
+				fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _hagl_sensor_status.flags.range_finder);
 
 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
 
-				if (isHeightResetRequired()) {
+				if (isHeightResetRequired() && _control_status.flags.rng_hgt) {
 					// All height sources are failing
 					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
 
 					_information_events.flags.reset_hgt_to_rng = true;
-					resetVerticalPositionTo(-(measurement - bias_est.getBias()));
+					resetVerticalPositionTo(-(aid_src.observation - _state.terrain));
-					bias_est.setBias(_state.pos(2) + measurement);
 
 					// reset vertical velocity
 					resetVerticalVelocityToZero();
@@ -153,96 +187,123 @@ void Ekf::controlRangeHeightFusion()
 
 				} else if (is_fusion_failing) {
 					// Some other height source is still working
-					ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME);
+					if (_hagl_sensor_status.flags.flow) {
-					stopRngHgtFusion();
+						ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME);
-					_control_status.flags.rng_fault = true;
+						stopRngHgtFusion();
-					_range_sensor.setFaulty();
+						stopRngTerrFusion();
+
+					} else {
+						resetHaglRng(aid_src);
+					}
 				}
 
 			} else {
-				ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
+				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
 				stopRngHgtFusion();
+				stopRngTerrFusion();
 			}
 
 		} else {
 			if (starting_conditions_passing) {
-				if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
+				if (_hagl_sensor_status.flags.flow) {
-				    && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
+					if (!aid_src.innovation_rejected) {
-				) {
+						_hagl_sensor_status.flags.range_finder = true;
-					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
+						fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _hagl_sensor_status.flags.range_finder);
-					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
+					}
-					_height_sensor_ref = HeightSensor::RANGE;
-					bias_est.setBias(_state.pos(2) + measurement);
-
-				} else if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
-					   && (_params.rng_ctrl != static_cast<int32_t>(RngCtrl::CONDITIONAL))
-				) {
-					// Range finder is the primary height source, the ground is now the datum used
-					// to compute the local vertical position
-					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
-					_height_sensor_ref = HeightSensor::RANGE;
-
-					_information_events.flags.reset_hgt_to_rng = true;
-					resetVerticalPositionTo(-measurement, measurement_var);
-					bias_est.reset();
 
 				} else {
-					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
+					if (aid_src.innovation_rejected) {
-					bias_est.setBias(_state.pos(2) + measurement);
+						resetHaglRng(aid_src);
-				}
+					}
 
-				aid_src.time_last_fuse = _time_delayed_us;
+					_hagl_sensor_status.flags.range_finder = true;
-				bias_est.setFusionActive();
+				}
-				_control_status.flags.rng_hgt = true;
 			}
 		}
 
-	} else if (_control_status.flags.rng_hgt
+	} else if ((_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder)
 		   && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
-		ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
+		ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME);
 		stopRngHgtFusion();
+		stopRngTerrFusion();
 	}
 }
 
+void Ekf::updateRangeHeight(estimator_aid_source1d_s &aid_src)
+{
+	aid_src.observation = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
+	aid_src.innovation = getHagl() - aid_src.observation;
+
+	const float observation_variance = getRngVar();
+	float innovation_variance;
+	sym::ComputeHaglInnovVar(P, observation_variance, &innovation_variance);
+
+	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
+	updateAidSourceStatus(aid_src,
+			      _range_sensor.getSampleAddress()->time_us,                           // sample timestamp
+			      math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance), // observation
+			      observation_variance,                                                // observation variance
+			      getHagl() - aid_src.observation,                                     // innovation
+			      innovation_variance,                                                 // innovation variance
+			      math::max(_params.range_innov_gate, 1.f));                            // innovation gate
+
+	// z special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
+		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
+		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
+		aid_src.innovation_rejected = false;
+	}
+}
+
+float Ekf::getRngVar() const
+{
+	return fmaxf(
+		     P(State::pos.idx + 2, State::pos.idx + 2)
+		     + sq(_params.range_noise)
+		     + sq(_params.range_noise_scaler * _range_sensor.getRange()),
+	       0.f);
+}
+
+void Ekf::resetHaglRng(estimator_aid_source1d_s &aid_src)
+{
+	_state.terrain = _state.pos(2) + aid_src.observation;
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, aid_src.observation_variance);
+	_terrain_vpos_reset_counter++;
+
+	aid_src.time_last_fuse = _time_delayed_us;
+}
+
 bool Ekf::isConditionalRangeAidSuitable()
 {
 #if defined(CONFIG_EKF2_TERRAIN)
 
-	if (_control_status.flags.in_air
+	// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
-	    && _range_sensor.isHealthy()
+	// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
-	    && isTerrainEstimateValid()) {
+	float range_hagl_max = _params.max_hagl_for_range_aid;
-		// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
+	float max_vel_xy = _params.max_vel_for_range_aid;
-		// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
-		float range_hagl_max = _params.max_hagl_for_range_aid;
-		float max_vel_xy = _params.max_vel_for_range_aid;
 
-		const float hagl_innov = _aid_src_terrain_range_finder.innovation;
+	const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio;
-		const float hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance;
 
-		const float hagl_test_ratio = (hagl_innov * hagl_innov / (sq(_params.range_aid_innov_gate) * hagl_innov_var));
+	bool is_hagl_stable = (hagl_test_ratio < 1.f);
 
-		bool is_hagl_stable = (hagl_test_ratio < 1.f);
+	if (!_control_status.flags.rng_hgt) {
-
+		range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
-		if (!_control_status.flags.rng_hgt) {
+		max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
-			range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
+		is_hagl_stable = (hagl_test_ratio < 0.01f);
-			max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
-			is_hagl_stable = (hagl_test_ratio < 0.01f);
-		}
-
-		const float range_hagl = _terrain_vpos - _state.pos(2);
-
-		const bool is_in_range = (range_hagl < range_hagl_max);
-
-		bool is_below_max_speed = true;
-
-		if (isHorizontalAidingActive()) {
-			is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
-		}
-
-		return is_in_range && is_hagl_stable && is_below_max_speed;
 	}
 
+	const bool is_in_range = (getHagl() < range_hagl_max);
+
+	bool is_below_max_speed = true;
+
+	if (isHorizontalAidingActive()) {
+		is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
+	}
+
+	return is_in_range && is_hagl_stable && is_below_max_speed;
+
 #endif // CONFIG_EKF2_TERRAIN
 
 	return false;
@@ -256,8 +317,11 @@ void Ekf::stopRngHgtFusion()
 			_height_sensor_ref = HeightSensor::UNKNOWN;
 		}
 
-		_rng_hgt_b_est.setFusionInactive();
-
 		_control_status.flags.rng_hgt = false;
 	}
 }
+
+void Ekf::stopRngTerrFusion()
+{
+	_hagl_sensor_status.flags.range_finder = false;
+}

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

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ekf.h"
+#include "ekf_derivation/generated/compute_hagl_h.h"
+
+bool Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain)
+{
+	if (aid_src.innovation_rejected) {
+		_innov_check_fail_status.flags.reject_hagl = true;
+		return false;
+	}
+
+	VectorState H;
+
+	sym::ComputeHaglH(&H);
+
+	// calculate the Kalman gain
+	VectorState K = P * H / aid_src.innovation_variance;
+
+	if (!update_terrain) {
+		K(State::terrain.idx) = 0.f;
+	}
+
+	if (!update_height) {
+		const float k_terrain = K(State::terrain.idx);
+		K.zero();
+		K(State::terrain.idx) = k_terrain;
+	}
+
+	measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
+
+	// record last successful fusion event
+	_innov_check_fail_status.flags.reject_hagl = false;
+
+	aid_src.time_last_fuse = _time_delayed_us;
+	aid_src.fused = true;
+
+	return true;
+}

src/modules/ekf2/EKF/common.h

@@ -107,7 +107,7 @@ enum MagFuseType : uint8_t {
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_TERRAIN)
-enum TerrainFusionMask : uint8_t {
+enum class TerrainFusionMask : uint8_t {
 	TerrainFuseRangeFinder = (1 << 0),
 	TerrainFuseOpticalFlow = (1 << 1)
 };
@@ -382,8 +382,8 @@ struct parameters {
 #endif // CONFIG_EKF2_SIDESLIP
 
 #if defined(CONFIG_EKF2_TERRAIN)
-	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
+	int32_t terrain_fusion_mode{static_cast<int32_t>(TerrainFusionMask::TerrainFuseRangeFinder)
-				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
+				    | static_cast<int32_t>(TerrainFusionMask::TerrainFuseOpticalFlow)}; ///< aiding source(s) selection bitmask for the terrain estimation
 
 	float terrain_p_noise{5.0f};            ///< process noise for terrain offset (m/sec)
 	float terrain_gradient{0.5f};           ///< gradient of terrain used to estimate process noise due to changing position (m/m)
@@ -401,10 +401,8 @@ struct parameters {
 	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float range_noise{0.1f};                ///< observation noise for range finder measurements (m)
 	float range_innov_gate{5.0f};           ///< range finder fusion innovation consistency gate size (STD)
-	float rng_hgt_bias_nsd{0.13f};          ///< process noise for range height bias estimation (m/s/sqrt(Hz))
 	float rng_sens_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
 	float range_noise_scaler{0.0f};         ///< scaling from range measurement to noise (m/m)
-	const float vehicle_variance_scaler{0.0f};      ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
 	float max_hagl_for_range_aid{5.0f};     ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
 	float max_vel_for_range_aid{1.0f};      ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
 	float range_aid_innov_gate{1.0f};       ///< gate size used for innovation consistency checks for range aid fusion
@@ -521,7 +519,7 @@ union innovation_fault_status_u {
 		bool reject_yaw       : 1; ///< 7 - true if the yaw observation has been rejected
 		bool reject_airspeed  : 1; ///< 8 - true if the airspeed observation has been rejected
 		bool reject_sideslip  : 1; ///< 9 - true if the synthetic sideslip observation has been rejected
-		bool reject_hagl      : 1; ///< 10 - true if the height above ground observation has been rejected
+		bool reject_hagl      : 1; ///< 10 - unused
 		bool reject_optflow_X : 1; ///< 11 - true if the X optical flow observation has been rejected
 		bool reject_optflow_Y : 1; ///< 12 - true if the Y optical flow observation has been rejected
 	} flags;

src/modules/ekf2/EKF/covariance.cpp

@@ -202,6 +202,17 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	}
 #endif // CONFIG_EKF2_WIND
 
+#if defined(CONFIG_EKF2_TERRAIN)
+	if (_height_sensor_ref != HeightSensor::RANGE) {
+		// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
+		// process noise due to errors in vehicle height estimate
+		float terrain_process_noise = sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
+
+		// 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)));
+		P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
+	}
+#endif // CONFIG_EKF2_TERRAIN
 
 	// covariance matrix is symmetrical, so copy upper half to lower half
 	for (unsigned row = 0; row < State::size; row++) {
@@ -239,6 +250,10 @@ void Ekf::constrainStateVariances()
 		constrainStateVarLimitRatio(State::wind_vel, 1e-6f, 1e6f);
 	}
 #endif // CONFIG_EKF2_WIND
+
+#if defined(CONFIG_EKF2_TERRAIN)
+	constrainStateVarLimitRatio(State::terrain, 0.f, 1e4f);
+#endif // CONFIG_EKF2_TERRAIN
 }
 
 void Ekf::constrainStateVar(const IdxDof &state, float min, float max)

src/modules/ekf2/EKF/ekf.cpp

@@ -161,7 +161,6 @@ bool Ekf::update()
 		controlFusionModes(imu_sample_delayed);
 
 #if defined(CONFIG_EKF2_TERRAIN)
-		// run a separate filter for terrain estimation
 		runTerrainEstimator(imu_sample_delayed);
 #endif // CONFIG_EKF2_TERRAIN
 
@@ -201,7 +200,7 @@ bool Ekf::initialiseFilter()
 	initialiseCovariance();
 
 #if defined(CONFIG_EKF2_TERRAIN)
-	// Initialise the terrain estimator
+	// Initialise the terrain state
 	initHagl();
 #endif // CONFIG_EKF2_TERRAIN
 
@@ -377,6 +376,14 @@ void Ekf::print_status()
 	      );
 #endif // CONFIG_EKF2_WIND
 
+#if defined(CONFIG_EKF2_TERRAIN)
+	printf("Terrain position (%d): %.3f var: %.1e\n",
+	       State::terrain.idx,
+	       (double)_state.terrain,
+	       (double)getStateVariance<State::terrain>()(0)
+	      );
+#endif // CONFIG_EKF2_TERRAIN
+
 	printf("\nP:\n");
 	P.print();
 

src/modules/ekf2/EKF/ekf.h

@@ -104,27 +104,19 @@ public:
 	uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
 
 	// get the estimated terrain vertical position relative to the NED origin
-	float getTerrainVertPos() const { return _terrain_vpos; };
+	float getTerrainVertPos() const { return _state.terrain; };
+	float getHagl() const { return _state.terrain - _state.pos(2); }
 
 	// get the number of times the vertical terrain position has been reset
 	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
 
 	// get the terrain variance
-	float get_terrain_var() const { return _terrain_var; }
+	float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }
-
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; }
-# endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
-# endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #endif // CONFIG_EKF2_TERRAIN
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
-	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
 	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
 
 	float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
@@ -592,27 +584,14 @@ private:
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Terrain height state estimation
-	float _terrain_vpos{0.0f};		///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
-	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
 	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
 
 	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
-
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	estimator_aid_source1d_s _aid_src_terrain_range_finder{};
-	uint64_t _time_last_healthy_rng_data{0};
-# endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-
 #endif // CONFIG_EKF2_TERRAIN
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	estimator_aid_source1d_s _aid_src_rng_hgt{};
-	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -831,29 +810,22 @@ private:
 	// terrain vertical position estimator
 	void initHagl();
 	void runTerrainEstimator(const imuSample &imu_delayed);
-	void predictHagl(const imuSample &imu_delayed);
 
-	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
+	float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _last_on_ground_posD; }
 
 	void controlHaglFakeFusion();
-	void terrainHandleVerticalPositionReset(float delta_z);
 
 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
-	void controlHaglRngFusion();
+	bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain);
-	void updateHaglRng(estimator_aid_source1d_s &aid_src) const;
+	void updateRangeHeight(estimator_aid_source1d_s &aid_src);
-	void fuseHaglRng(estimator_aid_source1d_s &aid_src);
+	void resetHaglRng(estimator_aid_source1d_s &aid_src);
-	void resetHaglRng();
-	void stopHaglRngFusion();
 	float getRngVar() const;
 # endif // CONFIG_EKF2_RANGE_FINDER
 
 # if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// update the terrain vertical position estimate using an optical flow measurement
-	void controlHaglFlowFusion();
 	void resetHaglFlow();
-	void stopHaglFlowFusion();
-	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
 # endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #endif // CONFIG_EKF2_TERRAIN
@@ -863,6 +835,7 @@ private:
 	void controlRangeHeightFusion();
 	bool isConditionalRangeAidSuitable();
 	void stopRngHgtFusion();
+	void stopRngTerrFusion();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -883,7 +856,7 @@ private:
 
 	// fuse optical flow line of sight rate measurements
 	void updateOptFlow(estimator_aid_source2d_s &aid_src);
-	void fuseOptFlow();
+	void fuseOptFlow(bool update_terrain);
 	float predictFlowRange();
 	Vector2f predictFlowVelBody();
 #endif // CONFIG_EKF2_OPTICAL_FLOW

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -211,7 +211,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 		if (_control_status.flags.opt_flow) {
 			float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
-			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
+			vel_err_conservative = math::max(getHagl(), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
 		}
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
@@ -271,7 +271,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance);
 		const float flow_hagl_max = fminf(rangefinder_hagl_max, _flow_max_distance);
 
-		const float flow_constrained_height = math::constrain(_terrain_vpos - _state.pos(2), flow_hagl_min, flow_hagl_max);
+		const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max);
 
 		// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
 		const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
@@ -472,18 +472,9 @@ float Ekf::getHeightAboveGroundInnovationTestRatio() const
 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_hagl_sensor_status.flags.range_finder) {
 		// return the terrain height innovation test ratio
-		test_ratio = math::max(test_ratio, fabsf(_aid_src_terrain_range_finder.test_ratio_filtered));
+		test_ratio = math::max(test_ratio, fabsf(_aid_src_rng_hgt.test_ratio_filtered));
 	}
-#endif // CONFIG_EKF2_RANGE_FINDER
+# endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	if (_hagl_sensor_status.flags.flow) {
-		// return the terrain height innovation test ratio
-		for (auto &test_ratio_filtered : _aid_src_terrain_optical_flow.test_ratio_filtered) {
-			test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
-		}
-	}
-# endif // CONFIG_EKF2_OPTICAL_FLOW
 #endif // CONFIG_EKF2_TERRAIN
 
 	if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
@@ -569,6 +560,10 @@ void Ekf::fuse(const VectorState &K, float innovation)
 		_state.wind_vel = matrix::constrain(_state.wind_vel - K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation, -1.e2f, 1.e2f);
 	}
 #endif // CONFIG_EKF2_WIND
+
+#if defined(CONFIG_EKF2_TERRAIN)
+	_state.terrain = math::constrain(_state.terrain - K(State::terrain.idx) * innovation, -1e4f, 1e4f);
+#endif // CONFIG_EKF2_TERRAIN
 }
 
 void Ekf::updateDeadReckoningStatus()
@@ -670,7 +665,7 @@ void Ekf::updateGroundEffect()
 #if defined(CONFIG_EKF2_TERRAIN)
 		if (isTerrainEstimateValid()) {
 			// automatically set ground effect if terrain is valid
-			float height = _terrain_vpos - _state.pos(2);
+			float height = getHagl();
 			_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
 
 		} else

src/modules/ekf2/EKF/height_control.cpp

@@ -301,7 +301,8 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
-		checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
+		// Range is a distance to ground measurement, not a direct height observation and has an opposite sign
+		checks[3] = {ReferenceType::GROUND, -_aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
 	}
 #endif // CONFIG_EKF2_RANGE_FINDER
 

src/modules/ekf2/EKF/position_fusion.cpp

@@ -161,12 +161,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 #if defined(CONFIG_EKF2_GNSS)
 	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
 #endif // CONFIG_EKF2_GNSS
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
-#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_TERRAIN)
-	terrainHandleVerticalPositionReset(delta_z);
+	_state.terrain += delta_z;
 #endif
 
 	// Reset the timout timer

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

@@ -68,7 +68,8 @@ State = Values(
     accel_bias = sf.V3(),
     mag_I = sf.V3(),
     mag_B = sf.V3(),
-    wind_vel = sf.V2()
+    wind_vel = sf.V2(),
+    terrain = sf.V1()
 )
 
 if args.disable_mag:
@@ -132,7 +133,8 @@ def predict_covariance(
         accel_bias = sf.V3.symbolic("delta_a_b"),
         mag_I = sf.V3.symbolic("mag_I"),
         mag_B = sf.V3.symbolic("mag_B"),
-        wind_vel = sf.V2.symbolic("wind_vel")
+        wind_vel = sf.V2.symbolic("wind_vel"),
+        terrain = sf.V1.symbolic("terrain")
     )
 
     if args.disable_mag:
@@ -456,7 +458,10 @@ def compute_mag_declination_pred_innov_var_and_h(
 
     return (meas_pred, innov_var, H.T)
 
-def predict_opt_flow(state, distance, epsilon):
+def predict_hagl(state):
+    return state["terrain"][0] - state["pos"][2]
+
+def predict_opt_flow(state, epsilon):
     R_to_body = state["quat_nominal"].inverse()
 
     # Calculate earth relative velocity in a non-rotating sensor frame
@@ -464,23 +469,23 @@ def predict_opt_flow(state, distance, epsilon):
 
     # Divide by range to get predicted angular LOS rates relative to X and Y
     # axes. Note these are rates in a non-rotating sensor frame
+    hagl = predict_hagl(state)
+    hagl = add_epsilon_sign(hagl, hagl, epsilon)
+    R_to_earth = state["quat_nominal"].to_rotation_matrix()
     flow_pred = sf.V2()
-    flow_pred[0] =  rel_vel_sensor[1] / distance
+    flow_pred[0] =  rel_vel_sensor[1] / hagl * R_to_earth[2, 2]
-    flow_pred[1] = -rel_vel_sensor[0] / distance
+    flow_pred[1] = -rel_vel_sensor[0] / hagl * R_to_earth[2, 2]
-    flow_pred = add_epsilon_sign(flow_pred, distance, epsilon)
 
     return flow_pred
 
-
 def compute_flow_xy_innov_var_and_hx(
         state: VState,
         P: MTangent,
-        distance: sf.Scalar,
         R: sf.Scalar,
         epsilon: sf.Scalar
 ) -> (sf.V2, VTangent):
     state = vstate_to_state(state)
-    meas_pred = predict_opt_flow(state, distance, epsilon);
+    meas_pred = predict_opt_flow(state, epsilon)
 
     innov_var = sf.V2()
     Hx = jacobian_chain_rule(meas_pred[0], state)
@@ -493,18 +498,40 @@ def compute_flow_xy_innov_var_and_hx(
 def compute_flow_y_innov_var_and_h(
         state: VState,
         P: MTangent,
-        distance: sf.Scalar,
         R: sf.Scalar,
         epsilon: sf.Scalar
 ) -> (sf.Scalar, VTangent):
     state = vstate_to_state(state)
-    meas_pred = predict_opt_flow(state, distance, epsilon);
+    meas_pred = predict_opt_flow(state, epsilon)
 
     Hy = jacobian_chain_rule(meas_pred[1], state)
     innov_var = (Hy * P * Hy.T + R)[0,0]
 
     return (innov_var, Hy.T)
 
+def compute_hagl_innov_var(
+        P: MTangent,
+        R: sf.Scalar,
+) -> (sf.Scalar):
+    state = VState.symbolic("state")
+    state = vstate_to_state(state)
+    meas_pred = predict_hagl(state)
+
+    H = jacobian_chain_rule(meas_pred, state)
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov_var)
+
+def compute_hagl_h(
+) -> (VTangent):
+    state = VState.symbolic("state")
+    state = vstate_to_state(state)
+    meas_pred = predict_hagl(state)
+
+    H = jacobian_chain_rule(meas_pred, state)
+
+    return (H.T)
+
 def compute_gnss_yaw_pred_innov_var_and_h(
         state: VState,
         P: MTangent,
@@ -664,6 +691,8 @@ if not args.disable_wind:
 generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"])
 generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
 generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
+generate_px4_function(compute_hagl_innov_var, output_names=["innov_var"])
+generate_px4_function(compute_hagl_h, output_names=["H"])
 generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"])
 generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"])
 generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"])

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

@@ -16,21 +16,21 @@ namespace sym {
  * Symbolic function: compute_airspeed_h_and_k
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     innov_var: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
- *     H: Matrix23_1
+ *     H: Matrix24_1
- *     K: Matrix23_1
+ *     K: Matrix24_1
  */
 template <typename Scalar>
-void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
-                          const matrix::Matrix<Scalar, 23, 23>& P, const Scalar innov_var,
+                          const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
-                          const Scalar epsilon, matrix::Matrix<Scalar, 23, 1>* const H = nullptr,
+                          const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
-                          matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
+                          matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
-  // Total ops: 246
+  // Total ops: 256
 
   // Input arrays
 
@@ -47,7 +47,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
 
   // Output terms (2)
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 
@@ -59,7 +59,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (K != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _k = (*K);
+    matrix::Matrix<Scalar, 24, 1>& _k = (*K);
 
     _k(0, 0) = _tmp6 * (-P(0, 21) * _tmp3 - P(0, 22) * _tmp4 + P(0, 3) * _tmp3 + P(0, 4) * _tmp4 +
                         P(0, 5) * _tmp5);
@@ -107,6 +107,8 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
                          P(21, 4) * _tmp4 + P(21, 5) * _tmp5);
     _k(22, 0) = _tmp6 * (-P(22, 21) * _tmp3 - P(22, 22) * _tmp4 + P(22, 3) * _tmp3 +
                          P(22, 4) * _tmp4 + P(22, 5) * _tmp5);
+    _k(23, 0) = _tmp6 * (-P(23, 21) * _tmp3 - P(23, 22) * _tmp4 + P(23, 3) * _tmp3 +
+                         P(23, 4) * _tmp4 + P(23, 5) * _tmp5);
   }
 }  // NOLINT(readability/fn_size)
 

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

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: compute_airspeed_innov_and_innov_var
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     airspeed: Scalar
  *     R: Scalar
  *     epsilon: Scalar
@@ -27,8 +27,8 @@ namespace sym {
  *     innov_var: Scalar
  */
 template <typename Scalar>
-void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
-                                     const matrix::Matrix<Scalar, 23, 23>& P, const Scalar airspeed,
+                                     const matrix::Matrix<Scalar, 24, 24>& P, const Scalar airspeed,
                                      const Scalar R, const Scalar epsilon,
                                      Scalar* const innov = nullptr,
                                      Scalar* const innov_var = nullptr) {

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

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: compute_drag_x_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     rho: Scalar
  *     cd: Scalar
  *     cm: Scalar
@@ -26,14 +26,14 @@ namespace sym {
  *
  * Outputs:
  *     innov_var: Scalar
- *     Hx: Matrix23_1
+ *     Hx: Matrix24_1
  */
 template <typename Scalar>
-void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
+                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
                               const Scalar cd, const Scalar cm, const Scalar R,
                               const Scalar epsilon, Scalar* const innov_var = nullptr,
-                              matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
+                              matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
   // Total ops: 357
 
   // Input arrays
@@ -162,7 +162,7 @@ void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (Hx != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
+    matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);
 
     _hx.setZero();
 

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

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: compute_drag_y_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     rho: Scalar
  *     cd: Scalar
  *     cm: Scalar
@@ -26,14 +26,14 @@ namespace sym {
  *
  * Outputs:
  *     innov_var: Scalar
- *     Hy: Matrix23_1
+ *     Hy: Matrix24_1
  */
 template <typename Scalar>
-void ComputeDragYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeDragYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
+                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
                               const Scalar cd, const Scalar cm, const Scalar R,
                               const Scalar epsilon, Scalar* const innov_var = nullptr,
-                              matrix::Matrix<Scalar, 23, 1>* const Hy = nullptr) {
+                              matrix::Matrix<Scalar, 24, 1>* const Hy = nullptr) {
   // Total ops: 360
 
   // Input arrays
@@ -162,7 +162,7 @@ void ComputeDragYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (Hy != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hy = (*Hy);
+    matrix::Matrix<Scalar, 24, 1>& _hy = (*Hy);
 
     _hy.setZero();
 

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

@@ -16,117 +16,153 @@ namespace sym {
  * Symbolic function: compute_flow_xy_innov_var_and_hx
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
- *     distance: Scalar
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Matrix21
- *     H: Matrix23_1
+ *     H: Matrix24_1
  */
 template <typename Scalar>
-void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
-                                const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
+                                const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
-                                const Scalar R, const Scalar epsilon,
+                                const Scalar epsilon,
                                 matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
-                                matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                                matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 275
+  // Total ops: 431
 
   // Input arrays
 
-  // Intermediate terms (42)
+  // Intermediate terms (66)
-  const Scalar _tmp0 = 2 * state(4, 0);
+  const Scalar _tmp0 = state(2, 0) * state(4, 0);
-  const Scalar _tmp1 = 2 * state(1, 0);
+  const Scalar _tmp1 = state(1, 0) * state(5, 0);
-  const Scalar _tmp2 = _tmp1 * state(6, 0);
+  const Scalar _tmp2 = 2 * state(6, 0);
-  const Scalar _tmp3 = -_tmp0 * state(3, 0) + _tmp2;
+  const Scalar _tmp3 = _tmp2 * state(0, 0);
-  const Scalar _tmp4 =
+  const Scalar _tmp4 = -2 * std::pow(state(2, 0), Scalar(2));
-      Scalar(1.0) /
+  const Scalar _tmp5 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
+  const Scalar _tmp6 = _tmp4 + _tmp5;
-  const Scalar _tmp5 = (Scalar(1) / Scalar(2)) * _tmp4;
+  const Scalar _tmp7 = state(24, 0) - state(9, 0);
-  const Scalar _tmp6 = _tmp5 * state(2, 0);
+  const Scalar _tmp8 =
-  const Scalar _tmp7 = 2 * state(3, 0) * state(6, 0);
+      _tmp7 + epsilon * (2 * math::min<Scalar>(0, (((_tmp7) > 0) - ((_tmp7) < 0))) + 1);
-  const Scalar _tmp8 = _tmp5 * (_tmp0 * state(1, 0) + _tmp7);
+  const Scalar _tmp9 = Scalar(1.0) / (_tmp8);
-  const Scalar _tmp9 = 2 * state(0, 0);
+  const Scalar _tmp10 = _tmp6 * _tmp9;
-  const Scalar _tmp10 = state(3, 0) * state(5, 0);
+  const Scalar _tmp11 = 2 * state(0, 0) * state(3, 0);
-  const Scalar _tmp11 = 2 * state(2, 0);
+  const Scalar _tmp12 = 2 * state(1, 0);
-  const Scalar _tmp12 = _tmp11 * state(6, 0);
+  const Scalar _tmp13 = _tmp12 * state(2, 0);
-  const Scalar _tmp13 = -4 * _tmp10 + _tmp12 - _tmp9 * state(4, 0);
+  const Scalar _tmp14 = -_tmp11 + _tmp13;
-  const Scalar _tmp14 = _tmp5 * state(1, 0);
+  const Scalar _tmp15 = 2 * state(2, 0);
-  const Scalar _tmp15 = _tmp9 * state(6, 0);
+  const Scalar _tmp16 = _tmp12 * state(0, 0) + _tmp15 * state(3, 0);
-  const Scalar _tmp16 = _tmp0 * state(2, 0) + _tmp15 - 4 * state(1, 0) * state(5, 0);
+  const Scalar _tmp17 = -2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp17 = _tmp5 * state(3, 0);
+  const Scalar _tmp18 = _tmp17 + _tmp5;
-  const Scalar _tmp18 = -_tmp13 * _tmp14 + _tmp16 * _tmp17 - _tmp3 * _tmp6 + _tmp8 * state(0, 0);
+  const Scalar _tmp19 = _tmp14 * state(4, 0) + _tmp16 * state(6, 0) + _tmp18 * state(5, 0);
-  const Scalar _tmp19 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp20 = 4 * _tmp9;
-  const Scalar _tmp20 = _tmp4 * (_tmp19 - 2 * std::pow(state(1, 0), Scalar(2)));
+  const Scalar _tmp21 = _tmp19 * _tmp20;
-  const Scalar _tmp21 = _tmp3 * _tmp5;
+  const Scalar _tmp22 = _tmp10 * (2 * _tmp0 - 4 * _tmp1 + _tmp3) - _tmp21 * state(1, 0);
-  const Scalar _tmp22 = _tmp5 * state(0, 0);
+  const Scalar _tmp23 = (Scalar(1) / Scalar(2)) * _tmp22;
-  const Scalar _tmp23 =
+  const Scalar _tmp24 = 2 * state(4, 0);
-      _tmp13 * _tmp6 + _tmp16 * _tmp22 - _tmp21 * state(1, 0) - _tmp8 * state(3, 0);
+  const Scalar _tmp25 = 4 * state(3, 0);
-  const Scalar _tmp24 =
+  const Scalar _tmp26 = _tmp2 * state(2, 0);
-      _tmp13 * _tmp22 - _tmp16 * _tmp6 - _tmp21 * state(3, 0) + _tmp8 * state(1, 0);
+  const Scalar _tmp27 = -_tmp24 * state(0, 0) - _tmp25 * state(5, 0) + _tmp26;
-  const Scalar _tmp25 = _tmp9 * state(3, 0);
+  const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp10;
-  const Scalar _tmp26 = _tmp1 * state(2, 0);
+  const Scalar _tmp29 = _tmp28 * state(0, 0);
-  const Scalar _tmp27 = _tmp4 * (-_tmp25 + _tmp26);
+  const Scalar _tmp30 = (Scalar(1) / Scalar(2)) * state(3, 0);
-  const Scalar _tmp28 = _tmp4 * (_tmp11 * state(3, 0) + _tmp9 * state(1, 0));
+  const Scalar _tmp31 = _tmp2 * state(1, 0);
-  const Scalar _tmp29 = _tmp4 * (_tmp19 - 2 * std::pow(state(2, 0), Scalar(2)));
+  const Scalar _tmp32 = _tmp10 * (-_tmp24 * state(3, 0) + _tmp31);
-  const Scalar _tmp30 = 4 * state(4, 0);
+  const Scalar _tmp33 = _tmp2 * state(3, 0);
-  const Scalar _tmp31 = _tmp2 - _tmp30 * state(3, 0) + _tmp9 * state(5, 0);
+  const Scalar _tmp34 = _tmp10 * (_tmp24 * state(1, 0) + _tmp33) - _tmp21 * state(2, 0);
-  const Scalar _tmp32 = 2 * state(5, 0);
+  const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp34;
-  const Scalar _tmp33 = -_tmp15 - _tmp30 * state(2, 0) + _tmp32 * state(1, 0);
+  const Scalar _tmp36 =
-  const Scalar _tmp34 = _tmp32 * state(2, 0) + _tmp7;
+      -_tmp23 * state(2, 0) + _tmp27 * _tmp29 - _tmp30 * _tmp32 + _tmp35 * state(1, 0);
-  const Scalar _tmp35 = 2 * _tmp10 - _tmp12;
+  const Scalar _tmp37 = _tmp10 * _tmp14;
-  const Scalar _tmp36 = _tmp35 * _tmp5;
+  const Scalar _tmp38 = _tmp28 * state(2, 0);
-  const Scalar _tmp37 = _tmp17 * _tmp33 - _tmp22 * _tmp34 - _tmp31 * _tmp6 + _tmp36 * state(1, 0);
+  const Scalar _tmp39 = (Scalar(1) / Scalar(2)) * _tmp32;
-  const Scalar _tmp38 = -_tmp14 * _tmp33 - _tmp22 * _tmp31 + _tmp34 * _tmp6 + _tmp36 * state(3, 0);
+  const Scalar _tmp40 =
-  const Scalar _tmp39 = _tmp14 * _tmp31 - _tmp17 * _tmp34 - _tmp22 * _tmp33 + _tmp35 * _tmp6;
+      _tmp23 * state(0, 0) + _tmp27 * _tmp38 - _tmp30 * _tmp34 - _tmp39 * state(1, 0);
-  const Scalar _tmp40 = _tmp4 * (_tmp25 + _tmp26);
+  const Scalar _tmp41 = _tmp28 * state(1, 0);
-  const Scalar _tmp41 = _tmp4 * (_tmp1 * state(3, 0) - _tmp9 * state(2, 0));
+  const Scalar _tmp42 =
+      _tmp22 * _tmp30 - _tmp27 * _tmp41 + _tmp35 * state(0, 0) - _tmp39 * state(2, 0);
+  const Scalar _tmp43 = _tmp10 * _tmp18;
+  const Scalar _tmp44 = _tmp6 / std::pow(_tmp8, Scalar(2));
+  const Scalar _tmp45 = _tmp19 * _tmp44;
+  const Scalar _tmp46 = _tmp10 * _tmp16;
+  const Scalar _tmp47 = _tmp12 * state(3, 0) - _tmp15 * state(0, 0);
+  const Scalar _tmp48 = _tmp10 * _tmp47;
+  const Scalar _tmp49 = _tmp11 + _tmp13;
+  const Scalar _tmp50 = _tmp10 * _tmp49;
+  const Scalar _tmp51 = _tmp17 + _tmp4 + 1;
+  const Scalar _tmp52 = _tmp10 * _tmp51;
+  const Scalar _tmp53 = _tmp47 * state(6, 0) + _tmp49 * state(5, 0) + _tmp51 * state(4, 0);
+  const Scalar _tmp54 = _tmp20 * _tmp53;
+  const Scalar _tmp55 = 2 * state(5, 0);
+  const Scalar _tmp56 = -_tmp10 * (_tmp33 + _tmp55 * state(2, 0)) + _tmp54 * state(1, 0);
+  const Scalar _tmp57 = -_tmp10 * (-4 * _tmp0 + 2 * _tmp1 - _tmp3) + _tmp54 * state(2, 0);
+  const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp57;
+  const Scalar _tmp59 = -_tmp25 * state(4, 0) + _tmp31 + _tmp55 * state(0, 0);
+  const Scalar _tmp60 = -_tmp26 + _tmp55 * state(3, 0);
+  const Scalar _tmp61 = _tmp30 * _tmp56 + _tmp38 * _tmp60 + _tmp41 * _tmp59 + _tmp58 * state(0, 0);
+  const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp56;
+  const Scalar _tmp63 = -_tmp30 * _tmp57 - _tmp38 * _tmp59 + _tmp41 * _tmp60 + _tmp62 * state(0, 0);
+  const Scalar _tmp64 =
+      _tmp10 * _tmp30 * _tmp60 - _tmp29 * _tmp59 + _tmp58 * state(1, 0) - _tmp62 * state(2, 0);
+  const Scalar _tmp65 = _tmp44 * _tmp53;
 
   // Output terms (2)
   if (innov_var != nullptr) {
     matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
 
-    _innov_var(0, 0) = R +
+    _innov_var(0, 0) =
-                       _tmp18 * (P(0, 1) * _tmp23 + P(1, 1) * _tmp18 + P(2, 1) * _tmp24 +
+        R +
-                                 P(3, 1) * _tmp27 + P(4, 1) * _tmp20 + P(5, 1) * _tmp28) +
+        _tmp36 * (P(0, 2) * _tmp40 + P(1, 2) * _tmp42 + P(2, 2) * _tmp36 - P(23, 2) * _tmp45 +
-                       _tmp20 * (P(0, 4) * _tmp23 + P(1, 4) * _tmp18 + P(2, 4) * _tmp24 +
+                  P(3, 2) * _tmp37 + P(4, 2) * _tmp43 + P(5, 2) * _tmp46 + P(8, 2) * _tmp45) +
-                                 P(3, 4) * _tmp27 + P(4, 4) * _tmp20 + P(5, 4) * _tmp28) +
+        _tmp37 * (P(0, 3) * _tmp40 + P(1, 3) * _tmp42 + P(2, 3) * _tmp36 - P(23, 3) * _tmp45 +
-                       _tmp23 * (P(0, 0) * _tmp23 + P(1, 0) * _tmp18 + P(2, 0) * _tmp24 +
+                  P(3, 3) * _tmp37 + P(4, 3) * _tmp43 + P(5, 3) * _tmp46 + P(8, 3) * _tmp45) +
-                                 P(3, 0) * _tmp27 + P(4, 0) * _tmp20 + P(5, 0) * _tmp28) +
+        _tmp40 * (P(0, 0) * _tmp40 + P(1, 0) * _tmp42 + P(2, 0) * _tmp36 - P(23, 0) * _tmp45 +
-                       _tmp24 * (P(0, 2) * _tmp23 + P(1, 2) * _tmp18 + P(2, 2) * _tmp24 +
+                  P(3, 0) * _tmp37 + P(4, 0) * _tmp43 + P(5, 0) * _tmp46 + P(8, 0) * _tmp45) +
-                                 P(3, 2) * _tmp27 + P(4, 2) * _tmp20 + P(5, 2) * _tmp28) +
+        _tmp42 * (P(0, 1) * _tmp40 + P(1, 1) * _tmp42 + P(2, 1) * _tmp36 - P(23, 1) * _tmp45 +
-                       _tmp27 * (P(0, 3) * _tmp23 + P(1, 3) * _tmp18 + P(2, 3) * _tmp24 +
+                  P(3, 1) * _tmp37 + P(4, 1) * _tmp43 + P(5, 1) * _tmp46 + P(8, 1) * _tmp45) +
-                                 P(3, 3) * _tmp27 + P(4, 3) * _tmp20 + P(5, 3) * _tmp28) +
+        _tmp43 * (P(0, 4) * _tmp40 + P(1, 4) * _tmp42 + P(2, 4) * _tmp36 - P(23, 4) * _tmp45 +
-                       _tmp28 * (P(0, 5) * _tmp23 + P(1, 5) * _tmp18 + P(2, 5) * _tmp24 +
+                  P(3, 4) * _tmp37 + P(4, 4) * _tmp43 + P(5, 4) * _tmp46 + P(8, 4) * _tmp45) -
-                                 P(3, 5) * _tmp27 + P(4, 5) * _tmp20 + P(5, 5) * _tmp28);
+        _tmp45 * (P(0, 23) * _tmp40 + P(1, 23) * _tmp42 + P(2, 23) * _tmp36 - P(23, 23) * _tmp45 +
-    _innov_var(1, 0) = R -
+                  P(3, 23) * _tmp37 + P(4, 23) * _tmp43 + P(5, 23) * _tmp46 + P(8, 23) * _tmp45) +
-                       _tmp29 * (P(0, 3) * _tmp37 + P(1, 3) * _tmp39 + P(2, 3) * _tmp38 -
+        _tmp45 * (P(0, 8) * _tmp40 + P(1, 8) * _tmp42 + P(2, 8) * _tmp36 - P(23, 8) * _tmp45 +
-                                 P(3, 3) * _tmp29 - P(4, 3) * _tmp40 - P(5, 3) * _tmp41) +
+                  P(3, 8) * _tmp37 + P(4, 8) * _tmp43 + P(5, 8) * _tmp46 + P(8, 8) * _tmp45) +
-                       _tmp37 * (P(0, 0) * _tmp37 + P(1, 0) * _tmp39 + P(2, 0) * _tmp38 -
+        _tmp46 * (P(0, 5) * _tmp40 + P(1, 5) * _tmp42 + P(2, 5) * _tmp36 - P(23, 5) * _tmp45 +
-                                 P(3, 0) * _tmp29 - P(4, 0) * _tmp40 - P(5, 0) * _tmp41) +
+                  P(3, 5) * _tmp37 + P(4, 5) * _tmp43 + P(5, 5) * _tmp46 + P(8, 5) * _tmp45);
-                       _tmp38 * (P(0, 2) * _tmp37 + P(1, 2) * _tmp39 + P(2, 2) * _tmp38 -
+    _innov_var(1, 0) =
-                                 P(3, 2) * _tmp29 - P(4, 2) * _tmp40 - P(5, 2) * _tmp41) +
+        R -
-                       _tmp39 * (P(0, 1) * _tmp37 + P(1, 1) * _tmp39 + P(2, 1) * _tmp38 -
+        _tmp48 * (P(0, 5) * _tmp63 + P(1, 5) * _tmp61 + P(2, 5) * _tmp64 + P(23, 5) * _tmp65 -
-                                 P(3, 1) * _tmp29 - P(4, 1) * _tmp40 - P(5, 1) * _tmp41) -
+                  P(3, 5) * _tmp52 - P(4, 5) * _tmp50 - P(5, 5) * _tmp48 - P(8, 5) * _tmp65) -
-                       _tmp40 * (P(0, 4) * _tmp37 + P(1, 4) * _tmp39 + P(2, 4) * _tmp38 -
+        _tmp50 * (P(0, 4) * _tmp63 + P(1, 4) * _tmp61 + P(2, 4) * _tmp64 + P(23, 4) * _tmp65 -
-                                 P(3, 4) * _tmp29 - P(4, 4) * _tmp40 - P(5, 4) * _tmp41) -
+                  P(3, 4) * _tmp52 - P(4, 4) * _tmp50 - P(5, 4) * _tmp48 - P(8, 4) * _tmp65) -
-                       _tmp41 * (P(0, 5) * _tmp37 + P(1, 5) * _tmp39 + P(2, 5) * _tmp38 -
+        _tmp52 * (P(0, 3) * _tmp63 + P(1, 3) * _tmp61 + P(2, 3) * _tmp64 + P(23, 3) * _tmp65 -
-                                 P(3, 5) * _tmp29 - P(4, 5) * _tmp40 - P(5, 5) * _tmp41);
+                  P(3, 3) * _tmp52 - P(4, 3) * _tmp50 - P(5, 3) * _tmp48 - P(8, 3) * _tmp65) +
+        _tmp61 * (P(0, 1) * _tmp63 + P(1, 1) * _tmp61 + P(2, 1) * _tmp64 + P(23, 1) * _tmp65 -
+                  P(3, 1) * _tmp52 - P(4, 1) * _tmp50 - P(5, 1) * _tmp48 - P(8, 1) * _tmp65) +
+        _tmp63 * (P(0, 0) * _tmp63 + P(1, 0) * _tmp61 + P(2, 0) * _tmp64 + P(23, 0) * _tmp65 -
+                  P(3, 0) * _tmp52 - P(4, 0) * _tmp50 - P(5, 0) * _tmp48 - P(8, 0) * _tmp65) +
+        _tmp64 * (P(0, 2) * _tmp63 + P(1, 2) * _tmp61 + P(2, 2) * _tmp64 + P(23, 2) * _tmp65 -
+                  P(3, 2) * _tmp52 - P(4, 2) * _tmp50 - P(5, 2) * _tmp48 - P(8, 2) * _tmp65) +
+        _tmp65 * (P(0, 23) * _tmp63 + P(1, 23) * _tmp61 + P(2, 23) * _tmp64 + P(23, 23) * _tmp65 -
+                  P(3, 23) * _tmp52 - P(4, 23) * _tmp50 - P(5, 23) * _tmp48 - P(8, 23) * _tmp65) -
+        _tmp65 * (P(0, 8) * _tmp63 + P(1, 8) * _tmp61 + P(2, 8) * _tmp64 + P(23, 8) * _tmp65 -
+                  P(3, 8) * _tmp52 - P(4, 8) * _tmp50 - P(5, 8) * _tmp48 - P(8, 8) * _tmp65);
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 
-    _h(0, 0) = _tmp23;
+    _h(0, 0) = _tmp40;
-    _h(1, 0) = _tmp18;
+    _h(1, 0) = _tmp42;
-    _h(2, 0) = _tmp24;
+    _h(2, 0) = _tmp36;
-    _h(3, 0) = _tmp27;
+    _h(3, 0) = _tmp37;
-    _h(4, 0) = _tmp20;
+    _h(4, 0) = _tmp43;
-    _h(5, 0) = _tmp28;
+    _h(5, 0) = _tmp46;
+    _h(8, 0) = _tmp45;
+    _h(23, 0) = -_tmp45;
   }
 }  // NOLINT(readability/fn_size)
 

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

@@ -16,85 +16,100 @@ namespace sym {
  * Symbolic function: compute_flow_y_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
- *     distance: Scalar
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
  */
 template <typename Scalar>
-void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
+                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
-                              const Scalar R, const Scalar epsilon,
+                              const Scalar epsilon, Scalar* const innov_var = nullptr,
-                              Scalar* const innov_var = nullptr,
+                              matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-                              matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+  // Total ops: 232
-  // Total ops: 151
 
   // Input arrays
 
-  // Intermediate terms (21)
+  // Intermediate terms (28)
-  const Scalar _tmp0 =
+  const Scalar _tmp0 = 2 * state(0, 0);
-      Scalar(1.0) /
+  const Scalar _tmp1 = 2 * state(1, 0);
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
+  const Scalar _tmp2 = -_tmp0 * state(2, 0) + _tmp1 * state(3, 0);
-  const Scalar _tmp1 =
+  const Scalar _tmp3 = 1 - 2 * std::pow(state(2, 0), Scalar(2));
-      _tmp0 * (-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1);
+  const Scalar _tmp4 = _tmp3 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp2 = 4 * state(4, 0);
+  const Scalar _tmp5 = state(24, 0) - state(9, 0);
-  const Scalar _tmp3 = 2 * state(0, 0);
+  const Scalar _tmp6 =
-  const Scalar _tmp4 = 2 * state(6, 0);
+      _tmp5 + epsilon * (2 * math::min<Scalar>(0, (((_tmp5) > 0) - ((_tmp5) < 0))) + 1);
-  const Scalar _tmp5 = -_tmp2 * state(3, 0) + _tmp3 * state(5, 0) + _tmp4 * state(1, 0);
+  const Scalar _tmp7 = Scalar(1.0) / (_tmp6);
-  const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp0;
+  const Scalar _tmp8 = _tmp4 * _tmp7;
-  const Scalar _tmp7 = _tmp5 * _tmp6;
+  const Scalar _tmp9 = _tmp2 * _tmp8;
-  const Scalar _tmp8 = 2 * state(1, 0);
+  const Scalar _tmp10 = _tmp0 * state(3, 0) + _tmp1 * state(2, 0);
-  const Scalar _tmp9 = -_tmp2 * state(2, 0) - _tmp4 * state(0, 0) + _tmp8 * state(5, 0);
+  const Scalar _tmp11 = _tmp10 * _tmp8;
-  const Scalar _tmp10 = _tmp6 * _tmp9;
+  const Scalar _tmp12 = _tmp3 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp11 = 2 * state(5, 0);
+  const Scalar _tmp13 = _tmp12 * _tmp8;
-  const Scalar _tmp12 = _tmp6 * (_tmp11 * state(2, 0) + _tmp4 * state(3, 0));
+  const Scalar _tmp14 = _tmp10 * state(5, 0) + _tmp12 * state(4, 0) + _tmp2 * state(6, 0);
-  const Scalar _tmp13 = _tmp11 * state(3, 0) - _tmp4 * state(2, 0);
+  const Scalar _tmp15 = 4 * _tmp14 * _tmp7;
-  const Scalar _tmp14 = _tmp6 * state(1, 0);
+  const Scalar _tmp16 = 2 * state(5, 0);
-  const Scalar _tmp15 =
+  const Scalar _tmp17 = 2 * state(6, 0);
-      _tmp10 * state(3, 0) - _tmp12 * state(0, 0) + _tmp13 * _tmp14 - _tmp7 * state(2, 0);
-  const Scalar _tmp16 = _tmp13 * _tmp6;
-  const Scalar _tmp17 =
-      _tmp12 * state(2, 0) - _tmp14 * _tmp9 + _tmp16 * state(3, 0) - _tmp7 * state(0, 0);
   const Scalar _tmp18 =
-      -_tmp10 * state(0, 0) - _tmp12 * state(3, 0) + _tmp14 * _tmp5 + _tmp16 * state(2, 0);
+      (Scalar(1) / Scalar(2)) * _tmp15 * state(1, 0) -
-  const Scalar _tmp19 = _tmp0 * (_tmp3 * state(3, 0) + _tmp8 * state(2, 0));
+      Scalar(1) / Scalar(2) * _tmp8 * (_tmp16 * state(2, 0) + _tmp17 * state(3, 0));
-  const Scalar _tmp20 = _tmp0 * (-_tmp3 * state(2, 0) + _tmp8 * state(3, 0));
+  const Scalar _tmp19 = 4 * state(4, 0);
+  const Scalar _tmp20 = (Scalar(1) / Scalar(2)) * _tmp15 * state(2, 0) -
+                        Scalar(1) / Scalar(2) * _tmp8 *
+                            (_tmp16 * state(1, 0) - _tmp17 * state(0, 0) - _tmp19 * state(2, 0));
+  const Scalar _tmp21 = (Scalar(1) / Scalar(2)) * _tmp8;
+  const Scalar _tmp22 =
+      _tmp21 * (_tmp16 * state(0, 0) + _tmp17 * state(1, 0) - _tmp19 * state(3, 0));
+  const Scalar _tmp23 = _tmp21 * (_tmp16 * state(3, 0) - _tmp17 * state(2, 0));
+  const Scalar _tmp24 =
+      _tmp18 * state(3, 0) + _tmp20 * state(0, 0) + _tmp22 * state(1, 0) + _tmp23 * state(2, 0);
+  const Scalar _tmp25 =
+      _tmp18 * state(0, 0) - _tmp20 * state(3, 0) - _tmp22 * state(2, 0) + _tmp23 * state(1, 0);
+  const Scalar _tmp26 =
+      -_tmp18 * state(2, 0) + _tmp20 * state(1, 0) - _tmp22 * state(0, 0) + _tmp23 * state(3, 0);
+  const Scalar _tmp27 = _tmp14 * _tmp4 / std::pow(_tmp6, Scalar(2));
 
   // Output terms (2)
   if (innov_var != nullptr) {
     Scalar& _innov_var = (*innov_var);
 
-    _innov_var = R -
+    _innov_var =
-                 _tmp1 * (P(0, 3) * _tmp15 + P(1, 3) * _tmp18 + P(2, 3) * _tmp17 - P(3, 3) * _tmp1 -
+        R -
-                          P(4, 3) * _tmp19 - P(5, 3) * _tmp20) +
+        _tmp11 * (P(0, 4) * _tmp25 + P(1, 4) * _tmp24 + P(2, 4) * _tmp26 + P(23, 4) * _tmp27 -
-                 _tmp15 * (P(0, 0) * _tmp15 + P(1, 0) * _tmp18 + P(2, 0) * _tmp17 -
+                  P(3, 4) * _tmp13 - P(4, 4) * _tmp11 - P(5, 4) * _tmp9 - P(8, 4) * _tmp27) -
-                           P(3, 0) * _tmp1 - P(4, 0) * _tmp19 - P(5, 0) * _tmp20) +
+        _tmp13 * (P(0, 3) * _tmp25 + P(1, 3) * _tmp24 + P(2, 3) * _tmp26 + P(23, 3) * _tmp27 -
-                 _tmp17 * (P(0, 2) * _tmp15 + P(1, 2) * _tmp18 + P(2, 2) * _tmp17 -
+                  P(3, 3) * _tmp13 - P(4, 3) * _tmp11 - P(5, 3) * _tmp9 - P(8, 3) * _tmp27) +
-                           P(3, 2) * _tmp1 - P(4, 2) * _tmp19 - P(5, 2) * _tmp20) +
+        _tmp24 * (P(0, 1) * _tmp25 + P(1, 1) * _tmp24 + P(2, 1) * _tmp26 + P(23, 1) * _tmp27 -
-                 _tmp18 * (P(0, 1) * _tmp15 + P(1, 1) * _tmp18 + P(2, 1) * _tmp17 -
+                  P(3, 1) * _tmp13 - P(4, 1) * _tmp11 - P(5, 1) * _tmp9 - P(8, 1) * _tmp27) +
-                           P(3, 1) * _tmp1 - P(4, 1) * _tmp19 - P(5, 1) * _tmp20) -
+        _tmp25 * (P(0, 0) * _tmp25 + P(1, 0) * _tmp24 + P(2, 0) * _tmp26 + P(23, 0) * _tmp27 -
-                 _tmp19 * (P(0, 4) * _tmp15 + P(1, 4) * _tmp18 + P(2, 4) * _tmp17 -
+                  P(3, 0) * _tmp13 - P(4, 0) * _tmp11 - P(5, 0) * _tmp9 - P(8, 0) * _tmp27) +
-                           P(3, 4) * _tmp1 - P(4, 4) * _tmp19 - P(5, 4) * _tmp20) -
+        _tmp26 * (P(0, 2) * _tmp25 + P(1, 2) * _tmp24 + P(2, 2) * _tmp26 + P(23, 2) * _tmp27 -
-                 _tmp20 * (P(0, 5) * _tmp15 + P(1, 5) * _tmp18 + P(2, 5) * _tmp17 -
+                  P(3, 2) * _tmp13 - P(4, 2) * _tmp11 - P(5, 2) * _tmp9 - P(8, 2) * _tmp27) +
-                           P(3, 5) * _tmp1 - P(4, 5) * _tmp19 - P(5, 5) * _tmp20);
+        _tmp27 * (P(0, 23) * _tmp25 + P(1, 23) * _tmp24 + P(2, 23) * _tmp26 + P(23, 23) * _tmp27 -
+                  P(3, 23) * _tmp13 - P(4, 23) * _tmp11 - P(5, 23) * _tmp9 - P(8, 23) * _tmp27) -
+        _tmp27 * (P(0, 8) * _tmp25 + P(1, 8) * _tmp24 + P(2, 8) * _tmp26 + P(23, 8) * _tmp27 -
+                  P(3, 8) * _tmp13 - P(4, 8) * _tmp11 - P(5, 8) * _tmp9 - P(8, 8) * _tmp27) -
+        _tmp9 * (P(0, 5) * _tmp25 + P(1, 5) * _tmp24 + P(2, 5) * _tmp26 + P(23, 5) * _tmp27 -
+                 P(3, 5) * _tmp13 - P(4, 5) * _tmp11 - P(5, 5) * _tmp9 - P(8, 5) * _tmp27);
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 
-    _h(0, 0) = _tmp15;
+    _h(0, 0) = _tmp25;
-    _h(1, 0) = _tmp18;
+    _h(1, 0) = _tmp24;
-    _h(2, 0) = _tmp17;
+    _h(2, 0) = _tmp26;
-    _h(3, 0) = -_tmp1;
+    _h(3, 0) = -_tmp13;
-    _h(4, 0) = -_tmp19;
+    _h(4, 0) = -_tmp11;
-    _h(5, 0) = -_tmp20;
+    _h(5, 0) = -_tmp9;
+    _h(8, 0) = -_tmp27;
+    _h(23, 0) = _tmp27;
   }
 }  // NOLINT(readability/fn_size)
 

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

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: compute_gnss_yaw_pred_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     antenna_yaw_offset: Scalar
  *     R: Scalar
  *     epsilon: Scalar
@@ -25,15 +25,15 @@ namespace sym {
  * Outputs:
  *     meas_pred: Scalar
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
  */
 template <typename Scalar>
-void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                                    const matrix::Matrix<Scalar, 23, 23>& P,
+                                    const matrix::Matrix<Scalar, 24, 24>& P,
                                     const Scalar antenna_yaw_offset, const Scalar R,
                                     const Scalar epsilon, Scalar* const meas_pred = nullptr,
                                     Scalar* const innov_var = nullptr,
-                                    matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                                    matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
   // Total ops: 114
 
   // Input arrays
@@ -93,7 +93,7 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 

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

@@ -16,19 +16,19 @@ namespace sym {
  * Symbolic function: compute_gravity_xyz_innov_var_and_hx
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     R: Scalar
  *
  * Outputs:
  *     innov_var: Matrix31
- *     Hx: Matrix23_1
+ *     Hx: Matrix24_1
  */
 template <typename Scalar>
-void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
-                                    const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                                    const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                     matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
-                                    matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
+                                    matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
   // Total ops: 53
 
   // Input arrays
@@ -61,7 +61,7 @@ void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (Hx != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
+    matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);
 
     _hx.setZero();
 

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

@@ -16,19 +16,19 @@ namespace sym {
  * Symbolic function: compute_gravity_y_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     R: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     Hy: Matrix23_1
+ *     Hy: Matrix24_1
  */
 template <typename Scalar>
-void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                                 const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                                 const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                  Scalar* const innov_var = nullptr,
-                                 matrix::Matrix<Scalar, 23, 1>* const Hy = nullptr) {
+                                 matrix::Matrix<Scalar, 24, 1>* const Hy = nullptr) {
   // Total ops: 22
 
   // Input arrays
@@ -47,7 +47,7 @@ void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (Hy != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hy = (*Hy);
+    matrix::Matrix<Scalar, 24, 1>& _hy = (*Hy);
 
     _hy.setZero();
 

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

@@ -16,19 +16,19 @@ namespace sym {
  * Symbolic function: compute_gravity_z_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     R: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     Hz: Matrix23_1
+ *     Hz: Matrix24_1
  */
 template <typename Scalar>
-void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                                 const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                                 const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                  Scalar* const innov_var = nullptr,
-                                 matrix::Matrix<Scalar, 23, 1>* const Hz = nullptr) {
+                                 matrix::Matrix<Scalar, 24, 1>* const Hz = nullptr) {
   // Total ops: 18
 
   // Input arrays
@@ -48,7 +48,7 @@ void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (Hz != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hz = (*Hz);
+    matrix::Matrix<Scalar, 24, 1>& _hz = (*Hz);
 
     _hz.setZero();
 

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

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

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

@@ -0,0 +1,43 @@
+// -----------------------------------------------------------------------------
+// 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: compute_hagl_innov_var
+ *
+ * Args:
+ *     P: Matrix24_24
+ *     R: Scalar
+ *
+ * Outputs:
+ *     innov_var: Scalar
+ */
+template <typename Scalar>
+void ComputeHaglInnovVar(const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
+                         Scalar* const innov_var = nullptr) {
+  // Total ops: 4
+
+  // Input arrays
+
+  // Intermediate terms (0)
+
+  // Output terms (1)
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var = P(23, 23) - P(23, 8) - P(8, 23) + P(8, 8) + R;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

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

@@ -16,22 +16,22 @@ namespace sym {
  * Symbolic function: compute_mag_declination_pred_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     pred: Scalar
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
  */
 template <typename Scalar>
-void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                                           const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                                           const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                            const Scalar epsilon, Scalar* const pred = nullptr,
                                            Scalar* const innov_var = nullptr,
-                                           matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                                           matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
   // Total ops: 22
 
   // Input arrays
@@ -59,7 +59,7 @@ void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>&
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 

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

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: compute_mag_innov_innov_var_and_hx
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     meas: Matrix31
  *     R: Scalar
  *     epsilon: Scalar
@@ -25,16 +25,16 @@ namespace sym {
  * Outputs:
  *     innov: Matrix31
  *     innov_var: Matrix31
- *     Hx: Matrix23_1
+ *     Hx: Matrix24_1
  */
 template <typename Scalar>
-void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
-                                  const matrix::Matrix<Scalar, 23, 23>& P,
+                                  const matrix::Matrix<Scalar, 24, 24>& P,
                                   const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
                                   const Scalar epsilon,
                                   matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
                                   matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
-                                  matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
+                                  matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
   // Total ops: 461
 
   // Unused inputs
@@ -185,7 +185,7 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (Hx != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
+    matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);
 
     _hx.setZero();
 

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

@@ -16,20 +16,20 @@ namespace sym {
  * Symbolic function: compute_mag_y_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
  */
 template <typename Scalar>
-void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                             const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                             const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                              const Scalar epsilon, Scalar* const innov_var = nullptr,
-                             matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                             matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
   // Total ops: 159
 
   // Unused inputs
@@ -85,7 +85,7 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 

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

@@ -16,20 +16,20 @@ namespace sym {
  * Symbolic function: compute_mag_z_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
  */
 template <typename Scalar>
-void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                             const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                             const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                              const Scalar epsilon, Scalar* const innov_var = nullptr,
-                             matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                             matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
   // Total ops: 161
 
   // Unused inputs
@@ -85,7 +85,7 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 

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

@@ -16,169 +16,172 @@ namespace sym {
  * Symbolic function: compute_sideslip_h_and_k
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     innov_var: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
- *     H: Matrix23_1
+ *     H: Matrix24_1
- *     K: Matrix23_1
+ *     K: Matrix24_1
  */
 template <typename Scalar>
-void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
-                          const matrix::Matrix<Scalar, 23, 23>& P, const Scalar innov_var,
+                          const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
-                          const Scalar epsilon, matrix::Matrix<Scalar, 23, 1>* const H = nullptr,
+                          const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
-                          matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
+                          matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
-  // Total ops: 497
+  // Total ops: 513
 
   // Input arrays
 
-  // Intermediate terms (45)
+  // Intermediate terms (43)
   const Scalar _tmp0 = -state(23, 0) + state(5, 0);
   const Scalar _tmp1 = 2 * state(1, 0);
   const Scalar _tmp2 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp3 = 2 * state(6, 0);
+  const Scalar _tmp3 = 4 * _tmp2;
-  const Scalar _tmp4 = _tmp3 * state(0, 0);
+  const Scalar _tmp4 = 2 * state(6, 0);
-  const Scalar _tmp5 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp5 = _tmp4 * state(0, 0);
-  const Scalar _tmp6 = _tmp5 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp6 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp7 = 2 * state(0, 0);
+  const Scalar _tmp7 = _tmp6 - 2 * std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp8 = _tmp7 * state(3, 0);
+  const Scalar _tmp8 = 2 * state(0, 0);
-  const Scalar _tmp9 = 2 * state(2, 0);
+  const Scalar _tmp9 = _tmp8 * state(3, 0);
-  const Scalar _tmp10 = _tmp9 * state(1, 0);
+  const Scalar _tmp10 = 2 * state(2, 0);
-  const Scalar _tmp11 = _tmp10 + _tmp8;
+  const Scalar _tmp11 = _tmp10 * state(1, 0);
-  const Scalar _tmp12 = _tmp1 * state(3, 0) - _tmp9 * state(0, 0);
+  const Scalar _tmp12 = _tmp11 + _tmp9;
-  const Scalar _tmp13 = _tmp0 * _tmp11 + _tmp12 * state(6, 0) + _tmp2 * _tmp6;
+  const Scalar _tmp13 = _tmp1 * state(3, 0) - _tmp10 * state(0, 0);
-  const Scalar _tmp14 =
+  const Scalar _tmp14 = _tmp0 * _tmp12 + _tmp13 * state(6, 0) + _tmp2 * _tmp7;
-      _tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
+  const Scalar _tmp15 =
-  const Scalar _tmp15 = _tmp5 - 2 * std::pow(state(1, 0), Scalar(2));
+      _tmp14 + epsilon * (2 * math::min<Scalar>(0, (((_tmp14) > 0) - ((_tmp14) < 0))) + 1);
-  const Scalar _tmp16 = _tmp10 - _tmp8;
+  const Scalar _tmp16 = _tmp6 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp17 = _tmp7 * state(1, 0) + _tmp9 * state(3, 0);
+  const Scalar _tmp17 = _tmp11 - _tmp9;
-  const Scalar _tmp18 =
+  const Scalar _tmp18 = _tmp10 * state(3, 0) + _tmp8 * state(1, 0);
-      (_tmp0 * _tmp15 + _tmp16 * _tmp2 + _tmp17 * state(6, 0)) / std::pow(_tmp14, Scalar(2));
+  const Scalar _tmp19 =
-  const Scalar _tmp19 = _tmp3 * state(3, 0);
+      (_tmp0 * _tmp16 + _tmp17 * _tmp2 + _tmp18 * state(6, 0)) / std::pow(_tmp15, Scalar(2));
-  const Scalar _tmp20 = Scalar(1.0) / (_tmp14);
+  const Scalar _tmp20 = _tmp4 * state(3, 0);
-  const Scalar _tmp21 = -_tmp18 * (_tmp0 * _tmp1 - 4 * _tmp2 * state(2, 0) - _tmp4) +
+  const Scalar _tmp21 = Scalar(1.0) / (_tmp15);
-                        _tmp20 * (_tmp1 * _tmp2 + _tmp19);
+  const Scalar _tmp22 =
-  const Scalar _tmp22 = (Scalar(1) / Scalar(2)) * state(3, 0);
+      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp1 - _tmp3 * state(2, 0) - _tmp5) +
-  const Scalar _tmp23 =
+      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp1 * _tmp2 + _tmp20);
-      -Scalar(1) / Scalar(2) * _tmp18 * (_tmp0 * _tmp9 + _tmp19) +
+  const Scalar _tmp23 = 4 * _tmp0;
-      (Scalar(1) / Scalar(2)) * _tmp20 * (-4 * _tmp0 * state(1, 0) + _tmp2 * _tmp9 + _tmp4);
+  const Scalar _tmp24 =
-  const Scalar _tmp24 = 2 * state(3, 0);
+      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp10 + _tmp20) +
-  const Scalar _tmp25 = _tmp3 * state(2, 0);
+      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp10 * _tmp2 - _tmp23 * state(1, 0) + _tmp5);
-  const Scalar _tmp26 = _tmp3 * state(1, 0);
+  const Scalar _tmp25 = 2 * state(3, 0);
-  const Scalar _tmp27 = -_tmp18 * (_tmp0 * _tmp24 - _tmp25) + _tmp20 * (-_tmp2 * _tmp24 + _tmp26);
+  const Scalar _tmp26 = _tmp4 * state(2, 0);
-  const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp27;
+  const Scalar _tmp27 = _tmp4 * state(1, 0);
-  const Scalar _tmp29 = 4 * state(3, 0);
+  const Scalar _tmp28 = -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp25 - _tmp26) +
+                        (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp25 + _tmp27);
+  const Scalar _tmp29 =
+      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp8 + _tmp27 - _tmp3 * state(3, 0)) +
+      (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp8 - _tmp23 * state(3, 0) + _tmp26);
   const Scalar _tmp30 =
-      -Scalar(1) / Scalar(2) * _tmp18 * (_tmp0 * _tmp7 - _tmp2 * _tmp29 + _tmp26) +
+      -_tmp22 * state(3, 0) + _tmp24 * state(0, 0) - _tmp28 * state(1, 0) + _tmp29 * state(2, 0);
-      (Scalar(1) / Scalar(2)) * _tmp20 * (-_tmp0 * _tmp29 - _tmp2 * _tmp7 + _tmp25);
   const Scalar _tmp31 =
-      -_tmp21 * _tmp22 + _tmp23 * state(0, 0) - _tmp28 * state(1, 0) + _tmp30 * state(2, 0);
+      _tmp22 * state(0, 0) + _tmp24 * state(3, 0) - _tmp28 * state(2, 0) - _tmp29 * state(1, 0);
-  const Scalar _tmp32 = (Scalar(1) / Scalar(2)) * _tmp21;
+  const Scalar _tmp32 =
-  const Scalar _tmp33 =
+      _tmp22 * state(1, 0) - _tmp24 * state(2, 0) - _tmp28 * state(3, 0) + _tmp29 * state(0, 0);
-      _tmp23 * state(3, 0) - _tmp28 * state(2, 0) - _tmp30 * state(1, 0) + _tmp32 * state(0, 0);
+  const Scalar _tmp33 = _tmp19 * _tmp7;
-  const Scalar _tmp34 =
+  const Scalar _tmp34 = _tmp17 * _tmp21;
-      -_tmp22 * _tmp27 - _tmp23 * state(2, 0) + _tmp30 * state(0, 0) + _tmp32 * state(1, 0);
+  const Scalar _tmp35 = -_tmp33 + _tmp34;
-  const Scalar _tmp35 = _tmp18 * _tmp6;
+  const Scalar _tmp36 = _tmp12 * _tmp19;
-  const Scalar _tmp36 = _tmp16 * _tmp20;
+  const Scalar _tmp37 = _tmp16 * _tmp21;
-  const Scalar _tmp37 = -_tmp35 + _tmp36;
+  const Scalar _tmp38 = -_tmp36 + _tmp37;
-  const Scalar _tmp38 = _tmp11 * _tmp18;
+  const Scalar _tmp39 = -_tmp13 * _tmp19 + _tmp18 * _tmp21;
-  const Scalar _tmp39 = _tmp15 * _tmp20;
+  const Scalar _tmp40 = _tmp33 - _tmp34;
-  const Scalar _tmp40 = -_tmp38 + _tmp39;
+  const Scalar _tmp41 = _tmp36 - _tmp37;
-  const Scalar _tmp41 = -_tmp12 * _tmp18 + _tmp17 * _tmp20;
+  const Scalar _tmp42 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
-  const Scalar _tmp42 = _tmp35 - _tmp36;
-  const Scalar _tmp43 = _tmp38 - _tmp39;
-  const Scalar _tmp44 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
 
   // Output terms (2)
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 
-    _h(0, 0) = _tmp31;
+    _h(0, 0) = _tmp30;
-    _h(1, 0) = _tmp33;
+    _h(1, 0) = _tmp31;
-    _h(2, 0) = _tmp34;
+    _h(2, 0) = _tmp32;
-    _h(3, 0) = _tmp37;
+    _h(3, 0) = _tmp35;
-    _h(4, 0) = _tmp40;
+    _h(4, 0) = _tmp38;
-    _h(5, 0) = _tmp41;
+    _h(5, 0) = _tmp39;
-    _h(21, 0) = _tmp42;
+    _h(21, 0) = _tmp40;
-    _h(22, 0) = _tmp43;
+    _h(22, 0) = _tmp41;
   }
 
   if (K != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _k = (*K);
+    matrix::Matrix<Scalar, 24, 1>& _k = (*K);
 
     _k(0, 0) =
-        _tmp44 * (P(0, 0) * _tmp31 + P(0, 1) * _tmp33 + P(0, 2) * _tmp34 + P(0, 21) * _tmp42 +
+        _tmp42 * (P(0, 0) * _tmp30 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + P(0, 21) * _tmp40 +
-                  P(0, 22) * _tmp43 + P(0, 3) * _tmp37 + P(0, 4) * _tmp40 + P(0, 5) * _tmp41);
+                  P(0, 22) * _tmp41 + P(0, 3) * _tmp35 + P(0, 4) * _tmp38 + P(0, 5) * _tmp39);
     _k(1, 0) =
-        _tmp44 * (P(1, 0) * _tmp31 + P(1, 1) * _tmp33 + P(1, 2) * _tmp34 + P(1, 21) * _tmp42 +
+        _tmp42 * (P(1, 0) * _tmp30 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + P(1, 21) * _tmp40 +
-                  P(1, 22) * _tmp43 + P(1, 3) * _tmp37 + P(1, 4) * _tmp40 + P(1, 5) * _tmp41);
+                  P(1, 22) * _tmp41 + P(1, 3) * _tmp35 + P(1, 4) * _tmp38 + P(1, 5) * _tmp39);
     _k(2, 0) =
-        _tmp44 * (P(2, 0) * _tmp31 + P(2, 1) * _tmp33 + P(2, 2) * _tmp34 + P(2, 21) * _tmp42 +
+        _tmp42 * (P(2, 0) * _tmp30 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + P(2, 21) * _tmp40 +
-                  P(2, 22) * _tmp43 + P(2, 3) * _tmp37 + P(2, 4) * _tmp40 + P(2, 5) * _tmp41);
+                  P(2, 22) * _tmp41 + P(2, 3) * _tmp35 + P(2, 4) * _tmp38 + P(2, 5) * _tmp39);
     _k(3, 0) =
-        _tmp44 * (P(3, 0) * _tmp31 + P(3, 1) * _tmp33 + P(3, 2) * _tmp34 + P(3, 21) * _tmp42 +
+        _tmp42 * (P(3, 0) * _tmp30 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + P(3, 21) * _tmp40 +
-                  P(3, 22) * _tmp43 + P(3, 3) * _tmp37 + P(3, 4) * _tmp40 + P(3, 5) * _tmp41);
+                  P(3, 22) * _tmp41 + P(3, 3) * _tmp35 + P(3, 4) * _tmp38 + P(3, 5) * _tmp39);
     _k(4, 0) =
-        _tmp44 * (P(4, 0) * _tmp31 + P(4, 1) * _tmp33 + P(4, 2) * _tmp34 + P(4, 21) * _tmp42 +
+        _tmp42 * (P(4, 0) * _tmp30 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + P(4, 21) * _tmp40 +
-                  P(4, 22) * _tmp43 + P(4, 3) * _tmp37 + P(4, 4) * _tmp40 + P(4, 5) * _tmp41);
+                  P(4, 22) * _tmp41 + P(4, 3) * _tmp35 + P(4, 4) * _tmp38 + P(4, 5) * _tmp39);
     _k(5, 0) =
-        _tmp44 * (P(5, 0) * _tmp31 + P(5, 1) * _tmp33 + P(5, 2) * _tmp34 + P(5, 21) * _tmp42 +
+        _tmp42 * (P(5, 0) * _tmp30 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + P(5, 21) * _tmp40 +
-                  P(5, 22) * _tmp43 + P(5, 3) * _tmp37 + P(5, 4) * _tmp40 + P(5, 5) * _tmp41);
+                  P(5, 22) * _tmp41 + P(5, 3) * _tmp35 + P(5, 4) * _tmp38 + P(5, 5) * _tmp39);
     _k(6, 0) =
-        _tmp44 * (P(6, 0) * _tmp31 + P(6, 1) * _tmp33 + P(6, 2) * _tmp34 + P(6, 21) * _tmp42 +
+        _tmp42 * (P(6, 0) * _tmp30 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + P(6, 21) * _tmp40 +
-                  P(6, 22) * _tmp43 + P(6, 3) * _tmp37 + P(6, 4) * _tmp40 + P(6, 5) * _tmp41);
+                  P(6, 22) * _tmp41 + P(6, 3) * _tmp35 + P(6, 4) * _tmp38 + P(6, 5) * _tmp39);
     _k(7, 0) =
-        _tmp44 * (P(7, 0) * _tmp31 + P(7, 1) * _tmp33 + P(7, 2) * _tmp34 + P(7, 21) * _tmp42 +
+        _tmp42 * (P(7, 0) * _tmp30 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + P(7, 21) * _tmp40 +
-                  P(7, 22) * _tmp43 + P(7, 3) * _tmp37 + P(7, 4) * _tmp40 + P(7, 5) * _tmp41);
+                  P(7, 22) * _tmp41 + P(7, 3) * _tmp35 + P(7, 4) * _tmp38 + P(7, 5) * _tmp39);
     _k(8, 0) =
-        _tmp44 * (P(8, 0) * _tmp31 + P(8, 1) * _tmp33 + P(8, 2) * _tmp34 + P(8, 21) * _tmp42 +
+        _tmp42 * (P(8, 0) * _tmp30 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + P(8, 21) * _tmp40 +
-                  P(8, 22) * _tmp43 + P(8, 3) * _tmp37 + P(8, 4) * _tmp40 + P(8, 5) * _tmp41);
+                  P(8, 22) * _tmp41 + P(8, 3) * _tmp35 + P(8, 4) * _tmp38 + P(8, 5) * _tmp39);
     _k(9, 0) =
-        _tmp44 * (P(9, 0) * _tmp31 + P(9, 1) * _tmp33 + P(9, 2) * _tmp34 + P(9, 21) * _tmp42 +
+        _tmp42 * (P(9, 0) * _tmp30 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + P(9, 21) * _tmp40 +
-                  P(9, 22) * _tmp43 + P(9, 3) * _tmp37 + P(9, 4) * _tmp40 + P(9, 5) * _tmp41);
+                  P(9, 22) * _tmp41 + P(9, 3) * _tmp35 + P(9, 4) * _tmp38 + P(9, 5) * _tmp39);
     _k(10, 0) =
-        _tmp44 * (P(10, 0) * _tmp31 + P(10, 1) * _tmp33 + P(10, 2) * _tmp34 + P(10, 21) * _tmp42 +
+        _tmp42 * (P(10, 0) * _tmp30 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + P(10, 21) * _tmp40 +
-                  P(10, 22) * _tmp43 + P(10, 3) * _tmp37 + P(10, 4) * _tmp40 + P(10, 5) * _tmp41);
+                  P(10, 22) * _tmp41 + P(10, 3) * _tmp35 + P(10, 4) * _tmp38 + P(10, 5) * _tmp39);
     _k(11, 0) =
-        _tmp44 * (P(11, 0) * _tmp31 + P(11, 1) * _tmp33 + P(11, 2) * _tmp34 + P(11, 21) * _tmp42 +
+        _tmp42 * (P(11, 0) * _tmp30 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + P(11, 21) * _tmp40 +
-                  P(11, 22) * _tmp43 + P(11, 3) * _tmp37 + P(11, 4) * _tmp40 + P(11, 5) * _tmp41);
+                  P(11, 22) * _tmp41 + P(11, 3) * _tmp35 + P(11, 4) * _tmp38 + P(11, 5) * _tmp39);
     _k(12, 0) =
-        _tmp44 * (P(12, 0) * _tmp31 + P(12, 1) * _tmp33 + P(12, 2) * _tmp34 + P(12, 21) * _tmp42 +
+        _tmp42 * (P(12, 0) * _tmp30 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + P(12, 21) * _tmp40 +
-                  P(12, 22) * _tmp43 + P(12, 3) * _tmp37 + P(12, 4) * _tmp40 + P(12, 5) * _tmp41);
+                  P(12, 22) * _tmp41 + P(12, 3) * _tmp35 + P(12, 4) * _tmp38 + P(12, 5) * _tmp39);
     _k(13, 0) =
-        _tmp44 * (P(13, 0) * _tmp31 + P(13, 1) * _tmp33 + P(13, 2) * _tmp34 + P(13, 21) * _tmp42 +
+        _tmp42 * (P(13, 0) * _tmp30 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + P(13, 21) * _tmp40 +
-                  P(13, 22) * _tmp43 + P(13, 3) * _tmp37 + P(13, 4) * _tmp40 + P(13, 5) * _tmp41);
+                  P(13, 22) * _tmp41 + P(13, 3) * _tmp35 + P(13, 4) * _tmp38 + P(13, 5) * _tmp39);
     _k(14, 0) =
-        _tmp44 * (P(14, 0) * _tmp31 + P(14, 1) * _tmp33 + P(14, 2) * _tmp34 + P(14, 21) * _tmp42 +
+        _tmp42 * (P(14, 0) * _tmp30 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + P(14, 21) * _tmp40 +
-                  P(14, 22) * _tmp43 + P(14, 3) * _tmp37 + P(14, 4) * _tmp40 + P(14, 5) * _tmp41);
+                  P(14, 22) * _tmp41 + P(14, 3) * _tmp35 + P(14, 4) * _tmp38 + P(14, 5) * _tmp39);
     _k(15, 0) =
-        _tmp44 * (P(15, 0) * _tmp31 + P(15, 1) * _tmp33 + P(15, 2) * _tmp34 + P(15, 21) * _tmp42 +
+        _tmp42 * (P(15, 0) * _tmp30 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + P(15, 21) * _tmp40 +
-                  P(15, 22) * _tmp43 + P(15, 3) * _tmp37 + P(15, 4) * _tmp40 + P(15, 5) * _tmp41);
+                  P(15, 22) * _tmp41 + P(15, 3) * _tmp35 + P(15, 4) * _tmp38 + P(15, 5) * _tmp39);
     _k(16, 0) =
-        _tmp44 * (P(16, 0) * _tmp31 + P(16, 1) * _tmp33 + P(16, 2) * _tmp34 + P(16, 21) * _tmp42 +
+        _tmp42 * (P(16, 0) * _tmp30 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + P(16, 21) * _tmp40 +
-                  P(16, 22) * _tmp43 + P(16, 3) * _tmp37 + P(16, 4) * _tmp40 + P(16, 5) * _tmp41);
+                  P(16, 22) * _tmp41 + P(16, 3) * _tmp35 + P(16, 4) * _tmp38 + P(16, 5) * _tmp39);
     _k(17, 0) =
-        _tmp44 * (P(17, 0) * _tmp31 + P(17, 1) * _tmp33 + P(17, 2) * _tmp34 + P(17, 21) * _tmp42 +
+        _tmp42 * (P(17, 0) * _tmp30 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + P(17, 21) * _tmp40 +
-                  P(17, 22) * _tmp43 + P(17, 3) * _tmp37 + P(17, 4) * _tmp40 + P(17, 5) * _tmp41);
+                  P(17, 22) * _tmp41 + P(17, 3) * _tmp35 + P(17, 4) * _tmp38 + P(17, 5) * _tmp39);
     _k(18, 0) =
-        _tmp44 * (P(18, 0) * _tmp31 + P(18, 1) * _tmp33 + P(18, 2) * _tmp34 + P(18, 21) * _tmp42 +
+        _tmp42 * (P(18, 0) * _tmp30 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + P(18, 21) * _tmp40 +
-                  P(18, 22) * _tmp43 + P(18, 3) * _tmp37 + P(18, 4) * _tmp40 + P(18, 5) * _tmp41);
+                  P(18, 22) * _tmp41 + P(18, 3) * _tmp35 + P(18, 4) * _tmp38 + P(18, 5) * _tmp39);
     _k(19, 0) =
-        _tmp44 * (P(19, 0) * _tmp31 + P(19, 1) * _tmp33 + P(19, 2) * _tmp34 + P(19, 21) * _tmp42 +
+        _tmp42 * (P(19, 0) * _tmp30 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + P(19, 21) * _tmp40 +
-                  P(19, 22) * _tmp43 + P(19, 3) * _tmp37 + P(19, 4) * _tmp40 + P(19, 5) * _tmp41);
+                  P(19, 22) * _tmp41 + P(19, 3) * _tmp35 + P(19, 4) * _tmp38 + P(19, 5) * _tmp39);
     _k(20, 0) =
-        _tmp44 * (P(20, 0) * _tmp31 + P(20, 1) * _tmp33 + P(20, 2) * _tmp34 + P(20, 21) * _tmp42 +
+        _tmp42 * (P(20, 0) * _tmp30 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + P(20, 21) * _tmp40 +
-                  P(20, 22) * _tmp43 + P(20, 3) * _tmp37 + P(20, 4) * _tmp40 + P(20, 5) * _tmp41);
+                  P(20, 22) * _tmp41 + P(20, 3) * _tmp35 + P(20, 4) * _tmp38 + P(20, 5) * _tmp39);
     _k(21, 0) =
-        _tmp44 * (P(21, 0) * _tmp31 + P(21, 1) * _tmp33 + P(21, 2) * _tmp34 + P(21, 21) * _tmp42 +
+        _tmp42 * (P(21, 0) * _tmp30 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + P(21, 21) * _tmp40 +
-                  P(21, 22) * _tmp43 + P(21, 3) * _tmp37 + P(21, 4) * _tmp40 + P(21, 5) * _tmp41);
+                  P(21, 22) * _tmp41 + P(21, 3) * _tmp35 + P(21, 4) * _tmp38 + P(21, 5) * _tmp39);
     _k(22, 0) =
-        _tmp44 * (P(22, 0) * _tmp31 + P(22, 1) * _tmp33 + P(22, 2) * _tmp34 + P(22, 21) * _tmp42 +
+        _tmp42 * (P(22, 0) * _tmp30 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + P(22, 21) * _tmp40 +
-                  P(22, 22) * _tmp43 + P(22, 3) * _tmp37 + P(22, 4) * _tmp40 + P(22, 5) * _tmp41);
+                  P(22, 22) * _tmp41 + P(22, 3) * _tmp35 + P(22, 4) * _tmp38 + P(22, 5) * _tmp39);
+    _k(23, 0) =
+        _tmp42 * (P(23, 0) * _tmp30 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + P(23, 21) * _tmp40 +
+                  P(23, 22) * _tmp41 + P(23, 3) * _tmp35 + P(23, 4) * _tmp38 + P(23, 5) * _tmp39);
   }
 }  // NOLINT(readability/fn_size)
 

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

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: compute_sideslip_innov_and_innov_var
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     R: Scalar
  *     epsilon: Scalar
  *
@@ -26,8 +26,8 @@ namespace sym {
  *     innov_var: Scalar
  */
 template <typename Scalar>
-void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
-                                     const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                                     const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                      const Scalar epsilon, Scalar* const innov = nullptr,
                                      Scalar* const innov_var = nullptr) {
   // Total ops: 265

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

@@ -16,19 +16,19 @@ namespace sym {
  * Symbolic function: compute_yaw_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     R: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
  */
 template <typename Scalar>
-void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                            const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                            const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                             Scalar* const innov_var = nullptr,
-                            matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                            matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
   // Total ops: 1
 
   // Unused inputs
@@ -46,7 +46,7 @@ void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 

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

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: predict_covariance
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     accel: Matrix31
  *     accel_var: Matrix31
  *     gyro: Matrix31
@@ -25,23 +25,23 @@ namespace sym {
  *     dt: Scalar
  *
  * Outputs:
- *     res: Matrix23_23
+ *     res: Matrix24_24
  */
 template <typename Scalar>
-matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24, 1>& state,
+matrix::Matrix<Scalar, 24, 24> PredictCovariance(const matrix::Matrix<Scalar, 25, 1>& state,
-                                                const matrix::Matrix<Scalar, 23, 23>& P,
+                                                const matrix::Matrix<Scalar, 24, 24>& P,
                                                 const matrix::Matrix<Scalar, 3, 1>& accel,
                                                 const matrix::Matrix<Scalar, 3, 1>& accel_var,
                                                 const matrix::Matrix<Scalar, 3, 1>& gyro,
                                                 const Scalar gyro_var, const Scalar dt) {
-  // Total ops: 1754
+  // Total ops: 1810
 
   // Unused inputs
   (void)gyro;
 
   // Input arrays
 
-  // Intermediate terms (147)
+  // Intermediate terms (144)
   const Scalar _tmp0 = 2 * state(1, 0);
   const Scalar _tmp1 = _tmp0 * state(3, 0);
   const Scalar _tmp2 = -_tmp1 * dt;
@@ -177,64 +177,62 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   const Scalar _tmp110 = _tmp109 * dt;
   const Scalar _tmp111 = -_tmp1;
   const Scalar _tmp112 = _tmp11 + _tmp77;
-  const Scalar _tmp113 =
+  const Scalar _tmp113 = dt * (_tmp67 * (_tmp111 + _tmp4) + _tmp72 * (_tmp104 + _tmp112) +
-      _tmp67 * (_tmp111 + _tmp4) + _tmp72 * (_tmp104 + _tmp112) + _tmp74 * (_tmp106 + _tmp78);
+                               _tmp74 * (_tmp106 + _tmp78));
-  const Scalar _tmp114 = _tmp26 * dt;
+  const Scalar _tmp114 = P(0, 4) * _tmp18 + P(10, 4) * _tmp23 + P(11, 4) * _tmp6 + P(9, 4) * _tmp15;
-  const Scalar _tmp115 = P(0, 4) * _tmp18 + P(10, 4) * _tmp23 + P(11, 4) * _tmp6 + P(9, 4) * _tmp15;
+  const Scalar _tmp115 =
-  const Scalar _tmp116 =
       P(1, 0) * _tmp18 + P(10, 0) * _tmp29 + P(11, 0) * _tmp34 + P(9, 0) * _tmp36;
-  const Scalar _tmp117 = _tmp113 * dt;
+  const Scalar _tmp116 =
-  const Scalar _tmp118 =
       P(1, 4) * _tmp18 + P(10, 4) * _tmp29 + P(11, 4) * _tmp34 + P(9, 4) * _tmp36;
-  const Scalar _tmp119 =
+  const Scalar _tmp117 =
       P(10, 0) * _tmp45 + P(11, 0) * _tmp44 + P(2, 0) * _tmp18 + P(9, 0) * _tmp47;
-  const Scalar _tmp120 =
+  const Scalar _tmp118 =
       P(10, 4) * _tmp45 + P(11, 4) * _tmp44 + P(2, 4) * _tmp18 + P(9, 4) * _tmp47;
-  const Scalar _tmp121 = _tmp56 * _tmp96;
+  const Scalar _tmp119 = _tmp56 * _tmp96;
-  const Scalar _tmp122 = _tmp60 * _tmp97;
+  const Scalar _tmp120 = _tmp60 * _tmp97;
-  const Scalar _tmp123 = P(1, 0) * _tmp75 - P(12, 0) * _tmp57 - P(13, 0) * _tmp61 -
+  const Scalar _tmp121 = P(1, 0) * _tmp75 - P(12, 0) * _tmp57 - P(13, 0) * _tmp61 -
                          P(14, 0) * _tmp64 + P(2, 0) * _tmp79 + P(3, 0);
-  const Scalar _tmp124 = _tmp63 * _tmp98;
+  const Scalar _tmp122 = _tmp63 * _tmp98;
-  const Scalar _tmp125 = P(1, 4) * _tmp75 - P(12, 4) * _tmp57 - P(13, 4) * _tmp61 -
+  const Scalar _tmp123 = P(1, 4) * _tmp75 - P(12, 4) * _tmp57 - P(13, 4) * _tmp61 -
                          P(14, 4) * _tmp64 + P(2, 4) * _tmp79 + P(3, 4);
-  const Scalar _tmp126 = P(0, 0) * _tmp117 - P(12, 0) * _tmp110 - P(13, 0) * _tmp102 -
+  const Scalar _tmp124 = P(0, 0) * _tmp113 - P(12, 0) * _tmp110 - P(13, 0) * _tmp102 -
                          P(14, 0) * _tmp108 + P(2, 0) * _tmp105 + P(4, 0);
-  const Scalar _tmp127 = P(0, 13) * _tmp117 - P(12, 13) * _tmp110 - P(13, 13) * _tmp102 -
+  const Scalar _tmp125 = P(0, 13) * _tmp113 - P(12, 13) * _tmp110 - P(13, 13) * _tmp102 -
                          P(14, 13) * _tmp108 + P(2, 13) * _tmp105 + P(4, 13);
-  const Scalar _tmp128 = P(0, 14) * _tmp117 - P(12, 14) * _tmp110 - P(13, 14) * _tmp102 -
+  const Scalar _tmp126 = P(0, 14) * _tmp113 - P(12, 14) * _tmp110 - P(13, 14) * _tmp102 -
                          P(14, 14) * _tmp108 + P(2, 14) * _tmp105 + P(4, 14);
-  const Scalar _tmp129 = P(0, 12) * _tmp117 - P(12, 12) * _tmp110 - P(13, 12) * _tmp102 -
+  const Scalar _tmp127 = P(0, 12) * _tmp113 - P(12, 12) * _tmp110 - P(13, 12) * _tmp102 -
                          P(14, 12) * _tmp108 + P(2, 12) * _tmp105 + P(4, 12);
-  const Scalar _tmp130 = P(0, 4) * _tmp117 - P(12, 4) * _tmp110 - P(13, 4) * _tmp102 -
+  const Scalar _tmp128 = P(0, 4) * _tmp113 - P(12, 4) * _tmp110 - P(13, 4) * _tmp102 -
                          P(14, 4) * _tmp108 + P(2, 4) * _tmp105 + P(4, 4);
-  const Scalar _tmp131 = _tmp100 + _tmp55;
+  const Scalar _tmp129 = _tmp100 + _tmp55;
-  const Scalar _tmp132 = _tmp131 * dt;
+  const Scalar _tmp130 = _tmp129 * dt;
-  const Scalar _tmp133 =
+  const Scalar _tmp131 =
       dt * (_tmp67 * (_tmp112 + _tmp69) + _tmp72 * (_tmp111 + _tmp65) + _tmp74 * (_tmp21 + _tmp76));
-  const Scalar _tmp134 = _tmp73 * dt;
+  const Scalar _tmp132 = _tmp73 * dt;
-  const Scalar _tmp135 = _tmp66 * dt;
+  const Scalar _tmp133 = _tmp66 * dt;
-  const Scalar _tmp136 = _tmp107 * _tmp72 + _tmp109 * _tmp67 + _tmp74 * (_tmp103 + _tmp17 + _tmp68);
+  const Scalar _tmp134 =
-  const Scalar _tmp137 = P(0, 5) * _tmp18 + P(10, 5) * _tmp23 + P(11, 5) * _tmp6 + P(9, 5) * _tmp15;
+      dt * (_tmp107 * _tmp72 + _tmp109 * _tmp67 + _tmp74 * (_tmp103 + _tmp17 + _tmp68));
-  const Scalar _tmp138 = _tmp136 * dt;
+  const Scalar _tmp135 = P(0, 5) * _tmp18 + P(10, 5) * _tmp23 + P(11, 5) * _tmp6 + P(9, 5) * _tmp15;
-  const Scalar _tmp139 =
+  const Scalar _tmp136 =
       P(1, 5) * _tmp18 + P(10, 5) * _tmp29 + P(11, 5) * _tmp34 + P(9, 5) * _tmp36;
-  const Scalar _tmp140 =
+  const Scalar _tmp137 =
       P(10, 5) * _tmp45 + P(11, 5) * _tmp44 + P(2, 5) * _tmp18 + P(9, 5) * _tmp47;
-  const Scalar _tmp141 = P(1, 5) * _tmp75 - P(12, 5) * _tmp57 - P(13, 5) * _tmp61 -
+  const Scalar _tmp138 = P(1, 5) * _tmp75 - P(12, 5) * _tmp57 - P(13, 5) * _tmp61 -
                          P(14, 5) * _tmp64 + P(2, 5) * _tmp79 + P(3, 5);
-  const Scalar _tmp142 = P(0, 5) * _tmp117 - P(12, 5) * _tmp110 - P(13, 5) * _tmp102 -
+  const Scalar _tmp139 = P(0, 5) * _tmp113 - P(12, 5) * _tmp110 - P(13, 5) * _tmp102 -
                          P(14, 5) * _tmp108 + P(2, 5) * _tmp105 + P(4, 5);
-  const Scalar _tmp143 = P(0, 14) * _tmp138 + P(1, 14) * _tmp133 - P(12, 14) * _tmp135 -
+  const Scalar _tmp140 = P(0, 14) * _tmp134 + P(1, 14) * _tmp131 - P(12, 14) * _tmp133 -
-                         P(13, 14) * _tmp134 - P(14, 14) * _tmp132 + P(5, 14);
+                         P(13, 14) * _tmp132 - P(14, 14) * _tmp130 + P(5, 14);
-  const Scalar _tmp144 = P(0, 13) * _tmp138 + P(1, 13) * _tmp133 - P(12, 13) * _tmp135 -
+  const Scalar _tmp141 = P(0, 13) * _tmp134 + P(1, 13) * _tmp131 - P(12, 13) * _tmp133 -
-                         P(13, 13) * _tmp134 - P(14, 13) * _tmp132 + P(5, 13);
+                         P(13, 13) * _tmp132 - P(14, 13) * _tmp130 + P(5, 13);
-  const Scalar _tmp145 = P(0, 12) * _tmp138 + P(1, 12) * _tmp133 - P(12, 12) * _tmp135 -
+  const Scalar _tmp142 = P(0, 12) * _tmp134 + P(1, 12) * _tmp131 - P(12, 12) * _tmp133 -
-                         P(13, 12) * _tmp134 - P(14, 12) * _tmp132 + P(5, 12);
+                         P(13, 12) * _tmp132 - P(14, 12) * _tmp130 + P(5, 12);
-  const Scalar _tmp146 = P(0, 5) * _tmp138 + P(1, 5) * _tmp133 - P(12, 5) * _tmp135 -
+  const Scalar _tmp143 = P(0, 5) * _tmp134 + P(1, 5) * _tmp131 - P(12, 5) * _tmp133 -
-                         P(13, 5) * _tmp134 - P(14, 5) * _tmp132 + P(5, 5);
+                         P(13, 5) * _tmp132 - P(14, 5) * _tmp130 + P(5, 5);
 
   // Output terms (1)
-  matrix::Matrix<Scalar, 23, 23> _res;
+  matrix::Matrix<Scalar, 24, 24> _res;
 
   _res.setZero();
 
@@ -264,40 +262,40 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                std::pow(_tmp63, Scalar(2)) * _tmp98 - _tmp64 * _tmp94 + _tmp75 * _tmp91 +
                _tmp79 * _tmp92 + _tmp99;
   _res(0, 4) = -_tmp102 * _tmp58 + _tmp105 * _tmp46 - _tmp108 * _tmp62 - _tmp110 * _tmp53 +
-               _tmp113 * _tmp114 + _tmp115;
+               _tmp113 * _tmp26 + _tmp114;
   _res(1, 4) = -_tmp102 * _tmp82 + _tmp105 * _tmp48 - _tmp108 * _tmp83 - _tmp110 * _tmp81 +
-               _tmp116 * _tmp117 + _tmp118;
+               _tmp113 * _tmp115 + _tmp116;
   _res(2, 4) = -_tmp102 * _tmp87 + _tmp105 * _tmp51 - _tmp108 * _tmp88 - _tmp110 * _tmp85 +
-               _tmp117 * _tmp119 + _tmp120;
+               _tmp113 * _tmp117 + _tmp118;
-  _res(3, 4) = _tmp101 * _tmp122 - _tmp102 * _tmp93 + _tmp105 * _tmp92 + _tmp107 * _tmp124 -
+  _res(3, 4) = _tmp101 * _tmp120 - _tmp102 * _tmp93 + _tmp105 * _tmp92 + _tmp107 * _tmp122 -
-               _tmp108 * _tmp94 + _tmp109 * _tmp121 - _tmp110 * _tmp90 + _tmp117 * _tmp123 +
+               _tmp108 * _tmp94 + _tmp109 * _tmp119 - _tmp110 * _tmp90 + _tmp113 * _tmp121 +
-               _tmp125;
+               _tmp123;
-  _res(4, 4) = std::pow(_tmp101, Scalar(2)) * _tmp97 - _tmp102 * _tmp127 +
+  _res(4, 4) = std::pow(_tmp101, Scalar(2)) * _tmp97 - _tmp102 * _tmp125 +
-               _tmp105 * (P(0, 2) * _tmp117 - P(12, 2) * _tmp110 - P(13, 2) * _tmp102 -
+               _tmp105 * (P(0, 2) * _tmp113 - P(12, 2) * _tmp110 - P(13, 2) * _tmp102 -
                           P(14, 2) * _tmp108 + P(2, 2) * _tmp105 + P(4, 2)) +
-               std::pow(_tmp107, Scalar(2)) * _tmp98 - _tmp108 * _tmp128 +
+               std::pow(_tmp107, Scalar(2)) * _tmp98 - _tmp108 * _tmp126 +
-               std::pow(_tmp109, Scalar(2)) * _tmp96 - _tmp110 * _tmp129 + _tmp117 * _tmp126 +
+               std::pow(_tmp109, Scalar(2)) * _tmp96 - _tmp110 * _tmp127 + _tmp113 * _tmp124 +
-               _tmp130;
+               _tmp128;
-  _res(0, 5) = _tmp114 * _tmp136 - _tmp132 * _tmp62 + _tmp133 * _tmp35 - _tmp134 * _tmp58 -
+  _res(0, 5) = -_tmp130 * _tmp62 + _tmp131 * _tmp35 - _tmp132 * _tmp58 - _tmp133 * _tmp53 +
-               _tmp135 * _tmp53 + _tmp137;
+               _tmp134 * _tmp26 + _tmp135;
-  _res(1, 5) = _tmp116 * _tmp138 - _tmp132 * _tmp83 + _tmp133 * _tmp43 - _tmp134 * _tmp82 -
+  _res(1, 5) = _tmp115 * _tmp134 - _tmp130 * _tmp83 + _tmp131 * _tmp43 - _tmp132 * _tmp82 -
-               _tmp135 * _tmp81 + _tmp139;
+               _tmp133 * _tmp81 + _tmp136;
-  _res(2, 5) = _tmp119 * _tmp138 - _tmp132 * _tmp88 + _tmp133 * _tmp86 - _tmp134 * _tmp87 -
+  _res(2, 5) = _tmp117 * _tmp134 - _tmp130 * _tmp88 + _tmp131 * _tmp86 - _tmp132 * _tmp87 -
-               _tmp135 * _tmp85 + _tmp140;
+               _tmp133 * _tmp85 + _tmp137;
-  _res(3, 5) = _tmp121 * _tmp66 + _tmp122 * _tmp73 + _tmp123 * _tmp138 + _tmp124 * _tmp131 -
+  _res(3, 5) = _tmp119 * _tmp66 + _tmp120 * _tmp73 + _tmp121 * _tmp134 + _tmp122 * _tmp129 -
-               _tmp132 * _tmp94 + _tmp133 * _tmp91 - _tmp134 * _tmp93 - _tmp135 * _tmp90 + _tmp141;
+               _tmp130 * _tmp94 + _tmp131 * _tmp91 - _tmp132 * _tmp93 - _tmp133 * _tmp90 + _tmp138;
-  _res(4, 5) = _tmp101 * _tmp73 * _tmp97 + _tmp107 * _tmp131 * _tmp98 + _tmp109 * _tmp66 * _tmp96 +
+  _res(4, 5) = _tmp101 * _tmp73 * _tmp97 + _tmp107 * _tmp129 * _tmp98 + _tmp109 * _tmp66 * _tmp96 +
-               _tmp126 * _tmp138 - _tmp127 * _tmp134 - _tmp128 * _tmp132 - _tmp129 * _tmp135 +
+               _tmp124 * _tmp134 - _tmp125 * _tmp132 - _tmp126 * _tmp130 - _tmp127 * _tmp133 +
-               _tmp133 * (P(0, 1) * _tmp117 - P(12, 1) * _tmp110 - P(13, 1) * _tmp102 -
+               _tmp131 * (P(0, 1) * _tmp113 - P(12, 1) * _tmp110 - P(13, 1) * _tmp102 -
                           P(14, 1) * _tmp108 + P(2, 1) * _tmp105 + P(4, 1)) +
-               _tmp142;
+               _tmp139;
-  _res(5, 5) = std::pow(_tmp131, Scalar(2)) * _tmp98 - _tmp132 * _tmp143 +
+  _res(5, 5) = std::pow(_tmp129, Scalar(2)) * _tmp98 - _tmp130 * _tmp140 +
-               _tmp133 * (P(0, 1) * _tmp138 + P(1, 1) * _tmp133 - P(12, 1) * _tmp135 -
+               _tmp131 * (P(0, 1) * _tmp134 + P(1, 1) * _tmp131 - P(12, 1) * _tmp133 -
-                          P(13, 1) * _tmp134 - P(14, 1) * _tmp132 + P(5, 1)) -
+                          P(13, 1) * _tmp132 - P(14, 1) * _tmp130 + P(5, 1)) -
-               _tmp134 * _tmp144 - _tmp135 * _tmp145 +
+               _tmp132 * _tmp141 - _tmp133 * _tmp142 +
-               _tmp138 * (P(0, 0) * _tmp138 + P(1, 0) * _tmp133 - P(12, 0) * _tmp135 -
+               _tmp134 * (P(0, 0) * _tmp134 + P(1, 0) * _tmp131 - P(12, 0) * _tmp133 -
-                          P(13, 0) * _tmp134 - P(14, 0) * _tmp132 + P(5, 0)) +
+                          P(13, 0) * _tmp132 - P(14, 0) * _tmp130 + P(5, 0)) +
-               _tmp146 + std::pow(_tmp66, Scalar(2)) * _tmp96 +
+               _tmp143 + std::pow(_tmp66, Scalar(2)) * _tmp96 +
                std::pow(_tmp73, Scalar(2)) * _tmp97;
   _res(0, 6) =
       P(0, 6) * _tmp18 + P(10, 6) * _tmp23 + P(11, 6) * _tmp6 + P(9, 6) * _tmp15 + _tmp80 * dt;
@@ -307,43 +305,43 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
       P(10, 6) * _tmp45 + P(11, 6) * _tmp44 + P(2, 6) * _tmp18 + P(9, 6) * _tmp47 + _tmp89 * dt;
   _res(3, 6) = P(1, 6) * _tmp75 - P(12, 6) * _tmp57 - P(13, 6) * _tmp61 - P(14, 6) * _tmp64 +
                P(2, 6) * _tmp79 + P(3, 6) + _tmp99 * dt;
-  _res(4, 6) = P(0, 6) * _tmp117 - P(12, 6) * _tmp110 - P(13, 6) * _tmp102 - P(14, 6) * _tmp108 +
+  _res(4, 6) = P(0, 6) * _tmp113 - P(12, 6) * _tmp110 - P(13, 6) * _tmp102 - P(14, 6) * _tmp108 +
                P(2, 6) * _tmp105 + P(4, 6) +
-               dt * (P(0, 3) * _tmp117 - P(12, 3) * _tmp110 - P(13, 3) * _tmp102 -
+               dt * (P(0, 3) * _tmp113 - P(12, 3) * _tmp110 - P(13, 3) * _tmp102 -
                      P(14, 3) * _tmp108 + P(2, 3) * _tmp105 + P(4, 3));
-  _res(5, 6) = P(0, 6) * _tmp138 + P(1, 6) * _tmp133 - P(12, 6) * _tmp135 - P(13, 6) * _tmp134 -
+  _res(5, 6) = P(0, 6) * _tmp134 + P(1, 6) * _tmp131 - P(12, 6) * _tmp133 - P(13, 6) * _tmp132 -
-               P(14, 6) * _tmp132 + P(5, 6) +
+               P(14, 6) * _tmp130 + P(5, 6) +
-               dt * (P(0, 3) * _tmp138 + P(1, 3) * _tmp133 - P(12, 3) * _tmp135 -
+               dt * (P(0, 3) * _tmp134 + P(1, 3) * _tmp131 - P(12, 3) * _tmp133 -
-                     P(13, 3) * _tmp134 - P(14, 3) * _tmp132 + P(5, 3));
+                     P(13, 3) * _tmp132 - P(14, 3) * _tmp130 + P(5, 3));
   _res(6, 6) = P(3, 6) * dt + P(6, 6) + dt * (P(3, 3) * dt + P(6, 3));
   _res(0, 7) =
-      P(0, 7) * _tmp18 + P(10, 7) * _tmp23 + P(11, 7) * _tmp6 + P(9, 7) * _tmp15 + _tmp115 * dt;
+      P(0, 7) * _tmp18 + P(10, 7) * _tmp23 + P(11, 7) * _tmp6 + P(9, 7) * _tmp15 + _tmp114 * dt;
   _res(1, 7) =
-      P(1, 7) * _tmp18 + P(10, 7) * _tmp29 + P(11, 7) * _tmp34 + P(9, 7) * _tmp36 + _tmp118 * dt;
+      P(1, 7) * _tmp18 + P(10, 7) * _tmp29 + P(11, 7) * _tmp34 + P(9, 7) * _tmp36 + _tmp116 * dt;
   _res(2, 7) =
-      P(10, 7) * _tmp45 + P(11, 7) * _tmp44 + P(2, 7) * _tmp18 + P(9, 7) * _tmp47 + _tmp120 * dt;
+      P(10, 7) * _tmp45 + P(11, 7) * _tmp44 + P(2, 7) * _tmp18 + P(9, 7) * _tmp47 + _tmp118 * dt;
   _res(3, 7) = P(1, 7) * _tmp75 - P(12, 7) * _tmp57 - P(13, 7) * _tmp61 - P(14, 7) * _tmp64 +
-               P(2, 7) * _tmp79 + P(3, 7) + _tmp125 * dt;
+               P(2, 7) * _tmp79 + P(3, 7) + _tmp123 * dt;
-  _res(4, 7) = P(0, 7) * _tmp117 - P(12, 7) * _tmp110 - P(13, 7) * _tmp102 - P(14, 7) * _tmp108 +
+  _res(4, 7) = P(0, 7) * _tmp113 - P(12, 7) * _tmp110 - P(13, 7) * _tmp102 - P(14, 7) * _tmp108 +
-               P(2, 7) * _tmp105 + P(4, 7) + _tmp130 * dt;
+               P(2, 7) * _tmp105 + P(4, 7) + _tmp128 * dt;
-  _res(5, 7) = P(0, 7) * _tmp138 + P(1, 7) * _tmp133 - P(12, 7) * _tmp135 - P(13, 7) * _tmp134 -
+  _res(5, 7) = P(0, 7) * _tmp134 + P(1, 7) * _tmp131 - P(12, 7) * _tmp133 - P(13, 7) * _tmp132 -
-               P(14, 7) * _tmp132 + P(5, 7) +
+               P(14, 7) * _tmp130 + P(5, 7) +
-               dt * (P(0, 4) * _tmp138 + P(1, 4) * _tmp133 - P(12, 4) * _tmp135 -
+               dt * (P(0, 4) * _tmp134 + P(1, 4) * _tmp131 - P(12, 4) * _tmp133 -
-                     P(13, 4) * _tmp134 - P(14, 4) * _tmp132 + P(5, 4));
+                     P(13, 4) * _tmp132 - P(14, 4) * _tmp130 + P(5, 4));
   _res(6, 7) = P(3, 7) * dt + P(6, 7) + dt * (P(3, 4) * dt + P(6, 4));
   _res(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4));
   _res(0, 8) =
-      P(0, 8) * _tmp18 + P(10, 8) * _tmp23 + P(11, 8) * _tmp6 + P(9, 8) * _tmp15 + _tmp137 * dt;
+      P(0, 8) * _tmp18 + P(10, 8) * _tmp23 + P(11, 8) * _tmp6 + P(9, 8) * _tmp15 + _tmp135 * dt;
   _res(1, 8) =
-      P(1, 8) * _tmp18 + P(10, 8) * _tmp29 + P(11, 8) * _tmp34 + P(9, 8) * _tmp36 + _tmp139 * dt;
+      P(1, 8) * _tmp18 + P(10, 8) * _tmp29 + P(11, 8) * _tmp34 + P(9, 8) * _tmp36 + _tmp136 * dt;
   _res(2, 8) =
-      P(10, 8) * _tmp45 + P(11, 8) * _tmp44 + P(2, 8) * _tmp18 + P(9, 8) * _tmp47 + _tmp140 * dt;
+      P(10, 8) * _tmp45 + P(11, 8) * _tmp44 + P(2, 8) * _tmp18 + P(9, 8) * _tmp47 + _tmp137 * dt;
   _res(3, 8) = P(1, 8) * _tmp75 - P(12, 8) * _tmp57 - P(13, 8) * _tmp61 - P(14, 8) * _tmp64 +
-               P(2, 8) * _tmp79 + P(3, 8) + _tmp141 * dt;
+               P(2, 8) * _tmp79 + P(3, 8) + _tmp138 * dt;
-  _res(4, 8) = P(0, 8) * _tmp117 - P(12, 8) * _tmp110 - P(13, 8) * _tmp102 - P(14, 8) * _tmp108 +
+  _res(4, 8) = P(0, 8) * _tmp113 - P(12, 8) * _tmp110 - P(13, 8) * _tmp102 - P(14, 8) * _tmp108 +
-               P(2, 8) * _tmp105 + P(4, 8) + _tmp142 * dt;
+               P(2, 8) * _tmp105 + P(4, 8) + _tmp139 * dt;
-  _res(5, 8) = P(0, 8) * _tmp138 + P(1, 8) * _tmp133 - P(12, 8) * _tmp135 - P(13, 8) * _tmp134 -
+  _res(5, 8) = P(0, 8) * _tmp134 + P(1, 8) * _tmp131 - P(12, 8) * _tmp133 - P(13, 8) * _tmp132 -
-               P(14, 8) * _tmp132 + P(5, 8) + _tmp146 * dt;
+               P(14, 8) * _tmp130 + P(5, 8) + _tmp143 * dt;
   _res(6, 8) = P(3, 8) * dt + P(6, 8) + dt * (P(3, 5) * dt + P(6, 5));
   _res(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5));
   _res(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5));
@@ -352,10 +350,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 9) = _tmp52;
   _res(3, 9) = P(1, 9) * _tmp75 - P(12, 9) * _tmp57 - P(13, 9) * _tmp61 - P(14, 9) * _tmp64 +
                P(2, 9) * _tmp79 + P(3, 9);
-  _res(4, 9) = P(0, 9) * _tmp117 - P(12, 9) * _tmp110 - P(13, 9) * _tmp102 - P(14, 9) * _tmp108 +
+  _res(4, 9) = P(0, 9) * _tmp113 - P(12, 9) * _tmp110 - P(13, 9) * _tmp102 - P(14, 9) * _tmp108 +
                P(2, 9) * _tmp105 + P(4, 9);
-  _res(5, 9) = P(0, 9) * _tmp138 + P(1, 9) * _tmp133 - P(12, 9) * _tmp135 - P(13, 9) * _tmp134 -
+  _res(5, 9) = P(0, 9) * _tmp134 + P(1, 9) * _tmp131 - P(12, 9) * _tmp133 - P(13, 9) * _tmp132 -
-               P(14, 9) * _tmp132 + P(5, 9);
+               P(14, 9) * _tmp130 + P(5, 9);
   _res(6, 9) = P(3, 9) * dt + P(6, 9);
   _res(7, 9) = P(4, 9) * dt + P(7, 9);
   _res(8, 9) = P(5, 9) * dt + P(8, 9);
@@ -365,10 +363,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 10) = _tmp49;
   _res(3, 10) = P(1, 10) * _tmp75 - P(12, 10) * _tmp57 - P(13, 10) * _tmp61 - P(14, 10) * _tmp64 +
                 P(2, 10) * _tmp79 + P(3, 10);
-  _res(4, 10) = P(0, 10) * _tmp117 - P(12, 10) * _tmp110 - P(13, 10) * _tmp102 -
+  _res(4, 10) = P(0, 10) * _tmp113 - P(12, 10) * _tmp110 - P(13, 10) * _tmp102 -
                 P(14, 10) * _tmp108 + P(2, 10) * _tmp105 + P(4, 10);
-  _res(5, 10) = P(0, 10) * _tmp138 + P(1, 10) * _tmp133 - P(12, 10) * _tmp135 -
+  _res(5, 10) = P(0, 10) * _tmp134 + P(1, 10) * _tmp131 - P(12, 10) * _tmp133 -
-                P(13, 10) * _tmp134 - P(14, 10) * _tmp132 + P(5, 10);
+                P(13, 10) * _tmp132 - P(14, 10) * _tmp130 + P(5, 10);
   _res(6, 10) = P(3, 10) * dt + P(6, 10);
   _res(7, 10) = P(4, 10) * dt + P(7, 10);
   _res(8, 10) = P(5, 10) * dt + P(8, 10);
@@ -379,10 +377,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 11) = _tmp50;
   _res(3, 11) = P(1, 11) * _tmp75 - P(12, 11) * _tmp57 - P(13, 11) * _tmp61 - P(14, 11) * _tmp64 +
                 P(2, 11) * _tmp79 + P(3, 11);
-  _res(4, 11) = P(0, 11) * _tmp117 - P(12, 11) * _tmp110 - P(13, 11) * _tmp102 -
+  _res(4, 11) = P(0, 11) * _tmp113 - P(12, 11) * _tmp110 - P(13, 11) * _tmp102 -
                 P(14, 11) * _tmp108 + P(2, 11) * _tmp105 + P(4, 11);
-  _res(5, 11) = P(0, 11) * _tmp138 + P(1, 11) * _tmp133 - P(12, 11) * _tmp135 -
+  _res(5, 11) = P(0, 11) * _tmp134 + P(1, 11) * _tmp131 - P(12, 11) * _tmp133 -
-                P(13, 11) * _tmp134 - P(14, 11) * _tmp132 + P(5, 11);
+                P(13, 11) * _tmp132 - P(14, 11) * _tmp130 + P(5, 11);
   _res(6, 11) = P(3, 11) * dt + P(6, 11);
   _res(7, 11) = P(4, 11) * dt + P(7, 11);
   _res(8, 11) = P(5, 11) * dt + P(8, 11);
@@ -393,8 +391,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(1, 12) = _tmp81;
   _res(2, 12) = _tmp85;
   _res(3, 12) = _tmp90;
-  _res(4, 12) = _tmp129;
+  _res(4, 12) = _tmp127;
-  _res(5, 12) = _tmp145;
+  _res(5, 12) = _tmp142;
   _res(6, 12) = P(3, 12) * dt + P(6, 12);
   _res(7, 12) = P(4, 12) * dt + P(7, 12);
   _res(8, 12) = P(5, 12) * dt + P(8, 12);
@@ -406,8 +404,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(1, 13) = _tmp82;
   _res(2, 13) = _tmp87;
   _res(3, 13) = _tmp93;
-  _res(4, 13) = _tmp127;
+  _res(4, 13) = _tmp125;
-  _res(5, 13) = _tmp144;
+  _res(5, 13) = _tmp141;
   _res(6, 13) = P(3, 13) * dt + P(6, 13);
   _res(7, 13) = P(4, 13) * dt + P(7, 13);
   _res(8, 13) = P(5, 13) * dt + P(8, 13);
@@ -420,8 +418,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(1, 14) = _tmp83;
   _res(2, 14) = _tmp88;
   _res(3, 14) = _tmp94;
-  _res(4, 14) = _tmp128;
+  _res(4, 14) = _tmp126;
-  _res(5, 14) = _tmp143;
+  _res(5, 14) = _tmp140;
   _res(6, 14) = P(3, 14) * dt + P(6, 14);
   _res(7, 14) = P(4, 14) * dt + P(7, 14);
   _res(8, 14) = P(5, 14) * dt + P(8, 14);
@@ -436,10 +434,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 15) = P(10, 15) * _tmp45 + P(11, 15) * _tmp44 + P(2, 15) * _tmp18 + P(9, 15) * _tmp47;
   _res(3, 15) = P(1, 15) * _tmp75 - P(12, 15) * _tmp57 - P(13, 15) * _tmp61 - P(14, 15) * _tmp64 +
                 P(2, 15) * _tmp79 + P(3, 15);
-  _res(4, 15) = P(0, 15) * _tmp117 - P(12, 15) * _tmp110 - P(13, 15) * _tmp102 -
+  _res(4, 15) = P(0, 15) * _tmp113 - P(12, 15) * _tmp110 - P(13, 15) * _tmp102 -
                 P(14, 15) * _tmp108 + P(2, 15) * _tmp105 + P(4, 15);
-  _res(5, 15) = P(0, 15) * _tmp138 + P(1, 15) * _tmp133 - P(12, 15) * _tmp135 -
+  _res(5, 15) = P(0, 15) * _tmp134 + P(1, 15) * _tmp131 - P(12, 15) * _tmp133 -
-                P(13, 15) * _tmp134 - P(14, 15) * _tmp132 + P(5, 15);
+                P(13, 15) * _tmp132 - P(14, 15) * _tmp130 + P(5, 15);
   _res(6, 15) = P(3, 15) * dt + P(6, 15);
   _res(7, 15) = P(4, 15) * dt + P(7, 15);
   _res(8, 15) = P(5, 15) * dt + P(8, 15);
@@ -455,10 +453,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 16) = P(10, 16) * _tmp45 + P(11, 16) * _tmp44 + P(2, 16) * _tmp18 + P(9, 16) * _tmp47;
   _res(3, 16) = P(1, 16) * _tmp75 - P(12, 16) * _tmp57 - P(13, 16) * _tmp61 - P(14, 16) * _tmp64 +
                 P(2, 16) * _tmp79 + P(3, 16);
-  _res(4, 16) = P(0, 16) * _tmp117 - P(12, 16) * _tmp110 - P(13, 16) * _tmp102 -
+  _res(4, 16) = P(0, 16) * _tmp113 - P(12, 16) * _tmp110 - P(13, 16) * _tmp102 -
                 P(14, 16) * _tmp108 + P(2, 16) * _tmp105 + P(4, 16);
-  _res(5, 16) = P(0, 16) * _tmp138 + P(1, 16) * _tmp133 - P(12, 16) * _tmp135 -
+  _res(5, 16) = P(0, 16) * _tmp134 + P(1, 16) * _tmp131 - P(12, 16) * _tmp133 -
-                P(13, 16) * _tmp134 - P(14, 16) * _tmp132 + P(5, 16);
+                P(13, 16) * _tmp132 - P(14, 16) * _tmp130 + P(5, 16);
   _res(6, 16) = P(3, 16) * dt + P(6, 16);
   _res(7, 16) = P(4, 16) * dt + P(7, 16);
   _res(8, 16) = P(5, 16) * dt + P(8, 16);
@@ -475,10 +473,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 17) = P(10, 17) * _tmp45 + P(11, 17) * _tmp44 + P(2, 17) * _tmp18 + P(9, 17) * _tmp47;
   _res(3, 17) = P(1, 17) * _tmp75 - P(12, 17) * _tmp57 - P(13, 17) * _tmp61 - P(14, 17) * _tmp64 +
                 P(2, 17) * _tmp79 + P(3, 17);
-  _res(4, 17) = P(0, 17) * _tmp117 - P(12, 17) * _tmp110 - P(13, 17) * _tmp102 -
+  _res(4, 17) = P(0, 17) * _tmp113 - P(12, 17) * _tmp110 - P(13, 17) * _tmp102 -
                 P(14, 17) * _tmp108 + P(2, 17) * _tmp105 + P(4, 17);
-  _res(5, 17) = P(0, 17) * _tmp138 + P(1, 17) * _tmp133 - P(12, 17) * _tmp135 -
+  _res(5, 17) = P(0, 17) * _tmp134 + P(1, 17) * _tmp131 - P(12, 17) * _tmp133 -
-                P(13, 17) * _tmp134 - P(14, 17) * _tmp132 + P(5, 17);
+                P(13, 17) * _tmp132 - P(14, 17) * _tmp130 + P(5, 17);
   _res(6, 17) = P(3, 17) * dt + P(6, 17);
   _res(7, 17) = P(4, 17) * dt + P(7, 17);
   _res(8, 17) = P(5, 17) * dt + P(8, 17);
@@ -496,10 +494,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 18) = P(10, 18) * _tmp45 + P(11, 18) * _tmp44 + P(2, 18) * _tmp18 + P(9, 18) * _tmp47;
   _res(3, 18) = P(1, 18) * _tmp75 - P(12, 18) * _tmp57 - P(13, 18) * _tmp61 - P(14, 18) * _tmp64 +
                 P(2, 18) * _tmp79 + P(3, 18);
-  _res(4, 18) = P(0, 18) * _tmp117 - P(12, 18) * _tmp110 - P(13, 18) * _tmp102 -
+  _res(4, 18) = P(0, 18) * _tmp113 - P(12, 18) * _tmp110 - P(13, 18) * _tmp102 -
                 P(14, 18) * _tmp108 + P(2, 18) * _tmp105 + P(4, 18);
-  _res(5, 18) = P(0, 18) * _tmp138 + P(1, 18) * _tmp133 - P(12, 18) * _tmp135 -
+  _res(5, 18) = P(0, 18) * _tmp134 + P(1, 18) * _tmp131 - P(12, 18) * _tmp133 -
-                P(13, 18) * _tmp134 - P(14, 18) * _tmp132 + P(5, 18);
+                P(13, 18) * _tmp132 - P(14, 18) * _tmp130 + P(5, 18);
   _res(6, 18) = P(3, 18) * dt + P(6, 18);
   _res(7, 18) = P(4, 18) * dt + P(7, 18);
   _res(8, 18) = P(5, 18) * dt + P(8, 18);
@@ -518,10 +516,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 19) = P(10, 19) * _tmp45 + P(11, 19) * _tmp44 + P(2, 19) * _tmp18 + P(9, 19) * _tmp47;
   _res(3, 19) = P(1, 19) * _tmp75 - P(12, 19) * _tmp57 - P(13, 19) * _tmp61 - P(14, 19) * _tmp64 +
                 P(2, 19) * _tmp79 + P(3, 19);
-  _res(4, 19) = P(0, 19) * _tmp117 - P(12, 19) * _tmp110 - P(13, 19) * _tmp102 -
+  _res(4, 19) = P(0, 19) * _tmp113 - P(12, 19) * _tmp110 - P(13, 19) * _tmp102 -
                 P(14, 19) * _tmp108 + P(2, 19) * _tmp105 + P(4, 19);
-  _res(5, 19) = P(0, 19) * _tmp138 + P(1, 19) * _tmp133 - P(12, 19) * _tmp135 -
+  _res(5, 19) = P(0, 19) * _tmp134 + P(1, 19) * _tmp131 - P(12, 19) * _tmp133 -
-                P(13, 19) * _tmp134 - P(14, 19) * _tmp132 + P(5, 19);
+                P(13, 19) * _tmp132 - P(14, 19) * _tmp130 + P(5, 19);
   _res(6, 19) = P(3, 19) * dt + P(6, 19);
   _res(7, 19) = P(4, 19) * dt + P(7, 19);
   _res(8, 19) = P(5, 19) * dt + P(8, 19);
@@ -541,10 +539,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 20) = P(10, 20) * _tmp45 + P(11, 20) * _tmp44 + P(2, 20) * _tmp18 + P(9, 20) * _tmp47;
   _res(3, 20) = P(1, 20) * _tmp75 - P(12, 20) * _tmp57 - P(13, 20) * _tmp61 - P(14, 20) * _tmp64 +
                 P(2, 20) * _tmp79 + P(3, 20);
-  _res(4, 20) = P(0, 20) * _tmp117 - P(12, 20) * _tmp110 - P(13, 20) * _tmp102 -
+  _res(4, 20) = P(0, 20) * _tmp113 - P(12, 20) * _tmp110 - P(13, 20) * _tmp102 -
                 P(14, 20) * _tmp108 + P(2, 20) * _tmp105 + P(4, 20);
-  _res(5, 20) = P(0, 20) * _tmp138 + P(1, 20) * _tmp133 - P(12, 20) * _tmp135 -
+  _res(5, 20) = P(0, 20) * _tmp134 + P(1, 20) * _tmp131 - P(12, 20) * _tmp133 -
-                P(13, 20) * _tmp134 - P(14, 20) * _tmp132 + P(5, 20);
+                P(13, 20) * _tmp132 - P(14, 20) * _tmp130 + P(5, 20);
   _res(6, 20) = P(3, 20) * dt + P(6, 20);
   _res(7, 20) = P(4, 20) * dt + P(7, 20);
   _res(8, 20) = P(5, 20) * dt + P(8, 20);
@@ -565,10 +563,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 21) = P(10, 21) * _tmp45 + P(11, 21) * _tmp44 + P(2, 21) * _tmp18 + P(9, 21) * _tmp47;
   _res(3, 21) = P(1, 21) * _tmp75 - P(12, 21) * _tmp57 - P(13, 21) * _tmp61 - P(14, 21) * _tmp64 +
                 P(2, 21) * _tmp79 + P(3, 21);
-  _res(4, 21) = P(0, 21) * _tmp117 - P(12, 21) * _tmp110 - P(13, 21) * _tmp102 -
+  _res(4, 21) = P(0, 21) * _tmp113 - P(12, 21) * _tmp110 - P(13, 21) * _tmp102 -
                 P(14, 21) * _tmp108 + P(2, 21) * _tmp105 + P(4, 21);
-  _res(5, 21) = P(0, 21) * _tmp138 + P(1, 21) * _tmp133 - P(12, 21) * _tmp135 -
+  _res(5, 21) = P(0, 21) * _tmp134 + P(1, 21) * _tmp131 - P(12, 21) * _tmp133 -
-                P(13, 21) * _tmp134 - P(14, 21) * _tmp132 + P(5, 21);
+                P(13, 21) * _tmp132 - P(14, 21) * _tmp130 + P(5, 21);
   _res(6, 21) = P(3, 21) * dt + P(6, 21);
   _res(7, 21) = P(4, 21) * dt + P(7, 21);
   _res(8, 21) = P(5, 21) * dt + P(8, 21);
@@ -590,10 +588,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(2, 22) = P(10, 22) * _tmp45 + P(11, 22) * _tmp44 + P(2, 22) * _tmp18 + P(9, 22) * _tmp47;
   _res(3, 22) = P(1, 22) * _tmp75 - P(12, 22) * _tmp57 - P(13, 22) * _tmp61 - P(14, 22) * _tmp64 +
                 P(2, 22) * _tmp79 + P(3, 22);
-  _res(4, 22) = P(0, 22) * _tmp117 - P(12, 22) * _tmp110 - P(13, 22) * _tmp102 -
+  _res(4, 22) = P(0, 22) * _tmp113 - P(12, 22) * _tmp110 - P(13, 22) * _tmp102 -
                 P(14, 22) * _tmp108 + P(2, 22) * _tmp105 + P(4, 22);
-  _res(5, 22) = P(0, 22) * _tmp138 + P(1, 22) * _tmp133 - P(12, 22) * _tmp135 -
+  _res(5, 22) = P(0, 22) * _tmp134 + P(1, 22) * _tmp131 - P(12, 22) * _tmp133 -
-                P(13, 22) * _tmp134 - P(14, 22) * _tmp132 + P(5, 22);
+                P(13, 22) * _tmp132 - P(14, 22) * _tmp130 + P(5, 22);
   _res(6, 22) = P(3, 22) * dt + P(6, 22);
   _res(7, 22) = P(4, 22) * dt + P(7, 22);
   _res(8, 22) = P(5, 22) * dt + P(8, 22);
@@ -611,6 +609,33 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 22) = P(20, 22);
   _res(21, 22) = P(21, 22);
   _res(22, 22) = P(22, 22);
+  _res(0, 23) = P(0, 23) * _tmp18 + P(10, 23) * _tmp23 + P(11, 23) * _tmp6 + P(9, 23) * _tmp15;
+  _res(1, 23) = P(1, 23) * _tmp18 + P(10, 23) * _tmp29 + P(11, 23) * _tmp34 + P(9, 23) * _tmp36;
+  _res(2, 23) = P(10, 23) * _tmp45 + P(11, 23) * _tmp44 + P(2, 23) * _tmp18 + P(9, 23) * _tmp47;
+  _res(3, 23) = P(1, 23) * _tmp75 - P(12, 23) * _tmp57 - P(13, 23) * _tmp61 - P(14, 23) * _tmp64 +
+                P(2, 23) * _tmp79 + P(3, 23);
+  _res(4, 23) = P(0, 23) * _tmp113 - P(12, 23) * _tmp110 - P(13, 23) * _tmp102 -
+                P(14, 23) * _tmp108 + P(2, 23) * _tmp105 + P(4, 23);
+  _res(5, 23) = P(0, 23) * _tmp134 + P(1, 23) * _tmp131 - P(12, 23) * _tmp133 -
+                P(13, 23) * _tmp132 - P(14, 23) * _tmp130 + P(5, 23);
+  _res(6, 23) = P(3, 23) * dt + P(6, 23);
+  _res(7, 23) = P(4, 23) * dt + P(7, 23);
+  _res(8, 23) = P(5, 23) * dt + P(8, 23);
+  _res(9, 23) = P(9, 23);
+  _res(10, 23) = P(10, 23);
+  _res(11, 23) = P(11, 23);
+  _res(12, 23) = P(12, 23);
+  _res(13, 23) = P(13, 23);
+  _res(14, 23) = P(14, 23);
+  _res(15, 23) = P(15, 23);
+  _res(16, 23) = P(16, 23);
+  _res(17, 23) = P(17, 23);
+  _res(18, 23) = P(18, 23);
+  _res(19, 23) = P(19, 23);
+  _res(20, 23) = P(20, 23);
+  _res(21, 23) = P(21, 23);
+  _res(22, 23) = P(22, 23);
+  _res(23, 23) = P(23, 23);
 
   return _res;
 }  // NOLINT(readability/fn_size)

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

@@ -18,9 +18,10 @@ struct StateSample {
 	matrix::Vector3<float> mag_I{};
 	matrix::Vector3<float> mag_B{};
 	matrix::Vector2<float> wind_vel{};
+	float terrain{};
 
-	matrix::Vector<float, 24> Data() const {
+	matrix::Vector<float, 25> Data() const {
-		matrix::Vector<float, 24> state;
+		matrix::Vector<float, 25> state;
 		state.slice<4, 1>(0, 0) = quat_nominal;
 		state.slice<3, 1>(4, 0) = vel;
 		state.slice<3, 1>(7, 0) = pos;
@@ -29,15 +30,16 @@ struct StateSample {
 		state.slice<3, 1>(16, 0) = mag_I;
 		state.slice<3, 1>(19, 0) = mag_B;
 		state.slice<2, 1>(22, 0) = wind_vel;
+		state.slice<1, 1>(24, 0) = terrain;
 		return state;
 	};
 
-	const matrix::Vector<float, 24>& vector() const {
+	const matrix::Vector<float, 25>& vector() const {
-		return *reinterpret_cast<matrix::Vector<float, 24>*>(const_cast<float*>(reinterpret_cast<const float*>(&quat_nominal)));
+		return *reinterpret_cast<matrix::Vector<float, 25>*>(const_cast<float*>(reinterpret_cast<const float*>(&quat_nominal)));
 	};
 
 };
-static_assert(sizeof(matrix::Vector<float, 24>) == sizeof(StateSample), "state vector doesn't match StateSample size");
+static_assert(sizeof(matrix::Vector<float, 25>) == sizeof(StateSample), "state vector doesn't match StateSample size");
 
 struct IdxDof { unsigned idx; unsigned dof; };
 namespace State {
@@ -49,7 +51,8 @@ namespace State {
 	static constexpr IdxDof mag_I{15, 3};
 	static constexpr IdxDof mag_B{18, 3};
 	static constexpr IdxDof wind_vel{21, 2};
-	static constexpr uint8_t size{23};
+	static constexpr IdxDof terrain{23, 1};
+	static constexpr uint8_t size{24};
 };
 }
 #endif // !EKF_STATE_H

src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp

@@ -38,26 +38,17 @@
  */
 
 #include "ekf.h"
-#include "terrain_estimator/derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h"
+#include "ekf_derivation/generated/state.h"
-#include "terrain_estimator/derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h"
 
 #include <mathlib/mathlib.h>
 
 void Ekf::initHagl()
 {
 	// assume a ground clearance
-	_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
+	_state.terrain = _state.pos(2) + _params.rng_gnd_clearance;
 
 	// use the ground clearance value as our uncertainty
-	_terrain_var = sq(_params.rng_gnd_clearance);
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.rng_gnd_clearance));
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us;
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us;
-#endif // CONFIG_EKF2_OPTICAL_FLOW
 }
 
 void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
@@ -74,333 +65,9 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
 		initHagl();
 	}
 
-	predictHagl(imu_delayed);
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	controlHaglRngFusion();
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	controlHaglFlowFusion();
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
 	controlHaglFakeFusion();
-
-	// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
-	if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
-		_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
-	}
 }
 
-void Ekf::predictHagl(const imuSample &imu_delayed)
-{
-	// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
-
-	// process noise due to errors in vehicle height estimate
-	_terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
-
-	// process noise due to terrain gradient
-	_terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient)
-			* (sq(_state.vel(0)) + sq(_state.vel(1)));
-
-	// limit the variance to prevent it becoming badly conditioned
-	_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
-}
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-void Ekf::controlHaglRngFusion()
-{
-	if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
-	    || _control_status.flags.rng_fault) {
-
-		stopHaglRngFusion();
-		return;
-	}
-
-	if (_range_sensor.isDataReady()) {
-		updateHaglRng(_aid_src_terrain_range_finder);
-	}
-
-	if (_range_sensor.isDataHealthy()) {
-
-		const bool continuing_conditions_passing = _rng_consistency_check.isKinematicallyConsistent();
-				//&& !_control_status.flags.rng_hgt // TODO: should not be fused when using range height
-
-		const bool starting_conditions_passing = continuing_conditions_passing
-				&& _range_sensor.isRegularlySendingData()
-				&& (_rng_consistency_check.getTestRatio() < 1.f);
-
-		_time_last_healthy_rng_data = _time_delayed_us;
-
-		if (_hagl_sensor_status.flags.range_finder) {
-			if (continuing_conditions_passing) {
-				fuseHaglRng(_aid_src_terrain_range_finder);
-
-				// We have been rejecting range data for too long
-				const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
-				const bool is_fusion_failing = isTimedOut(_aid_src_terrain_range_finder.time_last_fuse, timeout);
-
-				if (is_fusion_failing) {
-					if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) {
-						// Data seems good, attempt a reset
-						resetHaglRng();
-
-					} else if (starting_conditions_passing) {
-						// The sensor can probably not detect the ground properly
-						// declare the sensor faulty and stop the fusion
-						_control_status.flags.rng_fault = true;
-						_range_sensor.setFaulty(true);
-						stopHaglRngFusion();
-
-					} else {
-						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-						stopHaglRngFusion();
-					}
-				}
-
-			} else {
-				stopHaglRngFusion();
-			}
-
-		} else {
-			if (starting_conditions_passing) {
-				_hagl_sensor_status.flags.range_finder = true;
-
-				// Reset the state to the measurement only if the test ratio is large,
-				// otherwise let it converge through the fusion
-				if (_hagl_sensor_status.flags.flow && (_aid_src_terrain_range_finder.test_ratio < 0.2f)) {
-					fuseHaglRng(_aid_src_terrain_range_finder);
-
-				} else {
-					resetHaglRng();
-				}
-			}
-		}
-
-	} else if (_hagl_sensor_status.flags.range_finder && isTimedOut(_time_last_healthy_rng_data, _params.reset_timeout_max)) {
-		// No data anymore. Stop until it comes back.
-		stopHaglRngFusion();
-	}
-}
-
-float Ekf::getRngVar() const
-{
-	return fmaxf(P(State::pos.idx + 2, State::pos.idx + 2) * _params.vehicle_variance_scaler, 0.0f)
-	       + sq(_params.range_noise)
-	       + sq(_params.range_noise_scaler * _range_sensor.getRange());
-}
-
-void Ekf::resetHaglRng()
-{
-	_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
-	_terrain_var = getRngVar();
-	_terrain_vpos_reset_counter++;
-
-	_aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us;
-}
-
-void Ekf::stopHaglRngFusion()
-{
-	if (_hagl_sensor_status.flags.range_finder) {
-		ECL_INFO("stopping HAGL range fusion");
-
-		// reset flags
-		_innov_check_fail_status.flags.reject_hagl = false;
-
-		_hagl_sensor_status.flags.range_finder = false;
-	}
-}
-
-void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const
-{
-	// get a height above ground measurement from the range finder assuming a flat earth
-	const float meas_hagl = _range_sensor.getDistBottom();
-
-	// predict the hagl from the vehicle position and terrain height
-	const float pred_hagl = _terrain_vpos - _state.pos(2);
-
-	// calculate the observation variance adding the variance of the vehicles own height uncertainty
-	const float obs_variance = getRngVar();
-
-	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
-	const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
-
-	updateAidSourceStatus(aid_src,
-		_time_delayed_us,                          // sample timestamp
-		meas_hagl,                                 // observation
-		obs_variance,                              // observation variance
-		pred_hagl - meas_hagl,                     // innovation
-		hagl_innov_var,                            // innovation variance
-		math::max(_params.range_innov_gate, 1.f)); // innovation gate
-}
-
-void Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src)
-{
-	if (!aid_src.innovation_rejected) {
-		// calculate the Kalman gain
-		const float gain = _terrain_var / aid_src.innovation_variance;
-
-		// correct the state
-		_terrain_vpos -= gain * aid_src.innovation;
-
-		// correct the variance
-		_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
-
-		// record last successful fusion event
-		_innov_check_fail_status.flags.reject_hagl = false;
-
-		aid_src.time_last_fuse = _time_delayed_us;
-		aid_src.fused = true;
-
-	} else {
-		_innov_check_fail_status.flags.reject_hagl = true;
-		aid_src.fused = false;
-	}
-}
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-void Ekf::controlHaglFlowFusion()
-{
-	if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) {
-		stopHaglFlowFusion();
-		return;
-	}
-
-	if (_flow_data_ready) {
-		updateOptFlow(_aid_src_terrain_optical_flow);
-
-		const bool continuing_conditions_passing = _control_status.flags.in_air
-				&& !_control_status.flags.opt_flow
-				&& _control_status.flags.gps
-				&& !_hagl_sensor_status.flags.range_finder;
-
-		const bool starting_conditions_passing = continuing_conditions_passing;
-
-		if (_hagl_sensor_status.flags.flow) {
-			if (continuing_conditions_passing) {
-
-				// TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon
-				fuseFlowForTerrain(_aid_src_terrain_optical_flow);
-
-				// TODO: do something when failing continuously the innovation check
-				/* const bool is_fusion_failing = isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); */
-
-				/* if (is_fusion_failing) { */
-				/* 	resetHaglFlow(); */
-				/* } */
-
-			} else {
-				stopHaglFlowFusion();
-			}
-
-		} else {
-			if (starting_conditions_passing) {
-				_hagl_sensor_status.flags.flow = true;
-				// TODO: do a reset instead of trying to fuse the data?
-				fuseFlowForTerrain(_aid_src_terrain_optical_flow);
-			}
-		}
-
-		_flow_data_ready = false;
-
-	} else if (_hagl_sensor_status.flags.flow && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) {
-		// No data anymore. Stop until it comes back.
-		stopHaglFlowFusion();
-	}
-}
-
-void Ekf::stopHaglFlowFusion()
-{
-	if (_hagl_sensor_status.flags.flow) {
-		ECL_INFO("stopping HAGL flow fusion");
-
-		_hagl_sensor_status.flags.flow = false;
-	}
-}
-
-void Ekf::resetHaglFlow()
-{
-	// TODO: use the flow data
-	_terrain_vpos = fmaxf(0.0f, _state.pos(2));
-	_terrain_var = 100.0f;
-	_terrain_vpos_reset_counter++;
-
-	_aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us;
-}
-
-void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
-{
-	flow.fused = true;
-
-	const float R_LOS = flow.observation_variance[0];
-
-	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
-	// a positive offset in earth frame leads to a smaller height above the ground.
-	float range = predictFlowRange();
-
-	const float state = _terrain_vpos; // linearize both axes using the same state value
-	Vector2f innov_var;
-	float H;
-	sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H);
-	innov_var.copyTo(flow.innovation_variance);
-
-	if ((flow.innovation_variance[0] < R_LOS)
-	    || (flow.innovation_variance[1] < R_LOS)) {
-		// we need to reinitialise the covariance matrix and abort this fusion step
-		ECL_ERR("Opt flow error - covariance reset");
-		_terrain_var = 100.0f;
-		return;
-	}
-
-	_innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f);
-	_innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f);
-
-	// if either axis fails we abort the fusion
-	if (flow.innovation_rejected) {
-		return;
-	}
-
-	// fuse observation axes sequentially
-	for (uint8_t index = 0; index <= 1; index++) {
-		if (index == 0) {
-			// everything was already computed above
-
-		} else if (index == 1) {
-			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
-			sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &flow.innovation_variance[1], &H);
-
-			// recalculate the innovation using the updated state
-			const Vector2f vel_body = predictFlowVelBody();
-			range = predictFlowRange();
-			flow.innovation[1] = (-vel_body(0) / range) - flow.observation[1];
-
-			if (flow.innovation_variance[1] < R_LOS) {
-				// we need to reinitialise the covariance matrix and abort this fusion step
-				ECL_ERR("Opt flow error - covariance reset");
-				_terrain_var = 100.0f;
-				return;
-			}
-		}
-
-		float Kfusion = _terrain_var * H / flow.innovation_variance[index];
-
-		_terrain_vpos += Kfusion * flow.innovation[0];
-		// constrain terrain to minimum allowed value and predict height above ground
-		_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
-
-		// guard against negative variance
-		_terrain_var = fmaxf(_terrain_var - Kfusion * H * _terrain_var, sq(0.01f));
-	}
-
-	_fault_status.flags.bad_optflow_X = false;
-	_fault_status.flags.bad_optflow_Y = false;
-
-	flow.time_last_fuse = _time_delayed_us;
-	flow.fused = true;
-}
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
 void Ekf::controlHaglFakeFusion()
 {
 	if (!_control_status.flags.in_air
@@ -410,11 +77,11 @@ void Ekf::controlHaglFakeFusion()
 		bool recent_terrain_aiding = false;
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-		recent_terrain_aiding |= isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)1e6);
+		recent_terrain_aiding |= isRecent(_aid_src_rng_hgt.time_last_fuse, (uint64_t)1e6);
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-		recent_terrain_aiding |= isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)1e6);
+		recent_terrain_aiding |= isRecent(_aid_src_optical_flow.time_last_fuse, (uint64_t)1e6);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 		if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) {
@@ -425,24 +92,20 @@ void Ekf::controlHaglFakeFusion()
 
 bool Ekf::isTerrainEstimateValid() const
 {
-	bool valid = false;
+	// Assume being valid when the uncertainty is small compared to the height above ground
+	bool valid = getTerrainVariance() < sq(0.1f * getHagl());
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 
-	// we have been fusing range finder measurements in the last 5 seconds
+	if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6)) {
-	if (isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) {
+		valid = true;
-		if (_hagl_sensor_status.flags.range_finder || !_control_status.flags.in_air) {
-			valid = true;
-		}
 	}
 
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 
-	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
+	if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_optical_flow.time_last_fuse, (uint64_t)5e6)) {
-	// this can only be the case if the main filter does not fuse optical flow
-	if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)5e6)) {
 		valid = true;
 	}
 
@@ -450,7 +113,3 @@ bool Ekf::isTerrainEstimateValid() const
 
 	return valid;
 }
-
-void Ekf::terrainHandleVerticalPositionReset(const float delta_z) {
-	_terrain_vpos += delta_z;
-}

src/modules/ekf2/EKF2.cpp

@@ -326,7 +326,6 @@ bool EKF2::multi_init(int imu, int mag)
 	// RNG advertise
 	if (_param_ekf2_rng_ctrl.get()) {
 		_estimator_aid_src_rng_hgt_pub.advertise();
-		_estimator_rng_hgt_bias_pub.advertise();
 	}
 
 #endif // CONFIG_EKF2_RANGE_FINDER
@@ -728,10 +727,6 @@ void EKF2::Run()
 			PublishBaroBias(now);
 #endif // CONFIG_EKF2_BAROMETER
 
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-			PublishRngHgtBias(now);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 			PublishEvPosBias(now);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -928,21 +923,6 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	// optical flow
 	PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#if defined(CONFIG_EKF2_TERRAIN)
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	// range finder
-	PublishAidSourceStatus(_ekf.aid_src_terrain_range_finder(), _status_terrain_range_finder_pub_last,
-			       _estimator_aid_src_terrain_range_finder_pub);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	// optical flow
-	PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last,
-			       _estimator_aid_src_terrain_optical_flow_pub);
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#endif // CONFIG_EKF2_TERRAIN
 }
 
 void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@@ -996,21 +976,6 @@ void EKF2::PublishGnssHgtBias(const hrt_abstime &timestamp)
 }
 #endif // CONFIG_EKF2_GNSS
 
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-void EKF2::PublishRngHgtBias(const hrt_abstime &timestamp)
-{
-	if (_ekf.get_rng_sample_delayed().time_us != 0) {
-		const BiasEstimator::status &status = _ekf.getRngHgtBiasEstimatorStatus();
-
-		if (fabsf(status.bias - _last_rng_hgt_bias_published) > 0.001f) {
-			_estimator_rng_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_rng_sample_delayed().time_us, timestamp));
-
-			_last_rng_hgt_bias_published = status.bias;
-		}
-	}
-}
-#endif // CONFIG_EKF2_RANGE_FINDER
-
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 void EKF2::PublishEvPosBias(const hrt_abstime &timestamp)
 {
@@ -1272,10 +1237,6 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	// Optical flow
 	innovations.flow[0] = _ekf.aid_src_optical_flow().innovation[0];
 	innovations.flow[1] = _ekf.aid_src_optical_flow().innovation[1];
-# if defined(CONFIG_EKF2_TERRAIN)
-	innovations.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation[0];
-	innovations.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation[1];
-# endif // CONFIG_EKF2_TERRAIN
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	// heading
@@ -1313,7 +1274,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 
 #if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
 	// hagl
-	innovations.hagl = _ekf.aid_src_terrain_range_finder().innovation;
+	innovations.hagl = _ekf.aid_src_rng_hgt().innovation;
 #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -1339,7 +1300,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 
 #if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_OPTICAL_FLOW)
 		// set dist bottom to scale flow innovation
-		const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2);
+		const float dist_bottom = _ekf.getHagl();
 		_preflt_checker.setDistBottom(dist_bottom);
 #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_OPTICAL_FLOW
 
@@ -1406,10 +1367,6 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	// Optical flow
 	test_ratios.flow[0] = _ekf.aid_src_optical_flow().test_ratio[0];
 	test_ratios.flow[1] = _ekf.aid_src_optical_flow().test_ratio[1];
-# if defined(CONFIG_EKF2_TERRAIN)
-	test_ratios.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().test_ratio[0];
-	test_ratios.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().test_ratio[1];
-# endif // CONFIG_EKF2_TERRAIN
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	// heading
@@ -1447,7 +1404,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 
 #if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
 	// hagl
-	test_ratios.hagl = _ekf.aid_src_terrain_range_finder().test_ratio;
+	test_ratios.hagl = _ekf.aid_src_rng_hgt().test_ratio;
 #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -1503,10 +1460,6 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	// Optical flow
 	variances.flow[0] = _ekf.aid_src_optical_flow().innovation_variance[0];
 	variances.flow[1] = _ekf.aid_src_optical_flow().innovation_variance[1];
-# if defined(CONFIG_EKF2_TERRAIN)
-	variances.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation_variance[0];
-	variances.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation_variance[1];
-# endif // CONFIG_EKF2_TERRAIN
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	// heading
@@ -1544,7 +1497,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 
 #if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
 	// hagl
-	variances.hagl = _ekf.aid_src_terrain_range_finder().innovation_variance;
+	variances.hagl = _ekf.aid_src_rng_hgt().innovation_variance;
 #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -1620,7 +1573,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive
-	lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f);
+	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 	lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
 #endif // CONFIG_EKF2_TERRAIN

src/modules/ekf2/EKF2.hpp

@@ -337,19 +337,6 @@ private:
 	hrt_abstime _status_aux_vel_pub_last{0};
 #endif // CONFIG_EKF2_AUXVEL
 
-#if defined(CONFIG_EKF2_TERRAIN)
-
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_terrain_range_finder_pub {ORB_ID(estimator_aid_src_terrain_range_finder)};
-	hrt_abstime _status_terrain_range_finder_pub_last{0};
-# endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_terrain_optical_flow_pub {ORB_ID(estimator_aid_src_terrain_optical_flow)};
-	hrt_abstime _status_terrain_optical_flow_pub_last{0};
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-#endif // CONFIG_EKF2_TERRAIN
-
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)};
 	uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
@@ -407,9 +394,7 @@ private:
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	hrt_abstime _status_rng_hgt_pub_last {0};
-	float _last_rng_hgt_bias_published{};
 
-	uORB::PublicationMulti<estimator_bias_s> _estimator_rng_hgt_bias_pub {ORB_ID(estimator_rng_hgt_bias)};
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
 
 	uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};

src/modules/ekf2/test/CMakeLists.txt

@@ -41,6 +41,7 @@ px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
+px4_add_unit_gtest(SRC test_EKF_flow_generated.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -240,16 +240,6 @@ bool EkfWrapper::isWindVelocityEstimated() const
 	return _ekf->control_status_flags().wind;
 }
 
-void EkfWrapper::enableTerrainRngFusion()
-{
-	_ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseRangeFinder;
-}
-
-void EkfWrapper::disableTerrainRngFusion()
-{
-	_ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseRangeFinder;
-}
-
 bool EkfWrapper::isIntendingTerrainRngFusion() const
 {
 	terrain_fusion_status_u terrain_status;
@@ -257,16 +247,6 @@ bool EkfWrapper::isIntendingTerrainRngFusion() const
 	return terrain_status.flags.range_finder;
 }
 
-void EkfWrapper::enableTerrainFlowFusion()
-{
-	_ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseOpticalFlow;
-}
-
-void EkfWrapper::disableTerrainFlowFusion()
-{
-	_ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseOpticalFlow;
-}
-
 bool EkfWrapper::isIntendingTerrainFlowFusion() const
 {
 	terrain_fusion_status_u terrain_status;

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -110,12 +110,8 @@ public:
 
 	bool isWindVelocityEstimated() const;
 
-	void enableTerrainRngFusion();
-	void disableTerrainRngFusion();
 	bool isIntendingTerrainRngFusion() const;
 
-	void enableTerrainFlowFusion();
-	void disableTerrainFlowFusion();
 	bool isIntendingTerrainFlowFusion() const;
 
 	Eulerf getEulerAngles() const;

src/modules/ekf2/test/test_EKF_flow.cpp

@@ -120,7 +120,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
 
 	_sensor_simulator.runSeconds(5.f);
 
-	const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
+	const float estimated_distance_to_ground = _ekf->getHagl();
 	EXPECT_FLOAT_EQ(estimated_distance_to_ground, simulated_distance_to_ground);
 
 	reset_logging_checker.capturePreResetState();
@@ -133,10 +133,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
 
-	// Let it reset but not fuse more measurements. We actually need to send 2
+	_sensor_simulator.runTrajectorySeconds(1);
-	// samples to get a reset because the first one cannot be used as the gyro
-	// compensation needs to be accumulated between two samples.
-	_sensor_simulator.runTrajectorySeconds(0.14);
 
 	// THEN: estimated velocity should match simulated velocity
 	const Vector3f estimated_velocity = _ekf->getVelocity();
@@ -144,7 +141,11 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
 	simulated_velocity.print();
 	EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity))
 			<< "estimated vel = " << estimated_velocity(0) << ", "
-			<< estimated_velocity(1);
+			<< estimated_velocity(1) << "\n"
+			<< "simulated vel = " << simulated_velocity(0) << ", "
+			<< simulated_velocity(1);
+
+	EXPECT_NEAR(simulated_distance_to_ground, _ekf->getHagl(), 0.1f);
 
 	// AND: the reset in velocity should be saved correctly
 	reset_logging_checker.capturePostResetState();
@@ -158,7 +159,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
 	ResetLoggingChecker reset_logging_checker(_ekf);
 
 	// WHEN: being on ground
-	const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
+	const float estimated_distance_to_ground = _ekf->getHagl();
 	EXPECT_LT(estimated_distance_to_ground, 0.3f);
 
 	reset_logging_checker.capturePreResetState();
@@ -177,11 +178,10 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
 	EXPECT_TRUE(isEqual(estimated_horz_velocity, Vector2f(0.f, 0.f)))
 			<< estimated_horz_velocity(0) << ", " << estimated_horz_velocity(1);
 
-	// AND: the reset in velocity should be saved correctly
+	// AND: the velocity isn't reset as it is already correct
 	reset_logging_checker.capturePostResetState();
-	EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
+	EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(0));
 	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));
-	EXPECT_TRUE(reset_logging_checker.isVelocityDeltaLoggedCorrectly(1e-9f));
 }
 
 TEST_F(EkfFlowTest, inAirConvergence)
@@ -222,9 +222,9 @@ TEST_F(EkfFlowTest, inAirConvergence)
 	// THEN: estimated velocity should converge to the simulated velocity
 	// This takes a bit of time because the data is inconsistent with IMU measurements
 	estimated_velocity = _ekf->getVelocity();
-	EXPECT_NEAR(estimated_velocity(0), simulated_velocity(0), 0.05f)
+	EXPECT_NEAR(estimated_velocity(0), simulated_velocity(0), 0.01f)
 			<< "estimated vel = " << estimated_velocity(0);
-	EXPECT_NEAR(estimated_velocity(1), simulated_velocity(1), 0.05f)
+	EXPECT_NEAR(estimated_velocity(1), simulated_velocity(1), 0.01f)
 			<< estimated_velocity(1);
 }
 

src/modules/ekf2/test/test_EKF_flow_generated.cpp

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <cfloat>
+#include <gtest/gtest.h>
+#include "EKF/ekf.h"
+#include "test_helper/comparison_helper.h"
+
+#include "../EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h"
+#include "../EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h"
+
+using namespace matrix;
+
+TEST(FlowGenerated, distBottom0xy)
+{
+	// GIVEN: 0 distance to the ground (singularity)
+	StateSample state{};
+	state.quat_nominal = Quatf();
+
+	const float R = sq(radians(sq(0.5f)));
+	SquareMatrixState P = createRandomCovarianceMatrix();
+
+	VectorState H;
+	Vector2f innov_var;
+	sym::ComputeFlowXyInnovVarAndHx(state.vector(), P, R, FLT_EPSILON, &innov_var, &H);
+	EXPECT_GT(innov_var(0), 1e12);
+	EXPECT_GT(innov_var(1), 1e12);
+}
+
+TEST(FlowGenerated, distBottom0y)
+{
+	// GIVEN: 0 distance to the ground (singularity)
+	StateSample state{};
+	state.quat_nominal = Quatf();
+
+	const float R = sq(radians(sq(0.5f)));
+	SquareMatrixState P = createRandomCovarianceMatrix();
+
+	VectorState H;
+	float innov_var;
+	sym::ComputeFlowYInnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H);
+	EXPECT_GT(innov_var, 1e12);
+}