build and kia forte

2026-09-30 03:13:48 +08:00 · 2026-05-18 11:49:28 -05:00
parent 3f8d368b87
commit f56bfb7d4b
47 changed files with 1453 additions and 1348 deletions
@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-9d8a055a-DEBUG";
+const uint8_t gitversion[19] = "DEV-3f8d368b-DEBUG";
@@ -1 +1 @@
-DEV-9d8a055a-DEBUG
+DEV-3f8d368b-DEBUG
@@ -379,7 +379,7 @@ setup_sysroot_from_agnos() {
    -v "${HOST_CACHE_DIR}:/work/.cache" \
    -w /work \
    "${IMAGE_NAME}" \
-    python3 tools/laptop_device_build/extract_sysroot_from_agnos.py \
+    /usr/bin/python3 tools/laptop_device_build/extract_sysroot_from_agnos.py \
      --manifest "${manifest}" \
      --output-dir /opt/tici-sysroot \
      --cache-dir /work/.cache/agnos
@@ -129,6 +129,9 @@ SONATA_HYBRID_CARS = (
 KIA_EV6_CARS = (
  HYUNDAI_CAR.KIA_EV6,
 )
+KIA_FORTE_CARS = (
+  HYUNDAI_CAR.KIA_FORTE,
+)

 BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3
 BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045
@@ -261,6 +264,25 @@ SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02
 SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 6.0
 SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0

+KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT = 1.08
+KIA_FORTE_FF_REDUCTION_LEFT = 0.08
+KIA_FORTE_FF_REDUCTION_RIGHT = 0.20
+KIA_FORTE_FF_ONSET = 0.16
+KIA_FORTE_FF_ONSET_WIDTH = 0.06
+KIA_FORTE_FF_CUTOFF = 1.20
+KIA_FORTE_FF_CUTOFF_WIDTH = 0.36
+KIA_FORTE_TRANSITION_SPEED = 9.0
+KIA_FORTE_PHASE_SCALE = 0.10
+KIA_FORTE_TURN_IN_BOOST_LEFT = 0.06
+KIA_FORTE_TURN_IN_BOOST_RIGHT = 0.00
+KIA_FORTE_UNWIND_TAPER_LEFT = 0.18
+KIA_FORTE_UNWIND_TAPER_RIGHT = 0.08
+KIA_FORTE_CENTER_TAPER_MAX = 0.11
+KIA_FORTE_CENTER_TAPER_LAT = 0.14
+KIA_FORTE_CENTER_TAPER_LAT_WIDTH = 0.03
+KIA_FORTE_CENTER_TAPER_SPEED = 24.0
+KIA_FORTE_CENTER_TAPER_SPEED_WIDTH = 2.5
+
 GENESIS_G90_LATERAL_TESTING_GROUND_ID = testing_ground.id_4
 GENESIS_G90_FF_GAIN_LEFT = 0.20
 GENESIS_G90_FF_GAIN_RIGHT = 0.10
@@ -939,6 +961,48 @@ def get_sonata_hybrid_center_taper_scale(desired_lateral_accel: float, v_ego: fl
  return 1.0 - reduction


+def _kia_forte_sigmoid(x: float) -> float:
+  return _sigmoid(x)
+
+
+def _kia_forte_low_speed_factor(v_ego: float) -> float:
+  return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_FORTE_TRANSITION_SPEED) ** 2)
+
+
+def _kia_forte_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
+  return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_FORTE_PHASE_SCALE)
+
+
+def _kia_forte_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
+  return left_value if desired_lateral_accel >= 0.0 else right_value
+
+
+def get_kia_forte_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
+  if desired_lateral_accel == 0.0:
+    return 1.0
+
+  abs_lateral_accel = abs(desired_lateral_accel)
+  onset = _kia_forte_sigmoid((abs_lateral_accel - KIA_FORTE_FF_ONSET) / KIA_FORTE_FF_ONSET_WIDTH)
+  cutoff = _kia_forte_sigmoid((KIA_FORTE_FF_CUTOFF - abs_lateral_accel) / KIA_FORTE_FF_CUTOFF_WIDTH)
+  base_reduction = _kia_forte_side_value(desired_lateral_accel, KIA_FORTE_FF_REDUCTION_LEFT, KIA_FORTE_FF_REDUCTION_RIGHT) * onset * cutoff
+  phase = _kia_forte_transition_phase(desired_lateral_accel, desired_lateral_jerk)
+  turn_in_weight = max(phase, 0.0)
+  unwind_weight = max(-phase, 0.0)
+  low_speed_factor = _kia_forte_low_speed_factor(v_ego)
+  turn_in_boost = 1.0 + (_kia_forte_side_value(desired_lateral_accel, KIA_FORTE_TURN_IN_BOOST_LEFT, KIA_FORTE_TURN_IN_BOOST_RIGHT) *
+                         turn_in_weight * (0.35 + 0.65 * low_speed_factor))
+  unwind_taper = 1.0 - (_kia_forte_side_value(desired_lateral_accel, KIA_FORTE_UNWIND_TAPER_LEFT, KIA_FORTE_UNWIND_TAPER_RIGHT) *
+                        unwind_weight * (0.35 + 0.65 * low_speed_factor))
+  return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
+
+
+def get_kia_forte_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
+  speed_weight = _kia_forte_sigmoid((v_ego - KIA_FORTE_CENTER_TAPER_SPEED) / KIA_FORTE_CENTER_TAPER_SPEED_WIDTH)
+  center_weight = _kia_forte_sigmoid((KIA_FORTE_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_FORTE_CENTER_TAPER_LAT_WIDTH)
+  reduction = KIA_FORTE_CENTER_TAPER_MAX * speed_weight * center_weight
+  return 1.0 - reduction
+
+
 def genesis_g90_lateral_testing_ground_active() -> bool:
  return testing_ground.use(GENESIS_G90_LATERAL_TESTING_GROUND_ID)

@@ -1420,6 +1484,7 @@ class LatControlTorque(LatControl):
    self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS
    self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS
    self.is_sonata_hybrid = CP.carFingerprint in SONATA_HYBRID_CARS
+    self.is_kia_forte = CP.carFingerprint in KIA_FORTE_CARS
    self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS
    self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED)
    self.is_volt_cc = CP.carFingerprint == GM_CAR.CHEVROLET_VOLT_CC
@@ -1438,6 +1503,8 @@ class LatControlTorque(LatControl):
      self.torque_params.latAccelFactor *= IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT
    if self.is_sonata_hybrid:
      self.torque_params.latAccelFactor *= SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT
