WIP: migrate range height control to use terrain state

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/EKF/common.h

@@ -423,10 +423,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

src/modules/ekf2/EKF/covariance.cpp

@@ -99,6 +99,10 @@ void Ekf::initialiseCovariance()
 #if defined(CONFIG_EKF2_WIND)
 	resetWindCov();
 #endif // CONFIG_EKF2_WIND
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
+	P(State::terrain_vpos.idx, State::terrain_vpos.idx) = sq(_params.rng_gnd_clearance);
+#endif // CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
 }
 
 void Ekf::predictCovariance(const imuSample &imu_delayed)
@@ -207,6 +211,18 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	}
 #endif // CONFIG_EKF2_WIND
 
+#if defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
+	if (_control_status.flags.opt_flow || _control_status.flags.rng_hgt) {
+		// process noise due to errors in vehicle height estimate
+		const float var_static = sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
+
+		// process noise due to terrain gradient
+		const float var_dynamic = sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1)));
+
+		P(State::terrain_vpos.idx, State::terrain_vpos.idx) += var_static + var_dynamic;
+	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
+
 	// covariance matrix is symmetrical, so copy upper half to lower half
 	for (unsigned row = 0; row < State::size; row++) {
 		for (unsigned column = 0 ; column < row; column++) {
@@ -327,7 +343,16 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		}
 	}
 #endif // CONFIG_EKF2_WIND
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
+	if (!_control_status.flags.rng_hgt && !_control_status.flags.opt_flow) {
+		P.uncorrelateCovarianceSetVariance<State::terrain_vpos.dof>(State::terrain_vpos.idx, 0.f);
+
+	} else {
+		constrainStateVar(State::terrain_vpos, 0.f, 1e4f);
+	}
 }
+#endif // CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
 
 void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
 {

src/modules/ekf2/EKF/ekf.cpp

@@ -72,6 +72,10 @@ void Ekf::reset()
 	_state.wind_vel.setZero();
 #endif // CONFIG_EKF2_WIND
 
+#if defined(CONFIG_EKF2_RANGE_FINDER) || defined(CONFIG_EKF2_OPTICAL_FLOW)
+	_state.terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
+#endif // CONFIG_EKF2_RANGE_FINDER || CONFIG_EKF2_OPTICAL_FLOW
+
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);

src/modules/ekf2/EKF/ekf.h

@@ -98,7 +98,7 @@ 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_vpos; };
 
 	// get the number of times the vertical terrain position has been reset
 	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
@@ -837,10 +837,7 @@ private:
 	void runTerrainEstimator(const imuSample &imu_delayed);
 	void predictHagl(const imuSample &imu_delayed);
 
-	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _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

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -207,13 +207,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)
+#if defined(CONFIG_EKF2_RANGE_FINDER) || defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
+	_state.terrain_vpos += delta_z;
-#endif // CONFIG_EKF2_RANGE_FINDER
+#endif // CONFIG_EKF2_RANGE_FINDER || CONFIG_EKF2_OPTICAL_FLOW
-
-#if defined(CONFIG_EKF2_TERRAIN)
-	terrainHandleVerticalPositionReset(delta_z);
-#endif
 
 	// Reset the timout timer
 	_time_last_hgt_fuse = _time_delayed_us;
@@ -247,6 +243,10 @@ void Ekf::constrainStates()
 #if defined(CONFIG_EKF2_WIND)
 	_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
 #endif // CONFIG_EKF2_WIND
+
+#if defined(CONFIG_EKF2_RANGE_FINDER) || defined(CONFIG_EKF2_OPTICAL_FLOW)
+	_state.terrain_vpos = matrix::constrain(_state.terrain_vpos, -1.e6f, 1.e6f);
+#endif // CONFIG_EKF2_RANGE_FINDER || CONFIG_EKF2_OPTICAL_FLOW
 }
 
 #if defined(CONFIG_EKF2_BARO_COMPENSATION)
@@ -774,6 +774,7 @@ void Ekf::fuse(const VectorState &K, float innovation)
 #if defined(CONFIG_EKF2_WIND)
 	_state.wind_vel -= K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation;
 #endif // CONFIG_EKF2_WIND
+	_state.terrain_vpos -= (K.slice<State::terrain_vpos.dof, 1>(State::terrain_vpos.idx, 0) * innovation)(0, 0);
 }
 
 void Ekf::uncorrelateQuatFromOtherStates()

src/modules/ekf2/EKF/mag_control.cpp

@@ -130,7 +130,7 @@ bool Ekf::checkHaglYawResetReq() const
 		// Check if height has increased sufficiently to be away from ground magnetic anomalies
 		// and request a yaw reset if not already requested.
 		static constexpr float mag_anomalies_max_hagl = 1.5f;
-		const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
+		const bool above_mag_anomalies = (getTerrainVertPos() - _state.pos(2)) > mag_anomalies_max_hagl;
 		return above_mag_anomalies;
 	}
 #endif // CONFIG_EKF2_TERRAIN

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -79,7 +79,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);
 	innov_var.copyTo(aid_src.innovation_variance);
 
 	// run the innovation consistency check and record result
@@ -98,7 +98,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 ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
@@ -129,7 +129,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();
@@ -170,7 +170,7 @@ 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 = math::max(_state.terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
 
 	// 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

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_vpos = 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_vpos = sf.V1.symbolic("terrain_vpos")
     )
 
     if args.disable_mag:
@@ -473,18 +475,20 @@ 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_opt_flow(state, epsilon):
     R_to_body = state["quat_nominal"].inverse()
 
     # Calculate earth relative velocity in a non-rotating sensor frame
     rel_vel_sensor = R_to_body * state["vel"]
+    hagl = state["terrain_vpos"][0] - state["pos"][2]
+    dist = hagl / state["quat_nominal"].to_rotation_matrix()[2, 2]
 
     # 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
     flow_pred = sf.V2()
-    flow_pred[0] =  rel_vel_sensor[1] / distance
+    flow_pred[0] =  rel_vel_sensor[1] / dist
-    flow_pred[1] = -rel_vel_sensor[0] / distance
+    flow_pred[1] = -rel_vel_sensor[0] / dist
-    flow_pred = add_epsilon_sign(flow_pred, distance, epsilon)
+    flow_pred = add_epsilon_sign(flow_pred, hagl, epsilon)
 
     return flow_pred
 
@@ -492,12 +496,11 @@ def predict_opt_flow(state, distance, epsilon):
 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 = sf.V1(meas_pred[0]).jacobian(state)
@@ -510,12 +513,11 @@ 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 = sf.V1(meas_pred[1]).jacobian(state)
     innov_var = (Hy * P * Hy.T + R)[0,0]

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_k.h

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: compute_drag_x_innov_var_and_k
  *
  * 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
- *     K: Matrix23_1
+ *     K: Matrix24_1
  */
 template <typename Scalar>
