flow: add distance exponent factor

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[26] states		# Internal filter states
 uint8 n_states		# Number of states effectively used
 
-float32[23] covariances	# Diagonal Elements of Covariance Matrix
+float32[25] covariances	# Diagonal Elements of Covariance Matrix

src/drivers/optical_flow/paw3902/PAW3902.cpp

@@ -111,7 +111,7 @@ void PAW3902::print_status()
 	perf_print_counter(_mode_change_bright_perf);
 	perf_print_counter(_mode_change_low_light_perf);
 	perf_print_counter(_mode_change_super_low_light_perf);
-	perf_print_counter(_no_motion_interrupt_perf);
+	PX4_INFO_RAW("Resolution %" PRIu8 "\n", _resolution);
 }
 
 int PAW3902::probe()
@@ -496,6 +496,8 @@ void PAW3902::RunImpl()
 					Reset();
 				}
 			}
+
+			_resolution = RegisterRead(Register::Resolution);
 		}
 
 		break;

src/drivers/optical_flow/paw3902/PAW3902.hpp

@@ -122,6 +122,8 @@ private:
 	uint8_t _quality_prev{0};
 	uint8_t _raw_data_sum_prev{0};
 
+	uint8_t _resolution{0};
+
 	int _failure_count{0};
 	int _discard_reading{0};
 

src/modules/ekf2/EKF/covariance.cpp

@@ -99,6 +99,8 @@ void Ekf::initialiseCovariance()
 #if defined(CONFIG_EKF2_WIND)
 	resetWindCov();
 #endif // CONFIG_EKF2_WIND
+	P(State::flow_scale.idx, State::flow_scale.idx) = sq(0.1f);
+	P(State::flow_exp.idx, State::flow_exp.idx) = sq(0.1f);
 }
 
 void Ekf::predictCovariance(const imuSample &imu_delayed)
@@ -206,6 +208,12 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	}
 #endif // CONFIG_EKF2_WIND
 
+	if (_control_status.flags.opt_flow) {
+		const float flow_scale_nsd = 0.1f;
+		P(State::flow_scale.idx, State::flow_scale.idx) += sq(flow_scale_nsd) * dt;
+		P(State::flow_exp.idx, State::flow_exp.idx) += sq(flow_scale_nsd) * dt;
+	}
+
 	// 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++) {

src/modules/ekf2/EKF/ekf.cpp

@@ -71,6 +71,8 @@ void Ekf::reset()
 #if defined(CONFIG_EKF2_WIND)
 	_state.wind_vel.setZero();
 #endif // CONFIG_EKF2_WIND
+	_state.flow_scale = 1.f;
+	_state.flow_exp = 1.f;
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -774,6 +774,8 @@ 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.flow_scale -= (K.slice<State::flow_scale.dof, 1>(State::flow_scale.idx, 0) * innovation)(0, 0);
+	_state.flow_exp -= (K.slice<State::flow_exp.dof, 1>(State::flow_exp.idx, 0) * innovation)(0, 0);
 }
 
 void Ekf::uncorrelateQuatFromOtherStates()

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -368,6 +368,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 
 		flowSample optflow_sample_new{flow};
 		optflow_sample_new.time_us = time_us;
+		optflow_sample_new.flow_xy_rad *= 0.82f;
 
 		_flow_buffer->push(optflow_sample_new);
 

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -61,16 +61,17 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 	// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
 	const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt;
 
+	const float range_scaled = std::pow(range, _state.flow_exp);
 	// compute the velocities in body and local frames from corrected optical flow measurement for logging only
-	_flow_vel_body(0) = -opt_flow_rate(1) * range;
+	_flow_vel_body(0) = -opt_flow_rate(1) * range_scaled / _state.flow_scale;
-	_flow_vel_body(1) =  opt_flow_rate(0) * range;
+	_flow_vel_body(1) =  opt_flow_rate(0) * range_scaled / _state.flow_scale;
 	_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
 
 	aid_src.observation[0] = opt_flow_rate(0); // flow around the X axis
 	aid_src.observation[1] = opt_flow_rate(1); // flow around the Y axis
 
-	aid_src.innovation[0] =  (vel_body(1) / range) - aid_src.observation[0];
+	aid_src.innovation[0] = _state.flow_scale * (vel_body(1) / range_scaled) - aid_src.observation[0];
-	aid_src.innovation[1] = (-vel_body(0) / range) - aid_src.observation[1];
+	aid_src.innovation[1] = _state.flow_scale * (-vel_body(0) / range_scaled) - aid_src.observation[1];
 
 	// calculate the optical flow observation variance
 	const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
@@ -134,7 +135,7 @@ void Ekf::fuseOptFlow()
 			// recalculate the innovation using the updated state
 			const Vector2f vel_body = predictFlowVelBody();
 			range = predictFlowRange();