+    if self.is_kia_forte:
+      self.torque_params.latAccelFactor *= KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT
    if self.is_civic_bosch_modified:
      self.torque_params.latAccelFactor *= CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT
      if civic_bosch_modified_a_lateral_testing_ground_active():
@@ -1463,6 +1530,8 @@ class LatControlTorque(LatControl):
      latAccelFactor *= IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT
    if self.is_sonata_hybrid:
      latAccelFactor *= SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT
+    if self.is_kia_forte:
+      latAccelFactor *= KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT
    if self.is_civic_bosch_modified:
      latAccelFactor *= CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT
      if civic_bosch_modified_a_lateral_testing_ground_active():
@@ -1540,12 +1609,14 @@ class LatControlTorque(LatControl):
      ioniq_ev_old_active = self.is_ioniq_ev_old
      ioniq_6_active = self.is_ioniq_6
      sonata_hybrid_active = self.is_sonata_hybrid
+      kia_forte_active = self.is_kia_forte
      kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active()
      volt_plexy_test_active = self.is_volt_cc and volt_plexy_lateral_testing_ground_active()
      volt_standard_center_taper = get_volt_standard_center_taper_scale(setpoint, CS.vEgo) if volt_standard_test_active else 1.0
      ioniq_ev_old_center_taper = get_ioniq_ev_old_center_taper_scale(setpoint, CS.vEgo) if ioniq_ev_old_active else 1.0
      ioniq_6_center_taper = get_ioniq_6_center_taper_scale(setpoint, CS.vEgo) if ioniq_6_active else 1.0
      sonata_hybrid_center_taper = get_sonata_hybrid_center_taper_scale(setpoint, CS.vEgo) if sonata_hybrid_active else 1.0
+      kia_forte_center_taper = get_kia_forte_center_taper_scale(setpoint, CS.vEgo) if kia_forte_active else 1.0
      civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if (
        self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active()
      ) else 1.0
@@ -1581,6 +1652,8 @@ class LatControlTorque(LatControl):
        friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_6_center_taper)
      elif sonata_hybrid_active:
        ff *= get_sonata_hybrid_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * sonata_hybrid_center_taper
+      elif kia_forte_active:
+        ff *= get_kia_forte_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_forte_center_taper
      elif kia_ev6_test_active:
        ff *= get_kia_ev6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
        friction_threshold = get_kia_ev6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
@@ -50,6 +50,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
  get_ioniq_6_ff_scale,
  get_ioniq_6_friction_scale,
  get_ioniq_6_friction_threshold,
+  get_kia_forte_center_taper_scale,
+  get_kia_forte_ff_scale,
  get_kia_ev6_ff_scale,
  get_kia_ev6_friction_scale,
  get_kia_ev6_friction_threshold,
@@ -235,6 +237,26 @@ class TestLatControl:
    assert get_sonata_hybrid_center_taper_scale(0.0, 3.0) < get_sonata_hybrid_center_taper_scale(0.0, 10.0)
    assert get_sonata_hybrid_center_taper_scale(0.0, 30.0) < get_sonata_hybrid_center_taper_scale(0.20, 30.0) <= 1.0

+  def test_kia_forte_ff_scale_curve(self):
+    assert get_kia_forte_ff_scale(0.0, 0.0, 20.0) == 1.0
+    steady_left = get_kia_forte_ff_scale(0.45, 0.0, 25.0)
+    steady_right = get_kia_forte_ff_scale(-0.45, 0.0, 25.0)
+    turn_in_left = get_kia_forte_ff_scale(0.45, 0.7, 10.0)
+    turn_in_right = get_kia_forte_ff_scale(-0.45, -0.7, 10.0)
+    unwind_left = get_kia_forte_ff_scale(0.45, -0.7, 10.0)
+    unwind_right = get_kia_forte_ff_scale(-0.45, 0.7, 10.0)
+    assert steady_left < 1.0
+    assert steady_right < steady_left
+    assert turn_in_left > steady_left
+    assert turn_in_right == pytest.approx(steady_right)
+    assert unwind_left < steady_left
+    assert unwind_right < steady_right
+    assert unwind_right > unwind_left
+
+  def test_kia_forte_center_taper_curve(self):
+    assert get_kia_forte_center_taper_scale(0.0, 30.0) < get_kia_forte_center_taper_scale(0.0, 15.0)
+    assert get_kia_forte_center_taper_scale(0.0, 30.0) < get_kia_forte_center_taper_scale(0.20, 30.0) <= 1.0
+
  def test_genesis_g90_ff_scale_curve(self):
    assert get_genesis_g90_ff_scale(0.0, 0.0, 20.0) == 1.0
    assert get_genesis_g90_ff_scale(0.5, 0.0, 20.0) > get_genesis_g90_ff_scale(-0.5, 0.0, 20.0)
@@ -460,6 +482,16 @@ class TestLatControl:
    assert lac_log.active
    assert controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.22)

