ci: run on PR

.clusterfuzzlite/Dockerfile

@@ -3,5 +3,7 @@ COPY . $SRC/PX4-Autopilot
 RUN apt-get install -y libjpeg8-dev zlib1g-dev
 RUN pip3 install --upgrade pip
 RUN python3 -m pip install -r $SRC/PX4-Autopilot/Tools/setup/requirements.txt
+RUN touch /.dockerenv
+RUN $SRC/PX4-Autopilot/Tools/setup/ubuntu.sh
 WORKDIR $SRC/PX4-Autopilot
 COPY ./.clusterfuzzlite/build.sh $SRC/

.github/workflows/cflite_batch.yml

@@ -2,6 +2,9 @@ name: ClusterFuzzLite batch fuzzing
 on:
   schedule:
     - cron: '0 6 * * *'  # UTC 6am every day.
+  pull_request:
+    branches:
+      - '*'
 permissions: read-all
 jobs:
   BatchFuzzing:

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

@@ -337,9 +337,7 @@ def predict_sideslip(
     vel_rel = state["vel"] - wind
     relative_wind_body = state["quat_nominal"].inverse() * vel_rel
 
-    # Small angle approximation of side slip model
-    # Protect division by zero using epsilon
-    sideslip_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon)
+    sideslip_pred = sf.atan2(relative_wind_body[1], relative_wind_body[0], epsilon)
 
     return sideslip_pred
 

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

@@ -30,66 +30,65 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
                           const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
                           const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
                           matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
-  // Total ops: 513
+  // Total ops: 518
 
   // Input arrays
 
-  // Intermediate terms (43)
-  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
-  const Scalar _tmp1 = 2 * state(1, 0);
-  const Scalar _tmp2 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp3 = 4 * _tmp2;
-  const Scalar _tmp4 = 2 * state(6, 0);
-  const Scalar _tmp5 = _tmp4 * state(0, 0);
-  const Scalar _tmp6 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp7 = _tmp6 - 2 * std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp8 = 2 * state(0, 0);
-  const Scalar _tmp9 = _tmp8 * state(3, 0);
-  const Scalar _tmp10 = 2 * state(2, 0);
-  const Scalar _tmp11 = _tmp10 * state(1, 0);
-  const Scalar _tmp12 = _tmp11 + _tmp9;
-  const Scalar _tmp13 = _tmp1 * state(3, 0) - _tmp10 * state(0, 0);
-  const Scalar _tmp14 = _tmp0 * _tmp12 + _tmp13 * state(6, 0) + _tmp2 * _tmp7;
-  const Scalar _tmp15 =
-      _tmp14 + epsilon * (2 * math::min<Scalar>(0, (((_tmp14) > 0) - ((_tmp14) < 0))) + 1);
-  const Scalar _tmp16 = _tmp6 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp17 = _tmp11 - _tmp9;
-  const Scalar _tmp18 = _tmp10 * state(3, 0) + _tmp8 * state(1, 0);
-  const Scalar _tmp19 =
-      (_tmp0 * _tmp16 + _tmp17 * _tmp2 + _tmp18 * state(6, 0)) / std::pow(_tmp15, Scalar(2));
-  const Scalar _tmp20 = _tmp4 * state(3, 0);
-  const Scalar _tmp21 = Scalar(1.0) / (_tmp15);
-  const Scalar _tmp22 =
-      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp1 - _tmp3 * state(2, 0) - _tmp5) +
-      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp1 * _tmp2 + _tmp20);
-  const Scalar _tmp23 = 4 * _tmp0;
-  const Scalar _tmp24 =
-      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp10 + _tmp20) +
-      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp10 * _tmp2 - _tmp23 * state(1, 0) + _tmp5);
-  const Scalar _tmp25 = 2 * state(3, 0);
-  const Scalar _tmp26 = _tmp4 * state(2, 0);
-  const Scalar _tmp27 = _tmp4 * state(1, 0);
-  const Scalar _tmp28 = -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp25 - _tmp26) +
-                        (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp25 + _tmp27);
-  const Scalar _tmp29 =
-      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp8 + _tmp27 - _tmp3 * state(3, 0)) +
-      (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp8 - _tmp23 * state(3, 0) + _tmp26);
+  // Intermediate terms (50)
+  const Scalar _tmp0 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp1 = 2 * _tmp0;
+  const Scalar _tmp2 = 2 * state(3, 0);
+  const Scalar _tmp3 = _tmp2 * state(6, 0);
+  const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp6 = _tmp2 * state(0, 0);
+  const Scalar _tmp7 = 2 * 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 = 2 * state(2, 0);
+  const Scalar _tmp12 = -_tmp11 * state(0, 0) + _tmp2 * state(1, 0);
+  const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp9 + _tmp12 * state(6, 0);
+  const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5));
+  const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
+  const Scalar _tmp16 = 2 * _tmp10;
+  const Scalar _tmp17 = 2 * state(0, 0) * state(6, 0);
+  const Scalar _tmp18 = std::pow(_tmp14, Scalar(2));
+  const Scalar _tmp19 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp20 = -_tmp6 + _tmp8;
+  const Scalar _tmp21 = _tmp2 * state(2, 0) + _tmp7 * state(0, 0);
+  const Scalar _tmp22 = _tmp0 * _tmp20 + _tmp10 * _tmp19 + _tmp21 * state(6, 0);
+  const Scalar _tmp23 = _tmp22 / _tmp18;
+  const Scalar _tmp24 = _tmp15 * (_tmp1 * state(1, 0) + _tmp3) -
+                        _tmp23 * (-4 * _tmp0 * state(2, 0) + _tmp16 * state(1, 0) - _tmp17);
+  const Scalar _tmp25 = _tmp18 / (_tmp18 + std::pow(_tmp22, Scalar(2)));
+  const Scalar _tmp26 = (Scalar(1) / Scalar(2)) * _tmp25;
+  const Scalar _tmp27 = _tmp26 * state(3, 0);
+  const Scalar _tmp28 = _tmp7 * state(6, 0);
+  const Scalar _tmp29 = _tmp11 * state(6, 0);
   const Scalar _tmp30 =
-      -_tmp22 * state(3, 0) + _tmp24 * state(0, 0) - _tmp28 * state(1, 0) + _tmp29 * state(2, 0);
-  const Scalar _tmp31 =
-      _tmp22 * state(0, 0) + _tmp24 * state(3, 0) - _tmp28 * state(2, 0) - _tmp29 * state(1, 0);
-  const Scalar _tmp32 =
-      _tmp22 * state(1, 0) - _tmp24 * state(2, 0) - _tmp28 * state(3, 0) + _tmp29 * state(0, 0);
-  const Scalar _tmp33 = _tmp19 * _tmp7;
-  const Scalar _tmp34 = _tmp17 * _tmp21;
-  const Scalar _tmp35 = -_tmp33 + _tmp34;
-  const Scalar _tmp36 = _tmp12 * _tmp19;
-  const Scalar _tmp37 = _tmp16 * _tmp21;
-  const Scalar _tmp38 = -_tmp36 + _tmp37;
-  const Scalar _tmp39 = -_tmp13 * _tmp19 + _tmp18 * _tmp21;
-  const Scalar _tmp40 = _tmp33 - _tmp34;
-  const Scalar _tmp41 = _tmp36 - _tmp37;
-  const Scalar _tmp42 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
+      _tmp15 * (-_tmp1 * state(3, 0) + _tmp28) - _tmp23 * (_tmp16 * state(3, 0) - _tmp29);
+  const Scalar _tmp31 = _tmp26 * state(1, 0);
+  const Scalar _tmp32 = _tmp15 * (_tmp1 * state(2, 0) - 4 * _tmp10 * state(1, 0) + _tmp17) -
+                        _tmp23 * (_tmp16 * state(2, 0) + _tmp3);
+  const Scalar _tmp33 = _tmp26 * state(0, 0);
+  const Scalar _tmp34 = 4 * state(3, 0);
+  const Scalar _tmp35 = _tmp15 * (-_tmp1 * state(0, 0) - _tmp10 * _tmp34 + _tmp29) -
+                        _tmp23 * (-_tmp0 * _tmp34 + _tmp16 * state(0, 0) + _tmp28);
+  const Scalar _tmp36 = _tmp26 * state(2, 0);
+  const Scalar _tmp37 = -_tmp24 * _tmp27 - _tmp30 * _tmp31 + _tmp32 * _tmp33 + _tmp35 * _tmp36;
+  const Scalar _tmp38 = _tmp24 * _tmp33 + _tmp27 * _tmp32 - _tmp30 * _tmp36 - _tmp31 * _tmp35;
+  const Scalar _tmp39 = _tmp24 * _tmp31 - _tmp27 * _tmp30 - _tmp32 * _tmp36 + _tmp33 * _tmp35;
+  const Scalar _tmp40 = _tmp23 * _tmp5;
+  const Scalar _tmp41 = _tmp15 * _tmp20;
+  const Scalar _tmp42 = _tmp25 * (-_tmp40 + _tmp41);
+  const Scalar _tmp43 = _tmp15 * _tmp19;
+  const Scalar _tmp44 = _tmp23 * _tmp9;
+  const Scalar _tmp45 = _tmp25 * (_tmp43 - _tmp44);
+  const Scalar _tmp46 = _tmp25 * (-_tmp12 * _tmp23 + _tmp15 * _tmp21);
+  const Scalar _tmp47 = _tmp25 * (_tmp40 - _tmp41);
+  const Scalar _tmp48 = _tmp25 * (-_tmp43 + _tmp44);
+  const Scalar _tmp49 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
 
   // Output terms (2)
   if (H != nullptr) {
@@ -97,91 +96,91 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
 
     _h.setZero();
 
-    _h(0, 0) = _tmp30;
-    _h(1, 0) = _tmp31;
-    _h(2, 0) = _tmp32;
-    _h(3, 0) = _tmp35;
-    _h(4, 0) = _tmp38;
-    _h(5, 0) = _tmp39;
-    _h(21, 0) = _tmp40;
-    _h(22, 0) = _tmp41;
+    _h(0, 0) = _tmp37;
+    _h(1, 0) = _tmp38;
+    _h(2, 0) = _tmp39;
+    _h(3, 0) = _tmp42;
+    _h(4, 0) = _tmp45;
+    _h(5, 0) = _tmp46;
+    _h(21, 0) = _tmp47;
+    _h(22, 0) = _tmp48;
   }
 
   if (K != nullptr) {
     matrix::Matrix<Scalar, 24, 1>& _k = (*K);
 
     _k(0, 0) =
-        _tmp42 * (P(0, 0) * _tmp30 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + P(0, 21) * _tmp40 +
-                  P(0, 22) * _tmp41 + P(0, 3) * _tmp35 + P(0, 4) * _tmp38 + P(0, 5) * _tmp39);
+        _tmp49 * (P(0, 0) * _tmp37 + P(0, 1) * _tmp38 + P(0, 2) * _tmp39 + P(0, 21) * _tmp47 +
+                  P(0, 22) * _tmp48 + P(0, 3) * _tmp42 + P(0, 4) * _tmp45 + P(0, 5) * _tmp46);
     _k(1, 0) =
-        _tmp42 * (P(1, 0) * _tmp30 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + P(1, 21) * _tmp40 +
-                  P(1, 22) * _tmp41 + P(1, 3) * _tmp35 + P(1, 4) * _tmp38 + P(1, 5) * _tmp39);
+        _tmp49 * (P(1, 0) * _tmp37 + P(1, 1) * _tmp38 + P(1, 2) * _tmp39 + P(1, 21) * _tmp47 +
+                  P(1, 22) * _tmp48 + P(1, 3) * _tmp42 + P(1, 4) * _tmp45 + P(1, 5) * _tmp46);
     _k(2, 0) =
-        _tmp42 * (P(2, 0) * _tmp30 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + P(2, 21) * _tmp40 +
-                  P(2, 22) * _tmp41 + P(2, 3) * _tmp35 + P(2, 4) * _tmp38 + P(2, 5) * _tmp39);
+        _tmp49 * (P(2, 0) * _tmp37 + P(2, 1) * _tmp38 + P(2, 2) * _tmp39 + P(2, 21) * _tmp47 +
+                  P(2, 22) * _tmp48 + P(2, 3) * _tmp42 + P(2, 4) * _tmp45 + P(2, 5) * _tmp46);
     _k(3, 0) =
-        _tmp42 * (P(3, 0) * _tmp30 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + P(3, 21) * _tmp40 +
-                  P(3, 22) * _tmp41 + P(3, 3) * _tmp35 + P(3, 4) * _tmp38 + P(3, 5) * _tmp39);
+        _tmp49 * (P(3, 0) * _tmp37 + P(3, 1) * _tmp38 + P(3, 2) * _tmp39 + P(3, 21) * _tmp47 +
+                  P(3, 22) * _tmp48 + P(3, 3) * _tmp42 + P(3, 4) * _tmp45 + P(3, 5) * _tmp46);
     _k(4, 0) =
-        _tmp42 * (P(4, 0) * _tmp30 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + P(4, 21) * _tmp40 +
-                  P(4, 22) * _tmp41 + P(4, 3) * _tmp35 + P(4, 4) * _tmp38 + P(4, 5) * _tmp39);
+        _tmp49 * (P(4, 0) * _tmp37 + P(4, 1) * _tmp38 + P(4, 2) * _tmp39 + P(4, 21) * _tmp47 +
+                  P(4, 22) * _tmp48 + P(4, 3) * _tmp42 + P(4, 4) * _tmp45 + P(4, 5) * _tmp46);
     _k(5, 0) =
-        _tmp42 * (P(5, 0) * _tmp30 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + P(5, 21) * _tmp40 +
-                  P(5, 22) * _tmp41 + P(5, 3) * _tmp35 + P(5, 4) * _tmp38 + P(5, 5) * _tmp39);
+        _tmp49 * (P(5, 0) * _tmp37 + P(5, 1) * _tmp38 + P(5, 2) * _tmp39 + P(5, 21) * _tmp47 +
+                  P(5, 22) * _tmp48 + P(5, 3) * _tmp42 + P(5, 4) * _tmp45 + P(5, 5) * _tmp46);
     _k(6, 0) =
-        _tmp42 * (P(6, 0) * _tmp30 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + P(6, 21) * _tmp40 +
-                  P(6, 22) * _tmp41 + P(6, 3) * _tmp35 + P(6, 4) * _tmp38 + P(6, 5) * _tmp39);
+        _tmp49 * (P(6, 0) * _tmp37 + P(6, 1) * _tmp38 + P(6, 2) * _tmp39 + P(6, 21) * _tmp47 +
+                  P(6, 22) * _tmp48 + P(6, 3) * _tmp42 + P(6, 4) * _tmp45 + P(6, 5) * _tmp46);
     _k(7, 0) =
-        _tmp42 * (P(7, 0) * _tmp30 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + P(7, 21) * _tmp40 +
-                  P(7, 22) * _tmp41 + P(7, 3) * _tmp35 + P(7, 4) * _tmp38 + P(7, 5) * _tmp39);
+        _tmp49 * (P(7, 0) * _tmp37 + P(7, 1) * _tmp38 + P(7, 2) * _tmp39 + P(7, 21) * _tmp47 +
+                  P(7, 22) * _tmp48 + P(7, 3) * _tmp42 + P(7, 4) * _tmp45 + P(7, 5) * _tmp46);
     _k(8, 0) =
-        _tmp42 * (P(8, 0) * _tmp30 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + P(8, 21) * _tmp40 +
-                  P(8, 22) * _tmp41 + P(8, 3) * _tmp35 + P(8, 4) * _tmp38 + P(8, 5) * _tmp39);
+        _tmp49 * (P(8, 0) * _tmp37 + P(8, 1) * _tmp38 + P(8, 2) * _tmp39 + P(8, 21) * _tmp47 +
+                  P(8, 22) * _tmp48 + P(8, 3) * _tmp42 + P(8, 4) * _tmp45 + P(8, 5) * _tmp46);
     _k(9, 0) =
-        _tmp42 * (P(9, 0) * _tmp30 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + P(9, 21) * _tmp40 +
-                  P(9, 22) * _tmp41 + P(9, 3) * _tmp35 + P(9, 4) * _tmp38 + P(9, 5) * _tmp39);
+        _tmp49 * (P(9, 0) * _tmp37 + P(9, 1) * _tmp38 + P(9, 2) * _tmp39 + P(9, 21) * _tmp47 +
+                  P(9, 22) * _tmp48 + P(9, 3) * _tmp42 + P(9, 4) * _tmp45 + P(9, 5) * _tmp46);
     _k(10, 0) =
-        _tmp42 * (P(10, 0) * _tmp30 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + P(10, 21) * _tmp40 +
-                  P(10, 22) * _tmp41 + P(10, 3) * _tmp35 + P(10, 4) * _tmp38 + P(10, 5) * _tmp39);
+        _tmp49 * (P(10, 0) * _tmp37 + P(10, 1) * _tmp38 + P(10, 2) * _tmp39 + P(10, 21) * _tmp47 +
+                  P(10, 22) * _tmp48 + P(10, 3) * _tmp42 + P(10, 4) * _tmp45 + P(10, 5) * _tmp46);
     _k(11, 0) =
-        _tmp42 * (P(11, 0) * _tmp30 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + P(11, 21) * _tmp40 +
-                  P(11, 22) * _tmp41 + P(11, 3) * _tmp35 + P(11, 4) * _tmp38 + P(11, 5) * _tmp39);
+        _tmp49 * (P(11, 0) * _tmp37 + P(11, 1) * _tmp38 + P(11, 2) * _tmp39 + P(11, 21) * _tmp47 +
+                  P(11, 22) * _tmp48 + P(11, 3) * _tmp42 + P(11, 4) * _tmp45 + P(11, 5) * _tmp46);
     _k(12, 0) =
-        _tmp42 * (P(12, 0) * _tmp30 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + P(12, 21) * _tmp40 +
-                  P(12, 22) * _tmp41 + P(12, 3) * _tmp35 + P(12, 4) * _tmp38 + P(12, 5) * _tmp39);
+        _tmp49 * (P(12, 0) * _tmp37 + P(12, 1) * _tmp38 + P(12, 2) * _tmp39 + P(12, 21) * _tmp47 +
+                  P(12, 22) * _tmp48 + P(12, 3) * _tmp42 + P(12, 4) * _tmp45 + P(12, 5) * _tmp46);
     _k(13, 0) =
-        _tmp42 * (P(13, 0) * _tmp30 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + P(13, 21) * _tmp40 +
-                  P(13, 22) * _tmp41 + P(13, 3) * _tmp35 + P(13, 4) * _tmp38 + P(13, 5) * _tmp39);
+        _tmp49 * (P(13, 0) * _tmp37 + P(13, 1) * _tmp38 + P(13, 2) * _tmp39 + P(13, 21) * _tmp47 +
+                  P(13, 22) * _tmp48 + P(13, 3) * _tmp42 + P(13, 4) * _tmp45 + P(13, 5) * _tmp46);
     _k(14, 0) =
-        _tmp42 * (P(14, 0) * _tmp30 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + P(14, 21) * _tmp40 +
-                  P(14, 22) * _tmp41 + P(14, 3) * _tmp35 + P(14, 4) * _tmp38 + P(14, 5) * _tmp39);
+        _tmp49 * (P(14, 0) * _tmp37 + P(14, 1) * _tmp38 + P(14, 2) * _tmp39 + P(14, 21) * _tmp47 +
+                  P(14, 22) * _tmp48 + P(14, 3) * _tmp42 + P(14, 4) * _tmp45 + P(14, 5) * _tmp46);
     _k(15, 0) =
-        _tmp42 * (P(15, 0) * _tmp30 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + P(15, 21) * _tmp40 +
-                  P(15, 22) * _tmp41 + P(15, 3) * _tmp35 + P(15, 4) * _tmp38 + P(15, 5) * _tmp39);
+        _tmp49 * (P(15, 0) * _tmp37 + P(15, 1) * _tmp38 + P(15, 2) * _tmp39 + P(15, 21) * _tmp47 +
+                  P(15, 22) * _tmp48 + P(15, 3) * _tmp42 + P(15, 4) * _tmp45 + P(15, 5) * _tmp46);
     _k(16, 0) =
-        _tmp42 * (P(16, 0) * _tmp30 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + P(16, 21) * _tmp40 +
-                  P(16, 22) * _tmp41 + P(16, 3) * _tmp35 + P(16, 4) * _tmp38 + P(16, 5) * _tmp39);
+        _tmp49 * (P(16, 0) * _tmp37 + P(16, 1) * _tmp38 + P(16, 2) * _tmp39 + P(16, 21) * _tmp47 +
+                  P(16, 22) * _tmp48 + P(16, 3) * _tmp42 + P(16, 4) * _tmp45 + P(16, 5) * _tmp46);
     _k(17, 0) =
-        _tmp42 * (P(17, 0) * _tmp30 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + P(17, 21) * _tmp40 +
-                  P(17, 22) * _tmp41 + P(17, 3) * _tmp35 + P(17, 4) * _tmp38 + P(17, 5) * _tmp39);
+        _tmp49 * (P(17, 0) * _tmp37 + P(17, 1) * _tmp38 + P(17, 2) * _tmp39 + P(17, 21) * _tmp47 +
+                  P(17, 22) * _tmp48 + P(17, 3) * _tmp42 + P(17, 4) * _tmp45 + P(17, 5) * _tmp46);
     _k(18, 0) =
-        _tmp42 * (P(18, 0) * _tmp30 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + P(18, 21) * _tmp40 +
-                  P(18, 22) * _tmp41 + P(18, 3) * _tmp35 + P(18, 4) * _tmp38 + P(18, 5) * _tmp39);
+        _tmp49 * (P(18, 0) * _tmp37 + P(18, 1) * _tmp38 + P(18, 2) * _tmp39 + P(18, 21) * _tmp47 +
+                  P(18, 22) * _tmp48 + P(18, 3) * _tmp42 + P(18, 4) * _tmp45 + P(18, 5) * _tmp46);
     _k(19, 0) =
-        _tmp42 * (P(19, 0) * _tmp30 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + P(19, 21) * _tmp40 +
-                  P(19, 22) * _tmp41 + P(19, 3) * _tmp35 + P(19, 4) * _tmp38 + P(19, 5) * _tmp39);
+        _tmp49 * (P(19, 0) * _tmp37 + P(19, 1) * _tmp38 + P(19, 2) * _tmp39 + P(19, 21) * _tmp47 +
+                  P(19, 22) * _tmp48 + P(19, 3) * _tmp42 + P(19, 4) * _tmp45 + P(19, 5) * _tmp46);
     _k(20, 0) =
-        _tmp42 * (P(20, 0) * _tmp30 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + P(20, 21) * _tmp40 +
-                  P(20, 22) * _tmp41 + P(20, 3) * _tmp35 + P(20, 4) * _tmp38 + P(20, 5) * _tmp39);
+        _tmp49 * (P(20, 0) * _tmp37 + P(20, 1) * _tmp38 + P(20, 2) * _tmp39 + P(20, 21) * _tmp47 +
+                  P(20, 22) * _tmp48 + P(20, 3) * _tmp42 + P(20, 4) * _tmp45 + P(20, 5) * _tmp46);
     _k(21, 0) =
-        _tmp42 * (P(21, 0) * _tmp30 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + P(21, 21) * _tmp40 +
-                  P(21, 22) * _tmp41 + P(21, 3) * _tmp35 + P(21, 4) * _tmp38 + P(21, 5) * _tmp39);
+        _tmp49 * (P(21, 0) * _tmp37 + P(21, 1) * _tmp38 + P(21, 2) * _tmp39 + P(21, 21) * _tmp47 +
+                  P(21, 22) * _tmp48 + P(21, 3) * _tmp42 + P(21, 4) * _tmp45 + P(21, 5) * _tmp46);
     _k(22, 0) =
-        _tmp42 * (P(22, 0) * _tmp30 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + P(22, 21) * _tmp40 +
-                  P(22, 22) * _tmp41 + P(22, 3) * _tmp35 + P(22, 4) * _tmp38 + P(22, 5) * _tmp39);
+        _tmp49 * (P(22, 0) * _tmp37 + P(22, 1) * _tmp38 + P(22, 2) * _tmp39 + P(22, 21) * _tmp47 +
+                  P(22, 22) * _tmp48 + P(22, 3) * _tmp42 + P(22, 4) * _tmp45 + P(22, 5) * _tmp46);
     _k(23, 0) =
-        _tmp42 * (P(23, 0) * _tmp30 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + P(23, 21) * _tmp40 +
-                  P(23, 22) * _tmp41 + P(23, 3) * _tmp35 + P(23, 4) * _tmp38 + P(23, 5) * _tmp39);
+        _tmp49 * (P(23, 0) * _tmp37 + P(23, 1) * _tmp38 + P(23, 2) * _tmp39 + P(23, 21) * _tmp47 +
+                  P(23, 22) * _tmp48 + P(23, 3) * _tmp42 + P(23, 4) * _tmp45 + P(23, 5) * _tmp46);
   }
 }  // NOLINT(readability/fn_size)
 

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

