diff --git a/common/libcommon.a b/common/libcommon.a index ed7391637b..ce22a5c340 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index abdf9413aa..ac3bb66f1f 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -380,6 +380,7 @@ inline static std::unordered_map keys = { {"MapsSelected", {PERSISTENT, STRING, "", "", 0}}, {"MapSpeedLimit", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}}, {"NavDesiresAllowed", {PERSISTENT, BOOL, "1", "0", 2}}, + {"NavLongitudinalAllowed", {PERSISTENT, BOOL, "0", "0", 2}}, {"NavDestination", {PERSISTENT, STRING, "", ""}}, {"NavInstructionState", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON, "{}", "{}"}}, {"NextMapSpeedLimit", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index cc0951a6d5..29b7959053 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 13f1d4a2c7..897f76d7a5 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 8ab287908d..20f155f023 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 677b879df0..854a006c79 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 db72e9465d..068ed19e81 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 fd7db607c8..962cec3f86 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 58b609b8d0..aa4711f2ea 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 947133f9bd..2e06cf6ed8 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 947133f9bd..2e06cf6ed8 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 c11e0b03a9..93364aeafc 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 58b609b8d0..aa4711f2ea 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 1dc9980cbe..a26f3af8b9 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-c744f66b-DEBUG"; +const uint8_t gitversion[19] = "DEV-01bd291b-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 1a7efd5362..d6435c90b1 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 0dcfe8ba7a..cd8d77ec2e 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 fe4db36c7e..96e0b433d7 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 f8cb14bee8..62fc79fd04 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 d698312ced..22ff9d1eb7 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 9f07d10d98..bffab83d3a 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 1ae12dfbc6..f85fed869d 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 ec9c0dd404..5995c30d0e 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 b69f5aefbe..1a11b6337f 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 b1291f85e9..69f8229cf0 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 b61a770638..b5ec6b87b1 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 912f7cfacf..1fde217788 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 5e45e2503e..63295bc6d3 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 dbb5808902..bdaea494d7 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 d1044f1a71..709fc5fde7 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 d01bde924b..a3fc811f59 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 f8c20e47fb..258d1f7693 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 d29d714fce..86b060e2ed 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 f36ac76081..83bd42e071 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 b09c121ff0..2cd3853d8d 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 3e567527b1..18a86a2cf2 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-c744f66b-DEBUG \ No newline at end of file +DEV-01bd291b-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index e7f613f2fd..d97f73d730 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -6,8 +6,12 @@ from types import SimpleNamespace class FakeParams: + def __init__(self, values=None): + self.values = dict(values or {}) + def get(self, *args, **kwargs): - return None + key = args[0] if args else None + return self.values.get(key) def get_float(self, *args, **kwargs): return 0.0 @@ -16,10 +20,10 @@ class FakeParams: pass -def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False): +def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False, nav_state=None): planner = SimpleNamespace( params=FakeParams(), - params_memory=FakeParams(), + params_memory=FakeParams({"NavInstructionState": nav_state or {}}), lead_one=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0), starpilot_cem=SimpleNamespace(stop_light_detected=red_light), tracking_lead=False, @@ -35,10 +39,11 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False return planner, vcruise -def make_sm(*, standstill=True): +def make_sm(*, standstill=True, min_steer_speed=0.0): return { "carControl": SimpleNamespace(longActive=True), "carState": SimpleNamespace(standstill=standstill, gasPressed=False, vCruiseCluster=0.0, vEgoCluster=0.0), + "carParams": SimpleNamespace(minSteerSpeed=min_steer_speed), "starpilotCarState": SimpleNamespace(accelPressed=False, dashboardStopSign=0, dashboardSpeedLimit=0), } @@ -48,6 +53,7 @@ def make_toggles(): force_stops=True, force_standstill=False, curve_speed_controller=False, + nav_longitudinal_allowed=False, speed_limit_controller=False, show_speed_limits=False, force_stop_distance_offset=0, @@ -133,3 +139,164 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens(): assert result == pytest.approx(0.0) assert vcruise.force_stop_timer >= 0.5 assert vcruise.forcing_stop + + +def test_nav_turn_speed_control_default_off(): + _, vcruise = make_vcruise(nav_state={ + "valid": True, + "maneuverType": "turn", + "maneuverModifier": "right", + "maneuverDistance": 30.0, + "nextManeuverType": "", + "nextManeuverModifier": "", + "nextManeuverDistance": 0.0, + }) + + result = vcruise.update( + controls_enabled=True, + now=0.0, + time_validated=True, + v_cruise=20.0, + v_ego=20.0, + sm=make_sm(standstill=False), + starpilot_toggles=make_toggles(), + ) + + assert result == pytest.approx(20.0) + assert vcruise.nav_turn_target == pytest.approx(0.0) + + +def test_nav_turn_speed_control_slows_for_imminent_turn(): + _, vcruise = make_vcruise(nav_state={ + "valid": True, + "maneuverType": "turn", + "maneuverModifier": "right", + "maneuverDistance": 30.0, + "nextManeuverType": "", + "nextManeuverModifier": "", + "nextManeuverDistance": 0.0, + }) + + toggles = make_toggles() + toggles.nav_longitudinal_allowed = True + result = vcruise.update( + controls_enabled=True, + now=0.0, + time_validated=True, + v_cruise=20.0, + v_ego=20.0, + sm=make_sm(standstill=False), + starpilot_toggles=toggles, + ) + + assert result < 20.0 + assert result == pytest.approx(vcruise.nav_turn_target) + assert result > 0.0 + + +def test_nav_turn_speed_control_ignores_distant_turn(): + _, vcruise = make_vcruise(nav_state={ + "valid": True, + "maneuverType": "turn", + "maneuverModifier": "right", + "maneuverDistance": 400.0, + "nextManeuverType": "", + "nextManeuverModifier": "", + "nextManeuverDistance": 0.0, + }) + + toggles = make_toggles() + toggles.nav_longitudinal_allowed = True + result = vcruise.update( + controls_enabled=True, + now=0.0, + time_validated=True, + v_cruise=20.0, + v_ego=20.0, + sm=make_sm(standstill=False), + starpilot_toggles=toggles, + ) + + assert result == pytest.approx(20.0) + assert vcruise.nav_turn_target == pytest.approx(0.0) + + +def test_nav_turn_speed_control_ignores_off_ramp(): + _, vcruise = make_vcruise(nav_state={ + "valid": True, + "maneuverType": "off ramp", + "maneuverModifier": "slightRight", + "maneuverDistance": 40.0, + "nextManeuverType": "", + "nextManeuverModifier": "", + "nextManeuverDistance": 0.0, + }) + + toggles = make_toggles() + toggles.nav_longitudinal_allowed = True + result = vcruise.update( + controls_enabled=True, + now=0.0, + time_validated=True, + v_cruise=30.0, + v_ego=30.0, + sm=make_sm(standstill=False), + starpilot_toggles=toggles, + ) + + assert result == pytest.approx(30.0) + assert vcruise.nav_turn_target == pytest.approx(0.0) + + +def test_nav_turn_speed_control_respects_car_min_steer_speed(): + _, vcruise = make_vcruise(nav_state={ + "valid": True, + "maneuverType": "turn", + "maneuverModifier": "uturn", + "maneuverDistance": 8.0, + "nextManeuverType": "", + "nextManeuverModifier": "", + "nextManeuverDistance": 0.0, + }) + + toggles = make_toggles() + toggles.nav_longitudinal_allowed = True + result = vcruise.update( + controls_enabled=True, + now=0.0, + time_validated=True, + v_cruise=20.0, + v_ego=20.0, + sm=make_sm(standstill=False, min_steer_speed=7.0 * CV.MPH_TO_MS), + starpilot_toggles=toggles, + ) + + assert result == pytest.approx(vcruise.nav_turn_target) + assert vcruise.nav_turn_target >= 7.0 * CV.MPH_TO_MS + + +def test_nav_turn_speed_control_does_not_floor_steer_to_zero_cars(): + _, vcruise = make_vcruise(nav_state={ + "valid": True, + "maneuverType": "turn", + "maneuverModifier": "uturn", + "maneuverDistance": 8.0, + "nextManeuverType": "", + "nextManeuverModifier": "", + "nextManeuverDistance": 0.0, + }) + + toggles = make_toggles() + toggles.nav_longitudinal_allowed = True + result = vcruise.update( + controls_enabled=True, + now=0.0, + time_validated=True, + v_cruise=20.0, + v_ego=20.0, + sm=make_sm(standstill=False, min_steer_speed=0.0), + starpilot_toggles=toggles, + ) + + assert result == pytest.approx(vcruise.nav_turn_target) + assert vcruise.nav_turn_target == pytest.approx(5.0 * CV.MPH_TO_MS) diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 88bf7a9133..5ff6c78378 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_284207352707246819) { - out_284207352707246819[0] = delta_x[0] + nom_x[0]; - out_284207352707246819[1] = delta_x[1] + nom_x[1]; - out_284207352707246819[2] = delta_x[2] + nom_x[2]; - out_284207352707246819[3] = delta_x[3] + nom_x[3]; - out_284207352707246819[4] = delta_x[4] + nom_x[4]; - out_284207352707246819[5] = delta_x[5] + nom_x[5]; - out_284207352707246819[6] = delta_x[6] + nom_x[6]; - out_284207352707246819[7] = delta_x[7] + nom_x[7]; - out_284207352707246819[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_575866575379183764) { + out_575866575379183764[0] = delta_x[0] + nom_x[0]; + out_575866575379183764[1] = delta_x[1] + nom_x[1]; + out_575866575379183764[2] = delta_x[2] + nom_x[2]; + out_575866575379183764[3] = delta_x[3] + nom_x[3]; + out_575866575379183764[4] = delta_x[4] + nom_x[4]; + out_575866575379183764[5] = delta_x[5] + nom_x[5]; + out_575866575379183764[6] = delta_x[6] + nom_x[6]; + out_575866575379183764[7] = delta_x[7] + nom_x[7]; + out_575866575379183764[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_494072413816411138) { - out_494072413816411138[0] = -nom_x[0] + true_x[0]; - out_494072413816411138[1] = -nom_x[1] + true_x[1]; - out_494072413816411138[2] = -nom_x[2] + true_x[2]; - out_494072413816411138[3] = -nom_x[3] + true_x[3]; - out_494072413816411138[4] = -nom_x[4] + true_x[4]; - out_494072413816411138[5] = -nom_x[5] + true_x[5]; - out_494072413816411138[6] = -nom_x[6] + true_x[6]; - out_494072413816411138[7] = -nom_x[7] + true_x[7]; - out_494072413816411138[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5616167858322380498) { + out_5616167858322380498[0] = -nom_x[0] + true_x[0]; + out_5616167858322380498[1] = -nom_x[1] + true_x[1]; + out_5616167858322380498[2] = -nom_x[2] + true_x[2]; + out_5616167858322380498[3] = -nom_x[3] + true_x[3]; + out_5616167858322380498[4] = -nom_x[4] + true_x[4]; + out_5616167858322380498[5] = -nom_x[5] + true_x[5]; + out_5616167858322380498[6] = -nom_x[6] + true_x[6]; + out_5616167858322380498[7] = -nom_x[7] + true_x[7]; + out_5616167858322380498[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_4975994215663677500) { - out_4975994215663677500[0] = 1.0; - out_4975994215663677500[1] = 0.0; - out_4975994215663677500[2] = 0.0; - out_4975994215663677500[3] = 0.0; - out_4975994215663677500[4] = 0.0; - out_4975994215663677500[5] = 0.0; - out_4975994215663677500[6] = 0.0; - out_4975994215663677500[7] = 0.0; - out_4975994215663677500[8] = 0.0; - out_4975994215663677500[9] = 0.0; - out_4975994215663677500[10] = 1.0; - out_4975994215663677500[11] = 0.0; - out_4975994215663677500[12] = 0.0; - out_4975994215663677500[13] = 0.0; - out_4975994215663677500[14] = 0.0; - out_4975994215663677500[15] = 0.0; - out_4975994215663677500[16] = 0.0; - out_4975994215663677500[17] = 0.0; - out_4975994215663677500[18] = 0.0; - out_4975994215663677500[19] = 0.0; - out_4975994215663677500[20] = 1.0; - out_4975994215663677500[21] = 0.0; - out_4975994215663677500[22] = 0.0; - out_4975994215663677500[23] = 0.0; - out_4975994215663677500[24] = 0.0; - out_4975994215663677500[25] = 0.0; - out_4975994215663677500[26] = 0.0; - out_4975994215663677500[27] = 0.0; - out_4975994215663677500[28] = 0.0; - out_4975994215663677500[29] = 0.0; - out_4975994215663677500[30] = 1.0; - out_4975994215663677500[31] = 0.0; - out_4975994215663677500[32] = 0.0; - out_4975994215663677500[33] = 0.0; - out_4975994215663677500[34] = 0.0; - out_4975994215663677500[35] = 0.0; - out_4975994215663677500[36] = 0.0; - out_4975994215663677500[37] = 0.0; - out_4975994215663677500[38] = 0.0; - out_4975994215663677500[39] = 0.0; - out_4975994215663677500[40] = 1.0; - out_4975994215663677500[41] = 0.0; - out_4975994215663677500[42] = 0.0; - out_4975994215663677500[43] = 0.0; - out_4975994215663677500[44] = 0.0; - out_4975994215663677500[45] = 0.0; - out_4975994215663677500[46] = 0.0; - out_4975994215663677500[47] = 0.0; - out_4975994215663677500[48] = 0.0; - out_4975994215663677500[49] = 0.0; - out_4975994215663677500[50] = 1.0; - out_4975994215663677500[51] = 0.0; - out_4975994215663677500[52] = 0.0; - out_4975994215663677500[53] = 0.0; - out_4975994215663677500[54] = 0.0; - out_4975994215663677500[55] = 0.0; - out_4975994215663677500[56] = 0.0; - out_4975994215663677500[57] = 0.0; - out_4975994215663677500[58] = 0.0; - out_4975994215663677500[59] = 0.0; - out_4975994215663677500[60] = 1.0; - out_4975994215663677500[61] = 0.0; - out_4975994215663677500[62] = 0.0; - out_4975994215663677500[63] = 0.0; - out_4975994215663677500[64] = 0.0; - out_4975994215663677500[65] = 0.0; - out_4975994215663677500[66] = 0.0; - out_4975994215663677500[67] = 0.0; - out_4975994215663677500[68] = 0.0; - out_4975994215663677500[69] = 0.0; - out_4975994215663677500[70] = 1.0; - out_4975994215663677500[71] = 0.0; - out_4975994215663677500[72] = 0.0; - out_4975994215663677500[73] = 0.0; - out_4975994215663677500[74] = 0.0; - out_4975994215663677500[75] = 0.0; - out_4975994215663677500[76] = 0.0; - out_4975994215663677500[77] = 0.0; - out_4975994215663677500[78] = 0.0; - out_4975994215663677500[79] = 0.0; - out_4975994215663677500[80] = 1.0; +void H_mod_fun(double *state, double *out_6639538403135995006) { + out_6639538403135995006[0] = 1.0; + out_6639538403135995006[1] = 0.0; + out_6639538403135995006[2] = 0.0; + out_6639538403135995006[3] = 0.0; + out_6639538403135995006[4] = 0.0; + out_6639538403135995006[5] = 0.0; + out_6639538403135995006[6] = 0.0; + out_6639538403135995006[7] = 0.0; + out_6639538403135995006[8] = 0.0; + out_6639538403135995006[9] = 0.0; + out_6639538403135995006[10] = 1.0; + out_6639538403135995006[11] = 0.0; + out_6639538403135995006[12] = 0.0; + out_6639538403135995006[13] = 0.0; + out_6639538403135995006[14] = 0.0; + out_6639538403135995006[15] = 0.0; + out_6639538403135995006[16] = 0.0; + out_6639538403135995006[17] = 0.0; + out_6639538403135995006[18] = 0.0; + out_6639538403135995006[19] = 0.0; + out_6639538403135995006[20] = 1.0; + out_6639538403135995006[21] = 0.0; + out_6639538403135995006[22] = 0.0; + out_6639538403135995006[23] = 0.0; + out_6639538403135995006[24] = 0.0; + out_6639538403135995006[25] = 0.0; + out_6639538403135995006[26] = 0.0; + out_6639538403135995006[27] = 0.0; + out_6639538403135995006[28] = 0.0; + out_6639538403135995006[29] = 0.0; + out_6639538403135995006[30] = 1.0; + out_6639538403135995006[31] = 0.0; + out_6639538403135995006[32] = 0.0; + out_6639538403135995006[33] = 0.0; + out_6639538403135995006[34] = 0.0; + out_6639538403135995006[35] = 0.0; + out_6639538403135995006[36] = 0.0; + out_6639538403135995006[37] = 0.0; + out_6639538403135995006[38] = 0.0; + out_6639538403135995006[39] = 0.0; + out_6639538403135995006[40] = 1.0; + out_6639538403135995006[41] = 0.0; + out_6639538403135995006[42] = 0.0; + out_6639538403135995006[43] = 0.0; + out_6639538403135995006[44] = 0.0; + out_6639538403135995006[45] = 0.0; + out_6639538403135995006[46] = 0.0; + out_6639538403135995006[47] = 0.0; + out_6639538403135995006[48] = 0.0; + out_6639538403135995006[49] = 0.0; + out_6639538403135995006[50] = 1.0; + out_6639538403135995006[51] = 0.0; + out_6639538403135995006[52] = 0.0; + out_6639538403135995006[53] = 0.0; + out_6639538403135995006[54] = 0.0; + out_6639538403135995006[55] = 0.0; + out_6639538403135995006[56] = 0.0; + out_6639538403135995006[57] = 0.0; + out_6639538403135995006[58] = 0.0; + out_6639538403135995006[59] = 0.0; + out_6639538403135995006[60] = 1.0; + out_6639538403135995006[61] = 0.0; + out_6639538403135995006[62] = 0.0; + out_6639538403135995006[63] = 0.0; + out_6639538403135995006[64] = 0.0; + out_6639538403135995006[65] = 0.0; + out_6639538403135995006[66] = 0.0; + out_6639538403135995006[67] = 0.0; + out_6639538403135995006[68] = 0.0; + out_6639538403135995006[69] = 0.0; + out_6639538403135995006[70] = 1.0; + out_6639538403135995006[71] = 0.0; + out_6639538403135995006[72] = 0.0; + out_6639538403135995006[73] = 0.0; + out_6639538403135995006[74] = 0.0; + out_6639538403135995006[75] = 0.0; + out_6639538403135995006[76] = 0.0; + out_6639538403135995006[77] = 0.0; + out_6639538403135995006[78] = 0.0; + out_6639538403135995006[79] = 0.0; + out_6639538403135995006[80] = 1.0; } -void f_fun(double *state, double dt, double *out_1721040043494422786) { - out_1721040043494422786[0] = state[0]; - out_1721040043494422786[1] = state[1]; - out_1721040043494422786[2] = state[2]; - out_1721040043494422786[3] = state[3]; - out_1721040043494422786[4] = state[4]; - out_1721040043494422786[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_1721040043494422786[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_1721040043494422786[7] = state[7]; - out_1721040043494422786[8] = state[8]; +void f_fun(double *state, double dt, double *out_6721417124249708482) { + out_6721417124249708482[0] = state[0]; + out_6721417124249708482[1] = state[1]; + out_6721417124249708482[2] = state[2]; + out_6721417124249708482[3] = state[3]; + out_6721417124249708482[4] = state[4]; + out_6721417124249708482[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_6721417124249708482[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_6721417124249708482[7] = state[7]; + out_6721417124249708482[8] = state[8]; } -void F_fun(double *state, double dt, double *out_6408657319260780093) { - out_6408657319260780093[0] = 1; - out_6408657319260780093[1] = 0; - out_6408657319260780093[2] = 0; - out_6408657319260780093[3] = 0; - out_6408657319260780093[4] = 0; - out_6408657319260780093[5] = 0; - out_6408657319260780093[6] = 0; - out_6408657319260780093[7] = 0; - out_6408657319260780093[8] = 0; - out_6408657319260780093[9] = 0; - out_6408657319260780093[10] = 1; - out_6408657319260780093[11] = 0; - out_6408657319260780093[12] = 0; - out_6408657319260780093[13] = 0; - out_6408657319260780093[14] = 0; - out_6408657319260780093[15] = 0; - out_6408657319260780093[16] = 0; - out_6408657319260780093[17] = 0; - out_6408657319260780093[18] = 0; - out_6408657319260780093[19] = 0; - out_6408657319260780093[20] = 1; - out_6408657319260780093[21] = 0; - out_6408657319260780093[22] = 0; - out_6408657319260780093[23] = 0; - out_6408657319260780093[24] = 0; - out_6408657319260780093[25] = 0; - out_6408657319260780093[26] = 0; - out_6408657319260780093[27] = 0; - out_6408657319260780093[28] = 0; - out_6408657319260780093[29] = 0; - out_6408657319260780093[30] = 1; - out_6408657319260780093[31] = 0; - out_6408657319260780093[32] = 0; - out_6408657319260780093[33] = 0; - out_6408657319260780093[34] = 0; - out_6408657319260780093[35] = 0; - out_6408657319260780093[36] = 0; - out_6408657319260780093[37] = 0; - out_6408657319260780093[38] = 0; - out_6408657319260780093[39] = 0; - out_6408657319260780093[40] = 1; - out_6408657319260780093[41] = 0; - out_6408657319260780093[42] = 0; - out_6408657319260780093[43] = 0; - out_6408657319260780093[44] = 0; - out_6408657319260780093[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_6408657319260780093[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_6408657319260780093[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6408657319260780093[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6408657319260780093[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_6408657319260780093[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_6408657319260780093[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_6408657319260780093[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_6408657319260780093[53] = -9.8100000000000005*dt; - out_6408657319260780093[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_6408657319260780093[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_6408657319260780093[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6408657319260780093[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6408657319260780093[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_6408657319260780093[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_6408657319260780093[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_6408657319260780093[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6408657319260780093[62] = 0; - out_6408657319260780093[63] = 0; - out_6408657319260780093[64] = 0; - out_6408657319260780093[65] = 0; - out_6408657319260780093[66] = 0; - out_6408657319260780093[67] = 0; - out_6408657319260780093[68] = 0; - out_6408657319260780093[69] = 0; - out_6408657319260780093[70] = 1; - out_6408657319260780093[71] = 0; - out_6408657319260780093[72] = 0; - out_6408657319260780093[73] = 0; - out_6408657319260780093[74] = 0; - out_6408657319260780093[75] = 0; - out_6408657319260780093[76] = 0; - out_6408657319260780093[77] = 0; - out_6408657319260780093[78] = 0; - out_6408657319260780093[79] = 0; - out_6408657319260780093[80] = 1; +void F_fun(double *state, double dt, double *out_2916284287213904038) { + out_2916284287213904038[0] = 1; + out_2916284287213904038[1] = 0; + out_2916284287213904038[2] = 0; + out_2916284287213904038[3] = 0; + out_2916284287213904038[4] = 0; + out_2916284287213904038[5] = 0; + out_2916284287213904038[6] = 0; + out_2916284287213904038[7] = 0; + out_2916284287213904038[8] = 0; + out_2916284287213904038[9] = 0; + out_2916284287213904038[10] = 1; + out_2916284287213904038[11] = 0; + out_2916284287213904038[12] = 0; + out_2916284287213904038[13] = 0; + out_2916284287213904038[14] = 0; + out_2916284287213904038[15] = 0; + out_2916284287213904038[16] = 0; + out_2916284287213904038[17] = 0; + out_2916284287213904038[18] = 0; + out_2916284287213904038[19] = 0; + out_2916284287213904038[20] = 1; + out_2916284287213904038[21] = 0; + out_2916284287213904038[22] = 0; + out_2916284287213904038[23] = 0; + out_2916284287213904038[24] = 0; + out_2916284287213904038[25] = 0; + out_2916284287213904038[26] = 0; + out_2916284287213904038[27] = 0; + out_2916284287213904038[28] = 0; + out_2916284287213904038[29] = 0; + out_2916284287213904038[30] = 1; + out_2916284287213904038[31] = 0; + out_2916284287213904038[32] = 0; + out_2916284287213904038[33] = 0; + out_2916284287213904038[34] = 0; + out_2916284287213904038[35] = 0; + out_2916284287213904038[36] = 0; + out_2916284287213904038[37] = 0; + out_2916284287213904038[38] = 0; + out_2916284287213904038[39] = 0; + out_2916284287213904038[40] = 1; + out_2916284287213904038[41] = 0; + out_2916284287213904038[42] = 0; + out_2916284287213904038[43] = 0; + out_2916284287213904038[44] = 0; + out_2916284287213904038[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_2916284287213904038[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2916284287213904038[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2916284287213904038[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2916284287213904038[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_2916284287213904038[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2916284287213904038[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2916284287213904038[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2916284287213904038[53] = -9.8100000000000005*dt; + out_2916284287213904038[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_2916284287213904038[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2916284287213904038[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2916284287213904038[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2916284287213904038[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_2916284287213904038[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2916284287213904038[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_2916284287213904038[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2916284287213904038[62] = 0; + out_2916284287213904038[63] = 0; + out_2916284287213904038[64] = 0; + out_2916284287213904038[65] = 0; + out_2916284287213904038[66] = 0; + out_2916284287213904038[67] = 0; + out_2916284287213904038[68] = 0; + out_2916284287213904038[69] = 0; + out_2916284287213904038[70] = 1; + out_2916284287213904038[71] = 0; + out_2916284287213904038[72] = 0; + out_2916284287213904038[73] = 0; + out_2916284287213904038[74] = 0; + out_2916284287213904038[75] = 0; + out_2916284287213904038[76] = 0; + out_2916284287213904038[77] = 0; + out_2916284287213904038[78] = 0; + out_2916284287213904038[79] = 0; + out_2916284287213904038[80] = 1; } -void h_25(double *state, double *unused, double *out_8679682106878424978) { - out_8679682106878424978[0] = state[6]; +void h_25(double *state, double *unused, double *out_2044795451145406261) { + out_2044795451145406261[0] = state[6]; } -void H_25(double *state, double *unused, double *out_4470659773463380788) { - out_4470659773463380788[0] = 0; - out_4470659773463380788[1] = 0; - out_4470659773463380788[2] = 0; - out_4470659773463380788[3] = 0; - out_4470659773463380788[4] = 0; - out_4470659773463380788[5] = 0; - out_4470659773463380788[6] = 1; - out_4470659773463380788[7] = 0; - out_4470659773463380788[8] = 0; +void H_25(double *state, double *unused, double *out_8284225020584064933) { + out_8284225020584064933[0] = 0; + out_8284225020584064933[1] = 0; + out_8284225020584064933[2] = 0; + out_8284225020584064933[3] = 0; + out_8284225020584064933[4] = 0; + out_8284225020584064933[5] = 0; + out_8284225020584064933[6] = 1; + out_8284225020584064933[7] = 0; + out_8284225020584064933[8] = 0; } -void h_24(double *state, double *unused, double *out_5665719925403943373) { - out_5665719925403943373[0] = state[4]; - out_5665719925403943373[1] = state[5]; +void h_24(double *state, double *unused, double *out_1441131931248447039) { + out_1441131931248447039[0] = state[4]; + out_1441131931248447039[1] = state[5]; } -void H_24(double *state, double *unused, double *out_7514976983467728980) { - out_7514976983467728980[0] = 0; - out_7514976983467728980[1] = 0; - out_7514976983467728980[2] = 0; - out_7514976983467728980[3] = 0; - out_7514976983467728980[4] = 1; - out_7514976983467728980[5] = 0; - out_7514976983467728980[6] = 0; - out_7514976983467728980[7] = 0; - out_7514976983467728980[8] = 0; - out_7514976983467728980[9] = 0; - out_7514976983467728980[10] = 0; - out_7514976983467728980[11] = 0; - out_7514976983467728980[12] = 0; - out_7514976983467728980[13] = 0; - out_7514976983467728980[14] = 1; - out_7514976983467728980[15] = 0; - out_7514976983467728980[16] = 0; - out_7514976983467728980[17] = 0; +void H_24(double *state, double *unused, double *out_4726299421781741797) { + out_4726299421781741797[0] = 0; + out_4726299421781741797[1] = 0; + out_4726299421781741797[2] = 0; + out_4726299421781741797[3] = 0; + out_4726299421781741797[4] = 1; + out_4726299421781741797[5] = 0; + out_4726299421781741797[6] = 0; + out_4726299421781741797[7] = 0; + out_4726299421781741797[8] = 0; + out_4726299421781741797[9] = 0; + out_4726299421781741797[10] = 0; + out_4726299421781741797[11] = 0; + out_4726299421781741797[12] = 0; + out_4726299421781741797[13] = 0; + out_4726299421781741797[14] = 1; + out_4726299421781741797[15] = 0; + out_4726299421781741797[16] = 0; + out_4726299421781741797[17] = 0; } -void h_30(double *state, double *unused, double *out_6129612438577104288) { - out_6129612438577104288[0] = state[4]; +void h_30(double *state, double *unused, double *out_8638683365743472914) { + out_8638683365743472914[0] = state[4]; } -void H_30(double *state, double *unused, double *out_4599998720606620858) { - out_4599998720606620858[0] = 0; - out_4599998720606620858[1] = 0; - out_4599998720606620858[2] = 0; - out_4599998720606620858[3] = 0; - out_4599998720606620858[4] = 1; - out_4599998720606620858[5] = 0; - out_4599998720606620858[6] = 0; - out_4599998720606620858[7] = 0; - out_4599998720606620858[8] = 0; +void H_30(double *state, double *unused, double *out_8413563967727305003) { + out_8413563967727305003[0] = 0; + out_8413563967727305003[1] = 0; + out_8413563967727305003[2] = 0; + out_8413563967727305003[3] = 0; + out_8413563967727305003[4] = 1; + out_8413563967727305003[5] = 0; + out_8413563967727305003[6] = 0; + out_8413563967727305003[7] = 0; + out_8413563967727305003[8] = 0; } -void h_26(double *state, double *unused, double *out_6887462583737923098) { - out_6887462583737923098[0] = state[7]; +void h_26(double *state, double *unused, double *out_989338192432138572) { + out_989338192432138572[0] = state[7]; } -void H_26(double *state, double *unused, double *out_3813805709353068884) { - out_3813805709353068884[0] = 0; - out_3813805709353068884[1] = 0; - out_3813805709353068884[2] = 0; - out_3813805709353068884[3] = 0; - out_3813805709353068884[4] = 0; - out_3813805709353068884[5] = 0; - out_3813805709353068884[6] = 0; - out_3813805709353068884[7] = 1; - out_3813805709353068884[8] = 0; +void H_26(double *state, double *unused, double *out_6421015734251430459) { + out_6421015734251430459[0] = 0; + out_6421015734251430459[1] = 0; + out_6421015734251430459[2] = 0; + out_6421015734251430459[3] = 0; + out_6421015734251430459[4] = 0; + out_6421015734251430459[5] = 0; + out_6421015734251430459[6] = 0; + out_6421015734251430459[7] = 1; + out_6421015734251430459[8] = 0; } -void h_27(double *state, double *unused, double *out_3337814870992394415) { - out_3337814870992394415[0] = state[3]; +void h_27(double *state, double *unused, double *out_7375345222001897423) { + out_7375345222001897423[0] = state[3]; } -void H_27(double *state, double *unused, double *out_6774762032407045769) { - out_6774762032407045769[0] = 0; - out_6774762032407045769[1] = 0; - out_6774762032407045769[2] = 0; - out_6774762032407045769[3] = 1; - out_6774762032407045769[4] = 0; - out_6774762032407045769[5] = 0; - out_6774762032407045769[6] = 0; - out_6774762032407045769[7] = 0; - out_6774762032407045769[8] = 0; +void H_27(double *state, double *unused, double *out_7858416794181821702) { + out_7858416794181821702[0] = 0; + out_7858416794181821702[1] = 0; + out_7858416794181821702[2] = 0; + out_7858416794181821702[3] = 1; + out_7858416794181821702[4] = 0; + out_7858416794181821702[5] = 0; + out_7858416794181821702[6] = 0; + out_7858416794181821702[7] = 0; + out_7858416794181821702[8] = 0; } -void h_29(double *state, double *unused, double *out_5302062605726147469) { - out_5302062605726147469[0] = state[1]; +void h_29(double *state, double *unused, double *out_4806387440305233057) { + out_4806387440305233057[0] = state[1]; } -void H_29(double *state, double *unused, double *out_4089767376292228674) { - out_4089767376292228674[0] = 0; - out_4089767376292228674[1] = 1; - out_4089767376292228674[2] = 0; - out_4089767376292228674[3] = 0; - out_4089767376292228674[4] = 0; - out_4089767376292228674[5] = 0; - out_4089767376292228674[6] = 0; - out_4089767376292228674[7] = 0; - out_4089767376292228674[8] = 0; +void H_29(double *state, double *unused, double *out_6145054067312270669) { + out_6145054067312270669[0] = 0; + out_6145054067312270669[1] = 1; + out_6145054067312270669[2] = 0; + out_6145054067312270669[3] = 0; + out_6145054067312270669[4] = 0; + out_6145054067312270669[5] = 0; + out_6145054067312270669[6] = 0; + out_6145054067312270669[7] = 0; + out_6145054067312270669[8] = 0; } -void h_28(double *state, double *unused, double *out_301028819548061730) { - out_301028819548061730[0] = state[0]; +void h_28(double *state, double *unused, double *out_6025091355491401381) { + out_6025091355491401381[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2126137104726902423) { - out_2126137104726902423[0] = 1; - out_2126137104726902423[1] = 0; - out_2126137104726902423[2] = 0; - out_2126137104726902423[3] = 0; - out_2126137104726902423[4] = 0; - out_2126137104726902423[5] = 0; - out_2126137104726902423[6] = 0; - out_2126137104726902423[7] = 0; - out_2126137104726902423[8] = 0; +void H_28(double *state, double *unused, double *out_8108684338877596920) { + out_8108684338877596920[0] = 1; + out_8108684338877596920[1] = 0; + out_8108684338877596920[2] = 0; + out_8108684338877596920[3] = 0; + out_8108684338877596920[4] = 0; + out_8108684338877596920[5] = 0; + out_8108684338877596920[6] = 0; + out_8108684338877596920[7] = 0; + out_8108684338877596920[8] = 0; } -void h_31(double *state, double *unused, double *out_213227004039568063) { - out_213227004039568063[0] = state[8]; +void h_31(double *state, double *unused, double *out_2208042107618306896) { + out_2208042107618306896[0] = state[8]; } -void H_31(double *state, double *unused, double *out_4440013811586420360) { - out_4440013811586420360[0] = 0; - out_4440013811586420360[1] = 0; - out_4440013811586420360[2] = 0; - out_4440013811586420360[3] = 0; - out_4440013811586420360[4] = 0; - out_4440013811586420360[5] = 0; - out_4440013811586420360[6] = 0; - out_4440013811586420360[7] = 0; - out_4440013811586420360[8] = 1; +void H_31(double *state, double *unused, double *out_8253579058707104505) { + out_8253579058707104505[0] = 0; + out_8253579058707104505[1] = 0; + out_8253579058707104505[2] = 0; + out_8253579058707104505[3] = 0; + out_8253579058707104505[4] = 0; + out_8253579058707104505[5] = 0; + out_8253579058707104505[6] = 0; + out_8253579058707104505[7] = 0; + out_8253579058707104505[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_284207352707246819) { - err_fun(nom_x, delta_x, out_284207352707246819); +void car_err_fun(double *nom_x, double *delta_x, double *out_575866575379183764) { + err_fun(nom_x, delta_x, out_575866575379183764); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_494072413816411138) { - inv_err_fun(nom_x, true_x, out_494072413816411138); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5616167858322380498) { + inv_err_fun(nom_x, true_x, out_5616167858322380498); } -void car_H_mod_fun(double *state, double *out_4975994215663677500) { - H_mod_fun(state, out_4975994215663677500); +void car_H_mod_fun(double *state, double *out_6639538403135995006) { + H_mod_fun(state, out_6639538403135995006); } -void car_f_fun(double *state, double dt, double *out_1721040043494422786) { - f_fun(state, dt, out_1721040043494422786); +void car_f_fun(double *state, double dt, double *out_6721417124249708482) { + f_fun(state, dt, out_6721417124249708482); } -void car_F_fun(double *state, double dt, double *out_6408657319260780093) { - F_fun(state, dt, out_6408657319260780093); +void car_F_fun(double *state, double dt, double *out_2916284287213904038) { + F_fun(state, dt, out_2916284287213904038); } -void car_h_25(double *state, double *unused, double *out_8679682106878424978) { - h_25(state, unused, out_8679682106878424978); +void car_h_25(double *state, double *unused, double *out_2044795451145406261) { + h_25(state, unused, out_2044795451145406261); } -void car_H_25(double *state, double *unused, double *out_4470659773463380788) { - H_25(state, unused, out_4470659773463380788); +void car_H_25(double *state, double *unused, double *out_8284225020584064933) { + H_25(state, unused, out_8284225020584064933); } -void car_h_24(double *state, double *unused, double *out_5665719925403943373) { - h_24(state, unused, out_5665719925403943373); +void car_h_24(double *state, double *unused, double *out_1441131931248447039) { + h_24(state, unused, out_1441131931248447039); } -void car_H_24(double *state, double *unused, double *out_7514976983467728980) { - H_24(state, unused, out_7514976983467728980); +void car_H_24(double *state, double *unused, double *out_4726299421781741797) { + H_24(state, unused, out_4726299421781741797); } -void car_h_30(double *state, double *unused, double *out_6129612438577104288) { - h_30(state, unused, out_6129612438577104288); +void car_h_30(double *state, double *unused, double *out_8638683365743472914) { + h_30(state, unused, out_8638683365743472914); } -void car_H_30(double *state, double *unused, double *out_4599998720606620858) { - H_30(state, unused, out_4599998720606620858); +void car_H_30(double *state, double *unused, double *out_8413563967727305003) { + H_30(state, unused, out_8413563967727305003); } -void car_h_26(double *state, double *unused, double *out_6887462583737923098) { - h_26(state, unused, out_6887462583737923098); +void car_h_26(double *state, double *unused, double *out_989338192432138572) { + h_26(state, unused, out_989338192432138572); } -void car_H_26(double *state, double *unused, double *out_3813805709353068884) { - H_26(state, unused, out_3813805709353068884); +void car_H_26(double *state, double *unused, double *out_6421015734251430459) { + H_26(state, unused, out_6421015734251430459); } -void car_h_27(double *state, double *unused, double *out_3337814870992394415) { - h_27(state, unused, out_3337814870992394415); +void car_h_27(double *state, double *unused, double *out_7375345222001897423) { + h_27(state, unused, out_7375345222001897423); } -void car_H_27(double *state, double *unused, double *out_6774762032407045769) { - H_27(state, unused, out_6774762032407045769); +void car_H_27(double *state, double *unused, double *out_7858416794181821702) { + H_27(state, unused, out_7858416794181821702); } -void car_h_29(double *state, double *unused, double *out_5302062605726147469) { - h_29(state, unused, out_5302062605726147469); +void car_h_29(double *state, double *unused, double *out_4806387440305233057) { + h_29(state, unused, out_4806387440305233057); } -void car_H_29(double *state, double *unused, double *out_4089767376292228674) { - H_29(state, unused, out_4089767376292228674); +void car_H_29(double *state, double *unused, double *out_6145054067312270669) { + H_29(state, unused, out_6145054067312270669); } -void car_h_28(double *state, double *unused, double *out_301028819548061730) { - h_28(state, unused, out_301028819548061730); +void car_h_28(double *state, double *unused, double *out_6025091355491401381) { + h_28(state, unused, out_6025091355491401381); } -void car_H_28(double *state, double *unused, double *out_2126137104726902423) { - H_28(state, unused, out_2126137104726902423); +void car_H_28(double *state, double *unused, double *out_8108684338877596920) { + H_28(state, unused, out_8108684338877596920); } -void car_h_31(double *state, double *unused, double *out_213227004039568063) { - h_31(state, unused, out_213227004039568063); +void car_h_31(double *state, double *unused, double *out_2208042107618306896) { + h_31(state, unused, out_2208042107618306896); } -void car_H_31(double *state, double *unused, double *out_4440013811586420360) { - H_31(state, unused, out_4440013811586420360); +void car_H_31(double *state, double *unused, double *out_8253579058707104505) { + H_31(state, unused, out_8253579058707104505); } 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 4035e5f06f..5c08a725dd 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_284207352707246819); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_494072413816411138); -void car_H_mod_fun(double *state, double *out_4975994215663677500); -void car_f_fun(double *state, double dt, double *out_1721040043494422786); -void car_F_fun(double *state, double dt, double *out_6408657319260780093); -void car_h_25(double *state, double *unused, double *out_8679682106878424978); -void car_H_25(double *state, double *unused, double *out_4470659773463380788); -void car_h_24(double *state, double *unused, double *out_5665719925403943373); -void car_H_24(double *state, double *unused, double *out_7514976983467728980); -void car_h_30(double *state, double *unused, double *out_6129612438577104288); -void car_H_30(double *state, double *unused, double *out_4599998720606620858); -void car_h_26(double *state, double *unused, double *out_6887462583737923098); -void car_H_26(double *state, double *unused, double *out_3813805709353068884); -void car_h_27(double *state, double *unused, double *out_3337814870992394415); -void car_H_27(double *state, double *unused, double *out_6774762032407045769); -void car_h_29(double *state, double *unused, double *out_5302062605726147469); -void car_H_29(double *state, double *unused, double *out_4089767376292228674); -void car_h_28(double *state, double *unused, double *out_301028819548061730); -void car_H_28(double *state, double *unused, double *out_2126137104726902423); -void car_h_31(double *state, double *unused, double *out_213227004039568063); -void car_H_31(double *state, double *unused, double *out_4440013811586420360); +void car_err_fun(double *nom_x, double *delta_x, double *out_575866575379183764); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5616167858322380498); +void car_H_mod_fun(double *state, double *out_6639538403135995006); +void car_f_fun(double *state, double dt, double *out_6721417124249708482); +void car_F_fun(double *state, double dt, double *out_2916284287213904038); +void car_h_25(double *state, double *unused, double *out_2044795451145406261); +void car_H_25(double *state, double *unused, double *out_8284225020584064933); +void car_h_24(double *state, double *unused, double *out_1441131931248447039); +void car_H_24(double *state, double *unused, double *out_4726299421781741797); +void car_h_30(double *state, double *unused, double *out_8638683365743472914); +void car_H_30(double *state, double *unused, double *out_8413563967727305003); +void car_h_26(double *state, double *unused, double *out_989338192432138572); +void car_H_26(double *state, double *unused, double *out_6421015734251430459); +void car_h_27(double *state, double *unused, double *out_7375345222001897423); +void car_H_27(double *state, double *unused, double *out_7858416794181821702); +void car_h_29(double *state, double *unused, double *out_4806387440305233057); +void car_H_29(double *state, double *unused, double *out_6145054067312270669); +void car_h_28(double *state, double *unused, double *out_6025091355491401381); +void car_H_28(double *state, double *unused, double *out_8108684338877596920); +void car_h_31(double *state, double *unused, double *out_2208042107618306896); +void car_H_31(double *state, double *unused, double *out_8253579058707104505); 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 9fa0411cc2..749d3bf030 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_628539194751276084) { - out_628539194751276084[0] = delta_x[0] + nom_x[0]; - out_628539194751276084[1] = delta_x[1] + nom_x[1]; - out_628539194751276084[2] = delta_x[2] + nom_x[2]; - out_628539194751276084[3] = delta_x[3] + nom_x[3]; - out_628539194751276084[4] = delta_x[4] + nom_x[4]; - out_628539194751276084[5] = delta_x[5] + nom_x[5]; - out_628539194751276084[6] = delta_x[6] + nom_x[6]; - out_628539194751276084[7] = delta_x[7] + nom_x[7]; - out_628539194751276084[8] = delta_x[8] + nom_x[8]; - out_628539194751276084[9] = delta_x[9] + nom_x[9]; - out_628539194751276084[10] = delta_x[10] + nom_x[10]; - out_628539194751276084[11] = delta_x[11] + nom_x[11]; - out_628539194751276084[12] = delta_x[12] + nom_x[12]; - out_628539194751276084[13] = delta_x[13] + nom_x[13]; - out_628539194751276084[14] = delta_x[14] + nom_x[14]; - out_628539194751276084[15] = delta_x[15] + nom_x[15]; - out_628539194751276084[16] = delta_x[16] + nom_x[16]; - out_628539194751276084[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_8315489890310503187) { + out_8315489890310503187[0] = delta_x[0] + nom_x[0]; + out_8315489890310503187[1] = delta_x[1] + nom_x[1]; + out_8315489890310503187[2] = delta_x[2] + nom_x[2]; + out_8315489890310503187[3] = delta_x[3] + nom_x[3]; + out_8315489890310503187[4] = delta_x[4] + nom_x[4]; + out_8315489890310503187[5] = delta_x[5] + nom_x[5]; + out_8315489890310503187[6] = delta_x[6] + nom_x[6]; + out_8315489890310503187[7] = delta_x[7] + nom_x[7]; + out_8315489890310503187[8] = delta_x[8] + nom_x[8]; + out_8315489890310503187[9] = delta_x[9] + nom_x[9]; + out_8315489890310503187[10] = delta_x[10] + nom_x[10]; + out_8315489890310503187[11] = delta_x[11] + nom_x[11]; + out_8315489890310503187[12] = delta_x[12] + nom_x[12]; + out_8315489890310503187[13] = delta_x[13] + nom_x[13]; + out_8315489890310503187[14] = delta_x[14] + nom_x[14]; + out_8315489890310503187[15] = delta_x[15] + nom_x[15]; + out_8315489890310503187[16] = delta_x[16] + nom_x[16]; + out_8315489890310503187[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5785405288888399451) { - out_5785405288888399451[0] = -nom_x[0] + true_x[0]; - out_5785405288888399451[1] = -nom_x[1] + true_x[1]; - out_5785405288888399451[2] = -nom_x[2] + true_x[2]; - out_5785405288888399451[3] = -nom_x[3] + true_x[3]; - out_5785405288888399451[4] = -nom_x[4] + true_x[4]; - out_5785405288888399451[5] = -nom_x[5] + true_x[5]; - out_5785405288888399451[6] = -nom_x[6] + true_x[6]; - out_5785405288888399451[7] = -nom_x[7] + true_x[7]; - out_5785405288888399451[8] = -nom_x[8] + true_x[8]; - out_5785405288888399451[9] = -nom_x[9] + true_x[9]; - out_5785405288888399451[10] = -nom_x[10] + true_x[10]; - out_5785405288888399451[11] = -nom_x[11] + true_x[11]; - out_5785405288888399451[12] = -nom_x[12] + true_x[12]; - out_5785405288888399451[13] = -nom_x[13] + true_x[13]; - out_5785405288888399451[14] = -nom_x[14] + true_x[14]; - out_5785405288888399451[15] = -nom_x[15] + true_x[15]; - out_5785405288888399451[16] = -nom_x[16] + true_x[16]; - out_5785405288888399451[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6008596051390826841) { + out_6008596051390826841[0] = -nom_x[0] + true_x[0]; + out_6008596051390826841[1] = -nom_x[1] + true_x[1]; + out_6008596051390826841[2] = -nom_x[2] + true_x[2]; + out_6008596051390826841[3] = -nom_x[3] + true_x[3]; + out_6008596051390826841[4] = -nom_x[4] + true_x[4]; + out_6008596051390826841[5] = -nom_x[5] + true_x[5]; + out_6008596051390826841[6] = -nom_x[6] + true_x[6]; + out_6008596051390826841[7] = -nom_x[7] + true_x[7]; + out_6008596051390826841[8] = -nom_x[8] + true_x[8]; + out_6008596051390826841[9] = -nom_x[9] + true_x[9]; + out_6008596051390826841[10] = -nom_x[10] + true_x[10]; + out_6008596051390826841[11] = -nom_x[11] + true_x[11]; + out_6008596051390826841[12] = -nom_x[12] + true_x[12]; + out_6008596051390826841[13] = -nom_x[13] + true_x[13]; + out_6008596051390826841[14] = -nom_x[14] + true_x[14]; + out_6008596051390826841[15] = -nom_x[15] + true_x[15]; + out_6008596051390826841[16] = -nom_x[16] + true_x[16]; + out_6008596051390826841[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_1308901075637477725) { - out_1308901075637477725[0] = 1.0; - out_1308901075637477725[1] = 0.0; - out_1308901075637477725[2] = 0.0; - out_1308901075637477725[3] = 0.0; - out_1308901075637477725[4] = 0.0; - out_1308901075637477725[5] = 0.0; - out_1308901075637477725[6] = 0.0; - out_1308901075637477725[7] = 0.0; - out_1308901075637477725[8] = 0.0; - out_1308901075637477725[9] = 0.0; - out_1308901075637477725[10] = 0.0; - out_1308901075637477725[11] = 0.0; - out_1308901075637477725[12] = 0.0; - out_1308901075637477725[13] = 0.0; - out_1308901075637477725[14] = 0.0; - out_1308901075637477725[15] = 0.0; - out_1308901075637477725[16] = 0.0; - out_1308901075637477725[17] = 0.0; - out_1308901075637477725[18] = 0.0; - out_1308901075637477725[19] = 1.0; - out_1308901075637477725[20] = 0.0; - out_1308901075637477725[21] = 0.0; - out_1308901075637477725[22] = 0.0; - out_1308901075637477725[23] = 0.0; - out_1308901075637477725[24] = 0.0; - out_1308901075637477725[25] = 0.0; - out_1308901075637477725[26] = 0.0; - out_1308901075637477725[27] = 0.0; - out_1308901075637477725[28] = 0.0; - out_1308901075637477725[29] = 0.0; - out_1308901075637477725[30] = 0.0; - out_1308901075637477725[31] = 0.0; - out_1308901075637477725[32] = 0.0; - out_1308901075637477725[33] = 0.0; - out_1308901075637477725[34] = 0.0; - out_1308901075637477725[35] = 0.0; - out_1308901075637477725[36] = 0.0; - out_1308901075637477725[37] = 0.0; - out_1308901075637477725[38] = 1.0; - out_1308901075637477725[39] = 0.0; - out_1308901075637477725[40] = 0.0; - out_1308901075637477725[41] = 0.0; - out_1308901075637477725[42] = 0.0; - out_1308901075637477725[43] = 0.0; - out_1308901075637477725[44] = 0.0; - out_1308901075637477725[45] = 0.0; - out_1308901075637477725[46] = 0.0; - out_1308901075637477725[47] = 0.0; - out_1308901075637477725[48] = 0.0; - out_1308901075637477725[49] = 0.0; - out_1308901075637477725[50] = 0.0; - out_1308901075637477725[51] = 0.0; - out_1308901075637477725[52] = 0.0; - out_1308901075637477725[53] = 0.0; - out_1308901075637477725[54] = 0.0; - out_1308901075637477725[55] = 0.0; - out_1308901075637477725[56] = 0.0; - out_1308901075637477725[57] = 1.0; - out_1308901075637477725[58] = 0.0; - out_1308901075637477725[59] = 0.0; - out_1308901075637477725[60] = 0.0; - out_1308901075637477725[61] = 0.0; - out_1308901075637477725[62] = 0.0; - out_1308901075637477725[63] = 0.0; - out_1308901075637477725[64] = 0.0; - out_1308901075637477725[65] = 0.0; - out_1308901075637477725[66] = 0.0; - out_1308901075637477725[67] = 0.0; - out_1308901075637477725[68] = 0.0; - out_1308901075637477725[69] = 0.0; - out_1308901075637477725[70] = 0.0; - out_1308901075637477725[71] = 0.0; - out_1308901075637477725[72] = 0.0; - out_1308901075637477725[73] = 0.0; - out_1308901075637477725[74] = 0.0; - out_1308901075637477725[75] = 0.0; - out_1308901075637477725[76] = 1.0; - out_1308901075637477725[77] = 0.0; - out_1308901075637477725[78] = 0.0; - out_1308901075637477725[79] = 0.0; - out_1308901075637477725[80] = 0.0; - out_1308901075637477725[81] = 0.0; - out_1308901075637477725[82] = 0.0; - out_1308901075637477725[83] = 0.0; - out_1308901075637477725[84] = 0.0; - out_1308901075637477725[85] = 0.0; - out_1308901075637477725[86] = 0.0; - out_1308901075637477725[87] = 0.0; - out_1308901075637477725[88] = 0.0; - out_1308901075637477725[89] = 0.0; - out_1308901075637477725[90] = 0.0; - out_1308901075637477725[91] = 0.0; - out_1308901075637477725[92] = 0.0; - out_1308901075637477725[93] = 0.0; - out_1308901075637477725[94] = 0.0; - out_1308901075637477725[95] = 1.0; - out_1308901075637477725[96] = 0.0; - out_1308901075637477725[97] = 0.0; - out_1308901075637477725[98] = 0.0; - out_1308901075637477725[99] = 0.0; - out_1308901075637477725[100] = 0.0; - out_1308901075637477725[101] = 0.0; - out_1308901075637477725[102] = 0.0; - out_1308901075637477725[103] = 0.0; - out_1308901075637477725[104] = 0.0; - out_1308901075637477725[105] = 0.0; - out_1308901075637477725[106] = 0.0; - out_1308901075637477725[107] = 0.0; - out_1308901075637477725[108] = 0.0; - out_1308901075637477725[109] = 0.0; - out_1308901075637477725[110] = 0.0; - out_1308901075637477725[111] = 0.0; - out_1308901075637477725[112] = 0.0; - out_1308901075637477725[113] = 0.0; - out_1308901075637477725[114] = 1.0; - out_1308901075637477725[115] = 0.0; - out_1308901075637477725[116] = 0.0; - out_1308901075637477725[117] = 0.0; - out_1308901075637477725[118] = 0.0; - out_1308901075637477725[119] = 0.0; - out_1308901075637477725[120] = 0.0; - out_1308901075637477725[121] = 0.0; - out_1308901075637477725[122] = 0.0; - out_1308901075637477725[123] = 0.0; - out_1308901075637477725[124] = 0.0; - out_1308901075637477725[125] = 0.0; - out_1308901075637477725[126] = 0.0; - out_1308901075637477725[127] = 0.0; - out_1308901075637477725[128] = 0.0; - out_1308901075637477725[129] = 0.0; - out_1308901075637477725[130] = 0.0; - out_1308901075637477725[131] = 0.0; - out_1308901075637477725[132] = 0.0; - out_1308901075637477725[133] = 1.0; - out_1308901075637477725[134] = 0.0; - out_1308901075637477725[135] = 0.0; - out_1308901075637477725[136] = 0.0; - out_1308901075637477725[137] = 0.0; - out_1308901075637477725[138] = 0.0; - out_1308901075637477725[139] = 0.0; - out_1308901075637477725[140] = 0.0; - out_1308901075637477725[141] = 0.0; - out_1308901075637477725[142] = 0.0; - out_1308901075637477725[143] = 0.0; - out_1308901075637477725[144] = 0.0; - out_1308901075637477725[145] = 0.0; - out_1308901075637477725[146] = 0.0; - out_1308901075637477725[147] = 0.0; - out_1308901075637477725[148] = 0.0; - out_1308901075637477725[149] = 0.0; - out_1308901075637477725[150] = 0.0; - out_1308901075637477725[151] = 0.0; - out_1308901075637477725[152] = 1.0; - out_1308901075637477725[153] = 0.0; - out_1308901075637477725[154] = 0.0; - out_1308901075637477725[155] = 0.0; - out_1308901075637477725[156] = 0.0; - out_1308901075637477725[157] = 0.0; - out_1308901075637477725[158] = 0.0; - out_1308901075637477725[159] = 0.0; - out_1308901075637477725[160] = 0.0; - out_1308901075637477725[161] = 0.0; - out_1308901075637477725[162] = 0.0; - out_1308901075637477725[163] = 0.0; - out_1308901075637477725[164] = 0.0; - out_1308901075637477725[165] = 0.0; - out_1308901075637477725[166] = 0.0; - out_1308901075637477725[167] = 0.0; - out_1308901075637477725[168] = 0.0; - out_1308901075637477725[169] = 0.0; - out_1308901075637477725[170] = 0.0; - out_1308901075637477725[171] = 1.0; - out_1308901075637477725[172] = 0.0; - out_1308901075637477725[173] = 0.0; - out_1308901075637477725[174] = 0.0; - out_1308901075637477725[175] = 0.0; - out_1308901075637477725[176] = 0.0; - out_1308901075637477725[177] = 0.0; - out_1308901075637477725[178] = 0.0; - out_1308901075637477725[179] = 0.0; - out_1308901075637477725[180] = 0.0; - out_1308901075637477725[181] = 0.0; - out_1308901075637477725[182] = 0.0; - out_1308901075637477725[183] = 0.0; - out_1308901075637477725[184] = 0.0; - out_1308901075637477725[185] = 0.0; - out_1308901075637477725[186] = 0.0; - out_1308901075637477725[187] = 0.0; - out_1308901075637477725[188] = 0.0; - out_1308901075637477725[189] = 0.0; - out_1308901075637477725[190] = 1.0; - out_1308901075637477725[191] = 0.0; - out_1308901075637477725[192] = 0.0; - out_1308901075637477725[193] = 0.0; - out_1308901075637477725[194] = 0.0; - out_1308901075637477725[195] = 0.0; - out_1308901075637477725[196] = 0.0; - out_1308901075637477725[197] = 0.0; - out_1308901075637477725[198] = 0.0; - out_1308901075637477725[199] = 0.0; - out_1308901075637477725[200] = 0.0; - out_1308901075637477725[201] = 0.0; - out_1308901075637477725[202] = 0.0; - out_1308901075637477725[203] = 0.0; - out_1308901075637477725[204] = 0.0; - out_1308901075637477725[205] = 0.0; - out_1308901075637477725[206] = 0.0; - out_1308901075637477725[207] = 0.0; - out_1308901075637477725[208] = 0.0; - out_1308901075637477725[209] = 1.0; - out_1308901075637477725[210] = 0.0; - out_1308901075637477725[211] = 0.0; - out_1308901075637477725[212] = 0.0; - out_1308901075637477725[213] = 0.0; - out_1308901075637477725[214] = 0.0; - out_1308901075637477725[215] = 0.0; - out_1308901075637477725[216] = 0.0; - out_1308901075637477725[217] = 0.0; - out_1308901075637477725[218] = 0.0; - out_1308901075637477725[219] = 0.0; - out_1308901075637477725[220] = 0.0; - out_1308901075637477725[221] = 0.0; - out_1308901075637477725[222] = 0.0; - out_1308901075637477725[223] = 0.0; - out_1308901075637477725[224] = 0.0; - out_1308901075637477725[225] = 0.0; - out_1308901075637477725[226] = 0.0; - out_1308901075637477725[227] = 0.0; - out_1308901075637477725[228] = 1.0; - out_1308901075637477725[229] = 0.0; - out_1308901075637477725[230] = 0.0; - out_1308901075637477725[231] = 0.0; - out_1308901075637477725[232] = 0.0; - out_1308901075637477725[233] = 0.0; - out_1308901075637477725[234] = 0.0; - out_1308901075637477725[235] = 0.0; - out_1308901075637477725[236] = 0.0; - out_1308901075637477725[237] = 0.0; - out_1308901075637477725[238] = 0.0; - out_1308901075637477725[239] = 0.0; - out_1308901075637477725[240] = 0.0; - out_1308901075637477725[241] = 0.0; - out_1308901075637477725[242] = 0.0; - out_1308901075637477725[243] = 0.0; - out_1308901075637477725[244] = 0.0; - out_1308901075637477725[245] = 0.0; - out_1308901075637477725[246] = 0.0; - out_1308901075637477725[247] = 1.0; - out_1308901075637477725[248] = 0.0; - out_1308901075637477725[249] = 0.0; - out_1308901075637477725[250] = 0.0; - out_1308901075637477725[251] = 0.0; - out_1308901075637477725[252] = 0.0; - out_1308901075637477725[253] = 0.0; - out_1308901075637477725[254] = 0.0; - out_1308901075637477725[255] = 0.0; - out_1308901075637477725[256] = 0.0; - out_1308901075637477725[257] = 0.0; - out_1308901075637477725[258] = 0.0; - out_1308901075637477725[259] = 0.0; - out_1308901075637477725[260] = 0.0; - out_1308901075637477725[261] = 0.0; - out_1308901075637477725[262] = 0.0; - out_1308901075637477725[263] = 0.0; - out_1308901075637477725[264] = 0.0; - out_1308901075637477725[265] = 0.0; - out_1308901075637477725[266] = 1.0; - out_1308901075637477725[267] = 0.0; - out_1308901075637477725[268] = 0.0; - out_1308901075637477725[269] = 0.0; - out_1308901075637477725[270] = 0.0; - out_1308901075637477725[271] = 0.0; - out_1308901075637477725[272] = 0.0; - out_1308901075637477725[273] = 0.0; - out_1308901075637477725[274] = 0.0; - out_1308901075637477725[275] = 0.0; - out_1308901075637477725[276] = 0.0; - out_1308901075637477725[277] = 0.0; - out_1308901075637477725[278] = 0.0; - out_1308901075637477725[279] = 0.0; - out_1308901075637477725[280] = 0.0; - out_1308901075637477725[281] = 0.0; - out_1308901075637477725[282] = 0.0; - out_1308901075637477725[283] = 0.0; - out_1308901075637477725[284] = 0.0; - out_1308901075637477725[285] = 1.0; - out_1308901075637477725[286] = 0.0; - out_1308901075637477725[287] = 0.0; - out_1308901075637477725[288] = 0.0; - out_1308901075637477725[289] = 0.0; - out_1308901075637477725[290] = 0.0; - out_1308901075637477725[291] = 0.0; - out_1308901075637477725[292] = 0.0; - out_1308901075637477725[293] = 0.0; - out_1308901075637477725[294] = 0.0; - out_1308901075637477725[295] = 0.0; - out_1308901075637477725[296] = 0.0; - out_1308901075637477725[297] = 0.0; - out_1308901075637477725[298] = 0.0; - out_1308901075637477725[299] = 0.0; - out_1308901075637477725[300] = 0.0; - out_1308901075637477725[301] = 0.0; - out_1308901075637477725[302] = 0.0; - out_1308901075637477725[303] = 0.0; - out_1308901075637477725[304] = 1.0; - out_1308901075637477725[305] = 0.0; - out_1308901075637477725[306] = 0.0; - out_1308901075637477725[307] = 0.0; - out_1308901075637477725[308] = 0.0; - out_1308901075637477725[309] = 0.0; - out_1308901075637477725[310] = 0.0; - out_1308901075637477725[311] = 0.0; - out_1308901075637477725[312] = 0.0; - out_1308901075637477725[313] = 0.0; - out_1308901075637477725[314] = 0.0; - out_1308901075637477725[315] = 0.0; - out_1308901075637477725[316] = 0.0; - out_1308901075637477725[317] = 0.0; - out_1308901075637477725[318] = 0.0; - out_1308901075637477725[319] = 0.0; - out_1308901075637477725[320] = 0.0; - out_1308901075637477725[321] = 0.0; - out_1308901075637477725[322] = 0.0; - out_1308901075637477725[323] = 1.0; +void H_mod_fun(double *state, double *out_5634677350720166788) { + out_5634677350720166788[0] = 1.0; + out_5634677350720166788[1] = 0.0; + out_5634677350720166788[2] = 0.0; + out_5634677350720166788[3] = 0.0; + out_5634677350720166788[4] = 0.0; + out_5634677350720166788[5] = 0.0; + out_5634677350720166788[6] = 0.0; + out_5634677350720166788[7] = 0.0; + out_5634677350720166788[8] = 0.0; + out_5634677350720166788[9] = 0.0; + out_5634677350720166788[10] = 0.0; + out_5634677350720166788[11] = 0.0; + out_5634677350720166788[12] = 0.0; + out_5634677350720166788[13] = 0.0; + out_5634677350720166788[14] = 0.0; + out_5634677350720166788[15] = 0.0; + out_5634677350720166788[16] = 0.0; + out_5634677350720166788[17] = 0.0; + out_5634677350720166788[18] = 0.0; + out_5634677350720166788[19] = 1.0; + out_5634677350720166788[20] = 0.0; + out_5634677350720166788[21] = 0.0; + out_5634677350720166788[22] = 0.0; + out_5634677350720166788[23] = 0.0; + out_5634677350720166788[24] = 0.0; + out_5634677350720166788[25] = 0.0; + out_5634677350720166788[26] = 0.0; + out_5634677350720166788[27] = 0.0; + out_5634677350720166788[28] = 0.0; + out_5634677350720166788[29] = 0.0; + out_5634677350720166788[30] = 0.0; + out_5634677350720166788[31] = 0.0; + out_5634677350720166788[32] = 0.0; + out_5634677350720166788[33] = 0.0; + out_5634677350720166788[34] = 0.0; + out_5634677350720166788[35] = 0.0; + out_5634677350720166788[36] = 0.0; + out_5634677350720166788[37] = 0.0; + out_5634677350720166788[38] = 1.0; + out_5634677350720166788[39] = 0.0; + out_5634677350720166788[40] = 0.0; + out_5634677350720166788[41] = 0.0; + out_5634677350720166788[42] = 0.0; + out_5634677350720166788[43] = 0.0; + out_5634677350720166788[44] = 0.0; + out_5634677350720166788[45] = 0.0; + out_5634677350720166788[46] = 0.0; + out_5634677350720166788[47] = 0.0; + out_5634677350720166788[48] = 0.0; + out_5634677350720166788[49] = 0.0; + out_5634677350720166788[50] = 0.0; + out_5634677350720166788[51] = 0.0; + out_5634677350720166788[52] = 0.0; + out_5634677350720166788[53] = 0.0; + out_5634677350720166788[54] = 0.0; + out_5634677350720166788[55] = 0.0; + out_5634677350720166788[56] = 0.0; + out_5634677350720166788[57] = 1.0; + out_5634677350720166788[58] = 0.0; + out_5634677350720166788[59] = 0.0; + out_5634677350720166788[60] = 0.0; + out_5634677350720166788[61] = 0.0; + out_5634677350720166788[62] = 0.0; + out_5634677350720166788[63] = 0.0; + out_5634677350720166788[64] = 0.0; + out_5634677350720166788[65] = 0.0; + out_5634677350720166788[66] = 0.0; + out_5634677350720166788[67] = 0.0; + out_5634677350720166788[68] = 0.0; + out_5634677350720166788[69] = 0.0; + out_5634677350720166788[70] = 0.0; + out_5634677350720166788[71] = 0.0; + out_5634677350720166788[72] = 0.0; + out_5634677350720166788[73] = 0.0; + out_5634677350720166788[74] = 0.0; + out_5634677350720166788[75] = 0.0; + out_5634677350720166788[76] = 1.0; + out_5634677350720166788[77] = 0.0; + out_5634677350720166788[78] = 0.0; + out_5634677350720166788[79] = 0.0; + out_5634677350720166788[80] = 0.0; + out_5634677350720166788[81] = 0.0; + out_5634677350720166788[82] = 0.0; + out_5634677350720166788[83] = 0.0; + out_5634677350720166788[84] = 0.0; + out_5634677350720166788[85] = 0.0; + out_5634677350720166788[86] = 0.0; + out_5634677350720166788[87] = 0.0; + out_5634677350720166788[88] = 0.0; + out_5634677350720166788[89] = 0.0; + out_5634677350720166788[90] = 0.0; + out_5634677350720166788[91] = 0.0; + out_5634677350720166788[92] = 0.0; + out_5634677350720166788[93] = 0.0; + out_5634677350720166788[94] = 0.0; + out_5634677350720166788[95] = 1.0; + out_5634677350720166788[96] = 0.0; + out_5634677350720166788[97] = 0.0; + out_5634677350720166788[98] = 0.0; + out_5634677350720166788[99] = 0.0; + out_5634677350720166788[100] = 0.0; + out_5634677350720166788[101] = 0.0; + out_5634677350720166788[102] = 0.0; + out_5634677350720166788[103] = 0.0; + out_5634677350720166788[104] = 0.0; + out_5634677350720166788[105] = 0.0; + out_5634677350720166788[106] = 0.0; + out_5634677350720166788[107] = 0.0; + out_5634677350720166788[108] = 0.0; + out_5634677350720166788[109] = 0.0; + out_5634677350720166788[110] = 0.0; + out_5634677350720166788[111] = 0.0; + out_5634677350720166788[112] = 0.0; + out_5634677350720166788[113] = 0.0; + out_5634677350720166788[114] = 1.0; + out_5634677350720166788[115] = 0.0; + out_5634677350720166788[116] = 0.0; + out_5634677350720166788[117] = 0.0; + out_5634677350720166788[118] = 0.0; + out_5634677350720166788[119] = 0.0; + out_5634677350720166788[120] = 0.0; + out_5634677350720166788[121] = 0.0; + out_5634677350720166788[122] = 0.0; + out_5634677350720166788[123] = 0.0; + out_5634677350720166788[124] = 0.0; + out_5634677350720166788[125] = 0.0; + out_5634677350720166788[126] = 0.0; + out_5634677350720166788[127] = 0.0; + out_5634677350720166788[128] = 0.0; + out_5634677350720166788[129] = 0.0; + out_5634677350720166788[130] = 0.0; + out_5634677350720166788[131] = 0.0; + out_5634677350720166788[132] = 0.0; + out_5634677350720166788[133] = 1.0; + out_5634677350720166788[134] = 0.0; + out_5634677350720166788[135] = 0.0; + out_5634677350720166788[136] = 0.0; + out_5634677350720166788[137] = 0.0; + out_5634677350720166788[138] = 0.0; + out_5634677350720166788[139] = 0.0; + out_5634677350720166788[140] = 0.0; + out_5634677350720166788[141] = 0.0; + out_5634677350720166788[142] = 0.0; + out_5634677350720166788[143] = 0.0; + out_5634677350720166788[144] = 0.0; + out_5634677350720166788[145] = 0.0; + out_5634677350720166788[146] = 0.0; + out_5634677350720166788[147] = 0.0; + out_5634677350720166788[148] = 0.0; + out_5634677350720166788[149] = 0.0; + out_5634677350720166788[150] = 0.0; + out_5634677350720166788[151] = 0.0; + out_5634677350720166788[152] = 1.0; + out_5634677350720166788[153] = 0.0; + out_5634677350720166788[154] = 0.0; + out_5634677350720166788[155] = 0.0; + out_5634677350720166788[156] = 0.0; + out_5634677350720166788[157] = 0.0; + out_5634677350720166788[158] = 0.0; + out_5634677350720166788[159] = 0.0; + out_5634677350720166788[160] = 0.0; + out_5634677350720166788[161] = 0.0; + out_5634677350720166788[162] = 0.0; + out_5634677350720166788[163] = 0.0; + out_5634677350720166788[164] = 0.0; + out_5634677350720166788[165] = 0.0; + out_5634677350720166788[166] = 0.0; + out_5634677350720166788[167] = 0.0; + out_5634677350720166788[168] = 0.0; + out_5634677350720166788[169] = 0.0; + out_5634677350720166788[170] = 0.0; + out_5634677350720166788[171] = 1.0; + out_5634677350720166788[172] = 0.0; + out_5634677350720166788[173] = 0.0; + out_5634677350720166788[174] = 0.0; + out_5634677350720166788[175] = 0.0; + out_5634677350720166788[176] = 0.0; + out_5634677350720166788[177] = 0.0; + out_5634677350720166788[178] = 0.0; + out_5634677350720166788[179] = 0.0; + out_5634677350720166788[180] = 0.0; + out_5634677350720166788[181] = 0.0; + out_5634677350720166788[182] = 0.0; + out_5634677350720166788[183] = 0.0; + out_5634677350720166788[184] = 0.0; + out_5634677350720166788[185] = 0.0; + out_5634677350720166788[186] = 0.0; + out_5634677350720166788[187] = 0.0; + out_5634677350720166788[188] = 0.0; + out_5634677350720166788[189] = 0.0; + out_5634677350720166788[190] = 1.0; + out_5634677350720166788[191] = 0.0; + out_5634677350720166788[192] = 0.0; + out_5634677350720166788[193] = 0.0; + out_5634677350720166788[194] = 0.0; + out_5634677350720166788[195] = 0.0; + out_5634677350720166788[196] = 0.0; + out_5634677350720166788[197] = 0.0; + out_5634677350720166788[198] = 0.0; + out_5634677350720166788[199] = 0.0; + out_5634677350720166788[200] = 0.0; + out_5634677350720166788[201] = 0.0; + out_5634677350720166788[202] = 0.0; + out_5634677350720166788[203] = 0.0; + out_5634677350720166788[204] = 0.0; + out_5634677350720166788[205] = 0.0; + out_5634677350720166788[206] = 0.0; + out_5634677350720166788[207] = 0.0; + out_5634677350720166788[208] = 0.0; + out_5634677350720166788[209] = 1.0; + out_5634677350720166788[210] = 0.0; + out_5634677350720166788[211] = 0.0; + out_5634677350720166788[212] = 0.0; + out_5634677350720166788[213] = 0.0; + out_5634677350720166788[214] = 0.0; + out_5634677350720166788[215] = 0.0; + out_5634677350720166788[216] = 0.0; + out_5634677350720166788[217] = 0.0; + out_5634677350720166788[218] = 0.0; + out_5634677350720166788[219] = 0.0; + out_5634677350720166788[220] = 0.0; + out_5634677350720166788[221] = 0.0; + out_5634677350720166788[222] = 0.0; + out_5634677350720166788[223] = 0.0; + out_5634677350720166788[224] = 0.0; + out_5634677350720166788[225] = 0.0; + out_5634677350720166788[226] = 0.0; + out_5634677350720166788[227] = 0.0; + out_5634677350720166788[228] = 1.0; + out_5634677350720166788[229] = 0.0; + out_5634677350720166788[230] = 0.0; + out_5634677350720166788[231] = 0.0; + out_5634677350720166788[232] = 0.0; + out_5634677350720166788[233] = 0.0; + out_5634677350720166788[234] = 0.0; + out_5634677350720166788[235] = 0.0; + out_5634677350720166788[236] = 0.0; + out_5634677350720166788[237] = 0.0; + out_5634677350720166788[238] = 0.0; + out_5634677350720166788[239] = 0.0; + out_5634677350720166788[240] = 0.0; + out_5634677350720166788[241] = 0.0; + out_5634677350720166788[242] = 0.0; + out_5634677350720166788[243] = 0.0; + out_5634677350720166788[244] = 0.0; + out_5634677350720166788[245] = 0.0; + out_5634677350720166788[246] = 0.0; + out_5634677350720166788[247] = 1.0; + out_5634677350720166788[248] = 0.0; + out_5634677350720166788[249] = 0.0; + out_5634677350720166788[250] = 0.0; + out_5634677350720166788[251] = 0.0; + out_5634677350720166788[252] = 0.0; + out_5634677350720166788[253] = 0.0; + out_5634677350720166788[254] = 0.0; + out_5634677350720166788[255] = 0.0; + out_5634677350720166788[256] = 0.0; + out_5634677350720166788[257] = 0.0; + out_5634677350720166788[258] = 0.0; + out_5634677350720166788[259] = 0.0; + out_5634677350720166788[260] = 0.0; + out_5634677350720166788[261] = 0.0; + out_5634677350720166788[262] = 0.0; + out_5634677350720166788[263] = 0.0; + out_5634677350720166788[264] = 0.0; + out_5634677350720166788[265] = 0.0; + out_5634677350720166788[266] = 1.0; + out_5634677350720166788[267] = 0.0; + out_5634677350720166788[268] = 0.0; + out_5634677350720166788[269] = 0.0; + out_5634677350720166788[270] = 0.0; + out_5634677350720166788[271] = 0.0; + out_5634677350720166788[272] = 0.0; + out_5634677350720166788[273] = 0.0; + out_5634677350720166788[274] = 0.0; + out_5634677350720166788[275] = 0.0; + out_5634677350720166788[276] = 0.0; + out_5634677350720166788[277] = 0.0; + out_5634677350720166788[278] = 0.0; + out_5634677350720166788[279] = 0.0; + out_5634677350720166788[280] = 0.0; + out_5634677350720166788[281] = 0.0; + out_5634677350720166788[282] = 0.0; + out_5634677350720166788[283] = 0.0; + out_5634677350720166788[284] = 0.0; + out_5634677350720166788[285] = 1.0; + out_5634677350720166788[286] = 0.0; + out_5634677350720166788[287] = 0.0; + out_5634677350720166788[288] = 0.0; + out_5634677350720166788[289] = 0.0; + out_5634677350720166788[290] = 0.0; + out_5634677350720166788[291] = 0.0; + out_5634677350720166788[292] = 0.0; + out_5634677350720166788[293] = 0.0; + out_5634677350720166788[294] = 0.0; + out_5634677350720166788[295] = 0.0; + out_5634677350720166788[296] = 0.0; + out_5634677350720166788[297] = 0.0; + out_5634677350720166788[298] = 0.0; + out_5634677350720166788[299] = 0.0; + out_5634677350720166788[300] = 0.0; + out_5634677350720166788[301] = 0.0; + out_5634677350720166788[302] = 0.0; + out_5634677350720166788[303] = 0.0; + out_5634677350720166788[304] = 1.0; + out_5634677350720166788[305] = 0.0; + out_5634677350720166788[306] = 0.0; + out_5634677350720166788[307] = 0.0; + out_5634677350720166788[308] = 0.0; + out_5634677350720166788[309] = 0.0; + out_5634677350720166788[310] = 0.0; + out_5634677350720166788[311] = 0.0; + out_5634677350720166788[312] = 0.0; + out_5634677350720166788[313] = 0.0; + out_5634677350720166788[314] = 0.0; + out_5634677350720166788[315] = 0.0; + out_5634677350720166788[316] = 0.0; + out_5634677350720166788[317] = 0.0; + out_5634677350720166788[318] = 0.0; + out_5634677350720166788[319] = 0.0; + out_5634677350720166788[320] = 0.0; + out_5634677350720166788[321] = 0.0; + out_5634677350720166788[322] = 0.0; + out_5634677350720166788[323] = 1.0; } -void f_fun(double *state, double dt, double *out_8416334481406024670) { - out_8416334481406024670[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_8416334481406024670[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_8416334481406024670[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_8416334481406024670[3] = dt*state[12] + state[3]; - out_8416334481406024670[4] = dt*state[13] + state[4]; - out_8416334481406024670[5] = dt*state[14] + state[5]; - out_8416334481406024670[6] = state[6]; - out_8416334481406024670[7] = state[7]; - out_8416334481406024670[8] = state[8]; - out_8416334481406024670[9] = state[9]; - out_8416334481406024670[10] = state[10]; - out_8416334481406024670[11] = state[11]; - out_8416334481406024670[12] = state[12]; - out_8416334481406024670[13] = state[13]; - out_8416334481406024670[14] = state[14]; - out_8416334481406024670[15] = state[15]; - out_8416334481406024670[16] = state[16]; - out_8416334481406024670[17] = state[17]; +void f_fun(double *state, double dt, double *out_5401222257802876036) { + out_5401222257802876036[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_5401222257802876036[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_5401222257802876036[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_5401222257802876036[3] = dt*state[12] + state[3]; + out_5401222257802876036[4] = dt*state[13] + state[4]; + out_5401222257802876036[5] = dt*state[14] + state[5]; + out_5401222257802876036[6] = state[6]; + out_5401222257802876036[7] = state[7]; + out_5401222257802876036[8] = state[8]; + out_5401222257802876036[9] = state[9]; + out_5401222257802876036[10] = state[10]; + out_5401222257802876036[11] = state[11]; + out_5401222257802876036[12] = state[12]; + out_5401222257802876036[13] = state[13]; + out_5401222257802876036[14] = state[14]; + out_5401222257802876036[15] = state[15]; + out_5401222257802876036[16] = state[16]; + out_5401222257802876036[17] = state[17]; } -void F_fun(double *state, double dt, double *out_5288884962144890875) { - out_5288884962144890875[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_5288884962144890875[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_5288884962144890875[2] = 0; - out_5288884962144890875[3] = 0; - out_5288884962144890875[4] = 0; - out_5288884962144890875[5] = 0; - out_5288884962144890875[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_5288884962144890875[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_5288884962144890875[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_5288884962144890875[9] = 0; - out_5288884962144890875[10] = 0; - out_5288884962144890875[11] = 0; - out_5288884962144890875[12] = 0; - out_5288884962144890875[13] = 0; - out_5288884962144890875[14] = 0; - out_5288884962144890875[15] = 0; - out_5288884962144890875[16] = 0; - out_5288884962144890875[17] = 0; - out_5288884962144890875[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_5288884962144890875[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_5288884962144890875[20] = 0; - out_5288884962144890875[21] = 0; - out_5288884962144890875[22] = 0; - out_5288884962144890875[23] = 0; - out_5288884962144890875[24] = 0; - out_5288884962144890875[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_5288884962144890875[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_5288884962144890875[27] = 0; - out_5288884962144890875[28] = 0; - out_5288884962144890875[29] = 0; - out_5288884962144890875[30] = 0; - out_5288884962144890875[31] = 0; - out_5288884962144890875[32] = 0; - out_5288884962144890875[33] = 0; - out_5288884962144890875[34] = 0; - out_5288884962144890875[35] = 0; - out_5288884962144890875[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_5288884962144890875[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_5288884962144890875[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_5288884962144890875[39] = 0; - out_5288884962144890875[40] = 0; - out_5288884962144890875[41] = 0; - out_5288884962144890875[42] = 0; - out_5288884962144890875[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_5288884962144890875[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_5288884962144890875[45] = 0; - out_5288884962144890875[46] = 0; - out_5288884962144890875[47] = 0; - out_5288884962144890875[48] = 0; - out_5288884962144890875[49] = 0; - out_5288884962144890875[50] = 0; - out_5288884962144890875[51] = 0; - out_5288884962144890875[52] = 0; - out_5288884962144890875[53] = 0; - out_5288884962144890875[54] = 0; - out_5288884962144890875[55] = 0; - out_5288884962144890875[56] = 0; - out_5288884962144890875[57] = 1; - out_5288884962144890875[58] = 0; - out_5288884962144890875[59] = 0; - out_5288884962144890875[60] = 0; - out_5288884962144890875[61] = 0; - out_5288884962144890875[62] = 0; - out_5288884962144890875[63] = 0; - out_5288884962144890875[64] = 0; - out_5288884962144890875[65] = 0; - out_5288884962144890875[66] = dt; - out_5288884962144890875[67] = 0; - out_5288884962144890875[68] = 0; - out_5288884962144890875[69] = 0; - out_5288884962144890875[70] = 0; - out_5288884962144890875[71] = 0; - out_5288884962144890875[72] = 0; - out_5288884962144890875[73] = 0; - out_5288884962144890875[74] = 0; - out_5288884962144890875[75] = 0; - out_5288884962144890875[76] = 1; - out_5288884962144890875[77] = 0; - out_5288884962144890875[78] = 0; - out_5288884962144890875[79] = 0; - out_5288884962144890875[80] = 0; - out_5288884962144890875[81] = 0; - out_5288884962144890875[82] = 0; - out_5288884962144890875[83] = 0; - out_5288884962144890875[84] = 0; - out_5288884962144890875[85] = dt; - out_5288884962144890875[86] = 0; - out_5288884962144890875[87] = 0; - out_5288884962144890875[88] = 0; - out_5288884962144890875[89] = 0; - out_5288884962144890875[90] = 0; - out_5288884962144890875[91] = 0; - out_5288884962144890875[92] = 0; - out_5288884962144890875[93] = 0; - out_5288884962144890875[94] = 0; - out_5288884962144890875[95] = 1; - out_5288884962144890875[96] = 0; - out_5288884962144890875[97] = 0; - out_5288884962144890875[98] = 0; - out_5288884962144890875[99] = 0; - out_5288884962144890875[100] = 0; - out_5288884962144890875[101] = 0; - out_5288884962144890875[102] = 0; - out_5288884962144890875[103] = 0; - out_5288884962144890875[104] = dt; - out_5288884962144890875[105] = 0; - out_5288884962144890875[106] = 0; - out_5288884962144890875[107] = 0; - out_5288884962144890875[108] = 0; - out_5288884962144890875[109] = 0; - out_5288884962144890875[110] = 0; - out_5288884962144890875[111] = 0; - out_5288884962144890875[112] = 0; - out_5288884962144890875[113] = 0; - out_5288884962144890875[114] = 1; - out_5288884962144890875[115] = 0; - out_5288884962144890875[116] = 0; - out_5288884962144890875[117] = 0; - out_5288884962144890875[118] = 0; - out_5288884962144890875[119] = 0; - out_5288884962144890875[120] = 0; - out_5288884962144890875[121] = 0; - out_5288884962144890875[122] = 0; - out_5288884962144890875[123] = 0; - out_5288884962144890875[124] = 0; - out_5288884962144890875[125] = 0; - out_5288884962144890875[126] = 0; - out_5288884962144890875[127] = 0; - out_5288884962144890875[128] = 0; - out_5288884962144890875[129] = 0; - out_5288884962144890875[130] = 0; - out_5288884962144890875[131] = 0; - out_5288884962144890875[132] = 0; - out_5288884962144890875[133] = 1; - out_5288884962144890875[134] = 0; - out_5288884962144890875[135] = 0; - out_5288884962144890875[136] = 0; - out_5288884962144890875[137] = 0; - out_5288884962144890875[138] = 0; - out_5288884962144890875[139] = 0; - out_5288884962144890875[140] = 0; - out_5288884962144890875[141] = 0; - out_5288884962144890875[142] = 0; - out_5288884962144890875[143] = 0; - out_5288884962144890875[144] = 0; - out_5288884962144890875[145] = 0; - out_5288884962144890875[146] = 0; - out_5288884962144890875[147] = 0; - out_5288884962144890875[148] = 0; - out_5288884962144890875[149] = 0; - out_5288884962144890875[150] = 0; - out_5288884962144890875[151] = 0; - out_5288884962144890875[152] = 1; - out_5288884962144890875[153] = 0; - out_5288884962144890875[154] = 0; - out_5288884962144890875[155] = 0; - out_5288884962144890875[156] = 0; - out_5288884962144890875[157] = 0; - out_5288884962144890875[158] = 0; - out_5288884962144890875[159] = 0; - out_5288884962144890875[160] = 0; - out_5288884962144890875[161] = 0; - out_5288884962144890875[162] = 0; - out_5288884962144890875[163] = 0; - out_5288884962144890875[164] = 0; - out_5288884962144890875[165] = 0; - out_5288884962144890875[166] = 0; - out_5288884962144890875[167] = 0; - out_5288884962144890875[168] = 0; - out_5288884962144890875[169] = 0; - out_5288884962144890875[170] = 0; - out_5288884962144890875[171] = 1; - out_5288884962144890875[172] = 0; - out_5288884962144890875[173] = 0; - out_5288884962144890875[174] = 0; - out_5288884962144890875[175] = 0; - out_5288884962144890875[176] = 0; - out_5288884962144890875[177] = 0; - out_5288884962144890875[178] = 0; - out_5288884962144890875[179] = 0; - out_5288884962144890875[180] = 0; - out_5288884962144890875[181] = 0; - out_5288884962144890875[182] = 0; - out_5288884962144890875[183] = 0; - out_5288884962144890875[184] = 0; - out_5288884962144890875[185] = 0; - out_5288884962144890875[186] = 0; - out_5288884962144890875[187] = 0; - out_5288884962144890875[188] = 0; - out_5288884962144890875[189] = 0; - out_5288884962144890875[190] = 1; - out_5288884962144890875[191] = 0; - out_5288884962144890875[192] = 0; - out_5288884962144890875[193] = 0; - out_5288884962144890875[194] = 0; - out_5288884962144890875[195] = 0; - out_5288884962144890875[196] = 0; - out_5288884962144890875[197] = 0; - out_5288884962144890875[198] = 0; - out_5288884962144890875[199] = 0; - out_5288884962144890875[200] = 0; - out_5288884962144890875[201] = 0; - out_5288884962144890875[202] = 0; - out_5288884962144890875[203] = 0; - out_5288884962144890875[204] = 0; - out_5288884962144890875[205] = 0; - out_5288884962144890875[206] = 0; - out_5288884962144890875[207] = 0; - out_5288884962144890875[208] = 0; - out_5288884962144890875[209] = 1; - out_5288884962144890875[210] = 0; - out_5288884962144890875[211] = 0; - out_5288884962144890875[212] = 0; - out_5288884962144890875[213] = 0; - out_5288884962144890875[214] = 0; - out_5288884962144890875[215] = 0; - out_5288884962144890875[216] = 0; - out_5288884962144890875[217] = 0; - out_5288884962144890875[218] = 0; - out_5288884962144890875[219] = 0; - out_5288884962144890875[220] = 0; - out_5288884962144890875[221] = 0; - out_5288884962144890875[222] = 0; - out_5288884962144890875[223] = 0; - out_5288884962144890875[224] = 0; - out_5288884962144890875[225] = 0; - out_5288884962144890875[226] = 0; - out_5288884962144890875[227] = 0; - out_5288884962144890875[228] = 1; - out_5288884962144890875[229] = 0; - out_5288884962144890875[230] = 0; - out_5288884962144890875[231] = 0; - out_5288884962144890875[232] = 0; - out_5288884962144890875[233] = 0; - out_5288884962144890875[234] = 0; - out_5288884962144890875[235] = 0; - out_5288884962144890875[236] = 0; - out_5288884962144890875[237] = 0; - out_5288884962144890875[238] = 0; - out_5288884962144890875[239] = 0; - out_5288884962144890875[240] = 0; - out_5288884962144890875[241] = 0; - out_5288884962144890875[242] = 0; - out_5288884962144890875[243] = 0; - out_5288884962144890875[244] = 0; - out_5288884962144890875[245] = 0; - out_5288884962144890875[246] = 0; - out_5288884962144890875[247] = 1; - out_5288884962144890875[248] = 0; - out_5288884962144890875[249] = 0; - out_5288884962144890875[250] = 0; - out_5288884962144890875[251] = 0; - out_5288884962144890875[252] = 0; - out_5288884962144890875[253] = 0; - out_5288884962144890875[254] = 0; - out_5288884962144890875[255] = 0; - out_5288884962144890875[256] = 0; - out_5288884962144890875[257] = 0; - out_5288884962144890875[258] = 0; - out_5288884962144890875[259] = 0; - out_5288884962144890875[260] = 0; - out_5288884962144890875[261] = 0; - out_5288884962144890875[262] = 0; - out_5288884962144890875[263] = 0; - out_5288884962144890875[264] = 0; - out_5288884962144890875[265] = 0; - out_5288884962144890875[266] = 1; - out_5288884962144890875[267] = 0; - out_5288884962144890875[268] = 0; - out_5288884962144890875[269] = 0; - out_5288884962144890875[270] = 0; - out_5288884962144890875[271] = 0; - out_5288884962144890875[272] = 0; - out_5288884962144890875[273] = 0; - out_5288884962144890875[274] = 0; - out_5288884962144890875[275] = 0; - out_5288884962144890875[276] = 0; - out_5288884962144890875[277] = 0; - out_5288884962144890875[278] = 0; - out_5288884962144890875[279] = 0; - out_5288884962144890875[280] = 0; - out_5288884962144890875[281] = 0; - out_5288884962144890875[282] = 0; - out_5288884962144890875[283] = 0; - out_5288884962144890875[284] = 0; - out_5288884962144890875[285] = 1; - out_5288884962144890875[286] = 0; - out_5288884962144890875[287] = 0; - out_5288884962144890875[288] = 0; - out_5288884962144890875[289] = 0; - out_5288884962144890875[290] = 0; - out_5288884962144890875[291] = 0; - out_5288884962144890875[292] = 0; - out_5288884962144890875[293] = 0; - out_5288884962144890875[294] = 0; - out_5288884962144890875[295] = 0; - out_5288884962144890875[296] = 0; - out_5288884962144890875[297] = 0; - out_5288884962144890875[298] = 0; - out_5288884962144890875[299] = 0; - out_5288884962144890875[300] = 0; - out_5288884962144890875[301] = 0; - out_5288884962144890875[302] = 0; - out_5288884962144890875[303] = 0; - out_5288884962144890875[304] = 1; - out_5288884962144890875[305] = 0; - out_5288884962144890875[306] = 0; - out_5288884962144890875[307] = 0; - out_5288884962144890875[308] = 0; - out_5288884962144890875[309] = 0; - out_5288884962144890875[310] = 0; - out_5288884962144890875[311] = 0; - out_5288884962144890875[312] = 0; - out_5288884962144890875[313] = 0; - out_5288884962144890875[314] = 0; - out_5288884962144890875[315] = 0; - out_5288884962144890875[316] = 0; - out_5288884962144890875[317] = 0; - out_5288884962144890875[318] = 0; - out_5288884962144890875[319] = 0; - out_5288884962144890875[320] = 0; - out_5288884962144890875[321] = 0; - out_5288884962144890875[322] = 0; - out_5288884962144890875[323] = 1; +void F_fun(double *state, double dt, double *out_1313183186567530475) { + out_1313183186567530475[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_1313183186567530475[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_1313183186567530475[2] = 0; + out_1313183186567530475[3] = 0; + out_1313183186567530475[4] = 0; + out_1313183186567530475[5] = 0; + out_1313183186567530475[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_1313183186567530475[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_1313183186567530475[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_1313183186567530475[9] = 0; + out_1313183186567530475[10] = 0; + out_1313183186567530475[11] = 0; + out_1313183186567530475[12] = 0; + out_1313183186567530475[13] = 0; + out_1313183186567530475[14] = 0; + out_1313183186567530475[15] = 0; + out_1313183186567530475[16] = 0; + out_1313183186567530475[17] = 0; + out_1313183186567530475[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_1313183186567530475[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_1313183186567530475[20] = 0; + out_1313183186567530475[21] = 0; + out_1313183186567530475[22] = 0; + out_1313183186567530475[23] = 0; + out_1313183186567530475[24] = 0; + out_1313183186567530475[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_1313183186567530475[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_1313183186567530475[27] = 0; + out_1313183186567530475[28] = 0; + out_1313183186567530475[29] = 0; + out_1313183186567530475[30] = 0; + out_1313183186567530475[31] = 0; + out_1313183186567530475[32] = 0; + out_1313183186567530475[33] = 0; + out_1313183186567530475[34] = 0; + out_1313183186567530475[35] = 0; + out_1313183186567530475[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_1313183186567530475[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_1313183186567530475[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_1313183186567530475[39] = 0; + out_1313183186567530475[40] = 0; + out_1313183186567530475[41] = 0; + out_1313183186567530475[42] = 0; + out_1313183186567530475[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_1313183186567530475[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_1313183186567530475[45] = 0; + out_1313183186567530475[46] = 0; + out_1313183186567530475[47] = 0; + out_1313183186567530475[48] = 0; + out_1313183186567530475[49] = 0; + out_1313183186567530475[50] = 0; + out_1313183186567530475[51] = 0; + out_1313183186567530475[52] = 0; + out_1313183186567530475[53] = 0; + out_1313183186567530475[54] = 0; + out_1313183186567530475[55] = 0; + out_1313183186567530475[56] = 0; + out_1313183186567530475[57] = 1; + out_1313183186567530475[58] = 0; + out_1313183186567530475[59] = 0; + out_1313183186567530475[60] = 0; + out_1313183186567530475[61] = 0; + out_1313183186567530475[62] = 0; + out_1313183186567530475[63] = 0; + out_1313183186567530475[64] = 0; + out_1313183186567530475[65] = 0; + out_1313183186567530475[66] = dt; + out_1313183186567530475[67] = 0; + out_1313183186567530475[68] = 0; + out_1313183186567530475[69] = 0; + out_1313183186567530475[70] = 0; + out_1313183186567530475[71] = 0; + out_1313183186567530475[72] = 0; + out_1313183186567530475[73] = 0; + out_1313183186567530475[74] = 0; + out_1313183186567530475[75] = 0; + out_1313183186567530475[76] = 1; + out_1313183186567530475[77] = 0; + out_1313183186567530475[78] = 0; + out_1313183186567530475[79] = 0; + out_1313183186567530475[80] = 0; + out_1313183186567530475[81] = 0; + out_1313183186567530475[82] = 0; + out_1313183186567530475[83] = 0; + out_1313183186567530475[84] = 0; + out_1313183186567530475[85] = dt; + out_1313183186567530475[86] = 0; + out_1313183186567530475[87] = 0; + out_1313183186567530475[88] = 0; + out_1313183186567530475[89] = 0; + out_1313183186567530475[90] = 0; + out_1313183186567530475[91] = 0; + out_1313183186567530475[92] = 0; + out_1313183186567530475[93] = 0; + out_1313183186567530475[94] = 0; + out_1313183186567530475[95] = 1; + out_1313183186567530475[96] = 0; + out_1313183186567530475[97] = 0; + out_1313183186567530475[98] = 0; + out_1313183186567530475[99] = 0; + out_1313183186567530475[100] = 0; + out_1313183186567530475[101] = 0; + out_1313183186567530475[102] = 0; + out_1313183186567530475[103] = 0; + out_1313183186567530475[104] = dt; + out_1313183186567530475[105] = 0; + out_1313183186567530475[106] = 0; + out_1313183186567530475[107] = 0; + out_1313183186567530475[108] = 0; + out_1313183186567530475[109] = 0; + out_1313183186567530475[110] = 0; + out_1313183186567530475[111] = 0; + out_1313183186567530475[112] = 0; + out_1313183186567530475[113] = 0; + out_1313183186567530475[114] = 1; + out_1313183186567530475[115] = 0; + out_1313183186567530475[116] = 0; + out_1313183186567530475[117] = 0; + out_1313183186567530475[118] = 0; + out_1313183186567530475[119] = 0; + out_1313183186567530475[120] = 0; + out_1313183186567530475[121] = 0; + out_1313183186567530475[122] = 0; + out_1313183186567530475[123] = 0; + out_1313183186567530475[124] = 0; + out_1313183186567530475[125] = 0; + out_1313183186567530475[126] = 0; + out_1313183186567530475[127] = 0; + out_1313183186567530475[128] = 0; + out_1313183186567530475[129] = 0; + out_1313183186567530475[130] = 0; + out_1313183186567530475[131] = 0; + out_1313183186567530475[132] = 0; + out_1313183186567530475[133] = 1; + out_1313183186567530475[134] = 0; + out_1313183186567530475[135] = 0; + out_1313183186567530475[136] = 0; + out_1313183186567530475[137] = 0; + out_1313183186567530475[138] = 0; + out_1313183186567530475[139] = 0; + out_1313183186567530475[140] = 0; + out_1313183186567530475[141] = 0; + out_1313183186567530475[142] = 0; + out_1313183186567530475[143] = 0; + out_1313183186567530475[144] = 0; + out_1313183186567530475[145] = 0; + out_1313183186567530475[146] = 0; + out_1313183186567530475[147] = 0; + out_1313183186567530475[148] = 0; + out_1313183186567530475[149] = 0; + out_1313183186567530475[150] = 0; + out_1313183186567530475[151] = 0; + out_1313183186567530475[152] = 1; + out_1313183186567530475[153] = 0; + out_1313183186567530475[154] = 0; + out_1313183186567530475[155] = 0; + out_1313183186567530475[156] = 0; + out_1313183186567530475[157] = 0; + out_1313183186567530475[158] = 0; + out_1313183186567530475[159] = 0; + out_1313183186567530475[160] = 0; + out_1313183186567530475[161] = 0; + out_1313183186567530475[162] = 0; + out_1313183186567530475[163] = 0; + out_1313183186567530475[164] = 0; + out_1313183186567530475[165] = 0; + out_1313183186567530475[166] = 0; + out_1313183186567530475[167] = 0; + out_1313183186567530475[168] = 0; + out_1313183186567530475[169] = 0; + out_1313183186567530475[170] = 0; + out_1313183186567530475[171] = 1; + out_1313183186567530475[172] = 0; + out_1313183186567530475[173] = 0; + out_1313183186567530475[174] = 0; + out_1313183186567530475[175] = 0; + out_1313183186567530475[176] = 0; + out_1313183186567530475[177] = 0; + out_1313183186567530475[178] = 0; + out_1313183186567530475[179] = 0; + out_1313183186567530475[180] = 0; + out_1313183186567530475[181] = 0; + out_1313183186567530475[182] = 0; + out_1313183186567530475[183] = 0; + out_1313183186567530475[184] = 0; + out_1313183186567530475[185] = 0; + out_1313183186567530475[186] = 0; + out_1313183186567530475[187] = 0; + out_1313183186567530475[188] = 0; + out_1313183186567530475[189] = 0; + out_1313183186567530475[190] = 1; + out_1313183186567530475[191] = 0; + out_1313183186567530475[192] = 0; + out_1313183186567530475[193] = 0; + out_1313183186567530475[194] = 0; + out_1313183186567530475[195] = 0; + out_1313183186567530475[196] = 0; + out_1313183186567530475[197] = 0; + out_1313183186567530475[198] = 0; + out_1313183186567530475[199] = 0; + out_1313183186567530475[200] = 0; + out_1313183186567530475[201] = 0; + out_1313183186567530475[202] = 0; + out_1313183186567530475[203] = 0; + out_1313183186567530475[204] = 0; + out_1313183186567530475[205] = 0; + out_1313183186567530475[206] = 0; + out_1313183186567530475[207] = 0; + out_1313183186567530475[208] = 0; + out_1313183186567530475[209] = 1; + out_1313183186567530475[210] = 0; + out_1313183186567530475[211] = 0; + out_1313183186567530475[212] = 0; + out_1313183186567530475[213] = 0; + out_1313183186567530475[214] = 0; + out_1313183186567530475[215] = 0; + out_1313183186567530475[216] = 0; + out_1313183186567530475[217] = 0; + out_1313183186567530475[218] = 0; + out_1313183186567530475[219] = 0; + out_1313183186567530475[220] = 0; + out_1313183186567530475[221] = 0; + out_1313183186567530475[222] = 0; + out_1313183186567530475[223] = 0; + out_1313183186567530475[224] = 0; + out_1313183186567530475[225] = 0; + out_1313183186567530475[226] = 0; + out_1313183186567530475[227] = 0; + out_1313183186567530475[228] = 1; + out_1313183186567530475[229] = 0; + out_1313183186567530475[230] = 0; + out_1313183186567530475[231] = 0; + out_1313183186567530475[232] = 0; + out_1313183186567530475[233] = 0; + out_1313183186567530475[234] = 0; + out_1313183186567530475[235] = 0; + out_1313183186567530475[236] = 0; + out_1313183186567530475[237] = 0; + out_1313183186567530475[238] = 0; + out_1313183186567530475[239] = 0; + out_1313183186567530475[240] = 0; + out_1313183186567530475[241] = 0; + out_1313183186567530475[242] = 0; + out_1313183186567530475[243] = 0; + out_1313183186567530475[244] = 0; + out_1313183186567530475[245] = 0; + out_1313183186567530475[246] = 0; + out_1313183186567530475[247] = 1; + out_1313183186567530475[248] = 0; + out_1313183186567530475[249] = 0; + out_1313183186567530475[250] = 0; + out_1313183186567530475[251] = 0; + out_1313183186567530475[252] = 0; + out_1313183186567530475[253] = 0; + out_1313183186567530475[254] = 0; + out_1313183186567530475[255] = 0; + out_1313183186567530475[256] = 0; + out_1313183186567530475[257] = 0; + out_1313183186567530475[258] = 0; + out_1313183186567530475[259] = 0; + out_1313183186567530475[260] = 0; + out_1313183186567530475[261] = 0; + out_1313183186567530475[262] = 0; + out_1313183186567530475[263] = 0; + out_1313183186567530475[264] = 0; + out_1313183186567530475[265] = 0; + out_1313183186567530475[266] = 1; + out_1313183186567530475[267] = 0; + out_1313183186567530475[268] = 0; + out_1313183186567530475[269] = 0; + out_1313183186567530475[270] = 0; + out_1313183186567530475[271] = 0; + out_1313183186567530475[272] = 0; + out_1313183186567530475[273] = 0; + out_1313183186567530475[274] = 0; + out_1313183186567530475[275] = 0; + out_1313183186567530475[276] = 0; + out_1313183186567530475[277] = 0; + out_1313183186567530475[278] = 0; + out_1313183186567530475[279] = 0; + out_1313183186567530475[280] = 0; + out_1313183186567530475[281] = 0; + out_1313183186567530475[282] = 0; + out_1313183186567530475[283] = 0; + out_1313183186567530475[284] = 0; + out_1313183186567530475[285] = 1; + out_1313183186567530475[286] = 0; + out_1313183186567530475[287] = 0; + out_1313183186567530475[288] = 0; + out_1313183186567530475[289] = 0; + out_1313183186567530475[290] = 0; + out_1313183186567530475[291] = 0; + out_1313183186567530475[292] = 0; + out_1313183186567530475[293] = 0; + out_1313183186567530475[294] = 0; + out_1313183186567530475[295] = 0; + out_1313183186567530475[296] = 0; + out_1313183186567530475[297] = 0; + out_1313183186567530475[298] = 0; + out_1313183186567530475[299] = 0; + out_1313183186567530475[300] = 0; + out_1313183186567530475[301] = 0; + out_1313183186567530475[302] = 0; + out_1313183186567530475[303] = 0; + out_1313183186567530475[304] = 1; + out_1313183186567530475[305] = 0; + out_1313183186567530475[306] = 0; + out_1313183186567530475[307] = 0; + out_1313183186567530475[308] = 0; + out_1313183186567530475[309] = 0; + out_1313183186567530475[310] = 0; + out_1313183186567530475[311] = 0; + out_1313183186567530475[312] = 0; + out_1313183186567530475[313] = 0; + out_1313183186567530475[314] = 0; + out_1313183186567530475[315] = 0; + out_1313183186567530475[316] = 0; + out_1313183186567530475[317] = 0; + out_1313183186567530475[318] = 0; + out_1313183186567530475[319] = 0; + out_1313183186567530475[320] = 0; + out_1313183186567530475[321] = 0; + out_1313183186567530475[322] = 0; + out_1313183186567530475[323] = 1; } -void h_4(double *state, double *unused, double *out_90872324407998874) { - out_90872324407998874[0] = state[6] + state[9]; - out_90872324407998874[1] = state[7] + state[10]; - out_90872324407998874[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_717943751669182438) { + out_717943751669182438[0] = state[6] + state[9]; + out_717943751669182438[1] = state[7] + state[10]; + out_717943751669182438[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_7635460590420343617) { - out_7635460590420343617[0] = 0; - out_7635460590420343617[1] = 0; - out_7635460590420343617[2] = 0; - out_7635460590420343617[3] = 0; - out_7635460590420343617[4] = 0; - out_7635460590420343617[5] = 0; - out_7635460590420343617[6] = 1; - out_7635460590420343617[7] = 0; - out_7635460590420343617[8] = 0; - out_7635460590420343617[9] = 1; - out_7635460590420343617[10] = 0; - out_7635460590420343617[11] = 0; - out_7635460590420343617[12] = 0; - out_7635460590420343617[13] = 0; - out_7635460590420343617[14] = 0; - out_7635460590420343617[15] = 0; - out_7635460590420343617[16] = 0; - out_7635460590420343617[17] = 0; - out_7635460590420343617[18] = 0; - out_7635460590420343617[19] = 0; - out_7635460590420343617[20] = 0; - out_7635460590420343617[21] = 0; - out_7635460590420343617[22] = 0; - out_7635460590420343617[23] = 0; - out_7635460590420343617[24] = 0; - out_7635460590420343617[25] = 1; - out_7635460590420343617[26] = 0; - out_7635460590420343617[27] = 0; - out_7635460590420343617[28] = 1; - out_7635460590420343617[29] = 0; - out_7635460590420343617[30] = 0; - out_7635460590420343617[31] = 0; - out_7635460590420343617[32] = 0; - out_7635460590420343617[33] = 0; - out_7635460590420343617[34] = 0; - out_7635460590420343617[35] = 0; - out_7635460590420343617[36] = 0; - out_7635460590420343617[37] = 0; - out_7635460590420343617[38] = 0; - out_7635460590420343617[39] = 0; - out_7635460590420343617[40] = 0; - out_7635460590420343617[41] = 0; - out_7635460590420343617[42] = 0; - out_7635460590420343617[43] = 0; - out_7635460590420343617[44] = 1; - out_7635460590420343617[45] = 0; - out_7635460590420343617[46] = 0; - out_7635460590420343617[47] = 1; - out_7635460590420343617[48] = 0; - out_7635460590420343617[49] = 0; - out_7635460590420343617[50] = 0; - out_7635460590420343617[51] = 0; - out_7635460590420343617[52] = 0; - out_7635460590420343617[53] = 0; +void H_4(double *state, double *unused, double *out_7659548337950304093) { + out_7659548337950304093[0] = 0; + out_7659548337950304093[1] = 0; + out_7659548337950304093[2] = 0; + out_7659548337950304093[3] = 0; + out_7659548337950304093[4] = 0; + out_7659548337950304093[5] = 0; + out_7659548337950304093[6] = 1; + out_7659548337950304093[7] = 0; + out_7659548337950304093[8] = 0; + out_7659548337950304093[9] = 1; + out_7659548337950304093[10] = 0; + out_7659548337950304093[11] = 0; + out_7659548337950304093[12] = 0; + out_7659548337950304093[13] = 0; + out_7659548337950304093[14] = 0; + out_7659548337950304093[15] = 0; + out_7659548337950304093[16] = 0; + out_7659548337950304093[17] = 0; + out_7659548337950304093[18] = 0; + out_7659548337950304093[19] = 0; + out_7659548337950304093[20] = 0; + out_7659548337950304093[21] = 0; + out_7659548337950304093[22] = 0; + out_7659548337950304093[23] = 0; + out_7659548337950304093[24] = 0; + out_7659548337950304093[25] = 1; + out_7659548337950304093[26] = 0; + out_7659548337950304093[27] = 0; + out_7659548337950304093[28] = 1; + out_7659548337950304093[29] = 0; + out_7659548337950304093[30] = 0; + out_7659548337950304093[31] = 0; + out_7659548337950304093[32] = 0; + out_7659548337950304093[33] = 0; + out_7659548337950304093[34] = 0; + out_7659548337950304093[35] = 0; + out_7659548337950304093[36] = 0; + out_7659548337950304093[37] = 0; + out_7659548337950304093[38] = 0; + out_7659548337950304093[39] = 0; + out_7659548337950304093[40] = 0; + out_7659548337950304093[41] = 0; + out_7659548337950304093[42] = 0; + out_7659548337950304093[43] = 0; + out_7659548337950304093[44] = 1; + out_7659548337950304093[45] = 0; + out_7659548337950304093[46] = 0; + out_7659548337950304093[47] = 1; + out_7659548337950304093[48] = 0; + out_7659548337950304093[49] = 0; + out_7659548337950304093[50] = 0; + out_7659548337950304093[51] = 0; + out_7659548337950304093[52] = 0; + out_7659548337950304093[53] = 0; } -void h_10(double *state, double *unused, double *out_729559215867171711) { - out_729559215867171711[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_729559215867171711[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_729559215867171711[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_5351201218378086589) { + out_5351201218378086589[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_5351201218378086589[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_5351201218378086589[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_4724811786445925588) { - out_4724811786445925588[0] = 0; - out_4724811786445925588[1] = 9.8100000000000005*cos(state[1]); - out_4724811786445925588[2] = 0; - out_4724811786445925588[3] = 0; - out_4724811786445925588[4] = -state[8]; - out_4724811786445925588[5] = state[7]; - out_4724811786445925588[6] = 0; - out_4724811786445925588[7] = state[5]; - out_4724811786445925588[8] = -state[4]; - out_4724811786445925588[9] = 0; - out_4724811786445925588[10] = 0; - out_4724811786445925588[11] = 0; - out_4724811786445925588[12] = 1; - out_4724811786445925588[13] = 0; - out_4724811786445925588[14] = 0; - out_4724811786445925588[15] = 1; - out_4724811786445925588[16] = 0; - out_4724811786445925588[17] = 0; - out_4724811786445925588[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_4724811786445925588[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_4724811786445925588[20] = 0; - out_4724811786445925588[21] = state[8]; - out_4724811786445925588[22] = 0; - out_4724811786445925588[23] = -state[6]; - out_4724811786445925588[24] = -state[5]; - out_4724811786445925588[25] = 0; - out_4724811786445925588[26] = state[3]; - out_4724811786445925588[27] = 0; - out_4724811786445925588[28] = 0; - out_4724811786445925588[29] = 0; - out_4724811786445925588[30] = 0; - out_4724811786445925588[31] = 1; - out_4724811786445925588[32] = 0; - out_4724811786445925588[33] = 0; - out_4724811786445925588[34] = 1; - out_4724811786445925588[35] = 0; - out_4724811786445925588[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_4724811786445925588[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_4724811786445925588[38] = 0; - out_4724811786445925588[39] = -state[7]; - out_4724811786445925588[40] = state[6]; - out_4724811786445925588[41] = 0; - out_4724811786445925588[42] = state[4]; - out_4724811786445925588[43] = -state[3]; - out_4724811786445925588[44] = 0; - out_4724811786445925588[45] = 0; - out_4724811786445925588[46] = 0; - out_4724811786445925588[47] = 0; - out_4724811786445925588[48] = 0; - out_4724811786445925588[49] = 0; - out_4724811786445925588[50] = 1; - out_4724811786445925588[51] = 0; - out_4724811786445925588[52] = 0; - out_4724811786445925588[53] = 1; +void H_10(double *state, double *unused, double *out_5063285185285945841) { + out_5063285185285945841[0] = 0; + out_5063285185285945841[1] = 9.8100000000000005*cos(state[1]); + out_5063285185285945841[2] = 0; + out_5063285185285945841[3] = 0; + out_5063285185285945841[4] = -state[8]; + out_5063285185285945841[5] = state[7]; + out_5063285185285945841[6] = 0; + out_5063285185285945841[7] = state[5]; + out_5063285185285945841[8] = -state[4]; + out_5063285185285945841[9] = 0; + out_5063285185285945841[10] = 0; + out_5063285185285945841[11] = 0; + out_5063285185285945841[12] = 1; + out_5063285185285945841[13] = 0; + out_5063285185285945841[14] = 0; + out_5063285185285945841[15] = 1; + out_5063285185285945841[16] = 0; + out_5063285185285945841[17] = 0; + out_5063285185285945841[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_5063285185285945841[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_5063285185285945841[20] = 0; + out_5063285185285945841[21] = state[8]; + out_5063285185285945841[22] = 0; + out_5063285185285945841[23] = -state[6]; + out_5063285185285945841[24] = -state[5]; + out_5063285185285945841[25] = 0; + out_5063285185285945841[26] = state[3]; + out_5063285185285945841[27] = 0; + out_5063285185285945841[28] = 0; + out_5063285185285945841[29] = 0; + out_5063285185285945841[30] = 0; + out_5063285185285945841[31] = 1; + out_5063285185285945841[32] = 0; + out_5063285185285945841[33] = 0; + out_5063285185285945841[34] = 1; + out_5063285185285945841[35] = 0; + out_5063285185285945841[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_5063285185285945841[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_5063285185285945841[38] = 0; + out_5063285185285945841[39] = -state[7]; + out_5063285185285945841[40] = state[6]; + out_5063285185285945841[41] = 0; + out_5063285185285945841[42] = state[4]; + out_5063285185285945841[43] = -state[3]; + out_5063285185285945841[44] = 0; + out_5063285185285945841[45] = 0; + out_5063285185285945841[46] = 0; + out_5063285185285945841[47] = 0; + out_5063285185285945841[48] = 0; + out_5063285185285945841[49] = 0; + out_5063285185285945841[50] = 1; + out_5063285185285945841[51] = 0; + out_5063285185285945841[52] = 0; + out_5063285185285945841[53] = 1; } -void h_13(double *state, double *unused, double *out_6088791521295825946) { - out_6088791521295825946[0] = state[3]; - out_6088791521295825946[1] = state[4]; - out_6088791521295825946[2] = state[5]; +void h_13(double *state, double *unused, double *out_8511287818621351769) { + out_8511287818621351769[0] = state[3]; + out_8511287818621351769[1] = state[4]; + out_8511287818621351769[2] = state[5]; } -void H_13(double *state, double *unused, double *out_4423186765088010816) { - out_4423186765088010816[0] = 0; - out_4423186765088010816[1] = 0; - out_4423186765088010816[2] = 0; - out_4423186765088010816[3] = 1; - out_4423186765088010816[4] = 0; - out_4423186765088010816[5] = 0; - out_4423186765088010816[6] = 0; - out_4423186765088010816[7] = 0; - out_4423186765088010816[8] = 0; - out_4423186765088010816[9] = 0; - out_4423186765088010816[10] = 0; - out_4423186765088010816[11] = 0; - out_4423186765088010816[12] = 0; - out_4423186765088010816[13] = 0; - out_4423186765088010816[14] = 0; - out_4423186765088010816[15] = 0; - out_4423186765088010816[16] = 0; - out_4423186765088010816[17] = 0; - out_4423186765088010816[18] = 0; - out_4423186765088010816[19] = 0; - out_4423186765088010816[20] = 0; - out_4423186765088010816[21] = 0; - out_4423186765088010816[22] = 1; - out_4423186765088010816[23] = 0; - out_4423186765088010816[24] = 0; - out_4423186765088010816[25] = 0; - out_4423186765088010816[26] = 0; - out_4423186765088010816[27] = 0; - out_4423186765088010816[28] = 0; - out_4423186765088010816[29] = 0; - out_4423186765088010816[30] = 0; - out_4423186765088010816[31] = 0; - out_4423186765088010816[32] = 0; - out_4423186765088010816[33] = 0; - out_4423186765088010816[34] = 0; - out_4423186765088010816[35] = 0; - out_4423186765088010816[36] = 0; - out_4423186765088010816[37] = 0; - out_4423186765088010816[38] = 0; - out_4423186765088010816[39] = 0; - out_4423186765088010816[40] = 0; - out_4423186765088010816[41] = 1; - out_4423186765088010816[42] = 0; - out_4423186765088010816[43] = 0; - out_4423186765088010816[44] = 0; - out_4423186765088010816[45] = 0; - out_4423186765088010816[46] = 0; - out_4423186765088010816[47] = 0; - out_4423186765088010816[48] = 0; - out_4423186765088010816[49] = 0; - out_4423186765088010816[50] = 0; - out_4423186765088010816[51] = 0; - out_4423186765088010816[52] = 0; - out_4423186765088010816[53] = 0; +void H_13(double *state, double *unused, double *out_4447274512617971292) { + out_4447274512617971292[0] = 0; + out_4447274512617971292[1] = 0; + out_4447274512617971292[2] = 0; + out_4447274512617971292[3] = 1; + out_4447274512617971292[4] = 0; + out_4447274512617971292[5] = 0; + out_4447274512617971292[6] = 0; + out_4447274512617971292[7] = 0; + out_4447274512617971292[8] = 0; + out_4447274512617971292[9] = 0; + out_4447274512617971292[10] = 0; + out_4447274512617971292[11] = 0; + out_4447274512617971292[12] = 0; + out_4447274512617971292[13] = 0; + out_4447274512617971292[14] = 0; + out_4447274512617971292[15] = 0; + out_4447274512617971292[16] = 0; + out_4447274512617971292[17] = 0; + out_4447274512617971292[18] = 0; + out_4447274512617971292[19] = 0; + out_4447274512617971292[20] = 0; + out_4447274512617971292[21] = 0; + out_4447274512617971292[22] = 1; + out_4447274512617971292[23] = 0; + out_4447274512617971292[24] = 0; + out_4447274512617971292[25] = 0; + out_4447274512617971292[26] = 0; + out_4447274512617971292[27] = 0; + out_4447274512617971292[28] = 0; + out_4447274512617971292[29] = 0; + out_4447274512617971292[30] = 0; + out_4447274512617971292[31] = 0; + out_4447274512617971292[32] = 0; + out_4447274512617971292[33] = 0; + out_4447274512617971292[34] = 0; + out_4447274512617971292[35] = 0; + out_4447274512617971292[36] = 0; + out_4447274512617971292[37] = 0; + out_4447274512617971292[38] = 0; + out_4447274512617971292[39] = 0; + out_4447274512617971292[40] = 0; + out_4447274512617971292[41] = 1; + out_4447274512617971292[42] = 0; + out_4447274512617971292[43] = 0; + out_4447274512617971292[44] = 0; + out_4447274512617971292[45] = 0; + out_4447274512617971292[46] = 0; + out_4447274512617971292[47] = 0; + out_4447274512617971292[48] = 0; + out_4447274512617971292[49] = 0; + out_4447274512617971292[50] = 0; + out_4447274512617971292[51] = 0; + out_4447274512617971292[52] = 0; + out_4447274512617971292[53] = 0; } -void h_14(double *state, double *unused, double *out_1848639786756728796) { - out_1848639786756728796[0] = state[6]; - out_1848639786756728796[1] = state[7]; - out_1848639786756728796[2] = state[8]; +void h_14(double *state, double *unused, double *out_7796170631529448638) { + out_7796170631529448638[0] = state[6]; + out_7796170631529448638[1] = state[7]; + out_7796170631529448638[2] = state[8]; } -void H_14(double *state, double *unused, double *out_7728495050993835703) { - out_7728495050993835703[0] = 0; - out_7728495050993835703[1] = 0; - out_7728495050993835703[2] = 0; - out_7728495050993835703[3] = 0; - out_7728495050993835703[4] = 0; - out_7728495050993835703[5] = 0; - out_7728495050993835703[6] = 1; - out_7728495050993835703[7] = 0; - out_7728495050993835703[8] = 0; - out_7728495050993835703[9] = 0; - out_7728495050993835703[10] = 0; - out_7728495050993835703[11] = 0; - out_7728495050993835703[12] = 0; - out_7728495050993835703[13] = 0; - out_7728495050993835703[14] = 0; - out_7728495050993835703[15] = 0; - out_7728495050993835703[16] = 0; - out_7728495050993835703[17] = 0; - out_7728495050993835703[18] = 0; - out_7728495050993835703[19] = 0; - out_7728495050993835703[20] = 0; - out_7728495050993835703[21] = 0; - out_7728495050993835703[22] = 0; - out_7728495050993835703[23] = 0; - out_7728495050993835703[24] = 0; - out_7728495050993835703[25] = 1; - out_7728495050993835703[26] = 0; - out_7728495050993835703[27] = 0; - out_7728495050993835703[28] = 0; - out_7728495050993835703[29] = 0; - out_7728495050993835703[30] = 0; - out_7728495050993835703[31] = 0; - out_7728495050993835703[32] = 0; - out_7728495050993835703[33] = 0; - out_7728495050993835703[34] = 0; - out_7728495050993835703[35] = 0; - out_7728495050993835703[36] = 0; - out_7728495050993835703[37] = 0; - out_7728495050993835703[38] = 0; - out_7728495050993835703[39] = 0; - out_7728495050993835703[40] = 0; - out_7728495050993835703[41] = 0; - out_7728495050993835703[42] = 0; - out_7728495050993835703[43] = 0; - out_7728495050993835703[44] = 1; - out_7728495050993835703[45] = 0; - out_7728495050993835703[46] = 0; - out_7728495050993835703[47] = 0; - out_7728495050993835703[48] = 0; - out_7728495050993835703[49] = 0; - out_7728495050993835703[50] = 0; - out_7728495050993835703[51] = 0; - out_7728495050993835703[52] = 0; - out_7728495050993835703[53] = 0; +void H_14(double *state, double *unused, double *out_3696307481610819564) { + out_3696307481610819564[0] = 0; + out_3696307481610819564[1] = 0; + out_3696307481610819564[2] = 0; + out_3696307481610819564[3] = 0; + out_3696307481610819564[4] = 0; + out_3696307481610819564[5] = 0; + out_3696307481610819564[6] = 1; + out_3696307481610819564[7] = 0; + out_3696307481610819564[8] = 0; + out_3696307481610819564[9] = 0; + out_3696307481610819564[10] = 0; + out_3696307481610819564[11] = 0; + out_3696307481610819564[12] = 0; + out_3696307481610819564[13] = 0; + out_3696307481610819564[14] = 0; + out_3696307481610819564[15] = 0; + out_3696307481610819564[16] = 0; + out_3696307481610819564[17] = 0; + out_3696307481610819564[18] = 0; + out_3696307481610819564[19] = 0; + out_3696307481610819564[20] = 0; + out_3696307481610819564[21] = 0; + out_3696307481610819564[22] = 0; + out_3696307481610819564[23] = 0; + out_3696307481610819564[24] = 0; + out_3696307481610819564[25] = 1; + out_3696307481610819564[26] = 0; + out_3696307481610819564[27] = 0; + out_3696307481610819564[28] = 0; + out_3696307481610819564[29] = 0; + out_3696307481610819564[30] = 0; + out_3696307481610819564[31] = 0; + out_3696307481610819564[32] = 0; + out_3696307481610819564[33] = 0; + out_3696307481610819564[34] = 0; + out_3696307481610819564[35] = 0; + out_3696307481610819564[36] = 0; + out_3696307481610819564[37] = 0; + out_3696307481610819564[38] = 0; + out_3696307481610819564[39] = 0; + out_3696307481610819564[40] = 0; + out_3696307481610819564[41] = 0; + out_3696307481610819564[42] = 0; + out_3696307481610819564[43] = 0; + out_3696307481610819564[44] = 1; + out_3696307481610819564[45] = 0; + out_3696307481610819564[46] = 0; + out_3696307481610819564[47] = 0; + out_3696307481610819564[48] = 0; + out_3696307481610819564[49] = 0; + out_3696307481610819564[50] = 0; + out_3696307481610819564[51] = 0; + out_3696307481610819564[52] = 0; + out_3696307481610819564[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_628539194751276084) { - err_fun(nom_x, delta_x, out_628539194751276084); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8315489890310503187) { + err_fun(nom_x, delta_x, out_8315489890310503187); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5785405288888399451) { - inv_err_fun(nom_x, true_x, out_5785405288888399451); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6008596051390826841) { + inv_err_fun(nom_x, true_x, out_6008596051390826841); } -void pose_H_mod_fun(double *state, double *out_1308901075637477725) { - H_mod_fun(state, out_1308901075637477725); +void pose_H_mod_fun(double *state, double *out_5634677350720166788) { + H_mod_fun(state, out_5634677350720166788); } -void pose_f_fun(double *state, double dt, double *out_8416334481406024670) { - f_fun(state, dt, out_8416334481406024670); +void pose_f_fun(double *state, double dt, double *out_5401222257802876036) { + f_fun(state, dt, out_5401222257802876036); } -void pose_F_fun(double *state, double dt, double *out_5288884962144890875) { - F_fun(state, dt, out_5288884962144890875); +void pose_F_fun(double *state, double dt, double *out_1313183186567530475) { + F_fun(state, dt, out_1313183186567530475); } -void pose_h_4(double *state, double *unused, double *out_90872324407998874) { - h_4(state, unused, out_90872324407998874); +void pose_h_4(double *state, double *unused, double *out_717943751669182438) { + h_4(state, unused, out_717943751669182438); } -void pose_H_4(double *state, double *unused, double *out_7635460590420343617) { - H_4(state, unused, out_7635460590420343617); +void pose_H_4(double *state, double *unused, double *out_7659548337950304093) { + H_4(state, unused, out_7659548337950304093); } -void pose_h_10(double *state, double *unused, double *out_729559215867171711) { - h_10(state, unused, out_729559215867171711); +void pose_h_10(double *state, double *unused, double *out_5351201218378086589) { + h_10(state, unused, out_5351201218378086589); } -void pose_H_10(double *state, double *unused, double *out_4724811786445925588) { - H_10(state, unused, out_4724811786445925588); +void pose_H_10(double *state, double *unused, double *out_5063285185285945841) { + H_10(state, unused, out_5063285185285945841); } -void pose_h_13(double *state, double *unused, double *out_6088791521295825946) { - h_13(state, unused, out_6088791521295825946); +void pose_h_13(double *state, double *unused, double *out_8511287818621351769) { + h_13(state, unused, out_8511287818621351769); } -void pose_H_13(double *state, double *unused, double *out_4423186765088010816) { - H_13(state, unused, out_4423186765088010816); +void pose_H_13(double *state, double *unused, double *out_4447274512617971292) { + H_13(state, unused, out_4447274512617971292); } -void pose_h_14(double *state, double *unused, double *out_1848639786756728796) { - h_14(state, unused, out_1848639786756728796); +void pose_h_14(double *state, double *unused, double *out_7796170631529448638) { + h_14(state, unused, out_7796170631529448638); } -void pose_H_14(double *state, double *unused, double *out_7728495050993835703) { - H_14(state, unused, out_7728495050993835703); +void pose_H_14(double *state, double *unused, double *out_3696307481610819564) { + H_14(state, unused, out_3696307481610819564); } 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 8ede3acd02..a5b74a750b 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_628539194751276084); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5785405288888399451); -void pose_H_mod_fun(double *state, double *out_1308901075637477725); -void pose_f_fun(double *state, double dt, double *out_8416334481406024670); -void pose_F_fun(double *state, double dt, double *out_5288884962144890875); -void pose_h_4(double *state, double *unused, double *out_90872324407998874); -void pose_H_4(double *state, double *unused, double *out_7635460590420343617); -void pose_h_10(double *state, double *unused, double *out_729559215867171711); -void pose_H_10(double *state, double *unused, double *out_4724811786445925588); -void pose_h_13(double *state, double *unused, double *out_6088791521295825946); -void pose_H_13(double *state, double *unused, double *out_4423186765088010816); -void pose_h_14(double *state, double *unused, double *out_1848639786756728796); -void pose_H_14(double *state, double *unused, double *out_7728495050993835703); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8315489890310503187); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6008596051390826841); +void pose_H_mod_fun(double *state, double *out_5634677350720166788); +void pose_f_fun(double *state, double dt, double *out_5401222257802876036); +void pose_F_fun(double *state, double dt, double *out_1313183186567530475); +void pose_h_4(double *state, double *unused, double *out_717943751669182438); +void pose_H_4(double *state, double *unused, double *out_7659548337950304093); +void pose_h_10(double *state, double *unused, double *out_5351201218378086589); +void pose_H_10(double *state, double *unused, double *out_5063285185285945841); +void pose_h_13(double *state, double *unused, double *out_8511287818621351769); +void pose_H_13(double *state, double *unused, double *out_4447274512617971292); +void pose_h_14(double *state, double *unused, double *out_7796170631529448638); +void pose_H_14(double *state, double *unused, double *out_3696307481610819564); 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 8013f1a67f..3d28919ce6 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index 8161596fca..8635e897e2 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -382,6 +382,11 @@ class StarPilotLongitudinalTuneLayout(_SettingsPage): get_state=lambda: self._params.get_bool("TacoTune"), set_state=lambda s: self._params.put_bool("TacoTune", s), visible=self._longitudinal_enabled), + SettingRow("NavLongitudinalAllowed", "toggle", tr_noop("Use Route Speed Control"), + subtitle=tr_noop("Allow an active navigation route to reduce cruise speed for upcoming turns, ramps, and roundabouts."), + get_state=lambda: self._params.get_bool("NavLongitudinalAllowed"), + set_state=lambda s: self._params.put_bool("NavLongitudinalAllowed", s), + visible=self._longitudinal_enabled), ], column_pair="detection_tune"), ] self._manager_view = AetherSettingsView( diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 6ead8955a3..887cf6f4a2 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 6b9a502cc7..6a88761ebc 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -42,6 +42,7 @@ SAFE_MODE_MANAGED_KEYS = ( "NNFFLite", "TurnDesires", "NavDesiresAllowed", + "NavLongitudinalAllowed", "QOLLateral", "PauseLateralSpeed", "PauseLateralOnSignal", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 21133cf331..6de0fc1799 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -1050,6 +1050,7 @@ class StarPilotVariables: if longitudinal_tuning and self.params.get("CoastUpToLeads") is None: toggle.coast_up_to_leads = True toggle.human_lane_changes = has_radar and self.get_value("HumanLaneChanges", condition=longitudinal_tuning) + toggle.nav_longitudinal_allowed = toggle.openpilot_longitudinal and self.get_value("NavLongitudinalAllowed", condition=longitudinal_tuning) # Keep lead detection sensitivity normalized even when longitudinal tuning is disabled. # Some branches can return raw integer defaults (e.g. 35) when condition=False. lead_detection_probability = self.get_value("LeadDetectionThreshold", cast=float, condition=toggle.openpilot_longitudinal, diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index e26015b777..51964a495f 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import json import math from openpilot.common.constants import CV @@ -10,6 +11,16 @@ from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitCo CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS OVERRIDE_FORCE_STOP_TIMER = 10 +NAV_TURN_COMFORT_DECEL = 1.25 +NAV_TURN_DISTANCE_BUFFER = 8.0 +NAV_TURN_MIN_TARGET_DELTA = 0.25 +NAV_TURN_TARGET_SPEEDS = { + "uturn": 5.0 * CV.MPH_TO_MS, + "sharpLeft": 10.0 * CV.MPH_TO_MS, + "sharpRight": 10.0 * CV.MPH_TO_MS, + "left": 14.0 * CV.MPH_TO_MS, + "right": 14.0 * CV.MPH_TO_MS, +} # Force-stop kinematic profile. The user tunes one signed knob (ForceStopDistanceOffset, # in feet); positive = stop later/longer, negative = stop sooner/shorter. All other @@ -58,6 +69,99 @@ class StarPilotVCruise: self.tracked_model_length = 0.0 self.stop_sign_confirmed = False + self.nav_turn_target = 0.0 + self._nav_instruction_state_raw = None + self._nav_instruction_state = {} + + def _update_nav_instruction_state(self): + raw = self.starpilot_planner.params_memory.get("NavInstructionState") or {} + if raw == self._nav_instruction_state_raw: + return + + self._nav_instruction_state_raw = raw + if not raw: + self._nav_instruction_state = {} + return + + if isinstance(raw, dict): + self._nav_instruction_state = raw + return + + if isinstance(raw, str): + try: + parsed = json.loads(raw) + self._nav_instruction_state = parsed if isinstance(parsed, dict) else {} + return + except Exception: + pass + + self._nav_instruction_state = {} + + @staticmethod + def _nav_maneuver_target_speed(maneuver_type, maneuver_modifier): + maneuver_type = str(maneuver_type or "").strip().lower() + maneuver_modifier = str(maneuver_modifier or "").strip() + + if not maneuver_modifier and not maneuver_type: + return None + + if maneuver_modifier == "uturn" or "uturn" in maneuver_type or "u-turn" in maneuver_type: + return NAV_TURN_TARGET_SPEEDS["uturn"] + + if "roundabout" in maneuver_type or "rotary" in maneuver_type: + return 12.0 * CV.MPH_TO_MS + + if maneuver_type == "turn": + return NAV_TURN_TARGET_SPEEDS.get(maneuver_modifier) + + return None + + @staticmethod + def _nav_target_for_distance(target_speed, maneuver_distance): + try: + remaining_distance = max(float(maneuver_distance) - NAV_TURN_DISTANCE_BUFFER, 0.0) + except (TypeError, ValueError): + return 0.0 + + return math.sqrt(max(target_speed * target_speed + (2.0 * NAV_TURN_COMFORT_DECEL * remaining_distance), 0.0)) + + @staticmethod + def _get_nav_long_min_target_speed(sm): + car_params = sm.get("carParams") + return max(float(getattr(car_params, "minSteerSpeed", 0.0) or 0.0), 0.0) + + def _get_nav_turn_control_target(self, v_cruise, sm, starpilot_toggles): + self._update_nav_instruction_state() + if not getattr(starpilot_toggles, "nav_longitudinal_allowed", False): + return 0.0 + if not bool(self._nav_instruction_state.get("valid", False)): + return 0.0 + + nav_long_min_target_speed = self._get_nav_long_min_target_speed(sm) + + candidates = [ + ( + self._nav_instruction_state.get("maneuverType"), + self._nav_instruction_state.get("maneuverModifier"), + self._nav_instruction_state.get("maneuverDistance"), + ), + ( + self._nav_instruction_state.get("nextManeuverType"), + self._nav_instruction_state.get("nextManeuverModifier"), + self._nav_instruction_state.get("nextManeuverDistance"), + ), + ] + for maneuver_type, maneuver_modifier, maneuver_distance in candidates: + target_speed = self._nav_maneuver_target_speed(maneuver_type, maneuver_modifier) + if target_speed is None: + continue + target_speed = max(target_speed, nav_long_min_target_speed) + + target = max(target_speed, self._nav_target_for_distance(target_speed, maneuver_distance)) + if target + NAV_TURN_MIN_TARGET_DELTA < v_cruise: + return target + + return 0.0 # ===== Main update ===== @@ -176,6 +280,8 @@ class StarPilotVCruise: self.slc_offset = 0 self.slc_target = 0 + self.nav_turn_target = self._get_nav_turn_control_target(v_cruise, sm, starpilot_toggles) + # Single tuning knob (signed feet -> meters). Defense clamp on top of UI bounds. offset_ft_raw = int(getattr(starpilot_toggles, 'force_stop_distance_offset', 0) or 0) offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, offset_ft_raw)) @@ -215,7 +321,9 @@ class StarPilotVCruise: self.tracked_model_length = self.starpilot_planner.model_length - targets = [self.csc_target, v_cruise] + targets = [v_cruise] + if self.csc_target >= CSC_MIN_SPEED: + targets.append(self.csc_target) slc_control_target = get_active_slc_control_target( starpilot_toggles.speed_limit_controller, getattr(starpilot_toggles, "set_speed_limit", False), @@ -224,8 +332,10 @@ class StarPilotVCruise: self.slc.overridden_speed, v_ego_diff, ) - if slc_control_target > 0.0: + if slc_control_target >= CSC_MIN_SPEED: targets.append(slc_control_target) - v_cruise = min([target if target >= CSC_MIN_SPEED else v_cruise for target in targets]) + if self.nav_turn_target > 0.0: + targets.append(self.nav_turn_target) + v_cruise = min(targets) return v_cruise diff --git a/starpilot/navigation/navigationd.py b/starpilot/navigation/navigationd.py index 392ecb9fa1..cf3b9b863a 100644 --- a/starpilot/navigation/navigationd.py +++ b/starpilot/navigation/navigationd.py @@ -263,6 +263,7 @@ class Navigationd: "maneuverDistance": float(payload.get("maneuverDistance") or 0.0), "nextManeuverType": str(next_maneuver.get("type") or ""), "nextManeuverModifier": str(next_maneuver.get("modifier") or ""), + "nextManeuverDistance": float(next_maneuver.get("distance") or 0.0), } if state != self._last_nav_state: self.params_memory.put_nonblocking("NavInstructionState", state) diff --git a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json index 37935567f6..1b6e77e3b7 100644 --- a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json @@ -1031,6 +1031,14 @@ "ui_type": "toggle", "parent_key": "LongitudinalTune" }, + { + "key": "NavLongitudinalAllowed", + "label": "Use Route Speed Control", + "description": "Allow an active navigation route to reduce cruise speed for upcoming turns, ramps, and roundabouts.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "LongitudinalTune" + }, { "key": "QOLLongitudinal", "label": "Quality of Life", diff --git a/starpilot/ui/qt/offroad/longitudinal_settings.cc b/starpilot/ui/qt/offroad/longitudinal_settings.cc index ef8c2f0fd4..2b70e3a2db 100644 --- a/starpilot/ui/qt/offroad/longitudinal_settings.cc +++ b/starpilot/ui/qt/offroad/longitudinal_settings.cc @@ -151,6 +151,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow * {"HumanLaneChanges", tr("Human-Like Lane Changes"), tr("Lane-change behavior that mimics human drivers by anticipating and tracking adjacent vehicles during lane changes."), ""}, {"LeadDetectionThreshold", tr("Lead Detection Sensitivity"), tr("How sensitive openpilot is to detecting vehicles. Higher sensitivity allows quicker detection at longer distances but may react to non-vehicle objects; lower sensitivity is more conservative and reduces false detections."), ""}, {"TacoTune", tr("\"Taco Bell Run\" Turn Speed Hack"), tr("The turn-speed hack from comma's 2022 \"Taco Bell Run\". Designed to slow down for left and right turns."), ""}, + {"NavLongitudinalAllowed", tr("Use Route Speed Control"), tr("Allow an active navigation route to reduce cruise speed for upcoming turns, ramps, and roundabouts."), ""}, {"QOLLongitudinal", tr("Quality of Life"), tr("Miscellaneous acceleration and braking control changes to fine-tune how openpilot drives."), "../../starpilot/assets/toggle_icons/icon_quality_of_life.png"}, {"CustomCruise", tr("Cruise Interval"), tr("How much the set speed increases or decreases for each + or – cruise control button press."), ""}, diff --git a/starpilot/ui/qt/offroad/longitudinal_settings.h b/starpilot/ui/qt/offroad/longitudinal_settings.h index d8af0afa76..e98de73805 100644 --- a/starpilot/ui/qt/offroad/longitudinal_settings.h +++ b/starpilot/ui/qt/offroad/longitudinal_settings.h @@ -33,7 +33,7 @@ private: QSet conditionalExperimentalKeys = {"PersistExperimentalState", "CESpeed", "CESpeedLead", "CECurves", "CELead", "CEModelStopTime", "CESignalSpeed", "CEStopLights", "ShowCEMStatus"}; QSet curveSpeedKeys = {"CalibratedLateralAcceleration", "CalibrationProgress", "ResetCurveData", "ShowCSCStatus"}; QSet customDrivingPersonalityKeys = {"AggressivePersonalityProfile", "RelaxedPersonalityProfile", "StandardPersonalityProfile", "TrafficPersonalityProfile"}; - QSet longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", "TacoTune"}; + QSet longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", "TacoTune", "NavLongitudinalAllowed"}; QSet qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStopDistanceOffset", "ForceStandstill", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"}; QSet relaxedPersonalityKeys = {"RelaxedFollow", "RelaxedFollowHigh", "RelaxedJerkAcceleration", "RelaxedJerkDeceleration", "RelaxedJerkDanger", "RelaxedJerkSpeed", "RelaxedJerkSpeedDecrease", "ResetRelaxedPersonality"}; QSet speedLimitControllerKeys = {"SLCOffsets", "SLCFallback", "SLCOverride", "SLCPriority", "SLCQOL", "SLCVisuals"}; diff --git a/system/camerad/camerad b/system/camerad/camerad index d4e4802fed..447d808204 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 370c190add..65f318fac5 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 00c9f4c981..8cad6fd448 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index f47b3908f1..97c8dcdb8b 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