diff --git a/common/libcommon.a b/common/libcommon.a index a9b709e90..d677cf3d3 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index a01e645f9..d62346674 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 595e3b7e4..84d2645e8 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/bootstub.elf b/panda/board/obj/body_h7/bootstub.elf index 36d5ee6d2..2fa00d948 100755 Binary files a/panda/board/obj/body_h7/bootstub.elf and b/panda/board/obj/body_h7/bootstub.elf differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index 40bd36ef9..7d2947639 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index 1bf155e71..9cbe7cb85 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index 14f4fcbb5..280836cd6 100755 Binary files a/panda/board/obj/bootstub.body_h7.bin and b/panda/board/obj/bootstub.body_h7.bin differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index e6520d6a4..f3b9db16a 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index 18e52f7ea..3b510570b 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote.bin b/panda/board/obj/bootstub.panda_h7_remote.bin index 18e52f7ea..3b510570b 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote.bin and b/panda/board/obj/bootstub.panda_h7_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_jungle_h7.bin b/panda/board/obj/bootstub.panda_jungle_h7.bin index f06cc17d2..0bd11e83a 100755 Binary files a/panda/board/obj/bootstub.panda_jungle_h7.bin and b/panda/board/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_remote.bin b/panda/board/obj/bootstub.panda_remote.bin index e6520d6a4..f3b9db16a 100755 Binary files a/panda/board/obj/bootstub.panda_remote.bin and b/panda/board/obj/bootstub.panda_remote.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index 32d1b5df7..6a64bcd64 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -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"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 646456850..310ff2b56 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda/bootstub.elf b/panda/board/obj/panda/bootstub.elf index 10c244af3..6fdcfdb25 100755 Binary files a/panda/board/obj/panda/bootstub.elf and b/panda/board/obj/panda/bootstub.elf differ diff --git a/panda/board/obj/panda/main.bin b/panda/board/obj/panda/main.bin index a24c29993..2a51a3993 100755 Binary files a/panda/board/obj/panda/main.bin and b/panda/board/obj/panda/main.bin differ diff --git a/panda/board/obj/panda/main.elf b/panda/board/obj/panda/main.elf index 7b4547ad1..e386927d2 100755 Binary files a/panda/board/obj/panda/main.elf and b/panda/board/obj/panda/main.elf differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index dcb5eb78f..40916113c 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/bootstub.elf b/panda/board/obj/panda_h7/bootstub.elf index 47b59e3ab..1808c5fac 100755 Binary files a/panda/board/obj/panda_h7/bootstub.elf and b/panda/board/obj/panda_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index 7b4753217..72ee81ca5 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index 673171001..e2e61c1cc 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index 66bd7d420..8f788d6f5 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote/bootstub.elf b/panda/board/obj/panda_h7_remote/bootstub.elf index 9773d34c5..7fc503215 100755 Binary files a/panda/board/obj/panda_h7_remote/bootstub.elf and b/panda/board/obj/panda_h7_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote/main.bin b/panda/board/obj/panda_h7_remote/main.bin index eccab39e8..b6927ac02 100755 Binary files a/panda/board/obj/panda_h7_remote/main.bin and b/panda/board/obj/panda_h7_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_remote/main.elf b/panda/board/obj/panda_h7_remote/main.elf index 2b9d23363..c1055a5e7 100755 Binary files a/panda/board/obj/panda_h7_remote/main.elf and b/panda/board/obj/panda_h7_remote/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index 1fbcaf127..d46d7f1ed 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/bootstub.elf b/panda/board/obj/panda_jungle_h7/bootstub.elf index 163467550..ccf44b0f1 100755 Binary files a/panda/board/obj/panda_jungle_h7/bootstub.elf and b/panda/board/obj/panda_jungle_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index 12e67d70f..c86d69e87 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index f8525e80c..618bfa508 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed index c268a2f67..5c2fcc6b7 100644 Binary files a/panda/board/obj/panda_remote.bin.signed and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote/bootstub.elf b/panda/board/obj/panda_remote/bootstub.elf index 09433b7cf..fba0c0d3e 100755 Binary files a/panda/board/obj/panda_remote/bootstub.elf and b/panda/board/obj/panda_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_remote/main.bin b/panda/board/obj/panda_remote/main.bin index f5c6cdf65..38ded52c4 100755 Binary files a/panda/board/obj/panda_remote/main.bin and b/panda/board/obj/panda_remote/main.bin differ diff --git a/panda/board/obj/panda_remote/main.elf b/panda/board/obj/panda_remote/main.elf index c3783ff75..557b08cf4 100755 Binary files a/panda/board/obj/panda_remote/main.elf and b/panda/board/obj/panda_remote/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index e67a37ad2..e120c9072 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-9d8a055a-DEBUG \ No newline at end of file +DEV-3f8d368b-DEBUG \ No newline at end of file diff --git a/scripts/laptop_device_build.sh b/scripts/laptop_device_build.sh index e4bfaec42..ddf81a6d2 100755 --- a/scripts/laptop_device_build.sh +++ b/scripts/laptop_device_build.sh @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 96f0f837d..d07dd5bfc 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index f38fac218..57ba1a09b 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -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) diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 732a0b140..716f03eee 100644 --- a/selfdrive/locationd/models/generated/car.cpp +++ b/selfdrive/locationd/models/generated/car.cpp @@ -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 #include @@ -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); diff --git a/selfdrive/locationd/models/generated/car.h b/selfdrive/locationd/models/generated/car.h index 803b6cd43..11c95d2db 100644 --- a/selfdrive/locationd/models/generated/car.h +++ b/selfdrive/locationd/models/generated/car.h @@ -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); diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index 1292bbef8..27335ea33 100644 --- a/selfdrive/locationd/models/generated/pose.cpp +++ b/selfdrive/locationd/models/generated/pose.cpp @@ -17,961 +17,961 @@ const static double MAHA_THRESH_14 = 7.814727903251177; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_7704133318394013706) { - out_7704133318394013706[0] = delta_x[0] + nom_x[0]; - out_7704133318394013706[1] = delta_x[1] + nom_x[1]; - out_7704133318394013706[2] = delta_x[2] + nom_x[2]; - out_7704133318394013706[3] = delta_x[3] + nom_x[3]; - out_7704133318394013706[4] = delta_x[4] + nom_x[4]; - out_7704133318394013706[5] = delta_x[5] + nom_x[5]; - out_7704133318394013706[6] = delta_x[6] + nom_x[6]; - out_7704133318394013706[7] = delta_x[7] + nom_x[7]; - out_7704133318394013706[8] = delta_x[8] + nom_x[8]; - out_7704133318394013706[9] = delta_x[9] + nom_x[9]; - out_7704133318394013706[10] = delta_x[10] + nom_x[10]; - out_7704133318394013706[11] = delta_x[11] + nom_x[11]; - out_7704133318394013706[12] = delta_x[12] + nom_x[12]; - out_7704133318394013706[13] = delta_x[13] + nom_x[13]; - out_7704133318394013706[14] = delta_x[14] + nom_x[14]; - out_7704133318394013706[15] = delta_x[15] + nom_x[15]; - out_7704133318394013706[16] = delta_x[16] + nom_x[16]; - out_7704133318394013706[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_6520327552806096841) { + out_6520327552806096841[0] = delta_x[0] + nom_x[0]; + out_6520327552806096841[1] = delta_x[1] + nom_x[1]; + out_6520327552806096841[2] = delta_x[2] + nom_x[2]; + out_6520327552806096841[3] = delta_x[3] + nom_x[3]; + out_6520327552806096841[4] = delta_x[4] + nom_x[4]; + out_6520327552806096841[5] = delta_x[5] + nom_x[5]; + out_6520327552806096841[6] = delta_x[6] + nom_x[6]; + out_6520327552806096841[7] = delta_x[7] + nom_x[7]; + out_6520327552806096841[8] = delta_x[8] + nom_x[8]; + out_6520327552806096841[9] = delta_x[9] + nom_x[9]; + out_6520327552806096841[10] = delta_x[10] + nom_x[10]; + out_6520327552806096841[11] = delta_x[11] + nom_x[11]; + out_6520327552806096841[12] = delta_x[12] + nom_x[12]; + out_6520327552806096841[13] = delta_x[13] + nom_x[13]; + out_6520327552806096841[14] = delta_x[14] + nom_x[14]; + out_6520327552806096841[15] = delta_x[15] + nom_x[15]; + out_6520327552806096841[16] = delta_x[16] + nom_x[16]; + out_6520327552806096841[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5732870200970735889) { - out_5732870200970735889[0] = -nom_x[0] + true_x[0]; - out_5732870200970735889[1] = -nom_x[1] + true_x[1]; - out_5732870200970735889[2] = -nom_x[2] + true_x[2]; - out_5732870200970735889[3] = -nom_x[3] + true_x[3]; - out_5732870200970735889[4] = -nom_x[4] + true_x[4]; - out_5732870200970735889[5] = -nom_x[5] + true_x[5]; - out_5732870200970735889[6] = -nom_x[6] + true_x[6]; - out_5732870200970735889[7] = -nom_x[7] + true_x[7]; - out_5732870200970735889[8] = -nom_x[8] + true_x[8]; - out_5732870200970735889[9] = -nom_x[9] + true_x[9]; - out_5732870200970735889[10] = -nom_x[10] + true_x[10]; - out_5732870200970735889[11] = -nom_x[11] + true_x[11]; - out_5732870200970735889[12] = -nom_x[12] + true_x[12]; - out_5732870200970735889[13] = -nom_x[13] + true_x[13]; - out_5732870200970735889[14] = -nom_x[14] + true_x[14]; - out_5732870200970735889[15] = -nom_x[15] + true_x[15]; - out_5732870200970735889[16] = -nom_x[16] + true_x[16]; - out_5732870200970735889[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3933583929675800728) { + out_3933583929675800728[0] = -nom_x[0] + true_x[0]; + out_3933583929675800728[1] = -nom_x[1] + true_x[1]; + out_3933583929675800728[2] = -nom_x[2] + true_x[2]; + out_3933583929675800728[3] = -nom_x[3] + true_x[3]; + out_3933583929675800728[4] = -nom_x[4] + true_x[4]; + out_3933583929675800728[5] = -nom_x[5] + true_x[5]; + out_3933583929675800728[6] = -nom_x[6] + true_x[6]; + out_3933583929675800728[7] = -nom_x[7] + true_x[7]; + out_3933583929675800728[8] = -nom_x[8] + true_x[8]; + out_3933583929675800728[9] = -nom_x[9] + true_x[9]; + out_3933583929675800728[10] = -nom_x[10] + true_x[10]; + out_3933583929675800728[11] = -nom_x[11] + true_x[11]; + out_3933583929675800728[12] = -nom_x[12] + true_x[12]; + out_3933583929675800728[13] = -nom_x[13] + true_x[13]; + out_3933583929675800728[14] = -nom_x[14] + true_x[14]; + out_3933583929675800728[15] = -nom_x[15] + true_x[15]; + out_3933583929675800728[16] = -nom_x[16] + true_x[16]; + out_3933583929675800728[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_5613029472060395452) { - out_5613029472060395452[0] = 1.0; - out_5613029472060395452[1] = 0.0; - out_5613029472060395452[2] = 0.0; - out_5613029472060395452[3] = 0.0; - out_5613029472060395452[4] = 0.0; - out_5613029472060395452[5] = 0.0; - out_5613029472060395452[6] = 0.0; - out_5613029472060395452[7] = 0.0; - out_5613029472060395452[8] = 0.0; - out_5613029472060395452[9] = 0.0; - out_5613029472060395452[10] = 0.0; - out_5613029472060395452[11] = 0.0; - out_5613029472060395452[12] = 0.0; - out_5613029472060395452[13] = 0.0; - out_5613029472060395452[14] = 0.0; - out_5613029472060395452[15] = 0.0; - out_5613029472060395452[16] = 0.0; - out_5613029472060395452[17] = 0.0; - out_5613029472060395452[18] = 0.0; - out_5613029472060395452[19] = 1.0; - out_5613029472060395452[20] = 0.0; - out_5613029472060395452[21] = 0.0; - out_5613029472060395452[22] = 0.0; - out_5613029472060395452[23] = 0.0; - out_5613029472060395452[24] = 0.0; - out_5613029472060395452[25] = 0.0; - out_5613029472060395452[26] = 0.0; - out_5613029472060395452[27] = 0.0; - out_5613029472060395452[28] = 0.0; - out_5613029472060395452[29] = 0.0; - out_5613029472060395452[30] = 0.0; - out_5613029472060395452[31] = 0.0; - out_5613029472060395452[32] = 0.0; - out_5613029472060395452[33] = 0.0; - out_5613029472060395452[34] = 0.0; - out_5613029472060395452[35] = 0.0; - out_5613029472060395452[36] = 0.0; - out_5613029472060395452[37] = 0.0; - out_5613029472060395452[38] = 1.0; - out_5613029472060395452[39] = 0.0; - out_5613029472060395452[40] = 0.0; - out_5613029472060395452[41] = 0.0; - out_5613029472060395452[42] = 0.0; - out_5613029472060395452[43] = 0.0; - out_5613029472060395452[44] = 0.0; - out_5613029472060395452[45] = 0.0; - out_5613029472060395452[46] = 0.0; - out_5613029472060395452[47] = 0.0; - out_5613029472060395452[48] = 0.0; - out_5613029472060395452[49] = 0.0; - out_5613029472060395452[50] = 0.0; - out_5613029472060395452[51] = 0.0; - out_5613029472060395452[52] = 0.0; - out_5613029472060395452[53] = 0.0; - out_5613029472060395452[54] = 0.0; - out_5613029472060395452[55] = 0.0; - out_5613029472060395452[56] = 0.0; - out_5613029472060395452[57] = 1.0; - out_5613029472060395452[58] = 0.0; - out_5613029472060395452[59] = 0.0; - out_5613029472060395452[60] = 0.0; - out_5613029472060395452[61] = 0.0; - out_5613029472060395452[62] = 0.0; - out_5613029472060395452[63] = 0.0; - out_5613029472060395452[64] = 0.0; - out_5613029472060395452[65] = 0.0; - out_5613029472060395452[66] = 0.0; - out_5613029472060395452[67] = 0.0; - out_5613029472060395452[68] = 0.0; - out_5613029472060395452[69] = 0.0; - out_5613029472060395452[70] = 0.0; - out_5613029472060395452[71] = 0.0; - out_5613029472060395452[72] = 0.0; - out_5613029472060395452[73] = 0.0; - out_5613029472060395452[74] = 0.0; - out_5613029472060395452[75] = 0.0; - out_5613029472060395452[76] = 1.0; - out_5613029472060395452[77] = 0.0; - out_5613029472060395452[78] = 0.0; - out_5613029472060395452[79] = 0.0; - out_5613029472060395452[80] = 0.0; - out_5613029472060395452[81] = 0.0; - out_5613029472060395452[82] = 0.0; - out_5613029472060395452[83] = 0.0; - out_5613029472060395452[84] = 0.0; - out_5613029472060395452[85] = 0.0; - out_5613029472060395452[86] = 0.0; - out_5613029472060395452[87] = 0.0; - out_5613029472060395452[88] = 0.0; - out_5613029472060395452[89] = 0.0; - out_5613029472060395452[90] = 0.0; - out_5613029472060395452[91] = 0.0; - out_5613029472060395452[92] = 0.0; - out_5613029472060395452[93] = 0.0; - out_5613029472060395452[94] = 0.0; - out_5613029472060395452[95] = 1.0; - out_5613029472060395452[96] = 0.0; - out_5613029472060395452[97] = 0.0; - out_5613029472060395452[98] = 0.0; - out_5613029472060395452[99] = 0.0; - out_5613029472060395452[100] = 0.0; - out_5613029472060395452[101] = 0.0; - out_5613029472060395452[102] = 0.0; - out_5613029472060395452[103] = 0.0; - out_5613029472060395452[104] = 0.0; - out_5613029472060395452[105] = 0.0; - out_5613029472060395452[106] = 0.0; - out_5613029472060395452[107] = 0.0; - out_5613029472060395452[108] = 0.0; - out_5613029472060395452[109] = 0.0; - out_5613029472060395452[110] = 0.0; - out_5613029472060395452[111] = 0.0; - out_5613029472060395452[112] = 0.0; - out_5613029472060395452[113] = 0.0; - out_5613029472060395452[114] = 1.0; - out_5613029472060395452[115] = 0.0; - out_5613029472060395452[116] = 0.0; - out_5613029472060395452[117] = 0.0; - out_5613029472060395452[118] = 0.0; - out_5613029472060395452[119] = 0.0; - out_5613029472060395452[120] = 0.0; - out_5613029472060395452[121] = 0.0; - out_5613029472060395452[122] = 0.0; - out_5613029472060395452[123] = 0.0; - out_5613029472060395452[124] = 0.0; - out_5613029472060395452[125] = 0.0; - out_5613029472060395452[126] = 0.0; - out_5613029472060395452[127] = 0.0; - out_5613029472060395452[128] = 0.0; - out_5613029472060395452[129] = 0.0; - out_5613029472060395452[130] = 0.0; - out_5613029472060395452[131] = 0.0; - out_5613029472060395452[132] = 0.0; - out_5613029472060395452[133] = 1.0; - out_5613029472060395452[134] = 0.0; - out_5613029472060395452[135] = 0.0; - out_5613029472060395452[136] = 0.0; - out_5613029472060395452[137] = 0.0; - out_5613029472060395452[138] = 0.0; - out_5613029472060395452[139] = 0.0; - out_5613029472060395452[140] = 0.0; - out_5613029472060395452[141] = 0.0; - out_5613029472060395452[142] = 0.0; - out_5613029472060395452[143] = 0.0; - out_5613029472060395452[144] = 0.0; - out_5613029472060395452[145] = 0.0; - out_5613029472060395452[146] = 0.0; - out_5613029472060395452[147] = 0.0; - out_5613029472060395452[148] = 0.0; - out_5613029472060395452[149] = 0.0; - out_5613029472060395452[150] = 0.0; - out_5613029472060395452[151] = 0.0; - out_5613029472060395452[152] = 1.0; - out_5613029472060395452[153] = 0.0; - out_5613029472060395452[154] = 0.0; - out_5613029472060395452[155] = 0.0; - out_5613029472060395452[156] = 0.0; - out_5613029472060395452[157] = 0.0; - out_5613029472060395452[158] = 0.0; - out_5613029472060395452[159] = 0.0; - out_5613029472060395452[160] = 0.0; - out_5613029472060395452[161] = 0.0; - out_5613029472060395452[162] = 0.0; - out_5613029472060395452[163] = 0.0; - out_5613029472060395452[164] = 0.0; - out_5613029472060395452[165] = 0.0; - out_5613029472060395452[166] = 0.0; - out_5613029472060395452[167] = 0.0; - out_5613029472060395452[168] = 0.0; - out_5613029472060395452[169] = 0.0; - out_5613029472060395452[170] = 0.0; - out_5613029472060395452[171] = 1.0; - out_5613029472060395452[172] = 0.0; - out_5613029472060395452[173] = 0.0; - out_5613029472060395452[174] = 0.0; - out_5613029472060395452[175] = 0.0; - out_5613029472060395452[176] = 0.0; - out_5613029472060395452[177] = 0.0; - out_5613029472060395452[178] = 0.0; - out_5613029472060395452[179] = 0.0; - out_5613029472060395452[180] = 0.0; - out_5613029472060395452[181] = 0.0; - out_5613029472060395452[182] = 0.0; - out_5613029472060395452[183] = 0.0; - out_5613029472060395452[184] = 0.0; - out_5613029472060395452[185] = 0.0; - out_5613029472060395452[186] = 0.0; - out_5613029472060395452[187] = 0.0; - out_5613029472060395452[188] = 0.0; - out_5613029472060395452[189] = 0.0; - out_5613029472060395452[190] = 1.0; - out_5613029472060395452[191] = 0.0; - out_5613029472060395452[192] = 0.0; - out_5613029472060395452[193] = 0.0; - out_5613029472060395452[194] = 0.0; - out_5613029472060395452[195] = 0.0; - out_5613029472060395452[196] = 0.0; - out_5613029472060395452[197] = 0.0; - out_5613029472060395452[198] = 0.0; - out_5613029472060395452[199] = 0.0; - out_5613029472060395452[200] = 0.0; - out_5613029472060395452[201] = 0.0; - out_5613029472060395452[202] = 0.0; - out_5613029472060395452[203] = 0.0; - out_5613029472060395452[204] = 0.0; - out_5613029472060395452[205] = 0.0; - out_5613029472060395452[206] = 0.0; - out_5613029472060395452[207] = 0.0; - out_5613029472060395452[208] = 0.0; - out_5613029472060395452[209] = 1.0; - out_5613029472060395452[210] = 0.0; - out_5613029472060395452[211] = 0.0; - out_5613029472060395452[212] = 0.0; - out_5613029472060395452[213] = 0.0; - out_5613029472060395452[214] = 0.0; - out_5613029472060395452[215] = 0.0; - out_5613029472060395452[216] = 0.0; - out_5613029472060395452[217] = 0.0; - out_5613029472060395452[218] = 0.0; - out_5613029472060395452[219] = 0.0; - out_5613029472060395452[220] = 0.0; - out_5613029472060395452[221] = 0.0; - out_5613029472060395452[222] = 0.0; - out_5613029472060395452[223] = 0.0; - out_5613029472060395452[224] = 0.0; - out_5613029472060395452[225] = 0.0; - out_5613029472060395452[226] = 0.0; - out_5613029472060395452[227] = 0.0; - out_5613029472060395452[228] = 1.0; - out_5613029472060395452[229] = 0.0; - out_5613029472060395452[230] = 0.0; - out_5613029472060395452[231] = 0.0; - out_5613029472060395452[232] = 0.0; - out_5613029472060395452[233] = 0.0; - out_5613029472060395452[234] = 0.0; - out_5613029472060395452[235] = 0.0; - out_5613029472060395452[236] = 0.0; - out_5613029472060395452[237] = 0.0; - out_5613029472060395452[238] = 0.0; - out_5613029472060395452[239] = 0.0; - out_5613029472060395452[240] = 0.0; - out_5613029472060395452[241] = 0.0; - out_5613029472060395452[242] = 0.0; - out_5613029472060395452[243] = 0.0; - out_5613029472060395452[244] = 0.0; - out_5613029472060395452[245] = 0.0; - out_5613029472060395452[246] = 0.0; - out_5613029472060395452[247] = 1.0; - out_5613029472060395452[248] = 0.0; - out_5613029472060395452[249] = 0.0; - out_5613029472060395452[250] = 0.0; - out_5613029472060395452[251] = 0.0; - out_5613029472060395452[252] = 0.0; - out_5613029472060395452[253] = 0.0; - out_5613029472060395452[254] = 0.0; - out_5613029472060395452[255] = 0.0; - out_5613029472060395452[256] = 0.0; - out_5613029472060395452[257] = 0.0; - out_5613029472060395452[258] = 0.0; - out_5613029472060395452[259] = 0.0; - out_5613029472060395452[260] = 0.0; - out_5613029472060395452[261] = 0.0; - out_5613029472060395452[262] = 0.0; - out_5613029472060395452[263] = 0.0; - out_5613029472060395452[264] = 0.0; - out_5613029472060395452[265] = 0.0; - out_5613029472060395452[266] = 1.0; - out_5613029472060395452[267] = 0.0; - out_5613029472060395452[268] = 0.0; - out_5613029472060395452[269] = 0.0; - out_5613029472060395452[270] = 0.0; - out_5613029472060395452[271] = 0.0; - out_5613029472060395452[272] = 0.0; - out_5613029472060395452[273] = 0.0; - out_5613029472060395452[274] = 0.0; - out_5613029472060395452[275] = 0.0; - out_5613029472060395452[276] = 0.0; - out_5613029472060395452[277] = 0.0; - out_5613029472060395452[278] = 0.0; - out_5613029472060395452[279] = 0.0; - out_5613029472060395452[280] = 0.0; - out_5613029472060395452[281] = 0.0; - out_5613029472060395452[282] = 0.0; - out_5613029472060395452[283] = 0.0; - out_5613029472060395452[284] = 0.0; - out_5613029472060395452[285] = 1.0; - out_5613029472060395452[286] = 0.0; - out_5613029472060395452[287] = 0.0; - out_5613029472060395452[288] = 0.0; - out_5613029472060395452[289] = 0.0; - out_5613029472060395452[290] = 0.0; - out_5613029472060395452[291] = 0.0; - out_5613029472060395452[292] = 0.0; - out_5613029472060395452[293] = 0.0; - out_5613029472060395452[294] = 0.0; - out_5613029472060395452[295] = 0.0; - out_5613029472060395452[296] = 0.0; - out_5613029472060395452[297] = 0.0; - out_5613029472060395452[298] = 0.0; - out_5613029472060395452[299] = 0.0; - out_5613029472060395452[300] = 0.0; - out_5613029472060395452[301] = 0.0; - out_5613029472060395452[302] = 0.0; - out_5613029472060395452[303] = 0.0; - out_5613029472060395452[304] = 1.0; - out_5613029472060395452[305] = 0.0; - out_5613029472060395452[306] = 0.0; - out_5613029472060395452[307] = 0.0; - out_5613029472060395452[308] = 0.0; - out_5613029472060395452[309] = 0.0; - out_5613029472060395452[310] = 0.0; - out_5613029472060395452[311] = 0.0; - out_5613029472060395452[312] = 0.0; - out_5613029472060395452[313] = 0.0; - out_5613029472060395452[314] = 0.0; - out_5613029472060395452[315] = 0.0; - out_5613029472060395452[316] = 0.0; - out_5613029472060395452[317] = 0.0; - out_5613029472060395452[318] = 0.0; - out_5613029472060395452[319] = 0.0; - out_5613029472060395452[320] = 0.0; - out_5613029472060395452[321] = 0.0; - out_5613029472060395452[322] = 0.0; - out_5613029472060395452[323] = 1.0; +void H_mod_fun(double *state, double *out_6974741239016485936) { + out_6974741239016485936[0] = 1.0; + out_6974741239016485936[1] = 0.0; + out_6974741239016485936[2] = 0.0; + out_6974741239016485936[3] = 0.0; + out_6974741239016485936[4] = 0.0; + out_6974741239016485936[5] = 0.0; + out_6974741239016485936[6] = 0.0; + out_6974741239016485936[7] = 0.0; + out_6974741239016485936[8] = 0.0; + out_6974741239016485936[9] = 0.0; + out_6974741239016485936[10] = 0.0; + out_6974741239016485936[11] = 0.0; + out_6974741239016485936[12] = 0.0; + out_6974741239016485936[13] = 0.0; + out_6974741239016485936[14] = 0.0; + out_6974741239016485936[15] = 0.0; + out_6974741239016485936[16] = 0.0; + out_6974741239016485936[17] = 0.0; + out_6974741239016485936[18] = 0.0; + out_6974741239016485936[19] = 1.0; + out_6974741239016485936[20] = 0.0; + out_6974741239016485936[21] = 0.0; + out_6974741239016485936[22] = 0.0; + out_6974741239016485936[23] = 0.0; + out_6974741239016485936[24] = 0.0; + out_6974741239016485936[25] = 0.0; + out_6974741239016485936[26] = 0.0; + out_6974741239016485936[27] = 0.0; + out_6974741239016485936[28] = 0.0; + out_6974741239016485936[29] = 0.0; + out_6974741239016485936[30] = 0.0; + out_6974741239016485936[31] = 0.0; + out_6974741239016485936[32] = 0.0; + out_6974741239016485936[33] = 0.0; + out_6974741239016485936[34] = 0.0; + out_6974741239016485936[35] = 0.0; + out_6974741239016485936[36] = 0.0; + out_6974741239016485936[37] = 0.0; + out_6974741239016485936[38] = 1.0; + out_6974741239016485936[39] = 0.0; + out_6974741239016485936[40] = 0.0; + out_6974741239016485936[41] = 0.0; + out_6974741239016485936[42] = 0.0; + out_6974741239016485936[43] = 0.0; + out_6974741239016485936[44] = 0.0; + out_6974741239016485936[45] = 0.0; + out_6974741239016485936[46] = 0.0; + out_6974741239016485936[47] = 0.0; + out_6974741239016485936[48] = 0.0; + out_6974741239016485936[49] = 0.0; + out_6974741239016485936[50] = 0.0; + out_6974741239016485936[51] = 0.0; + out_6974741239016485936[52] = 0.0; + out_6974741239016485936[53] = 0.0; + out_6974741239016485936[54] = 0.0; + out_6974741239016485936[55] = 0.0; + out_6974741239016485936[56] = 0.0; + out_6974741239016485936[57] = 1.0; + out_6974741239016485936[58] = 0.0; + out_6974741239016485936[59] = 0.0; + out_6974741239016485936[60] = 0.0; + out_6974741239016485936[61] = 0.0; + out_6974741239016485936[62] = 0.0; + out_6974741239016485936[63] = 0.0; + out_6974741239016485936[64] = 0.0; + out_6974741239016485936[65] = 0.0; + out_6974741239016485936[66] = 0.0; + out_6974741239016485936[67] = 0.0; + out_6974741239016485936[68] = 0.0; + out_6974741239016485936[69] = 0.0; + out_6974741239016485936[70] = 0.0; + out_6974741239016485936[71] = 0.0; + out_6974741239016485936[72] = 0.0; + out_6974741239016485936[73] = 0.0; + out_6974741239016485936[74] = 0.0; + out_6974741239016485936[75] = 0.0; + out_6974741239016485936[76] = 1.0; + out_6974741239016485936[77] = 0.0; + out_6974741239016485936[78] = 0.0; + out_6974741239016485936[79] = 0.0; + out_6974741239016485936[80] = 0.0; + out_6974741239016485936[81] = 0.0; + out_6974741239016485936[82] = 0.0; + out_6974741239016485936[83] = 0.0; + out_6974741239016485936[84] = 0.0; + out_6974741239016485936[85] = 0.0; + out_6974741239016485936[86] = 0.0; + out_6974741239016485936[87] = 0.0; + out_6974741239016485936[88] = 0.0; + out_6974741239016485936[89] = 0.0; + out_6974741239016485936[90] = 0.0; + out_6974741239016485936[91] = 0.0; + out_6974741239016485936[92] = 0.0; + out_6974741239016485936[93] = 0.0; + out_6974741239016485936[94] = 0.0; + out_6974741239016485936[95] = 1.0; + out_6974741239016485936[96] = 0.0; + out_6974741239016485936[97] = 0.0; + out_6974741239016485936[98] = 0.0; + out_6974741239016485936[99] = 0.0; + out_6974741239016485936[100] = 0.0; + out_6974741239016485936[101] = 0.0; + out_6974741239016485936[102] = 0.0; + out_6974741239016485936[103] = 0.0; + out_6974741239016485936[104] = 0.0; + out_6974741239016485936[105] = 0.0; + out_6974741239016485936[106] = 0.0; + out_6974741239016485936[107] = 0.0; + out_6974741239016485936[108] = 0.0; + out_6974741239016485936[109] = 0.0; + out_6974741239016485936[110] = 0.0; + out_6974741239016485936[111] = 0.0; + out_6974741239016485936[112] = 0.0; + out_6974741239016485936[113] = 0.0; + out_6974741239016485936[114] = 1.0; + out_6974741239016485936[115] = 0.0; + out_6974741239016485936[116] = 0.0; + out_6974741239016485936[117] = 0.0; + out_6974741239016485936[118] = 0.0; + out_6974741239016485936[119] = 0.0; + out_6974741239016485936[120] = 0.0; + out_6974741239016485936[121] = 0.0; + out_6974741239016485936[122] = 0.0; + out_6974741239016485936[123] = 0.0; + out_6974741239016485936[124] = 0.0; + out_6974741239016485936[125] = 0.0; + out_6974741239016485936[126] = 0.0; + out_6974741239016485936[127] = 0.0; + out_6974741239016485936[128] = 0.0; + out_6974741239016485936[129] = 0.0; + out_6974741239016485936[130] = 0.0; + out_6974741239016485936[131] = 0.0; + out_6974741239016485936[132] = 0.0; + out_6974741239016485936[133] = 1.0; + out_6974741239016485936[134] = 0.0; + out_6974741239016485936[135] = 0.0; + out_6974741239016485936[136] = 0.0; + out_6974741239016485936[137] = 0.0; + out_6974741239016485936[138] = 0.0; + out_6974741239016485936[139] = 0.0; + out_6974741239016485936[140] = 0.0; + out_6974741239016485936[141] = 0.0; + out_6974741239016485936[142] = 0.0; + out_6974741239016485936[143] = 0.0; + out_6974741239016485936[144] = 0.0; + out_6974741239016485936[145] = 0.0; + out_6974741239016485936[146] = 0.0; + out_6974741239016485936[147] = 0.0; + out_6974741239016485936[148] = 0.0; + out_6974741239016485936[149] = 0.0; + out_6974741239016485936[150] = 0.0; + out_6974741239016485936[151] = 0.0; + out_6974741239016485936[152] = 1.0; + out_6974741239016485936[153] = 0.0; + out_6974741239016485936[154] = 0.0; + out_6974741239016485936[155] = 0.0; + out_6974741239016485936[156] = 0.0; + out_6974741239016485936[157] = 0.0; + out_6974741239016485936[158] = 0.0; + out_6974741239016485936[159] = 0.0; + out_6974741239016485936[160] = 0.0; + out_6974741239016485936[161] = 0.0; + out_6974741239016485936[162] = 0.0; + out_6974741239016485936[163] = 0.0; + out_6974741239016485936[164] = 0.0; + out_6974741239016485936[165] = 0.0; + out_6974741239016485936[166] = 0.0; + out_6974741239016485936[167] = 0.0; + out_6974741239016485936[168] = 0.0; + out_6974741239016485936[169] = 0.0; + out_6974741239016485936[170] = 0.0; + out_6974741239016485936[171] = 1.0; + out_6974741239016485936[172] = 0.0; + out_6974741239016485936[173] = 0.0; + out_6974741239016485936[174] = 0.0; + out_6974741239016485936[175] = 0.0; + out_6974741239016485936[176] = 0.0; + out_6974741239016485936[177] = 0.0; + out_6974741239016485936[178] = 0.0; + out_6974741239016485936[179] = 0.0; + out_6974741239016485936[180] = 0.0; + out_6974741239016485936[181] = 0.0; + out_6974741239016485936[182] = 0.0; + out_6974741239016485936[183] = 0.0; + out_6974741239016485936[184] = 0.0; + out_6974741239016485936[185] = 0.0; + out_6974741239016485936[186] = 0.0; + out_6974741239016485936[187] = 0.0; + out_6974741239016485936[188] = 0.0; + out_6974741239016485936[189] = 0.0; + out_6974741239016485936[190] = 1.0; + out_6974741239016485936[191] = 0.0; + out_6974741239016485936[192] = 0.0; + out_6974741239016485936[193] = 0.0; + out_6974741239016485936[194] = 0.0; + out_6974741239016485936[195] = 0.0; + out_6974741239016485936[196] = 0.0; + out_6974741239016485936[197] = 0.0; + out_6974741239016485936[198] = 0.0; + out_6974741239016485936[199] = 0.0; + out_6974741239016485936[200] = 0.0; + out_6974741239016485936[201] = 0.0; + out_6974741239016485936[202] = 0.0; + out_6974741239016485936[203] = 0.0; + out_6974741239016485936[204] = 0.0; + out_6974741239016485936[205] = 0.0; + out_6974741239016485936[206] = 0.0; + out_6974741239016485936[207] = 0.0; + out_6974741239016485936[208] = 0.0; + out_6974741239016485936[209] = 1.0; + out_6974741239016485936[210] = 0.0; + out_6974741239016485936[211] = 0.0; + out_6974741239016485936[212] = 0.0; + out_6974741239016485936[213] = 0.0; + out_6974741239016485936[214] = 0.0; + out_6974741239016485936[215] = 0.0; + out_6974741239016485936[216] = 0.0; + out_6974741239016485936[217] = 0.0; + out_6974741239016485936[218] = 0.0; + out_6974741239016485936[219] = 0.0; + out_6974741239016485936[220] = 0.0; + out_6974741239016485936[221] = 0.0; + out_6974741239016485936[222] = 0.0; + out_6974741239016485936[223] = 0.0; + out_6974741239016485936[224] = 0.0; + out_6974741239016485936[225] = 0.0; + out_6974741239016485936[226] = 0.0; + out_6974741239016485936[227] = 0.0; + out_6974741239016485936[228] = 1.0; + out_6974741239016485936[229] = 0.0; + out_6974741239016485936[230] = 0.0; + out_6974741239016485936[231] = 0.0; + out_6974741239016485936[232] = 0.0; + out_6974741239016485936[233] = 0.0; + out_6974741239016485936[234] = 0.0; + out_6974741239016485936[235] = 0.0; + out_6974741239016485936[236] = 0.0; + out_6974741239016485936[237] = 0.0; + out_6974741239016485936[238] = 0.0; + out_6974741239016485936[239] = 0.0; + out_6974741239016485936[240] = 0.0; + out_6974741239016485936[241] = 0.0; + out_6974741239016485936[242] = 0.0; + out_6974741239016485936[243] = 0.0; + out_6974741239016485936[244] = 0.0; + out_6974741239016485936[245] = 0.0; + out_6974741239016485936[246] = 0.0; + out_6974741239016485936[247] = 1.0; + out_6974741239016485936[248] = 0.0; + out_6974741239016485936[249] = 0.0; + out_6974741239016485936[250] = 0.0; + out_6974741239016485936[251] = 0.0; + out_6974741239016485936[252] = 0.0; + out_6974741239016485936[253] = 0.0; + out_6974741239016485936[254] = 0.0; + out_6974741239016485936[255] = 0.0; + out_6974741239016485936[256] = 0.0; + out_6974741239016485936[257] = 0.0; + out_6974741239016485936[258] = 0.0; + out_6974741239016485936[259] = 0.0; + out_6974741239016485936[260] = 0.0; + out_6974741239016485936[261] = 0.0; + out_6974741239016485936[262] = 0.0; + out_6974741239016485936[263] = 0.0; + out_6974741239016485936[264] = 0.0; + out_6974741239016485936[265] = 0.0; + out_6974741239016485936[266] = 1.0; + out_6974741239016485936[267] = 0.0; + out_6974741239016485936[268] = 0.0; + out_6974741239016485936[269] = 0.0; + out_6974741239016485936[270] = 0.0; + out_6974741239016485936[271] = 0.0; + out_6974741239016485936[272] = 0.0; + out_6974741239016485936[273] = 0.0; + out_6974741239016485936[274] = 0.0; + out_6974741239016485936[275] = 0.0; + out_6974741239016485936[276] = 0.0; + out_6974741239016485936[277] = 0.0; + out_6974741239016485936[278] = 0.0; + out_6974741239016485936[279] = 0.0; + out_6974741239016485936[280] = 0.0; + out_6974741239016485936[281] = 0.0; + out_6974741239016485936[282] = 0.0; + out_6974741239016485936[283] = 0.0; + out_6974741239016485936[284] = 0.0; + out_6974741239016485936[285] = 1.0; + out_6974741239016485936[286] = 0.0; + out_6974741239016485936[287] = 0.0; + out_6974741239016485936[288] = 0.0; + out_6974741239016485936[289] = 0.0; + out_6974741239016485936[290] = 0.0; + out_6974741239016485936[291] = 0.0; + out_6974741239016485936[292] = 0.0; + out_6974741239016485936[293] = 0.0; + out_6974741239016485936[294] = 0.0; + out_6974741239016485936[295] = 0.0; + out_6974741239016485936[296] = 0.0; + out_6974741239016485936[297] = 0.0; + out_6974741239016485936[298] = 0.0; + out_6974741239016485936[299] = 0.0; + out_6974741239016485936[300] = 0.0; + out_6974741239016485936[301] = 0.0; + out_6974741239016485936[302] = 0.0; + out_6974741239016485936[303] = 0.0; + out_6974741239016485936[304] = 1.0; + out_6974741239016485936[305] = 0.0; + out_6974741239016485936[306] = 0.0; + out_6974741239016485936[307] = 0.0; + out_6974741239016485936[308] = 0.0; + out_6974741239016485936[309] = 0.0; + out_6974741239016485936[310] = 0.0; + out_6974741239016485936[311] = 0.0; + out_6974741239016485936[312] = 0.0; + out_6974741239016485936[313] = 0.0; + out_6974741239016485936[314] = 0.0; + out_6974741239016485936[315] = 0.0; + out_6974741239016485936[316] = 0.0; + out_6974741239016485936[317] = 0.0; + out_6974741239016485936[318] = 0.0; + out_6974741239016485936[319] = 0.0; + out_6974741239016485936[320] = 0.0; + out_6974741239016485936[321] = 0.0; + out_6974741239016485936[322] = 0.0; + out_6974741239016485936[323] = 1.0; } -void f_fun(double *state, double dt, double *out_3560257805552383557) { - out_3560257805552383557[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); - out_3560257805552383557[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); - out_3560257805552383557[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); - out_3560257805552383557[3] = dt*state[12] + state[3]; - out_3560257805552383557[4] = dt*state[13] + state[4]; - out_3560257805552383557[5] = dt*state[14] + state[5]; - out_3560257805552383557[6] = state[6]; - out_3560257805552383557[7] = state[7]; - out_3560257805552383557[8] = state[8]; - out_3560257805552383557[9] = state[9]; - out_3560257805552383557[10] = state[10]; - out_3560257805552383557[11] = state[11]; - out_3560257805552383557[12] = state[12]; - out_3560257805552383557[13] = state[13]; - out_3560257805552383557[14] = state[14]; - out_3560257805552383557[15] = state[15]; - out_3560257805552383557[16] = state[16]; - out_3560257805552383557[17] = state[17]; +void f_fun(double *state, double dt, double *out_7579043913228931093) { + out_7579043913228931093[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); + out_7579043913228931093[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); + out_7579043913228931093[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); + out_7579043913228931093[3] = dt*state[12] + state[3]; + out_7579043913228931093[4] = dt*state[13] + state[4]; + out_7579043913228931093[5] = dt*state[14] + state[5]; + out_7579043913228931093[6] = state[6]; + out_7579043913228931093[7] = state[7]; + out_7579043913228931093[8] = state[8]; + out_7579043913228931093[9] = state[9]; + out_7579043913228931093[10] = state[10]; + out_7579043913228931093[11] = state[11]; + out_7579043913228931093[12] = state[12]; + out_7579043913228931093[13] = state[13]; + out_7579043913228931093[14] = state[14]; + out_7579043913228931093[15] = state[15]; + out_7579043913228931093[16] = state[16]; + out_7579043913228931093[17] = state[17]; } -void F_fun(double *state, double dt, double *out_8405700866591566947) { - out_8405700866591566947[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_8405700866591566947[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_8405700866591566947[2] = 0; - out_8405700866591566947[3] = 0; - out_8405700866591566947[4] = 0; - out_8405700866591566947[5] = 0; - out_8405700866591566947[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_8405700866591566947[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_8405700866591566947[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_8405700866591566947[9] = 0; - out_8405700866591566947[10] = 0; - out_8405700866591566947[11] = 0; - out_8405700866591566947[12] = 0; - out_8405700866591566947[13] = 0; - out_8405700866591566947[14] = 0; - out_8405700866591566947[15] = 0; - out_8405700866591566947[16] = 0; - out_8405700866591566947[17] = 0; - out_8405700866591566947[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_8405700866591566947[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_8405700866591566947[20] = 0; - out_8405700866591566947[21] = 0; - out_8405700866591566947[22] = 0; - out_8405700866591566947[23] = 0; - out_8405700866591566947[24] = 0; - out_8405700866591566947[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_8405700866591566947[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_8405700866591566947[27] = 0; - out_8405700866591566947[28] = 0; - out_8405700866591566947[29] = 0; - out_8405700866591566947[30] = 0; - out_8405700866591566947[31] = 0; - out_8405700866591566947[32] = 0; - out_8405700866591566947[33] = 0; - out_8405700866591566947[34] = 0; - out_8405700866591566947[35] = 0; - out_8405700866591566947[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_8405700866591566947[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_8405700866591566947[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_8405700866591566947[39] = 0; - out_8405700866591566947[40] = 0; - out_8405700866591566947[41] = 0; - out_8405700866591566947[42] = 0; - out_8405700866591566947[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_8405700866591566947[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_8405700866591566947[45] = 0; - out_8405700866591566947[46] = 0; - out_8405700866591566947[47] = 0; - out_8405700866591566947[48] = 0; - out_8405700866591566947[49] = 0; - out_8405700866591566947[50] = 0; - out_8405700866591566947[51] = 0; - out_8405700866591566947[52] = 0; - out_8405700866591566947[53] = 0; - out_8405700866591566947[54] = 0; - out_8405700866591566947[55] = 0; - out_8405700866591566947[56] = 0; - out_8405700866591566947[57] = 1; - out_8405700866591566947[58] = 0; - out_8405700866591566947[59] = 0; - out_8405700866591566947[60] = 0; - out_8405700866591566947[61] = 0; - out_8405700866591566947[62] = 0; - out_8405700866591566947[63] = 0; - out_8405700866591566947[64] = 0; - out_8405700866591566947[65] = 0; - out_8405700866591566947[66] = dt; - out_8405700866591566947[67] = 0; - out_8405700866591566947[68] = 0; - out_8405700866591566947[69] = 0; - out_8405700866591566947[70] = 0; - out_8405700866591566947[71] = 0; - out_8405700866591566947[72] = 0; - out_8405700866591566947[73] = 0; - out_8405700866591566947[74] = 0; - out_8405700866591566947[75] = 0; - out_8405700866591566947[76] = 1; - out_8405700866591566947[77] = 0; - out_8405700866591566947[78] = 0; - out_8405700866591566947[79] = 0; - out_8405700866591566947[80] = 0; - out_8405700866591566947[81] = 0; - out_8405700866591566947[82] = 0; - out_8405700866591566947[83] = 0; - out_8405700866591566947[84] = 0; - out_8405700866591566947[85] = dt; - out_8405700866591566947[86] = 0; - out_8405700866591566947[87] = 0; - out_8405700866591566947[88] = 0; - out_8405700866591566947[89] = 0; - out_8405700866591566947[90] = 0; - out_8405700866591566947[91] = 0; - out_8405700866591566947[92] = 0; - out_8405700866591566947[93] = 0; - out_8405700866591566947[94] = 0; - out_8405700866591566947[95] = 1; - out_8405700866591566947[96] = 0; - out_8405700866591566947[97] = 0; - out_8405700866591566947[98] = 0; - out_8405700866591566947[99] = 0; - out_8405700866591566947[100] = 0; - out_8405700866591566947[101] = 0; - out_8405700866591566947[102] = 0; - out_8405700866591566947[103] = 0; - out_8405700866591566947[104] = dt; - out_8405700866591566947[105] = 0; - out_8405700866591566947[106] = 0; - out_8405700866591566947[107] = 0; - out_8405700866591566947[108] = 0; - out_8405700866591566947[109] = 0; - out_8405700866591566947[110] = 0; - out_8405700866591566947[111] = 0; - out_8405700866591566947[112] = 0; - out_8405700866591566947[113] = 0; - out_8405700866591566947[114] = 1; - out_8405700866591566947[115] = 0; - out_8405700866591566947[116] = 0; - out_8405700866591566947[117] = 0; - out_8405700866591566947[118] = 0; - out_8405700866591566947[119] = 0; - out_8405700866591566947[120] = 0; - out_8405700866591566947[121] = 0; - out_8405700866591566947[122] = 0; - out_8405700866591566947[123] = 0; - out_8405700866591566947[124] = 0; - out_8405700866591566947[125] = 0; - out_8405700866591566947[126] = 0; - out_8405700866591566947[127] = 0; - out_8405700866591566947[128] = 0; - out_8405700866591566947[129] = 0; - out_8405700866591566947[130] = 0; - out_8405700866591566947[131] = 0; - out_8405700866591566947[132] = 0; - out_8405700866591566947[133] = 1; - out_8405700866591566947[134] = 0; - out_8405700866591566947[135] = 0; - out_8405700866591566947[136] = 0; - out_8405700866591566947[137] = 0; - out_8405700866591566947[138] = 0; - out_8405700866591566947[139] = 0; - out_8405700866591566947[140] = 0; - out_8405700866591566947[141] = 0; - out_8405700866591566947[142] = 0; - out_8405700866591566947[143] = 0; - out_8405700866591566947[144] = 0; - out_8405700866591566947[145] = 0; - out_8405700866591566947[146] = 0; - out_8405700866591566947[147] = 0; - out_8405700866591566947[148] = 0; - out_8405700866591566947[149] = 0; - out_8405700866591566947[150] = 0; - out_8405700866591566947[151] = 0; - out_8405700866591566947[152] = 1; - out_8405700866591566947[153] = 0; - out_8405700866591566947[154] = 0; - out_8405700866591566947[155] = 0; - out_8405700866591566947[156] = 0; - out_8405700866591566947[157] = 0; - out_8405700866591566947[158] = 0; - out_8405700866591566947[159] = 0; - out_8405700866591566947[160] = 0; - out_8405700866591566947[161] = 0; - out_8405700866591566947[162] = 0; - out_8405700866591566947[163] = 0; - out_8405700866591566947[164] = 0; - out_8405700866591566947[165] = 0; - out_8405700866591566947[166] = 0; - out_8405700866591566947[167] = 0; - out_8405700866591566947[168] = 0; - out_8405700866591566947[169] = 0; - out_8405700866591566947[170] = 0; - out_8405700866591566947[171] = 1; - out_8405700866591566947[172] = 0; - out_8405700866591566947[173] = 0; - out_8405700866591566947[174] = 0; - out_8405700866591566947[175] = 0; - out_8405700866591566947[176] = 0; - out_8405700866591566947[177] = 0; - out_8405700866591566947[178] = 0; - out_8405700866591566947[179] = 0; - out_8405700866591566947[180] = 0; - out_8405700866591566947[181] = 0; - out_8405700866591566947[182] = 0; - out_8405700866591566947[183] = 0; - out_8405700866591566947[184] = 0; - out_8405700866591566947[185] = 0; - out_8405700866591566947[186] = 0; - out_8405700866591566947[187] = 0; - out_8405700866591566947[188] = 0; - out_8405700866591566947[189] = 0; - out_8405700866591566947[190] = 1; - out_8405700866591566947[191] = 0; - out_8405700866591566947[192] = 0; - out_8405700866591566947[193] = 0; - out_8405700866591566947[194] = 0; - out_8405700866591566947[195] = 0; - out_8405700866591566947[196] = 0; - out_8405700866591566947[197] = 0; - out_8405700866591566947[198] = 0; - out_8405700866591566947[199] = 0; - out_8405700866591566947[200] = 0; - out_8405700866591566947[201] = 0; - out_8405700866591566947[202] = 0; - out_8405700866591566947[203] = 0; - out_8405700866591566947[204] = 0; - out_8405700866591566947[205] = 0; - out_8405700866591566947[206] = 0; - out_8405700866591566947[207] = 0; - out_8405700866591566947[208] = 0; - out_8405700866591566947[209] = 1; - out_8405700866591566947[210] = 0; - out_8405700866591566947[211] = 0; - out_8405700866591566947[212] = 0; - out_8405700866591566947[213] = 0; - out_8405700866591566947[214] = 0; - out_8405700866591566947[215] = 0; - out_8405700866591566947[216] = 0; - out_8405700866591566947[217] = 0; - out_8405700866591566947[218] = 0; - out_8405700866591566947[219] = 0; - out_8405700866591566947[220] = 0; - out_8405700866591566947[221] = 0; - out_8405700866591566947[222] = 0; - out_8405700866591566947[223] = 0; - out_8405700866591566947[224] = 0; - out_8405700866591566947[225] = 0; - out_8405700866591566947[226] = 0; - out_8405700866591566947[227] = 0; - out_8405700866591566947[228] = 1; - out_8405700866591566947[229] = 0; - out_8405700866591566947[230] = 0; - out_8405700866591566947[231] = 0; - out_8405700866591566947[232] = 0; - out_8405700866591566947[233] = 0; - out_8405700866591566947[234] = 0; - out_8405700866591566947[235] = 0; - out_8405700866591566947[236] = 0; - out_8405700866591566947[237] = 0; - out_8405700866591566947[238] = 0; - out_8405700866591566947[239] = 0; - out_8405700866591566947[240] = 0; - out_8405700866591566947[241] = 0; - out_8405700866591566947[242] = 0; - out_8405700866591566947[243] = 0; - out_8405700866591566947[244] = 0; - out_8405700866591566947[245] = 0; - out_8405700866591566947[246] = 0; - out_8405700866591566947[247] = 1; - out_8405700866591566947[248] = 0; - out_8405700866591566947[249] = 0; - out_8405700866591566947[250] = 0; - out_8405700866591566947[251] = 0; - out_8405700866591566947[252] = 0; - out_8405700866591566947[253] = 0; - out_8405700866591566947[254] = 0; - out_8405700866591566947[255] = 0; - out_8405700866591566947[256] = 0; - out_8405700866591566947[257] = 0; - out_8405700866591566947[258] = 0; - out_8405700866591566947[259] = 0; - out_8405700866591566947[260] = 0; - out_8405700866591566947[261] = 0; - out_8405700866591566947[262] = 0; - out_8405700866591566947[263] = 0; - out_8405700866591566947[264] = 0; - out_8405700866591566947[265] = 0; - out_8405700866591566947[266] = 1; - out_8405700866591566947[267] = 0; - out_8405700866591566947[268] = 0; - out_8405700866591566947[269] = 0; - out_8405700866591566947[270] = 0; - out_8405700866591566947[271] = 0; - out_8405700866591566947[272] = 0; - out_8405700866591566947[273] = 0; - out_8405700866591566947[274] = 0; - out_8405700866591566947[275] = 0; - out_8405700866591566947[276] = 0; - out_8405700866591566947[277] = 0; - out_8405700866591566947[278] = 0; - out_8405700866591566947[279] = 0; - out_8405700866591566947[280] = 0; - out_8405700866591566947[281] = 0; - out_8405700866591566947[282] = 0; - out_8405700866591566947[283] = 0; - out_8405700866591566947[284] = 0; - out_8405700866591566947[285] = 1; - out_8405700866591566947[286] = 0; - out_8405700866591566947[287] = 0; - out_8405700866591566947[288] = 0; - out_8405700866591566947[289] = 0; - out_8405700866591566947[290] = 0; - out_8405700866591566947[291] = 0; - out_8405700866591566947[292] = 0; - out_8405700866591566947[293] = 0; - out_8405700866591566947[294] = 0; - out_8405700866591566947[295] = 0; - out_8405700866591566947[296] = 0; - out_8405700866591566947[297] = 0; - out_8405700866591566947[298] = 0; - out_8405700866591566947[299] = 0; - out_8405700866591566947[300] = 0; - out_8405700866591566947[301] = 0; - out_8405700866591566947[302] = 0; - out_8405700866591566947[303] = 0; - out_8405700866591566947[304] = 1; - out_8405700866591566947[305] = 0; - out_8405700866591566947[306] = 0; - out_8405700866591566947[307] = 0; - out_8405700866591566947[308] = 0; - out_8405700866591566947[309] = 0; - out_8405700866591566947[310] = 0; - out_8405700866591566947[311] = 0; - out_8405700866591566947[312] = 0; - out_8405700866591566947[313] = 0; - out_8405700866591566947[314] = 0; - out_8405700866591566947[315] = 0; - out_8405700866591566947[316] = 0; - out_8405700866591566947[317] = 0; - out_8405700866591566947[318] = 0; - out_8405700866591566947[319] = 0; - out_8405700866591566947[320] = 0; - out_8405700866591566947[321] = 0; - out_8405700866591566947[322] = 0; - out_8405700866591566947[323] = 1; +void F_fun(double *state, double dt, double *out_7552939469662313928) { + out_7552939469662313928[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_7552939469662313928[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_7552939469662313928[2] = 0; + out_7552939469662313928[3] = 0; + out_7552939469662313928[4] = 0; + out_7552939469662313928[5] = 0; + out_7552939469662313928[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_7552939469662313928[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_7552939469662313928[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_7552939469662313928[9] = 0; + out_7552939469662313928[10] = 0; + out_7552939469662313928[11] = 0; + out_7552939469662313928[12] = 0; + out_7552939469662313928[13] = 0; + out_7552939469662313928[14] = 0; + out_7552939469662313928[15] = 0; + out_7552939469662313928[16] = 0; + out_7552939469662313928[17] = 0; + out_7552939469662313928[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_7552939469662313928[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_7552939469662313928[20] = 0; + out_7552939469662313928[21] = 0; + out_7552939469662313928[22] = 0; + out_7552939469662313928[23] = 0; + out_7552939469662313928[24] = 0; + out_7552939469662313928[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_7552939469662313928[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_7552939469662313928[27] = 0; + out_7552939469662313928[28] = 0; + out_7552939469662313928[29] = 0; + out_7552939469662313928[30] = 0; + out_7552939469662313928[31] = 0; + out_7552939469662313928[32] = 0; + out_7552939469662313928[33] = 0; + out_7552939469662313928[34] = 0; + out_7552939469662313928[35] = 0; + out_7552939469662313928[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_7552939469662313928[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_7552939469662313928[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_7552939469662313928[39] = 0; + out_7552939469662313928[40] = 0; + out_7552939469662313928[41] = 0; + out_7552939469662313928[42] = 0; + out_7552939469662313928[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_7552939469662313928[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_7552939469662313928[45] = 0; + out_7552939469662313928[46] = 0; + out_7552939469662313928[47] = 0; + out_7552939469662313928[48] = 0; + out_7552939469662313928[49] = 0; + out_7552939469662313928[50] = 0; + out_7552939469662313928[51] = 0; + out_7552939469662313928[52] = 0; + out_7552939469662313928[53] = 0; + out_7552939469662313928[54] = 0; + out_7552939469662313928[55] = 0; + out_7552939469662313928[56] = 0; + out_7552939469662313928[57] = 1; + out_7552939469662313928[58] = 0; + out_7552939469662313928[59] = 0; + out_7552939469662313928[60] = 0; + out_7552939469662313928[61] = 0; + out_7552939469662313928[62] = 0; + out_7552939469662313928[63] = 0; + out_7552939469662313928[64] = 0; + out_7552939469662313928[65] = 0; + out_7552939469662313928[66] = dt; + out_7552939469662313928[67] = 0; + out_7552939469662313928[68] = 0; + out_7552939469662313928[69] = 0; + out_7552939469662313928[70] = 0; + out_7552939469662313928[71] = 0; + out_7552939469662313928[72] = 0; + out_7552939469662313928[73] = 0; + out_7552939469662313928[74] = 0; + out_7552939469662313928[75] = 0; + out_7552939469662313928[76] = 1; + out_7552939469662313928[77] = 0; + out_7552939469662313928[78] = 0; + out_7552939469662313928[79] = 0; + out_7552939469662313928[80] = 0; + out_7552939469662313928[81] = 0; + out_7552939469662313928[82] = 0; + out_7552939469662313928[83] = 0; + out_7552939469662313928[84] = 0; + out_7552939469662313928[85] = dt; + out_7552939469662313928[86] = 0; + out_7552939469662313928[87] = 0; + out_7552939469662313928[88] = 0; + out_7552939469662313928[89] = 0; + out_7552939469662313928[90] = 0; + out_7552939469662313928[91] = 0; + out_7552939469662313928[92] = 0; + out_7552939469662313928[93] = 0; + out_7552939469662313928[94] = 0; + out_7552939469662313928[95] = 1; + out_7552939469662313928[96] = 0; + out_7552939469662313928[97] = 0; + out_7552939469662313928[98] = 0; + out_7552939469662313928[99] = 0; + out_7552939469662313928[100] = 0; + out_7552939469662313928[101] = 0; + out_7552939469662313928[102] = 0; + out_7552939469662313928[103] = 0; + out_7552939469662313928[104] = dt; + out_7552939469662313928[105] = 0; + out_7552939469662313928[106] = 0; + out_7552939469662313928[107] = 0; + out_7552939469662313928[108] = 0; + out_7552939469662313928[109] = 0; + out_7552939469662313928[110] = 0; + out_7552939469662313928[111] = 0; + out_7552939469662313928[112] = 0; + out_7552939469662313928[113] = 0; + out_7552939469662313928[114] = 1; + out_7552939469662313928[115] = 0; + out_7552939469662313928[116] = 0; + out_7552939469662313928[117] = 0; + out_7552939469662313928[118] = 0; + out_7552939469662313928[119] = 0; + out_7552939469662313928[120] = 0; + out_7552939469662313928[121] = 0; + out_7552939469662313928[122] = 0; + out_7552939469662313928[123] = 0; + out_7552939469662313928[124] = 0; + out_7552939469662313928[125] = 0; + out_7552939469662313928[126] = 0; + out_7552939469662313928[127] = 0; + out_7552939469662313928[128] = 0; + out_7552939469662313928[129] = 0; + out_7552939469662313928[130] = 0; + out_7552939469662313928[131] = 0; + out_7552939469662313928[132] = 0; + out_7552939469662313928[133] = 1; + out_7552939469662313928[134] = 0; + out_7552939469662313928[135] = 0; + out_7552939469662313928[136] = 0; + out_7552939469662313928[137] = 0; + out_7552939469662313928[138] = 0; + out_7552939469662313928[139] = 0; + out_7552939469662313928[140] = 0; + out_7552939469662313928[141] = 0; + out_7552939469662313928[142] = 0; + out_7552939469662313928[143] = 0; + out_7552939469662313928[144] = 0; + out_7552939469662313928[145] = 0; + out_7552939469662313928[146] = 0; + out_7552939469662313928[147] = 0; + out_7552939469662313928[148] = 0; + out_7552939469662313928[149] = 0; + out_7552939469662313928[150] = 0; + out_7552939469662313928[151] = 0; + out_7552939469662313928[152] = 1; + out_7552939469662313928[153] = 0; + out_7552939469662313928[154] = 0; + out_7552939469662313928[155] = 0; + out_7552939469662313928[156] = 0; + out_7552939469662313928[157] = 0; + out_7552939469662313928[158] = 0; + out_7552939469662313928[159] = 0; + out_7552939469662313928[160] = 0; + out_7552939469662313928[161] = 0; + out_7552939469662313928[162] = 0; + out_7552939469662313928[163] = 0; + out_7552939469662313928[164] = 0; + out_7552939469662313928[165] = 0; + out_7552939469662313928[166] = 0; + out_7552939469662313928[167] = 0; + out_7552939469662313928[168] = 0; + out_7552939469662313928[169] = 0; + out_7552939469662313928[170] = 0; + out_7552939469662313928[171] = 1; + out_7552939469662313928[172] = 0; + out_7552939469662313928[173] = 0; + out_7552939469662313928[174] = 0; + out_7552939469662313928[175] = 0; + out_7552939469662313928[176] = 0; + out_7552939469662313928[177] = 0; + out_7552939469662313928[178] = 0; + out_7552939469662313928[179] = 0; + out_7552939469662313928[180] = 0; + out_7552939469662313928[181] = 0; + out_7552939469662313928[182] = 0; + out_7552939469662313928[183] = 0; + out_7552939469662313928[184] = 0; + out_7552939469662313928[185] = 0; + out_7552939469662313928[186] = 0; + out_7552939469662313928[187] = 0; + out_7552939469662313928[188] = 0; + out_7552939469662313928[189] = 0; + out_7552939469662313928[190] = 1; + out_7552939469662313928[191] = 0; + out_7552939469662313928[192] = 0; + out_7552939469662313928[193] = 0; + out_7552939469662313928[194] = 0; + out_7552939469662313928[195] = 0; + out_7552939469662313928[196] = 0; + out_7552939469662313928[197] = 0; + out_7552939469662313928[198] = 0; + out_7552939469662313928[199] = 0; + out_7552939469662313928[200] = 0; + out_7552939469662313928[201] = 0; + out_7552939469662313928[202] = 0; + out_7552939469662313928[203] = 0; + out_7552939469662313928[204] = 0; + out_7552939469662313928[205] = 0; + out_7552939469662313928[206] = 0; + out_7552939469662313928[207] = 0; + out_7552939469662313928[208] = 0; + out_7552939469662313928[209] = 1; + out_7552939469662313928[210] = 0; + out_7552939469662313928[211] = 0; + out_7552939469662313928[212] = 0; + out_7552939469662313928[213] = 0; + out_7552939469662313928[214] = 0; + out_7552939469662313928[215] = 0; + out_7552939469662313928[216] = 0; + out_7552939469662313928[217] = 0; + out_7552939469662313928[218] = 0; + out_7552939469662313928[219] = 0; + out_7552939469662313928[220] = 0; + out_7552939469662313928[221] = 0; + out_7552939469662313928[222] = 0; + out_7552939469662313928[223] = 0; + out_7552939469662313928[224] = 0; + out_7552939469662313928[225] = 0; + out_7552939469662313928[226] = 0; + out_7552939469662313928[227] = 0; + out_7552939469662313928[228] = 1; + out_7552939469662313928[229] = 0; + out_7552939469662313928[230] = 0; + out_7552939469662313928[231] = 0; + out_7552939469662313928[232] = 0; + out_7552939469662313928[233] = 0; + out_7552939469662313928[234] = 0; + out_7552939469662313928[235] = 0; + out_7552939469662313928[236] = 0; + out_7552939469662313928[237] = 0; + out_7552939469662313928[238] = 0; + out_7552939469662313928[239] = 0; + out_7552939469662313928[240] = 0; + out_7552939469662313928[241] = 0; + out_7552939469662313928[242] = 0; + out_7552939469662313928[243] = 0; + out_7552939469662313928[244] = 0; + out_7552939469662313928[245] = 0; + out_7552939469662313928[246] = 0; + out_7552939469662313928[247] = 1; + out_7552939469662313928[248] = 0; + out_7552939469662313928[249] = 0; + out_7552939469662313928[250] = 0; + out_7552939469662313928[251] = 0; + out_7552939469662313928[252] = 0; + out_7552939469662313928[253] = 0; + out_7552939469662313928[254] = 0; + out_7552939469662313928[255] = 0; + out_7552939469662313928[256] = 0; + out_7552939469662313928[257] = 0; + out_7552939469662313928[258] = 0; + out_7552939469662313928[259] = 0; + out_7552939469662313928[260] = 0; + out_7552939469662313928[261] = 0; + out_7552939469662313928[262] = 0; + out_7552939469662313928[263] = 0; + out_7552939469662313928[264] = 0; + out_7552939469662313928[265] = 0; + out_7552939469662313928[266] = 1; + out_7552939469662313928[267] = 0; + out_7552939469662313928[268] = 0; + out_7552939469662313928[269] = 0; + out_7552939469662313928[270] = 0; + out_7552939469662313928[271] = 0; + out_7552939469662313928[272] = 0; + out_7552939469662313928[273] = 0; + out_7552939469662313928[274] = 0; + out_7552939469662313928[275] = 0; + out_7552939469662313928[276] = 0; + out_7552939469662313928[277] = 0; + out_7552939469662313928[278] = 0; + out_7552939469662313928[279] = 0; + out_7552939469662313928[280] = 0; + out_7552939469662313928[281] = 0; + out_7552939469662313928[282] = 0; + out_7552939469662313928[283] = 0; + out_7552939469662313928[284] = 0; + out_7552939469662313928[285] = 1; + out_7552939469662313928[286] = 0; + out_7552939469662313928[287] = 0; + out_7552939469662313928[288] = 0; + out_7552939469662313928[289] = 0; + out_7552939469662313928[290] = 0; + out_7552939469662313928[291] = 0; + out_7552939469662313928[292] = 0; + out_7552939469662313928[293] = 0; + out_7552939469662313928[294] = 0; + out_7552939469662313928[295] = 0; + out_7552939469662313928[296] = 0; + out_7552939469662313928[297] = 0; + out_7552939469662313928[298] = 0; + out_7552939469662313928[299] = 0; + out_7552939469662313928[300] = 0; + out_7552939469662313928[301] = 0; + out_7552939469662313928[302] = 0; + out_7552939469662313928[303] = 0; + out_7552939469662313928[304] = 1; + out_7552939469662313928[305] = 0; + out_7552939469662313928[306] = 0; + out_7552939469662313928[307] = 0; + out_7552939469662313928[308] = 0; + out_7552939469662313928[309] = 0; + out_7552939469662313928[310] = 0; + out_7552939469662313928[311] = 0; + out_7552939469662313928[312] = 0; + out_7552939469662313928[313] = 0; + out_7552939469662313928[314] = 0; + out_7552939469662313928[315] = 0; + out_7552939469662313928[316] = 0; + out_7552939469662313928[317] = 0; + out_7552939469662313928[318] = 0; + out_7552939469662313928[319] = 0; + out_7552939469662313928[320] = 0; + out_7552939469662313928[321] = 0; + out_7552939469662313928[322] = 0; + out_7552939469662313928[323] = 1; } -void h_4(double *state, double *unused, double *out_6714333113820678959) { - out_6714333113820678959[0] = state[6] + state[9]; - out_6714333113820678959[1] = state[7] + state[10]; - out_6714333113820678959[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_3550900755285532144) { + out_3550900755285532144[0] = state[6] + state[9]; + out_3550900755285532144[1] = state[7] + state[10]; + out_3550900755285532144[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_6985632794897197443) { - out_6985632794897197443[0] = 0; - out_6985632794897197443[1] = 0; - out_6985632794897197443[2] = 0; - out_6985632794897197443[3] = 0; - out_6985632794897197443[4] = 0; - out_6985632794897197443[5] = 0; - out_6985632794897197443[6] = 1; - out_6985632794897197443[7] = 0; - out_6985632794897197443[8] = 0; - out_6985632794897197443[9] = 1; - out_6985632794897197443[10] = 0; - out_6985632794897197443[11] = 0; - out_6985632794897197443[12] = 0; - out_6985632794897197443[13] = 0; - out_6985632794897197443[14] = 0; - out_6985632794897197443[15] = 0; - out_6985632794897197443[16] = 0; - out_6985632794897197443[17] = 0; - out_6985632794897197443[18] = 0; - out_6985632794897197443[19] = 0; - out_6985632794897197443[20] = 0; - out_6985632794897197443[21] = 0; - out_6985632794897197443[22] = 0; - out_6985632794897197443[23] = 0; - out_6985632794897197443[24] = 0; - out_6985632794897197443[25] = 1; - out_6985632794897197443[26] = 0; - out_6985632794897197443[27] = 0; - out_6985632794897197443[28] = 1; - out_6985632794897197443[29] = 0; - out_6985632794897197443[30] = 0; - out_6985632794897197443[31] = 0; - out_6985632794897197443[32] = 0; - out_6985632794897197443[33] = 0; - out_6985632794897197443[34] = 0; - out_6985632794897197443[35] = 0; - out_6985632794897197443[36] = 0; - out_6985632794897197443[37] = 0; - out_6985632794897197443[38] = 0; - out_6985632794897197443[39] = 0; - out_6985632794897197443[40] = 0; - out_6985632794897197443[41] = 0; - out_6985632794897197443[42] = 0; - out_6985632794897197443[43] = 0; - out_6985632794897197443[44] = 1; - out_6985632794897197443[45] = 0; - out_6985632794897197443[46] = 0; - out_6985632794897197443[47] = 1; - out_6985632794897197443[48] = 0; - out_6985632794897197443[49] = 0; - out_6985632794897197443[50] = 0; - out_6985632794897197443[51] = 0; - out_6985632794897197443[52] = 0; - out_6985632794897197443[53] = 0; +void H_4(double *state, double *unused, double *out_3202934029172578364) { + out_3202934029172578364[0] = 0; + out_3202934029172578364[1] = 0; + out_3202934029172578364[2] = 0; + out_3202934029172578364[3] = 0; + out_3202934029172578364[4] = 0; + out_3202934029172578364[5] = 0; + out_3202934029172578364[6] = 1; + out_3202934029172578364[7] = 0; + out_3202934029172578364[8] = 0; + out_3202934029172578364[9] = 1; + out_3202934029172578364[10] = 0; + out_3202934029172578364[11] = 0; + out_3202934029172578364[12] = 0; + out_3202934029172578364[13] = 0; + out_3202934029172578364[14] = 0; + out_3202934029172578364[15] = 0; + out_3202934029172578364[16] = 0; + out_3202934029172578364[17] = 0; + out_3202934029172578364[18] = 0; + out_3202934029172578364[19] = 0; + out_3202934029172578364[20] = 0; + out_3202934029172578364[21] = 0; + out_3202934029172578364[22] = 0; + out_3202934029172578364[23] = 0; + out_3202934029172578364[24] = 0; + out_3202934029172578364[25] = 1; + out_3202934029172578364[26] = 0; + out_3202934029172578364[27] = 0; + out_3202934029172578364[28] = 1; + out_3202934029172578364[29] = 0; + out_3202934029172578364[30] = 0; + out_3202934029172578364[31] = 0; + out_3202934029172578364[32] = 0; + out_3202934029172578364[33] = 0; + out_3202934029172578364[34] = 0; + out_3202934029172578364[35] = 0; + out_3202934029172578364[36] = 0; + out_3202934029172578364[37] = 0; + out_3202934029172578364[38] = 0; + out_3202934029172578364[39] = 0; + out_3202934029172578364[40] = 0; + out_3202934029172578364[41] = 0; + out_3202934029172578364[42] = 0; + out_3202934029172578364[43] = 0; + out_3202934029172578364[44] = 1; + out_3202934029172578364[45] = 0; + out_3202934029172578364[46] = 0; + out_3202934029172578364[47] = 1; + out_3202934029172578364[48] = 0; + out_3202934029172578364[49] = 0; + out_3202934029172578364[50] = 0; + out_3202934029172578364[51] = 0; + out_3202934029172578364[52] = 0; + out_3202934029172578364[53] = 0; } -void h_10(double *state, double *unused, double *out_1436740119579277140) { - out_1436740119579277140[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_1436740119579277140[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_1436740119579277140[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; +void h_10(double *state, double *unused, double *out_7067714338468602861) { + out_7067714338468602861[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_7067714338468602861[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_7067714338468602861[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; } -void H_10(double *state, double *unused, double *out_1336760473497929829) { - out_1336760473497929829[0] = 0; - out_1336760473497929829[1] = 9.8100000000000005*cos(state[1]); - out_1336760473497929829[2] = 0; - out_1336760473497929829[3] = 0; - out_1336760473497929829[4] = -state[8]; - out_1336760473497929829[5] = state[7]; - out_1336760473497929829[6] = 0; - out_1336760473497929829[7] = state[5]; - out_1336760473497929829[8] = -state[4]; - out_1336760473497929829[9] = 0; - out_1336760473497929829[10] = 0; - out_1336760473497929829[11] = 0; - out_1336760473497929829[12] = 1; - out_1336760473497929829[13] = 0; - out_1336760473497929829[14] = 0; - out_1336760473497929829[15] = 1; - out_1336760473497929829[16] = 0; - out_1336760473497929829[17] = 0; - out_1336760473497929829[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_1336760473497929829[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_1336760473497929829[20] = 0; - out_1336760473497929829[21] = state[8]; - out_1336760473497929829[22] = 0; - out_1336760473497929829[23] = -state[6]; - out_1336760473497929829[24] = -state[5]; - out_1336760473497929829[25] = 0; - out_1336760473497929829[26] = state[3]; - out_1336760473497929829[27] = 0; - out_1336760473497929829[28] = 0; - out_1336760473497929829[29] = 0; - out_1336760473497929829[30] = 0; - out_1336760473497929829[31] = 1; - out_1336760473497929829[32] = 0; - out_1336760473497929829[33] = 0; - out_1336760473497929829[34] = 1; - out_1336760473497929829[35] = 0; - out_1336760473497929829[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_1336760473497929829[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_1336760473497929829[38] = 0; - out_1336760473497929829[39] = -state[7]; - out_1336760473497929829[40] = state[6]; - out_1336760473497929829[41] = 0; - out_1336760473497929829[42] = state[4]; - out_1336760473497929829[43] = -state[3]; - out_1336760473497929829[44] = 0; - out_1336760473497929829[45] = 0; - out_1336760473497929829[46] = 0; - out_1336760473497929829[47] = 0; - out_1336760473497929829[48] = 0; - out_1336760473497929829[49] = 0; - out_1336760473497929829[50] = 1; - out_1336760473497929829[51] = 0; - out_1336760473497929829[52] = 0; - out_1336760473497929829[53] = 1; +void H_10(double *state, double *unused, double *out_4832878358593471680) { + out_4832878358593471680[0] = 0; + out_4832878358593471680[1] = 9.8100000000000005*cos(state[1]); + out_4832878358593471680[2] = 0; + out_4832878358593471680[3] = 0; + out_4832878358593471680[4] = -state[8]; + out_4832878358593471680[5] = state[7]; + out_4832878358593471680[6] = 0; + out_4832878358593471680[7] = state[5]; + out_4832878358593471680[8] = -state[4]; + out_4832878358593471680[9] = 0; + out_4832878358593471680[10] = 0; + out_4832878358593471680[11] = 0; + out_4832878358593471680[12] = 1; + out_4832878358593471680[13] = 0; + out_4832878358593471680[14] = 0; + out_4832878358593471680[15] = 1; + out_4832878358593471680[16] = 0; + out_4832878358593471680[17] = 0; + out_4832878358593471680[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_4832878358593471680[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_4832878358593471680[20] = 0; + out_4832878358593471680[21] = state[8]; + out_4832878358593471680[22] = 0; + out_4832878358593471680[23] = -state[6]; + out_4832878358593471680[24] = -state[5]; + out_4832878358593471680[25] = 0; + out_4832878358593471680[26] = state[3]; + out_4832878358593471680[27] = 0; + out_4832878358593471680[28] = 0; + out_4832878358593471680[29] = 0; + out_4832878358593471680[30] = 0; + out_4832878358593471680[31] = 1; + out_4832878358593471680[32] = 0; + out_4832878358593471680[33] = 0; + out_4832878358593471680[34] = 1; + out_4832878358593471680[35] = 0; + out_4832878358593471680[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_4832878358593471680[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_4832878358593471680[38] = 0; + out_4832878358593471680[39] = -state[7]; + out_4832878358593471680[40] = state[6]; + out_4832878358593471680[41] = 0; + out_4832878358593471680[42] = state[4]; + out_4832878358593471680[43] = -state[3]; + out_4832878358593471680[44] = 0; + out_4832878358593471680[45] = 0; + out_4832878358593471680[46] = 0; + out_4832878358593471680[47] = 0; + out_4832878358593471680[48] = 0; + out_4832878358593471680[49] = 0; + out_4832878358593471680[50] = 1; + out_4832878358593471680[51] = 0; + out_4832878358593471680[52] = 0; + out_4832878358593471680[53] = 1; } -void h_13(double *state, double *unused, double *out_8610784800470542827) { - out_8610784800470542827[0] = state[3]; - out_8610784800470542827[1] = state[4]; - out_8610784800470542827[2] = state[5]; +void h_13(double *state, double *unused, double *out_6451839863147612103) { + out_6451839863147612103[0] = state[3]; + out_6451839863147612103[1] = state[4]; + out_6451839863147612103[2] = state[5]; } -void H_13(double *state, double *unused, double *out_8248837453480021372) { - out_8248837453480021372[0] = 0; - out_8248837453480021372[1] = 0; - out_8248837453480021372[2] = 0; - out_8248837453480021372[3] = 1; - out_8248837453480021372[4] = 0; - out_8248837453480021372[5] = 0; - out_8248837453480021372[6] = 0; - out_8248837453480021372[7] = 0; - out_8248837453480021372[8] = 0; - out_8248837453480021372[9] = 0; - out_8248837453480021372[10] = 0; - out_8248837453480021372[11] = 0; - out_8248837453480021372[12] = 0; - out_8248837453480021372[13] = 0; - out_8248837453480021372[14] = 0; - out_8248837453480021372[15] = 0; - out_8248837453480021372[16] = 0; - out_8248837453480021372[17] = 0; - out_8248837453480021372[18] = 0; - out_8248837453480021372[19] = 0; - out_8248837453480021372[20] = 0; - out_8248837453480021372[21] = 0; - out_8248837453480021372[22] = 1; - out_8248837453480021372[23] = 0; - out_8248837453480021372[24] = 0; - out_8248837453480021372[25] = 0; - out_8248837453480021372[26] = 0; - out_8248837453480021372[27] = 0; - out_8248837453480021372[28] = 0; - out_8248837453480021372[29] = 0; - out_8248837453480021372[30] = 0; - out_8248837453480021372[31] = 0; - out_8248837453480021372[32] = 0; - out_8248837453480021372[33] = 0; - out_8248837453480021372[34] = 0; - out_8248837453480021372[35] = 0; - out_8248837453480021372[36] = 0; - out_8248837453480021372[37] = 0; - out_8248837453480021372[38] = 0; - out_8248837453480021372[39] = 0; - out_8248837453480021372[40] = 0; - out_8248837453480021372[41] = 1; - out_8248837453480021372[42] = 0; - out_8248837453480021372[43] = 0; - out_8248837453480021372[44] = 0; - out_8248837453480021372[45] = 0; - out_8248837453480021372[46] = 0; - out_8248837453480021372[47] = 0; - out_8248837453480021372[48] = 0; - out_8248837453480021372[49] = 0; - out_8248837453480021372[50] = 0; - out_8248837453480021372[51] = 0; - out_8248837453480021372[52] = 0; - out_8248837453480021372[53] = 0; +void H_13(double *state, double *unused, double *out_9339796159754437) { + out_9339796159754437[0] = 0; + out_9339796159754437[1] = 0; + out_9339796159754437[2] = 0; + out_9339796159754437[3] = 1; + out_9339796159754437[4] = 0; + out_9339796159754437[5] = 0; + out_9339796159754437[6] = 0; + out_9339796159754437[7] = 0; + out_9339796159754437[8] = 0; + out_9339796159754437[9] = 0; + out_9339796159754437[10] = 0; + out_9339796159754437[11] = 0; + out_9339796159754437[12] = 0; + out_9339796159754437[13] = 0; + out_9339796159754437[14] = 0; + out_9339796159754437[15] = 0; + out_9339796159754437[16] = 0; + out_9339796159754437[17] = 0; + out_9339796159754437[18] = 0; + out_9339796159754437[19] = 0; + out_9339796159754437[20] = 0; + out_9339796159754437[21] = 0; + out_9339796159754437[22] = 1; + out_9339796159754437[23] = 0; + out_9339796159754437[24] = 0; + out_9339796159754437[25] = 0; + out_9339796159754437[26] = 0; + out_9339796159754437[27] = 0; + out_9339796159754437[28] = 0; + out_9339796159754437[29] = 0; + out_9339796159754437[30] = 0; + out_9339796159754437[31] = 0; + out_9339796159754437[32] = 0; + out_9339796159754437[33] = 0; + out_9339796159754437[34] = 0; + out_9339796159754437[35] = 0; + out_9339796159754437[36] = 0; + out_9339796159754437[37] = 0; + out_9339796159754437[38] = 0; + out_9339796159754437[39] = 0; + out_9339796159754437[40] = 0; + out_9339796159754437[41] = 1; + out_9339796159754437[42] = 0; + out_9339796159754437[43] = 0; + out_9339796159754437[44] = 0; + out_9339796159754437[45] = 0; + out_9339796159754437[46] = 0; + out_9339796159754437[47] = 0; + out_9339796159754437[48] = 0; + out_9339796159754437[49] = 0; + out_9339796159754437[50] = 0; + out_9339796159754437[51] = 0; + out_9339796159754437[52] = 0; + out_9339796159754437[53] = 0; } -void h_14(double *state, double *unused, double *out_7905030664698657319) { - out_7905030664698657319[0] = state[6]; - out_7905030664698657319[1] = state[7]; - out_7905030664698657319[2] = state[8]; +void h_14(double *state, double *unused, double *out_3486571009539339858) { + out_3486571009539339858[0] = state[6]; + out_3486571009539339858[1] = state[7]; + out_3486571009539339858[2] = state[8]; } -void H_14(double *state, double *unused, double *out_6550516268252313844) { - out_6550516268252313844[0] = 0; - out_6550516268252313844[1] = 0; - out_6550516268252313844[2] = 0; - out_6550516268252313844[3] = 0; - out_6550516268252313844[4] = 0; - out_6550516268252313844[5] = 0; - out_6550516268252313844[6] = 1; - out_6550516268252313844[7] = 0; - out_6550516268252313844[8] = 0; - out_6550516268252313844[9] = 0; - out_6550516268252313844[10] = 0; - out_6550516268252313844[11] = 0; - out_6550516268252313844[12] = 0; - out_6550516268252313844[13] = 0; - out_6550516268252313844[14] = 0; - out_6550516268252313844[15] = 0; - out_6550516268252313844[16] = 0; - out_6550516268252313844[17] = 0; - out_6550516268252313844[18] = 0; - out_6550516268252313844[19] = 0; - out_6550516268252313844[20] = 0; - out_6550516268252313844[21] = 0; - out_6550516268252313844[22] = 0; - out_6550516268252313844[23] = 0; - out_6550516268252313844[24] = 0; - out_6550516268252313844[25] = 1; - out_6550516268252313844[26] = 0; - out_6550516268252313844[27] = 0; - out_6550516268252313844[28] = 0; - out_6550516268252313844[29] = 0; - out_6550516268252313844[30] = 0; - out_6550516268252313844[31] = 0; - out_6550516268252313844[32] = 0; - out_6550516268252313844[33] = 0; - out_6550516268252313844[34] = 0; - out_6550516268252313844[35] = 0; - out_6550516268252313844[36] = 0; - out_6550516268252313844[37] = 0; - out_6550516268252313844[38] = 0; - out_6550516268252313844[39] = 0; - out_6550516268252313844[40] = 0; - out_6550516268252313844[41] = 0; - out_6550516268252313844[42] = 0; - out_6550516268252313844[43] = 0; - out_6550516268252313844[44] = 1; - out_6550516268252313844[45] = 0; - out_6550516268252313844[46] = 0; - out_6550516268252313844[47] = 0; - out_6550516268252313844[48] = 0; - out_6550516268252313844[49] = 0; - out_6550516268252313844[50] = 0; - out_6550516268252313844[51] = 0; - out_6550516268252313844[52] = 0; - out_6550516268252313844[53] = 0; +void H_14(double *state, double *unused, double *out_6285722461467950660) { + out_6285722461467950660[0] = 0; + out_6285722461467950660[1] = 0; + out_6285722461467950660[2] = 0; + out_6285722461467950660[3] = 0; + out_6285722461467950660[4] = 0; + out_6285722461467950660[5] = 0; + out_6285722461467950660[6] = 1; + out_6285722461467950660[7] = 0; + out_6285722461467950660[8] = 0; + out_6285722461467950660[9] = 0; + out_6285722461467950660[10] = 0; + out_6285722461467950660[11] = 0; + out_6285722461467950660[12] = 0; + out_6285722461467950660[13] = 0; + out_6285722461467950660[14] = 0; + out_6285722461467950660[15] = 0; + out_6285722461467950660[16] = 0; + out_6285722461467950660[17] = 0; + out_6285722461467950660[18] = 0; + out_6285722461467950660[19] = 0; + out_6285722461467950660[20] = 0; + out_6285722461467950660[21] = 0; + out_6285722461467950660[22] = 0; + out_6285722461467950660[23] = 0; + out_6285722461467950660[24] = 0; + out_6285722461467950660[25] = 1; + out_6285722461467950660[26] = 0; + out_6285722461467950660[27] = 0; + out_6285722461467950660[28] = 0; + out_6285722461467950660[29] = 0; + out_6285722461467950660[30] = 0; + out_6285722461467950660[31] = 0; + out_6285722461467950660[32] = 0; + out_6285722461467950660[33] = 0; + out_6285722461467950660[34] = 0; + out_6285722461467950660[35] = 0; + out_6285722461467950660[36] = 0; + out_6285722461467950660[37] = 0; + out_6285722461467950660[38] = 0; + out_6285722461467950660[39] = 0; + out_6285722461467950660[40] = 0; + out_6285722461467950660[41] = 0; + out_6285722461467950660[42] = 0; + out_6285722461467950660[43] = 0; + out_6285722461467950660[44] = 1; + out_6285722461467950660[45] = 0; + out_6285722461467950660[46] = 0; + out_6285722461467950660[47] = 0; + out_6285722461467950660[48] = 0; + out_6285722461467950660[49] = 0; + out_6285722461467950660[50] = 0; + out_6285722461467950660[51] = 0; + out_6285722461467950660[52] = 0; + out_6285722461467950660[53] = 0; } #include #include @@ -1113,44 +1113,44 @@ void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, doub void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_14, H_14, NULL, in_z, in_R, in_ea, MAHA_THRESH_14); } -void pose_err_fun(double *nom_x, double *delta_x, double *out_7704133318394013706) { - err_fun(nom_x, delta_x, out_7704133318394013706); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6520327552806096841) { + err_fun(nom_x, delta_x, out_6520327552806096841); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5732870200970735889) { - inv_err_fun(nom_x, true_x, out_5732870200970735889); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3933583929675800728) { + inv_err_fun(nom_x, true_x, out_3933583929675800728); } -void pose_H_mod_fun(double *state, double *out_5613029472060395452) { - H_mod_fun(state, out_5613029472060395452); +void pose_H_mod_fun(double *state, double *out_6974741239016485936) { + H_mod_fun(state, out_6974741239016485936); } -void pose_f_fun(double *state, double dt, double *out_3560257805552383557) { - f_fun(state, dt, out_3560257805552383557); +void pose_f_fun(double *state, double dt, double *out_7579043913228931093) { + f_fun(state, dt, out_7579043913228931093); } -void pose_F_fun(double *state, double dt, double *out_8405700866591566947) { - F_fun(state, dt, out_8405700866591566947); +void pose_F_fun(double *state, double dt, double *out_7552939469662313928) { + F_fun(state, dt, out_7552939469662313928); } -void pose_h_4(double *state, double *unused, double *out_6714333113820678959) { - h_4(state, unused, out_6714333113820678959); +void pose_h_4(double *state, double *unused, double *out_3550900755285532144) { + h_4(state, unused, out_3550900755285532144); } -void pose_H_4(double *state, double *unused, double *out_6985632794897197443) { - H_4(state, unused, out_6985632794897197443); +void pose_H_4(double *state, double *unused, double *out_3202934029172578364) { + H_4(state, unused, out_3202934029172578364); } -void pose_h_10(double *state, double *unused, double *out_1436740119579277140) { - h_10(state, unused, out_1436740119579277140); +void pose_h_10(double *state, double *unused, double *out_7067714338468602861) { + h_10(state, unused, out_7067714338468602861); } -void pose_H_10(double *state, double *unused, double *out_1336760473497929829) { - H_10(state, unused, out_1336760473497929829); +void pose_H_10(double *state, double *unused, double *out_4832878358593471680) { + H_10(state, unused, out_4832878358593471680); } -void pose_h_13(double *state, double *unused, double *out_8610784800470542827) { - h_13(state, unused, out_8610784800470542827); +void pose_h_13(double *state, double *unused, double *out_6451839863147612103) { + h_13(state, unused, out_6451839863147612103); } -void pose_H_13(double *state, double *unused, double *out_8248837453480021372) { - H_13(state, unused, out_8248837453480021372); +void pose_H_13(double *state, double *unused, double *out_9339796159754437) { + H_13(state, unused, out_9339796159754437); } -void pose_h_14(double *state, double *unused, double *out_7905030664698657319) { - h_14(state, unused, out_7905030664698657319); +void pose_h_14(double *state, double *unused, double *out_3486571009539339858) { + h_14(state, unused, out_3486571009539339858); } -void pose_H_14(double *state, double *unused, double *out_6550516268252313844) { - H_14(state, unused, out_6550516268252313844); +void pose_H_14(double *state, double *unused, double *out_6285722461467950660) { + H_14(state, unused, out_6285722461467950660); } void pose_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/pose.h b/selfdrive/locationd/models/generated/pose.h index f7b3c4f9c..e708a36ab 100644 --- a/selfdrive/locationd/models/generated/pose.h +++ b/selfdrive/locationd/models/generated/pose.h @@ -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); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 33844a161..d544c4820 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 43b12cd78..e435f7812 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 0cca07346..55fee553e 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 9ccc0a12f..d9159a81f 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index e504e2471..01692f281 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index eb753bd96..c0be52eb3 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