+  def test_kia_forte_default_update_path(self):
+    controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_FORTE)
+    CarInterface = interfaces[HYUNDAI.KIA_FORTE]
+    CP = CarInterface.get_non_essential_params(HYUNDAI.KIA_FORTE)
+
+    _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles)
+
+    assert lac_log.active
+    assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor * 1.08)
+
  def test_ioniq_6_update_path_does_not_post_taper_output(self, monkeypatch):
    base_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_6)
    base_output, _, _ = base_controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles)
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
 *                                                                            *
 *                         This file is part of 'ekf'                         *
 ******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_587611173581676283) {
-   out_587611173581676283[0] = delta_x[0] + nom_x[0];
-   out_587611173581676283[1] = delta_x[1] + nom_x[1];
-   out_587611173581676283[2] = delta_x[2] + nom_x[2];
-   out_587611173581676283[3] = delta_x[3] + nom_x[3];
-   out_587611173581676283[4] = delta_x[4] + nom_x[4];
-   out_587611173581676283[5] = delta_x[5] + nom_x[5];
-   out_587611173581676283[6] = delta_x[6] + nom_x[6];
-   out_587611173581676283[7] = delta_x[7] + nom_x[7];
-   out_587611173581676283[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_652600290238585961) {
+   out_652600290238585961[0] = delta_x[0] + nom_x[0];
+   out_652600290238585961[1] = delta_x[1] + nom_x[1];
+   out_652600290238585961[2] = delta_x[2] + nom_x[2];
+   out_652600290238585961[3] = delta_x[3] + nom_x[3];
+   out_652600290238585961[4] = delta_x[4] + nom_x[4];
+   out_652600290238585961[5] = delta_x[5] + nom_x[5];
+   out_652600290238585961[6] = delta_x[6] + nom_x[6];
+   out_652600290238585961[7] = delta_x[7] + nom_x[7];
+   out_652600290238585961[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_8120728011522269919) {
-   out_8120728011522269919[0] = -nom_x[0] + true_x[0];
-   out_8120728011522269919[1] = -nom_x[1] + true_x[1];
-   out_8120728011522269919[2] = -nom_x[2] + true_x[2];
-   out_8120728011522269919[3] = -nom_x[3] + true_x[3];
-   out_8120728011522269919[4] = -nom_x[4] + true_x[4];
-   out_8120728011522269919[5] = -nom_x[5] + true_x[5];
-   out_8120728011522269919[6] = -nom_x[6] + true_x[6];
-   out_8120728011522269919[7] = -nom_x[7] + true_x[7];
-   out_8120728011522269919[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_428056314371954903) {
+   out_428056314371954903[0] = -nom_x[0] + true_x[0];
+   out_428056314371954903[1] = -nom_x[1] + true_x[1];
+   out_428056314371954903[2] = -nom_x[2] + true_x[2];
+   out_428056314371954903[3] = -nom_x[3] + true_x[3];
+   out_428056314371954903[4] = -nom_x[4] + true_x[4];
+   out_428056314371954903[5] = -nom_x[5] + true_x[5];
+   out_428056314371954903[6] = -nom_x[6] + true_x[6];
+   out_428056314371954903[7] = -nom_x[7] + true_x[7];
+   out_428056314371954903[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_4001675034436284807) {
-   out_4001675034436284807[0] = 1.0;
-   out_4001675034436284807[1] = 0.0;
-   out_4001675034436284807[2] = 0.0;
-   out_4001675034436284807[3] = 0.0;
-   out_4001675034436284807[4] = 0.0;
-   out_4001675034436284807[5] = 0.0;
-   out_4001675034436284807[6] = 0.0;
-   out_4001675034436284807[7] = 0.0;
-   out_4001675034436284807[8] = 0.0;
-   out_4001675034436284807[9] = 0.0;
-   out_4001675034436284807[10] = 1.0;
-   out_4001675034436284807[11] = 0.0;
-   out_4001675034436284807[12] = 0.0;
-   out_4001675034436284807[13] = 0.0;
-   out_4001675034436284807[14] = 0.0;
-   out_4001675034436284807[15] = 0.0;
-   out_4001675034436284807[16] = 0.0;
-   out_4001675034436284807[17] = 0.0;
-   out_4001675034436284807[18] = 0.0;
-   out_4001675034436284807[19] = 0.0;
-   out_4001675034436284807[20] = 1.0;
-   out_4001675034436284807[21] = 0.0;
-   out_4001675034436284807[22] = 0.0;
-   out_4001675034436284807[23] = 0.0;
-   out_4001675034436284807[24] = 0.0;
-   out_4001675034436284807[25] = 0.0;
-   out_4001675034436284807[26] = 0.0;
-   out_4001675034436284807[27] = 0.0;
-   out_4001675034436284807[28] = 0.0;
-   out_4001675034436284807[29] = 0.0;
-   out_4001675034436284807[30] = 1.0;
-   out_4001675034436284807[31] = 0.0;
-   out_4001675034436284807[32] = 0.0;
-   out_4001675034436284807[33] = 0.0;
-   out_4001675034436284807[34] = 0.0;
-   out_4001675034436284807[35] = 0.0;
-   out_4001675034436284807[36] = 0.0;
-   out_4001675034436284807[37] = 0.0;
-   out_4001675034436284807[38] = 0.0;
-   out_4001675034436284807[39] = 0.0;
-   out_4001675034436284807[40] = 1.0;
-   out_4001675034436284807[41] = 0.0;
-   out_4001675034436284807[42] = 0.0;
-   out_4001675034436284807[43] = 0.0;
-   out_4001675034436284807[44] = 0.0;
-   out_4001675034436284807[45] = 0.0;
-   out_4001675034436284807[46] = 0.0;
-   out_4001675034436284807[47] = 0.0;
-   out_4001675034436284807[48] = 0.0;
-   out_4001675034436284807[49] = 0.0;
-   out_4001675034436284807[50] = 1.0;
-   out_4001675034436284807[51] = 0.0;
-   out_4001675034436284807[52] = 0.0;
-   out_4001675034436284807[53] = 0.0;
-   out_4001675034436284807[54] = 0.0;
-   out_4001675034436284807[55] = 0.0;
-   out_4001675034436284807[56] = 0.0;
-   out_4001675034436284807[57] = 0.0;
-   out_4001675034436284807[58] = 0.0;
-   out_4001675034436284807[59] = 0.0;
-   out_4001675034436284807[60] = 1.0;
-   out_4001675034436284807[61] = 0.0;
-   out_4001675034436284807[62] = 0.0;
-   out_4001675034436284807[63] = 0.0;
-   out_4001675034436284807[64] = 0.0;
-   out_4001675034436284807[65] = 0.0;
-   out_4001675034436284807[66] = 0.0;
-   out_4001675034436284807[67] = 0.0;
-   out_4001675034436284807[68] = 0.0;
-   out_4001675034436284807[69] = 0.0;
-   out_4001675034436284807[70] = 1.0;
-   out_4001675034436284807[71] = 0.0;
-   out_4001675034436284807[72] = 0.0;
-   out_4001675034436284807[73] = 0.0;
-   out_4001675034436284807[74] = 0.0;
-   out_4001675034436284807[75] = 0.0;
-   out_4001675034436284807[76] = 0.0;
-   out_4001675034436284807[77] = 0.0;
-   out_4001675034436284807[78] = 0.0;
-   out_4001675034436284807[79] = 0.0;
-   out_4001675034436284807[80] = 1.0;
+void H_mod_fun(double *state, double *out_636098599351187827) {
+   out_636098599351187827[0] = 1.0;
+   out_636098599351187827[1] = 0.0;
+   out_636098599351187827[2] = 0.0;
+   out_636098599351187827[3] = 0.0;
+   out_636098599351187827[4] = 0.0;
+   out_636098599351187827[5] = 0.0;
+   out_636098599351187827[6] = 0.0;
+   out_636098599351187827[7] = 0.0;
+   out_636098599351187827[8] = 0.0;
+   out_636098599351187827[9] = 0.0;
+   out_636098599351187827[10] = 1.0;
+   out_636098599351187827[11] = 0.0;
+   out_636098599351187827[12] = 0.0;
+   out_636098599351187827[13] = 0.0;
+   out_636098599351187827[14] = 0.0;
+   out_636098599351187827[15] = 0.0;
+   out_636098599351187827[16] = 0.0;
+   out_636098599351187827[17] = 0.0;
+   out_636098599351187827[18] = 0.0;
+   out_636098599351187827[19] = 0.0;
+   out_636098599351187827[20] = 1.0;
+   out_636098599351187827[21] = 0.0;
+   out_636098599351187827[22] = 0.0;
+   out_636098599351187827[23] = 0.0;
+   out_636098599351187827[24] = 0.0;
+   out_636098599351187827[25] = 0.0;
+   out_636098599351187827[26] = 0.0;
+   out_636098599351187827[27] = 0.0;
+   out_636098599351187827[28] = 0.0;
+   out_636098599351187827[29] = 0.0;
+   out_636098599351187827[30] = 1.0;
+   out_636098599351187827[31] = 0.0;
+   out_636098599351187827[32] = 0.0;
+   out_636098599351187827[33] = 0.0;
+   out_636098599351187827[34] = 0.0;
+   out_636098599351187827[35] = 0.0;
+   out_636098599351187827[36] = 0.0;
+   out_636098599351187827[37] = 0.0;
+   out_636098599351187827[38] = 0.0;
+   out_636098599351187827[39] = 0.0;
+   out_636098599351187827[40] = 1.0;
+   out_636098599351187827[41] = 0.0;
+   out_636098599351187827[42] = 0.0;
+   out_636098599351187827[43] = 0.0;
+   out_636098599351187827[44] = 0.0;
+   out_636098599351187827[45] = 0.0;
+   out_636098599351187827[46] = 0.0;
+   out_636098599351187827[47] = 0.0;
+   out_636098599351187827[48] = 0.0;
+   out_636098599351187827[49] = 0.0;
+   out_636098599351187827[50] = 1.0;
+   out_636098599351187827[51] = 0.0;
+   out_636098599351187827[52] = 0.0;
+   out_636098599351187827[53] = 0.0;
+   out_636098599351187827[54] = 0.0;
+   out_636098599351187827[55] = 0.0;
+   out_636098599351187827[56] = 0.0;
+   out_636098599351187827[57] = 0.0;
+   out_636098599351187827[58] = 0.0;
+   out_636098599351187827[59] = 0.0;
+   out_636098599351187827[60] = 1.0;
+   out_636098599351187827[61] = 0.0;
+   out_636098599351187827[62] = 0.0;
+   out_636098599351187827[63] = 0.0;
+   out_636098599351187827[64] = 0.0;
+   out_636098599351187827[65] = 0.0;
+   out_636098599351187827[66] = 0.0;
+   out_636098599351187827[67] = 0.0;
+   out_636098599351187827[68] = 0.0;
+   out_636098599351187827[69] = 0.0;
+   out_636098599351187827[70] = 1.0;
+   out_636098599351187827[71] = 0.0;
+   out_636098599351187827[72] = 0.0;
+   out_636098599351187827[73] = 0.0;
+   out_636098599351187827[74] = 0.0;
+   out_636098599351187827[75] = 0.0;
+   out_636098599351187827[76] = 0.0;
+   out_636098599351187827[77] = 0.0;
+   out_636098599351187827[78] = 0.0;
+   out_636098599351187827[79] = 0.0;
+   out_636098599351187827[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_6995360374056238489) {
-   out_6995360374056238489[0] = state[0];
-   out_6995360374056238489[1] = state[1];
-   out_6995360374056238489[2] = state[2];
-   out_6995360374056238489[3] = state[3];
-   out_6995360374056238489[4] = state[4];
-   out_6995360374056238489[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
-   out_6995360374056238489[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
-   out_6995360374056238489[7] = state[7];
-   out_6995360374056238489[8] = state[8];
+void f_fun(double *state, double dt, double *out_190909160143243867) {
+   out_190909160143243867[0] = state[0];
+   out_190909160143243867[1] = state[1];
+   out_190909160143243867[2] = state[2];
+   out_190909160143243867[3] = state[3];
+   out_190909160143243867[4] = state[4];
+   out_190909160143243867[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
+   out_190909160143243867[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
+   out_190909160143243867[7] = state[7];
+   out_190909160143243867[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_5620828310005666299) {
-   out_5620828310005666299[0] = 1;
-   out_5620828310005666299[1] = 0;
-   out_5620828310005666299[2] = 0;
-   out_5620828310005666299[3] = 0;
-   out_5620828310005666299[4] = 0;
-   out_5620828310005666299[5] = 0;
-   out_5620828310005666299[6] = 0;
-   out_5620828310005666299[7] = 0;
-   out_5620828310005666299[8] = 0;
-   out_5620828310005666299[9] = 0;
-   out_5620828310005666299[10] = 1;
-   out_5620828310005666299[11] = 0;
-   out_5620828310005666299[12] = 0;
-   out_5620828310005666299[13] = 0;
-   out_5620828310005666299[14] = 0;
-   out_5620828310005666299[15] = 0;
-   out_5620828310005666299[16] = 0;
-   out_5620828310005666299[17] = 0;
-   out_5620828310005666299[18] = 0;
-   out_5620828310005666299[19] = 0;
-   out_5620828310005666299[20] = 1;
-   out_5620828310005666299[21] = 0;
-   out_5620828310005666299[22] = 0;
-   out_5620828310005666299[23] = 0;
-   out_5620828310005666299[24] = 0;
-   out_5620828310005666299[25] = 0;
-   out_5620828310005666299[26] = 0;
-   out_5620828310005666299[27] = 0;
-   out_5620828310005666299[28] = 0;
-   out_5620828310005666299[29] = 0;
-   out_5620828310005666299[30] = 1;
-   out_5620828310005666299[31] = 0;
-   out_5620828310005666299[32] = 0;
-   out_5620828310005666299[33] = 0;
-   out_5620828310005666299[34] = 0;
-   out_5620828310005666299[35] = 0;
-   out_5620828310005666299[36] = 0;
-   out_5620828310005666299[37] = 0;
-   out_5620828310005666299[38] = 0;
-   out_5620828310005666299[39] = 0;
-   out_5620828310005666299[40] = 1;
-   out_5620828310005666299[41] = 0;
-   out_5620828310005666299[42] = 0;
-   out_5620828310005666299[43] = 0;
-   out_5620828310005666299[44] = 0;
-   out_5620828310005666299[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
-   out_5620828310005666299[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_5620828310005666299[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_5620828310005666299[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_5620828310005666299[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
-   out_5620828310005666299[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_5620828310005666299[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_5620828310005666299[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_5620828310005666299[53] = -9.8100000000000005*dt;
-   out_5620828310005666299[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
-   out_5620828310005666299[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_5620828310005666299[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_5620828310005666299[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_5620828310005666299[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
-   out_5620828310005666299[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_5620828310005666299[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
-   out_5620828310005666299[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_5620828310005666299[62] = 0;
-   out_5620828310005666299[63] = 0;
-   out_5620828310005666299[64] = 0;
-   out_5620828310005666299[65] = 0;
-   out_5620828310005666299[66] = 0;
-   out_5620828310005666299[67] = 0;
-   out_5620828310005666299[68] = 0;
-   out_5620828310005666299[69] = 0;
-   out_5620828310005666299[70] = 1;
-   out_5620828310005666299[71] = 0;
-   out_5620828310005666299[72] = 0;
-   out_5620828310005666299[73] = 0;
-   out_5620828310005666299[74] = 0;
-   out_5620828310005666299[75] = 0;
-   out_5620828310005666299[76] = 0;
-   out_5620828310005666299[77] = 0;
-   out_5620828310005666299[78] = 0;
-   out_5620828310005666299[79] = 0;
-   out_5620828310005666299[80] = 1;
+void F_fun(double *state, double dt, double *out_1195327144916471551) {
+   out_1195327144916471551[0] = 1;
+   out_1195327144916471551[1] = 0;
+   out_1195327144916471551[2] = 0;
+   out_1195327144916471551[3] = 0;
+   out_1195327144916471551[4] = 0;
+   out_1195327144916471551[5] = 0;
+   out_1195327144916471551[6] = 0;
+   out_1195327144916471551[7] = 0;
+   out_1195327144916471551[8] = 0;
+   out_1195327144916471551[9] = 0;
+   out_1195327144916471551[10] = 1;
+   out_1195327144916471551[11] = 0;
+   out_1195327144916471551[12] = 0;
+   out_1195327144916471551[13] = 0;
+   out_1195327144916471551[14] = 0;
+   out_1195327144916471551[15] = 0;
+   out_1195327144916471551[16] = 0;
+   out_1195327144916471551[17] = 0;
+   out_1195327144916471551[18] = 0;
+   out_1195327144916471551[19] = 0;
+   out_1195327144916471551[20] = 1;
+   out_1195327144916471551[21] = 0;
+   out_1195327144916471551[22] = 0;
+   out_1195327144916471551[23] = 0;
+   out_1195327144916471551[24] = 0;
+   out_1195327144916471551[25] = 0;
+   out_1195327144916471551[26] = 0;
+   out_1195327144916471551[27] = 0;
+   out_1195327144916471551[28] = 0;
+   out_1195327144916471551[29] = 0;
+   out_1195327144916471551[30] = 1;
+   out_1195327144916471551[31] = 0;
+   out_1195327144916471551[32] = 0;
+   out_1195327144916471551[33] = 0;
+   out_1195327144916471551[34] = 0;
+   out_1195327144916471551[35] = 0;
+   out_1195327144916471551[36] = 0;
+   out_1195327144916471551[37] = 0;
+   out_1195327144916471551[38] = 0;
+   out_1195327144916471551[39] = 0;
+   out_1195327144916471551[40] = 1;
+   out_1195327144916471551[41] = 0;
+   out_1195327144916471551[42] = 0;
+   out_1195327144916471551[43] = 0;
+   out_1195327144916471551[44] = 0;
+   out_1195327144916471551[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
+   out_1195327144916471551[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_1195327144916471551[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_1195327144916471551[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_1195327144916471551[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
+   out_1195327144916471551[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_1195327144916471551[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_1195327144916471551[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_1195327144916471551[53] = -9.8100000000000005*dt;
+   out_1195327144916471551[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
+   out_1195327144916471551[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_1195327144916471551[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_1195327144916471551[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_1195327144916471551[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
+   out_1195327144916471551[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_1195327144916471551[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
+   out_1195327144916471551[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_1195327144916471551[62] = 0;
+   out_1195327144916471551[63] = 0;
+   out_1195327144916471551[64] = 0;
+   out_1195327144916471551[65] = 0;
+   out_1195327144916471551[66] = 0;
+   out_1195327144916471551[67] = 0;
+   out_1195327144916471551[68] = 0;
+   out_1195327144916471551[69] = 0;
+   out_1195327144916471551[70] = 1;
+   out_1195327144916471551[71] = 0;
+   out_1195327144916471551[72] = 0;
+   out_1195327144916471551[73] = 0;
+   out_1195327144916471551[74] = 0;
+   out_1195327144916471551[75] = 0;
+   out_1195327144916471551[76] = 0;
+   out_1195327144916471551[77] = 0;
+   out_1195327144916471551[78] = 0;
+   out_1195327144916471551[79] = 0;
+   out_1195327144916471551[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_5932710635121359621) {
-   out_5932710635121359621[0] = state[6];
+void h_25(double *state, double *unused, double *out_8972033615374819768) {
+   out_8972033615374819768[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_4507009476636581519) {
-   out_4507009476636581519[0] = 0;
-   out_4507009476636581519[1] = 0;
-   out_4507009476636581519[2] = 0;
-   out_4507009476636581519[3] = 0;
-   out_4507009476636581519[4] = 0;
-   out_4507009476636581519[5] = 0;
-   out_4507009476636581519[6] = 1;
-   out_4507009476636581519[7] = 0;
-   out_4507009476636581519[8] = 0;
+void H_25(double *state, double *unused, double *out_4267593225833477013) {
+   out_4267593225833477013[0] = 0;
+   out_4267593225833477013[1] = 0;
+   out_4267593225833477013[2] = 0;
+   out_4267593225833477013[3] = 0;
+   out_4267593225833477013[4] = 0;
+   out_4267593225833477013[5] = 0;
+   out_4267593225833477013[6] = 1;
+   out_4267593225833477013[7] = 0;
+   out_4267593225833477013[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_8960081603047522700) {
-   out_8960081603047522700[0] = state[4];
-   out_8960081603047522700[1] = state[5];
+void h_24(double *state, double *unused, double *out_1619093620334214225) {
+   out_1619093620334214225[0] = state[4];
+   out_1619093620334214225[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_7364162790481451996) {
-   out_7364162790481451996[0] = 0;
-   out_7364162790481451996[1] = 0;
-   out_7364162790481451996[2] = 0;
-   out_7364162790481451996[3] = 0;
-   out_7364162790481451996[4] = 1;
-   out_7364162790481451996[5] = 0;
-   out_7364162790481451996[6] = 0;
-   out_7364162790481451996[7] = 0;
-   out_7364162790481451996[8] = 0;
-   out_7364162790481451996[9] = 0;
-   out_7364162790481451996[10] = 0;
-   out_7364162790481451996[11] = 0;
-   out_7364162790481451996[12] = 0;
-   out_7364162790481451996[13] = 0;
-   out_7364162790481451996[14] = 1;
-   out_7364162790481451996[15] = 0;
-   out_7364162790481451996[16] = 0;
-   out_7364162790481451996[17] = 0;
+void H_24(double *state, double *unused, double *out_2538730106656162944) {
+   out_2538730106656162944[0] = 0;
+   out_2538730106656162944[1] = 0;
+   out_2538730106656162944[2] = 0;
+   out_2538730106656162944[3] = 0;
+   out_2538730106656162944[4] = 1;
+   out_2538730106656162944[5] = 0;
+   out_2538730106656162944[6] = 0;
+   out_2538730106656162944[7] = 0;
+   out_2538730106656162944[8] = 0;
+   out_2538730106656162944[9] = 0;
+   out_2538730106656162944[10] = 0;
+   out_2538730106656162944[11] = 0;
+   out_2538730106656162944[12] = 0;
+   out_2538730106656162944[13] = 0;
+   out_2538730106656162944[14] = 1;
+   out_2538730106656162944[15] = 0;
+   out_2538730106656162944[16] = 0;
+   out_2538730106656162944[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_6293656255741178026) {
-   out_6293656255741178026[0] = state[4];
+void h_30(double *state, double *unused, double *out_2004718450693787274) {
+   out_2004718450693787274[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_7023044255581353342) {
-   out_7023044255581353342[0] = 0;
-   out_7023044255581353342[1] = 0;
-   out_7023044255581353342[2] = 0;
-   out_7023044255581353342[3] = 0;
-   out_7023044255581353342[4] = 1;
-   out_7023044255581353342[5] = 0;
-   out_7023044255581353342[6] = 0;
-   out_7023044255581353342[7] = 0;
-   out_7023044255581353342[8] = 0;
+void H_30(double *state, double *unused, double *out_6785926184340725640) {
+   out_6785926184340725640[0] = 0;
+   out_6785926184340725640[1] = 0;
+   out_6785926184340725640[2] = 0;
+   out_6785926184340725640[3] = 0;
+   out_6785926184340725640[4] = 1;
+   out_6785926184340725640[5] = 0;
+   out_6785926184340725640[6] = 0;
+   out_6785926184340725640[7] = 0;
+   out_6785926184340725640[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_7151414550307527945) {
-   out_7151414550307527945[0] = state[7];
+void h_26(double *state, double *unused, double *out_7006391718354632919) {
+   out_7006391718354632919[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_765506157762525295) {
-   out_765506157762525295[0] = 0;
-   out_765506157762525295[1] = 0;
-   out_765506157762525295[2] = 0;
-   out_765506157762525295[3] = 0;
-   out_765506157762525295[4] = 0;
-   out_765506157762525295[5] = 0;
-   out_765506157762525295[6] = 0;
-   out_765506157762525295[7] = 1;
-   out_765506157762525295[8] = 0;
+void H_26(double *state, double *unused, double *out_7572119195594277614) {
+   out_7572119195594277614[0] = 0;
+   out_7572119195594277614[1] = 0;
+   out_7572119195594277614[2] = 0;
+   out_7572119195594277614[3] = 0;
+   out_7572119195594277614[4] = 0;
+   out_7572119195594277614[5] = 0;
+   out_7572119195594277614[6] = 0;
+   out_7572119195594277614[7] = 1;
+   out_7572119195594277614[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_7512360170927346350) {
-   out_7512360170927346350[0] = state[3];
+void h_27(double *state, double *unused, double *out_4512330460653438616) {
+   out_4512330460653438616[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_9197807567381778253) {
-   out_9197807567381778253[0] = 0;
-   out_9197807567381778253[1] = 0;
-   out_9197807567381778253[2] = 0;
-   out_9197807567381778253[3] = 1;
-   out_9197807567381778253[4] = 0;
-   out_9197807567381778253[5] = 0;
-   out_9197807567381778253[6] = 0;
-   out_9197807567381778253[7] = 0;
-   out_9197807567381778253[8] = 0;
+void H_27(double *state, double *unused, double *out_9009520255524668857) {
+   out_9009520255524668857[0] = 0;
+   out_9009520255524668857[1] = 0;
+   out_9009520255524668857[2] = 0;
+   out_9009520255524668857[3] = 1;
+   out_9009520255524668857[4] = 0;
+   out_9009520255524668857[5] = 0;
+   out_9009520255524668857[6] = 0;
+   out_9009520255524668857[7] = 0;
+   out_9009520255524668857[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_3171118645961532825) {
-   out_3171118645961532825[0] = state[1];
+void h_29(double *state, double *unused, double *out_2948228303595449709) {
+   out_2948228303595449709[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_6512812911266961158) {
-   out_6512812911266961158[0] = 0;
-   out_6512812911266961158[1] = 1;
-   out_6512812911266961158[2] = 0;
-   out_6512812911266961158[3] = 0;
-   out_6512812911266961158[4] = 0;
-   out_6512812911266961158[5] = 0;
-   out_6512812911266961158[6] = 0;
-   out_6512812911266961158[7] = 0;
-   out_6512812911266961158[8] = 0;
+void H_29(double *state, double *unused, double *out_7296157528655117824) {
+   out_7296157528655117824[0] = 0;
+   out_7296157528655117824[1] = 1;
+   out_7296157528655117824[2] = 0;
+   out_7296157528655117824[3] = 0;
+   out_7296157528655117824[4] = 0;
+   out_7296157528655117824[5] = 0;
+   out_7296157528655117824[6] = 0;
+   out_7296157528655117824[7] = 0;
+   out_7296157528655117824[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_8329978581973507615) {
-   out_8329978581973507615[0] = state[0];
+void h_28(double *state, double *unused, double *out_1907925139844740965) {
+   out_1907925139844740965[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_2453174762388691756) {
-   out_2453174762388691756[0] = 1;
-   out_2453174762388691756[1] = 0;
-   out_2453174762388691756[2] = 0;
-   out_2453174762388691756[3] = 0;
-   out_2453174762388691756[4] = 0;
-   out_2453174762388691756[5] = 0;
-   out_2453174762388691756[6] = 0;
-   out_2453174762388691756[7] = 0;
-   out_2453174762388691756[8] = 0;
+void H_28(double *state, double *unused, double *out_2213758511585587250) {
+   out_2213758511585587250[0] = 1;
+   out_2213758511585587250[1] = 0;
+   out_2213758511585587250[2] = 0;
+   out_2213758511585587250[3] = 0;
+   out_2213758511585587250[4] = 0;
+   out_2213758511585587250[5] = 0;
+   out_2213758511585587250[6] = 0;
+   out_2213758511585587250[7] = 0;
+   out_2213758511585587250[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_222736560719300508) {
-   out_222736560719300508[0] = state[8];
+void h_31(double *state, double *unused, double *out_332156138656277884) {
+   out_332156138656277884[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_4537655438513541947) {
-   out_4537655438513541947[0] = 0;
-   out_4537655438513541947[1] = 0;
-   out_4537655438513541947[2] = 0;
-   out_4537655438513541947[3] = 0;
-   out_4537655438513541947[4] = 0;
-   out_4537655438513541947[5] = 0;
-   out_4537655438513541947[6] = 0;
-   out_4537655438513541947[7] = 0;
-   out_4537655438513541947[8] = 1;
+void H_31(double *state, double *unused, double *out_4298239187710437441) {
+   out_4298239187710437441[0] = 0;
+   out_4298239187710437441[1] = 0;
+   out_4298239187710437441[2] = 0;
+   out_4298239187710437441[3] = 0;
+   out_4298239187710437441[4] = 0;
+   out_4298239187710437441[5] = 0;
+   out_4298239187710437441[6] = 0;
+   out_4298239187710437441[7] = 0;
+   out_4298239187710437441[8] = 1;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
  update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
 }
-void car_err_fun(double *nom_x, double *delta_x, double *out_587611173581676283) {
-  err_fun(nom_x, delta_x, out_587611173581676283);
+void car_err_fun(double *nom_x, double *delta_x, double *out_652600290238585961) {
+  err_fun(nom_x, delta_x, out_652600290238585961);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_8120728011522269919) {
-  inv_err_fun(nom_x, true_x, out_8120728011522269919);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_428056314371954903) {
+  inv_err_fun(nom_x, true_x, out_428056314371954903);
 }
-void car_H_mod_fun(double *state, double *out_4001675034436284807) {
-  H_mod_fun(state, out_4001675034436284807);
+void car_H_mod_fun(double *state, double *out_636098599351187827) {
+  H_mod_fun(state, out_636098599351187827);
 }
-void car_f_fun(double *state, double dt, double *out_6995360374056238489) {
-  f_fun(state,  dt, out_6995360374056238489);
+void car_f_fun(double *state, double dt, double *out_190909160143243867) {
+  f_fun(state,  dt, out_190909160143243867);
 }
-void car_F_fun(double *state, double dt, double *out_5620828310005666299) {
-  F_fun(state,  dt, out_5620828310005666299);
+void car_F_fun(double *state, double dt, double *out_1195327144916471551) {
+  F_fun(state,  dt, out_1195327144916471551);
 }
-void car_h_25(double *state, double *unused, double *out_5932710635121359621) {
-  h_25(state, unused, out_5932710635121359621);
+void car_h_25(double *state, double *unused, double *out_8972033615374819768) {
+  h_25(state, unused, out_8972033615374819768);
 }
-void car_H_25(double *state, double *unused, double *out_4507009476636581519) {
-  H_25(state, unused, out_4507009476636581519);
+void car_H_25(double *state, double *unused, double *out_4267593225833477013) {
+  H_25(state, unused, out_4267593225833477013);
 }
-void car_h_24(double *state, double *unused, double *out_8960081603047522700) {
-  h_24(state, unused, out_8960081603047522700);
+void car_h_24(double *state, double *unused, double *out_1619093620334214225) {
+  h_24(state, unused, out_1619093620334214225);
 }
-void car_H_24(double *state, double *unused, double *out_7364162790481451996) {
-  H_24(state, unused, out_7364162790481451996);
+void car_H_24(double *state, double *unused, double *out_2538730106656162944) {
+  H_24(state, unused, out_2538730106656162944);
 }
-void car_h_30(double *state, double *unused, double *out_6293656255741178026) {
-  h_30(state, unused, out_6293656255741178026);
+void car_h_30(double *state, double *unused, double *out_2004718450693787274) {
+  h_30(state, unused, out_2004718450693787274);
 }
-void car_H_30(double *state, double *unused, double *out_7023044255581353342) {
-  H_30(state, unused, out_7023044255581353342);
+void car_H_30(double *state, double *unused, double *out_6785926184340725640) {
+  H_30(state, unused, out_6785926184340725640);
 }
-void car_h_26(double *state, double *unused, double *out_7151414550307527945) {
-  h_26(state, unused, out_7151414550307527945);
+void car_h_26(double *state, double *unused, double *out_7006391718354632919) {
+  h_26(state, unused, out_7006391718354632919);
 }
-void car_H_26(double *state, double *unused, double *out_765506157762525295) {
-  H_26(state, unused, out_765506157762525295);
+void car_H_26(double *state, double *unused, double *out_7572119195594277614) {
+  H_26(state, unused, out_7572119195594277614);
 }
-void car_h_27(double *state, double *unused, double *out_7512360170927346350) {
-  h_27(state, unused, out_7512360170927346350);
+void car_h_27(double *state, double *unused, double *out_4512330460653438616) {
+  h_27(state, unused, out_4512330460653438616);
 }
-void car_H_27(double *state, double *unused, double *out_9197807567381778253) {
-  H_27(state, unused, out_9197807567381778253);
+void car_H_27(double *state, double *unused, double *out_9009520255524668857) {
+  H_27(state, unused, out_9009520255524668857);
 }
-void car_h_29(double *state, double *unused, double *out_3171118645961532825) {
-  h_29(state, unused, out_3171118645961532825);
+void car_h_29(double *state, double *unused, double *out_2948228303595449709) {
+  h_29(state, unused, out_2948228303595449709);
 }
-void car_H_29(double *state, double *unused, double *out_6512812911266961158) {
-  H_29(state, unused, out_6512812911266961158);
+void car_H_29(double *state, double *unused, double *out_7296157528655117824) {
+  H_29(state, unused, out_7296157528655117824);
 }
-void car_h_28(double *state, double *unused, double *out_8329978581973507615) {
-  h_28(state, unused, out_8329978581973507615);
+void car_h_28(double *state, double *unused, double *out_1907925139844740965) {
+  h_28(state, unused, out_1907925139844740965);
 }
-void car_H_28(double *state, double *unused, double *out_2453174762388691756) {
-  H_28(state, unused, out_2453174762388691756);
+void car_H_28(double *state, double *unused, double *out_2213758511585587250) {
+  H_28(state, unused, out_2213758511585587250);
 }
-void car_h_31(double *state, double *unused, double *out_222736560719300508) {
-  h_31(state, unused, out_222736560719300508);
+void car_h_31(double *state, double *unused, double *out_332156138656277884) {
+  h_31(state, unused, out_332156138656277884);
 }
-void car_H_31(double *state, double *unused, double *out_4537655438513541947) {
-  H_31(state, unused, out_4537655438513541947);
+void car_H_31(double *state, double *unused, double *out_4298239187710437441) {
+  H_31(state, unused, out_4298239187710437441);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
  predict(in_x, in_P, in_Q, dt);
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_587611173581676283);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_8120728011522269919);
-void car_H_mod_fun(double *state, double *out_4001675034436284807);
-void car_f_fun(double *state, double dt, double *out_6995360374056238489);
-void car_F_fun(double *state, double dt, double *out_5620828310005666299);
-void car_h_25(double *state, double *unused, double *out_5932710635121359621);
-void car_H_25(double *state, double *unused, double *out_4507009476636581519);
-void car_h_24(double *state, double *unused, double *out_8960081603047522700);
-void car_H_24(double *state, double *unused, double *out_7364162790481451996);
-void car_h_30(double *state, double *unused, double *out_6293656255741178026);
-void car_H_30(double *state, double *unused, double *out_7023044255581353342);
-void car_h_26(double *state, double *unused, double *out_7151414550307527945);
-void car_H_26(double *state, double *unused, double *out_765506157762525295);
-void car_h_27(double *state, double *unused, double *out_7512360170927346350);
-void car_H_27(double *state, double *unused, double *out_9197807567381778253);
-void car_h_29(double *state, double *unused, double *out_3171118645961532825);
-void car_H_29(double *state, double *unused, double *out_6512812911266961158);
-void car_h_28(double *state, double *unused, double *out_8329978581973507615);
-void car_H_28(double *state, double *unused, double *out_2453174762388691756);
-void car_h_31(double *state, double *unused, double *out_222736560719300508);
-void car_H_31(double *state, double *unused, double *out_4537655438513541947);
+void car_err_fun(double *nom_x, double *delta_x, double *out_652600290238585961);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_428056314371954903);
+void car_H_mod_fun(double *state, double *out_636098599351187827);
+void car_f_fun(double *state, double dt, double *out_190909160143243867);
+void car_F_fun(double *state, double dt, double *out_1195327144916471551);
+void car_h_25(double *state, double *unused, double *out_8972033615374819768);
+void car_H_25(double *state, double *unused, double *out_4267593225833477013);
+void car_h_24(double *state, double *unused, double *out_1619093620334214225);
+void car_H_24(double *state, double *unused, double *out_2538730106656162944);
+void car_h_30(double *state, double *unused, double *out_2004718450693787274);
+void car_H_30(double *state, double *unused, double *out_6785926184340725640);
+void car_h_26(double *state, double *unused, double *out_7006391718354632919);
+void car_H_26(double *state, double *unused, double *out_7572119195594277614);
+void car_h_27(double *state, double *unused, double *out_4512330460653438616);
+void car_H_27(double *state, double *unused, double *out_9009520255524668857);
+void car_h_29(double *state, double *unused, double *out_2948228303595449709);
+void car_H_29(double *state, double *unused, double *out_7296157528655117824);
+void car_h_28(double *state, double *unused, double *out_1907925139844740965);
+void car_H_28(double *state, double *unused, double *out_2213758511585587250);
+void car_h_31(double *state, double *unused, double *out_332156138656277884);
+void car_H_31(double *state, double *unused, double *out_4298239187710437441);
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
 void car_set_mass(double x);
 void car_set_rotational_inertia(double x);
@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void pose_err_fun(double *nom_x, double *delta_x, double *out_7704133318394013706);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5732870200970735889);
-void pose_H_mod_fun(double *state, double *out_5613029472060395452);
-void pose_f_fun(double *state, double dt, double *out_3560257805552383557);
-void pose_F_fun(double *state, double dt, double *out_8405700866591566947);
-void pose_h_4(double *state, double *unused, double *out_6714333113820678959);
-void pose_H_4(double *state, double *unused, double *out_6985632794897197443);
-void pose_h_10(double *state, double *unused, double *out_1436740119579277140);
-void pose_H_10(double *state, double *unused, double *out_1336760473497929829);
-void pose_h_13(double *state, double *unused, double *out_8610784800470542827);
-void pose_H_13(double *state, double *unused, double *out_8248837453480021372);
-void pose_h_14(double *state, double *unused, double *out_7905030664698657319);
-void pose_H_14(double *state, double *unused, double *out_6550516268252313844);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_6520327552806096841);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3933583929675800728);
+void pose_H_mod_fun(double *state, double *out_6974741239016485936);
+void pose_f_fun(double *state, double dt, double *out_7579043913228931093);
+void pose_F_fun(double *state, double dt, double *out_7552939469662313928);
+void pose_h_4(double *state, double *unused, double *out_3550900755285532144);
+void pose_H_4(double *state, double *unused, double *out_3202934029172578364);
+void pose_h_10(double *state, double *unused, double *out_7067714338468602861);
+void pose_H_10(double *state, double *unused, double *out_4832878358593471680);
+void pose_h_13(double *state, double *unused, double *out_6451839863147612103);
+void pose_H_13(double *state, double *unused, double *out_9339796159754437);
+void pose_h_14(double *state, double *unused, double *out_3486571009539339858);
+void pose_H_14(double *state, double *unused, double *out_6285722461467950660);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }