diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index b875836da..09e4d7f6a 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -571,9 +571,9 @@ class CarInterface(CarInterfaceBase): ret.openpilotLongitudinalControl = not disable_openpilot_long volt_test_tune_active = ( + testing_ground.use_2 and ret.openpilotLongitudinalControl and - candidate in VOLT_LONG_TEST_TUNE_CARS and - (testing_ground.use_2 or not ret.enableGasInterceptorDEPRECATED) + candidate in VOLT_LONG_TEST_TUNE_CARS ) if volt_test_tune_active: # Volt long can still fall back to an all-I tune on both the interceptor and diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index 35303d60e..9d97cb515 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -60,41 +60,38 @@ class TestGMInterface: assert car_params.minSteerSpeed == pytest.approx(7 * CV.MPH_TO_MS) - def test_volt_testing_ground_tune_sets_nonzero_p_and_starting_state(self, monkeypatch): + @parameterized.expand([ + ("interceptor", True), + ("ascm_int", False), + ]) + def test_volt_testing_ground_tune_sets_nonzero_p_and_starting_state(self, _name, pedal_present): CarInterface = interfaces[CAR.CHEVROLET_VOLT_ASCM] fingerprint = _empty_fingerprint() - fingerprint[0][0x201] = 8 # pedal detected + if pedal_present: + fingerprint[0][0x201] = 8 # pedal detected fingerprint[0][0x2FF] = 8 # SASCM detected - monkeypatch.setattr(gm_interface, "testing_ground", SimpleNamespace(use_2=True), raising=False) + old_testing_ground = gm_interface.testing_ground + gm_interface.testing_ground = SimpleNamespace(use_2=True) - car_params = CarInterface.get_params(CAR.CHEVROLET_VOLT_ASCM, fingerprint, [], alpha_long=False, is_release=False, docs=False, - starpilot_toggles=_test_starpilot_toggles()) + try: + car_params = CarInterface.get_params(CAR.CHEVROLET_VOLT_ASCM, fingerprint, [], alpha_long=False, is_release=False, docs=False, + starpilot_toggles=_test_starpilot_toggles()) + finally: + gm_interface.testing_ground = old_testing_ground - assert list(car_params.longitudinalTuning.kpV) == pytest.approx([0.10, 0.072, 0.05, 0.04]) - assert list(car_params.longitudinalTuning.kiV) == pytest.approx([0.025, 0.03, 0.04, 0.055]) - assert car_params.startingState - assert car_params.startAccel == pytest.approx(1.15) - - @parameterized.expand([ - ("volt_2019", CAR.CHEVROLET_VOLT_2019, True), - ("volt_ascm", CAR.CHEVROLET_VOLT_ASCM, True), - ]) - def test_non_pedal_volt_alpha_long_uses_volt_tune_by_default(self, _name, car_model, needs_sascm): - CarInterface = interfaces[car_model] - fingerprint = _empty_fingerprint() - if needs_sascm: - fingerprint[0][0x2FF] = 8 # SASCM detected - - car_params = CarInterface.get_params(car_model, fingerprint, [], alpha_long=True, is_release=False, docs=False, - starpilot_toggles=_test_starpilot_toggles()) - - assert car_params.openpilotLongitudinalControl - assert not car_params.enableGasInterceptorDEPRECATED - assert list(car_params.longitudinalTuning.kpV) == pytest.approx([0.10, 0.072, 0.05, 0.04]) - assert list(car_params.longitudinalTuning.kiV) == pytest.approx([0.025, 0.03, 0.04, 0.055]) - assert car_params.startingState - assert car_params.startAccel == pytest.approx(1.15) + if pedal_present: + assert list(car_params.longitudinalTuning.kpV) == pytest.approx([0.10, 0.072, 0.05, 0.04]) + assert list(car_params.longitudinalTuning.kiV) == pytest.approx([0.025, 0.03, 0.04, 0.055]) + assert car_params.startingState + assert car_params.startAccel == pytest.approx(1.15) + else: + assert not car_params.openpilotLongitudinalControl + assert not car_params.enableGasInterceptorDEPRECATED + assert list(car_params.longitudinalTuning.kpV) == [0.0] + assert list(car_params.longitudinalTuning.kiV) == [0.5, 0.5] + assert not car_params.startingState + assert car_params.startAccel == pytest.approx(0.0) def test_volt_cc_sparse_fingerprint_without_camera_sets_no_camera(self): CarInterface = interfaces[CAR.CHEVROLET_VOLT_CC] diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 3081d0634..711bd932e 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -45,8 +45,10 @@ IONIQ_6_DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, - IONIQ_6_DYNAMIC_LOWER_JERK_V = [3.3, 1.5, 1.0, 0.8, 0.7, 0.65, 0.55, 0.5] IONIQ_6_LAUNCH_HOLD_SPEED_BP = [0.0, 0.6, 1.25, 2.5] IONIQ_6_LAUNCH_HOLD_SPEED_V = [0.75, 0.6, 0.4, 0.0] -IONIQ_6_STOP_HOLD_SPEED_BP = [0.0, 0.25, 0.6, 1.2] -IONIQ_6_STOP_HOLD_SPEED_V = [-0.12, -0.10, -0.05, 0.0] +IONIQ_6_STOP_HOLD_SPEED_BP = [0.0, 0.08, 0.25, 0.6, 1.2] +IONIQ_6_STOP_HOLD_SPEED_V = [-0.09, -0.095, -0.10, -0.05, 0.0] +IONIQ_6_STOP_HOLD_JERK_BP = [0.0, 0.15, 0.6, 1.2] +IONIQ_6_STOP_HOLD_JERK_V = [0.35, 0.40, 0.52, 0.65] IONIQ_6_STOP_RELEASE_JERK_BP = [0.0, 0.15, 0.5] IONIQ_6_STOP_RELEASE_JERK_V = [3.6 * IONIQ_6_RESPONSE_MULTIPLIER, 4.2 * IONIQ_6_RESPONSE_MULTIPLIER, @@ -136,7 +138,7 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc if state.stopping: state.desired_accel = float(np.interp(v_ego, IONIQ_6_STOP_HOLD_SPEED_BP, IONIQ_6_STOP_HOLD_SPEED_V)) - state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, [0.0, 1.2], [0.45, 0.65])) * IONIQ_6_RESPONSE_MULTIPLIER) + state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, IONIQ_6_STOP_HOLD_JERK_BP, IONIQ_6_STOP_HOLD_JERK_V)) * IONIQ_6_RESPONSE_MULTIPLIER) else: state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) if state.launch_active: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index d9937be1c..fb930afda 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -4,7 +4,7 @@ from types import SimpleNamespace import pytest from opendbc.can import CANPacker, CANParser -from opendbc.car import Bus, ButtonType, gen_empty_fingerprint +from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs from opendbc.car.structs import CarControl, CarParams from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car from opendbc.car.hyundai.carcontroller import Ioniq6LongitudinalTuningState, GenesisG90LongitudinalTuningState, \ @@ -366,6 +366,17 @@ class TestHyundaiFingerprint: long_control_state=LongCtrlState.starting, long_active=True) assert state.actual_accel == pytest.approx(0.312) + def test_ioniq_6_longitudinal_tuning_helper_softens_final_stop_hold(self): + state = Ioniq6LongitudinalTuningState(actual_accel=-0.12, accel_last=-0.12, + stopping=True, stopping_count=25, + long_control_state_last=LongCtrlState.stopping) + + state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-1.8, v_ego=0.0, a_ego=0.0, + long_control_state=LongCtrlState.stopping, long_active=True) + assert state.desired_accel == pytest.approx(-0.09) + assert state.jerk_upper == pytest.approx(0.42) + assert state.actual_accel == pytest.approx(-0.099) + def test_genesis_g90_longitudinal_tuning_softens_final_stop_hold(self): state = GenesisG90LongitudinalTuningState() diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 9cde239c7..ebe27109a 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 c8f096ffb..331c89f75 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 97d6ad756..9fc2b44b2 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 c57c38c4f..7a25a811d 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 e1e7c2834..747530168 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 a88d40113..27644cd2a 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 6e825729a..bcbf1ff4f 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 6e825729a..bcbf1ff4f 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 6ae5b95b3..cb797ed3b 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 a88d40113..27644cd2a 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 52f641c16..ddf591e5c 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-016e657e-DEBUG"; +const uint8_t gitversion[19] = "DEV-95fa880b-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index a32d2faf3..aef37ac6a 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 762664319..d60395c98 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 0244a3c79..146a899c8 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 b73b72ef9..b9f75a2b5 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 8e8b17aac..a0c8f2a1b 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 d207870de..7eae329e6 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 7235fde67..c27906d7b 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 88448b6f3..da484ac5f 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 b47e583b6..468b2460c 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 8b6b0a7bf..8d5b34b44 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 63c3e1a92..0ff5af1a9 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 940ef5277..84854f38c 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 c4af42a57..413e72145 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 ec7f0e7b8..84e8efd13 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 1a5301020..6f9c777bb 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 9d0900d63..2b193323c 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 442673f78..e617b2859 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 044a6301b..3a634b9bd 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 25ac49000..bf3e5b538 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 54a35741c..1312cfecc 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 86dc3ddf3..efe32d762 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-016e657e-DEBUG \ No newline at end of file +DEV-95fa880b-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 242fdfe2f..8ce2e8821 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -46,10 +46,12 @@ def get_civic_bosch_modified_pid_output_alpha(desired_angle_deg: float, desired_ onset = min(max((abs_angle - 3.0) / 5.0, 0.0), 1.0) cutoff = min(max((22.0 - abs_angle) / 8.0, 0.0), 1.0) band_weight = onset * cutoff + large_turn_weight = min(max((abs_angle - 16.0) / 6.0, 0.0), 1.0) transition_weight = min(abs(desired_angle_delta_deg) / 0.35, 1.0) sign_change_weight = 1.0 if (output_torque * prev_output_torque) < 0.0 else 0.0 smoothing = band_weight * (0.36 + (0.22 * speed_weight) + (0.12 * transition_weight) + (0.12 * sign_change_weight)) + smoothing *= 1.0 - (0.50 * large_turn_weight) return min(max(1.0 - smoothing, 0.18), 1.0) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index a0eb8a976..83ca66b1d 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -226,8 +226,8 @@ IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.18 IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.34 IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.86 IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 4.55 -IONIQ_6_CENTER_TAPER_MAX = 0.046 -IONIQ_6_CENTER_TAPER_LAT = 0.18 +IONIQ_6_CENTER_TAPER_MAX = 0.056 +IONIQ_6_CENTER_TAPER_LAT = 0.22 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02 IONIQ_6_CENTER_TAPER_SPEED = 18.0 IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5 diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 88cbb49d0..1519dd05f 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -332,6 +332,48 @@ def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead() assert cem.slow_lead_detected +def test_tracked_highway_mild_closing_lead_does_not_trigger_raw_slow_lead(): + v_ego = 65 * CV.MPH_TO_MS + cem = make_cem( + model_length=v_ego * 5.0, + tracking_lead=True, + lead_status=True, + lead_d_rel=46.0, + lead_v_lead=v_ego - 1.4, + lead_model_prob=1.0, + ) + toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) + + cem.slow_lead_filter.x = 0.0 + cem.slow_lead_detected = False + cem.starpilot_planner.starpilot_following.slower_lead = False + for _ in range(12): + cem.slow_lead(toggles, v_ego) + + assert not cem.slow_lead_detected + + +def test_tracked_following_slower_lead_still_triggers_slow_lead(): + v_ego = 65 * CV.MPH_TO_MS + cem = make_cem( + model_length=v_ego * 5.0, + tracking_lead=True, + lead_status=True, + lead_d_rel=36.0, + lead_v_lead=v_ego - 3.0, + lead_model_prob=1.0, + ) + toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) + + cem.slow_lead_filter.x = 0.0 + cem.slow_lead_detected = False + cem.starpilot_planner.starpilot_following.slower_lead = True + for _ in range(24): + cem.slow_lead(toggles, v_ego) + + assert cem.slow_lead_detected + + def test_slow_lead_does_not_linger_at_crawl_when_stopped_lead_disabled(): v_ego = 1.5 cem = make_cem( diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 266d3deeb..2cb8be037 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -279,7 +279,7 @@ class TestLatControl: assert get_ioniq_6_center_taper_scale(0.0, 10.0) < get_ioniq_6_center_taper_scale(0.0, 30.0) assert get_ioniq_6_center_taper_scale(0.0, 30.0) < get_ioniq_6_center_taper_scale(0.2, 30.0) assert get_ioniq_6_center_taper_scale(0.0, 12.0) < get_ioniq_6_center_taper_scale(0.25, 12.0) - assert abs(get_ioniq_6_center_taper_scale(0.2, 30.0) - 1.0) < 1.3e-2 + assert abs(get_ioniq_6_center_taper_scale(0.2, 30.0) - 1.0) < 4.2e-2 def test_kia_ev6_ff_scale_curve(self): assert get_kia_ev6_ff_scale(0.0, 0.0, 20.0) == 1.0 diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index f4ef56929..29cace37c 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_3321760492551468366) { - out_3321760492551468366[0] = delta_x[0] + nom_x[0]; - out_3321760492551468366[1] = delta_x[1] + nom_x[1]; - out_3321760492551468366[2] = delta_x[2] + nom_x[2]; - out_3321760492551468366[3] = delta_x[3] + nom_x[3]; - out_3321760492551468366[4] = delta_x[4] + nom_x[4]; - out_3321760492551468366[5] = delta_x[5] + nom_x[5]; - out_3321760492551468366[6] = delta_x[6] + nom_x[6]; - out_3321760492551468366[7] = delta_x[7] + nom_x[7]; - out_3321760492551468366[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_760253992355356602) { + out_760253992355356602[0] = delta_x[0] + nom_x[0]; + out_760253992355356602[1] = delta_x[1] + nom_x[1]; + out_760253992355356602[2] = delta_x[2] + nom_x[2]; + out_760253992355356602[3] = delta_x[3] + nom_x[3]; + out_760253992355356602[4] = delta_x[4] + nom_x[4]; + out_760253992355356602[5] = delta_x[5] + nom_x[5]; + out_760253992355356602[6] = delta_x[6] + nom_x[6]; + out_760253992355356602[7] = delta_x[7] + nom_x[7]; + out_760253992355356602[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3697485090417495632) { - out_3697485090417495632[0] = -nom_x[0] + true_x[0]; - out_3697485090417495632[1] = -nom_x[1] + true_x[1]; - out_3697485090417495632[2] = -nom_x[2] + true_x[2]; - out_3697485090417495632[3] = -nom_x[3] + true_x[3]; - out_3697485090417495632[4] = -nom_x[4] + true_x[4]; - out_3697485090417495632[5] = -nom_x[5] + true_x[5]; - out_3697485090417495632[6] = -nom_x[6] + true_x[6]; - out_3697485090417495632[7] = -nom_x[7] + true_x[7]; - out_3697485090417495632[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_4983548601721046404) { + out_4983548601721046404[0] = -nom_x[0] + true_x[0]; + out_4983548601721046404[1] = -nom_x[1] + true_x[1]; + out_4983548601721046404[2] = -nom_x[2] + true_x[2]; + out_4983548601721046404[3] = -nom_x[3] + true_x[3]; + out_4983548601721046404[4] = -nom_x[4] + true_x[4]; + out_4983548601721046404[5] = -nom_x[5] + true_x[5]; + out_4983548601721046404[6] = -nom_x[6] + true_x[6]; + out_4983548601721046404[7] = -nom_x[7] + true_x[7]; + out_4983548601721046404[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_4190227621087458402) { - out_4190227621087458402[0] = 1.0; - out_4190227621087458402[1] = 0.0; - out_4190227621087458402[2] = 0.0; - out_4190227621087458402[3] = 0.0; - out_4190227621087458402[4] = 0.0; - out_4190227621087458402[5] = 0.0; - out_4190227621087458402[6] = 0.0; - out_4190227621087458402[7] = 0.0; - out_4190227621087458402[8] = 0.0; - out_4190227621087458402[9] = 0.0; - out_4190227621087458402[10] = 1.0; - out_4190227621087458402[11] = 0.0; - out_4190227621087458402[12] = 0.0; - out_4190227621087458402[13] = 0.0; - out_4190227621087458402[14] = 0.0; - out_4190227621087458402[15] = 0.0; - out_4190227621087458402[16] = 0.0; - out_4190227621087458402[17] = 0.0; - out_4190227621087458402[18] = 0.0; - out_4190227621087458402[19] = 0.0; - out_4190227621087458402[20] = 1.0; - out_4190227621087458402[21] = 0.0; - out_4190227621087458402[22] = 0.0; - out_4190227621087458402[23] = 0.0; - out_4190227621087458402[24] = 0.0; - out_4190227621087458402[25] = 0.0; - out_4190227621087458402[26] = 0.0; - out_4190227621087458402[27] = 0.0; - out_4190227621087458402[28] = 0.0; - out_4190227621087458402[29] = 0.0; - out_4190227621087458402[30] = 1.0; - out_4190227621087458402[31] = 0.0; - out_4190227621087458402[32] = 0.0; - out_4190227621087458402[33] = 0.0; - out_4190227621087458402[34] = 0.0; - out_4190227621087458402[35] = 0.0; - out_4190227621087458402[36] = 0.0; - out_4190227621087458402[37] = 0.0; - out_4190227621087458402[38] = 0.0; - out_4190227621087458402[39] = 0.0; - out_4190227621087458402[40] = 1.0; - out_4190227621087458402[41] = 0.0; - out_4190227621087458402[42] = 0.0; - out_4190227621087458402[43] = 0.0; - out_4190227621087458402[44] = 0.0; - out_4190227621087458402[45] = 0.0; - out_4190227621087458402[46] = 0.0; - out_4190227621087458402[47] = 0.0; - out_4190227621087458402[48] = 0.0; - out_4190227621087458402[49] = 0.0; - out_4190227621087458402[50] = 1.0; - out_4190227621087458402[51] = 0.0; - out_4190227621087458402[52] = 0.0; - out_4190227621087458402[53] = 0.0; - out_4190227621087458402[54] = 0.0; - out_4190227621087458402[55] = 0.0; - out_4190227621087458402[56] = 0.0; - out_4190227621087458402[57] = 0.0; - out_4190227621087458402[58] = 0.0; - out_4190227621087458402[59] = 0.0; - out_4190227621087458402[60] = 1.0; - out_4190227621087458402[61] = 0.0; - out_4190227621087458402[62] = 0.0; - out_4190227621087458402[63] = 0.0; - out_4190227621087458402[64] = 0.0; - out_4190227621087458402[65] = 0.0; - out_4190227621087458402[66] = 0.0; - out_4190227621087458402[67] = 0.0; - out_4190227621087458402[68] = 0.0; - out_4190227621087458402[69] = 0.0; - out_4190227621087458402[70] = 1.0; - out_4190227621087458402[71] = 0.0; - out_4190227621087458402[72] = 0.0; - out_4190227621087458402[73] = 0.0; - out_4190227621087458402[74] = 0.0; - out_4190227621087458402[75] = 0.0; - out_4190227621087458402[76] = 0.0; - out_4190227621087458402[77] = 0.0; - out_4190227621087458402[78] = 0.0; - out_4190227621087458402[79] = 0.0; - out_4190227621087458402[80] = 1.0; +void H_mod_fun(double *state, double *out_8995833010704521871) { + out_8995833010704521871[0] = 1.0; + out_8995833010704521871[1] = 0.0; + out_8995833010704521871[2] = 0.0; + out_8995833010704521871[3] = 0.0; + out_8995833010704521871[4] = 0.0; + out_8995833010704521871[5] = 0.0; + out_8995833010704521871[6] = 0.0; + out_8995833010704521871[7] = 0.0; + out_8995833010704521871[8] = 0.0; + out_8995833010704521871[9] = 0.0; + out_8995833010704521871[10] = 1.0; + out_8995833010704521871[11] = 0.0; + out_8995833010704521871[12] = 0.0; + out_8995833010704521871[13] = 0.0; + out_8995833010704521871[14] = 0.0; + out_8995833010704521871[15] = 0.0; + out_8995833010704521871[16] = 0.0; + out_8995833010704521871[17] = 0.0; + out_8995833010704521871[18] = 0.0; + out_8995833010704521871[19] = 0.0; + out_8995833010704521871[20] = 1.0; + out_8995833010704521871[21] = 0.0; + out_8995833010704521871[22] = 0.0; + out_8995833010704521871[23] = 0.0; + out_8995833010704521871[24] = 0.0; + out_8995833010704521871[25] = 0.0; + out_8995833010704521871[26] = 0.0; + out_8995833010704521871[27] = 0.0; + out_8995833010704521871[28] = 0.0; + out_8995833010704521871[29] = 0.0; + out_8995833010704521871[30] = 1.0; + out_8995833010704521871[31] = 0.0; + out_8995833010704521871[32] = 0.0; + out_8995833010704521871[33] = 0.0; + out_8995833010704521871[34] = 0.0; + out_8995833010704521871[35] = 0.0; + out_8995833010704521871[36] = 0.0; + out_8995833010704521871[37] = 0.0; + out_8995833010704521871[38] = 0.0; + out_8995833010704521871[39] = 0.0; + out_8995833010704521871[40] = 1.0; + out_8995833010704521871[41] = 0.0; + out_8995833010704521871[42] = 0.0; + out_8995833010704521871[43] = 0.0; + out_8995833010704521871[44] = 0.0; + out_8995833010704521871[45] = 0.0; + out_8995833010704521871[46] = 0.0; + out_8995833010704521871[47] = 0.0; + out_8995833010704521871[48] = 0.0; + out_8995833010704521871[49] = 0.0; + out_8995833010704521871[50] = 1.0; + out_8995833010704521871[51] = 0.0; + out_8995833010704521871[52] = 0.0; + out_8995833010704521871[53] = 0.0; + out_8995833010704521871[54] = 0.0; + out_8995833010704521871[55] = 0.0; + out_8995833010704521871[56] = 0.0; + out_8995833010704521871[57] = 0.0; + out_8995833010704521871[58] = 0.0; + out_8995833010704521871[59] = 0.0; + out_8995833010704521871[60] = 1.0; + out_8995833010704521871[61] = 0.0; + out_8995833010704521871[62] = 0.0; + out_8995833010704521871[63] = 0.0; + out_8995833010704521871[64] = 0.0; + out_8995833010704521871[65] = 0.0; + out_8995833010704521871[66] = 0.0; + out_8995833010704521871[67] = 0.0; + out_8995833010704521871[68] = 0.0; + out_8995833010704521871[69] = 0.0; + out_8995833010704521871[70] = 1.0; + out_8995833010704521871[71] = 0.0; + out_8995833010704521871[72] = 0.0; + out_8995833010704521871[73] = 0.0; + out_8995833010704521871[74] = 0.0; + out_8995833010704521871[75] = 0.0; + out_8995833010704521871[76] = 0.0; + out_8995833010704521871[77] = 0.0; + out_8995833010704521871[78] = 0.0; + out_8995833010704521871[79] = 0.0; + out_8995833010704521871[80] = 1.0; } -void f_fun(double *state, double dt, double *out_6456348943274406543) { - out_6456348943274406543[0] = state[0]; - out_6456348943274406543[1] = state[1]; - out_6456348943274406543[2] = state[2]; - out_6456348943274406543[3] = state[3]; - out_6456348943274406543[4] = state[4]; - out_6456348943274406543[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_6456348943274406543[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_6456348943274406543[7] = state[7]; - out_6456348943274406543[8] = state[8]; +void f_fun(double *state, double dt, double *out_4562655195590537984) { + out_4562655195590537984[0] = state[0]; + out_4562655195590537984[1] = state[1]; + out_4562655195590537984[2] = state[2]; + out_4562655195590537984[3] = state[3]; + out_4562655195590537984[4] = state[4]; + out_4562655195590537984[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_4562655195590537984[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_4562655195590537984[7] = state[7]; + out_4562655195590537984[8] = state[8]; } -void F_fun(double *state, double dt, double *out_4201765287475637381) { - out_4201765287475637381[0] = 1; - out_4201765287475637381[1] = 0; - out_4201765287475637381[2] = 0; - out_4201765287475637381[3] = 0; - out_4201765287475637381[4] = 0; - out_4201765287475637381[5] = 0; - out_4201765287475637381[6] = 0; - out_4201765287475637381[7] = 0; - out_4201765287475637381[8] = 0; - out_4201765287475637381[9] = 0; - out_4201765287475637381[10] = 1; - out_4201765287475637381[11] = 0; - out_4201765287475637381[12] = 0; - out_4201765287475637381[13] = 0; - out_4201765287475637381[14] = 0; - out_4201765287475637381[15] = 0; - out_4201765287475637381[16] = 0; - out_4201765287475637381[17] = 0; - out_4201765287475637381[18] = 0; - out_4201765287475637381[19] = 0; - out_4201765287475637381[20] = 1; - out_4201765287475637381[21] = 0; - out_4201765287475637381[22] = 0; - out_4201765287475637381[23] = 0; - out_4201765287475637381[24] = 0; - out_4201765287475637381[25] = 0; - out_4201765287475637381[26] = 0; - out_4201765287475637381[27] = 0; - out_4201765287475637381[28] = 0; - out_4201765287475637381[29] = 0; - out_4201765287475637381[30] = 1; - out_4201765287475637381[31] = 0; - out_4201765287475637381[32] = 0; - out_4201765287475637381[33] = 0; - out_4201765287475637381[34] = 0; - out_4201765287475637381[35] = 0; - out_4201765287475637381[36] = 0; - out_4201765287475637381[37] = 0; - out_4201765287475637381[38] = 0; - out_4201765287475637381[39] = 0; - out_4201765287475637381[40] = 1; - out_4201765287475637381[41] = 0; - out_4201765287475637381[42] = 0; - out_4201765287475637381[43] = 0; - out_4201765287475637381[44] = 0; - out_4201765287475637381[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_4201765287475637381[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_4201765287475637381[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4201765287475637381[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4201765287475637381[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_4201765287475637381[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_4201765287475637381[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_4201765287475637381[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_4201765287475637381[53] = -9.8100000000000005*dt; - out_4201765287475637381[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_4201765287475637381[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_4201765287475637381[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4201765287475637381[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4201765287475637381[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_4201765287475637381[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_4201765287475637381[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_4201765287475637381[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4201765287475637381[62] = 0; - out_4201765287475637381[63] = 0; - out_4201765287475637381[64] = 0; - out_4201765287475637381[65] = 0; - out_4201765287475637381[66] = 0; - out_4201765287475637381[67] = 0; - out_4201765287475637381[68] = 0; - out_4201765287475637381[69] = 0; - out_4201765287475637381[70] = 1; - out_4201765287475637381[71] = 0; - out_4201765287475637381[72] = 0; - out_4201765287475637381[73] = 0; - out_4201765287475637381[74] = 0; - out_4201765287475637381[75] = 0; - out_4201765287475637381[76] = 0; - out_4201765287475637381[77] = 0; - out_4201765287475637381[78] = 0; - out_4201765287475637381[79] = 0; - out_4201765287475637381[80] = 1; +void F_fun(double *state, double dt, double *out_6423431565507769528) { + out_6423431565507769528[0] = 1; + out_6423431565507769528[1] = 0; + out_6423431565507769528[2] = 0; + out_6423431565507769528[3] = 0; + out_6423431565507769528[4] = 0; + out_6423431565507769528[5] = 0; + out_6423431565507769528[6] = 0; + out_6423431565507769528[7] = 0; + out_6423431565507769528[8] = 0; + out_6423431565507769528[9] = 0; + out_6423431565507769528[10] = 1; + out_6423431565507769528[11] = 0; + out_6423431565507769528[12] = 0; + out_6423431565507769528[13] = 0; + out_6423431565507769528[14] = 0; + out_6423431565507769528[15] = 0; + out_6423431565507769528[16] = 0; + out_6423431565507769528[17] = 0; + out_6423431565507769528[18] = 0; + out_6423431565507769528[19] = 0; + out_6423431565507769528[20] = 1; + out_6423431565507769528[21] = 0; + out_6423431565507769528[22] = 0; + out_6423431565507769528[23] = 0; + out_6423431565507769528[24] = 0; + out_6423431565507769528[25] = 0; + out_6423431565507769528[26] = 0; + out_6423431565507769528[27] = 0; + out_6423431565507769528[28] = 0; + out_6423431565507769528[29] = 0; + out_6423431565507769528[30] = 1; + out_6423431565507769528[31] = 0; + out_6423431565507769528[32] = 0; + out_6423431565507769528[33] = 0; + out_6423431565507769528[34] = 0; + out_6423431565507769528[35] = 0; + out_6423431565507769528[36] = 0; + out_6423431565507769528[37] = 0; + out_6423431565507769528[38] = 0; + out_6423431565507769528[39] = 0; + out_6423431565507769528[40] = 1; + out_6423431565507769528[41] = 0; + out_6423431565507769528[42] = 0; + out_6423431565507769528[43] = 0; + out_6423431565507769528[44] = 0; + out_6423431565507769528[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_6423431565507769528[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_6423431565507769528[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_6423431565507769528[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_6423431565507769528[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_6423431565507769528[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_6423431565507769528[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_6423431565507769528[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_6423431565507769528[53] = -9.8100000000000005*dt; + out_6423431565507769528[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_6423431565507769528[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_6423431565507769528[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_6423431565507769528[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_6423431565507769528[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_6423431565507769528[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_6423431565507769528[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_6423431565507769528[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_6423431565507769528[62] = 0; + out_6423431565507769528[63] = 0; + out_6423431565507769528[64] = 0; + out_6423431565507769528[65] = 0; + out_6423431565507769528[66] = 0; + out_6423431565507769528[67] = 0; + out_6423431565507769528[68] = 0; + out_6423431565507769528[69] = 0; + out_6423431565507769528[70] = 1; + out_6423431565507769528[71] = 0; + out_6423431565507769528[72] = 0; + out_6423431565507769528[73] = 0; + out_6423431565507769528[74] = 0; + out_6423431565507769528[75] = 0; + out_6423431565507769528[76] = 0; + out_6423431565507769528[77] = 0; + out_6423431565507769528[78] = 0; + out_6423431565507769528[79] = 0; + out_6423431565507769528[80] = 1; } -void h_25(double *state, double *unused, double *out_9136101134851125551) { - out_9136101134851125551[0] = state[6]; +void h_25(double *state, double *unused, double *out_8281489855210528984) { + out_8281489855210528984[0] = state[6]; } -void H_25(double *state, double *unused, double *out_7713208271076950079) { - out_7713208271076950079[0] = 0; - out_7713208271076950079[1] = 0; - out_7713208271076950079[2] = 0; - out_7713208271076950079[3] = 0; - out_7713208271076950079[4] = 0; - out_7713208271076950079[5] = 0; - out_7713208271076950079[6] = 1; - out_7713208271076950079[7] = 0; - out_7713208271076950079[8] = 0; +void H_25(double *state, double *unused, double *out_257722837092792508) { + out_257722837092792508[0] = 0; + out_257722837092792508[1] = 0; + out_257722837092792508[2] = 0; + out_257722837092792508[3] = 0; + out_257722837092792508[4] = 0; + out_257722837092792508[5] = 0; + out_257722837092792508[6] = 1; + out_257722837092792508[7] = 0; + out_257722837092792508[8] = 0; } -void h_24(double *state, double *unused, double *out_8665340836537612490) { - out_8665340836537612490[0] = state[4]; - out_8665340836537612490[1] = state[5]; +void h_24(double *state, double *unused, double *out_5654195456743599601) { + out_5654195456743599601[0] = state[4]; + out_5654195456743599601[1] = state[5]; } -void H_24(double *state, double *unused, double *out_7175610203830278401) { - out_7175610203830278401[0] = 0; - out_7175610203830278401[1] = 0; - out_7175610203830278401[2] = 0; - out_7175610203830278401[3] = 0; - out_7175610203830278401[4] = 1; - out_7175610203830278401[5] = 0; - out_7175610203830278401[6] = 0; - out_7175610203830278401[7] = 0; - out_7175610203830278401[8] = 0; - out_7175610203830278401[9] = 0; - out_7175610203830278401[10] = 0; - out_7175610203830278401[11] = 0; - out_7175610203830278401[12] = 0; - out_7175610203830278401[13] = 0; - out_7175610203830278401[14] = 1; - out_7175610203830278401[15] = 0; - out_7175610203830278401[16] = 0; - out_7175610203830278401[17] = 0; +void H_24(double *state, double *unused, double *out_2483430621071661070) { + out_2483430621071661070[0] = 0; + out_2483430621071661070[1] = 0; + out_2483430621071661070[2] = 0; + out_2483430621071661070[3] = 0; + out_2483430621071661070[4] = 1; + out_2483430621071661070[5] = 0; + out_2483430621071661070[6] = 0; + out_2483430621071661070[7] = 0; + out_2483430621071661070[8] = 0; + out_2483430621071661070[9] = 0; + out_2483430621071661070[10] = 0; + out_2483430621071661070[11] = 0; + out_2483430621071661070[12] = 0; + out_2483430621071661070[13] = 0; + out_2483430621071661070[14] = 1; + out_2483430621071661070[15] = 0; + out_2483430621071661070[16] = 0; + out_2483430621071661070[17] = 0; } -void h_30(double *state, double *unused, double *out_4641549406917500638) { - out_4641549406917500638[0] = state[4]; +void h_30(double *state, double *unused, double *out_8438676523561658129) { + out_8438676523561658129[0] = state[4]; } -void H_30(double *state, double *unused, double *out_3185511940949341881) { - out_3185511940949341881[0] = 0; - out_3185511940949341881[1] = 0; - out_3185511940949341881[2] = 0; - out_3185511940949341881[3] = 0; - out_3185511940949341881[4] = 1; - out_3185511940949341881[5] = 0; - out_3185511940949341881[6] = 0; - out_3185511940949341881[7] = 0; - out_3185511940949341881[8] = 0; +void H_30(double *state, double *unused, double *out_7174413178584409263) { + out_7174413178584409263[0] = 0; + out_7174413178584409263[1] = 0; + out_7174413178584409263[2] = 0; + out_7174413178584409263[3] = 0; + out_7174413178584409263[4] = 1; + out_7174413178584409263[5] = 0; + out_7174413178584409263[6] = 0; + out_7174413178584409263[7] = 0; + out_7174413178584409263[8] = 0; } -void h_26(double *state, double *unused, double *out_2793594027511200175) { - out_2793594027511200175[0] = state[7]; +void h_26(double *state, double *unused, double *out_1419904868343052079) { + out_1419904868343052079[0] = state[7]; } -void H_26(double *state, double *unused, double *out_3971704952202893855) { - out_3971704952202893855[0] = 0; - out_3971704952202893855[1] = 0; - out_3971704952202893855[2] = 0; - out_3971704952202893855[3] = 0; - out_3971704952202893855[4] = 0; - out_3971704952202893855[5] = 0; - out_3971704952202893855[6] = 0; - out_3971704952202893855[7] = 1; - out_3971704952202893855[8] = 0; +void H_26(double *state, double *unused, double *out_3562248806853593109) { + out_3562248806853593109[0] = 0; + out_3562248806853593109[1] = 0; + out_3562248806853593109[2] = 0; + out_3562248806853593109[3] = 0; + out_3562248806853593109[4] = 0; + out_3562248806853593109[5] = 0; + out_3562248806853593109[6] = 0; + out_3562248806853593109[7] = 1; + out_3562248806853593109[8] = 0; } -void h_27(double *state, double *unused, double *out_5860253322103668962) { - out_5860253322103668962[0] = state[3]; +void h_27(double *state, double *unused, double *out_1905364559686504598) { + out_1905364559686504598[0] = state[3]; } -void H_27(double *state, double *unused, double *out_5409106012133285098) { - out_5409106012133285098[0] = 0; - out_5409106012133285098[1] = 0; - out_5409106012133285098[2] = 0; - out_5409106012133285098[3] = 1; - out_5409106012133285098[4] = 0; - out_5409106012133285098[5] = 0; - out_5409106012133285098[6] = 0; - out_5409106012133285098[7] = 0; - out_5409106012133285098[8] = 0; +void H_27(double *state, double *unused, double *out_4999649866783984352) { + out_4999649866783984352[0] = 0; + out_4999649866783984352[1] = 0; + out_4999649866783984352[2] = 0; + out_4999649866783984352[3] = 1; + out_4999649866783984352[4] = 0; + out_4999649866783984352[5] = 0; + out_4999649866783984352[6] = 0; + out_4999649866783984352[7] = 0; + out_4999649866783984352[8] = 0; } -void h_29(double *state, double *unused, double *out_5585059259819163073) { - out_5585059259819163073[0] = state[1]; +void h_29(double *state, double *unused, double *out_3005304846354827708) { + out_3005304846354827708[0] = state[1]; } -void H_29(double *state, double *unused, double *out_3695743285263734065) { - out_3695743285263734065[0] = 0; - out_3695743285263734065[1] = 1; - out_3695743285263734065[2] = 0; - out_3695743285263734065[3] = 0; - out_3695743285263734065[4] = 0; - out_3695743285263734065[5] = 0; - out_3695743285263734065[6] = 0; - out_3695743285263734065[7] = 0; - out_3695743285263734065[8] = 0; +void H_29(double *state, double *unused, double *out_7684644522898801447) { + out_7684644522898801447[0] = 0; + out_7684644522898801447[1] = 1; + out_7684644522898801447[2] = 0; + out_7684644522898801447[3] = 0; + out_7684644522898801447[4] = 0; + out_7684644522898801447[5] = 0; + out_7684644522898801447[6] = 0; + out_7684644522898801447[7] = 0; + out_7684644522898801447[8] = 0; } -void h_28(double *state, double *unused, double *out_6187449234192033508) { - out_6187449234192033508[0] = state[0]; +void h_28(double *state, double *unused, double *out_8832630219803516209) { + out_8832630219803516209[0] = state[0]; } -void H_28(double *state, double *unused, double *out_1386655731805796509) { - out_1386655731805796509[0] = 1; - out_1386655731805796509[1] = 0; - out_1386655731805796509[2] = 0; - out_1386655731805796509[3] = 0; - out_1386655731805796509[4] = 0; - out_1386655731805796509[5] = 0; - out_1386655731805796509[6] = 0; - out_1386655731805796509[7] = 0; - out_1386655731805796509[8] = 0; +void H_28(double *state, double *unused, double *out_1796111877155097255) { + out_1796111877155097255[0] = 1; + out_1796111877155097255[1] = 0; + out_1796111877155097255[2] = 0; + out_1796111877155097255[3] = 0; + out_1796111877155097255[4] = 0; + out_1796111877155097255[5] = 0; + out_1796111877155097255[6] = 0; + out_1796111877155097255[7] = 0; + out_1796111877155097255[8] = 0; } -void h_31(double *state, double *unused, double *out_2365265908500774615) { - out_2365265908500774615[0] = state[8]; +void h_31(double *state, double *unused, double *out_6225098197673216166) { + out_6225098197673216166[0] = state[8]; } -void H_31(double *state, double *unused, double *out_7743854232953910507) { - out_7743854232953910507[0] = 0; - out_7743854232953910507[1] = 0; - out_7743854232953910507[2] = 0; - out_7743854232953910507[3] = 0; - out_7743854232953910507[4] = 0; - out_7743854232953910507[5] = 0; - out_7743854232953910507[6] = 0; - out_7743854232953910507[7] = 0; - out_7743854232953910507[8] = 1; +void H_31(double *state, double *unused, double *out_7334398087604609761) { + out_7334398087604609761[0] = 0; + out_7334398087604609761[1] = 0; + out_7334398087604609761[2] = 0; + out_7334398087604609761[3] = 0; + out_7334398087604609761[4] = 0; + out_7334398087604609761[5] = 0; + out_7334398087604609761[6] = 0; + out_7334398087604609761[7] = 0; + out_7334398087604609761[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_3321760492551468366) { - err_fun(nom_x, delta_x, out_3321760492551468366); +void car_err_fun(double *nom_x, double *delta_x, double *out_760253992355356602) { + err_fun(nom_x, delta_x, out_760253992355356602); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3697485090417495632) { - inv_err_fun(nom_x, true_x, out_3697485090417495632); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_4983548601721046404) { + inv_err_fun(nom_x, true_x, out_4983548601721046404); } -void car_H_mod_fun(double *state, double *out_4190227621087458402) { - H_mod_fun(state, out_4190227621087458402); +void car_H_mod_fun(double *state, double *out_8995833010704521871) { + H_mod_fun(state, out_8995833010704521871); } -void car_f_fun(double *state, double dt, double *out_6456348943274406543) { - f_fun(state, dt, out_6456348943274406543); +void car_f_fun(double *state, double dt, double *out_4562655195590537984) { + f_fun(state, dt, out_4562655195590537984); } -void car_F_fun(double *state, double dt, double *out_4201765287475637381) { - F_fun(state, dt, out_4201765287475637381); +void car_F_fun(double *state, double dt, double *out_6423431565507769528) { + F_fun(state, dt, out_6423431565507769528); } -void car_h_25(double *state, double *unused, double *out_9136101134851125551) { - h_25(state, unused, out_9136101134851125551); +void car_h_25(double *state, double *unused, double *out_8281489855210528984) { + h_25(state, unused, out_8281489855210528984); } -void car_H_25(double *state, double *unused, double *out_7713208271076950079) { - H_25(state, unused, out_7713208271076950079); +void car_H_25(double *state, double *unused, double *out_257722837092792508) { + H_25(state, unused, out_257722837092792508); } -void car_h_24(double *state, double *unused, double *out_8665340836537612490) { - h_24(state, unused, out_8665340836537612490); +void car_h_24(double *state, double *unused, double *out_5654195456743599601) { + h_24(state, unused, out_5654195456743599601); } -void car_H_24(double *state, double *unused, double *out_7175610203830278401) { - H_24(state, unused, out_7175610203830278401); +void car_H_24(double *state, double *unused, double *out_2483430621071661070) { + H_24(state, unused, out_2483430621071661070); } -void car_h_30(double *state, double *unused, double *out_4641549406917500638) { - h_30(state, unused, out_4641549406917500638); +void car_h_30(double *state, double *unused, double *out_8438676523561658129) { + h_30(state, unused, out_8438676523561658129); } -void car_H_30(double *state, double *unused, double *out_3185511940949341881) { - H_30(state, unused, out_3185511940949341881); +void car_H_30(double *state, double *unused, double *out_7174413178584409263) { + H_30(state, unused, out_7174413178584409263); } -void car_h_26(double *state, double *unused, double *out_2793594027511200175) { - h_26(state, unused, out_2793594027511200175); +void car_h_26(double *state, double *unused, double *out_1419904868343052079) { + h_26(state, unused, out_1419904868343052079); } -void car_H_26(double *state, double *unused, double *out_3971704952202893855) { - H_26(state, unused, out_3971704952202893855); +void car_H_26(double *state, double *unused, double *out_3562248806853593109) { + H_26(state, unused, out_3562248806853593109); } -void car_h_27(double *state, double *unused, double *out_5860253322103668962) { - h_27(state, unused, out_5860253322103668962); +void car_h_27(double *state, double *unused, double *out_1905364559686504598) { + h_27(state, unused, out_1905364559686504598); } -void car_H_27(double *state, double *unused, double *out_5409106012133285098) { - H_27(state, unused, out_5409106012133285098); +void car_H_27(double *state, double *unused, double *out_4999649866783984352) { + H_27(state, unused, out_4999649866783984352); } -void car_h_29(double *state, double *unused, double *out_5585059259819163073) { - h_29(state, unused, out_5585059259819163073); +void car_h_29(double *state, double *unused, double *out_3005304846354827708) { + h_29(state, unused, out_3005304846354827708); } -void car_H_29(double *state, double *unused, double *out_3695743285263734065) { - H_29(state, unused, out_3695743285263734065); +void car_H_29(double *state, double *unused, double *out_7684644522898801447) { + H_29(state, unused, out_7684644522898801447); } -void car_h_28(double *state, double *unused, double *out_6187449234192033508) { - h_28(state, unused, out_6187449234192033508); +void car_h_28(double *state, double *unused, double *out_8832630219803516209) { + h_28(state, unused, out_8832630219803516209); } -void car_H_28(double *state, double *unused, double *out_1386655731805796509) { - H_28(state, unused, out_1386655731805796509); +void car_H_28(double *state, double *unused, double *out_1796111877155097255) { + H_28(state, unused, out_1796111877155097255); } -void car_h_31(double *state, double *unused, double *out_2365265908500774615) { - h_31(state, unused, out_2365265908500774615); +void car_h_31(double *state, double *unused, double *out_6225098197673216166) { + h_31(state, unused, out_6225098197673216166); } -void car_H_31(double *state, double *unused, double *out_7743854232953910507) { - H_31(state, unused, out_7743854232953910507); +void car_H_31(double *state, double *unused, double *out_7334398087604609761) { + H_31(state, unused, out_7334398087604609761); } 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 8e602ba77..877bd06b3 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_3321760492551468366); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3697485090417495632); -void car_H_mod_fun(double *state, double *out_4190227621087458402); -void car_f_fun(double *state, double dt, double *out_6456348943274406543); -void car_F_fun(double *state, double dt, double *out_4201765287475637381); -void car_h_25(double *state, double *unused, double *out_9136101134851125551); -void car_H_25(double *state, double *unused, double *out_7713208271076950079); -void car_h_24(double *state, double *unused, double *out_8665340836537612490); -void car_H_24(double *state, double *unused, double *out_7175610203830278401); -void car_h_30(double *state, double *unused, double *out_4641549406917500638); -void car_H_30(double *state, double *unused, double *out_3185511940949341881); -void car_h_26(double *state, double *unused, double *out_2793594027511200175); -void car_H_26(double *state, double *unused, double *out_3971704952202893855); -void car_h_27(double *state, double *unused, double *out_5860253322103668962); -void car_H_27(double *state, double *unused, double *out_5409106012133285098); -void car_h_29(double *state, double *unused, double *out_5585059259819163073); -void car_H_29(double *state, double *unused, double *out_3695743285263734065); -void car_h_28(double *state, double *unused, double *out_6187449234192033508); -void car_H_28(double *state, double *unused, double *out_1386655731805796509); -void car_h_31(double *state, double *unused, double *out_2365265908500774615); -void car_H_31(double *state, double *unused, double *out_7743854232953910507); +void car_err_fun(double *nom_x, double *delta_x, double *out_760253992355356602); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_4983548601721046404); +void car_H_mod_fun(double *state, double *out_8995833010704521871); +void car_f_fun(double *state, double dt, double *out_4562655195590537984); +void car_F_fun(double *state, double dt, double *out_6423431565507769528); +void car_h_25(double *state, double *unused, double *out_8281489855210528984); +void car_H_25(double *state, double *unused, double *out_257722837092792508); +void car_h_24(double *state, double *unused, double *out_5654195456743599601); +void car_H_24(double *state, double *unused, double *out_2483430621071661070); +void car_h_30(double *state, double *unused, double *out_8438676523561658129); +void car_H_30(double *state, double *unused, double *out_7174413178584409263); +void car_h_26(double *state, double *unused, double *out_1419904868343052079); +void car_H_26(double *state, double *unused, double *out_3562248806853593109); +void car_h_27(double *state, double *unused, double *out_1905364559686504598); +void car_H_27(double *state, double *unused, double *out_4999649866783984352); +void car_h_29(double *state, double *unused, double *out_3005304846354827708); +void car_H_29(double *state, double *unused, double *out_7684644522898801447); +void car_h_28(double *state, double *unused, double *out_8832630219803516209); +void car_H_28(double *state, double *unused, double *out_1796111877155097255); +void car_h_31(double *state, double *unused, double *out_6225098197673216166); +void car_H_31(double *state, double *unused, double *out_7334398087604609761); 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 5a086344a..e73bbd6a8 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_6247163002975687744) { - out_6247163002975687744[0] = delta_x[0] + nom_x[0]; - out_6247163002975687744[1] = delta_x[1] + nom_x[1]; - out_6247163002975687744[2] = delta_x[2] + nom_x[2]; - out_6247163002975687744[3] = delta_x[3] + nom_x[3]; - out_6247163002975687744[4] = delta_x[4] + nom_x[4]; - out_6247163002975687744[5] = delta_x[5] + nom_x[5]; - out_6247163002975687744[6] = delta_x[6] + nom_x[6]; - out_6247163002975687744[7] = delta_x[7] + nom_x[7]; - out_6247163002975687744[8] = delta_x[8] + nom_x[8]; - out_6247163002975687744[9] = delta_x[9] + nom_x[9]; - out_6247163002975687744[10] = delta_x[10] + nom_x[10]; - out_6247163002975687744[11] = delta_x[11] + nom_x[11]; - out_6247163002975687744[12] = delta_x[12] + nom_x[12]; - out_6247163002975687744[13] = delta_x[13] + nom_x[13]; - out_6247163002975687744[14] = delta_x[14] + nom_x[14]; - out_6247163002975687744[15] = delta_x[15] + nom_x[15]; - out_6247163002975687744[16] = delta_x[16] + nom_x[16]; - out_6247163002975687744[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7113141565155944071) { + out_7113141565155944071[0] = delta_x[0] + nom_x[0]; + out_7113141565155944071[1] = delta_x[1] + nom_x[1]; + out_7113141565155944071[2] = delta_x[2] + nom_x[2]; + out_7113141565155944071[3] = delta_x[3] + nom_x[3]; + out_7113141565155944071[4] = delta_x[4] + nom_x[4]; + out_7113141565155944071[5] = delta_x[5] + nom_x[5]; + out_7113141565155944071[6] = delta_x[6] + nom_x[6]; + out_7113141565155944071[7] = delta_x[7] + nom_x[7]; + out_7113141565155944071[8] = delta_x[8] + nom_x[8]; + out_7113141565155944071[9] = delta_x[9] + nom_x[9]; + out_7113141565155944071[10] = delta_x[10] + nom_x[10]; + out_7113141565155944071[11] = delta_x[11] + nom_x[11]; + out_7113141565155944071[12] = delta_x[12] + nom_x[12]; + out_7113141565155944071[13] = delta_x[13] + nom_x[13]; + out_7113141565155944071[14] = delta_x[14] + nom_x[14]; + out_7113141565155944071[15] = delta_x[15] + nom_x[15]; + out_7113141565155944071[16] = delta_x[16] + nom_x[16]; + out_7113141565155944071[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_2784495282432373016) { - out_2784495282432373016[0] = -nom_x[0] + true_x[0]; - out_2784495282432373016[1] = -nom_x[1] + true_x[1]; - out_2784495282432373016[2] = -nom_x[2] + true_x[2]; - out_2784495282432373016[3] = -nom_x[3] + true_x[3]; - out_2784495282432373016[4] = -nom_x[4] + true_x[4]; - out_2784495282432373016[5] = -nom_x[5] + true_x[5]; - out_2784495282432373016[6] = -nom_x[6] + true_x[6]; - out_2784495282432373016[7] = -nom_x[7] + true_x[7]; - out_2784495282432373016[8] = -nom_x[8] + true_x[8]; - out_2784495282432373016[9] = -nom_x[9] + true_x[9]; - out_2784495282432373016[10] = -nom_x[10] + true_x[10]; - out_2784495282432373016[11] = -nom_x[11] + true_x[11]; - out_2784495282432373016[12] = -nom_x[12] + true_x[12]; - out_2784495282432373016[13] = -nom_x[13] + true_x[13]; - out_2784495282432373016[14] = -nom_x[14] + true_x[14]; - out_2784495282432373016[15] = -nom_x[15] + true_x[15]; - out_2784495282432373016[16] = -nom_x[16] + true_x[16]; - out_2784495282432373016[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_4273806753012627861) { + out_4273806753012627861[0] = -nom_x[0] + true_x[0]; + out_4273806753012627861[1] = -nom_x[1] + true_x[1]; + out_4273806753012627861[2] = -nom_x[2] + true_x[2]; + out_4273806753012627861[3] = -nom_x[3] + true_x[3]; + out_4273806753012627861[4] = -nom_x[4] + true_x[4]; + out_4273806753012627861[5] = -nom_x[5] + true_x[5]; + out_4273806753012627861[6] = -nom_x[6] + true_x[6]; + out_4273806753012627861[7] = -nom_x[7] + true_x[7]; + out_4273806753012627861[8] = -nom_x[8] + true_x[8]; + out_4273806753012627861[9] = -nom_x[9] + true_x[9]; + out_4273806753012627861[10] = -nom_x[10] + true_x[10]; + out_4273806753012627861[11] = -nom_x[11] + true_x[11]; + out_4273806753012627861[12] = -nom_x[12] + true_x[12]; + out_4273806753012627861[13] = -nom_x[13] + true_x[13]; + out_4273806753012627861[14] = -nom_x[14] + true_x[14]; + out_4273806753012627861[15] = -nom_x[15] + true_x[15]; + out_4273806753012627861[16] = -nom_x[16] + true_x[16]; + out_4273806753012627861[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_662979392671356586) { - out_662979392671356586[0] = 1.0; - out_662979392671356586[1] = 0.0; - out_662979392671356586[2] = 0.0; - out_662979392671356586[3] = 0.0; - out_662979392671356586[4] = 0.0; - out_662979392671356586[5] = 0.0; - out_662979392671356586[6] = 0.0; - out_662979392671356586[7] = 0.0; - out_662979392671356586[8] = 0.0; - out_662979392671356586[9] = 0.0; - out_662979392671356586[10] = 0.0; - out_662979392671356586[11] = 0.0; - out_662979392671356586[12] = 0.0; - out_662979392671356586[13] = 0.0; - out_662979392671356586[14] = 0.0; - out_662979392671356586[15] = 0.0; - out_662979392671356586[16] = 0.0; - out_662979392671356586[17] = 0.0; - out_662979392671356586[18] = 0.0; - out_662979392671356586[19] = 1.0; - out_662979392671356586[20] = 0.0; - out_662979392671356586[21] = 0.0; - out_662979392671356586[22] = 0.0; - out_662979392671356586[23] = 0.0; - out_662979392671356586[24] = 0.0; - out_662979392671356586[25] = 0.0; - out_662979392671356586[26] = 0.0; - out_662979392671356586[27] = 0.0; - out_662979392671356586[28] = 0.0; - out_662979392671356586[29] = 0.0; - out_662979392671356586[30] = 0.0; - out_662979392671356586[31] = 0.0; - out_662979392671356586[32] = 0.0; - out_662979392671356586[33] = 0.0; - out_662979392671356586[34] = 0.0; - out_662979392671356586[35] = 0.0; - out_662979392671356586[36] = 0.0; - out_662979392671356586[37] = 0.0; - out_662979392671356586[38] = 1.0; - out_662979392671356586[39] = 0.0; - out_662979392671356586[40] = 0.0; - out_662979392671356586[41] = 0.0; - out_662979392671356586[42] = 0.0; - out_662979392671356586[43] = 0.0; - out_662979392671356586[44] = 0.0; - out_662979392671356586[45] = 0.0; - out_662979392671356586[46] = 0.0; - out_662979392671356586[47] = 0.0; - out_662979392671356586[48] = 0.0; - out_662979392671356586[49] = 0.0; - out_662979392671356586[50] = 0.0; - out_662979392671356586[51] = 0.0; - out_662979392671356586[52] = 0.0; - out_662979392671356586[53] = 0.0; - out_662979392671356586[54] = 0.0; - out_662979392671356586[55] = 0.0; - out_662979392671356586[56] = 0.0; - out_662979392671356586[57] = 1.0; - out_662979392671356586[58] = 0.0; - out_662979392671356586[59] = 0.0; - out_662979392671356586[60] = 0.0; - out_662979392671356586[61] = 0.0; - out_662979392671356586[62] = 0.0; - out_662979392671356586[63] = 0.0; - out_662979392671356586[64] = 0.0; - out_662979392671356586[65] = 0.0; - out_662979392671356586[66] = 0.0; - out_662979392671356586[67] = 0.0; - out_662979392671356586[68] = 0.0; - out_662979392671356586[69] = 0.0; - out_662979392671356586[70] = 0.0; - out_662979392671356586[71] = 0.0; - out_662979392671356586[72] = 0.0; - out_662979392671356586[73] = 0.0; - out_662979392671356586[74] = 0.0; - out_662979392671356586[75] = 0.0; - out_662979392671356586[76] = 1.0; - out_662979392671356586[77] = 0.0; - out_662979392671356586[78] = 0.0; - out_662979392671356586[79] = 0.0; - out_662979392671356586[80] = 0.0; - out_662979392671356586[81] = 0.0; - out_662979392671356586[82] = 0.0; - out_662979392671356586[83] = 0.0; - out_662979392671356586[84] = 0.0; - out_662979392671356586[85] = 0.0; - out_662979392671356586[86] = 0.0; - out_662979392671356586[87] = 0.0; - out_662979392671356586[88] = 0.0; - out_662979392671356586[89] = 0.0; - out_662979392671356586[90] = 0.0; - out_662979392671356586[91] = 0.0; - out_662979392671356586[92] = 0.0; - out_662979392671356586[93] = 0.0; - out_662979392671356586[94] = 0.0; - out_662979392671356586[95] = 1.0; - out_662979392671356586[96] = 0.0; - out_662979392671356586[97] = 0.0; - out_662979392671356586[98] = 0.0; - out_662979392671356586[99] = 0.0; - out_662979392671356586[100] = 0.0; - out_662979392671356586[101] = 0.0; - out_662979392671356586[102] = 0.0; - out_662979392671356586[103] = 0.0; - out_662979392671356586[104] = 0.0; - out_662979392671356586[105] = 0.0; - out_662979392671356586[106] = 0.0; - out_662979392671356586[107] = 0.0; - out_662979392671356586[108] = 0.0; - out_662979392671356586[109] = 0.0; - out_662979392671356586[110] = 0.0; - out_662979392671356586[111] = 0.0; - out_662979392671356586[112] = 0.0; - out_662979392671356586[113] = 0.0; - out_662979392671356586[114] = 1.0; - out_662979392671356586[115] = 0.0; - out_662979392671356586[116] = 0.0; - out_662979392671356586[117] = 0.0; - out_662979392671356586[118] = 0.0; - out_662979392671356586[119] = 0.0; - out_662979392671356586[120] = 0.0; - out_662979392671356586[121] = 0.0; - out_662979392671356586[122] = 0.0; - out_662979392671356586[123] = 0.0; - out_662979392671356586[124] = 0.0; - out_662979392671356586[125] = 0.0; - out_662979392671356586[126] = 0.0; - out_662979392671356586[127] = 0.0; - out_662979392671356586[128] = 0.0; - out_662979392671356586[129] = 0.0; - out_662979392671356586[130] = 0.0; - out_662979392671356586[131] = 0.0; - out_662979392671356586[132] = 0.0; - out_662979392671356586[133] = 1.0; - out_662979392671356586[134] = 0.0; - out_662979392671356586[135] = 0.0; - out_662979392671356586[136] = 0.0; - out_662979392671356586[137] = 0.0; - out_662979392671356586[138] = 0.0; - out_662979392671356586[139] = 0.0; - out_662979392671356586[140] = 0.0; - out_662979392671356586[141] = 0.0; - out_662979392671356586[142] = 0.0; - out_662979392671356586[143] = 0.0; - out_662979392671356586[144] = 0.0; - out_662979392671356586[145] = 0.0; - out_662979392671356586[146] = 0.0; - out_662979392671356586[147] = 0.0; - out_662979392671356586[148] = 0.0; - out_662979392671356586[149] = 0.0; - out_662979392671356586[150] = 0.0; - out_662979392671356586[151] = 0.0; - out_662979392671356586[152] = 1.0; - out_662979392671356586[153] = 0.0; - out_662979392671356586[154] = 0.0; - out_662979392671356586[155] = 0.0; - out_662979392671356586[156] = 0.0; - out_662979392671356586[157] = 0.0; - out_662979392671356586[158] = 0.0; - out_662979392671356586[159] = 0.0; - out_662979392671356586[160] = 0.0; - out_662979392671356586[161] = 0.0; - out_662979392671356586[162] = 0.0; - out_662979392671356586[163] = 0.0; - out_662979392671356586[164] = 0.0; - out_662979392671356586[165] = 0.0; - out_662979392671356586[166] = 0.0; - out_662979392671356586[167] = 0.0; - out_662979392671356586[168] = 0.0; - out_662979392671356586[169] = 0.0; - out_662979392671356586[170] = 0.0; - out_662979392671356586[171] = 1.0; - out_662979392671356586[172] = 0.0; - out_662979392671356586[173] = 0.0; - out_662979392671356586[174] = 0.0; - out_662979392671356586[175] = 0.0; - out_662979392671356586[176] = 0.0; - out_662979392671356586[177] = 0.0; - out_662979392671356586[178] = 0.0; - out_662979392671356586[179] = 0.0; - out_662979392671356586[180] = 0.0; - out_662979392671356586[181] = 0.0; - out_662979392671356586[182] = 0.0; - out_662979392671356586[183] = 0.0; - out_662979392671356586[184] = 0.0; - out_662979392671356586[185] = 0.0; - out_662979392671356586[186] = 0.0; - out_662979392671356586[187] = 0.0; - out_662979392671356586[188] = 0.0; - out_662979392671356586[189] = 0.0; - out_662979392671356586[190] = 1.0; - out_662979392671356586[191] = 0.0; - out_662979392671356586[192] = 0.0; - out_662979392671356586[193] = 0.0; - out_662979392671356586[194] = 0.0; - out_662979392671356586[195] = 0.0; - out_662979392671356586[196] = 0.0; - out_662979392671356586[197] = 0.0; - out_662979392671356586[198] = 0.0; - out_662979392671356586[199] = 0.0; - out_662979392671356586[200] = 0.0; - out_662979392671356586[201] = 0.0; - out_662979392671356586[202] = 0.0; - out_662979392671356586[203] = 0.0; - out_662979392671356586[204] = 0.0; - out_662979392671356586[205] = 0.0; - out_662979392671356586[206] = 0.0; - out_662979392671356586[207] = 0.0; - out_662979392671356586[208] = 0.0; - out_662979392671356586[209] = 1.0; - out_662979392671356586[210] = 0.0; - out_662979392671356586[211] = 0.0; - out_662979392671356586[212] = 0.0; - out_662979392671356586[213] = 0.0; - out_662979392671356586[214] = 0.0; - out_662979392671356586[215] = 0.0; - out_662979392671356586[216] = 0.0; - out_662979392671356586[217] = 0.0; - out_662979392671356586[218] = 0.0; - out_662979392671356586[219] = 0.0; - out_662979392671356586[220] = 0.0; - out_662979392671356586[221] = 0.0; - out_662979392671356586[222] = 0.0; - out_662979392671356586[223] = 0.0; - out_662979392671356586[224] = 0.0; - out_662979392671356586[225] = 0.0; - out_662979392671356586[226] = 0.0; - out_662979392671356586[227] = 0.0; - out_662979392671356586[228] = 1.0; - out_662979392671356586[229] = 0.0; - out_662979392671356586[230] = 0.0; - out_662979392671356586[231] = 0.0; - out_662979392671356586[232] = 0.0; - out_662979392671356586[233] = 0.0; - out_662979392671356586[234] = 0.0; - out_662979392671356586[235] = 0.0; - out_662979392671356586[236] = 0.0; - out_662979392671356586[237] = 0.0; - out_662979392671356586[238] = 0.0; - out_662979392671356586[239] = 0.0; - out_662979392671356586[240] = 0.0; - out_662979392671356586[241] = 0.0; - out_662979392671356586[242] = 0.0; - out_662979392671356586[243] = 0.0; - out_662979392671356586[244] = 0.0; - out_662979392671356586[245] = 0.0; - out_662979392671356586[246] = 0.0; - out_662979392671356586[247] = 1.0; - out_662979392671356586[248] = 0.0; - out_662979392671356586[249] = 0.0; - out_662979392671356586[250] = 0.0; - out_662979392671356586[251] = 0.0; - out_662979392671356586[252] = 0.0; - out_662979392671356586[253] = 0.0; - out_662979392671356586[254] = 0.0; - out_662979392671356586[255] = 0.0; - out_662979392671356586[256] = 0.0; - out_662979392671356586[257] = 0.0; - out_662979392671356586[258] = 0.0; - out_662979392671356586[259] = 0.0; - out_662979392671356586[260] = 0.0; - out_662979392671356586[261] = 0.0; - out_662979392671356586[262] = 0.0; - out_662979392671356586[263] = 0.0; - out_662979392671356586[264] = 0.0; - out_662979392671356586[265] = 0.0; - out_662979392671356586[266] = 1.0; - out_662979392671356586[267] = 0.0; - out_662979392671356586[268] = 0.0; - out_662979392671356586[269] = 0.0; - out_662979392671356586[270] = 0.0; - out_662979392671356586[271] = 0.0; - out_662979392671356586[272] = 0.0; - out_662979392671356586[273] = 0.0; - out_662979392671356586[274] = 0.0; - out_662979392671356586[275] = 0.0; - out_662979392671356586[276] = 0.0; - out_662979392671356586[277] = 0.0; - out_662979392671356586[278] = 0.0; - out_662979392671356586[279] = 0.0; - out_662979392671356586[280] = 0.0; - out_662979392671356586[281] = 0.0; - out_662979392671356586[282] = 0.0; - out_662979392671356586[283] = 0.0; - out_662979392671356586[284] = 0.0; - out_662979392671356586[285] = 1.0; - out_662979392671356586[286] = 0.0; - out_662979392671356586[287] = 0.0; - out_662979392671356586[288] = 0.0; - out_662979392671356586[289] = 0.0; - out_662979392671356586[290] = 0.0; - out_662979392671356586[291] = 0.0; - out_662979392671356586[292] = 0.0; - out_662979392671356586[293] = 0.0; - out_662979392671356586[294] = 0.0; - out_662979392671356586[295] = 0.0; - out_662979392671356586[296] = 0.0; - out_662979392671356586[297] = 0.0; - out_662979392671356586[298] = 0.0; - out_662979392671356586[299] = 0.0; - out_662979392671356586[300] = 0.0; - out_662979392671356586[301] = 0.0; - out_662979392671356586[302] = 0.0; - out_662979392671356586[303] = 0.0; - out_662979392671356586[304] = 1.0; - out_662979392671356586[305] = 0.0; - out_662979392671356586[306] = 0.0; - out_662979392671356586[307] = 0.0; - out_662979392671356586[308] = 0.0; - out_662979392671356586[309] = 0.0; - out_662979392671356586[310] = 0.0; - out_662979392671356586[311] = 0.0; - out_662979392671356586[312] = 0.0; - out_662979392671356586[313] = 0.0; - out_662979392671356586[314] = 0.0; - out_662979392671356586[315] = 0.0; - out_662979392671356586[316] = 0.0; - out_662979392671356586[317] = 0.0; - out_662979392671356586[318] = 0.0; - out_662979392671356586[319] = 0.0; - out_662979392671356586[320] = 0.0; - out_662979392671356586[321] = 0.0; - out_662979392671356586[322] = 0.0; - out_662979392671356586[323] = 1.0; +void H_mod_fun(double *state, double *out_4327706851715669235) { + out_4327706851715669235[0] = 1.0; + out_4327706851715669235[1] = 0.0; + out_4327706851715669235[2] = 0.0; + out_4327706851715669235[3] = 0.0; + out_4327706851715669235[4] = 0.0; + out_4327706851715669235[5] = 0.0; + out_4327706851715669235[6] = 0.0; + out_4327706851715669235[7] = 0.0; + out_4327706851715669235[8] = 0.0; + out_4327706851715669235[9] = 0.0; + out_4327706851715669235[10] = 0.0; + out_4327706851715669235[11] = 0.0; + out_4327706851715669235[12] = 0.0; + out_4327706851715669235[13] = 0.0; + out_4327706851715669235[14] = 0.0; + out_4327706851715669235[15] = 0.0; + out_4327706851715669235[16] = 0.0; + out_4327706851715669235[17] = 0.0; + out_4327706851715669235[18] = 0.0; + out_4327706851715669235[19] = 1.0; + out_4327706851715669235[20] = 0.0; + out_4327706851715669235[21] = 0.0; + out_4327706851715669235[22] = 0.0; + out_4327706851715669235[23] = 0.0; + out_4327706851715669235[24] = 0.0; + out_4327706851715669235[25] = 0.0; + out_4327706851715669235[26] = 0.0; + out_4327706851715669235[27] = 0.0; + out_4327706851715669235[28] = 0.0; + out_4327706851715669235[29] = 0.0; + out_4327706851715669235[30] = 0.0; + out_4327706851715669235[31] = 0.0; + out_4327706851715669235[32] = 0.0; + out_4327706851715669235[33] = 0.0; + out_4327706851715669235[34] = 0.0; + out_4327706851715669235[35] = 0.0; + out_4327706851715669235[36] = 0.0; + out_4327706851715669235[37] = 0.0; + out_4327706851715669235[38] = 1.0; + out_4327706851715669235[39] = 0.0; + out_4327706851715669235[40] = 0.0; + out_4327706851715669235[41] = 0.0; + out_4327706851715669235[42] = 0.0; + out_4327706851715669235[43] = 0.0; + out_4327706851715669235[44] = 0.0; + out_4327706851715669235[45] = 0.0; + out_4327706851715669235[46] = 0.0; + out_4327706851715669235[47] = 0.0; + out_4327706851715669235[48] = 0.0; + out_4327706851715669235[49] = 0.0; + out_4327706851715669235[50] = 0.0; + out_4327706851715669235[51] = 0.0; + out_4327706851715669235[52] = 0.0; + out_4327706851715669235[53] = 0.0; + out_4327706851715669235[54] = 0.0; + out_4327706851715669235[55] = 0.0; + out_4327706851715669235[56] = 0.0; + out_4327706851715669235[57] = 1.0; + out_4327706851715669235[58] = 0.0; + out_4327706851715669235[59] = 0.0; + out_4327706851715669235[60] = 0.0; + out_4327706851715669235[61] = 0.0; + out_4327706851715669235[62] = 0.0; + out_4327706851715669235[63] = 0.0; + out_4327706851715669235[64] = 0.0; + out_4327706851715669235[65] = 0.0; + out_4327706851715669235[66] = 0.0; + out_4327706851715669235[67] = 0.0; + out_4327706851715669235[68] = 0.0; + out_4327706851715669235[69] = 0.0; + out_4327706851715669235[70] = 0.0; + out_4327706851715669235[71] = 0.0; + out_4327706851715669235[72] = 0.0; + out_4327706851715669235[73] = 0.0; + out_4327706851715669235[74] = 0.0; + out_4327706851715669235[75] = 0.0; + out_4327706851715669235[76] = 1.0; + out_4327706851715669235[77] = 0.0; + out_4327706851715669235[78] = 0.0; + out_4327706851715669235[79] = 0.0; + out_4327706851715669235[80] = 0.0; + out_4327706851715669235[81] = 0.0; + out_4327706851715669235[82] = 0.0; + out_4327706851715669235[83] = 0.0; + out_4327706851715669235[84] = 0.0; + out_4327706851715669235[85] = 0.0; + out_4327706851715669235[86] = 0.0; + out_4327706851715669235[87] = 0.0; + out_4327706851715669235[88] = 0.0; + out_4327706851715669235[89] = 0.0; + out_4327706851715669235[90] = 0.0; + out_4327706851715669235[91] = 0.0; + out_4327706851715669235[92] = 0.0; + out_4327706851715669235[93] = 0.0; + out_4327706851715669235[94] = 0.0; + out_4327706851715669235[95] = 1.0; + out_4327706851715669235[96] = 0.0; + out_4327706851715669235[97] = 0.0; + out_4327706851715669235[98] = 0.0; + out_4327706851715669235[99] = 0.0; + out_4327706851715669235[100] = 0.0; + out_4327706851715669235[101] = 0.0; + out_4327706851715669235[102] = 0.0; + out_4327706851715669235[103] = 0.0; + out_4327706851715669235[104] = 0.0; + out_4327706851715669235[105] = 0.0; + out_4327706851715669235[106] = 0.0; + out_4327706851715669235[107] = 0.0; + out_4327706851715669235[108] = 0.0; + out_4327706851715669235[109] = 0.0; + out_4327706851715669235[110] = 0.0; + out_4327706851715669235[111] = 0.0; + out_4327706851715669235[112] = 0.0; + out_4327706851715669235[113] = 0.0; + out_4327706851715669235[114] = 1.0; + out_4327706851715669235[115] = 0.0; + out_4327706851715669235[116] = 0.0; + out_4327706851715669235[117] = 0.0; + out_4327706851715669235[118] = 0.0; + out_4327706851715669235[119] = 0.0; + out_4327706851715669235[120] = 0.0; + out_4327706851715669235[121] = 0.0; + out_4327706851715669235[122] = 0.0; + out_4327706851715669235[123] = 0.0; + out_4327706851715669235[124] = 0.0; + out_4327706851715669235[125] = 0.0; + out_4327706851715669235[126] = 0.0; + out_4327706851715669235[127] = 0.0; + out_4327706851715669235[128] = 0.0; + out_4327706851715669235[129] = 0.0; + out_4327706851715669235[130] = 0.0; + out_4327706851715669235[131] = 0.0; + out_4327706851715669235[132] = 0.0; + out_4327706851715669235[133] = 1.0; + out_4327706851715669235[134] = 0.0; + out_4327706851715669235[135] = 0.0; + out_4327706851715669235[136] = 0.0; + out_4327706851715669235[137] = 0.0; + out_4327706851715669235[138] = 0.0; + out_4327706851715669235[139] = 0.0; + out_4327706851715669235[140] = 0.0; + out_4327706851715669235[141] = 0.0; + out_4327706851715669235[142] = 0.0; + out_4327706851715669235[143] = 0.0; + out_4327706851715669235[144] = 0.0; + out_4327706851715669235[145] = 0.0; + out_4327706851715669235[146] = 0.0; + out_4327706851715669235[147] = 0.0; + out_4327706851715669235[148] = 0.0; + out_4327706851715669235[149] = 0.0; + out_4327706851715669235[150] = 0.0; + out_4327706851715669235[151] = 0.0; + out_4327706851715669235[152] = 1.0; + out_4327706851715669235[153] = 0.0; + out_4327706851715669235[154] = 0.0; + out_4327706851715669235[155] = 0.0; + out_4327706851715669235[156] = 0.0; + out_4327706851715669235[157] = 0.0; + out_4327706851715669235[158] = 0.0; + out_4327706851715669235[159] = 0.0; + out_4327706851715669235[160] = 0.0; + out_4327706851715669235[161] = 0.0; + out_4327706851715669235[162] = 0.0; + out_4327706851715669235[163] = 0.0; + out_4327706851715669235[164] = 0.0; + out_4327706851715669235[165] = 0.0; + out_4327706851715669235[166] = 0.0; + out_4327706851715669235[167] = 0.0; + out_4327706851715669235[168] = 0.0; + out_4327706851715669235[169] = 0.0; + out_4327706851715669235[170] = 0.0; + out_4327706851715669235[171] = 1.0; + out_4327706851715669235[172] = 0.0; + out_4327706851715669235[173] = 0.0; + out_4327706851715669235[174] = 0.0; + out_4327706851715669235[175] = 0.0; + out_4327706851715669235[176] = 0.0; + out_4327706851715669235[177] = 0.0; + out_4327706851715669235[178] = 0.0; + out_4327706851715669235[179] = 0.0; + out_4327706851715669235[180] = 0.0; + out_4327706851715669235[181] = 0.0; + out_4327706851715669235[182] = 0.0; + out_4327706851715669235[183] = 0.0; + out_4327706851715669235[184] = 0.0; + out_4327706851715669235[185] = 0.0; + out_4327706851715669235[186] = 0.0; + out_4327706851715669235[187] = 0.0; + out_4327706851715669235[188] = 0.0; + out_4327706851715669235[189] = 0.0; + out_4327706851715669235[190] = 1.0; + out_4327706851715669235[191] = 0.0; + out_4327706851715669235[192] = 0.0; + out_4327706851715669235[193] = 0.0; + out_4327706851715669235[194] = 0.0; + out_4327706851715669235[195] = 0.0; + out_4327706851715669235[196] = 0.0; + out_4327706851715669235[197] = 0.0; + out_4327706851715669235[198] = 0.0; + out_4327706851715669235[199] = 0.0; + out_4327706851715669235[200] = 0.0; + out_4327706851715669235[201] = 0.0; + out_4327706851715669235[202] = 0.0; + out_4327706851715669235[203] = 0.0; + out_4327706851715669235[204] = 0.0; + out_4327706851715669235[205] = 0.0; + out_4327706851715669235[206] = 0.0; + out_4327706851715669235[207] = 0.0; + out_4327706851715669235[208] = 0.0; + out_4327706851715669235[209] = 1.0; + out_4327706851715669235[210] = 0.0; + out_4327706851715669235[211] = 0.0; + out_4327706851715669235[212] = 0.0; + out_4327706851715669235[213] = 0.0; + out_4327706851715669235[214] = 0.0; + out_4327706851715669235[215] = 0.0; + out_4327706851715669235[216] = 0.0; + out_4327706851715669235[217] = 0.0; + out_4327706851715669235[218] = 0.0; + out_4327706851715669235[219] = 0.0; + out_4327706851715669235[220] = 0.0; + out_4327706851715669235[221] = 0.0; + out_4327706851715669235[222] = 0.0; + out_4327706851715669235[223] = 0.0; + out_4327706851715669235[224] = 0.0; + out_4327706851715669235[225] = 0.0; + out_4327706851715669235[226] = 0.0; + out_4327706851715669235[227] = 0.0; + out_4327706851715669235[228] = 1.0; + out_4327706851715669235[229] = 0.0; + out_4327706851715669235[230] = 0.0; + out_4327706851715669235[231] = 0.0; + out_4327706851715669235[232] = 0.0; + out_4327706851715669235[233] = 0.0; + out_4327706851715669235[234] = 0.0; + out_4327706851715669235[235] = 0.0; + out_4327706851715669235[236] = 0.0; + out_4327706851715669235[237] = 0.0; + out_4327706851715669235[238] = 0.0; + out_4327706851715669235[239] = 0.0; + out_4327706851715669235[240] = 0.0; + out_4327706851715669235[241] = 0.0; + out_4327706851715669235[242] = 0.0; + out_4327706851715669235[243] = 0.0; + out_4327706851715669235[244] = 0.0; + out_4327706851715669235[245] = 0.0; + out_4327706851715669235[246] = 0.0; + out_4327706851715669235[247] = 1.0; + out_4327706851715669235[248] = 0.0; + out_4327706851715669235[249] = 0.0; + out_4327706851715669235[250] = 0.0; + out_4327706851715669235[251] = 0.0; + out_4327706851715669235[252] = 0.0; + out_4327706851715669235[253] = 0.0; + out_4327706851715669235[254] = 0.0; + out_4327706851715669235[255] = 0.0; + out_4327706851715669235[256] = 0.0; + out_4327706851715669235[257] = 0.0; + out_4327706851715669235[258] = 0.0; + out_4327706851715669235[259] = 0.0; + out_4327706851715669235[260] = 0.0; + out_4327706851715669235[261] = 0.0; + out_4327706851715669235[262] = 0.0; + out_4327706851715669235[263] = 0.0; + out_4327706851715669235[264] = 0.0; + out_4327706851715669235[265] = 0.0; + out_4327706851715669235[266] = 1.0; + out_4327706851715669235[267] = 0.0; + out_4327706851715669235[268] = 0.0; + out_4327706851715669235[269] = 0.0; + out_4327706851715669235[270] = 0.0; + out_4327706851715669235[271] = 0.0; + out_4327706851715669235[272] = 0.0; + out_4327706851715669235[273] = 0.0; + out_4327706851715669235[274] = 0.0; + out_4327706851715669235[275] = 0.0; + out_4327706851715669235[276] = 0.0; + out_4327706851715669235[277] = 0.0; + out_4327706851715669235[278] = 0.0; + out_4327706851715669235[279] = 0.0; + out_4327706851715669235[280] = 0.0; + out_4327706851715669235[281] = 0.0; + out_4327706851715669235[282] = 0.0; + out_4327706851715669235[283] = 0.0; + out_4327706851715669235[284] = 0.0; + out_4327706851715669235[285] = 1.0; + out_4327706851715669235[286] = 0.0; + out_4327706851715669235[287] = 0.0; + out_4327706851715669235[288] = 0.0; + out_4327706851715669235[289] = 0.0; + out_4327706851715669235[290] = 0.0; + out_4327706851715669235[291] = 0.0; + out_4327706851715669235[292] = 0.0; + out_4327706851715669235[293] = 0.0; + out_4327706851715669235[294] = 0.0; + out_4327706851715669235[295] = 0.0; + out_4327706851715669235[296] = 0.0; + out_4327706851715669235[297] = 0.0; + out_4327706851715669235[298] = 0.0; + out_4327706851715669235[299] = 0.0; + out_4327706851715669235[300] = 0.0; + out_4327706851715669235[301] = 0.0; + out_4327706851715669235[302] = 0.0; + out_4327706851715669235[303] = 0.0; + out_4327706851715669235[304] = 1.0; + out_4327706851715669235[305] = 0.0; + out_4327706851715669235[306] = 0.0; + out_4327706851715669235[307] = 0.0; + out_4327706851715669235[308] = 0.0; + out_4327706851715669235[309] = 0.0; + out_4327706851715669235[310] = 0.0; + out_4327706851715669235[311] = 0.0; + out_4327706851715669235[312] = 0.0; + out_4327706851715669235[313] = 0.0; + out_4327706851715669235[314] = 0.0; + out_4327706851715669235[315] = 0.0; + out_4327706851715669235[316] = 0.0; + out_4327706851715669235[317] = 0.0; + out_4327706851715669235[318] = 0.0; + out_4327706851715669235[319] = 0.0; + out_4327706851715669235[320] = 0.0; + out_4327706851715669235[321] = 0.0; + out_4327706851715669235[322] = 0.0; + out_4327706851715669235[323] = 1.0; } -void f_fun(double *state, double dt, double *out_5358842043988680887) { - out_5358842043988680887[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_5358842043988680887[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_5358842043988680887[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_5358842043988680887[3] = dt*state[12] + state[3]; - out_5358842043988680887[4] = dt*state[13] + state[4]; - out_5358842043988680887[5] = dt*state[14] + state[5]; - out_5358842043988680887[6] = state[6]; - out_5358842043988680887[7] = state[7]; - out_5358842043988680887[8] = state[8]; - out_5358842043988680887[9] = state[9]; - out_5358842043988680887[10] = state[10]; - out_5358842043988680887[11] = state[11]; - out_5358842043988680887[12] = state[12]; - out_5358842043988680887[13] = state[13]; - out_5358842043988680887[14] = state[14]; - out_5358842043988680887[15] = state[15]; - out_5358842043988680887[16] = state[16]; - out_5358842043988680887[17] = state[17]; +void f_fun(double *state, double dt, double *out_497730658810754078) { + out_497730658810754078[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_497730658810754078[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_497730658810754078[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_497730658810754078[3] = dt*state[12] + state[3]; + out_497730658810754078[4] = dt*state[13] + state[4]; + out_497730658810754078[5] = dt*state[14] + state[5]; + out_497730658810754078[6] = state[6]; + out_497730658810754078[7] = state[7]; + out_497730658810754078[8] = state[8]; + out_497730658810754078[9] = state[9]; + out_497730658810754078[10] = state[10]; + out_497730658810754078[11] = state[11]; + out_497730658810754078[12] = state[12]; + out_497730658810754078[13] = state[13]; + out_497730658810754078[14] = state[14]; + out_497730658810754078[15] = state[15]; + out_497730658810754078[16] = state[16]; + out_497730658810754078[17] = state[17]; } -void F_fun(double *state, double dt, double *out_6970610951587922797) { - out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[2] = 0; - out_6970610951587922797[3] = 0; - out_6970610951587922797[4] = 0; - out_6970610951587922797[5] = 0; - out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[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_6970610951587922797[9] = 0; - out_6970610951587922797[10] = 0; - out_6970610951587922797[11] = 0; - out_6970610951587922797[12] = 0; - out_6970610951587922797[13] = 0; - out_6970610951587922797[14] = 0; - out_6970610951587922797[15] = 0; - out_6970610951587922797[16] = 0; - out_6970610951587922797[17] = 0; - out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[20] = 0; - out_6970610951587922797[21] = 0; - out_6970610951587922797[22] = 0; - out_6970610951587922797[23] = 0; - out_6970610951587922797[24] = 0; - out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[27] = 0; - out_6970610951587922797[28] = 0; - out_6970610951587922797[29] = 0; - out_6970610951587922797[30] = 0; - out_6970610951587922797[31] = 0; - out_6970610951587922797[32] = 0; - out_6970610951587922797[33] = 0; - out_6970610951587922797[34] = 0; - out_6970610951587922797[35] = 0; - out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[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_6970610951587922797[39] = 0; - out_6970610951587922797[40] = 0; - out_6970610951587922797[41] = 0; - out_6970610951587922797[42] = 0; - out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[45] = 0; - out_6970610951587922797[46] = 0; - out_6970610951587922797[47] = 0; - out_6970610951587922797[48] = 0; - out_6970610951587922797[49] = 0; - out_6970610951587922797[50] = 0; - out_6970610951587922797[51] = 0; - out_6970610951587922797[52] = 0; - out_6970610951587922797[53] = 0; - out_6970610951587922797[54] = 0; - out_6970610951587922797[55] = 0; - out_6970610951587922797[56] = 0; - out_6970610951587922797[57] = 1; - out_6970610951587922797[58] = 0; - out_6970610951587922797[59] = 0; - out_6970610951587922797[60] = 0; - out_6970610951587922797[61] = 0; - out_6970610951587922797[62] = 0; - out_6970610951587922797[63] = 0; - out_6970610951587922797[64] = 0; - out_6970610951587922797[65] = 0; - out_6970610951587922797[66] = dt; - out_6970610951587922797[67] = 0; - out_6970610951587922797[68] = 0; - out_6970610951587922797[69] = 0; - out_6970610951587922797[70] = 0; - out_6970610951587922797[71] = 0; - out_6970610951587922797[72] = 0; - out_6970610951587922797[73] = 0; - out_6970610951587922797[74] = 0; - out_6970610951587922797[75] = 0; - out_6970610951587922797[76] = 1; - out_6970610951587922797[77] = 0; - out_6970610951587922797[78] = 0; - out_6970610951587922797[79] = 0; - out_6970610951587922797[80] = 0; - out_6970610951587922797[81] = 0; - out_6970610951587922797[82] = 0; - out_6970610951587922797[83] = 0; - out_6970610951587922797[84] = 0; - out_6970610951587922797[85] = dt; - out_6970610951587922797[86] = 0; - out_6970610951587922797[87] = 0; - out_6970610951587922797[88] = 0; - out_6970610951587922797[89] = 0; - out_6970610951587922797[90] = 0; - out_6970610951587922797[91] = 0; - out_6970610951587922797[92] = 0; - out_6970610951587922797[93] = 0; - out_6970610951587922797[94] = 0; - out_6970610951587922797[95] = 1; - out_6970610951587922797[96] = 0; - out_6970610951587922797[97] = 0; - out_6970610951587922797[98] = 0; - out_6970610951587922797[99] = 0; - out_6970610951587922797[100] = 0; - out_6970610951587922797[101] = 0; - out_6970610951587922797[102] = 0; - out_6970610951587922797[103] = 0; - out_6970610951587922797[104] = dt; - out_6970610951587922797[105] = 0; - out_6970610951587922797[106] = 0; - out_6970610951587922797[107] = 0; - out_6970610951587922797[108] = 0; - out_6970610951587922797[109] = 0; - out_6970610951587922797[110] = 0; - out_6970610951587922797[111] = 0; - out_6970610951587922797[112] = 0; - out_6970610951587922797[113] = 0; - out_6970610951587922797[114] = 1; - out_6970610951587922797[115] = 0; - out_6970610951587922797[116] = 0; - out_6970610951587922797[117] = 0; - out_6970610951587922797[118] = 0; - out_6970610951587922797[119] = 0; - out_6970610951587922797[120] = 0; - out_6970610951587922797[121] = 0; - out_6970610951587922797[122] = 0; - out_6970610951587922797[123] = 0; - out_6970610951587922797[124] = 0; - out_6970610951587922797[125] = 0; - out_6970610951587922797[126] = 0; - out_6970610951587922797[127] = 0; - out_6970610951587922797[128] = 0; - out_6970610951587922797[129] = 0; - out_6970610951587922797[130] = 0; - out_6970610951587922797[131] = 0; - out_6970610951587922797[132] = 0; - out_6970610951587922797[133] = 1; - out_6970610951587922797[134] = 0; - out_6970610951587922797[135] = 0; - out_6970610951587922797[136] = 0; - out_6970610951587922797[137] = 0; - out_6970610951587922797[138] = 0; - out_6970610951587922797[139] = 0; - out_6970610951587922797[140] = 0; - out_6970610951587922797[141] = 0; - out_6970610951587922797[142] = 0; - out_6970610951587922797[143] = 0; - out_6970610951587922797[144] = 0; - out_6970610951587922797[145] = 0; - out_6970610951587922797[146] = 0; - out_6970610951587922797[147] = 0; - out_6970610951587922797[148] = 0; - out_6970610951587922797[149] = 0; - out_6970610951587922797[150] = 0; - out_6970610951587922797[151] = 0; - out_6970610951587922797[152] = 1; - out_6970610951587922797[153] = 0; - out_6970610951587922797[154] = 0; - out_6970610951587922797[155] = 0; - out_6970610951587922797[156] = 0; - out_6970610951587922797[157] = 0; - out_6970610951587922797[158] = 0; - out_6970610951587922797[159] = 0; - out_6970610951587922797[160] = 0; - out_6970610951587922797[161] = 0; - out_6970610951587922797[162] = 0; - out_6970610951587922797[163] = 0; - out_6970610951587922797[164] = 0; - out_6970610951587922797[165] = 0; - out_6970610951587922797[166] = 0; - out_6970610951587922797[167] = 0; - out_6970610951587922797[168] = 0; - out_6970610951587922797[169] = 0; - out_6970610951587922797[170] = 0; - out_6970610951587922797[171] = 1; - out_6970610951587922797[172] = 0; - out_6970610951587922797[173] = 0; - out_6970610951587922797[174] = 0; - out_6970610951587922797[175] = 0; - out_6970610951587922797[176] = 0; - out_6970610951587922797[177] = 0; - out_6970610951587922797[178] = 0; - out_6970610951587922797[179] = 0; - out_6970610951587922797[180] = 0; - out_6970610951587922797[181] = 0; - out_6970610951587922797[182] = 0; - out_6970610951587922797[183] = 0; - out_6970610951587922797[184] = 0; - out_6970610951587922797[185] = 0; - out_6970610951587922797[186] = 0; - out_6970610951587922797[187] = 0; - out_6970610951587922797[188] = 0; - out_6970610951587922797[189] = 0; - out_6970610951587922797[190] = 1; - out_6970610951587922797[191] = 0; - out_6970610951587922797[192] = 0; - out_6970610951587922797[193] = 0; - out_6970610951587922797[194] = 0; - out_6970610951587922797[195] = 0; - out_6970610951587922797[196] = 0; - out_6970610951587922797[197] = 0; - out_6970610951587922797[198] = 0; - out_6970610951587922797[199] = 0; - out_6970610951587922797[200] = 0; - out_6970610951587922797[201] = 0; - out_6970610951587922797[202] = 0; - out_6970610951587922797[203] = 0; - out_6970610951587922797[204] = 0; - out_6970610951587922797[205] = 0; - out_6970610951587922797[206] = 0; - out_6970610951587922797[207] = 0; - out_6970610951587922797[208] = 0; - out_6970610951587922797[209] = 1; - out_6970610951587922797[210] = 0; - out_6970610951587922797[211] = 0; - out_6970610951587922797[212] = 0; - out_6970610951587922797[213] = 0; - out_6970610951587922797[214] = 0; - out_6970610951587922797[215] = 0; - out_6970610951587922797[216] = 0; - out_6970610951587922797[217] = 0; - out_6970610951587922797[218] = 0; - out_6970610951587922797[219] = 0; - out_6970610951587922797[220] = 0; - out_6970610951587922797[221] = 0; - out_6970610951587922797[222] = 0; - out_6970610951587922797[223] = 0; - out_6970610951587922797[224] = 0; - out_6970610951587922797[225] = 0; - out_6970610951587922797[226] = 0; - out_6970610951587922797[227] = 0; - out_6970610951587922797[228] = 1; - out_6970610951587922797[229] = 0; - out_6970610951587922797[230] = 0; - out_6970610951587922797[231] = 0; - out_6970610951587922797[232] = 0; - out_6970610951587922797[233] = 0; - out_6970610951587922797[234] = 0; - out_6970610951587922797[235] = 0; - out_6970610951587922797[236] = 0; - out_6970610951587922797[237] = 0; - out_6970610951587922797[238] = 0; - out_6970610951587922797[239] = 0; - out_6970610951587922797[240] = 0; - out_6970610951587922797[241] = 0; - out_6970610951587922797[242] = 0; - out_6970610951587922797[243] = 0; - out_6970610951587922797[244] = 0; - out_6970610951587922797[245] = 0; - out_6970610951587922797[246] = 0; - out_6970610951587922797[247] = 1; - out_6970610951587922797[248] = 0; - out_6970610951587922797[249] = 0; - out_6970610951587922797[250] = 0; - out_6970610951587922797[251] = 0; - out_6970610951587922797[252] = 0; - out_6970610951587922797[253] = 0; - out_6970610951587922797[254] = 0; - out_6970610951587922797[255] = 0; - out_6970610951587922797[256] = 0; - out_6970610951587922797[257] = 0; - out_6970610951587922797[258] = 0; - out_6970610951587922797[259] = 0; - out_6970610951587922797[260] = 0; - out_6970610951587922797[261] = 0; - out_6970610951587922797[262] = 0; - out_6970610951587922797[263] = 0; - out_6970610951587922797[264] = 0; - out_6970610951587922797[265] = 0; - out_6970610951587922797[266] = 1; - out_6970610951587922797[267] = 0; - out_6970610951587922797[268] = 0; - out_6970610951587922797[269] = 0; - out_6970610951587922797[270] = 0; - out_6970610951587922797[271] = 0; - out_6970610951587922797[272] = 0; - out_6970610951587922797[273] = 0; - out_6970610951587922797[274] = 0; - out_6970610951587922797[275] = 0; - out_6970610951587922797[276] = 0; - out_6970610951587922797[277] = 0; - out_6970610951587922797[278] = 0; - out_6970610951587922797[279] = 0; - out_6970610951587922797[280] = 0; - out_6970610951587922797[281] = 0; - out_6970610951587922797[282] = 0; - out_6970610951587922797[283] = 0; - out_6970610951587922797[284] = 0; - out_6970610951587922797[285] = 1; - out_6970610951587922797[286] = 0; - out_6970610951587922797[287] = 0; - out_6970610951587922797[288] = 0; - out_6970610951587922797[289] = 0; - out_6970610951587922797[290] = 0; - out_6970610951587922797[291] = 0; - out_6970610951587922797[292] = 0; - out_6970610951587922797[293] = 0; - out_6970610951587922797[294] = 0; - out_6970610951587922797[295] = 0; - out_6970610951587922797[296] = 0; - out_6970610951587922797[297] = 0; - out_6970610951587922797[298] = 0; - out_6970610951587922797[299] = 0; - out_6970610951587922797[300] = 0; - out_6970610951587922797[301] = 0; - out_6970610951587922797[302] = 0; - out_6970610951587922797[303] = 0; - out_6970610951587922797[304] = 1; - out_6970610951587922797[305] = 0; - out_6970610951587922797[306] = 0; - out_6970610951587922797[307] = 0; - out_6970610951587922797[308] = 0; - out_6970610951587922797[309] = 0; - out_6970610951587922797[310] = 0; - out_6970610951587922797[311] = 0; - out_6970610951587922797[312] = 0; - out_6970610951587922797[313] = 0; - out_6970610951587922797[314] = 0; - out_6970610951587922797[315] = 0; - out_6970610951587922797[316] = 0; - out_6970610951587922797[317] = 0; - out_6970610951587922797[318] = 0; - out_6970610951587922797[319] = 0; - out_6970610951587922797[320] = 0; - out_6970610951587922797[321] = 0; - out_6970610951587922797[322] = 0; - out_6970610951587922797[323] = 1; +void F_fun(double *state, double dt, double *out_6088738551748760158) { + out_6088738551748760158[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_6088738551748760158[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_6088738551748760158[2] = 0; + out_6088738551748760158[3] = 0; + out_6088738551748760158[4] = 0; + out_6088738551748760158[5] = 0; + out_6088738551748760158[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_6088738551748760158[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_6088738551748760158[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_6088738551748760158[9] = 0; + out_6088738551748760158[10] = 0; + out_6088738551748760158[11] = 0; + out_6088738551748760158[12] = 0; + out_6088738551748760158[13] = 0; + out_6088738551748760158[14] = 0; + out_6088738551748760158[15] = 0; + out_6088738551748760158[16] = 0; + out_6088738551748760158[17] = 0; + out_6088738551748760158[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_6088738551748760158[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_6088738551748760158[20] = 0; + out_6088738551748760158[21] = 0; + out_6088738551748760158[22] = 0; + out_6088738551748760158[23] = 0; + out_6088738551748760158[24] = 0; + out_6088738551748760158[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_6088738551748760158[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_6088738551748760158[27] = 0; + out_6088738551748760158[28] = 0; + out_6088738551748760158[29] = 0; + out_6088738551748760158[30] = 0; + out_6088738551748760158[31] = 0; + out_6088738551748760158[32] = 0; + out_6088738551748760158[33] = 0; + out_6088738551748760158[34] = 0; + out_6088738551748760158[35] = 0; + out_6088738551748760158[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_6088738551748760158[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_6088738551748760158[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_6088738551748760158[39] = 0; + out_6088738551748760158[40] = 0; + out_6088738551748760158[41] = 0; + out_6088738551748760158[42] = 0; + out_6088738551748760158[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_6088738551748760158[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_6088738551748760158[45] = 0; + out_6088738551748760158[46] = 0; + out_6088738551748760158[47] = 0; + out_6088738551748760158[48] = 0; + out_6088738551748760158[49] = 0; + out_6088738551748760158[50] = 0; + out_6088738551748760158[51] = 0; + out_6088738551748760158[52] = 0; + out_6088738551748760158[53] = 0; + out_6088738551748760158[54] = 0; + out_6088738551748760158[55] = 0; + out_6088738551748760158[56] = 0; + out_6088738551748760158[57] = 1; + out_6088738551748760158[58] = 0; + out_6088738551748760158[59] = 0; + out_6088738551748760158[60] = 0; + out_6088738551748760158[61] = 0; + out_6088738551748760158[62] = 0; + out_6088738551748760158[63] = 0; + out_6088738551748760158[64] = 0; + out_6088738551748760158[65] = 0; + out_6088738551748760158[66] = dt; + out_6088738551748760158[67] = 0; + out_6088738551748760158[68] = 0; + out_6088738551748760158[69] = 0; + out_6088738551748760158[70] = 0; + out_6088738551748760158[71] = 0; + out_6088738551748760158[72] = 0; + out_6088738551748760158[73] = 0; + out_6088738551748760158[74] = 0; + out_6088738551748760158[75] = 0; + out_6088738551748760158[76] = 1; + out_6088738551748760158[77] = 0; + out_6088738551748760158[78] = 0; + out_6088738551748760158[79] = 0; + out_6088738551748760158[80] = 0; + out_6088738551748760158[81] = 0; + out_6088738551748760158[82] = 0; + out_6088738551748760158[83] = 0; + out_6088738551748760158[84] = 0; + out_6088738551748760158[85] = dt; + out_6088738551748760158[86] = 0; + out_6088738551748760158[87] = 0; + out_6088738551748760158[88] = 0; + out_6088738551748760158[89] = 0; + out_6088738551748760158[90] = 0; + out_6088738551748760158[91] = 0; + out_6088738551748760158[92] = 0; + out_6088738551748760158[93] = 0; + out_6088738551748760158[94] = 0; + out_6088738551748760158[95] = 1; + out_6088738551748760158[96] = 0; + out_6088738551748760158[97] = 0; + out_6088738551748760158[98] = 0; + out_6088738551748760158[99] = 0; + out_6088738551748760158[100] = 0; + out_6088738551748760158[101] = 0; + out_6088738551748760158[102] = 0; + out_6088738551748760158[103] = 0; + out_6088738551748760158[104] = dt; + out_6088738551748760158[105] = 0; + out_6088738551748760158[106] = 0; + out_6088738551748760158[107] = 0; + out_6088738551748760158[108] = 0; + out_6088738551748760158[109] = 0; + out_6088738551748760158[110] = 0; + out_6088738551748760158[111] = 0; + out_6088738551748760158[112] = 0; + out_6088738551748760158[113] = 0; + out_6088738551748760158[114] = 1; + out_6088738551748760158[115] = 0; + out_6088738551748760158[116] = 0; + out_6088738551748760158[117] = 0; + out_6088738551748760158[118] = 0; + out_6088738551748760158[119] = 0; + out_6088738551748760158[120] = 0; + out_6088738551748760158[121] = 0; + out_6088738551748760158[122] = 0; + out_6088738551748760158[123] = 0; + out_6088738551748760158[124] = 0; + out_6088738551748760158[125] = 0; + out_6088738551748760158[126] = 0; + out_6088738551748760158[127] = 0; + out_6088738551748760158[128] = 0; + out_6088738551748760158[129] = 0; + out_6088738551748760158[130] = 0; + out_6088738551748760158[131] = 0; + out_6088738551748760158[132] = 0; + out_6088738551748760158[133] = 1; + out_6088738551748760158[134] = 0; + out_6088738551748760158[135] = 0; + out_6088738551748760158[136] = 0; + out_6088738551748760158[137] = 0; + out_6088738551748760158[138] = 0; + out_6088738551748760158[139] = 0; + out_6088738551748760158[140] = 0; + out_6088738551748760158[141] = 0; + out_6088738551748760158[142] = 0; + out_6088738551748760158[143] = 0; + out_6088738551748760158[144] = 0; + out_6088738551748760158[145] = 0; + out_6088738551748760158[146] = 0; + out_6088738551748760158[147] = 0; + out_6088738551748760158[148] = 0; + out_6088738551748760158[149] = 0; + out_6088738551748760158[150] = 0; + out_6088738551748760158[151] = 0; + out_6088738551748760158[152] = 1; + out_6088738551748760158[153] = 0; + out_6088738551748760158[154] = 0; + out_6088738551748760158[155] = 0; + out_6088738551748760158[156] = 0; + out_6088738551748760158[157] = 0; + out_6088738551748760158[158] = 0; + out_6088738551748760158[159] = 0; + out_6088738551748760158[160] = 0; + out_6088738551748760158[161] = 0; + out_6088738551748760158[162] = 0; + out_6088738551748760158[163] = 0; + out_6088738551748760158[164] = 0; + out_6088738551748760158[165] = 0; + out_6088738551748760158[166] = 0; + out_6088738551748760158[167] = 0; + out_6088738551748760158[168] = 0; + out_6088738551748760158[169] = 0; + out_6088738551748760158[170] = 0; + out_6088738551748760158[171] = 1; + out_6088738551748760158[172] = 0; + out_6088738551748760158[173] = 0; + out_6088738551748760158[174] = 0; + out_6088738551748760158[175] = 0; + out_6088738551748760158[176] = 0; + out_6088738551748760158[177] = 0; + out_6088738551748760158[178] = 0; + out_6088738551748760158[179] = 0; + out_6088738551748760158[180] = 0; + out_6088738551748760158[181] = 0; + out_6088738551748760158[182] = 0; + out_6088738551748760158[183] = 0; + out_6088738551748760158[184] = 0; + out_6088738551748760158[185] = 0; + out_6088738551748760158[186] = 0; + out_6088738551748760158[187] = 0; + out_6088738551748760158[188] = 0; + out_6088738551748760158[189] = 0; + out_6088738551748760158[190] = 1; + out_6088738551748760158[191] = 0; + out_6088738551748760158[192] = 0; + out_6088738551748760158[193] = 0; + out_6088738551748760158[194] = 0; + out_6088738551748760158[195] = 0; + out_6088738551748760158[196] = 0; + out_6088738551748760158[197] = 0; + out_6088738551748760158[198] = 0; + out_6088738551748760158[199] = 0; + out_6088738551748760158[200] = 0; + out_6088738551748760158[201] = 0; + out_6088738551748760158[202] = 0; + out_6088738551748760158[203] = 0; + out_6088738551748760158[204] = 0; + out_6088738551748760158[205] = 0; + out_6088738551748760158[206] = 0; + out_6088738551748760158[207] = 0; + out_6088738551748760158[208] = 0; + out_6088738551748760158[209] = 1; + out_6088738551748760158[210] = 0; + out_6088738551748760158[211] = 0; + out_6088738551748760158[212] = 0; + out_6088738551748760158[213] = 0; + out_6088738551748760158[214] = 0; + out_6088738551748760158[215] = 0; + out_6088738551748760158[216] = 0; + out_6088738551748760158[217] = 0; + out_6088738551748760158[218] = 0; + out_6088738551748760158[219] = 0; + out_6088738551748760158[220] = 0; + out_6088738551748760158[221] = 0; + out_6088738551748760158[222] = 0; + out_6088738551748760158[223] = 0; + out_6088738551748760158[224] = 0; + out_6088738551748760158[225] = 0; + out_6088738551748760158[226] = 0; + out_6088738551748760158[227] = 0; + out_6088738551748760158[228] = 1; + out_6088738551748760158[229] = 0; + out_6088738551748760158[230] = 0; + out_6088738551748760158[231] = 0; + out_6088738551748760158[232] = 0; + out_6088738551748760158[233] = 0; + out_6088738551748760158[234] = 0; + out_6088738551748760158[235] = 0; + out_6088738551748760158[236] = 0; + out_6088738551748760158[237] = 0; + out_6088738551748760158[238] = 0; + out_6088738551748760158[239] = 0; + out_6088738551748760158[240] = 0; + out_6088738551748760158[241] = 0; + out_6088738551748760158[242] = 0; + out_6088738551748760158[243] = 0; + out_6088738551748760158[244] = 0; + out_6088738551748760158[245] = 0; + out_6088738551748760158[246] = 0; + out_6088738551748760158[247] = 1; + out_6088738551748760158[248] = 0; + out_6088738551748760158[249] = 0; + out_6088738551748760158[250] = 0; + out_6088738551748760158[251] = 0; + out_6088738551748760158[252] = 0; + out_6088738551748760158[253] = 0; + out_6088738551748760158[254] = 0; + out_6088738551748760158[255] = 0; + out_6088738551748760158[256] = 0; + out_6088738551748760158[257] = 0; + out_6088738551748760158[258] = 0; + out_6088738551748760158[259] = 0; + out_6088738551748760158[260] = 0; + out_6088738551748760158[261] = 0; + out_6088738551748760158[262] = 0; + out_6088738551748760158[263] = 0; + out_6088738551748760158[264] = 0; + out_6088738551748760158[265] = 0; + out_6088738551748760158[266] = 1; + out_6088738551748760158[267] = 0; + out_6088738551748760158[268] = 0; + out_6088738551748760158[269] = 0; + out_6088738551748760158[270] = 0; + out_6088738551748760158[271] = 0; + out_6088738551748760158[272] = 0; + out_6088738551748760158[273] = 0; + out_6088738551748760158[274] = 0; + out_6088738551748760158[275] = 0; + out_6088738551748760158[276] = 0; + out_6088738551748760158[277] = 0; + out_6088738551748760158[278] = 0; + out_6088738551748760158[279] = 0; + out_6088738551748760158[280] = 0; + out_6088738551748760158[281] = 0; + out_6088738551748760158[282] = 0; + out_6088738551748760158[283] = 0; + out_6088738551748760158[284] = 0; + out_6088738551748760158[285] = 1; + out_6088738551748760158[286] = 0; + out_6088738551748760158[287] = 0; + out_6088738551748760158[288] = 0; + out_6088738551748760158[289] = 0; + out_6088738551748760158[290] = 0; + out_6088738551748760158[291] = 0; + out_6088738551748760158[292] = 0; + out_6088738551748760158[293] = 0; + out_6088738551748760158[294] = 0; + out_6088738551748760158[295] = 0; + out_6088738551748760158[296] = 0; + out_6088738551748760158[297] = 0; + out_6088738551748760158[298] = 0; + out_6088738551748760158[299] = 0; + out_6088738551748760158[300] = 0; + out_6088738551748760158[301] = 0; + out_6088738551748760158[302] = 0; + out_6088738551748760158[303] = 0; + out_6088738551748760158[304] = 1; + out_6088738551748760158[305] = 0; + out_6088738551748760158[306] = 0; + out_6088738551748760158[307] = 0; + out_6088738551748760158[308] = 0; + out_6088738551748760158[309] = 0; + out_6088738551748760158[310] = 0; + out_6088738551748760158[311] = 0; + out_6088738551748760158[312] = 0; + out_6088738551748760158[313] = 0; + out_6088738551748760158[314] = 0; + out_6088738551748760158[315] = 0; + out_6088738551748760158[316] = 0; + out_6088738551748760158[317] = 0; + out_6088738551748760158[318] = 0; + out_6088738551748760158[319] = 0; + out_6088738551748760158[320] = 0; + out_6088738551748760158[321] = 0; + out_6088738551748760158[322] = 0; + out_6088738551748760158[323] = 1; } -void h_4(double *state, double *unused, double *out_3613363558024225199) { - out_3613363558024225199[0] = state[6] + state[9]; - out_3613363558024225199[1] = state[7] + state[10]; - out_3613363558024225199[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_2742355909746826294) { + out_2742355909746826294[0] = state[6] + state[9]; + out_2742355909746826294[1] = state[7] + state[10]; + out_2742355909746826294[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_8770780261124510538) { - out_8770780261124510538[0] = 0; - out_8770780261124510538[1] = 0; - out_8770780261124510538[2] = 0; - out_8770780261124510538[3] = 0; - out_8770780261124510538[4] = 0; - out_8770780261124510538[5] = 0; - out_8770780261124510538[6] = 1; - out_8770780261124510538[7] = 0; - out_8770780261124510538[8] = 0; - out_8770780261124510538[9] = 1; - out_8770780261124510538[10] = 0; - out_8770780261124510538[11] = 0; - out_8770780261124510538[12] = 0; - out_8770780261124510538[13] = 0; - out_8770780261124510538[14] = 0; - out_8770780261124510538[15] = 0; - out_8770780261124510538[16] = 0; - out_8770780261124510538[17] = 0; - out_8770780261124510538[18] = 0; - out_8770780261124510538[19] = 0; - out_8770780261124510538[20] = 0; - out_8770780261124510538[21] = 0; - out_8770780261124510538[22] = 0; - out_8770780261124510538[23] = 0; - out_8770780261124510538[24] = 0; - out_8770780261124510538[25] = 1; - out_8770780261124510538[26] = 0; - out_8770780261124510538[27] = 0; - out_8770780261124510538[28] = 1; - out_8770780261124510538[29] = 0; - out_8770780261124510538[30] = 0; - out_8770780261124510538[31] = 0; - out_8770780261124510538[32] = 0; - out_8770780261124510538[33] = 0; - out_8770780261124510538[34] = 0; - out_8770780261124510538[35] = 0; - out_8770780261124510538[36] = 0; - out_8770780261124510538[37] = 0; - out_8770780261124510538[38] = 0; - out_8770780261124510538[39] = 0; - out_8770780261124510538[40] = 0; - out_8770780261124510538[41] = 0; - out_8770780261124510538[42] = 0; - out_8770780261124510538[43] = 0; - out_8770780261124510538[44] = 1; - out_8770780261124510538[45] = 0; - out_8770780261124510538[46] = 0; - out_8770780261124510538[47] = 1; - out_8770780261124510538[48] = 0; - out_8770780261124510538[49] = 0; - out_8770780261124510538[50] = 0; - out_8770780261124510538[51] = 0; - out_8770780261124510538[52] = 0; - out_8770780261124510538[53] = 0; +void H_4(double *state, double *unused, double *out_3203571547522250360) { + out_3203571547522250360[0] = 0; + out_3203571547522250360[1] = 0; + out_3203571547522250360[2] = 0; + out_3203571547522250360[3] = 0; + out_3203571547522250360[4] = 0; + out_3203571547522250360[5] = 0; + out_3203571547522250360[6] = 1; + out_3203571547522250360[7] = 0; + out_3203571547522250360[8] = 0; + out_3203571547522250360[9] = 1; + out_3203571547522250360[10] = 0; + out_3203571547522250360[11] = 0; + out_3203571547522250360[12] = 0; + out_3203571547522250360[13] = 0; + out_3203571547522250360[14] = 0; + out_3203571547522250360[15] = 0; + out_3203571547522250360[16] = 0; + out_3203571547522250360[17] = 0; + out_3203571547522250360[18] = 0; + out_3203571547522250360[19] = 0; + out_3203571547522250360[20] = 0; + out_3203571547522250360[21] = 0; + out_3203571547522250360[22] = 0; + out_3203571547522250360[23] = 0; + out_3203571547522250360[24] = 0; + out_3203571547522250360[25] = 1; + out_3203571547522250360[26] = 0; + out_3203571547522250360[27] = 0; + out_3203571547522250360[28] = 1; + out_3203571547522250360[29] = 0; + out_3203571547522250360[30] = 0; + out_3203571547522250360[31] = 0; + out_3203571547522250360[32] = 0; + out_3203571547522250360[33] = 0; + out_3203571547522250360[34] = 0; + out_3203571547522250360[35] = 0; + out_3203571547522250360[36] = 0; + out_3203571547522250360[37] = 0; + out_3203571547522250360[38] = 0; + out_3203571547522250360[39] = 0; + out_3203571547522250360[40] = 0; + out_3203571547522250360[41] = 0; + out_3203571547522250360[42] = 0; + out_3203571547522250360[43] = 0; + out_3203571547522250360[44] = 1; + out_3203571547522250360[45] = 0; + out_3203571547522250360[46] = 0; + out_3203571547522250360[47] = 1; + out_3203571547522250360[48] = 0; + out_3203571547522250360[49] = 0; + out_3203571547522250360[50] = 0; + out_3203571547522250360[51] = 0; + out_3203571547522250360[52] = 0; + out_3203571547522250360[53] = 0; } -void h_10(double *state, double *unused, double *out_7325246586980236980) { - out_7325246586980236980[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_7325246586980236980[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_7325246586980236980[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_5392750946095808839) { + out_5392750946095808839[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_5392750946095808839[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_5392750946095808839[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_8143750738918534214) { - out_8143750738918534214[0] = 0; - out_8143750738918534214[1] = 9.8100000000000005*cos(state[1]); - out_8143750738918534214[2] = 0; - out_8143750738918534214[3] = 0; - out_8143750738918534214[4] = -state[8]; - out_8143750738918534214[5] = state[7]; - out_8143750738918534214[6] = 0; - out_8143750738918534214[7] = state[5]; - out_8143750738918534214[8] = -state[4]; - out_8143750738918534214[9] = 0; - out_8143750738918534214[10] = 0; - out_8143750738918534214[11] = 0; - out_8143750738918534214[12] = 1; - out_8143750738918534214[13] = 0; - out_8143750738918534214[14] = 0; - out_8143750738918534214[15] = 1; - out_8143750738918534214[16] = 0; - out_8143750738918534214[17] = 0; - out_8143750738918534214[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_8143750738918534214[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_8143750738918534214[20] = 0; - out_8143750738918534214[21] = state[8]; - out_8143750738918534214[22] = 0; - out_8143750738918534214[23] = -state[6]; - out_8143750738918534214[24] = -state[5]; - out_8143750738918534214[25] = 0; - out_8143750738918534214[26] = state[3]; - out_8143750738918534214[27] = 0; - out_8143750738918534214[28] = 0; - out_8143750738918534214[29] = 0; - out_8143750738918534214[30] = 0; - out_8143750738918534214[31] = 1; - out_8143750738918534214[32] = 0; - out_8143750738918534214[33] = 0; - out_8143750738918534214[34] = 1; - out_8143750738918534214[35] = 0; - out_8143750738918534214[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_8143750738918534214[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_8143750738918534214[38] = 0; - out_8143750738918534214[39] = -state[7]; - out_8143750738918534214[40] = state[6]; - out_8143750738918534214[41] = 0; - out_8143750738918534214[42] = state[4]; - out_8143750738918534214[43] = -state[3]; - out_8143750738918534214[44] = 0; - out_8143750738918534214[45] = 0; - out_8143750738918534214[46] = 0; - out_8143750738918534214[47] = 0; - out_8143750738918534214[48] = 0; - out_8143750738918534214[49] = 0; - out_8143750738918534214[50] = 1; - out_8143750738918534214[51] = 0; - out_8143750738918534214[52] = 0; - out_8143750738918534214[53] = 1; +void H_10(double *state, double *unused, double *out_6832887606116687430) { + out_6832887606116687430[0] = 0; + out_6832887606116687430[1] = 9.8100000000000005*cos(state[1]); + out_6832887606116687430[2] = 0; + out_6832887606116687430[3] = 0; + out_6832887606116687430[4] = -state[8]; + out_6832887606116687430[5] = state[7]; + out_6832887606116687430[6] = 0; + out_6832887606116687430[7] = state[5]; + out_6832887606116687430[8] = -state[4]; + out_6832887606116687430[9] = 0; + out_6832887606116687430[10] = 0; + out_6832887606116687430[11] = 0; + out_6832887606116687430[12] = 1; + out_6832887606116687430[13] = 0; + out_6832887606116687430[14] = 0; + out_6832887606116687430[15] = 1; + out_6832887606116687430[16] = 0; + out_6832887606116687430[17] = 0; + out_6832887606116687430[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_6832887606116687430[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_6832887606116687430[20] = 0; + out_6832887606116687430[21] = state[8]; + out_6832887606116687430[22] = 0; + out_6832887606116687430[23] = -state[6]; + out_6832887606116687430[24] = -state[5]; + out_6832887606116687430[25] = 0; + out_6832887606116687430[26] = state[3]; + out_6832887606116687430[27] = 0; + out_6832887606116687430[28] = 0; + out_6832887606116687430[29] = 0; + out_6832887606116687430[30] = 0; + out_6832887606116687430[31] = 1; + out_6832887606116687430[32] = 0; + out_6832887606116687430[33] = 0; + out_6832887606116687430[34] = 1; + out_6832887606116687430[35] = 0; + out_6832887606116687430[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_6832887606116687430[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_6832887606116687430[38] = 0; + out_6832887606116687430[39] = -state[7]; + out_6832887606116687430[40] = state[6]; + out_6832887606116687430[41] = 0; + out_6832887606116687430[42] = state[4]; + out_6832887606116687430[43] = -state[3]; + out_6832887606116687430[44] = 0; + out_6832887606116687430[45] = 0; + out_6832887606116687430[46] = 0; + out_6832887606116687430[47] = 0; + out_6832887606116687430[48] = 0; + out_6832887606116687430[49] = 0; + out_6832887606116687430[50] = 1; + out_6832887606116687430[51] = 0; + out_6832887606116687430[52] = 0; + out_6832887606116687430[53] = 1; } -void h_13(double *state, double *unused, double *out_4440237870440636920) { - out_4440237870440636920[0] = state[3]; - out_4440237870440636920[1] = state[4]; - out_4440237870440636920[2] = state[5]; +void h_13(double *state, double *unused, double *out_7827281880185644605) { + out_7827281880185644605[0] = state[3]; + out_7827281880185644605[1] = state[4]; + out_7827281880185644605[2] = state[5]; } -void H_13(double *state, double *unused, double *out_5558506435792177737) { - out_5558506435792177737[0] = 0; - out_5558506435792177737[1] = 0; - out_5558506435792177737[2] = 0; - out_5558506435792177737[3] = 1; - out_5558506435792177737[4] = 0; - out_5558506435792177737[5] = 0; - out_5558506435792177737[6] = 0; - out_5558506435792177737[7] = 0; - out_5558506435792177737[8] = 0; - out_5558506435792177737[9] = 0; - out_5558506435792177737[10] = 0; - out_5558506435792177737[11] = 0; - out_5558506435792177737[12] = 0; - out_5558506435792177737[13] = 0; - out_5558506435792177737[14] = 0; - out_5558506435792177737[15] = 0; - out_5558506435792177737[16] = 0; - out_5558506435792177737[17] = 0; - out_5558506435792177737[18] = 0; - out_5558506435792177737[19] = 0; - out_5558506435792177737[20] = 0; - out_5558506435792177737[21] = 0; - out_5558506435792177737[22] = 1; - out_5558506435792177737[23] = 0; - out_5558506435792177737[24] = 0; - out_5558506435792177737[25] = 0; - out_5558506435792177737[26] = 0; - out_5558506435792177737[27] = 0; - out_5558506435792177737[28] = 0; - out_5558506435792177737[29] = 0; - out_5558506435792177737[30] = 0; - out_5558506435792177737[31] = 0; - out_5558506435792177737[32] = 0; - out_5558506435792177737[33] = 0; - out_5558506435792177737[34] = 0; - out_5558506435792177737[35] = 0; - out_5558506435792177737[36] = 0; - out_5558506435792177737[37] = 0; - out_5558506435792177737[38] = 0; - out_5558506435792177737[39] = 0; - out_5558506435792177737[40] = 0; - out_5558506435792177737[41] = 1; - out_5558506435792177737[42] = 0; - out_5558506435792177737[43] = 0; - out_5558506435792177737[44] = 0; - out_5558506435792177737[45] = 0; - out_5558506435792177737[46] = 0; - out_5558506435792177737[47] = 0; - out_5558506435792177737[48] = 0; - out_5558506435792177737[49] = 0; - out_5558506435792177737[50] = 0; - out_5558506435792177737[51] = 0; - out_5558506435792177737[52] = 0; - out_5558506435792177737[53] = 0; +void H_13(double *state, double *unused, double *out_8702277810082441) { + out_8702277810082441[0] = 0; + out_8702277810082441[1] = 0; + out_8702277810082441[2] = 0; + out_8702277810082441[3] = 1; + out_8702277810082441[4] = 0; + out_8702277810082441[5] = 0; + out_8702277810082441[6] = 0; + out_8702277810082441[7] = 0; + out_8702277810082441[8] = 0; + out_8702277810082441[9] = 0; + out_8702277810082441[10] = 0; + out_8702277810082441[11] = 0; + out_8702277810082441[12] = 0; + out_8702277810082441[13] = 0; + out_8702277810082441[14] = 0; + out_8702277810082441[15] = 0; + out_8702277810082441[16] = 0; + out_8702277810082441[17] = 0; + out_8702277810082441[18] = 0; + out_8702277810082441[19] = 0; + out_8702277810082441[20] = 0; + out_8702277810082441[21] = 0; + out_8702277810082441[22] = 1; + out_8702277810082441[23] = 0; + out_8702277810082441[24] = 0; + out_8702277810082441[25] = 0; + out_8702277810082441[26] = 0; + out_8702277810082441[27] = 0; + out_8702277810082441[28] = 0; + out_8702277810082441[29] = 0; + out_8702277810082441[30] = 0; + out_8702277810082441[31] = 0; + out_8702277810082441[32] = 0; + out_8702277810082441[33] = 0; + out_8702277810082441[34] = 0; + out_8702277810082441[35] = 0; + out_8702277810082441[36] = 0; + out_8702277810082441[37] = 0; + out_8702277810082441[38] = 0; + out_8702277810082441[39] = 0; + out_8702277810082441[40] = 0; + out_8702277810082441[41] = 1; + out_8702277810082441[42] = 0; + out_8702277810082441[43] = 0; + out_8702277810082441[44] = 0; + out_8702277810082441[45] = 0; + out_8702277810082441[46] = 0; + out_8702277810082441[47] = 0; + out_8702277810082441[48] = 0; + out_8702277810082441[49] = 0; + out_8702277810082441[50] = 0; + out_8702277810082441[51] = 0; + out_8702277810082441[52] = 0; + out_8702277810082441[53] = 0; } -void h_14(double *state, double *unused, double *out_854572314087465107) { - out_854572314087465107[0] = state[6]; - out_854572314087465107[1] = state[7]; - out_854572314087465107[2] = state[8]; +void h_14(double *state, double *unused, double *out_3917682265727873259) { + out_3917682265727873259[0] = state[6]; + out_3917682265727873259[1] = state[7]; + out_3917682265727873259[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4807539404785026009) { - out_4807539404785026009[0] = 0; - out_4807539404785026009[1] = 0; - out_4807539404785026009[2] = 0; - out_4807539404785026009[3] = 0; - out_4807539404785026009[4] = 0; - out_4807539404785026009[5] = 0; - out_4807539404785026009[6] = 1; - out_4807539404785026009[7] = 0; - out_4807539404785026009[8] = 0; - out_4807539404785026009[9] = 0; - out_4807539404785026009[10] = 0; - out_4807539404785026009[11] = 0; - out_4807539404785026009[12] = 0; - out_4807539404785026009[13] = 0; - out_4807539404785026009[14] = 0; - out_4807539404785026009[15] = 0; - out_4807539404785026009[16] = 0; - out_4807539404785026009[17] = 0; - out_4807539404785026009[18] = 0; - out_4807539404785026009[19] = 0; - out_4807539404785026009[20] = 0; - out_4807539404785026009[21] = 0; - out_4807539404785026009[22] = 0; - out_4807539404785026009[23] = 0; - out_4807539404785026009[24] = 0; - out_4807539404785026009[25] = 1; - out_4807539404785026009[26] = 0; - out_4807539404785026009[27] = 0; - out_4807539404785026009[28] = 0; - out_4807539404785026009[29] = 0; - out_4807539404785026009[30] = 0; - out_4807539404785026009[31] = 0; - out_4807539404785026009[32] = 0; - out_4807539404785026009[33] = 0; - out_4807539404785026009[34] = 0; - out_4807539404785026009[35] = 0; - out_4807539404785026009[36] = 0; - out_4807539404785026009[37] = 0; - out_4807539404785026009[38] = 0; - out_4807539404785026009[39] = 0; - out_4807539404785026009[40] = 0; - out_4807539404785026009[41] = 0; - out_4807539404785026009[42] = 0; - out_4807539404785026009[43] = 0; - out_4807539404785026009[44] = 1; - out_4807539404785026009[45] = 0; - out_4807539404785026009[46] = 0; - out_4807539404785026009[47] = 0; - out_4807539404785026009[48] = 0; - out_4807539404785026009[49] = 0; - out_4807539404785026009[50] = 0; - out_4807539404785026009[51] = 0; - out_4807539404785026009[52] = 0; - out_4807539404785026009[53] = 0; +void H_14(double *state, double *unused, double *out_759669308817234169) { + out_759669308817234169[0] = 0; + out_759669308817234169[1] = 0; + out_759669308817234169[2] = 0; + out_759669308817234169[3] = 0; + out_759669308817234169[4] = 0; + out_759669308817234169[5] = 0; + out_759669308817234169[6] = 1; + out_759669308817234169[7] = 0; + out_759669308817234169[8] = 0; + out_759669308817234169[9] = 0; + out_759669308817234169[10] = 0; + out_759669308817234169[11] = 0; + out_759669308817234169[12] = 0; + out_759669308817234169[13] = 0; + out_759669308817234169[14] = 0; + out_759669308817234169[15] = 0; + out_759669308817234169[16] = 0; + out_759669308817234169[17] = 0; + out_759669308817234169[18] = 0; + out_759669308817234169[19] = 0; + out_759669308817234169[20] = 0; + out_759669308817234169[21] = 0; + out_759669308817234169[22] = 0; + out_759669308817234169[23] = 0; + out_759669308817234169[24] = 0; + out_759669308817234169[25] = 1; + out_759669308817234169[26] = 0; + out_759669308817234169[27] = 0; + out_759669308817234169[28] = 0; + out_759669308817234169[29] = 0; + out_759669308817234169[30] = 0; + out_759669308817234169[31] = 0; + out_759669308817234169[32] = 0; + out_759669308817234169[33] = 0; + out_759669308817234169[34] = 0; + out_759669308817234169[35] = 0; + out_759669308817234169[36] = 0; + out_759669308817234169[37] = 0; + out_759669308817234169[38] = 0; + out_759669308817234169[39] = 0; + out_759669308817234169[40] = 0; + out_759669308817234169[41] = 0; + out_759669308817234169[42] = 0; + out_759669308817234169[43] = 0; + out_759669308817234169[44] = 1; + out_759669308817234169[45] = 0; + out_759669308817234169[46] = 0; + out_759669308817234169[47] = 0; + out_759669308817234169[48] = 0; + out_759669308817234169[49] = 0; + out_759669308817234169[50] = 0; + out_759669308817234169[51] = 0; + out_759669308817234169[52] = 0; + out_759669308817234169[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_6247163002975687744) { - err_fun(nom_x, delta_x, out_6247163002975687744); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7113141565155944071) { + err_fun(nom_x, delta_x, out_7113141565155944071); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2784495282432373016) { - inv_err_fun(nom_x, true_x, out_2784495282432373016); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4273806753012627861) { + inv_err_fun(nom_x, true_x, out_4273806753012627861); } -void pose_H_mod_fun(double *state, double *out_662979392671356586) { - H_mod_fun(state, out_662979392671356586); +void pose_H_mod_fun(double *state, double *out_4327706851715669235) { + H_mod_fun(state, out_4327706851715669235); } -void pose_f_fun(double *state, double dt, double *out_5358842043988680887) { - f_fun(state, dt, out_5358842043988680887); +void pose_f_fun(double *state, double dt, double *out_497730658810754078) { + f_fun(state, dt, out_497730658810754078); } -void pose_F_fun(double *state, double dt, double *out_6970610951587922797) { - F_fun(state, dt, out_6970610951587922797); +void pose_F_fun(double *state, double dt, double *out_6088738551748760158) { + F_fun(state, dt, out_6088738551748760158); } -void pose_h_4(double *state, double *unused, double *out_3613363558024225199) { - h_4(state, unused, out_3613363558024225199); +void pose_h_4(double *state, double *unused, double *out_2742355909746826294) { + h_4(state, unused, out_2742355909746826294); } -void pose_H_4(double *state, double *unused, double *out_8770780261124510538) { - H_4(state, unused, out_8770780261124510538); +void pose_H_4(double *state, double *unused, double *out_3203571547522250360) { + H_4(state, unused, out_3203571547522250360); } -void pose_h_10(double *state, double *unused, double *out_7325246586980236980) { - h_10(state, unused, out_7325246586980236980); +void pose_h_10(double *state, double *unused, double *out_5392750946095808839) { + h_10(state, unused, out_5392750946095808839); } -void pose_H_10(double *state, double *unused, double *out_8143750738918534214) { - H_10(state, unused, out_8143750738918534214); +void pose_H_10(double *state, double *unused, double *out_6832887606116687430) { + H_10(state, unused, out_6832887606116687430); } -void pose_h_13(double *state, double *unused, double *out_4440237870440636920) { - h_13(state, unused, out_4440237870440636920); +void pose_h_13(double *state, double *unused, double *out_7827281880185644605) { + h_13(state, unused, out_7827281880185644605); } -void pose_H_13(double *state, double *unused, double *out_5558506435792177737) { - H_13(state, unused, out_5558506435792177737); +void pose_H_13(double *state, double *unused, double *out_8702277810082441) { + H_13(state, unused, out_8702277810082441); } -void pose_h_14(double *state, double *unused, double *out_854572314087465107) { - h_14(state, unused, out_854572314087465107); +void pose_h_14(double *state, double *unused, double *out_3917682265727873259) { + h_14(state, unused, out_3917682265727873259); } -void pose_H_14(double *state, double *unused, double *out_4807539404785026009) { - H_14(state, unused, out_4807539404785026009); +void pose_H_14(double *state, double *unused, double *out_759669308817234169) { + H_14(state, unused, out_759669308817234169); } 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 29cbd8120..f82030be7 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_6247163002975687744); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2784495282432373016); -void pose_H_mod_fun(double *state, double *out_662979392671356586); -void pose_f_fun(double *state, double dt, double *out_5358842043988680887); -void pose_F_fun(double *state, double dt, double *out_6970610951587922797); -void pose_h_4(double *state, double *unused, double *out_3613363558024225199); -void pose_H_4(double *state, double *unused, double *out_8770780261124510538); -void pose_h_10(double *state, double *unused, double *out_7325246586980236980); -void pose_H_10(double *state, double *unused, double *out_8143750738918534214); -void pose_h_13(double *state, double *unused, double *out_4440237870440636920); -void pose_H_13(double *state, double *unused, double *out_5558506435792177737); -void pose_h_14(double *state, double *unused, double *out_854572314087465107); -void pose_H_14(double *state, double *unused, double *out_4807539404785026009); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7113141565155944071); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4273806753012627861); +void pose_H_mod_fun(double *state, double *out_4327706851715669235); +void pose_f_fun(double *state, double dt, double *out_497730658810754078); +void pose_F_fun(double *state, double dt, double *out_6088738551748760158); +void pose_h_4(double *state, double *unused, double *out_2742355909746826294); +void pose_H_4(double *state, double *unused, double *out_3203571547522250360); +void pose_h_10(double *state, double *unused, double *out_5392750946095808839); +void pose_H_10(double *state, double *unused, double *out_6832887606116687430); +void pose_h_13(double *state, double *unused, double *out_7827281880185644605); +void pose_H_13(double *state, double *unused, double *out_8702277810082441); +void pose_h_14(double *state, double *unused, double *out_3917682265727873259); +void pose_H_14(double *state, double *unused, double *out_759669308817234169); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 7bb6d0122..65c9fa3b0 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index a05ce2583..a9621cb3e 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -223,8 +223,10 @@ class ConditionalExperimentalMode: slower_lead = starpilot_toggles.conditional_slower_lead and self.starpilot_planner.starpilot_following.slower_lead stopped_lead = starpilot_toggles.conditional_stopped_lead and lead_speed < 1 + allow_raw_vision_slow_lead = not self.starpilot_planner.tracking_lead raw_vision_slow_lead = bool( starpilot_toggles.conditional_slower_lead and + allow_raw_vision_slow_lead and lead_status and lead_prob >= self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB and lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME) and