-void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeDragXInnovVarAndK(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 K = nullptr) {
+                              matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
   // Total ops: 348
 
   // Input arrays
@@ -153,7 +153,7 @@ void ComputeDragXInnovVarAndK(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.setZero();
 

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

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: compute_drag_y_innov_var_and_k
  *
  * 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
- *     K: Matrix23_1
+ *     K: Matrix24_1
  */
 template <typename Scalar>
-void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeDragYInnovVarAndK(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 K = nullptr) {
+                              matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
   // Total ops: 348
 
   // Input arrays
@@ -155,7 +155,7 @@ void ComputeDragYInnovVarAndK(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.setZero();
 

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

@@ -16,105 +16,146 @@ 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: 196
+  // Total ops: 383
 
   // Input arrays
 
-  // Intermediate terms (33)
+  // Intermediate terms (56)
-  const Scalar _tmp0 = std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp0 = 2 * state(0, 0);
-  const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp1 = _tmp0 * state(3, 0);
-  const Scalar _tmp2 = 1 - 2 * _tmp1;
+  const Scalar _tmp2 = -_tmp1;
-  const Scalar _tmp3 =
+  const Scalar _tmp3 = 2 * state(2, 0);
-      Scalar(1.0) /
+  const Scalar _tmp4 = _tmp3 * state(1, 0);
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
+  const Scalar _tmp5 = _tmp2 + _tmp4;
-  const Scalar _tmp4 = _tmp3 * (-2 * _tmp0 + _tmp2);
+  const Scalar _tmp6 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp5 = 2 * state(2, 0);
+  const Scalar _tmp7 = -2 * _tmp6;
-  const Scalar _tmp6 = _tmp5 * state(0, 0);
+  const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp7 = 2 * state(1, 0) * state(3, 0);
+  const Scalar _tmp9 = 1 - 2 * _tmp8;
-  const Scalar _tmp8 = _tmp5 * state(3, 0);
+  const Scalar _tmp10 = _tmp7 + _tmp9;
-  const Scalar _tmp9 = 2 * state(0, 0);
+  const Scalar _tmp11 = state(24, 0) - state(9, 0);
-  const Scalar _tmp10 = _tmp9 * state(1, 0);
+  const Scalar _tmp12 =
-  const Scalar _tmp11 = std::pow(state(2, 0), Scalar(2));
+      _tmp11 + epsilon * (2 * math::min<Scalar>(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1);
-  const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2));
+  const Scalar _tmp13 = Scalar(1.0) / (_tmp12);
-  const Scalar _tmp13 = -_tmp0;
+  const Scalar _tmp14 = _tmp10 * _tmp13;
-  const Scalar _tmp14 = _tmp12 + _tmp13;
+  const Scalar _tmp15 = _tmp14 * _tmp5;
-  const Scalar _tmp15 = _tmp3 * (state(4, 0) * (_tmp6 + _tmp7) + state(5, 0) * (-_tmp10 + _tmp8) +
+  const Scalar _tmp16 = _tmp0 * state(2, 0);
-                                 state(6, 0) * (_tmp1 - _tmp11 + _tmp14));
+  const Scalar _tmp17 = 2 * state(1, 0) * state(3, 0);
-  const Scalar _tmp16 = _tmp9 * state(3, 0);
+  const Scalar _tmp18 = _tmp3 * state(3, 0);
-  const Scalar _tmp17 = -_tmp16;
+  const Scalar _tmp19 = _tmp0 * state(1, 0);
-  const Scalar _tmp18 = _tmp5 * state(1, 0);
+  const Scalar _tmp20 = -_tmp19;
-  const Scalar _tmp19 = _tmp17 + _tmp18;
+  const Scalar _tmp21 = std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp20 = _tmp19 * _tmp3;
+  const Scalar _tmp22 = std::pow(state(0, 0), Scalar(2));
-  const Scalar _tmp21 = _tmp10 + _tmp8;
+  const Scalar _tmp23 = -_tmp6;
-  const Scalar _tmp22 = _tmp21 * _tmp3;
+  const Scalar _tmp24 = _tmp22 + _tmp23;
-  const Scalar _tmp23 = -_tmp7;
+  const Scalar _tmp25 = -_tmp18;
-  const Scalar _tmp24 = -_tmp12;
+  const Scalar _tmp26 = _tmp20 + _tmp25;
-  const Scalar _tmp25 = _tmp3 * (state(4, 0) * (_tmp1 + _tmp11 + _tmp13 + _tmp24) +
+  const Scalar _tmp27 = -2 * _tmp21;
-                                 state(5, 0) * (_tmp17 - _tmp18) + state(6, 0) * (_tmp23 + _tmp6));
+  const Scalar _tmp28 = _tmp27 + _tmp7 + 1;
-  const Scalar _tmp26 = _tmp3 * (-2 * _tmp11 + _tmp2);
+  const Scalar _tmp29 = _tmp18 + _tmp19;
-  const Scalar _tmp27 = -_tmp6;
+  const Scalar _tmp30 = _tmp29 * state(6, 0) + _tmp5 * state(4, 0);
-  const Scalar _tmp28 = -_tmp1 + _tmp11;
+  const Scalar _tmp31 = _tmp28 * state(5, 0) + _tmp30;
-  const Scalar _tmp29 = _tmp3 * (state(4, 0) * (_tmp23 + _tmp27) + state(5, 0) * (_tmp10 - _tmp8) +
+  const Scalar _tmp32 = _tmp13 * _tmp31;
-                                 state(6, 0) * (_tmp0 + _tmp24 + _tmp28));
+  const Scalar _tmp33 =
-  const Scalar _tmp30 =
+      _tmp14 * (state(4, 0) * (_tmp16 + _tmp17) + state(5, 0) * (_tmp18 + _tmp20) +
-      _tmp3 * (_tmp19 * state(4, 0) + _tmp21 * state(6, 0) + state(5, 0) * (_tmp14 + _tmp28));
+                state(6, 0) * (_tmp21 + _tmp24 - _tmp8)) +
-  const Scalar _tmp31 = _tmp3 * (_tmp16 + _tmp18);
+      _tmp26 * _tmp32;
-  const Scalar _tmp32 = _tmp3 * (_tmp27 + _tmp7);
+  const Scalar _tmp34 = -_tmp16;
+  const Scalar _tmp35 = _tmp17 + _tmp34;
+  const Scalar _tmp36 = _tmp32 * _tmp35;
+  const Scalar _tmp37 = _tmp14 * _tmp28;
+  const Scalar _tmp38 = _tmp10 / std::pow(_tmp12, Scalar(2));
+  const Scalar _tmp39 = _tmp31 * _tmp38;
+  const Scalar _tmp40 = -_tmp17;
+  const Scalar _tmp41 = -_tmp22;
+  const Scalar _tmp42 = _tmp14 * (state(4, 0) * (_tmp21 + _tmp23 + _tmp41 + _tmp8) +
+                                  state(5, 0) * (_tmp2 - _tmp4) + state(6, 0) * (_tmp16 + _tmp40));
+  const Scalar _tmp43 = _tmp14 * _tmp29;
+  const Scalar _tmp44 = _tmp14 * _tmp35;
+  const Scalar _tmp45 = _tmp1 + _tmp4;
+  const Scalar _tmp46 = _tmp14 * _tmp45;
+  const Scalar _tmp47 = _tmp27 + _tmp9;
+  const Scalar _tmp48 = _tmp14 * _tmp47;
+  const Scalar _tmp49 = -_tmp21 + _tmp8;
+  const Scalar _tmp50 = _tmp35 * state(6, 0) + _tmp45 * state(5, 0) + _tmp47 * state(4, 0);
+  const Scalar _tmp51 = _tmp13 * _tmp50;
+  const Scalar _tmp52 =
+      -_tmp14 * (state(4, 0) * (_tmp34 + _tmp40) + state(5, 0) * (_tmp19 + _tmp25) +
+                 state(6, 0) * (_tmp41 + _tmp49 + _tmp6)) -
+      _tmp35 * _tmp51;
+  const Scalar _tmp53 = _tmp38 * _tmp50;
+  const Scalar _tmp54 = _tmp14 * (_tmp30 + state(5, 0) * (_tmp24 + _tmp49));
+  const Scalar _tmp55 = _tmp26 * _tmp51;
 
   // 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) =
-                       _tmp15 * (P(0, 0) * _tmp15 + P(2, 0) * _tmp25 + P(3, 0) * _tmp20 +
+        R +
-                                 P(4, 0) * _tmp4 + P(5, 0) * _tmp22) +
+        _tmp15 * (P(0, 3) * _tmp33 + P(1, 3) * _tmp36 + P(2, 3) * _tmp42 - P(23, 3) * _tmp39 +
-                       _tmp20 * (P(0, 3) * _tmp15 + P(2, 3) * _tmp25 + P(3, 3) * _tmp20 +
+                  P(3, 3) * _tmp15 + P(4, 3) * _tmp37 + P(5, 3) * _tmp43 + P(8, 3) * _tmp39) +
-                                 P(4, 3) * _tmp4 + P(5, 3) * _tmp22) +
+        _tmp33 * (P(0, 0) * _tmp33 + P(1, 0) * _tmp36 + P(2, 0) * _tmp42 - P(23, 0) * _tmp39 +
-                       _tmp22 * (P(0, 5) * _tmp15 + P(2, 5) * _tmp25 + P(3, 5) * _tmp20 +
+                  P(3, 0) * _tmp15 + P(4, 0) * _tmp37 + P(5, 0) * _tmp43 + P(8, 0) * _tmp39) +
-                                 P(4, 5) * _tmp4 + P(5, 5) * _tmp22) +
+        _tmp36 * (P(0, 1) * _tmp33 + P(1, 1) * _tmp36 + P(2, 1) * _tmp42 - P(23, 1) * _tmp39 +
-                       _tmp25 * (P(0, 2) * _tmp15 + P(2, 2) * _tmp25 + P(3, 2) * _tmp20 +
+                  P(3, 1) * _tmp15 + P(4, 1) * _tmp37 + P(5, 1) * _tmp43 + P(8, 1) * _tmp39) +
-                                 P(4, 2) * _tmp4 + P(5, 2) * _tmp22) +
+        _tmp37 * (P(0, 4) * _tmp33 + P(1, 4) * _tmp36 + P(2, 4) * _tmp42 - P(23, 4) * _tmp39 +
-                       _tmp4 * (P(0, 4) * _tmp15 + P(2, 4) * _tmp25 + P(3, 4) * _tmp20 +
+                  P(3, 4) * _tmp15 + P(4, 4) * _tmp37 + P(5, 4) * _tmp43 + P(8, 4) * _tmp39) -
-                                P(4, 4) * _tmp4 + P(5, 4) * _tmp22);
+        _tmp39 * (P(0, 23) * _tmp33 + P(1, 23) * _tmp36 + P(2, 23) * _tmp42 - P(23, 23) * _tmp39 +
-    _innov_var(1, 0) = R -
+                  P(3, 23) * _tmp15 + P(4, 23) * _tmp37 + P(5, 23) * _tmp43 + P(8, 23) * _tmp39) +
-                       _tmp26 * (-P(1, 3) * _tmp29 - P(2, 3) * _tmp30 - P(3, 3) * _tmp26 -
+        _tmp39 * (P(0, 8) * _tmp33 + P(1, 8) * _tmp36 + P(2, 8) * _tmp42 - P(23, 8) * _tmp39 +
-                                 P(4, 3) * _tmp31 - P(5, 3) * _tmp32) -
+                  P(3, 8) * _tmp15 + P(4, 8) * _tmp37 + P(5, 8) * _tmp43 + P(8, 8) * _tmp39) +
-                       _tmp29 * (-P(1, 1) * _tmp29 - P(2, 1) * _tmp30 - P(3, 1) * _tmp26 -
+        _tmp42 * (P(0, 2) * _tmp33 + P(1, 2) * _tmp36 + P(2, 2) * _tmp42 - P(23, 2) * _tmp39 +
-                                 P(4, 1) * _tmp31 - P(5, 1) * _tmp32) -
+                  P(3, 2) * _tmp15 + P(4, 2) * _tmp37 + P(5, 2) * _tmp43 + P(8, 2) * _tmp39) +
-                       _tmp30 * (-P(1, 2) * _tmp29 - P(2, 2) * _tmp30 - P(3, 2) * _tmp26 -
+        _tmp43 * (P(0, 5) * _tmp33 + P(1, 5) * _tmp36 + P(2, 5) * _tmp42 - P(23, 5) * _tmp39 +
-                                 P(4, 2) * _tmp31 - P(5, 2) * _tmp32) -
+                  P(3, 5) * _tmp15 + P(4, 5) * _tmp37 + P(5, 5) * _tmp43 + P(8, 5) * _tmp39);
-                       _tmp31 * (-P(1, 4) * _tmp29 - P(2, 4) * _tmp30 - P(3, 4) * _tmp26 -
+    _innov_var(1, 0) =
-                                 P(4, 4) * _tmp31 - P(5, 4) * _tmp32) -
+        R -
-                       _tmp32 * (-P(1, 5) * _tmp29 - P(2, 5) * _tmp30 - P(3, 5) * _tmp26 -
+        _tmp44 * (-P(0, 5) * _tmp55 + P(1, 5) * _tmp52 - P(2, 5) * _tmp54 + P(23, 5) * _tmp53 -
-                                 P(4, 5) * _tmp31 - P(5, 5) * _tmp32);
+                  P(3, 5) * _tmp48 - P(4, 5) * _tmp46 - P(5, 5) * _tmp44 - P(8, 5) * _tmp53) -
+        _tmp46 * (-P(0, 4) * _tmp55 + P(1, 4) * _tmp52 - P(2, 4) * _tmp54 + P(23, 4) * _tmp53 -
+                  P(3, 4) * _tmp48 - P(4, 4) * _tmp46 - P(5, 4) * _tmp44 - P(8, 4) * _tmp53) -
+        _tmp48 * (-P(0, 3) * _tmp55 + P(1, 3) * _tmp52 - P(2, 3) * _tmp54 + P(23, 3) * _tmp53 -
+                  P(3, 3) * _tmp48 - P(4, 3) * _tmp46 - P(5, 3) * _tmp44 - P(8, 3) * _tmp53) +
+        _tmp52 * (-P(0, 1) * _tmp55 + P(1, 1) * _tmp52 - P(2, 1) * _tmp54 + P(23, 1) * _tmp53 -
+                  P(3, 1) * _tmp48 - P(4, 1) * _tmp46 - P(5, 1) * _tmp44 - P(8, 1) * _tmp53) +
+        _tmp53 * (-P(0, 23) * _tmp55 + P(1, 23) * _tmp52 - P(2, 23) * _tmp54 + P(23, 23) * _tmp53 -
+                  P(3, 23) * _tmp48 - P(4, 23) * _tmp46 - P(5, 23) * _tmp44 - P(8, 23) * _tmp53) -
+        _tmp53 * (-P(0, 8) * _tmp55 + P(1, 8) * _tmp52 - P(2, 8) * _tmp54 + P(23, 8) * _tmp53 -
+                  P(3, 8) * _tmp48 - P(4, 8) * _tmp46 - P(5, 8) * _tmp44 - P(8, 8) * _tmp53) -
+        _tmp54 * (-P(0, 2) * _tmp55 + P(1, 2) * _tmp52 - P(2, 2) * _tmp54 + P(23, 2) * _tmp53 -
+                  P(3, 2) * _tmp48 - P(4, 2) * _tmp46 - P(5, 2) * _tmp44 - P(8, 2) * _tmp53) -
+        _tmp55 * (-P(0, 0) * _tmp55 + P(1, 0) * _tmp52 - P(2, 0) * _tmp54 + P(23, 0) * _tmp53 -
+                  P(3, 0) * _tmp48 - P(4, 0) * _tmp46 - P(5, 0) * _tmp44 - P(8, 0) * _tmp53);
   }
 
   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) = _tmp33;
-    _h(2, 0) = _tmp25;
+    _h(1, 0) = _tmp36;
-    _h(3, 0) = _tmp20;
+    _h(2, 0) = _tmp42;
-    _h(4, 0) = _tmp4;
+    _h(3, 0) = _tmp15;
-    _h(5, 0) = _tmp22;
+    _h(4, 0) = _tmp37;
+    _h(5, 0) = _tmp43;
+    _h(8, 0) = _tmp39;
+    _h(23, 0) = -_tmp39;
   }
 }  // NOLINT(readability/fn_size)
 

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