-			_aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1];
+			_aid_src_optical_flow.innovation[1] = _state.flow_scale * (-vel_body(0) / std::pow(range, _state.flow_exp)) - _aid_src_optical_flow.observation[1];
 
 			if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
 				// we need to reinitialise the covariance matrix and abort this fusion step

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

@@ -62,7 +62,9 @@ State = Values(
     accel_bias = sf.V3(),
     mag_I = sf.V3(),
     mag_B = sf.V3(),
-    wind_vel = sf.V2()
+    wind_vel = sf.V2(),
+    flow_scale = sf.V1(),
+    flow_exp = sf.V1()
 )
 
 if args.disable_mag:
@@ -127,7 +129,9 @@ 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"),
+        flow_scale = sf.V1.symbolic("flow_scale"),
+        flow_exp = sf.V1.symbolic("flow_exp")
     )
 
     if args.disable_mag:
@@ -471,9 +475,11 @@ def predict_opt_flow(state, distance, epsilon):
     # Divide by range to get predicted angular LOS rates relative to X and Y
     # axes. Note these are rates in a non-rotating sensor frame
     flow_pred = sf.V2()
-    flow_pred[0] =  rel_vel_sensor[1] / distance
+    dist_scaled = distance**state["flow_exp"][0]
-    flow_pred[1] = -rel_vel_sensor[0] / distance
+    flow_pred[0] =  rel_vel_sensor[1] / dist_scaled
-    flow_pred = add_epsilon_sign(flow_pred, distance, epsilon)
+    flow_pred[1] = -rel_vel_sensor[0] / dist_scaled
+    flow_pred *= state["flow_scale"]
+    flow_pred = add_epsilon_sign(flow_pred, dist_scaled, epsilon)
 
     return flow_pred
 

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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     innov_var: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
- *     H: Matrix23_1
+ *     H: Matrix25_1
- *     K: Matrix23_1
+ *     K: Matrix25_1
  */
 template <typename Scalar>