@@ -30,70 +30,69 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
                                      const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                      const Scalar epsilon, Scalar* const innov = nullptr,
                                      Scalar* const innov_var = nullptr) {
-  // Total ops: 265
+  // Total ops: 266
 
   // Input arrays
 
-  // Intermediate terms (42)
+  // Intermediate terms (49)
   const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp2 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp3 = 2 * state(0, 0);
-  const Scalar _tmp4 = _tmp3 * state(3, 0);
+  const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp2 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp3 = 2 * state(3, 0);
+  const Scalar _tmp4 = _tmp3 * state(0, 0);
   const Scalar _tmp5 = 2 * state(1, 0);
   const Scalar _tmp6 = _tmp5 * state(2, 0);
-  const Scalar _tmp7 = _tmp4 + _tmp6;
-  const Scalar _tmp8 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp7 = -_tmp4 + _tmp6;
+  const Scalar _tmp8 = -state(22, 0) + state(4, 0);
   const Scalar _tmp9 = 2 * state(2, 0);
-  const Scalar _tmp10 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0);
+  const Scalar _tmp10 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
   const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8;
-  const Scalar _tmp12 =
-      _tmp11 + epsilon * (2 * math::min<Scalar>(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1);
-  const Scalar _tmp13 = Scalar(1.0) / (_tmp12);
-  const Scalar _tmp14 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp15 = -_tmp4 + _tmp6;
-  const Scalar _tmp16 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
-  const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0);
-  const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2));
-  const Scalar _tmp19 = _tmp18 * _tmp7;
-  const Scalar _tmp20 = _tmp13 * _tmp14;
-  const Scalar _tmp21 = _tmp19 - _tmp20;
-  const Scalar _tmp22 = -_tmp10 * _tmp18 + _tmp13 * _tmp16;
-  const Scalar _tmp23 = _tmp1 * _tmp18;
-  const Scalar _tmp24 = _tmp13 * _tmp15;
-  const Scalar _tmp25 = -_tmp23 + _tmp24;
-  const Scalar _tmp26 = 2 * state(6, 0);
-  const Scalar _tmp27 = _tmp26 * state(0, 0);
-  const Scalar _tmp28 = _tmp26 * state(3, 0);
-  const Scalar _tmp29 =
-      (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp5 + _tmp28) -
-      Scalar(1) / Scalar(2) * _tmp18 * (-4 * _tmp2 * state(2, 0) - _tmp27 + _tmp5 * _tmp8);
-  const Scalar _tmp30 =
-      (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp9 + _tmp27 - 4 * _tmp8 * state(1, 0)) -
-      Scalar(1) / Scalar(2) * _tmp18 * (_tmp28 + _tmp8 * _tmp9);
-  const Scalar _tmp31 = 2 * state(3, 0);
-  const Scalar _tmp32 = _tmp26 * state(2, 0);
-  const Scalar _tmp33 = _tmp26 * state(1, 0);
-  const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp31 + _tmp33) -
-                        Scalar(1) / Scalar(2) * _tmp18 * (_tmp31 * _tmp8 - _tmp32);
-  const Scalar _tmp35 = 4 * state(3, 0);
-  const Scalar _tmp36 =
-      (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp3 + _tmp32 - _tmp35 * _tmp8) -
-      Scalar(1) / Scalar(2) * _tmp18 * (-_tmp2 * _tmp35 + _tmp3 * _tmp8 + _tmp33);
-  const Scalar _tmp37 =
-      _tmp29 * state(1, 0) - _tmp30 * state(2, 0) - _tmp34 * state(3, 0) + _tmp36 * state(0, 0);
-  const Scalar _tmp38 =
-      -_tmp29 * state(3, 0) + _tmp30 * state(0, 0) - _tmp34 * state(1, 0) + _tmp36 * state(2, 0);
-  const Scalar _tmp39 =
-      _tmp29 * state(0, 0) + _tmp30 * state(3, 0) - _tmp34 * state(2, 0) - _tmp36 * state(1, 0);
-  const Scalar _tmp40 = _tmp23 - _tmp24;
-  const Scalar _tmp41 = -_tmp19 + _tmp20;
+  const Scalar _tmp12 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp13 = _tmp4 + _tmp6;
+  const Scalar _tmp14 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0);
+  const Scalar _tmp15 = _tmp12 * _tmp8 + _tmp13 * _tmp2 + _tmp14 * state(6, 0);
+  const Scalar _tmp16 = _tmp15 + epsilon * ((((_tmp15) > 0) - ((_tmp15) < 0)) + Scalar(0.5));
+  const Scalar _tmp17 = Scalar(1.0) / (_tmp16);
+  const Scalar _tmp18 = _tmp1 * _tmp17;
+  const Scalar _tmp19 = std::pow(_tmp16, Scalar(2));
+  const Scalar _tmp20 = _tmp11 / _tmp19;
+  const Scalar _tmp21 = _tmp13 * _tmp20;
+  const Scalar _tmp22 = _tmp19 / (std::pow(_tmp11, Scalar(2)) + _tmp19);
+  const Scalar _tmp23 = _tmp22 * (-_tmp18 + _tmp21);
+  const Scalar _tmp24 = _tmp12 * _tmp20;
+  const Scalar _tmp25 = _tmp17 * _tmp7;
+  const Scalar _tmp26 = _tmp22 * (-_tmp24 + _tmp25);
+  const Scalar _tmp27 = _tmp22 * (_tmp10 * _tmp17 - _tmp14 * _tmp20);
+  const Scalar _tmp28 = _tmp3 * state(6, 0);
+  const Scalar _tmp29 = 4 * _tmp8;
+  const Scalar _tmp30 = 2 * state(0, 0);
+  const Scalar _tmp31 = _tmp30 * state(6, 0);
+  const Scalar _tmp32 =
+      _tmp17 * (_tmp28 + _tmp5 * _tmp8) - _tmp20 * (_tmp2 * _tmp5 - _tmp29 * state(2, 0) - _tmp31);
+  const Scalar _tmp33 = (Scalar(1) / Scalar(2)) * _tmp22;
+  const Scalar _tmp34 = _tmp33 * state(1, 0);
+  const Scalar _tmp35 = _tmp5 * state(6, 0);
+  const Scalar _tmp36 = _tmp9 * state(6, 0);
+  const Scalar _tmp37 = _tmp17 * (-_tmp3 * _tmp8 + _tmp35) - _tmp20 * (_tmp2 * _tmp3 - _tmp36);
+  const Scalar _tmp38 = _tmp33 * state(3, 0);
+  const Scalar _tmp39 = 4 * _tmp2;
+  const Scalar _tmp40 =
+      _tmp17 * (_tmp31 - _tmp39 * state(1, 0) + _tmp8 * _tmp9) - _tmp20 * (_tmp2 * _tmp9 + _tmp28);
+  const Scalar _tmp41 = _tmp33 * state(2, 0);
+  const Scalar _tmp42 = _tmp17 * (-_tmp30 * _tmp8 + _tmp36 - _tmp39 * state(3, 0)) -
+                        _tmp20 * (_tmp2 * _tmp30 - _tmp29 * state(3, 0) + _tmp35);
+  const Scalar _tmp43 = _tmp33 * state(0, 0);
+  const Scalar _tmp44 = _tmp32 * _tmp34 - _tmp37 * _tmp38 - _tmp40 * _tmp41 + _tmp42 * _tmp43;
+  const Scalar _tmp45 = -_tmp32 * _tmp38 - _tmp34 * _tmp37 + _tmp40 * _tmp43 + _tmp41 * _tmp42;
+  const Scalar _tmp46 = _tmp32 * _tmp43 - _tmp34 * _tmp42 - _tmp37 * _tmp41 + _tmp38 * _tmp40;
+  const Scalar _tmp47 = _tmp22 * (_tmp24 - _tmp25);
+  const Scalar _tmp48 = _tmp22 * (_tmp18 - _tmp21);
 
   // Output terms (2)
   if (innov != nullptr) {
     Scalar& _innov = (*innov);
 
-    _innov = _tmp13 * _tmp17;
+    _innov = std::atan2(_tmp11, _tmp16);
   }
 
   if (innov_var != nullptr) {
@@ -101,22 +100,22 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
 
     _innov_var =
         R +
-        _tmp21 * (P(0, 22) * _tmp38 + P(1, 22) * _tmp39 + P(2, 22) * _tmp37 + P(21, 22) * _tmp40 +
-                  P(22, 22) * _tmp21 + P(3, 22) * _tmp25 + P(4, 22) * _tmp41 + P(5, 22) * _tmp22) +
-        _tmp22 * (P(0, 5) * _tmp38 + P(1, 5) * _tmp39 + P(2, 5) * _tmp37 + P(21, 5) * _tmp40 +
-                  P(22, 5) * _tmp21 + P(3, 5) * _tmp25 + P(4, 5) * _tmp41 + P(5, 5) * _tmp22) +
-        _tmp25 * (P(0, 3) * _tmp38 + P(1, 3) * _tmp39 + P(2, 3) * _tmp37 + P(21, 3) * _tmp40 +
-                  P(22, 3) * _tmp21 + P(3, 3) * _tmp25 + P(4, 3) * _tmp41 + P(5, 3) * _tmp22) +
-        _tmp37 * (P(0, 2) * _tmp38 + P(1, 2) * _tmp39 + P(2, 2) * _tmp37 + P(21, 2) * _tmp40 +
-                  P(22, 2) * _tmp21 + P(3, 2) * _tmp25 + P(4, 2) * _tmp41 + P(5, 2) * _tmp22) +
-        _tmp38 * (P(0, 0) * _tmp38 + P(1, 0) * _tmp39 + P(2, 0) * _tmp37 + P(21, 0) * _tmp40 +
-                  P(22, 0) * _tmp21 + P(3, 0) * _tmp25 + P(4, 0) * _tmp41 + P(5, 0) * _tmp22) +
-        _tmp39 * (P(0, 1) * _tmp38 + P(1, 1) * _tmp39 + P(2, 1) * _tmp37 + P(21, 1) * _tmp40 +
-                  P(22, 1) * _tmp21 + P(3, 1) * _tmp25 + P(4, 1) * _tmp41 + P(5, 1) * _tmp22) +
-        _tmp40 * (P(0, 21) * _tmp38 + P(1, 21) * _tmp39 + P(2, 21) * _tmp37 + P(21, 21) * _tmp40 +
-                  P(22, 21) * _tmp21 + P(3, 21) * _tmp25 + P(4, 21) * _tmp41 + P(5, 21) * _tmp22) +
-        _tmp41 * (P(0, 4) * _tmp38 + P(1, 4) * _tmp39 + P(2, 4) * _tmp37 + P(21, 4) * _tmp40 +
-                  P(22, 4) * _tmp21 + P(3, 4) * _tmp25 + P(4, 4) * _tmp41 + P(5, 4) * _tmp22);
+        _tmp23 * (P(0, 22) * _tmp45 + P(1, 22) * _tmp46 + P(2, 22) * _tmp44 + P(21, 22) * _tmp47 +
+                  P(22, 22) * _tmp23 + P(3, 22) * _tmp26 + P(4, 22) * _tmp48 + P(5, 22) * _tmp27) +
+        _tmp26 * (P(0, 3) * _tmp45 + P(1, 3) * _tmp46 + P(2, 3) * _tmp44 + P(21, 3) * _tmp47 +
+                  P(22, 3) * _tmp23 + P(3, 3) * _tmp26 + P(4, 3) * _tmp48 + P(5, 3) * _tmp27) +
+        _tmp27 * (P(0, 5) * _tmp45 + P(1, 5) * _tmp46 + P(2, 5) * _tmp44 + P(21, 5) * _tmp47 +
+                  P(22, 5) * _tmp23 + P(3, 5) * _tmp26 + P(4, 5) * _tmp48 + P(5, 5) * _tmp27) +
+        _tmp44 * (P(0, 2) * _tmp45 + P(1, 2) * _tmp46 + P(2, 2) * _tmp44 + P(21, 2) * _tmp47 +
+                  P(22, 2) * _tmp23 + P(3, 2) * _tmp26 + P(4, 2) * _tmp48 + P(5, 2) * _tmp27) +
+        _tmp45 * (P(0, 0) * _tmp45 + P(1, 0) * _tmp46 + P(2, 0) * _tmp44 + P(21, 0) * _tmp47 +
+                  P(22, 0) * _tmp23 + P(3, 0) * _tmp26 + P(4, 0) * _tmp48 + P(5, 0) * _tmp27) +
+        _tmp46 * (P(0, 1) * _tmp45 + P(1, 1) * _tmp46 + P(2, 1) * _tmp44 + P(21, 1) * _tmp47 +
+                  P(22, 1) * _tmp23 + P(3, 1) * _tmp26 + P(4, 1) * _tmp48 + P(5, 1) * _tmp27) +
+        _tmp47 * (P(0, 21) * _tmp45 + P(1, 21) * _tmp46 + P(2, 21) * _tmp44 + P(21, 21) * _tmp47 +
+                  P(22, 21) * _tmp23 + P(3, 21) * _tmp26 + P(4, 21) * _tmp48 + P(5, 21) * _tmp27) +
+        _tmp48 * (P(0, 4) * _tmp45 + P(1, 4) * _tmp46 + P(2, 4) * _tmp44 + P(21, 4) * _tmp47 +
+                  P(22, 4) * _tmp23 + P(3, 4) * _tmp26 + P(4, 4) * _tmp48 + P(5, 4) * _tmp27);
   }
 }  // NOLINT(readability/fn_size)