@@ -16,79 +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: 212
-  // Total ops: 116
 
   // Input arrays
 
-  // Intermediate terms (19)
+  // Intermediate terms (32)
-  const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp0 = 2 * state(2, 0);
-  const Scalar _tmp1 = std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp1 = -_tmp0 * state(0, 0);
-  const Scalar _tmp2 =
+  const Scalar _tmp2 = 2 * state(1, 0);
-      Scalar(1.0) /
+  const Scalar _tmp3 = _tmp2 * state(3, 0);
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
+  const Scalar _tmp4 = _tmp1 + _tmp3;
-  const Scalar _tmp3 = _tmp2 * (-2 * _tmp0 - 2 * _tmp1 + 1);
+  const Scalar _tmp5 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp4 = -2 * state(0, 0) * state(2, 0);
+  const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp5 = 2 * state(3, 0);
+  const Scalar _tmp7 = 1 - 2 * _tmp6;
-  const Scalar _tmp6 = _tmp5 * state(1, 0);
+  const Scalar _tmp8 = -2 * _tmp5 + _tmp7;
-  const Scalar _tmp7 = _tmp5 * state(2, 0);
+  const Scalar _tmp9 = state(24, 0) - state(9, 0);
-  const Scalar _tmp8 = 2 * state(1, 0);
+  const Scalar _tmp10 =
-  const Scalar _tmp9 = _tmp8 * state(0, 0);
+      _tmp9 + epsilon * (2 * math::min<Scalar>(0, (((_tmp9) > 0) - ((_tmp9) < 0))) + 1);
-  const Scalar _tmp10 = std::pow(state(0, 0), Scalar(2));
+  const Scalar _tmp11 = Scalar(1.0) / (_tmp10);
-  const Scalar _tmp11 = std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp12 = _tmp11 * _tmp8;
-  const Scalar _tmp12 = -_tmp0 + _tmp1;
+  const Scalar _tmp13 = _tmp12 * _tmp4;
-  const Scalar _tmp13 = _tmp2 * (state(4, 0) * (_tmp4 - _tmp6) + state(5, 0) * (-_tmp7 + _tmp9) +
+  const Scalar _tmp14 = 2 * state(0, 0) * state(3, 0);
-                                 state(6, 0) * (-_tmp10 + _tmp11 + _tmp12));
+  const Scalar _tmp15 = _tmp0 * state(1, 0);
-  const Scalar _tmp14 = _tmp5 * state(0, 0);
+  const Scalar _tmp16 = _tmp14 + _tmp15;
-  const Scalar _tmp15 = _tmp8 * state(2, 0);
+  const Scalar _tmp17 = _tmp12 * _tmp16;
-  const Scalar _tmp16 =
+  const Scalar _tmp18 = std::pow(state(3, 0), Scalar(2));
-      _tmp2 * (state(4, 0) * (-_tmp14 + _tmp15) + state(5, 0) * (_tmp10 - _tmp11 + _tmp12) +
+  const Scalar _tmp19 = -2 * _tmp18 + _tmp7;
-               state(6, 0) * (_tmp7 + _tmp9));
+  const Scalar _tmp20 = _tmp12 * _tmp19;
-  const Scalar _tmp17 = _tmp2 * (_tmp14 + _tmp15);
+  const Scalar _tmp21 = _tmp0 * state(3, 0);
-  const Scalar _tmp18 = _tmp2 * (_tmp4 + _tmp6);
+  const Scalar _tmp22 = -_tmp21;
+  const Scalar _tmp23 = _tmp2 * state(0, 0);
+  const Scalar _tmp24 = std::pow(state(0, 0), Scalar(2));
+  const Scalar _tmp25 = -_tmp18 + _tmp6;
+  const Scalar _tmp26 = _tmp16 * state(5, 0) + _tmp19 * state(4, 0) + _tmp4 * state(6, 0);
+  const Scalar _tmp27 = _tmp11 * _tmp26;
+  const Scalar _tmp28 = -_tmp12 * (state(4, 0) * (_tmp1 - _tmp3) + state(5, 0) * (_tmp22 + _tmp23) +
+                                   state(6, 0) * (-_tmp24 + _tmp25 + _tmp5)) -
+                        _tmp27 * _tmp4;
+  const Scalar _tmp29 = _tmp26 * _tmp8 / std::pow(_tmp10, Scalar(2));
+  const Scalar _tmp30 =
+      _tmp12 * (state(4, 0) * (-_tmp14 + _tmp15) + state(5, 0) * (_tmp24 + _tmp25 - _tmp5) +
+                state(6, 0) * (_tmp21 + _tmp23));
+  const Scalar _tmp31 = _tmp27 * (_tmp22 - _tmp23);
 
   // Output terms (2)
   if (innov_var != nullptr) {
     Scalar& _innov_var = (*innov_var);
 
-    _innov_var = R -
+    _innov_var =
-                 _tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp16 - P(3, 1) * _tmp3 -
+        R -
-                           P(4, 1) * _tmp17 - P(5, 1) * _tmp18) -
+        _tmp13 * (-P(0, 5) * _tmp31 + P(1, 5) * _tmp28 - P(2, 5) * _tmp30 + P(23, 5) * _tmp29 -
-                 _tmp16 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp16 - P(3, 2) * _tmp3 -
+                  P(3, 5) * _tmp20 - P(4, 5) * _tmp17 - P(5, 5) * _tmp13 - P(8, 5) * _tmp29) -
-                           P(4, 2) * _tmp17 - P(5, 2) * _tmp18) -
+        _tmp17 * (-P(0, 4) * _tmp31 + P(1, 4) * _tmp28 - P(2, 4) * _tmp30 + P(23, 4) * _tmp29 -
-                 _tmp17 * (-P(1, 4) * _tmp13 - P(2, 4) * _tmp16 - P(3, 4) * _tmp3 -
+                  P(3, 4) * _tmp20 - P(4, 4) * _tmp17 - P(5, 4) * _tmp13 - P(8, 4) * _tmp29) -
-                           P(4, 4) * _tmp17 - P(5, 4) * _tmp18) -
+        _tmp20 * (-P(0, 3) * _tmp31 + P(1, 3) * _tmp28 - P(2, 3) * _tmp30 + P(23, 3) * _tmp29 -
-                 _tmp18 * (-P(1, 5) * _tmp13 - P(2, 5) * _tmp16 - P(3, 5) * _tmp3 -
+                  P(3, 3) * _tmp20 - P(4, 3) * _tmp17 - P(5, 3) * _tmp13 - P(8, 3) * _tmp29) +
-                           P(4, 5) * _tmp17 - P(5, 5) * _tmp18) -
+        _tmp28 * (-P(0, 1) * _tmp31 + P(1, 1) * _tmp28 - P(2, 1) * _tmp30 + P(23, 1) * _tmp29 -
-                 _tmp3 * (-P(1, 3) * _tmp13 - P(2, 3) * _tmp16 - P(3, 3) * _tmp3 -
+                  P(3, 1) * _tmp20 - P(4, 1) * _tmp17 - P(5, 1) * _tmp13 - P(8, 1) * _tmp29) +
-                          P(4, 3) * _tmp17 - P(5, 3) * _tmp18);
+        _tmp29 * (-P(0, 23) * _tmp31 + P(1, 23) * _tmp28 - P(2, 23) * _tmp30 + P(23, 23) * _tmp29 -
+                  P(3, 23) * _tmp20 - P(4, 23) * _tmp17 - P(5, 23) * _tmp13 - P(8, 23) * _tmp29) -
+        _tmp29 * (-P(0, 8) * _tmp31 + P(1, 8) * _tmp28 - P(2, 8) * _tmp30 + P(23, 8) * _tmp29 -
+                  P(3, 8) * _tmp20 - P(4, 8) * _tmp17 - P(5, 8) * _tmp13 - P(8, 8) * _tmp29) -
+        _tmp30 * (-P(0, 2) * _tmp31 + P(1, 2) * _tmp28 - P(2, 2) * _tmp30 + P(23, 2) * _tmp29 -
+                  P(3, 2) * _tmp20 - P(4, 2) * _tmp17 - P(5, 2) * _tmp13 - P(8, 2) * _tmp29) -
+        _tmp31 * (-P(0, 0) * _tmp31 + P(1, 0) * _tmp28 - P(2, 0) * _tmp30 + P(23, 0) * _tmp29 -
+                  P(3, 0) * _tmp20 - P(4, 0) * _tmp17 - P(5, 0) * _tmp13 - P(8, 0) * _tmp29);
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 
-    _h(1, 0) = -_tmp13;
+    _h(0, 0) = -_tmp31;
-    _h(2, 0) = -_tmp16;
+    _h(1, 0) = _tmp28;
-    _h(3, 0) = -_tmp3;
+    _h(2, 0) = -_tmp30;
+    _h(3, 0) = -_tmp20;
     _h(4, 0) = -_tmp17;
-    _h(5, 0) = -_tmp18;
+    _h(5, 0) = -_tmp13;
+    _h(8, 0) = -_tmp29;
+    _h(23, 0) = _tmp29;
   }
 }  // 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: 95
 
   // Input arrays
@@ -88,7 +88,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_innov_var_and_k_and_h.h

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: compute_gravity_innov_var_and_k_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix25_1
- *     P: Matrix23_23
+ *     P: Matrix24_24
  *     meas: Matrix31
  *     R: Scalar
  *     epsilon: Scalar
@@ -25,21 +25,21 @@ namespace sym {
  * Outputs:
  *     innov: Matrix31
  *     innov_var: Matrix31
- *     Kx: Matrix23_1
+ *     Kx: Matrix24_1
- *     Ky: Matrix23_1
+ *     Ky: Matrix24_1
- *     Kz: Matrix23_1
+ *     Kz: Matrix24_1
  */
 template <typename Scalar>
-void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeGravityInnovVarAndKAndH(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 Kx = nullptr,
+                                    matrix::Matrix<Scalar, 24, 1>* const Kx = nullptr,
-                                    matrix::Matrix<Scalar, 23, 1>* const Ky = nullptr,
+                                    matrix::Matrix<Scalar, 24, 1>* const Ky = nullptr,
-                                    matrix::Matrix<Scalar, 23, 1>* const Kz = nullptr) {
+                                    matrix::Matrix<Scalar, 24, 1>* const Kz = nullptr) {
-  // Total ops: 361
+  // Total ops: 373
 
   // Input arrays
 
@@ -103,7 +103,7 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (Kx != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _kx = (*Kx);
+    matrix::Matrix<Scalar, 24, 1>& _kx = (*Kx);
 
     _kx(0, 0) = _tmp28 * (P(0, 1) * _tmp13 + P(0, 2) * _tmp14);
     _kx(1, 0) = _tmp28 * (P(1, 2) * _tmp14 + _tmp16);
@@ -128,10 +128,11 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
     _kx(20, 0) = _tmp28 * (P(20, 1) * _tmp13 + P(20, 2) * _tmp14);
     _kx(21, 0) = _tmp28 * (P(21, 1) * _tmp13 + P(21, 2) * _tmp14);
     _kx(22, 0) = _tmp28 * (P(22, 1) * _tmp13 + P(22, 2) * _tmp14);
+    _kx(23, 0) = _tmp28 * (P(23, 1) * _tmp13 + P(23, 2) * _tmp14);
   }
 
   if (Ky != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _ky = (*Ky);
+    matrix::Matrix<Scalar, 24, 1>& _ky = (*Ky);
 
     _ky(0, 0) = _tmp29 * (P(0, 2) * _tmp19 + _tmp21);
     _ky(1, 0) = _tmp29 * (P(1, 0) * _tmp18 + P(1, 2) * _tmp19);
@@ -156,10 +157,11 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
     _ky(20, 0) = _tmp29 * (P(20, 0) * _tmp18 + P(20, 2) * _tmp19);
     _ky(21, 0) = _tmp29 * (P(21, 0) * _tmp18 + P(21, 2) * _tmp19);
     _ky(22, 0) = _tmp29 * (P(22, 0) * _tmp18 + P(22, 2) * _tmp19);
+    _ky(23, 0) = _tmp29 * (P(23, 0) * _tmp18 + P(23, 2) * _tmp19);
   }
 
   if (Kz != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _kz = (*Kz);
+    matrix::Matrix<Scalar, 24, 1>& _kz = (*Kz);
 
     _kz(0, 0) = _tmp30 * (P(0, 1) * _tmp24 + _tmp26);
     _kz(1, 0) = _tmp30 * (P(1, 0) * _tmp23 + _tmp25);
@@ -184,6 +186,7 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
     _kz(20, 0) = _tmp30 * (P(20, 0) * _tmp23 + P(20, 1) * _tmp24);
     _kz(21, 0) = _tmp30 * (P(21, 0) * _tmp23 + P(21, 1) * _tmp24);
     _kz(22, 0) = _tmp30 * (P(22, 0) * _tmp23 + P(22, 1) * _tmp24);
+    _kz(23, 0) = _tmp30 * (P(23, 0) * _tmp23 + P(23, 1) * _tmp24);
   }
 }  // NOLINT(readability/fn_size)
 

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: 314
 
   // Unused inputs
@@ -146,7 +146,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: 110
 
   // Unused inputs
@@ -78,7 +78,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: 110
 
   // Unused inputs
@@ -78,7 +78,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,21 +16,21 @@ 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: 469
+  // Total ops: 485
 
   // Input arrays
 
@@ -42,20 +42,20 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
   const Scalar _tmp4 = -state(22, 0) + state(4, 0);
   const Scalar _tmp5 = 2 * state(0, 0);
   const Scalar _tmp6 = _tmp5 * state(3, 0);
-  const Scalar _tmp7 = 2 * state(2, 0);
+  const Scalar _tmp7 = 2 * state(1, 0);
-  const Scalar _tmp8 = _tmp7 * state(1, 0);
+  const Scalar _tmp8 = _tmp7 * state(2, 0);
   const Scalar _tmp9 = _tmp6 + _tmp8;
   const Scalar _tmp10 = -state(23, 0) + state(5, 0);
-  const Scalar _tmp11 = _tmp7 * state(0, 0);
+  const Scalar _tmp11 = _tmp5 * state(2, 0);
   const Scalar _tmp12 = -_tmp11;
-  const Scalar _tmp13 = 2 * state(1, 0) * state(3, 0);
+  const Scalar _tmp13 = _tmp7 * state(3, 0);
   const Scalar _tmp14 = _tmp12 + _tmp13;
   const Scalar _tmp15 = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4;
   const Scalar _tmp16 =
       _tmp15 + epsilon * (2 * math::min<Scalar>(0, (((_tmp15) > 0) - ((_tmp15) < 0))) + 1);
   const Scalar _tmp17 = Scalar(1.0) / (_tmp16);
-  const Scalar _tmp18 = _tmp7 * state(3, 0);
+  const Scalar _tmp18 = 2 * state(2, 0) * state(3, 0);
-  const Scalar _tmp19 = _tmp5 * state(1, 0);
+  const Scalar _tmp19 = _tmp7 * state(0, 0);
   const Scalar _tmp20 = std::pow(state(0, 0), Scalar(2));
   const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2));
   const Scalar _tmp22 = -_tmp21;
@@ -90,7 +90,7 @@ void ComputeSideslipHAndK(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();
 
@@ -105,7 +105,7 @@ void ComputeSideslipHAndK(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) =
         _tmp45 * (P(0, 0) * _tmp24 - P(0, 1) * _tmp34 + P(0, 2) * _tmp35 + P(0, 21) * _tmp43 +
@@ -176,6 +176,9 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
     _k(22, 0) =
         _tmp45 * (P(22, 0) * _tmp24 - P(22, 1) * _tmp34 + P(22, 2) * _tmp35 + P(22, 21) * _tmp43 +
                   P(22, 22) * _tmp44 + P(22, 3) * _tmp38 + P(22, 4) * _tmp41 + P(22, 5) * _tmp42);
+    _k(23, 0) =
+        _tmp45 * (P(23, 0) * _tmp24 - P(23, 1) * _tmp34 + P(23, 2) * _tmp35 + P(23, 21) * _tmp43 +
+                  P(23, 22) * _tmp44 + P(23, 3) * _tmp38 + P(23, 4) * _tmp41 + P(23, 5) * _tmp42);
   }
 }  // 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: 235

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