-void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 26, 1>& state,
-                          const matrix::Matrix<Scalar, 23, 23>& P, const Scalar innov_var,
+                          const matrix::Matrix<Scalar, 25, 25>& P, const Scalar innov_var,
-                          const Scalar epsilon, matrix::Matrix<Scalar, 23, 1>* const H = nullptr,
+                          const Scalar epsilon, matrix::Matrix<Scalar, 25, 1>* const H = nullptr,
-                          matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
+                          matrix::Matrix<Scalar, 25, 1>* const K = nullptr) {
-  // Total ops: 246
+  // Total ops: 266
 
   // 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, 25, 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, 25, 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,10 @@ 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);
+    _k(24, 0) = _tmp6 * (-P(24, 21) * _tmp3 - P(24, 22) * _tmp4 + P(24, 3) * _tmp3 +
+                         P(24, 4) * _tmp4 + P(24, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     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, 26, 1>& state,
-                                     const matrix::Matrix<Scalar, 23, 23>& P, const Scalar airspeed,
+                                     const matrix::Matrix<Scalar, 25, 25>& 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     rho: Scalar
  *     cd: Scalar
  *     cm: Scalar
@@ -26,14 +26,14 @@ namespace sym {
  *
  * Outputs:
  *     innov_var: Scalar
- *     K: Matrix23_1
+ *     K: Matrix25_1
  */
 template <typename Scalar>
-void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 26, 1>& state,
-                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
+                              const matrix::Matrix<Scalar, 25, 25>& 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, 25, 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, 25, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     rho: Scalar
  *     cd: Scalar
  *     cm: Scalar
@@ -26,14 +26,14 @@ namespace sym {
  *
  * Outputs:
  *     innov_var: Scalar
- *     K: Matrix23_1
+ *     K: Matrix25_1
  */
 template <typename Scalar>
-void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 26, 1>& state,
-                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
+                              const matrix::Matrix<Scalar, 25, 25>& 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, 25, 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, 25, 1>& _k = (*K);
 
     _k.setZero();
 

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

@@ -16,105 +16,130 @@ namespace sym {
  * Symbolic function: compute_flow_xy_innov_var_and_hx
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     distance: Scalar
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Matrix21
- *     H: Matrix23_1
+ *     H: Matrix25_1
  */
 template <typename Scalar>
-void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 26, 1>& state,
-                                const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
+                                const matrix::Matrix<Scalar, 25, 25>& P, const Scalar distance,
                                 const Scalar R, const Scalar epsilon,
                                 matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
-                                matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                                matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
-  // Total ops: 196
+  // Total ops: 302
+
+  // Unused inputs
+  (void)epsilon;
 
   // Input arrays
 
-  // Intermediate terms (33)
+  // Intermediate terms (45)
   const Scalar _tmp0 = std::pow(state(1, 0), Scalar(2));
   const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2));
   const Scalar _tmp2 = 1 - 2 * _tmp1;
-  const Scalar _tmp3 =
+  const Scalar _tmp3 = -2 * _tmp0 + _tmp2;
-      Scalar(1.0) /
+  const Scalar _tmp4 = std::pow(distance, Scalar(-state(25, 0)));
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
+  const Scalar _tmp5 = _tmp4 * state(24, 0);
-  const Scalar _tmp4 = _tmp3 * (-2 * _tmp0 + _tmp2);
+  const Scalar _tmp6 = _tmp3 * _tmp5;
-  const Scalar _tmp5 = 2 * state(2, 0);
+  const Scalar _tmp7 = 2 * state(0, 0);
-  const Scalar _tmp6 = _tmp5 * state(0, 0);
+  const Scalar _tmp8 = _tmp7 * state(2, 0);
-  const Scalar _tmp7 = 2 * state(1, 0) * state(3, 0);
+  const Scalar _tmp9 = 2 * state(1, 0);
-  const Scalar _tmp8 = _tmp5 * state(3, 0);
+  const Scalar _tmp10 = _tmp9 * state(3, 0);
-  const Scalar _tmp9 = 2 * state(0, 0);
+  const Scalar _tmp11 = 2 * state(2, 0) * state(3, 0);
-  const Scalar _tmp10 = _tmp9 * state(1, 0);
+  const Scalar _tmp12 = _tmp9 * state(0, 0);
-  const Scalar _tmp11 = std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp13 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2));
+  const Scalar _tmp14 = std::pow(state(0, 0), Scalar(2));
-  const Scalar _tmp13 = -_tmp0;
+  const Scalar _tmp15 = -_tmp0;
-  const Scalar _tmp14 = _tmp12 + _tmp13;
+  const Scalar _tmp16 = _tmp14 + _tmp15;
-  const Scalar _tmp15 = _tmp3 * (state(4, 0) * (_tmp6 + _tmp7) + state(5, 0) * (-_tmp10 + _tmp8) +
+  const Scalar _tmp17 = _tmp5 * (state(4, 0) * (_tmp10 + _tmp8) + state(5, 0) * (_tmp11 - _tmp12) +
-                                 state(6, 0) * (_tmp1 - _tmp11 + _tmp14));
+                                 state(6, 0) * (_tmp1 - _tmp13 + _tmp16));
-  const Scalar _tmp16 = _tmp9 * state(3, 0);
+  const Scalar _tmp18 = std::log(distance);
-  const Scalar _tmp17 = -_tmp16;
+  const Scalar _tmp19 = _tmp7 * state(3, 0);
-  const Scalar _tmp18 = _tmp5 * state(1, 0);
+  const Scalar _tmp20 = -_tmp19;
-  const Scalar _tmp19 = _tmp17 + _tmp18;
+  const Scalar _tmp21 = _tmp9 * state(2, 0);
-  const Scalar _tmp20 = _tmp19 * _tmp3;
+  const Scalar _tmp22 = _tmp20 + _tmp21;
-  const Scalar _tmp21 = _tmp10 + _tmp8;
+  const Scalar _tmp23 = _tmp11 + _tmp12;
-  const Scalar _tmp22 = _tmp21 * _tmp3;
+  const Scalar _tmp24 = _tmp22 * state(4, 0) + _tmp23 * state(6, 0);
-  const Scalar _tmp23 = -_tmp7;
+  const Scalar _tmp25 = _tmp4 * (_tmp24 + _tmp3 * state(5, 0));
-  const Scalar _tmp24 = -_tmp12;
+  const Scalar _tmp26 = _tmp18 * _tmp25 * state(24, 0);
-  const Scalar _tmp25 = _tmp3 * (state(4, 0) * (_tmp1 + _tmp11 + _tmp13 + _tmp24) +
+  const Scalar _tmp27 = _tmp22 * _tmp5;
-                                 state(5, 0) * (_tmp17 - _tmp18) + state(6, 0) * (_tmp23 + _tmp6));
+  const Scalar _tmp28 = -_tmp10;
-  const Scalar _tmp26 = _tmp3 * (-2 * _tmp11 + _tmp2);
+  const Scalar _tmp29 = -_tmp14;
-  const Scalar _tmp27 = -_tmp6;
+  const Scalar _tmp30 = _tmp5 * (state(4, 0) * (_tmp1 + _tmp13 + _tmp15 + _tmp29) +
-  const Scalar _tmp28 = -_tmp1 + _tmp11;
+                                 state(5, 0) * (_tmp20 - _tmp21) + state(6, 0) * (_tmp28 + _tmp8));
-  const Scalar _tmp29 = _tmp3 * (state(4, 0) * (_tmp23 + _tmp27) + state(5, 0) * (_tmp10 - _tmp8) +
+  const Scalar _tmp31 = _tmp23 * _tmp5;
-                                 state(6, 0) * (_tmp0 + _tmp24 + _tmp28));
+  const Scalar _tmp32 = -_tmp8;
-  const Scalar _tmp30 =
+  const Scalar _tmp33 = -_tmp1 + _tmp13;
-      _tmp3 * (_tmp19 * state(4, 0) + _tmp21 * state(6, 0) + state(5, 0) * (_tmp14 + _tmp28));
+  const Scalar _tmp34 =
-  const Scalar _tmp31 = _tmp3 * (_tmp16 + _tmp18);
+      _tmp5 * (state(4, 0) * (_tmp28 + _tmp32) + state(5, 0) * (-_tmp11 + _tmp12) +
-  const Scalar _tmp32 = _tmp3 * (_tmp27 + _tmp7);
+               state(6, 0) * (_tmp0 + _tmp29 + _tmp33));
+  const Scalar _tmp35 = _tmp5 * (_tmp24 + state(5, 0) * (_tmp16 + _tmp33));
+  const Scalar _tmp36 = _tmp10 + _tmp32;
+  const Scalar _tmp37 = _tmp19 + _tmp21;
+  const Scalar _tmp38 = -2 * _tmp13 + _tmp2;
+  const Scalar _tmp39 = _tmp36 * state(6, 0) + _tmp37 * state(5, 0) + _tmp38 * state(4, 0);
+  const Scalar _tmp40 = _tmp18 * _tmp39 * _tmp5;
+  const Scalar _tmp41 = _tmp38 * _tmp5;
+  const Scalar _tmp42 = _tmp37 * _tmp5;
+  const Scalar _tmp43 = _tmp36 * _tmp5;
+  const Scalar _tmp44 = _tmp39 * _tmp4;
 
   // 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) +
+        _tmp17 * (P(0, 0) * _tmp17 + P(2, 0) * _tmp30 + P(23, 0) * _tmp25 - P(24, 0) * _tmp26 +
-                       _tmp20 * (P(0, 3) * _tmp15 + P(2, 3) * _tmp25 + P(3, 3) * _tmp20 +
+                  P(3, 0) * _tmp27 + P(4, 0) * _tmp6 + P(5, 0) * _tmp31) +
-                                 P(4, 3) * _tmp4 + P(5, 3) * _tmp22) +
+        _tmp25 * (P(0, 23) * _tmp17 + P(2, 23) * _tmp30 + P(23, 23) * _tmp25 - P(24, 23) * _tmp26 +
-                       _tmp22 * (P(0, 5) * _tmp15 + P(2, 5) * _tmp25 + P(3, 5) * _tmp20 +
+                  P(3, 23) * _tmp27 + P(4, 23) * _tmp6 + P(5, 23) * _tmp31) -
-                                 P(4, 5) * _tmp4 + P(5, 5) * _tmp22) +
+        _tmp26 * (P(0, 24) * _tmp17 + P(2, 24) * _tmp30 + P(23, 24) * _tmp25 - P(24, 24) * _tmp26 +
-                       _tmp25 * (P(0, 2) * _tmp15 + P(2, 2) * _tmp25 + P(3, 2) * _tmp20 +
+                  P(3, 24) * _tmp27 + P(4, 24) * _tmp6 + P(5, 24) * _tmp31) +
-                                 P(4, 2) * _tmp4 + P(5, 2) * _tmp22) +
+        _tmp27 * (P(0, 3) * _tmp17 + P(2, 3) * _tmp30 + P(23, 3) * _tmp25 - P(24, 3) * _tmp26 +
-                       _tmp4 * (P(0, 4) * _tmp15 + P(2, 4) * _tmp25 + P(3, 4) * _tmp20 +
+                  P(3, 3) * _tmp27 + P(4, 3) * _tmp6 + P(5, 3) * _tmp31) +
-                                P(4, 4) * _tmp4 + P(5, 4) * _tmp22);
+        _tmp30 * (P(0, 2) * _tmp17 + P(2, 2) * _tmp30 + P(23, 2) * _tmp25 - P(24, 2) * _tmp26 +
-    _innov_var(1, 0) = R -
+                  P(3, 2) * _tmp27 + P(4, 2) * _tmp6 + P(5, 2) * _tmp31) +
-                       _tmp26 * (-P(1, 3) * _tmp29 - P(2, 3) * _tmp30 - P(3, 3) * _tmp26 -
+        _tmp31 * (P(0, 5) * _tmp17 + P(2, 5) * _tmp30 + P(23, 5) * _tmp25 - P(24, 5) * _tmp26 +
-                                 P(4, 3) * _tmp31 - P(5, 3) * _tmp32) -
+                  P(3, 5) * _tmp27 + P(4, 5) * _tmp6 + P(5, 5) * _tmp31) +
-                       _tmp29 * (-P(1, 1) * _tmp29 - P(2, 1) * _tmp30 - P(3, 1) * _tmp26 -
+        _tmp6 * (P(0, 4) * _tmp17 + P(2, 4) * _tmp30 + P(23, 4) * _tmp25 - P(24, 4) * _tmp26 +
-                                 P(4, 1) * _tmp31 - P(5, 1) * _tmp32) -
+                 P(3, 4) * _tmp27 + P(4, 4) * _tmp6 + P(5, 4) * _tmp31);
-                       _tmp30 * (-P(1, 2) * _tmp29 - P(2, 2) * _tmp30 - P(3, 2) * _tmp26 -
+    _innov_var(1, 0) =
-                                 P(4, 2) * _tmp31 - P(5, 2) * _tmp32) -
+        R -
-                       _tmp31 * (-P(1, 4) * _tmp29 - P(2, 4) * _tmp30 - P(3, 4) * _tmp26 -
+        _tmp34 * (-P(1, 1) * _tmp34 - P(2, 1) * _tmp35 - P(23, 1) * _tmp44 + P(24, 1) * _tmp40 -
-                                 P(4, 4) * _tmp31 - P(5, 4) * _tmp32) -
+                  P(3, 1) * _tmp41 - P(4, 1) * _tmp42 - P(5, 1) * _tmp43) -
-                       _tmp32 * (-P(1, 5) * _tmp29 - P(2, 5) * _tmp30 - P(3, 5) * _tmp26 -
+        _tmp35 * (-P(1, 2) * _tmp34 - P(2, 2) * _tmp35 - P(23, 2) * _tmp44 + P(24, 2) * _tmp40 -
-                                 P(4, 5) * _tmp31 - P(5, 5) * _tmp32);
+                  P(3, 2) * _tmp41 - P(4, 2) * _tmp42 - P(5, 2) * _tmp43) +
+        _tmp40 * (-P(1, 24) * _tmp34 - P(2, 24) * _tmp35 - P(23, 24) * _tmp44 + P(24, 24) * _tmp40 -
+                  P(3, 24) * _tmp41 - P(4, 24) * _tmp42 - P(5, 24) * _tmp43) -
+        _tmp41 * (-P(1, 3) * _tmp34 - P(2, 3) * _tmp35 - P(23, 3) * _tmp44 + P(24, 3) * _tmp40 -
+                  P(3, 3) * _tmp41 - P(4, 3) * _tmp42 - P(5, 3) * _tmp43) -
+        _tmp42 * (-P(1, 4) * _tmp34 - P(2, 4) * _tmp35 - P(23, 4) * _tmp44 + P(24, 4) * _tmp40 -
+                  P(3, 4) * _tmp41 - P(4, 4) * _tmp42 - P(5, 4) * _tmp43) -
+        _tmp43 * (-P(1, 5) * _tmp34 - P(2, 5) * _tmp35 - P(23, 5) * _tmp44 + P(24, 5) * _tmp40 -
+                  P(3, 5) * _tmp41 - P(4, 5) * _tmp42 - P(5, 5) * _tmp43) -
+        _tmp44 * (-P(1, 23) * _tmp34 - P(2, 23) * _tmp35 - P(23, 23) * _tmp44 + P(24, 23) * _tmp40 -
+                  P(3, 23) * _tmp41 - P(4, 23) * _tmp42 - P(5, 23) * _tmp43);
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 25, 1>& _h = (*H);
 
     _h.setZero();
 
-    _h(0, 0) = _tmp15;
+    _h(0, 0) = _tmp17;
-    _h(2, 0) = _tmp25;
+    _h(2, 0) = _tmp30;
-    _h(3, 0) = _tmp20;
+    _h(3, 0) = _tmp27;
-    _h(4, 0) = _tmp4;
+    _h(4, 0) = _tmp6;
-    _h(5, 0) = _tmp22;
+    _h(5, 0) = _tmp31;
+    _h(23, 0) = _tmp25;
+    _h(24, 0) = -_tmp26;
   }
 }  // NOLINT(readability/fn_size)
 

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