@@ -16,20 +16,20 @@ namespace sym {
  * Symbolic function: compute_yaw_312_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 ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeYaw312InnovVarAndH(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: 53
 
   // Input arrays
@@ -63,7 +63,7 @@ void ComputeYaw312InnovVarAndH(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_yaw_312_innov_var_and_h_alternate.h

@@ -16,20 +16,20 @@ namespace sym {
  * Symbolic function: compute_yaw_312_innov_var_and_h_alternate
  *
  * 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 ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeYaw312InnovVarAndHAlternate(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: 57
 
   // Input arrays
@@ -63,7 +63,7 @@ void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
   }
 
   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_yaw_321_innov_var_and_h.h

@@ -16,20 +16,20 @@ namespace sym {
  * Symbolic function: compute_yaw_321_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 ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeYaw321InnovVarAndH(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: 53
 
   // Input arrays
@@ -63,7 +63,7 @@ void ComputeYaw321InnovVarAndH(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_yaw_321_innov_var_and_h_alternate.h

@@ -16,20 +16,20 @@ namespace sym {
  * Symbolic function: compute_yaw_321_innov_var_and_h_alternate
  *
  * 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 ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeYaw321InnovVarAndHAlternate(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: 57
 
   // Input arrays
@@ -63,7 +63,7 @@ void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
   }
 
   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,16 +25,16 @@ 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: 1793
+  // Total ops: 1853
 
   // Input arrays
 
@@ -75,19 +75,19 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   const Scalar _tmp33 = _tmp32 * state(2, 0);
   const Scalar _tmp34 = _tmp31 + _tmp33;
   const Scalar _tmp35 = accel(0, 0) - state(13, 0);
-  const Scalar _tmp36 = std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp36 = std::pow(state(3, 0), Scalar(2));
   const Scalar _tmp37 = std::pow(state(1, 0), Scalar(2));
   const Scalar _tmp38 = -_tmp37;
   const Scalar _tmp39 = _tmp36 + _tmp38;
   const Scalar _tmp40 = std::pow(state(0, 0), Scalar(2));
   const Scalar _tmp41 = -_tmp40;
-  const Scalar _tmp42 = std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp42 = std::pow(state(2, 0), Scalar(2));
   const Scalar _tmp43 = _tmp41 + _tmp42;
   const Scalar _tmp44 = accel(1, 0) - state(14, 0);
   const Scalar _tmp45 = dt * (_tmp34 * _tmp35 + _tmp44 * (_tmp39 + _tmp43));
   const Scalar _tmp46 = P(0, 12) + P(1, 12) * _tmp5 + P(2, 12) * _tmp2 - P(9, 12) * dt;
-  const Scalar _tmp47 = -2 * _tmp42;
+  const Scalar _tmp47 = -2 * _tmp36;
-  const Scalar _tmp48 = -2 * _tmp36;
+  const Scalar _tmp48 = -2 * _tmp42;
   const Scalar _tmp49 = _tmp47 + _tmp48 + 1;
   const Scalar _tmp50 = _tmp49 * dt;
   const Scalar _tmp51 = _tmp29 * state(2, 0);
@@ -137,8 +137,9 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   const Scalar _tmp88 = _tmp29 * state(1, 0);
   const Scalar _tmp89 = -_tmp88;
   const Scalar _tmp90 = _tmp87 + _tmp89;
-  const Scalar _tmp91 = dt * (_tmp44 * _tmp90 + _tmp59 * (_tmp43 + _tmp58));
+  const Scalar _tmp91 = dt * (_tmp44 * _tmp90 + _tmp59 * (_tmp36 + _tmp37 + _tmp41 + _tmp55));
-  const Scalar _tmp92 = dt * (_tmp35 * (_tmp39 + _tmp56) + _tmp44 * (_tmp31 + _tmp62));
+  const Scalar _tmp92 =
+      dt * (_tmp35 * (_tmp38 + _tmp40 + _tmp42 + _tmp57) + _tmp44 * (_tmp31 + _tmp62));
   const Scalar _tmp93 = 1 - 2 * _tmp37;
   const Scalar _tmp94 = _tmp47 + _tmp93;
   const Scalar _tmp95 = _tmp94 * dt;
@@ -152,52 +153,51 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   const Scalar _tmp103 = _tmp74 * dt;
   const Scalar _tmp104 = P(0, 4) * _tmp22 + P(1, 4) * _tmp23 - P(11, 4) * dt + P(2, 4);
   const Scalar _tmp105 = _tmp81 * _tmp97;
-  const Scalar _tmp106 = _tmp84 * _tmp90;
+  const Scalar _tmp106 = _tmp83 * _tmp94;
-  const Scalar _tmp107 = P(0, 4) * _tmp63 + P(1, 4) * _tmp60 - P(12, 4) * _tmp50 -
+  const Scalar _tmp107 = _tmp84 * _tmp90;
+  const Scalar _tmp108 = P(0, 4) * _tmp63 + P(1, 4) * _tmp60 - P(12, 4) * _tmp50 -
                          P(13, 4) * _tmp65 - P(14, 4) * _tmp67 + P(2, 4) * _tmp45 + P(3, 4);
-  const Scalar _tmp108 = P(0, 14) * _tmp91 + P(1, 14) * _tmp99 - P(12, 14) * _tmp100 -
+  const Scalar _tmp109 = P(0, 14) * _tmp91 + P(1, 14) * _tmp99 - P(12, 14) * _tmp100 -
                          P(13, 14) * _tmp95 - P(14, 14) * _tmp96 + P(2, 14) * _tmp92 + P(4, 14);
-  const Scalar _tmp109 = P(0, 0) * _tmp91 + P(1, 0) * _tmp99 - P(12, 0) * _tmp100 -
+  const Scalar _tmp110 = P(0, 0) * _tmp91 + P(1, 0) * _tmp99 - P(12, 0) * _tmp100 -
                          P(13, 0) * _tmp95 - P(14, 0) * _tmp96 + P(2, 0) * _tmp92 + P(4, 0);
-  const Scalar _tmp110 = P(0, 13) * _tmp91 + P(1, 13) * _tmp99 - P(12, 13) * _tmp100 -
+  const Scalar _tmp111 = P(0, 13) * _tmp91 + P(1, 13) * _tmp99 - P(12, 13) * _tmp100 -
                          P(13, 13) * _tmp95 - P(14, 13) * _tmp96 + P(2, 13) * _tmp92 + P(4, 13);
-  const Scalar _tmp111 = P(0, 12) * _tmp91 + P(1, 12) * _tmp99 - P(12, 12) * _tmp100 -
+  const Scalar _tmp112 = P(0, 12) * _tmp91 + P(1, 12) * _tmp99 - P(12, 12) * _tmp100 -
                          P(13, 12) * _tmp95 - P(14, 12) * _tmp96 + P(2, 12) * _tmp92 + P(4, 12);
-  const Scalar _tmp112 = P(0, 1) * _tmp91 + P(1, 1) * _tmp99 - P(12, 1) * _tmp100 -
+  const Scalar _tmp113 = P(0, 1) * _tmp91 + P(1, 1) * _tmp99 - P(12, 1) * _tmp100 -
                          P(13, 1) * _tmp95 - P(14, 1) * _tmp96 + P(2, 1) * _tmp92 + P(4, 1);
-  const Scalar _tmp113 = P(0, 2) * _tmp91 + P(1, 2) * _tmp99 - P(12, 2) * _tmp100 -
+  const Scalar _tmp114 = P(0, 2) * _tmp91 + P(1, 2) * _tmp99 - P(12, 2) * _tmp100 -
                          P(13, 2) * _tmp95 - P(14, 2) * _tmp96 + P(2, 2) * _tmp92 + P(4, 2);
-  const Scalar _tmp114 = P(0, 4) * _tmp91 + P(1, 4) * _tmp99 - P(12, 4) * _tmp100 -
+  const Scalar _tmp115 = P(0, 4) * _tmp91 + P(1, 4) * _tmp99 - P(12, 4) * _tmp100 -
                          P(13, 4) * _tmp95 - P(14, 4) * _tmp96 + P(2, 4) * _tmp92 + P(4, 4);
-  const Scalar _tmp115 =
+  const Scalar _tmp116 = dt * (_tmp44 * (_tmp39 + _tmp56) + _tmp59 * (_tmp89 + _tmp98));
-      dt * (_tmp44 * (_tmp38 + _tmp40 + _tmp42 + _tmp57) + _tmp59 * (_tmp89 + _tmp98));
+  const Scalar _tmp117 = _tmp48 + _tmp93;
-  const Scalar _tmp116 = _tmp48 + _tmp93;
+  const Scalar _tmp118 = _tmp117 * dt;
-  const Scalar _tmp117 = _tmp116 * dt;
+  const Scalar _tmp119 = _tmp87 + _tmp88;
-  const Scalar _tmp118 = _tmp87 + _tmp88;
+  const Scalar _tmp120 = _tmp119 * dt;
-  const Scalar _tmp119 = _tmp118 * dt;
+  const Scalar _tmp121 = dt * (_tmp119 * _tmp35 + _tmp44 * (_tmp51 + _tmp54));
-  const Scalar _tmp120 = dt * (_tmp118 * _tmp35 + _tmp44 * (_tmp51 + _tmp54));
+  const Scalar _tmp122 = _tmp52 + _tmp53;
-  const Scalar _tmp121 = _tmp52 + _tmp53;
+  const Scalar _tmp123 = dt * (_tmp122 * _tmp59 + _tmp35 * (_tmp43 + _tmp58));
-  const Scalar _tmp122 = dt * (_tmp121 * _tmp59 + _tmp35 * (_tmp36 + _tmp37 + _tmp41 + _tmp55));
+  const Scalar _tmp124 = _tmp122 * dt;
-  const Scalar _tmp123 = _tmp121 * dt;
+  const Scalar _tmp125 = P(0, 5) + P(1, 5) * _tmp5 + P(2, 5) * _tmp2 - P(9, 5) * dt;
-  const Scalar _tmp124 = P(0, 5) + P(1, 5) * _tmp5 + P(2, 5) * _tmp2 - P(9, 5) * dt;
+  const Scalar _tmp126 = P(0, 5) * _tmp13 + P(1, 5) - P(10, 5) * dt + P(2, 5) * _tmp16;
-  const Scalar _tmp125 = P(0, 5) * _tmp13 + P(1, 5) - P(10, 5) * dt + P(2, 5) * _tmp16;
+  const Scalar _tmp127 = P(0, 5) * _tmp22 + P(1, 5) * _tmp23 - P(11, 5) * dt + P(2, 5);
-  const Scalar _tmp126 = P(0, 5) * _tmp22 + P(1, 5) * _tmp23 - P(11, 5) * dt + P(2, 5);
-  const Scalar _tmp127 = _tmp118 * _tmp83;
   const Scalar _tmp128 = P(0, 5) * _tmp63 + P(1, 5) * _tmp60 - P(12, 5) * _tmp50 -
                          P(13, 5) * _tmp65 - P(14, 5) * _tmp67 + P(2, 5) * _tmp45 + P(3, 5);
   const Scalar _tmp129 = P(0, 5) * _tmp91 + P(1, 5) * _tmp99 - P(12, 5) * _tmp100 -
                          P(13, 5) * _tmp95 - P(14, 5) * _tmp96 + P(2, 5) * _tmp92 + P(4, 5);
-  const Scalar _tmp130 = P(0, 13) * _tmp115 + P(1, 13) * _tmp122 - P(12, 13) * _tmp123 -
+  const Scalar _tmp130 = P(0, 13) * _tmp116 + P(1, 13) * _tmp123 - P(12, 13) * _tmp124 -
-                         P(13, 13) * _tmp119 - P(14, 13) * _tmp117 + P(2, 13) * _tmp120 + P(5, 13);
+                         P(13, 13) * _tmp120 - P(14, 13) * _tmp118 + P(2, 13) * _tmp121 + P(5, 13);
-  const Scalar _tmp131 = P(0, 14) * _tmp115 + P(1, 14) * _tmp122 - P(12, 14) * _tmp123 -
+  const Scalar _tmp131 = P(0, 14) * _tmp116 + P(1, 14) * _tmp123 - P(12, 14) * _tmp124 -
-                         P(13, 14) * _tmp119 - P(14, 14) * _tmp117 + P(2, 14) * _tmp120 + P(5, 14);
+                         P(13, 14) * _tmp120 - P(14, 14) * _tmp118 + P(2, 14) * _tmp121 + P(5, 14);
-  const Scalar _tmp132 = P(0, 12) * _tmp115 + P(1, 12) * _tmp122 - P(12, 12) * _tmp123 -
+  const Scalar _tmp132 = P(0, 12) * _tmp116 + P(1, 12) * _tmp123 - P(12, 12) * _tmp124 -
-                         P(13, 12) * _tmp119 - P(14, 12) * _tmp117 + P(2, 12) * _tmp120 + P(5, 12);
+                         P(13, 12) * _tmp120 - P(14, 12) * _tmp118 + P(2, 12) * _tmp121 + P(5, 12);
-  const Scalar _tmp133 = P(0, 5) * _tmp115 + P(1, 5) * _tmp122 - P(12, 5) * _tmp123 -
+  const Scalar _tmp133 = P(0, 5) * _tmp116 + P(1, 5) * _tmp123 - P(12, 5) * _tmp124 -
-                         P(13, 5) * _tmp119 - P(14, 5) * _tmp117 + P(2, 5) * _tmp120 + P(5, 5);
+                         P(13, 5) * _tmp120 - P(14, 5) * _tmp118 + P(2, 5) * _tmp121 + P(5, 5);
 
   // Output terms (1)
-  matrix::Matrix<Scalar, 23, 23> _res;
+  matrix::Matrix<Scalar, 24, 24> _res;
 
   _res(0, 0) = _tmp10 + _tmp11 + _tmp2 * _tmp6 + _tmp5 * _tmp7 - _tmp8 * dt;
   _res(1, 0) = 0;
@@ -222,6 +222,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 0) = 0;
   _res(21, 0) = 0;
   _res(22, 0) = 0;
+  _res(23, 0) = 0;
   _res(0, 1) = _tmp11 * _tmp13 - _tmp12 * dt + _tmp16 * _tmp6 + _tmp7;
   _res(1, 1) = _tmp10 + _tmp13 * _tmp18 + _tmp16 * _tmp19 - _tmp17 * dt + _tmp20;
   _res(2, 1) = 0;
@@ -245,6 +246,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 1) = 0;
   _res(21, 1) = 0;
   _res(22, 1) = 0;
+  _res(23, 1) = 0;
   _res(0, 2) = _tmp11 * _tmp22 - _tmp21 * dt + _tmp23 * _tmp7 + _tmp6;
   _res(1, 2) = _tmp18 * _tmp22 + _tmp19 + _tmp20 * _tmp23 - _tmp24 * dt;
   _res(2, 2) = _tmp10 + _tmp22 * _tmp27 + _tmp23 * _tmp26 - _tmp25 * dt + _tmp28;
@@ -268,6 +270,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 2) = 0;
   _res(21, 2) = 0;
   _res(22, 2) = 0;
+  _res(23, 2) = 0;
   _res(0, 3) = _tmp11 * _tmp63 + _tmp45 * _tmp6 - _tmp46 * _tmp50 + _tmp60 * _tmp7 -
                _tmp64 * _tmp65 - _tmp66 * _tmp67 + _tmp68;
   _res(1, 3) = _tmp18 * _tmp63 + _tmp19 * _tmp45 + _tmp20 * _tmp60 - _tmp50 * _tmp71 -
@@ -297,17 +300,18 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 3) = 0;
   _res(21, 3) = 0;
   _res(22, 3) = 0;
+  _res(23, 3) = 0;
   _res(0, 4) = -_tmp100 * _tmp46 + _tmp101 + _tmp11 * _tmp91 + _tmp6 * _tmp92 - _tmp64 * _tmp95 -
                _tmp66 * _tmp96 + _tmp7 * _tmp99;
   _res(1, 4) = -_tmp100 * _tmp71 + _tmp102 + _tmp18 * _tmp91 + _tmp19 * _tmp92 + _tmp20 * _tmp99 -
                _tmp69 * _tmp95 - _tmp70 * _tmp96;
   _res(2, 4) = -_tmp100 * _tmp75 - _tmp103 * _tmp94 + _tmp104 + _tmp26 * _tmp99 + _tmp27 * _tmp91 +
                _tmp28 * _tmp92 - _tmp73 * _tmp96;
-  _res(3, 4) = -_tmp100 * _tmp85 + _tmp105 * _tmp49 + _tmp106 * _tmp61 + _tmp107 +
+  _res(3, 4) = -_tmp100 * _tmp85 + _tmp105 * _tmp49 + _tmp106 * _tmp34 + _tmp107 * _tmp61 +
-               _tmp34 * _tmp83 * _tmp94 + _tmp77 * _tmp99 - _tmp78 * _tmp96 - _tmp79 * _tmp95 +
+               _tmp108 + _tmp77 * _tmp99 - _tmp78 * _tmp96 - _tmp79 * _tmp95 + _tmp80 * _tmp91 +
-               _tmp80 * _tmp91 + _tmp82 * _tmp92;
+               _tmp82 * _tmp92;
-  _res(4, 4) = -_tmp100 * _tmp111 - _tmp108 * _tmp96 + _tmp109 * _tmp91 - _tmp110 * _tmp95 +
+  _res(4, 4) = -_tmp100 * _tmp112 - _tmp109 * _tmp96 + _tmp110 * _tmp91 - _tmp111 * _tmp95 +
-               _tmp112 * _tmp99 + _tmp113 * _tmp92 + _tmp114 +
+               _tmp113 * _tmp99 + _tmp114 * _tmp92 + _tmp115 +
                _tmp81 * std::pow(_tmp97, Scalar(2)) + _tmp83 * std::pow(_tmp94, Scalar(2)) +
                _tmp84 * std::pow(_tmp90, Scalar(2));
   _res(5, 4) = 0;
@@ -328,28 +332,29 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 4) = 0;
   _res(21, 4) = 0;
   _res(22, 4) = 0;
-  _res(0, 5) = _tmp11 * _tmp115 - _tmp117 * _tmp66 - _tmp119 * _tmp64 + _tmp120 * _tmp6 +
+  _res(23, 4) = 0;
-               _tmp122 * _tmp7 - _tmp123 * _tmp46 + _tmp124;
+  _res(0, 5) = _tmp11 * _tmp116 - _tmp118 * _tmp66 - _tmp120 * _tmp64 + _tmp121 * _tmp6 +
-  _res(1, 5) = _tmp115 * _tmp18 - _tmp117 * _tmp70 - _tmp119 * _tmp69 + _tmp120 * _tmp19 +
+               _tmp123 * _tmp7 - _tmp124 * _tmp46 + _tmp125;
-               _tmp122 * _tmp20 - _tmp123 * _tmp71 + _tmp125;
+  _res(1, 5) = _tmp116 * _tmp18 - _tmp118 * _tmp70 - _tmp120 * _tmp69 + _tmp121 * _tmp19 +
-  _res(2, 5) = -_tmp103 * _tmp118 + _tmp115 * _tmp27 - _tmp117 * _tmp73 + _tmp120 * _tmp28 +
+               _tmp123 * _tmp20 - _tmp124 * _tmp71 + _tmp126;
-               _tmp122 * _tmp26 - _tmp123 * _tmp75 + _tmp126;
+  _res(2, 5) = -_tmp103 * _tmp119 + _tmp116 * _tmp27 - _tmp118 * _tmp73 + _tmp121 * _tmp28 +
-  _res(3, 5) = _tmp115 * _tmp80 + _tmp116 * _tmp61 * _tmp84 - _tmp117 * _tmp78 - _tmp119 * _tmp79 +
+               _tmp123 * _tmp26 - _tmp124 * _tmp75 + _tmp127;
-               _tmp120 * _tmp82 + _tmp121 * _tmp49 * _tmp81 + _tmp122 * _tmp77 - _tmp123 * _tmp85 +
+  _res(3, 5) = _tmp116 * _tmp80 + _tmp117 * _tmp61 * _tmp84 - _tmp118 * _tmp78 +
-               _tmp127 * _tmp34 + _tmp128;
+               _tmp119 * _tmp34 * _tmp83 - _tmp120 * _tmp79 + _tmp121 * _tmp82 +
-  _res(4, 5) = _tmp105 * _tmp121 + _tmp106 * _tmp116 - _tmp108 * _tmp117 + _tmp109 * _tmp115 -
+               _tmp122 * _tmp49 * _tmp81 + _tmp123 * _tmp77 - _tmp124 * _tmp85 + _tmp128;
-               _tmp110 * _tmp119 - _tmp111 * _tmp123 + _tmp112 * _tmp122 + _tmp113 * _tmp120 +
+  _res(4, 5) = _tmp105 * _tmp122 + _tmp106 * _tmp119 + _tmp107 * _tmp117 - _tmp109 * _tmp118 +
-               _tmp127 * _tmp94 + _tmp129;
+               _tmp110 * _tmp116 - _tmp111 * _tmp120 - _tmp112 * _tmp124 + _tmp113 * _tmp123 +
-  _res(5, 5) = _tmp115 * (P(0, 0) * _tmp115 + P(1, 0) * _tmp122 - P(12, 0) * _tmp123 -
+               _tmp114 * _tmp121 + _tmp129;
-                          P(13, 0) * _tmp119 - P(14, 0) * _tmp117 + P(2, 0) * _tmp120 + P(5, 0)) +
+  _res(5, 5) = _tmp116 * (P(0, 0) * _tmp116 + P(1, 0) * _tmp123 - P(12, 0) * _tmp124 -
-               std::pow(_tmp116, Scalar(2)) * _tmp84 - _tmp117 * _tmp131 +
+                          P(13, 0) * _tmp120 - P(14, 0) * _tmp118 + P(2, 0) * _tmp121 + P(5, 0)) +
-               std::pow(_tmp118, Scalar(2)) * _tmp83 - _tmp119 * _tmp130 +
+               std::pow(_tmp117, Scalar(2)) * _tmp84 - _tmp118 * _tmp131 +
-               _tmp120 * (P(0, 2) * _tmp115 + P(1, 2) * _tmp122 - P(12, 2) * _tmp123 -
+               std::pow(_tmp119, Scalar(2)) * _tmp83 - _tmp120 * _tmp130 +
-                          P(13, 2) * _tmp119 - P(14, 2) * _tmp117 + P(2, 2) * _tmp120 + P(5, 2)) +
+               _tmp121 * (P(0, 2) * _tmp116 + P(1, 2) * _tmp123 - P(12, 2) * _tmp124 -
-               std::pow(_tmp121, Scalar(2)) * _tmp81 +
+                          P(13, 2) * _tmp120 - P(14, 2) * _tmp118 + P(2, 2) * _tmp121 + P(5, 2)) +
-               _tmp122 * (P(0, 1) * _tmp115 + P(1, 1) * _tmp122 - P(12, 1) * _tmp123 -
+               std::pow(_tmp122, Scalar(2)) * _tmp81 +
-                          P(13, 1) * _tmp119 - P(14, 1) * _tmp117 + P(2, 1) * _tmp120 + P(5, 1)) -
+               _tmp123 * (P(0, 1) * _tmp116 + P(1, 1) * _tmp123 - P(12, 1) * _tmp124 -
-               _tmp123 * _tmp132 + _tmp133;
+                          P(13, 1) * _tmp120 - P(14, 1) * _tmp118 + P(2, 1) * _tmp121 + P(5, 1)) -
+               _tmp124 * _tmp132 + _tmp133;
   _res(6, 5) = 0;
   _res(7, 5) = 0;
   _res(8, 5) = 0;
@@ -367,6 +372,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 5) = 0;
   _res(21, 5) = 0;
   _res(22, 5) = 0;
+  _res(23, 5) = 0;
   _res(0, 6) = P(0, 6) + P(1, 6) * _tmp5 + P(2, 6) * _tmp2 - P(9, 6) * dt + _tmp68 * dt;
   _res(1, 6) = P(0, 6) * _tmp13 + P(1, 6) - P(10, 6) * dt + P(2, 6) * _tmp16 + _tmp72 * dt;
   _res(2, 6) = P(0, 6) * _tmp22 + P(1, 6) * _tmp23 - P(11, 6) * dt + P(2, 6) + _tmp76 * dt;
@@ -376,10 +382,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                P(14, 6) * _tmp96 + P(2, 6) * _tmp92 + P(4, 6) +
                dt * (P(0, 3) * _tmp91 + P(1, 3) * _tmp99 - P(12, 3) * _tmp100 - P(13, 3) * _tmp95 -
                      P(14, 3) * _tmp96 + P(2, 3) * _tmp92 + P(4, 3));
-  _res(5, 6) = P(0, 6) * _tmp115 + P(1, 6) * _tmp122 - P(12, 6) * _tmp123 - P(13, 6) * _tmp119 -
+  _res(5, 6) = P(0, 6) * _tmp116 + P(1, 6) * _tmp123 - P(12, 6) * _tmp124 - P(13, 6) * _tmp120 -
-               P(14, 6) * _tmp117 + P(2, 6) * _tmp120 + P(5, 6) +
+               P(14, 6) * _tmp118 + P(2, 6) * _tmp121 + P(5, 6) +
-               dt * (P(0, 3) * _tmp115 + P(1, 3) * _tmp122 - P(12, 3) * _tmp123 -
+               dt * (P(0, 3) * _tmp116 + P(1, 3) * _tmp123 - P(12, 3) * _tmp124 -
-                     P(13, 3) * _tmp119 - P(14, 3) * _tmp117 + P(2, 3) * _tmp120 + P(5, 3));
+                     P(13, 3) * _tmp120 - P(14, 3) * _tmp118 + P(2, 3) * _tmp121 + P(5, 3));
   _res(6, 6) = P(3, 6) * dt + P(6, 6) + dt * (P(3, 3) * dt + P(6, 3));
   _res(7, 6) = 0;
   _res(8, 6) = 0;
@@ -397,17 +403,18 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 6) = 0;
   _res(21, 6) = 0;
   _res(22, 6) = 0;
+  _res(23, 6) = 0;
   _res(0, 7) = P(0, 7) + P(1, 7) * _tmp5 + P(2, 7) * _tmp2 - P(9, 7) * dt + _tmp101 * dt;
   _res(1, 7) = P(0, 7) * _tmp13 + P(1, 7) - P(10, 7) * dt + P(2, 7) * _tmp16 + _tmp102 * dt;
   _res(2, 7) = P(0, 7) * _tmp22 + P(1, 7) * _tmp23 - P(11, 7) * dt + P(2, 7) + _tmp104 * dt;
   _res(3, 7) = P(0, 7) * _tmp63 + P(1, 7) * _tmp60 - P(12, 7) * _tmp50 - P(13, 7) * _tmp65 -
-               P(14, 7) * _tmp67 + P(2, 7) * _tmp45 + P(3, 7) + _tmp107 * dt;
+               P(14, 7) * _tmp67 + P(2, 7) * _tmp45 + P(3, 7) + _tmp108 * dt;
   _res(4, 7) = P(0, 7) * _tmp91 + P(1, 7) * _tmp99 - P(12, 7) * _tmp100 - P(13, 7) * _tmp95 -
-               P(14, 7) * _tmp96 + P(2, 7) * _tmp92 + P(4, 7) + _tmp114 * dt;
+               P(14, 7) * _tmp96 + P(2, 7) * _tmp92 + P(4, 7) + _tmp115 * dt;
-  _res(5, 7) = P(0, 7) * _tmp115 + P(1, 7) * _tmp122 - P(12, 7) * _tmp123 - P(13, 7) * _tmp119 -
+  _res(5, 7) = P(0, 7) * _tmp116 + P(1, 7) * _tmp123 - P(12, 7) * _tmp124 - P(13, 7) * _tmp120 -
-               P(14, 7) * _tmp117 + P(2, 7) * _tmp120 + P(5, 7) +
+               P(14, 7) * _tmp118 + P(2, 7) * _tmp121 + P(5, 7) +
-               dt * (P(0, 4) * _tmp115 + P(1, 4) * _tmp122 - P(12, 4) * _tmp123 -
+               dt * (P(0, 4) * _tmp116 + P(1, 4) * _tmp123 - P(12, 4) * _tmp124 -
-                     P(13, 4) * _tmp119 - P(14, 4) * _tmp117 + P(2, 4) * _tmp120 + P(5, 4));
+                     P(13, 4) * _tmp120 - P(14, 4) * _tmp118 + P(2, 4) * _tmp121 + 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(8, 7) = 0;
@@ -425,15 +432,16 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 7) = 0;
   _res(21, 7) = 0;
   _res(22, 7) = 0;
-  _res(0, 8) = P(0, 8) + P(1, 8) * _tmp5 + P(2, 8) * _tmp2 - P(9, 8) * dt + _tmp124 * dt;
+  _res(23, 7) = 0;
-  _res(1, 8) = P(0, 8) * _tmp13 + P(1, 8) - P(10, 8) * dt + P(2, 8) * _tmp16 + _tmp125 * dt;
+  _res(0, 8) = P(0, 8) + P(1, 8) * _tmp5 + P(2, 8) * _tmp2 - P(9, 8) * dt + _tmp125 * dt;
-  _res(2, 8) = P(0, 8) * _tmp22 + P(1, 8) * _tmp23 - P(11, 8) * dt + P(2, 8) + _tmp126 * dt;
+  _res(1, 8) = P(0, 8) * _tmp13 + P(1, 8) - P(10, 8) * dt + P(2, 8) * _tmp16 + _tmp126 * dt;
+  _res(2, 8) = P(0, 8) * _tmp22 + P(1, 8) * _tmp23 - P(11, 8) * dt + P(2, 8) + _tmp127 * dt;
   _res(3, 8) = P(0, 8) * _tmp63 + P(1, 8) * _tmp60 - P(12, 8) * _tmp50 - P(13, 8) * _tmp65 -
                P(14, 8) * _tmp67 + P(2, 8) * _tmp45 + P(3, 8) + _tmp128 * dt;
   _res(4, 8) = P(0, 8) * _tmp91 + P(1, 8) * _tmp99 - P(12, 8) * _tmp100 - P(13, 8) * _tmp95 -
                P(14, 8) * _tmp96 + P(2, 8) * _tmp92 + P(4, 8) + _tmp129 * dt;
-  _res(5, 8) = P(0, 8) * _tmp115 + P(1, 8) * _tmp122 - P(12, 8) * _tmp123 - P(13, 8) * _tmp119 -
+  _res(5, 8) = P(0, 8) * _tmp116 + P(1, 8) * _tmp123 - P(12, 8) * _tmp124 - P(13, 8) * _tmp120 -
-               P(14, 8) * _tmp117 + P(2, 8) * _tmp120 + P(5, 8) + _tmp133 * dt;
+               P(14, 8) * _tmp118 + P(2, 8) * _tmp121 + P(5, 8) + _tmp133 * 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));
@@ -451,6 +459,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 8) = 0;
   _res(21, 8) = 0;
   _res(22, 8) = 0;
+  _res(23, 8) = 0;
   _res(0, 9) = _tmp8;
   _res(1, 9) = P(0, 9) * _tmp13 + P(1, 9) - P(10, 9) * dt + P(2, 9) * _tmp16;
   _res(2, 9) = P(0, 9) * _tmp22 + P(1, 9) * _tmp23 - P(11, 9) * dt + P(2, 9);
@@ -458,8 +467,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                P(14, 9) * _tmp67 + P(2, 9) * _tmp45 + P(3, 9);
   _res(4, 9) = P(0, 9) * _tmp91 + P(1, 9) * _tmp99 - P(12, 9) * _tmp100 - P(13, 9) * _tmp95 -
                P(14, 9) * _tmp96 + P(2, 9) * _tmp92 + P(4, 9);
-  _res(5, 9) = P(0, 9) * _tmp115 + P(1, 9) * _tmp122 - P(12, 9) * _tmp123 - P(13, 9) * _tmp119 -
+  _res(5, 9) = P(0, 9) * _tmp116 + P(1, 9) * _tmp123 - P(12, 9) * _tmp124 - P(13, 9) * _tmp120 -
-               P(14, 9) * _tmp117 + P(2, 9) * _tmp120 + P(5, 9);
+               P(14, 9) * _tmp118 + P(2, 9) * _tmp121 + 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);
@@ -477,6 +486,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 9) = 0;
   _res(21, 9) = 0;
   _res(22, 9) = 0;
+  _res(23, 9) = 0;
   _res(0, 10) = _tmp12;
   _res(1, 10) = _tmp17;
   _res(2, 10) = P(0, 10) * _tmp22 + P(1, 10) * _tmp23 - P(11, 10) * dt + P(2, 10);
@@ -484,8 +494,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                 P(14, 10) * _tmp67 + P(2, 10) * _tmp45 + P(3, 10);
   _res(4, 10) = P(0, 10) * _tmp91 + P(1, 10) * _tmp99 - P(12, 10) * _tmp100 - P(13, 10) * _tmp95 -
                 P(14, 10) * _tmp96 + P(2, 10) * _tmp92 + P(4, 10);
-  _res(5, 10) = P(0, 10) * _tmp115 + P(1, 10) * _tmp122 - P(12, 10) * _tmp123 -
+  _res(5, 10) = P(0, 10) * _tmp116 + P(1, 10) * _tmp123 - P(12, 10) * _tmp124 -
-                P(13, 10) * _tmp119 - P(14, 10) * _tmp117 + P(2, 10) * _tmp120 + P(5, 10);
+                P(13, 10) * _tmp120 - P(14, 10) * _tmp118 + P(2, 10) * _tmp121 + 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);
@@ -503,6 +513,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 10) = 0;
   _res(21, 10) = 0;
   _res(22, 10) = 0;
+  _res(23, 10) = 0;
   _res(0, 11) = _tmp21;
   _res(1, 11) = _tmp24;
   _res(2, 11) = _tmp25;
@@ -510,8 +521,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                 P(14, 11) * _tmp67 + P(2, 11) * _tmp45 + P(3, 11);
   _res(4, 11) = P(0, 11) * _tmp91 + P(1, 11) * _tmp99 - P(12, 11) * _tmp100 - P(13, 11) * _tmp95 -
                 P(14, 11) * _tmp96 + P(2, 11) * _tmp92 + P(4, 11);
-  _res(5, 11) = P(0, 11) * _tmp115 + P(1, 11) * _tmp122 - P(12, 11) * _tmp123 -
+  _res(5, 11) = P(0, 11) * _tmp116 + P(1, 11) * _tmp123 - P(12, 11) * _tmp124 -
-                P(13, 11) * _tmp119 - P(14, 11) * _tmp117 + P(2, 11) * _tmp120 + P(5, 11);
+                P(13, 11) * _tmp120 - P(14, 11) * _tmp118 + P(2, 11) * _tmp121 + 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);
@@ -529,11 +540,12 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 11) = 0;
   _res(21, 11) = 0;
   _res(22, 11) = 0;
+  _res(23, 11) = 0;
   _res(0, 12) = _tmp46;
   _res(1, 12) = _tmp71;
   _res(2, 12) = _tmp75;
   _res(3, 12) = _tmp85;
-  _res(4, 12) = _tmp111;
+  _res(4, 12) = _tmp112;
   _res(5, 12) = _tmp132;
   _res(6, 12) = P(3, 12) * dt + P(6, 12);
   _res(7, 12) = P(4, 12) * dt + P(7, 12);
@@ -552,11 +564,12 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 12) = 0;
   _res(21, 12) = 0;
   _res(22, 12) = 0;
+  _res(23, 12) = 0;
   _res(0, 13) = _tmp64;
   _res(1, 13) = _tmp69;
   _res(2, 13) = _tmp74;
   _res(3, 13) = _tmp79;
-  _res(4, 13) = _tmp110;
+  _res(4, 13) = _tmp111;
   _res(5, 13) = _tmp130;
   _res(6, 13) = P(3, 13) * dt + P(6, 13);
   _res(7, 13) = P(4, 13) * dt + P(7, 13);
@@ -575,11 +588,12 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 13) = 0;
   _res(21, 13) = 0;
   _res(22, 13) = 0;
+  _res(23, 13) = 0;
   _res(0, 14) = _tmp66;
   _res(1, 14) = _tmp70;
   _res(2, 14) = _tmp73;
   _res(3, 14) = _tmp78;
-  _res(4, 14) = _tmp108;
+  _res(4, 14) = _tmp109;
   _res(5, 14) = _tmp131;
   _res(6, 14) = P(3, 14) * dt + P(6, 14);
   _res(7, 14) = P(4, 14) * dt + P(7, 14);
@@ -598,6 +612,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 14) = 0;
   _res(21, 14) = 0;
   _res(22, 14) = 0;
+  _res(23, 14) = 0;
   _res(0, 15) = P(0, 15) + P(1, 15) * _tmp5 + P(2, 15) * _tmp2 - P(9, 15) * dt;
   _res(1, 15) = P(0, 15) * _tmp13 + P(1, 15) - P(10, 15) * dt + P(2, 15) * _tmp16;
   _res(2, 15) = P(0, 15) * _tmp22 + P(1, 15) * _tmp23 - P(11, 15) * dt + P(2, 15);
@@ -605,8 +620,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                 P(14, 15) * _tmp67 + P(2, 15) * _tmp45 + P(3, 15);
   _res(4, 15) = P(0, 15) * _tmp91 + P(1, 15) * _tmp99 - P(12, 15) * _tmp100 - P(13, 15) * _tmp95 -
                 P(14, 15) * _tmp96 + P(2, 15) * _tmp92 + P(4, 15);
-  _res(5, 15) = P(0, 15) * _tmp115 + P(1, 15) * _tmp122 - P(12, 15) * _tmp123 -
+  _res(5, 15) = P(0, 15) * _tmp116 + P(1, 15) * _tmp123 - P(12, 15) * _tmp124 -
-                P(13, 15) * _tmp119 - P(14, 15) * _tmp117 + P(2, 15) * _tmp120 + P(5, 15);
+                P(13, 15) * _tmp120 - P(14, 15) * _tmp118 + P(2, 15) * _tmp121 + 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);
@@ -624,6 +639,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 15) = 0;
   _res(21, 15) = 0;
   _res(22, 15) = 0;
+  _res(23, 15) = 0;
   _res(0, 16) = P(0, 16) + P(1, 16) * _tmp5 + P(2, 16) * _tmp2 - P(9, 16) * dt;
   _res(1, 16) = P(0, 16) * _tmp13 + P(1, 16) - P(10, 16) * dt + P(2, 16) * _tmp16;
   _res(2, 16) = P(0, 16) * _tmp22 + P(1, 16) * _tmp23 - P(11, 16) * dt + P(2, 16);
@@ -631,8 +647,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                 P(14, 16) * _tmp67 + P(2, 16) * _tmp45 + P(3, 16);
   _res(4, 16) = P(0, 16) * _tmp91 + P(1, 16) * _tmp99 - P(12, 16) * _tmp100 - P(13, 16) * _tmp95 -
                 P(14, 16) * _tmp96 + P(2, 16) * _tmp92 + P(4, 16);
-  _res(5, 16) = P(0, 16) * _tmp115 + P(1, 16) * _tmp122 - P(12, 16) * _tmp123 -
+  _res(5, 16) = P(0, 16) * _tmp116 + P(1, 16) * _tmp123 - P(12, 16) * _tmp124 -
-                P(13, 16) * _tmp119 - P(14, 16) * _tmp117 + P(2, 16) * _tmp120 + P(5, 16);
+                P(13, 16) * _tmp120 - P(14, 16) * _tmp118 + P(2, 16) * _tmp121 + 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);
@@ -650,6 +666,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 16) = 0;
   _res(21, 16) = 0;
   _res(22, 16) = 0;
+  _res(23, 16) = 0;
   _res(0, 17) = P(0, 17) + P(1, 17) * _tmp5 + P(2, 17) * _tmp2 - P(9, 17) * dt;
   _res(1, 17) = P(0, 17) * _tmp13 + P(1, 17) - P(10, 17) * dt + P(2, 17) * _tmp16;
   _res(2, 17) = P(0, 17) * _tmp22 + P(1, 17) * _tmp23 - P(11, 17) * dt + P(2, 17);
@@ -657,8 +674,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                 P(14, 17) * _tmp67 + P(2, 17) * _tmp45 + P(3, 17);
   _res(4, 17) = P(0, 17) * _tmp91 + P(1, 17) * _tmp99 - P(12, 17) * _tmp100 - P(13, 17) * _tmp95 -
                 P(14, 17) * _tmp96 + P(2, 17) * _tmp92 + P(4, 17);
-  _res(5, 17) = P(0, 17) * _tmp115 + P(1, 17) * _tmp122 - P(12, 17) * _tmp123 -
+  _res(5, 17) = P(0, 17) * _tmp116 + P(1, 17) * _tmp123 - P(12, 17) * _tmp124 -
-                P(13, 17) * _tmp119 - P(14, 17) * _tmp117 + P(2, 17) * _tmp120 + P(5, 17);
+                P(13, 17) * _tmp120 - P(14, 17) * _tmp118 + P(2, 17) * _tmp121 + 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);
@@ -676,6 +693,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 17) = 0;
   _res(21, 17) = 0;
   _res(22, 17) = 0;