@@ -16,79 +16,94 @@ namespace sym {
  * Symbolic function: compute_flow_y_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
+ *     state: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     distance: Scalar
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix25_1
  */
 template <typename Scalar>
-void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
-                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
+                              const matrix::Matrix<Scalar, 25, 25>& P, const Scalar distance,
                               const Scalar R, const Scalar epsilon,
                               Scalar* const innov_var = nullptr,
-                              matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                              matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
-  // Total ops: 116
+  // Total ops: 167
+
+  // Unused inputs
+  (void)epsilon;
 
   // Input arrays
 
-  // Intermediate terms (19)
+  // Intermediate terms (25)
-  const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp0 = 2 * state(0, 0);
-  const Scalar _tmp1 = std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp1 = -_tmp0 * state(2, 0);
-  const Scalar _tmp2 =
+  const Scalar _tmp2 = 2 * state(3, 0);
-      Scalar(1.0) /
+  const Scalar _tmp3 = _tmp2 * state(1, 0);
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
+  const Scalar _tmp4 = _tmp2 * state(2, 0);
-  const Scalar _tmp3 = _tmp2 * (-2 * _tmp0 - 2 * _tmp1 + 1);
+  const Scalar _tmp5 = _tmp0 * state(1, 0);
-  const Scalar _tmp4 = -2 * state(0, 0) * state(2, 0);
+  const Scalar _tmp6 = std::pow(state(0, 0), Scalar(2));
-  const Scalar _tmp5 = 2 * state(3, 0);
+  const Scalar _tmp7 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp6 = _tmp5 * state(1, 0);
+  const Scalar _tmp8 = std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp7 = _tmp5 * state(2, 0);
+  const Scalar _tmp9 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp8 = 2 * state(1, 0);
+  const Scalar _tmp10 = -_tmp8 + _tmp9;
-  const Scalar _tmp9 = _tmp8 * state(0, 0);
+  const Scalar _tmp11 = std::pow(distance, Scalar(-state(25, 0)));
-  const Scalar _tmp10 = std::pow(state(0, 0), Scalar(2));
+  const Scalar _tmp12 = _tmp11 * state(24, 0);
-  const Scalar _tmp11 = std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp13 = _tmp12 * (state(4, 0) * (_tmp1 - _tmp3) + state(5, 0) * (-_tmp4 + _tmp5) +
-  const Scalar _tmp12 = -_tmp0 + _tmp1;
+                                  state(6, 0) * (_tmp10 - _tmp6 + _tmp7));
-  const Scalar _tmp13 = _tmp2 * (state(4, 0) * (_tmp4 - _tmp6) + state(5, 0) * (-_tmp7 + _tmp9) +
+  const Scalar _tmp14 = _tmp2 * state(0, 0);
-                                 state(6, 0) * (-_tmp10 + _tmp11 + _tmp12));
+  const Scalar _tmp15 = 2 * state(1, 0) * state(2, 0);
-  const Scalar _tmp14 = _tmp5 * state(0, 0);
-  const Scalar _tmp15 = _tmp8 * state(2, 0);
   const Scalar _tmp16 =
-      _tmp2 * (state(4, 0) * (-_tmp14 + _tmp15) + state(5, 0) * (_tmp10 - _tmp11 + _tmp12) +
+      _tmp12 * (state(4, 0) * (-_tmp14 + _tmp15) + state(5, 0) * (_tmp10 + _tmp6 - _tmp7) +
-               state(6, 0) * (_tmp7 + _tmp9));
+                state(6, 0) * (_tmp4 + _tmp5));
-  const Scalar _tmp17 = _tmp2 * (_tmp14 + _tmp15);
+  const Scalar _tmp17 = _tmp1 + _tmp3;
-  const Scalar _tmp18 = _tmp2 * (_tmp4 + _tmp6);
+  const Scalar _tmp18 = _tmp14 + _tmp15;
+  const Scalar _tmp19 = -2 * _tmp8 - 2 * _tmp9 + 1;
+  const Scalar _tmp20 =
+      _tmp11 * (_tmp17 * state(6, 0) + _tmp18 * state(5, 0) + _tmp19 * state(4, 0));
+  const Scalar _tmp21 = _tmp20 * state(24, 0) * std::log(distance);
+  const Scalar _tmp22 = _tmp12 * _tmp19;
+  const Scalar _tmp23 = _tmp12 * _tmp18;
+  const Scalar _tmp24 = _tmp12 * _tmp17;
 
   // 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(1, 1) * _tmp13 - P(2, 1) * _tmp16 - P(23, 1) * _tmp20 + P(24, 1) * _tmp21 -
-                 _tmp16 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp16 - P(3, 2) * _tmp3 -
+                  P(3, 1) * _tmp22 - P(4, 1) * _tmp23 - P(5, 1) * _tmp24) -
-                           P(4, 2) * _tmp17 - P(5, 2) * _tmp18) -
+        _tmp16 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp16 - P(23, 2) * _tmp20 + P(24, 2) * _tmp21 -
-                 _tmp17 * (-P(1, 4) * _tmp13 - P(2, 4) * _tmp16 - P(3, 4) * _tmp3 -
+                  P(3, 2) * _tmp22 - P(4, 2) * _tmp23 - P(5, 2) * _tmp24) -
-                           P(4, 4) * _tmp17 - P(5, 4) * _tmp18) -
+        _tmp20 * (-P(1, 23) * _tmp13 - P(2, 23) * _tmp16 - P(23, 23) * _tmp20 + P(24, 23) * _tmp21 -
-                 _tmp18 * (-P(1, 5) * _tmp13 - P(2, 5) * _tmp16 - P(3, 5) * _tmp3 -
+                  P(3, 23) * _tmp22 - P(4, 23) * _tmp23 - P(5, 23) * _tmp24) +
-                           P(4, 5) * _tmp17 - P(5, 5) * _tmp18) -
+        _tmp21 * (-P(1, 24) * _tmp13 - P(2, 24) * _tmp16 - P(23, 24) * _tmp20 + P(24, 24) * _tmp21 -
-                 _tmp3 * (-P(1, 3) * _tmp13 - P(2, 3) * _tmp16 - P(3, 3) * _tmp3 -
+                  P(3, 24) * _tmp22 - P(4, 24) * _tmp23 - P(5, 24) * _tmp24) -
-                          P(4, 3) * _tmp17 - P(5, 3) * _tmp18);
+        _tmp22 * (-P(1, 3) * _tmp13 - P(2, 3) * _tmp16 - P(23, 3) * _tmp20 + P(24, 3) * _tmp21 -
+                  P(3, 3) * _tmp22 - P(4, 3) * _tmp23 - P(5, 3) * _tmp24) -
+        _tmp23 * (-P(1, 4) * _tmp13 - P(2, 4) * _tmp16 - P(23, 4) * _tmp20 + P(24, 4) * _tmp21 -
+                  P(3, 4) * _tmp22 - P(4, 4) * _tmp23 - P(5, 4) * _tmp24) -
+        _tmp24 * (-P(1, 5) * _tmp13 - P(2, 5) * _tmp16 - P(23, 5) * _tmp20 + P(24, 5) * _tmp21 -
+                  P(3, 5) * _tmp22 - P(4, 5) * _tmp23 - P(5, 5) * _tmp24);
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 25, 1>& _h = (*H);
 
     _h.setZero();
 
     _h(1, 0) = -_tmp13;
     _h(2, 0) = -_tmp16;
-    _h(3, 0) = -_tmp3;
+    _h(3, 0) = -_tmp22;
-    _h(4, 0) = -_tmp17;
+    _h(4, 0) = -_tmp23;
-    _h(5, 0) = -_tmp18;
+    _h(5, 0) = -_tmp24;
+    _h(23, 0) = -_tmp20;
+    _h(24, 0) = _tmp21;
   }
 }  // 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     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: Matrix25_1
  */
 template <typename Scalar>
-void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
-                                    const matrix::Matrix<Scalar, 23, 23>& P,
+                                    const matrix::Matrix<Scalar, 25, 25>& 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, 25, 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, 25, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     meas: Matrix31
  *     R: Scalar
  *     epsilon: Scalar
@@ -25,21 +25,21 @@ namespace sym {
  * Outputs:
  *     innov: Matrix31
  *     innov_var: Matrix31
- *     Kx: Matrix23_1
+ *     Kx: Matrix25_1
- *     Ky: Matrix23_1
+ *     Ky: Matrix25_1
- *     Kz: Matrix23_1
+ *     Kz: Matrix25_1
  */
 template <typename Scalar>
-void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 26, 1>& state,
-                                    const matrix::Matrix<Scalar, 23, 23>& P,
+                                    const matrix::Matrix<Scalar, 25, 25>& 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, 25, 1>* const Kx = nullptr,
-                                    matrix::Matrix<Scalar, 23, 1>* const Ky = nullptr,
+                                    matrix::Matrix<Scalar, 25, 1>* const Ky = nullptr,
-                                    matrix::Matrix<Scalar, 23, 1>* const Kz = nullptr) {
+                                    matrix::Matrix<Scalar, 25, 1>* const Kz = nullptr) {
-  // Total ops: 361
+  // Total ops: 385
 
   // 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, 25, 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,12 @@ 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);
+    _kx(24, 0) = _tmp28 * (P(24, 1) * _tmp13 + P(24, 2) * _tmp14);
   }
 
   if (Ky != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _ky = (*Ky);
+    matrix::Matrix<Scalar, 25, 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 +158,12 @@ 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);
+    _ky(24, 0) = _tmp29 * (P(24, 0) * _tmp18 + P(24, 2) * _tmp19);
   }
 
   if (Kz != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _kz = (*Kz);
+    matrix::Matrix<Scalar, 25, 1>& _kz = (*Kz);
 
     _kz(0, 0) = _tmp30 * (P(0, 1) * _tmp24 + _tmp26);
     _kz(1, 0) = _tmp30 * (P(1, 0) * _tmp23 + _tmp25);
@@ -184,6 +188,8 @@ 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);
+    _kz(24, 0) = _tmp30 * (P(24, 0) * _tmp23 + P(24, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     pred: Scalar
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix25_1
  */
 template <typename Scalar>
-void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
-                                           const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                                           const matrix::Matrix<Scalar, 25, 25>& 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, 25, 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, 25, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     meas: Matrix31
  *     R: Scalar
  *     epsilon: Scalar
@@ -25,16 +25,16 @@ namespace sym {
  * Outputs:
  *     innov: Matrix31
  *     innov_var: Matrix31
- *     Hx: Matrix23_1
+ *     Hx: Matrix25_1
  */
 template <typename Scalar>
-void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 26, 1>& state,
-                                  const matrix::Matrix<Scalar, 23, 23>& P,
+                                  const matrix::Matrix<Scalar, 25, 25>& 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, 25, 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, 25, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix25_1
  */
 template <typename Scalar>
-void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
-                             const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                             const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
                              const Scalar epsilon, Scalar* const innov_var = nullptr,
-                             matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                             matrix::Matrix<Scalar, 25, 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, 25, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix25_1
  */
 template <typename Scalar>
-void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
-                             const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                             const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
                              const Scalar epsilon, Scalar* const innov_var = nullptr,
-                             matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                             matrix::Matrix<Scalar, 25, 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, 25, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     innov_var: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
- *     H: Matrix23_1
+ *     H: Matrix25_1
- *     K: Matrix23_1
+ *     K: Matrix25_1
  */
 template <typename Scalar>
-void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 26, 1>& state,
-                          const matrix::Matrix<Scalar, 23, 23>& P, const Scalar innov_var,
+                          const matrix::Matrix<Scalar, 25, 25>& P, const Scalar innov_var,
-                          const Scalar epsilon, matrix::Matrix<Scalar, 23, 1>* const H = nullptr,
+                          const Scalar epsilon, matrix::Matrix<Scalar, 25, 1>* const H = nullptr,
-                          matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
+                          matrix::Matrix<Scalar, 25, 1>* const K = nullptr) {
-  // Total ops: 469
+  // Total ops: 501
 
   // Input arrays
 
@@ -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, 25, 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, 25, 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,12 @@ 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);
+    _k(24, 0) =
+        _tmp45 * (P(24, 0) * _tmp24 - P(24, 1) * _tmp34 + P(24, 2) * _tmp35 + P(24, 21) * _tmp43 +
+                  P(24, 22) * _tmp44 + P(24, 3) * _tmp38 + P(24, 4) * _tmp41 + P(24, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     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, 26, 1>& state,
-                                     const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                                     const matrix::Matrix<Scalar, 25, 25>& 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix25_1
  */
 template <typename Scalar>
-void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
-                               const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                               const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
                                const Scalar epsilon, Scalar* const innov_var = nullptr,
-                               matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                               matrix::Matrix<Scalar, 25, 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, 25, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix25_1
  */
 template <typename Scalar>
-void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 26, 1>& state,
-                                        const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                                        const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
                                         const Scalar epsilon, Scalar* const innov_var = nullptr,
-                                        matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                                        matrix::Matrix<Scalar, 25, 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, 25, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix25_1
  */
 template <typename Scalar>
-void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
-                               const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                               const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
                                const Scalar epsilon, Scalar* const innov_var = nullptr,
-                               matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                               matrix::Matrix<Scalar, 25, 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, 25, 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: Matrix26_1
- *     P: Matrix23_23
+ *     P: Matrix25_25
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix25_1
  */
 template <typename Scalar>
-void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
+void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 26, 1>& state,
-                                        const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+                                        const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
                                         const Scalar epsilon, Scalar* const innov_var = nullptr,
-                                        matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                                        matrix::Matrix<Scalar, 25, 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, 25, 1>& _h = (*H);
 
     _h.setZero();
 

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

@@ -18,9 +18,11 @@ struct StateSample {
 	matrix::Vector3<float> mag_I{};
 	matrix::Vector3<float> mag_B{};
 	matrix::Vector2<float> wind_vel{};
+	float flow_scale{};
+	float flow_exp{};
 
-	matrix::Vector<float, 24> Data() const {
+	matrix::Vector<float, 26> Data() const {
-		matrix::Vector<float, 24> state;
+		matrix::Vector<float, 26> 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 +31,17 @@ 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) = flow_scale;
+		state.slice<1, 1>(25, 0) = flow_exp;
 		return state;
 	};
 
-	const matrix::Vector<float, 24>& vector() const {
+	const matrix::Vector<float, 26>& vector() const {
-		return *reinterpret_cast<matrix::Vector<float, 24>*>(const_cast<float*>(reinterpret_cast<const float*>(&quat_nominal)));
+		return *reinterpret_cast<matrix::Vector<float, 26>*>(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, 26>) == sizeof(StateSample), "state vector doesn't match StateSample size");
 
 struct IdxDof { unsigned idx; unsigned dof; };
 namespace State {
@@ -49,7 +53,9 @@ 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 flow_scale{23, 1};
+	static constexpr IdxDof flow_exp{24, 1};
+	static constexpr uint8_t size{25};
 };
 }
 #endif // !EKF_STATE_H