+  _res(23, 17) = 0;
   _res(0, 18) = P(0, 18) + P(1, 18) * _tmp5 + P(2, 18) * _tmp2 - P(9, 18) * dt;
   _res(1, 18) = P(0, 18) * _tmp13 + P(1, 18) - P(10, 18) * dt + P(2, 18) * _tmp16;
   _res(2, 18) = P(0, 18) * _tmp22 + P(1, 18) * _tmp23 - P(11, 18) * dt + P(2, 18);
@@ -683,8 +701,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                 P(14, 18) * _tmp67 + P(2, 18) * _tmp45 + P(3, 18);
   _res(4, 18) = P(0, 18) * _tmp91 + P(1, 18) * _tmp99 - P(12, 18) * _tmp100 - P(13, 18) * _tmp95 -
                 P(14, 18) * _tmp96 + P(2, 18) * _tmp92 + P(4, 18);
-  _res(5, 18) = P(0, 18) * _tmp115 + P(1, 18) * _tmp122 - P(12, 18) * _tmp123 -
+  _res(5, 18) = P(0, 18) * _tmp116 + P(1, 18) * _tmp123 - P(12, 18) * _tmp124 -
-                P(13, 18) * _tmp119 - P(14, 18) * _tmp117 + P(2, 18) * _tmp120 + P(5, 18);
+                P(13, 18) * _tmp120 - P(14, 18) * _tmp118 + P(2, 18) * _tmp121 + 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);
@@ -702,6 +720,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 18) = 0;
   _res(21, 18) = 0;
   _res(22, 18) = 0;
+  _res(23, 18) = 0;
   _res(0, 19) = P(0, 19) + P(1, 19) * _tmp5 + P(2, 19) * _tmp2 - P(9, 19) * dt;
   _res(1, 19) = P(0, 19) * _tmp13 + P(1, 19) - P(10, 19) * dt + P(2, 19) * _tmp16;
   _res(2, 19) = P(0, 19) * _tmp22 + P(1, 19) * _tmp23 - P(11, 19) * dt + P(2, 19);
@@ -709,8 +728,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                 P(14, 19) * _tmp67 + P(2, 19) * _tmp45 + P(3, 19);
   _res(4, 19) = P(0, 19) * _tmp91 + P(1, 19) * _tmp99 - P(12, 19) * _tmp100 - P(13, 19) * _tmp95 -
                 P(14, 19) * _tmp96 + P(2, 19) * _tmp92 + P(4, 19);
-  _res(5, 19) = P(0, 19) * _tmp115 + P(1, 19) * _tmp122 - P(12, 19) * _tmp123 -
+  _res(5, 19) = P(0, 19) * _tmp116 + P(1, 19) * _tmp123 - P(12, 19) * _tmp124 -
-                P(13, 19) * _tmp119 - P(14, 19) * _tmp117 + P(2, 19) * _tmp120 + P(5, 19);
+                P(13, 19) * _tmp120 - P(14, 19) * _tmp118 + P(2, 19) * _tmp121 + 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);
@@ -728,6 +747,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 19) = 0;
   _res(21, 19) = 0;
   _res(22, 19) = 0;
+  _res(23, 19) = 0;
   _res(0, 20) = P(0, 20) + P(1, 20) * _tmp5 + P(2, 20) * _tmp2 - P(9, 20) * dt;
   _res(1, 20) = P(0, 20) * _tmp13 + P(1, 20) - P(10, 20) * dt + P(2, 20) * _tmp16;
   _res(2, 20) = P(0, 20) * _tmp22 + P(1, 20) * _tmp23 - P(11, 20) * dt + P(2, 20);
@@ -735,8 +755,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                 P(14, 20) * _tmp67 + P(2, 20) * _tmp45 + P(3, 20);
   _res(4, 20) = P(0, 20) * _tmp91 + P(1, 20) * _tmp99 - P(12, 20) * _tmp100 - P(13, 20) * _tmp95 -
                 P(14, 20) * _tmp96 + P(2, 20) * _tmp92 + P(4, 20);
-  _res(5, 20) = P(0, 20) * _tmp115 + P(1, 20) * _tmp122 - P(12, 20) * _tmp123 -
+  _res(5, 20) = P(0, 20) * _tmp116 + P(1, 20) * _tmp123 - P(12, 20) * _tmp124 -
-                P(13, 20) * _tmp119 - P(14, 20) * _tmp117 + P(2, 20) * _tmp120 + P(5, 20);
+                P(13, 20) * _tmp120 - P(14, 20) * _tmp118 + P(2, 20) * _tmp121 + 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);
@@ -754,6 +774,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 20) = P(20, 20);
   _res(21, 20) = 0;
   _res(22, 20) = 0;
+  _res(23, 20) = 0;
   _res(0, 21) = P(0, 21) + P(1, 21) * _tmp5 + P(2, 21) * _tmp2 - P(9, 21) * dt;
   _res(1, 21) = P(0, 21) * _tmp13 + P(1, 21) - P(10, 21) * dt + P(2, 21) * _tmp16;
   _res(2, 21) = P(0, 21) * _tmp22 + P(1, 21) * _tmp23 - P(11, 21) * dt + P(2, 21);
@@ -761,8 +782,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                 P(14, 21) * _tmp67 + P(2, 21) * _tmp45 + P(3, 21);
   _res(4, 21) = P(0, 21) * _tmp91 + P(1, 21) * _tmp99 - P(12, 21) * _tmp100 - P(13, 21) * _tmp95 -
                 P(14, 21) * _tmp96 + P(2, 21) * _tmp92 + P(4, 21);
-  _res(5, 21) = P(0, 21) * _tmp115 + P(1, 21) * _tmp122 - P(12, 21) * _tmp123 -
+  _res(5, 21) = P(0, 21) * _tmp116 + P(1, 21) * _tmp123 - P(12, 21) * _tmp124 -
-                P(13, 21) * _tmp119 - P(14, 21) * _tmp117 + P(2, 21) * _tmp120 + P(5, 21);
+                P(13, 21) * _tmp120 - P(14, 21) * _tmp118 + P(2, 21) * _tmp121 + 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);
@@ -780,6 +801,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
   _res(20, 21) = P(20, 21);
   _res(21, 21) = P(21, 21);
   _res(22, 21) = 0;
+  _res(23, 21) = 0;
   _res(0, 22) = P(0, 22) + P(1, 22) * _tmp5 + P(2, 22) * _tmp2 - P(9, 22) * dt;
   _res(1, 22) = P(0, 22) * _tmp13 + P(1, 22) - P(10, 22) * dt + P(2, 22) * _tmp16;
   _res(2, 22) = P(0, 22) * _tmp22 + P(1, 22) * _tmp23 - P(11, 22) * dt + P(2, 22);
@@ -787,8 +809,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                 P(14, 22) * _tmp67 + P(2, 22) * _tmp45 + P(3, 22);
   _res(4, 22) = P(0, 22) * _tmp91 + P(1, 22) * _tmp99 - P(12, 22) * _tmp100 - P(13, 22) * _tmp95 -
                 P(14, 22) * _tmp96 + P(2, 22) * _tmp92 + P(4, 22);
-  _res(5, 22) = P(0, 22) * _tmp115 + P(1, 22) * _tmp122 - P(12, 22) * _tmp123 -
+  _res(5, 22) = P(0, 22) * _tmp116 + P(1, 22) * _tmp123 - P(12, 22) * _tmp124 -
-                P(13, 22) * _tmp119 - P(14, 22) * _tmp117 + P(2, 22) * _tmp120 + P(5, 22);
+                P(13, 22) * _tmp120 - P(14, 22) * _tmp118 + P(2, 22) * _tmp121 + 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);
@@ -806,6 +828,34 @@ 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(23, 22) = 0;
+  _res(0, 23) = P(0, 23) + P(1, 23) * _tmp5 + P(2, 23) * _tmp2 - P(9, 23) * dt;
+  _res(1, 23) = P(0, 23) * _tmp13 + P(1, 23) - P(10, 23) * dt + P(2, 23) * _tmp16;
+  _res(2, 23) = P(0, 23) * _tmp22 + P(1, 23) * _tmp23 - P(11, 23) * dt + P(2, 23);
+  _res(3, 23) = P(0, 23) * _tmp63 + P(1, 23) * _tmp60 - P(12, 23) * _tmp50 - P(13, 23) * _tmp65 -
+                P(14, 23) * _tmp67 + P(2, 23) * _tmp45 + P(3, 23);
+  _res(4, 23) = P(0, 23) * _tmp91 + P(1, 23) * _tmp99 - P(12, 23) * _tmp100 - P(13, 23) * _tmp95 -
+                P(14, 23) * _tmp96 + P(2, 23) * _tmp92 + P(4, 23);
+  _res(5, 23) = P(0, 23) * _tmp116 + P(1, 23) * _tmp123 - P(12, 23) * _tmp124 -
+                P(13, 23) * _tmp120 - P(14, 23) * _tmp118 + P(2, 23) * _tmp121 + 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_vpos{};
 
-	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_vpos;
 		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_vpos{23, 1};
+	static constexpr uint8_t size{24};
 };
 }
 #endif // !EKF_STATE_H

src/modules/ekf2/EKF/range_height_control.cpp

@@ -112,17 +112,8 @@ void Ekf::controlRangeHeightFusion()
 						   innov_gate,
 						   aid_src);
 
-		// 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 == RngCtrl::CONDITIONAL) && isConditionalRangeAidSuitable();
+		const bool continuing_conditions_passing = (_params.rng_ctrl == RngCtrl::ENABLED);
-		const bool continuing_conditions_passing = ((_params.rng_ctrl == RngCtrl::ENABLED) || do_conditional_range_aid)
 				&& measurement_valid
 				&& _range_sensor.isDataHealthy();
 
@@ -142,8 +133,8 @@ void Ekf::controlRangeHeightFusion()
 					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(-(measurement - _state.terrain_vpos));
-					bias_est.setBias(_state.pos(2) + measurement);
+					_state.terrain_vpos = _state.pos(2) + measurement;
 
 					// reset vertical velocity
 					resetVerticalVelocityToZero();
@@ -200,47 +191,6 @@ void Ekf::controlRangeHeightFusion()
 	}
 }
 
-bool Ekf::isConditionalRangeAidSuitable()
-{
-#if defined(CONFIG_EKF2_TERRAIN)
-	if (_control_status.flags.in_air
-	    && _range_sensor.isHealthy()
-	    && isTerrainEstimateValid()) {
-		// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
-		// 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_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);
-
-		if (!_control_status.flags.rng_hgt) {
-			range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
-			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;
-	}
-#endif // CONFIG_EKF2_TERRAIN
-
-	return false;
-}
-
 void Ekf::stopRngHgtFusion()
 {
 	if (_control_status.flags.rng_hgt) {

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -45,11 +45,6 @@
 
 void Ekf::initHagl()
 {
-	// assume a ground clearance
-	_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
-
-	// use the ground clearance value as our uncertainty
-	_terrain_var = sq(_params.rng_gnd_clearance);
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	_aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us;
@@ -73,38 +68,6 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
 		// as an overestimated distance leads to an overestimated velocity, causing a dangerous behavior.
 		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)
@@ -183,13 +146,6 @@ void Ekf::controlHaglRngFusion()
 	}
 }
 
-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();
@@ -343,81 +299,6 @@ void Ekf::resetHaglFlow()
 	_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;
-	}
-
-	// run the innovation consistency check and record result
-	setEstimatorAidStatusTestRatio(flow, math::max(_params.flow_innov_gate, 1.f));
-
-	_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
@@ -467,7 +348,3 @@ bool Ekf::isTerrainEstimateValid() const
 
 	return valid;
 }
-
-void Ekf::terrainHandleVerticalPositionReset(const float delta_z) {
-	_terrain_vpos += delta_z;
-}