diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index c468f3f98..690223353 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 94ad01f7b..8ebbebec4 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 24d6ddbd9..2668d0791 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 17fc477f3..c0886cc5f 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 4950c4a4e..96fc4d83d 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 b50a4b4f1..7d8499cc8 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 7b6dc5cc1..f1f55a045 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 7b6dc5cc1..f1f55a045 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 ff682d434..6c83d4dc9 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 b50a4b4f1..7d8499cc8 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 cb28c39ee..d5c9da117 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-e969a96c-DEBUG"; +const uint8_t gitversion[19] = "DEV-eebfd509-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index ad26da870..a5b07711c 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 c5ed73914..8909e0b40 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 adc3084a9..4d6a78181 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 dc461751f..6e1dc22fe 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 f8008d480..a3dced3c1 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 dff1b1ef0..9e8228d62 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 529ae9fb1..82fd16434 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 e6893fdc9..d716ae7a3 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 d5e102871..d37012f7c 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 2520baa03..a9cf88384 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 f2524feef..f7112ef43 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 da44041ff..c9e24f4c7 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 f581d2016..cc5f988b6 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 54aae4f77..e616bed1c 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 8e14c2455..2b6ed6f2c 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 5e9d86e13..016c4507b 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 a904ea396..ae86262ba 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 39030b6dc..b91e452aa 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 45d8a540f..bd7984536 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 728892884..aee49dac5 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 7ac1d753f..b2b42de34 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-e969a96c-DEBUG \ No newline at end of file +DEV-eebfd509-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 3018a9518..bb045cfe5 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -191,29 +191,29 @@ BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT = 0.27 BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT = 0.23 VOLT_STANDARD_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 -VOLT_STANDARD_FF_GAIN_LEFT = 0.09 -VOLT_STANDARD_FF_GAIN_RIGHT = 0.09 +VOLT_STANDARD_FF_GAIN_LEFT = 0.11 +VOLT_STANDARD_FF_GAIN_RIGHT = 0.08 VOLT_STANDARD_FF_ONSET = 0.10 VOLT_STANDARD_FF_ONSET_WIDTH = 0.05 -VOLT_STANDARD_FF_CUTOFF = 1.30 -VOLT_STANDARD_FF_CUTOFF_WIDTH = 0.24 +VOLT_STANDARD_FF_CUTOFF = 1.38 +VOLT_STANDARD_FF_CUTOFF_WIDTH = 0.28 VOLT_STANDARD_TRANSITION_SPEED = 10.0 VOLT_STANDARD_PHASE_SCALE = 0.10 -VOLT_STANDARD_TURN_IN_BOOST_LEFT = 0.18 -VOLT_STANDARD_TURN_IN_BOOST_RIGHT = 0.52 -VOLT_STANDARD_UNWIND_TAPER_LEFT = 0.22 -VOLT_STANDARD_UNWIND_TAPER_RIGHT = 0.50 +VOLT_STANDARD_TURN_IN_BOOST_LEFT = 0.26 +VOLT_STANDARD_TURN_IN_BOOST_RIGHT = 0.44 +VOLT_STANDARD_UNWIND_TAPER_LEFT = 0.34 +VOLT_STANDARD_UNWIND_TAPER_RIGHT = 0.72 VOLT_STANDARD_FRICTION_MULT = 1.04 VOLT_STANDARD_FRICTION_LAT_RISE = 0.20 VOLT_STANDARD_FRICTION_JERK_RISE = 0.24 -VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.10 -VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.30 -VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT = 0.12 -VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.42 -VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT = 0.05 -VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT = 0.16 -VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT = 0.12 -VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT = 0.36 +VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.14 +VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.24 +VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT = 0.18 +VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.56 +VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT = 0.07 +VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT = 0.13 +VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT = 0.20 +VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT = 0.50 VOLT_STANDARD_CENTER_TAPER_MAX = 0.12 VOLT_STANDARD_CENTER_TAPER_LAT = 0.10 VOLT_STANDARD_CENTER_TAPER_LAT_WIDTH = 0.018 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 765c8b7e4..b17c6b701 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -188,7 +188,7 @@ class TestLatControl: def test_volt_standard_ff_scale_curve(self): assert get_volt_standard_ff_scale(0.0, 0.0, 20.0) == 1.0 - assert get_volt_standard_ff_scale(-0.5, 0.0, 20.0) >= get_volt_standard_ff_scale(0.5, 0.0, 20.0) + assert get_volt_standard_ff_scale(0.5, 0.0, 20.0) >= get_volt_standard_ff_scale(-0.5, 0.0, 20.0) assert get_volt_standard_ff_scale(0.6, 0.7, 8.0) > get_volt_standard_ff_scale(0.6, 0.0, 8.0) > get_volt_standard_ff_scale(0.6, -0.7, 8.0) assert get_volt_standard_ff_scale(-0.6, -0.7, 8.0) > get_volt_standard_ff_scale(-0.6, 0.0, 8.0) > get_volt_standard_ff_scale(-0.6, 0.7, 8.0) assert get_volt_standard_ff_scale(2.0, 0.0, 20.0) < get_volt_standard_ff_scale(0.8, 0.0, 20.0) @@ -200,7 +200,7 @@ class TestLatControl: left_unwind = get_volt_standard_friction_threshold(6.0, 0.7, -0.8) right_unwind = get_volt_standard_friction_threshold(6.0, -0.7, 0.8) assert right_turn_in < left_turn_in < base - assert base < left_unwind < right_unwind + assert base < left_unwind <= right_unwind def test_volt_standard_friction_scale_curve(self): base = get_volt_standard_friction_scale(25.0, 0.7, 0.8) @@ -324,10 +324,10 @@ class TestLatControl: assert base > left_unwind >= right_unwind def test_ioniq_6_center_taper_curve(self): - 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, 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) < 4.5e-2 + assert abs(get_ioniq_6_center_taper_scale(0.2, 30.0) - 1.0) < 5.5e-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 da7392775..fbc5cad33 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_2567017903067559466) { - out_2567017903067559466[0] = delta_x[0] + nom_x[0]; - out_2567017903067559466[1] = delta_x[1] + nom_x[1]; - out_2567017903067559466[2] = delta_x[2] + nom_x[2]; - out_2567017903067559466[3] = delta_x[3] + nom_x[3]; - out_2567017903067559466[4] = delta_x[4] + nom_x[4]; - out_2567017903067559466[5] = delta_x[5] + nom_x[5]; - out_2567017903067559466[6] = delta_x[6] + nom_x[6]; - out_2567017903067559466[7] = delta_x[7] + nom_x[7]; - out_2567017903067559466[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_7149517118868685515) { + out_7149517118868685515[0] = delta_x[0] + nom_x[0]; + out_7149517118868685515[1] = delta_x[1] + nom_x[1]; + out_7149517118868685515[2] = delta_x[2] + nom_x[2]; + out_7149517118868685515[3] = delta_x[3] + nom_x[3]; + out_7149517118868685515[4] = delta_x[4] + nom_x[4]; + out_7149517118868685515[5] = delta_x[5] + nom_x[5]; + out_7149517118868685515[6] = delta_x[6] + nom_x[6]; + out_7149517118868685515[7] = delta_x[7] + nom_x[7]; + out_7149517118868685515[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1749285410370024184) { - out_1749285410370024184[0] = -nom_x[0] + true_x[0]; - out_1749285410370024184[1] = -nom_x[1] + true_x[1]; - out_1749285410370024184[2] = -nom_x[2] + true_x[2]; - out_1749285410370024184[3] = -nom_x[3] + true_x[3]; - out_1749285410370024184[4] = -nom_x[4] + true_x[4]; - out_1749285410370024184[5] = -nom_x[5] + true_x[5]; - out_1749285410370024184[6] = -nom_x[6] + true_x[6]; - out_1749285410370024184[7] = -nom_x[7] + true_x[7]; - out_1749285410370024184[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1206282320038144058) { + out_1206282320038144058[0] = -nom_x[0] + true_x[0]; + out_1206282320038144058[1] = -nom_x[1] + true_x[1]; + out_1206282320038144058[2] = -nom_x[2] + true_x[2]; + out_1206282320038144058[3] = -nom_x[3] + true_x[3]; + out_1206282320038144058[4] = -nom_x[4] + true_x[4]; + out_1206282320038144058[5] = -nom_x[5] + true_x[5]; + out_1206282320038144058[6] = -nom_x[6] + true_x[6]; + out_1206282320038144058[7] = -nom_x[7] + true_x[7]; + out_1206282320038144058[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_8979551951706364182) { - out_8979551951706364182[0] = 1.0; - out_8979551951706364182[1] = 0.0; - out_8979551951706364182[2] = 0.0; - out_8979551951706364182[3] = 0.0; - out_8979551951706364182[4] = 0.0; - out_8979551951706364182[5] = 0.0; - out_8979551951706364182[6] = 0.0; - out_8979551951706364182[7] = 0.0; - out_8979551951706364182[8] = 0.0; - out_8979551951706364182[9] = 0.0; - out_8979551951706364182[10] = 1.0; - out_8979551951706364182[11] = 0.0; - out_8979551951706364182[12] = 0.0; - out_8979551951706364182[13] = 0.0; - out_8979551951706364182[14] = 0.0; - out_8979551951706364182[15] = 0.0; - out_8979551951706364182[16] = 0.0; - out_8979551951706364182[17] = 0.0; - out_8979551951706364182[18] = 0.0; - out_8979551951706364182[19] = 0.0; - out_8979551951706364182[20] = 1.0; - out_8979551951706364182[21] = 0.0; - out_8979551951706364182[22] = 0.0; - out_8979551951706364182[23] = 0.0; - out_8979551951706364182[24] = 0.0; - out_8979551951706364182[25] = 0.0; - out_8979551951706364182[26] = 0.0; - out_8979551951706364182[27] = 0.0; - out_8979551951706364182[28] = 0.0; - out_8979551951706364182[29] = 0.0; - out_8979551951706364182[30] = 1.0; - out_8979551951706364182[31] = 0.0; - out_8979551951706364182[32] = 0.0; - out_8979551951706364182[33] = 0.0; - out_8979551951706364182[34] = 0.0; - out_8979551951706364182[35] = 0.0; - out_8979551951706364182[36] = 0.0; - out_8979551951706364182[37] = 0.0; - out_8979551951706364182[38] = 0.0; - out_8979551951706364182[39] = 0.0; - out_8979551951706364182[40] = 1.0; - out_8979551951706364182[41] = 0.0; - out_8979551951706364182[42] = 0.0; - out_8979551951706364182[43] = 0.0; - out_8979551951706364182[44] = 0.0; - out_8979551951706364182[45] = 0.0; - out_8979551951706364182[46] = 0.0; - out_8979551951706364182[47] = 0.0; - out_8979551951706364182[48] = 0.0; - out_8979551951706364182[49] = 0.0; - out_8979551951706364182[50] = 1.0; - out_8979551951706364182[51] = 0.0; - out_8979551951706364182[52] = 0.0; - out_8979551951706364182[53] = 0.0; - out_8979551951706364182[54] = 0.0; - out_8979551951706364182[55] = 0.0; - out_8979551951706364182[56] = 0.0; - out_8979551951706364182[57] = 0.0; - out_8979551951706364182[58] = 0.0; - out_8979551951706364182[59] = 0.0; - out_8979551951706364182[60] = 1.0; - out_8979551951706364182[61] = 0.0; - out_8979551951706364182[62] = 0.0; - out_8979551951706364182[63] = 0.0; - out_8979551951706364182[64] = 0.0; - out_8979551951706364182[65] = 0.0; - out_8979551951706364182[66] = 0.0; - out_8979551951706364182[67] = 0.0; - out_8979551951706364182[68] = 0.0; - out_8979551951706364182[69] = 0.0; - out_8979551951706364182[70] = 1.0; - out_8979551951706364182[71] = 0.0; - out_8979551951706364182[72] = 0.0; - out_8979551951706364182[73] = 0.0; - out_8979551951706364182[74] = 0.0; - out_8979551951706364182[75] = 0.0; - out_8979551951706364182[76] = 0.0; - out_8979551951706364182[77] = 0.0; - out_8979551951706364182[78] = 0.0; - out_8979551951706364182[79] = 0.0; - out_8979551951706364182[80] = 1.0; +void H_mod_fun(double *state, double *out_2536050837023062909) { + out_2536050837023062909[0] = 1.0; + out_2536050837023062909[1] = 0.0; + out_2536050837023062909[2] = 0.0; + out_2536050837023062909[3] = 0.0; + out_2536050837023062909[4] = 0.0; + out_2536050837023062909[5] = 0.0; + out_2536050837023062909[6] = 0.0; + out_2536050837023062909[7] = 0.0; + out_2536050837023062909[8] = 0.0; + out_2536050837023062909[9] = 0.0; + out_2536050837023062909[10] = 1.0; + out_2536050837023062909[11] = 0.0; + out_2536050837023062909[12] = 0.0; + out_2536050837023062909[13] = 0.0; + out_2536050837023062909[14] = 0.0; + out_2536050837023062909[15] = 0.0; + out_2536050837023062909[16] = 0.0; + out_2536050837023062909[17] = 0.0; + out_2536050837023062909[18] = 0.0; + out_2536050837023062909[19] = 0.0; + out_2536050837023062909[20] = 1.0; + out_2536050837023062909[21] = 0.0; + out_2536050837023062909[22] = 0.0; + out_2536050837023062909[23] = 0.0; + out_2536050837023062909[24] = 0.0; + out_2536050837023062909[25] = 0.0; + out_2536050837023062909[26] = 0.0; + out_2536050837023062909[27] = 0.0; + out_2536050837023062909[28] = 0.0; + out_2536050837023062909[29] = 0.0; + out_2536050837023062909[30] = 1.0; + out_2536050837023062909[31] = 0.0; + out_2536050837023062909[32] = 0.0; + out_2536050837023062909[33] = 0.0; + out_2536050837023062909[34] = 0.0; + out_2536050837023062909[35] = 0.0; + out_2536050837023062909[36] = 0.0; + out_2536050837023062909[37] = 0.0; + out_2536050837023062909[38] = 0.0; + out_2536050837023062909[39] = 0.0; + out_2536050837023062909[40] = 1.0; + out_2536050837023062909[41] = 0.0; + out_2536050837023062909[42] = 0.0; + out_2536050837023062909[43] = 0.0; + out_2536050837023062909[44] = 0.0; + out_2536050837023062909[45] = 0.0; + out_2536050837023062909[46] = 0.0; + out_2536050837023062909[47] = 0.0; + out_2536050837023062909[48] = 0.0; + out_2536050837023062909[49] = 0.0; + out_2536050837023062909[50] = 1.0; + out_2536050837023062909[51] = 0.0; + out_2536050837023062909[52] = 0.0; + out_2536050837023062909[53] = 0.0; + out_2536050837023062909[54] = 0.0; + out_2536050837023062909[55] = 0.0; + out_2536050837023062909[56] = 0.0; + out_2536050837023062909[57] = 0.0; + out_2536050837023062909[58] = 0.0; + out_2536050837023062909[59] = 0.0; + out_2536050837023062909[60] = 1.0; + out_2536050837023062909[61] = 0.0; + out_2536050837023062909[62] = 0.0; + out_2536050837023062909[63] = 0.0; + out_2536050837023062909[64] = 0.0; + out_2536050837023062909[65] = 0.0; + out_2536050837023062909[66] = 0.0; + out_2536050837023062909[67] = 0.0; + out_2536050837023062909[68] = 0.0; + out_2536050837023062909[69] = 0.0; + out_2536050837023062909[70] = 1.0; + out_2536050837023062909[71] = 0.0; + out_2536050837023062909[72] = 0.0; + out_2536050837023062909[73] = 0.0; + out_2536050837023062909[74] = 0.0; + out_2536050837023062909[75] = 0.0; + out_2536050837023062909[76] = 0.0; + out_2536050837023062909[77] = 0.0; + out_2536050837023062909[78] = 0.0; + out_2536050837023062909[79] = 0.0; + out_2536050837023062909[80] = 1.0; } -void f_fun(double *state, double dt, double *out_3674179640817441392) { - out_3674179640817441392[0] = state[0]; - out_3674179640817441392[1] = state[1]; - out_3674179640817441392[2] = state[2]; - out_3674179640817441392[3] = state[3]; - out_3674179640817441392[4] = state[4]; - out_3674179640817441392[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_3674179640817441392[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_3674179640817441392[7] = state[7]; - out_3674179640817441392[8] = state[8]; +void f_fun(double *state, double dt, double *out_4666135608333935516) { + out_4666135608333935516[0] = state[0]; + out_4666135608333935516[1] = state[1]; + out_4666135608333935516[2] = state[2]; + out_4666135608333935516[3] = state[3]; + out_4666135608333935516[4] = state[4]; + out_4666135608333935516[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_4666135608333935516[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_4666135608333935516[7] = state[7]; + out_4666135608333935516[8] = state[8]; } -void F_fun(double *state, double dt, double *out_258925546890284745) { - out_258925546890284745[0] = 1; - out_258925546890284745[1] = 0; - out_258925546890284745[2] = 0; - out_258925546890284745[3] = 0; - out_258925546890284745[4] = 0; - out_258925546890284745[5] = 0; - out_258925546890284745[6] = 0; - out_258925546890284745[7] = 0; - out_258925546890284745[8] = 0; - out_258925546890284745[9] = 0; - out_258925546890284745[10] = 1; - out_258925546890284745[11] = 0; - out_258925546890284745[12] = 0; - out_258925546890284745[13] = 0; - out_258925546890284745[14] = 0; - out_258925546890284745[15] = 0; - out_258925546890284745[16] = 0; - out_258925546890284745[17] = 0; - out_258925546890284745[18] = 0; - out_258925546890284745[19] = 0; - out_258925546890284745[20] = 1; - out_258925546890284745[21] = 0; - out_258925546890284745[22] = 0; - out_258925546890284745[23] = 0; - out_258925546890284745[24] = 0; - out_258925546890284745[25] = 0; - out_258925546890284745[26] = 0; - out_258925546890284745[27] = 0; - out_258925546890284745[28] = 0; - out_258925546890284745[29] = 0; - out_258925546890284745[30] = 1; - out_258925546890284745[31] = 0; - out_258925546890284745[32] = 0; - out_258925546890284745[33] = 0; - out_258925546890284745[34] = 0; - out_258925546890284745[35] = 0; - out_258925546890284745[36] = 0; - out_258925546890284745[37] = 0; - out_258925546890284745[38] = 0; - out_258925546890284745[39] = 0; - out_258925546890284745[40] = 1; - out_258925546890284745[41] = 0; - out_258925546890284745[42] = 0; - out_258925546890284745[43] = 0; - out_258925546890284745[44] = 0; - out_258925546890284745[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_258925546890284745[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_258925546890284745[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_258925546890284745[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_258925546890284745[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_258925546890284745[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_258925546890284745[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_258925546890284745[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_258925546890284745[53] = -9.8100000000000005*dt; - out_258925546890284745[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_258925546890284745[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_258925546890284745[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_258925546890284745[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_258925546890284745[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_258925546890284745[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_258925546890284745[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_258925546890284745[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_258925546890284745[62] = 0; - out_258925546890284745[63] = 0; - out_258925546890284745[64] = 0; - out_258925546890284745[65] = 0; - out_258925546890284745[66] = 0; - out_258925546890284745[67] = 0; - out_258925546890284745[68] = 0; - out_258925546890284745[69] = 0; - out_258925546890284745[70] = 1; - out_258925546890284745[71] = 0; - out_258925546890284745[72] = 0; - out_258925546890284745[73] = 0; - out_258925546890284745[74] = 0; - out_258925546890284745[75] = 0; - out_258925546890284745[76] = 0; - out_258925546890284745[77] = 0; - out_258925546890284745[78] = 0; - out_258925546890284745[79] = 0; - out_258925546890284745[80] = 1; +void F_fun(double *state, double dt, double *out_2752692491938964849) { + out_2752692491938964849[0] = 1; + out_2752692491938964849[1] = 0; + out_2752692491938964849[2] = 0; + out_2752692491938964849[3] = 0; + out_2752692491938964849[4] = 0; + out_2752692491938964849[5] = 0; + out_2752692491938964849[6] = 0; + out_2752692491938964849[7] = 0; + out_2752692491938964849[8] = 0; + out_2752692491938964849[9] = 0; + out_2752692491938964849[10] = 1; + out_2752692491938964849[11] = 0; + out_2752692491938964849[12] = 0; + out_2752692491938964849[13] = 0; + out_2752692491938964849[14] = 0; + out_2752692491938964849[15] = 0; + out_2752692491938964849[16] = 0; + out_2752692491938964849[17] = 0; + out_2752692491938964849[18] = 0; + out_2752692491938964849[19] = 0; + out_2752692491938964849[20] = 1; + out_2752692491938964849[21] = 0; + out_2752692491938964849[22] = 0; + out_2752692491938964849[23] = 0; + out_2752692491938964849[24] = 0; + out_2752692491938964849[25] = 0; + out_2752692491938964849[26] = 0; + out_2752692491938964849[27] = 0; + out_2752692491938964849[28] = 0; + out_2752692491938964849[29] = 0; + out_2752692491938964849[30] = 1; + out_2752692491938964849[31] = 0; + out_2752692491938964849[32] = 0; + out_2752692491938964849[33] = 0; + out_2752692491938964849[34] = 0; + out_2752692491938964849[35] = 0; + out_2752692491938964849[36] = 0; + out_2752692491938964849[37] = 0; + out_2752692491938964849[38] = 0; + out_2752692491938964849[39] = 0; + out_2752692491938964849[40] = 1; + out_2752692491938964849[41] = 0; + out_2752692491938964849[42] = 0; + out_2752692491938964849[43] = 0; + out_2752692491938964849[44] = 0; + out_2752692491938964849[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_2752692491938964849[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2752692491938964849[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2752692491938964849[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2752692491938964849[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_2752692491938964849[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2752692491938964849[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2752692491938964849[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2752692491938964849[53] = -9.8100000000000005*dt; + out_2752692491938964849[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_2752692491938964849[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2752692491938964849[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2752692491938964849[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2752692491938964849[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_2752692491938964849[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2752692491938964849[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_2752692491938964849[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2752692491938964849[62] = 0; + out_2752692491938964849[63] = 0; + out_2752692491938964849[64] = 0; + out_2752692491938964849[65] = 0; + out_2752692491938964849[66] = 0; + out_2752692491938964849[67] = 0; + out_2752692491938964849[68] = 0; + out_2752692491938964849[69] = 0; + out_2752692491938964849[70] = 1; + out_2752692491938964849[71] = 0; + out_2752692491938964849[72] = 0; + out_2752692491938964849[73] = 0; + out_2752692491938964849[74] = 0; + out_2752692491938964849[75] = 0; + out_2752692491938964849[76] = 0; + out_2752692491938964849[77] = 0; + out_2752692491938964849[78] = 0; + out_2752692491938964849[79] = 0; + out_2752692491938964849[80] = 1; } -void h_25(double *state, double *unused, double *out_5262304077966076166) { - out_5262304077966076166[0] = state[6]; +void h_25(double *state, double *unused, double *out_3787568519496882893) { + out_3787568519496882893[0] = state[6]; } -void H_25(double *state, double *unused, double *out_4563500296818522594) { - out_4563500296818522594[0] = 0; - out_4563500296818522594[1] = 0; - out_4563500296818522594[2] = 0; - out_4563500296818522594[3] = 0; - out_4563500296818522594[4] = 0; - out_4563500296818522594[5] = 0; - out_4563500296818522594[6] = 1; - out_4563500296818522594[7] = 0; - out_4563500296818522594[8] = 0; +void H_25(double *state, double *unused, double *out_3699858802985056505) { + out_3699858802985056505[0] = 0; + out_3699858802985056505[1] = 0; + out_3699858802985056505[2] = 0; + out_3699858802985056505[3] = 0; + out_3699858802985056505[4] = 0; + out_3699858802985056505[5] = 0; + out_3699858802985056505[6] = 1; + out_3699858802985056505[7] = 0; + out_3699858802985056505[8] = 0; } -void h_24(double *state, double *unused, double *out_2894074188837958464) { - out_2894074188837958464[0] = state[4]; - out_2894074188837958464[1] = state[5]; +void h_24(double *state, double *unused, double *out_1528856480665249325) { + out_1528856480665249325[0] = state[4]; + out_1528856480665249325[1] = state[5]; } -void H_24(double *state, double *unused, double *out_2012071509772995507) { - out_2012071509772995507[0] = 0; - out_2012071509772995507[1] = 0; - out_2012071509772995507[2] = 0; - out_2012071509772995507[3] = 0; - out_2012071509772995507[4] = 1; - out_2012071509772995507[5] = 0; - out_2012071509772995507[6] = 0; - out_2012071509772995507[7] = 0; - out_2012071509772995507[8] = 0; - out_2012071509772995507[9] = 0; - out_2012071509772995507[10] = 0; - out_2012071509772995507[11] = 0; - out_2012071509772995507[12] = 0; - out_2012071509772995507[13] = 0; - out_2012071509772995507[14] = 1; - out_2012071509772995507[15] = 0; - out_2012071509772995507[16] = 0; - out_2012071509772995507[17] = 0; +void H_24(double *state, double *unused, double *out_1970995683807742436) { + out_1970995683807742436[0] = 0; + out_1970995683807742436[1] = 0; + out_1970995683807742436[2] = 0; + out_1970995683807742436[3] = 0; + out_1970995683807742436[4] = 1; + out_1970995683807742436[5] = 0; + out_1970995683807742436[6] = 0; + out_1970995683807742436[7] = 0; + out_1970995683807742436[8] = 0; + out_1970995683807742436[9] = 0; + out_1970995683807742436[10] = 0; + out_1970995683807742436[11] = 0; + out_1970995683807742436[12] = 0; + out_1970995683807742436[13] = 0; + out_1970995683807742436[14] = 1; + out_1970995683807742436[15] = 0; + out_1970995683807742436[16] = 0; + out_1970995683807742436[17] = 0; } -void h_30(double *state, double *unused, double *out_9005980160461201074) { - out_9005980160461201074[0] = state[4]; +void h_30(double *state, double *unused, double *out_4709092161766857099) { + out_4709092161766857099[0] = state[4]; } -void H_30(double *state, double *unused, double *out_2045167338311273967) { - out_2045167338311273967[0] = 0; - out_2045167338311273967[1] = 0; - out_2045167338311273967[2] = 0; - out_2045167338311273967[3] = 0; - out_2045167338311273967[4] = 1; - out_2045167338311273967[5] = 0; - out_2045167338311273967[6] = 0; - out_2045167338311273967[7] = 0; - out_2045167338311273967[8] = 0; +void H_30(double *state, double *unused, double *out_7830194929232878356) { + out_7830194929232878356[0] = 0; + out_7830194929232878356[1] = 0; + out_7830194929232878356[2] = 0; + out_7830194929232878356[3] = 0; + out_7830194929232878356[4] = 1; + out_7830194929232878356[5] = 0; + out_7830194929232878356[6] = 0; + out_7830194929232878356[7] = 0; + out_7830194929232878356[8] = 0; } -void h_26(double *state, double *unused, double *out_8837448028571873025) { - out_8837448028571873025[0] = state[7]; +void h_26(double *state, double *unused, double *out_1815522905777326306) { + out_1815522905777326306[0] = state[7]; } -void H_26(double *state, double *unused, double *out_1258974327057721993) { - out_1258974327057721993[0] = 0; - out_1258974327057721993[1] = 0; - out_1258974327057721993[2] = 0; - out_1258974327057721993[3] = 0; - out_1258974327057721993[4] = 0; - out_1258974327057721993[5] = 0; - out_1258974327057721993[6] = 0; - out_1258974327057721993[7] = 1; - out_1258974327057721993[8] = 0; +void H_26(double *state, double *unused, double *out_41644515888999719) { + out_41644515888999719[0] = 0; + out_41644515888999719[1] = 0; + out_41644515888999719[2] = 0; + out_41644515888999719[3] = 0; + out_41644515888999719[4] = 0; + out_41644515888999719[5] = 0; + out_41644515888999719[6] = 0; + out_41644515888999719[7] = 1; + out_41644515888999719[8] = 0; } -void h_27(double *state, double *unused, double *out_6785398478329626304) { - out_6785398478329626304[0] = state[3]; +void h_27(double *state, double *unused, double *out_8789241787157728538) { + out_8789241787157728538[0] = state[3]; } -void H_27(double *state, double *unused, double *out_178426732872669250) { - out_178426732872669250[0] = 0; - out_178426732872669250[1] = 0; - out_178426732872669250[2] = 0; - out_178426732872669250[3] = 1; - out_178426732872669250[4] = 0; - out_178426732872669250[5] = 0; - out_178426732872669250[6] = 0; - out_178426732872669250[7] = 0; - out_178426732872669250[8] = 0; +void H_27(double *state, double *unused, double *out_8441785832676248349) { + out_8441785832676248349[0] = 0; + out_8441785832676248349[1] = 0; + out_8441785832676248349[2] = 0; + out_8441785832676248349[3] = 1; + out_8441785832676248349[4] = 0; + out_8441785832676248349[5] = 0; + out_8441785832676248349[6] = 0; + out_8441785832676248349[7] = 0; + out_8441785832676248349[8] = 0; } -void h_29(double *state, double *unused, double *out_8023896067125902962) { - out_8023896067125902962[0] = state[1]; +void h_29(double *state, double *unused, double *out_8252567880601862062) { + out_8252567880601862062[0] = state[1]; } -void H_29(double *state, double *unused, double *out_1534935993996881783) { - out_1534935993996881783[0] = 0; - out_1534935993996881783[1] = 1; - out_1534935993996881783[2] = 0; - out_1534935993996881783[3] = 0; - out_1534935993996881783[4] = 0; - out_1534935993996881783[5] = 0; - out_1534935993996881783[6] = 0; - out_1534935993996881783[7] = 0; - out_1534935993996881783[8] = 0; +void H_29(double *state, double *unused, double *out_7319963584918486172) { + out_7319963584918486172[0] = 0; + out_7319963584918486172[1] = 1; + out_7319963584918486172[2] = 0; + out_7319963584918486172[3] = 0; + out_7319963584918486172[4] = 0; + out_7319963584918486172[5] = 0; + out_7319963584918486172[6] = 0; + out_7319963584918486172[7] = 0; + out_7319963584918486172[8] = 0; } -void h_28(double *state, double *unused, double *out_5617654677253484567) { - out_5617654677253484567[0] = state[0]; +void h_28(double *state, double *unused, double *out_5372968497508658522) { + out_5372968497508658522[0] = state[0]; } -void H_28(double *state, double *unused, double *out_6617335011066412357) { - out_6617335011066412357[0] = 1; - out_6617335011066412357[1] = 0; - out_6617335011066412357[2] = 0; - out_6617335011066412357[3] = 0; - out_6617335011066412357[4] = 0; - out_6617335011066412357[5] = 0; - out_6617335011066412357[6] = 0; - out_6617335011066412357[7] = 0; - out_6617335011066412357[8] = 0; +void H_28(double *state, double *unused, double *out_6044381471721534870) { + out_6044381471721534870[0] = 1; + out_6044381471721534870[1] = 0; + out_6044381471721534870[2] = 0; + out_6044381471721534870[3] = 0; + out_6044381471721534870[4] = 0; + out_6044381471721534870[5] = 0; + out_6044381471721534870[6] = 0; + out_6044381471721534870[7] = 0; + out_6044381471721534870[8] = 0; } -void h_31(double *state, double *unused, double *out_9123987554394577818) { - out_9123987554394577818[0] = state[8]; +void h_31(double *state, double *unused, double *out_6895910297391996282) { + out_6895910297391996282[0] = state[8]; } -void H_31(double *state, double *unused, double *out_4532854334941562166) { - out_4532854334941562166[0] = 0; - out_4532854334941562166[1] = 0; - out_4532854334941562166[2] = 0; - out_4532854334941562166[3] = 0; - out_4532854334941562166[4] = 0; - out_4532854334941562166[5] = 0; - out_4532854334941562166[6] = 0; - out_4532854334941562166[7] = 0; - out_4532854334941562166[8] = 1; +void H_31(double *state, double *unused, double *out_3730504764862016933) { + out_3730504764862016933[0] = 0; + out_3730504764862016933[1] = 0; + out_3730504764862016933[2] = 0; + out_3730504764862016933[3] = 0; + out_3730504764862016933[4] = 0; + out_3730504764862016933[5] = 0; + out_3730504764862016933[6] = 0; + out_3730504764862016933[7] = 0; + out_3730504764862016933[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_2567017903067559466) { - err_fun(nom_x, delta_x, out_2567017903067559466); +void car_err_fun(double *nom_x, double *delta_x, double *out_7149517118868685515) { + err_fun(nom_x, delta_x, out_7149517118868685515); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1749285410370024184) { - inv_err_fun(nom_x, true_x, out_1749285410370024184); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1206282320038144058) { + inv_err_fun(nom_x, true_x, out_1206282320038144058); } -void car_H_mod_fun(double *state, double *out_8979551951706364182) { - H_mod_fun(state, out_8979551951706364182); +void car_H_mod_fun(double *state, double *out_2536050837023062909) { + H_mod_fun(state, out_2536050837023062909); } -void car_f_fun(double *state, double dt, double *out_3674179640817441392) { - f_fun(state, dt, out_3674179640817441392); +void car_f_fun(double *state, double dt, double *out_4666135608333935516) { + f_fun(state, dt, out_4666135608333935516); } -void car_F_fun(double *state, double dt, double *out_258925546890284745) { - F_fun(state, dt, out_258925546890284745); +void car_F_fun(double *state, double dt, double *out_2752692491938964849) { + F_fun(state, dt, out_2752692491938964849); } -void car_h_25(double *state, double *unused, double *out_5262304077966076166) { - h_25(state, unused, out_5262304077966076166); +void car_h_25(double *state, double *unused, double *out_3787568519496882893) { + h_25(state, unused, out_3787568519496882893); } -void car_H_25(double *state, double *unused, double *out_4563500296818522594) { - H_25(state, unused, out_4563500296818522594); +void car_H_25(double *state, double *unused, double *out_3699858802985056505) { + H_25(state, unused, out_3699858802985056505); } -void car_h_24(double *state, double *unused, double *out_2894074188837958464) { - h_24(state, unused, out_2894074188837958464); +void car_h_24(double *state, double *unused, double *out_1528856480665249325) { + h_24(state, unused, out_1528856480665249325); } -void car_H_24(double *state, double *unused, double *out_2012071509772995507) { - H_24(state, unused, out_2012071509772995507); +void car_H_24(double *state, double *unused, double *out_1970995683807742436) { + H_24(state, unused, out_1970995683807742436); } -void car_h_30(double *state, double *unused, double *out_9005980160461201074) { - h_30(state, unused, out_9005980160461201074); +void car_h_30(double *state, double *unused, double *out_4709092161766857099) { + h_30(state, unused, out_4709092161766857099); } -void car_H_30(double *state, double *unused, double *out_2045167338311273967) { - H_30(state, unused, out_2045167338311273967); +void car_H_30(double *state, double *unused, double *out_7830194929232878356) { + H_30(state, unused, out_7830194929232878356); } -void car_h_26(double *state, double *unused, double *out_8837448028571873025) { - h_26(state, unused, out_8837448028571873025); +void car_h_26(double *state, double *unused, double *out_1815522905777326306) { + h_26(state, unused, out_1815522905777326306); } -void car_H_26(double *state, double *unused, double *out_1258974327057721993) { - H_26(state, unused, out_1258974327057721993); +void car_H_26(double *state, double *unused, double *out_41644515888999719) { + H_26(state, unused, out_41644515888999719); } -void car_h_27(double *state, double *unused, double *out_6785398478329626304) { - h_27(state, unused, out_6785398478329626304); +void car_h_27(double *state, double *unused, double *out_8789241787157728538) { + h_27(state, unused, out_8789241787157728538); } -void car_H_27(double *state, double *unused, double *out_178426732872669250) { - H_27(state, unused, out_178426732872669250); +void car_H_27(double *state, double *unused, double *out_8441785832676248349) { + H_27(state, unused, out_8441785832676248349); } -void car_h_29(double *state, double *unused, double *out_8023896067125902962) { - h_29(state, unused, out_8023896067125902962); +void car_h_29(double *state, double *unused, double *out_8252567880601862062) { + h_29(state, unused, out_8252567880601862062); } -void car_H_29(double *state, double *unused, double *out_1534935993996881783) { - H_29(state, unused, out_1534935993996881783); +void car_H_29(double *state, double *unused, double *out_7319963584918486172) { + H_29(state, unused, out_7319963584918486172); } -void car_h_28(double *state, double *unused, double *out_5617654677253484567) { - h_28(state, unused, out_5617654677253484567); +void car_h_28(double *state, double *unused, double *out_5372968497508658522) { + h_28(state, unused, out_5372968497508658522); } -void car_H_28(double *state, double *unused, double *out_6617335011066412357) { - H_28(state, unused, out_6617335011066412357); +void car_H_28(double *state, double *unused, double *out_6044381471721534870) { + H_28(state, unused, out_6044381471721534870); } -void car_h_31(double *state, double *unused, double *out_9123987554394577818) { - h_31(state, unused, out_9123987554394577818); +void car_h_31(double *state, double *unused, double *out_6895910297391996282) { + h_31(state, unused, out_6895910297391996282); } -void car_H_31(double *state, double *unused, double *out_4532854334941562166) { - H_31(state, unused, out_4532854334941562166); +void car_H_31(double *state, double *unused, double *out_3730504764862016933) { + H_31(state, unused, out_3730504764862016933); } 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 f3a9348bc..6b5119a70 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_2567017903067559466); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1749285410370024184); -void car_H_mod_fun(double *state, double *out_8979551951706364182); -void car_f_fun(double *state, double dt, double *out_3674179640817441392); -void car_F_fun(double *state, double dt, double *out_258925546890284745); -void car_h_25(double *state, double *unused, double *out_5262304077966076166); -void car_H_25(double *state, double *unused, double *out_4563500296818522594); -void car_h_24(double *state, double *unused, double *out_2894074188837958464); -void car_H_24(double *state, double *unused, double *out_2012071509772995507); -void car_h_30(double *state, double *unused, double *out_9005980160461201074); -void car_H_30(double *state, double *unused, double *out_2045167338311273967); -void car_h_26(double *state, double *unused, double *out_8837448028571873025); -void car_H_26(double *state, double *unused, double *out_1258974327057721993); -void car_h_27(double *state, double *unused, double *out_6785398478329626304); -void car_H_27(double *state, double *unused, double *out_178426732872669250); -void car_h_29(double *state, double *unused, double *out_8023896067125902962); -void car_H_29(double *state, double *unused, double *out_1534935993996881783); -void car_h_28(double *state, double *unused, double *out_5617654677253484567); -void car_H_28(double *state, double *unused, double *out_6617335011066412357); -void car_h_31(double *state, double *unused, double *out_9123987554394577818); -void car_H_31(double *state, double *unused, double *out_4532854334941562166); +void car_err_fun(double *nom_x, double *delta_x, double *out_7149517118868685515); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1206282320038144058); +void car_H_mod_fun(double *state, double *out_2536050837023062909); +void car_f_fun(double *state, double dt, double *out_4666135608333935516); +void car_F_fun(double *state, double dt, double *out_2752692491938964849); +void car_h_25(double *state, double *unused, double *out_3787568519496882893); +void car_H_25(double *state, double *unused, double *out_3699858802985056505); +void car_h_24(double *state, double *unused, double *out_1528856480665249325); +void car_H_24(double *state, double *unused, double *out_1970995683807742436); +void car_h_30(double *state, double *unused, double *out_4709092161766857099); +void car_H_30(double *state, double *unused, double *out_7830194929232878356); +void car_h_26(double *state, double *unused, double *out_1815522905777326306); +void car_H_26(double *state, double *unused, double *out_41644515888999719); +void car_h_27(double *state, double *unused, double *out_8789241787157728538); +void car_H_27(double *state, double *unused, double *out_8441785832676248349); +void car_h_29(double *state, double *unused, double *out_8252567880601862062); +void car_H_29(double *state, double *unused, double *out_7319963584918486172); +void car_h_28(double *state, double *unused, double *out_5372968497508658522); +void car_H_28(double *state, double *unused, double *out_6044381471721534870); +void car_h_31(double *state, double *unused, double *out_6895910297391996282); +void car_H_31(double *state, double *unused, double *out_3730504764862016933); 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 415e6382d..67ac28975 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_2526150130430510074) { - out_2526150130430510074[0] = delta_x[0] + nom_x[0]; - out_2526150130430510074[1] = delta_x[1] + nom_x[1]; - out_2526150130430510074[2] = delta_x[2] + nom_x[2]; - out_2526150130430510074[3] = delta_x[3] + nom_x[3]; - out_2526150130430510074[4] = delta_x[4] + nom_x[4]; - out_2526150130430510074[5] = delta_x[5] + nom_x[5]; - out_2526150130430510074[6] = delta_x[6] + nom_x[6]; - out_2526150130430510074[7] = delta_x[7] + nom_x[7]; - out_2526150130430510074[8] = delta_x[8] + nom_x[8]; - out_2526150130430510074[9] = delta_x[9] + nom_x[9]; - out_2526150130430510074[10] = delta_x[10] + nom_x[10]; - out_2526150130430510074[11] = delta_x[11] + nom_x[11]; - out_2526150130430510074[12] = delta_x[12] + nom_x[12]; - out_2526150130430510074[13] = delta_x[13] + nom_x[13]; - out_2526150130430510074[14] = delta_x[14] + nom_x[14]; - out_2526150130430510074[15] = delta_x[15] + nom_x[15]; - out_2526150130430510074[16] = delta_x[16] + nom_x[16]; - out_2526150130430510074[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_8259662850093450970) { + out_8259662850093450970[0] = delta_x[0] + nom_x[0]; + out_8259662850093450970[1] = delta_x[1] + nom_x[1]; + out_8259662850093450970[2] = delta_x[2] + nom_x[2]; + out_8259662850093450970[3] = delta_x[3] + nom_x[3]; + out_8259662850093450970[4] = delta_x[4] + nom_x[4]; + out_8259662850093450970[5] = delta_x[5] + nom_x[5]; + out_8259662850093450970[6] = delta_x[6] + nom_x[6]; + out_8259662850093450970[7] = delta_x[7] + nom_x[7]; + out_8259662850093450970[8] = delta_x[8] + nom_x[8]; + out_8259662850093450970[9] = delta_x[9] + nom_x[9]; + out_8259662850093450970[10] = delta_x[10] + nom_x[10]; + out_8259662850093450970[11] = delta_x[11] + nom_x[11]; + out_8259662850093450970[12] = delta_x[12] + nom_x[12]; + out_8259662850093450970[13] = delta_x[13] + nom_x[13]; + out_8259662850093450970[14] = delta_x[14] + nom_x[14]; + out_8259662850093450970[15] = delta_x[15] + nom_x[15]; + out_8259662850093450970[16] = delta_x[16] + nom_x[16]; + out_8259662850093450970[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3515561648914426109) { - out_3515561648914426109[0] = -nom_x[0] + true_x[0]; - out_3515561648914426109[1] = -nom_x[1] + true_x[1]; - out_3515561648914426109[2] = -nom_x[2] + true_x[2]; - out_3515561648914426109[3] = -nom_x[3] + true_x[3]; - out_3515561648914426109[4] = -nom_x[4] + true_x[4]; - out_3515561648914426109[5] = -nom_x[5] + true_x[5]; - out_3515561648914426109[6] = -nom_x[6] + true_x[6]; - out_3515561648914426109[7] = -nom_x[7] + true_x[7]; - out_3515561648914426109[8] = -nom_x[8] + true_x[8]; - out_3515561648914426109[9] = -nom_x[9] + true_x[9]; - out_3515561648914426109[10] = -nom_x[10] + true_x[10]; - out_3515561648914426109[11] = -nom_x[11] + true_x[11]; - out_3515561648914426109[12] = -nom_x[12] + true_x[12]; - out_3515561648914426109[13] = -nom_x[13] + true_x[13]; - out_3515561648914426109[14] = -nom_x[14] + true_x[14]; - out_3515561648914426109[15] = -nom_x[15] + true_x[15]; - out_3515561648914426109[16] = -nom_x[16] + true_x[16]; - out_3515561648914426109[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_7477554158578246383) { + out_7477554158578246383[0] = -nom_x[0] + true_x[0]; + out_7477554158578246383[1] = -nom_x[1] + true_x[1]; + out_7477554158578246383[2] = -nom_x[2] + true_x[2]; + out_7477554158578246383[3] = -nom_x[3] + true_x[3]; + out_7477554158578246383[4] = -nom_x[4] + true_x[4]; + out_7477554158578246383[5] = -nom_x[5] + true_x[5]; + out_7477554158578246383[6] = -nom_x[6] + true_x[6]; + out_7477554158578246383[7] = -nom_x[7] + true_x[7]; + out_7477554158578246383[8] = -nom_x[8] + true_x[8]; + out_7477554158578246383[9] = -nom_x[9] + true_x[9]; + out_7477554158578246383[10] = -nom_x[10] + true_x[10]; + out_7477554158578246383[11] = -nom_x[11] + true_x[11]; + out_7477554158578246383[12] = -nom_x[12] + true_x[12]; + out_7477554158578246383[13] = -nom_x[13] + true_x[13]; + out_7477554158578246383[14] = -nom_x[14] + true_x[14]; + out_7477554158578246383[15] = -nom_x[15] + true_x[15]; + out_7477554158578246383[16] = -nom_x[16] + true_x[16]; + out_7477554158578246383[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_6056162720073724257) { - out_6056162720073724257[0] = 1.0; - out_6056162720073724257[1] = 0.0; - out_6056162720073724257[2] = 0.0; - out_6056162720073724257[3] = 0.0; - out_6056162720073724257[4] = 0.0; - out_6056162720073724257[5] = 0.0; - out_6056162720073724257[6] = 0.0; - out_6056162720073724257[7] = 0.0; - out_6056162720073724257[8] = 0.0; - out_6056162720073724257[9] = 0.0; - out_6056162720073724257[10] = 0.0; - out_6056162720073724257[11] = 0.0; - out_6056162720073724257[12] = 0.0; - out_6056162720073724257[13] = 0.0; - out_6056162720073724257[14] = 0.0; - out_6056162720073724257[15] = 0.0; - out_6056162720073724257[16] = 0.0; - out_6056162720073724257[17] = 0.0; - out_6056162720073724257[18] = 0.0; - out_6056162720073724257[19] = 1.0; - out_6056162720073724257[20] = 0.0; - out_6056162720073724257[21] = 0.0; - out_6056162720073724257[22] = 0.0; - out_6056162720073724257[23] = 0.0; - out_6056162720073724257[24] = 0.0; - out_6056162720073724257[25] = 0.0; - out_6056162720073724257[26] = 0.0; - out_6056162720073724257[27] = 0.0; - out_6056162720073724257[28] = 0.0; - out_6056162720073724257[29] = 0.0; - out_6056162720073724257[30] = 0.0; - out_6056162720073724257[31] = 0.0; - out_6056162720073724257[32] = 0.0; - out_6056162720073724257[33] = 0.0; - out_6056162720073724257[34] = 0.0; - out_6056162720073724257[35] = 0.0; - out_6056162720073724257[36] = 0.0; - out_6056162720073724257[37] = 0.0; - out_6056162720073724257[38] = 1.0; - out_6056162720073724257[39] = 0.0; - out_6056162720073724257[40] = 0.0; - out_6056162720073724257[41] = 0.0; - out_6056162720073724257[42] = 0.0; - out_6056162720073724257[43] = 0.0; - out_6056162720073724257[44] = 0.0; - out_6056162720073724257[45] = 0.0; - out_6056162720073724257[46] = 0.0; - out_6056162720073724257[47] = 0.0; - out_6056162720073724257[48] = 0.0; - out_6056162720073724257[49] = 0.0; - out_6056162720073724257[50] = 0.0; - out_6056162720073724257[51] = 0.0; - out_6056162720073724257[52] = 0.0; - out_6056162720073724257[53] = 0.0; - out_6056162720073724257[54] = 0.0; - out_6056162720073724257[55] = 0.0; - out_6056162720073724257[56] = 0.0; - out_6056162720073724257[57] = 1.0; - out_6056162720073724257[58] = 0.0; - out_6056162720073724257[59] = 0.0; - out_6056162720073724257[60] = 0.0; - out_6056162720073724257[61] = 0.0; - out_6056162720073724257[62] = 0.0; - out_6056162720073724257[63] = 0.0; - out_6056162720073724257[64] = 0.0; - out_6056162720073724257[65] = 0.0; - out_6056162720073724257[66] = 0.0; - out_6056162720073724257[67] = 0.0; - out_6056162720073724257[68] = 0.0; - out_6056162720073724257[69] = 0.0; - out_6056162720073724257[70] = 0.0; - out_6056162720073724257[71] = 0.0; - out_6056162720073724257[72] = 0.0; - out_6056162720073724257[73] = 0.0; - out_6056162720073724257[74] = 0.0; - out_6056162720073724257[75] = 0.0; - out_6056162720073724257[76] = 1.0; - out_6056162720073724257[77] = 0.0; - out_6056162720073724257[78] = 0.0; - out_6056162720073724257[79] = 0.0; - out_6056162720073724257[80] = 0.0; - out_6056162720073724257[81] = 0.0; - out_6056162720073724257[82] = 0.0; - out_6056162720073724257[83] = 0.0; - out_6056162720073724257[84] = 0.0; - out_6056162720073724257[85] = 0.0; - out_6056162720073724257[86] = 0.0; - out_6056162720073724257[87] = 0.0; - out_6056162720073724257[88] = 0.0; - out_6056162720073724257[89] = 0.0; - out_6056162720073724257[90] = 0.0; - out_6056162720073724257[91] = 0.0; - out_6056162720073724257[92] = 0.0; - out_6056162720073724257[93] = 0.0; - out_6056162720073724257[94] = 0.0; - out_6056162720073724257[95] = 1.0; - out_6056162720073724257[96] = 0.0; - out_6056162720073724257[97] = 0.0; - out_6056162720073724257[98] = 0.0; - out_6056162720073724257[99] = 0.0; - out_6056162720073724257[100] = 0.0; - out_6056162720073724257[101] = 0.0; - out_6056162720073724257[102] = 0.0; - out_6056162720073724257[103] = 0.0; - out_6056162720073724257[104] = 0.0; - out_6056162720073724257[105] = 0.0; - out_6056162720073724257[106] = 0.0; - out_6056162720073724257[107] = 0.0; - out_6056162720073724257[108] = 0.0; - out_6056162720073724257[109] = 0.0; - out_6056162720073724257[110] = 0.0; - out_6056162720073724257[111] = 0.0; - out_6056162720073724257[112] = 0.0; - out_6056162720073724257[113] = 0.0; - out_6056162720073724257[114] = 1.0; - out_6056162720073724257[115] = 0.0; - out_6056162720073724257[116] = 0.0; - out_6056162720073724257[117] = 0.0; - out_6056162720073724257[118] = 0.0; - out_6056162720073724257[119] = 0.0; - out_6056162720073724257[120] = 0.0; - out_6056162720073724257[121] = 0.0; - out_6056162720073724257[122] = 0.0; - out_6056162720073724257[123] = 0.0; - out_6056162720073724257[124] = 0.0; - out_6056162720073724257[125] = 0.0; - out_6056162720073724257[126] = 0.0; - out_6056162720073724257[127] = 0.0; - out_6056162720073724257[128] = 0.0; - out_6056162720073724257[129] = 0.0; - out_6056162720073724257[130] = 0.0; - out_6056162720073724257[131] = 0.0; - out_6056162720073724257[132] = 0.0; - out_6056162720073724257[133] = 1.0; - out_6056162720073724257[134] = 0.0; - out_6056162720073724257[135] = 0.0; - out_6056162720073724257[136] = 0.0; - out_6056162720073724257[137] = 0.0; - out_6056162720073724257[138] = 0.0; - out_6056162720073724257[139] = 0.0; - out_6056162720073724257[140] = 0.0; - out_6056162720073724257[141] = 0.0; - out_6056162720073724257[142] = 0.0; - out_6056162720073724257[143] = 0.0; - out_6056162720073724257[144] = 0.0; - out_6056162720073724257[145] = 0.0; - out_6056162720073724257[146] = 0.0; - out_6056162720073724257[147] = 0.0; - out_6056162720073724257[148] = 0.0; - out_6056162720073724257[149] = 0.0; - out_6056162720073724257[150] = 0.0; - out_6056162720073724257[151] = 0.0; - out_6056162720073724257[152] = 1.0; - out_6056162720073724257[153] = 0.0; - out_6056162720073724257[154] = 0.0; - out_6056162720073724257[155] = 0.0; - out_6056162720073724257[156] = 0.0; - out_6056162720073724257[157] = 0.0; - out_6056162720073724257[158] = 0.0; - out_6056162720073724257[159] = 0.0; - out_6056162720073724257[160] = 0.0; - out_6056162720073724257[161] = 0.0; - out_6056162720073724257[162] = 0.0; - out_6056162720073724257[163] = 0.0; - out_6056162720073724257[164] = 0.0; - out_6056162720073724257[165] = 0.0; - out_6056162720073724257[166] = 0.0; - out_6056162720073724257[167] = 0.0; - out_6056162720073724257[168] = 0.0; - out_6056162720073724257[169] = 0.0; - out_6056162720073724257[170] = 0.0; - out_6056162720073724257[171] = 1.0; - out_6056162720073724257[172] = 0.0; - out_6056162720073724257[173] = 0.0; - out_6056162720073724257[174] = 0.0; - out_6056162720073724257[175] = 0.0; - out_6056162720073724257[176] = 0.0; - out_6056162720073724257[177] = 0.0; - out_6056162720073724257[178] = 0.0; - out_6056162720073724257[179] = 0.0; - out_6056162720073724257[180] = 0.0; - out_6056162720073724257[181] = 0.0; - out_6056162720073724257[182] = 0.0; - out_6056162720073724257[183] = 0.0; - out_6056162720073724257[184] = 0.0; - out_6056162720073724257[185] = 0.0; - out_6056162720073724257[186] = 0.0; - out_6056162720073724257[187] = 0.0; - out_6056162720073724257[188] = 0.0; - out_6056162720073724257[189] = 0.0; - out_6056162720073724257[190] = 1.0; - out_6056162720073724257[191] = 0.0; - out_6056162720073724257[192] = 0.0; - out_6056162720073724257[193] = 0.0; - out_6056162720073724257[194] = 0.0; - out_6056162720073724257[195] = 0.0; - out_6056162720073724257[196] = 0.0; - out_6056162720073724257[197] = 0.0; - out_6056162720073724257[198] = 0.0; - out_6056162720073724257[199] = 0.0; - out_6056162720073724257[200] = 0.0; - out_6056162720073724257[201] = 0.0; - out_6056162720073724257[202] = 0.0; - out_6056162720073724257[203] = 0.0; - out_6056162720073724257[204] = 0.0; - out_6056162720073724257[205] = 0.0; - out_6056162720073724257[206] = 0.0; - out_6056162720073724257[207] = 0.0; - out_6056162720073724257[208] = 0.0; - out_6056162720073724257[209] = 1.0; - out_6056162720073724257[210] = 0.0; - out_6056162720073724257[211] = 0.0; - out_6056162720073724257[212] = 0.0; - out_6056162720073724257[213] = 0.0; - out_6056162720073724257[214] = 0.0; - out_6056162720073724257[215] = 0.0; - out_6056162720073724257[216] = 0.0; - out_6056162720073724257[217] = 0.0; - out_6056162720073724257[218] = 0.0; - out_6056162720073724257[219] = 0.0; - out_6056162720073724257[220] = 0.0; - out_6056162720073724257[221] = 0.0; - out_6056162720073724257[222] = 0.0; - out_6056162720073724257[223] = 0.0; - out_6056162720073724257[224] = 0.0; - out_6056162720073724257[225] = 0.0; - out_6056162720073724257[226] = 0.0; - out_6056162720073724257[227] = 0.0; - out_6056162720073724257[228] = 1.0; - out_6056162720073724257[229] = 0.0; - out_6056162720073724257[230] = 0.0; - out_6056162720073724257[231] = 0.0; - out_6056162720073724257[232] = 0.0; - out_6056162720073724257[233] = 0.0; - out_6056162720073724257[234] = 0.0; - out_6056162720073724257[235] = 0.0; - out_6056162720073724257[236] = 0.0; - out_6056162720073724257[237] = 0.0; - out_6056162720073724257[238] = 0.0; - out_6056162720073724257[239] = 0.0; - out_6056162720073724257[240] = 0.0; - out_6056162720073724257[241] = 0.0; - out_6056162720073724257[242] = 0.0; - out_6056162720073724257[243] = 0.0; - out_6056162720073724257[244] = 0.0; - out_6056162720073724257[245] = 0.0; - out_6056162720073724257[246] = 0.0; - out_6056162720073724257[247] = 1.0; - out_6056162720073724257[248] = 0.0; - out_6056162720073724257[249] = 0.0; - out_6056162720073724257[250] = 0.0; - out_6056162720073724257[251] = 0.0; - out_6056162720073724257[252] = 0.0; - out_6056162720073724257[253] = 0.0; - out_6056162720073724257[254] = 0.0; - out_6056162720073724257[255] = 0.0; - out_6056162720073724257[256] = 0.0; - out_6056162720073724257[257] = 0.0; - out_6056162720073724257[258] = 0.0; - out_6056162720073724257[259] = 0.0; - out_6056162720073724257[260] = 0.0; - out_6056162720073724257[261] = 0.0; - out_6056162720073724257[262] = 0.0; - out_6056162720073724257[263] = 0.0; - out_6056162720073724257[264] = 0.0; - out_6056162720073724257[265] = 0.0; - out_6056162720073724257[266] = 1.0; - out_6056162720073724257[267] = 0.0; - out_6056162720073724257[268] = 0.0; - out_6056162720073724257[269] = 0.0; - out_6056162720073724257[270] = 0.0; - out_6056162720073724257[271] = 0.0; - out_6056162720073724257[272] = 0.0; - out_6056162720073724257[273] = 0.0; - out_6056162720073724257[274] = 0.0; - out_6056162720073724257[275] = 0.0; - out_6056162720073724257[276] = 0.0; - out_6056162720073724257[277] = 0.0; - out_6056162720073724257[278] = 0.0; - out_6056162720073724257[279] = 0.0; - out_6056162720073724257[280] = 0.0; - out_6056162720073724257[281] = 0.0; - out_6056162720073724257[282] = 0.0; - out_6056162720073724257[283] = 0.0; - out_6056162720073724257[284] = 0.0; - out_6056162720073724257[285] = 1.0; - out_6056162720073724257[286] = 0.0; - out_6056162720073724257[287] = 0.0; - out_6056162720073724257[288] = 0.0; - out_6056162720073724257[289] = 0.0; - out_6056162720073724257[290] = 0.0; - out_6056162720073724257[291] = 0.0; - out_6056162720073724257[292] = 0.0; - out_6056162720073724257[293] = 0.0; - out_6056162720073724257[294] = 0.0; - out_6056162720073724257[295] = 0.0; - out_6056162720073724257[296] = 0.0; - out_6056162720073724257[297] = 0.0; - out_6056162720073724257[298] = 0.0; - out_6056162720073724257[299] = 0.0; - out_6056162720073724257[300] = 0.0; - out_6056162720073724257[301] = 0.0; - out_6056162720073724257[302] = 0.0; - out_6056162720073724257[303] = 0.0; - out_6056162720073724257[304] = 1.0; - out_6056162720073724257[305] = 0.0; - out_6056162720073724257[306] = 0.0; - out_6056162720073724257[307] = 0.0; - out_6056162720073724257[308] = 0.0; - out_6056162720073724257[309] = 0.0; - out_6056162720073724257[310] = 0.0; - out_6056162720073724257[311] = 0.0; - out_6056162720073724257[312] = 0.0; - out_6056162720073724257[313] = 0.0; - out_6056162720073724257[314] = 0.0; - out_6056162720073724257[315] = 0.0; - out_6056162720073724257[316] = 0.0; - out_6056162720073724257[317] = 0.0; - out_6056162720073724257[318] = 0.0; - out_6056162720073724257[319] = 0.0; - out_6056162720073724257[320] = 0.0; - out_6056162720073724257[321] = 0.0; - out_6056162720073724257[322] = 0.0; - out_6056162720073724257[323] = 1.0; +void H_mod_fun(double *state, double *out_6622639630704307229) { + out_6622639630704307229[0] = 1.0; + out_6622639630704307229[1] = 0.0; + out_6622639630704307229[2] = 0.0; + out_6622639630704307229[3] = 0.0; + out_6622639630704307229[4] = 0.0; + out_6622639630704307229[5] = 0.0; + out_6622639630704307229[6] = 0.0; + out_6622639630704307229[7] = 0.0; + out_6622639630704307229[8] = 0.0; + out_6622639630704307229[9] = 0.0; + out_6622639630704307229[10] = 0.0; + out_6622639630704307229[11] = 0.0; + out_6622639630704307229[12] = 0.0; + out_6622639630704307229[13] = 0.0; + out_6622639630704307229[14] = 0.0; + out_6622639630704307229[15] = 0.0; + out_6622639630704307229[16] = 0.0; + out_6622639630704307229[17] = 0.0; + out_6622639630704307229[18] = 0.0; + out_6622639630704307229[19] = 1.0; + out_6622639630704307229[20] = 0.0; + out_6622639630704307229[21] = 0.0; + out_6622639630704307229[22] = 0.0; + out_6622639630704307229[23] = 0.0; + out_6622639630704307229[24] = 0.0; + out_6622639630704307229[25] = 0.0; + out_6622639630704307229[26] = 0.0; + out_6622639630704307229[27] = 0.0; + out_6622639630704307229[28] = 0.0; + out_6622639630704307229[29] = 0.0; + out_6622639630704307229[30] = 0.0; + out_6622639630704307229[31] = 0.0; + out_6622639630704307229[32] = 0.0; + out_6622639630704307229[33] = 0.0; + out_6622639630704307229[34] = 0.0; + out_6622639630704307229[35] = 0.0; + out_6622639630704307229[36] = 0.0; + out_6622639630704307229[37] = 0.0; + out_6622639630704307229[38] = 1.0; + out_6622639630704307229[39] = 0.0; + out_6622639630704307229[40] = 0.0; + out_6622639630704307229[41] = 0.0; + out_6622639630704307229[42] = 0.0; + out_6622639630704307229[43] = 0.0; + out_6622639630704307229[44] = 0.0; + out_6622639630704307229[45] = 0.0; + out_6622639630704307229[46] = 0.0; + out_6622639630704307229[47] = 0.0; + out_6622639630704307229[48] = 0.0; + out_6622639630704307229[49] = 0.0; + out_6622639630704307229[50] = 0.0; + out_6622639630704307229[51] = 0.0; + out_6622639630704307229[52] = 0.0; + out_6622639630704307229[53] = 0.0; + out_6622639630704307229[54] = 0.0; + out_6622639630704307229[55] = 0.0; + out_6622639630704307229[56] = 0.0; + out_6622639630704307229[57] = 1.0; + out_6622639630704307229[58] = 0.0; + out_6622639630704307229[59] = 0.0; + out_6622639630704307229[60] = 0.0; + out_6622639630704307229[61] = 0.0; + out_6622639630704307229[62] = 0.0; + out_6622639630704307229[63] = 0.0; + out_6622639630704307229[64] = 0.0; + out_6622639630704307229[65] = 0.0; + out_6622639630704307229[66] = 0.0; + out_6622639630704307229[67] = 0.0; + out_6622639630704307229[68] = 0.0; + out_6622639630704307229[69] = 0.0; + out_6622639630704307229[70] = 0.0; + out_6622639630704307229[71] = 0.0; + out_6622639630704307229[72] = 0.0; + out_6622639630704307229[73] = 0.0; + out_6622639630704307229[74] = 0.0; + out_6622639630704307229[75] = 0.0; + out_6622639630704307229[76] = 1.0; + out_6622639630704307229[77] = 0.0; + out_6622639630704307229[78] = 0.0; + out_6622639630704307229[79] = 0.0; + out_6622639630704307229[80] = 0.0; + out_6622639630704307229[81] = 0.0; + out_6622639630704307229[82] = 0.0; + out_6622639630704307229[83] = 0.0; + out_6622639630704307229[84] = 0.0; + out_6622639630704307229[85] = 0.0; + out_6622639630704307229[86] = 0.0; + out_6622639630704307229[87] = 0.0; + out_6622639630704307229[88] = 0.0; + out_6622639630704307229[89] = 0.0; + out_6622639630704307229[90] = 0.0; + out_6622639630704307229[91] = 0.0; + out_6622639630704307229[92] = 0.0; + out_6622639630704307229[93] = 0.0; + out_6622639630704307229[94] = 0.0; + out_6622639630704307229[95] = 1.0; + out_6622639630704307229[96] = 0.0; + out_6622639630704307229[97] = 0.0; + out_6622639630704307229[98] = 0.0; + out_6622639630704307229[99] = 0.0; + out_6622639630704307229[100] = 0.0; + out_6622639630704307229[101] = 0.0; + out_6622639630704307229[102] = 0.0; + out_6622639630704307229[103] = 0.0; + out_6622639630704307229[104] = 0.0; + out_6622639630704307229[105] = 0.0; + out_6622639630704307229[106] = 0.0; + out_6622639630704307229[107] = 0.0; + out_6622639630704307229[108] = 0.0; + out_6622639630704307229[109] = 0.0; + out_6622639630704307229[110] = 0.0; + out_6622639630704307229[111] = 0.0; + out_6622639630704307229[112] = 0.0; + out_6622639630704307229[113] = 0.0; + out_6622639630704307229[114] = 1.0; + out_6622639630704307229[115] = 0.0; + out_6622639630704307229[116] = 0.0; + out_6622639630704307229[117] = 0.0; + out_6622639630704307229[118] = 0.0; + out_6622639630704307229[119] = 0.0; + out_6622639630704307229[120] = 0.0; + out_6622639630704307229[121] = 0.0; + out_6622639630704307229[122] = 0.0; + out_6622639630704307229[123] = 0.0; + out_6622639630704307229[124] = 0.0; + out_6622639630704307229[125] = 0.0; + out_6622639630704307229[126] = 0.0; + out_6622639630704307229[127] = 0.0; + out_6622639630704307229[128] = 0.0; + out_6622639630704307229[129] = 0.0; + out_6622639630704307229[130] = 0.0; + out_6622639630704307229[131] = 0.0; + out_6622639630704307229[132] = 0.0; + out_6622639630704307229[133] = 1.0; + out_6622639630704307229[134] = 0.0; + out_6622639630704307229[135] = 0.0; + out_6622639630704307229[136] = 0.0; + out_6622639630704307229[137] = 0.0; + out_6622639630704307229[138] = 0.0; + out_6622639630704307229[139] = 0.0; + out_6622639630704307229[140] = 0.0; + out_6622639630704307229[141] = 0.0; + out_6622639630704307229[142] = 0.0; + out_6622639630704307229[143] = 0.0; + out_6622639630704307229[144] = 0.0; + out_6622639630704307229[145] = 0.0; + out_6622639630704307229[146] = 0.0; + out_6622639630704307229[147] = 0.0; + out_6622639630704307229[148] = 0.0; + out_6622639630704307229[149] = 0.0; + out_6622639630704307229[150] = 0.0; + out_6622639630704307229[151] = 0.0; + out_6622639630704307229[152] = 1.0; + out_6622639630704307229[153] = 0.0; + out_6622639630704307229[154] = 0.0; + out_6622639630704307229[155] = 0.0; + out_6622639630704307229[156] = 0.0; + out_6622639630704307229[157] = 0.0; + out_6622639630704307229[158] = 0.0; + out_6622639630704307229[159] = 0.0; + out_6622639630704307229[160] = 0.0; + out_6622639630704307229[161] = 0.0; + out_6622639630704307229[162] = 0.0; + out_6622639630704307229[163] = 0.0; + out_6622639630704307229[164] = 0.0; + out_6622639630704307229[165] = 0.0; + out_6622639630704307229[166] = 0.0; + out_6622639630704307229[167] = 0.0; + out_6622639630704307229[168] = 0.0; + out_6622639630704307229[169] = 0.0; + out_6622639630704307229[170] = 0.0; + out_6622639630704307229[171] = 1.0; + out_6622639630704307229[172] = 0.0; + out_6622639630704307229[173] = 0.0; + out_6622639630704307229[174] = 0.0; + out_6622639630704307229[175] = 0.0; + out_6622639630704307229[176] = 0.0; + out_6622639630704307229[177] = 0.0; + out_6622639630704307229[178] = 0.0; + out_6622639630704307229[179] = 0.0; + out_6622639630704307229[180] = 0.0; + out_6622639630704307229[181] = 0.0; + out_6622639630704307229[182] = 0.0; + out_6622639630704307229[183] = 0.0; + out_6622639630704307229[184] = 0.0; + out_6622639630704307229[185] = 0.0; + out_6622639630704307229[186] = 0.0; + out_6622639630704307229[187] = 0.0; + out_6622639630704307229[188] = 0.0; + out_6622639630704307229[189] = 0.0; + out_6622639630704307229[190] = 1.0; + out_6622639630704307229[191] = 0.0; + out_6622639630704307229[192] = 0.0; + out_6622639630704307229[193] = 0.0; + out_6622639630704307229[194] = 0.0; + out_6622639630704307229[195] = 0.0; + out_6622639630704307229[196] = 0.0; + out_6622639630704307229[197] = 0.0; + out_6622639630704307229[198] = 0.0; + out_6622639630704307229[199] = 0.0; + out_6622639630704307229[200] = 0.0; + out_6622639630704307229[201] = 0.0; + out_6622639630704307229[202] = 0.0; + out_6622639630704307229[203] = 0.0; + out_6622639630704307229[204] = 0.0; + out_6622639630704307229[205] = 0.0; + out_6622639630704307229[206] = 0.0; + out_6622639630704307229[207] = 0.0; + out_6622639630704307229[208] = 0.0; + out_6622639630704307229[209] = 1.0; + out_6622639630704307229[210] = 0.0; + out_6622639630704307229[211] = 0.0; + out_6622639630704307229[212] = 0.0; + out_6622639630704307229[213] = 0.0; + out_6622639630704307229[214] = 0.0; + out_6622639630704307229[215] = 0.0; + out_6622639630704307229[216] = 0.0; + out_6622639630704307229[217] = 0.0; + out_6622639630704307229[218] = 0.0; + out_6622639630704307229[219] = 0.0; + out_6622639630704307229[220] = 0.0; + out_6622639630704307229[221] = 0.0; + out_6622639630704307229[222] = 0.0; + out_6622639630704307229[223] = 0.0; + out_6622639630704307229[224] = 0.0; + out_6622639630704307229[225] = 0.0; + out_6622639630704307229[226] = 0.0; + out_6622639630704307229[227] = 0.0; + out_6622639630704307229[228] = 1.0; + out_6622639630704307229[229] = 0.0; + out_6622639630704307229[230] = 0.0; + out_6622639630704307229[231] = 0.0; + out_6622639630704307229[232] = 0.0; + out_6622639630704307229[233] = 0.0; + out_6622639630704307229[234] = 0.0; + out_6622639630704307229[235] = 0.0; + out_6622639630704307229[236] = 0.0; + out_6622639630704307229[237] = 0.0; + out_6622639630704307229[238] = 0.0; + out_6622639630704307229[239] = 0.0; + out_6622639630704307229[240] = 0.0; + out_6622639630704307229[241] = 0.0; + out_6622639630704307229[242] = 0.0; + out_6622639630704307229[243] = 0.0; + out_6622639630704307229[244] = 0.0; + out_6622639630704307229[245] = 0.0; + out_6622639630704307229[246] = 0.0; + out_6622639630704307229[247] = 1.0; + out_6622639630704307229[248] = 0.0; + out_6622639630704307229[249] = 0.0; + out_6622639630704307229[250] = 0.0; + out_6622639630704307229[251] = 0.0; + out_6622639630704307229[252] = 0.0; + out_6622639630704307229[253] = 0.0; + out_6622639630704307229[254] = 0.0; + out_6622639630704307229[255] = 0.0; + out_6622639630704307229[256] = 0.0; + out_6622639630704307229[257] = 0.0; + out_6622639630704307229[258] = 0.0; + out_6622639630704307229[259] = 0.0; + out_6622639630704307229[260] = 0.0; + out_6622639630704307229[261] = 0.0; + out_6622639630704307229[262] = 0.0; + out_6622639630704307229[263] = 0.0; + out_6622639630704307229[264] = 0.0; + out_6622639630704307229[265] = 0.0; + out_6622639630704307229[266] = 1.0; + out_6622639630704307229[267] = 0.0; + out_6622639630704307229[268] = 0.0; + out_6622639630704307229[269] = 0.0; + out_6622639630704307229[270] = 0.0; + out_6622639630704307229[271] = 0.0; + out_6622639630704307229[272] = 0.0; + out_6622639630704307229[273] = 0.0; + out_6622639630704307229[274] = 0.0; + out_6622639630704307229[275] = 0.0; + out_6622639630704307229[276] = 0.0; + out_6622639630704307229[277] = 0.0; + out_6622639630704307229[278] = 0.0; + out_6622639630704307229[279] = 0.0; + out_6622639630704307229[280] = 0.0; + out_6622639630704307229[281] = 0.0; + out_6622639630704307229[282] = 0.0; + out_6622639630704307229[283] = 0.0; + out_6622639630704307229[284] = 0.0; + out_6622639630704307229[285] = 1.0; + out_6622639630704307229[286] = 0.0; + out_6622639630704307229[287] = 0.0; + out_6622639630704307229[288] = 0.0; + out_6622639630704307229[289] = 0.0; + out_6622639630704307229[290] = 0.0; + out_6622639630704307229[291] = 0.0; + out_6622639630704307229[292] = 0.0; + out_6622639630704307229[293] = 0.0; + out_6622639630704307229[294] = 0.0; + out_6622639630704307229[295] = 0.0; + out_6622639630704307229[296] = 0.0; + out_6622639630704307229[297] = 0.0; + out_6622639630704307229[298] = 0.0; + out_6622639630704307229[299] = 0.0; + out_6622639630704307229[300] = 0.0; + out_6622639630704307229[301] = 0.0; + out_6622639630704307229[302] = 0.0; + out_6622639630704307229[303] = 0.0; + out_6622639630704307229[304] = 1.0; + out_6622639630704307229[305] = 0.0; + out_6622639630704307229[306] = 0.0; + out_6622639630704307229[307] = 0.0; + out_6622639630704307229[308] = 0.0; + out_6622639630704307229[309] = 0.0; + out_6622639630704307229[310] = 0.0; + out_6622639630704307229[311] = 0.0; + out_6622639630704307229[312] = 0.0; + out_6622639630704307229[313] = 0.0; + out_6622639630704307229[314] = 0.0; + out_6622639630704307229[315] = 0.0; + out_6622639630704307229[316] = 0.0; + out_6622639630704307229[317] = 0.0; + out_6622639630704307229[318] = 0.0; + out_6622639630704307229[319] = 0.0; + out_6622639630704307229[320] = 0.0; + out_6622639630704307229[321] = 0.0; + out_6622639630704307229[322] = 0.0; + out_6622639630704307229[323] = 1.0; } -void f_fun(double *state, double dt, double *out_8780284750928315681) { - out_8780284750928315681[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_8780284750928315681[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_8780284750928315681[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_8780284750928315681[3] = dt*state[12] + state[3]; - out_8780284750928315681[4] = dt*state[13] + state[4]; - out_8780284750928315681[5] = dt*state[14] + state[5]; - out_8780284750928315681[6] = state[6]; - out_8780284750928315681[7] = state[7]; - out_8780284750928315681[8] = state[8]; - out_8780284750928315681[9] = state[9]; - out_8780284750928315681[10] = state[10]; - out_8780284750928315681[11] = state[11]; - out_8780284750928315681[12] = state[12]; - out_8780284750928315681[13] = state[13]; - out_8780284750928315681[14] = state[14]; - out_8780284750928315681[15] = state[15]; - out_8780284750928315681[16] = state[16]; - out_8780284750928315681[17] = state[17]; +void f_fun(double *state, double dt, double *out_2747407686181114080) { + out_2747407686181114080[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_2747407686181114080[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_2747407686181114080[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_2747407686181114080[3] = dt*state[12] + state[3]; + out_2747407686181114080[4] = dt*state[13] + state[4]; + out_2747407686181114080[5] = dt*state[14] + state[5]; + out_2747407686181114080[6] = state[6]; + out_2747407686181114080[7] = state[7]; + out_2747407686181114080[8] = state[8]; + out_2747407686181114080[9] = state[9]; + out_2747407686181114080[10] = state[10]; + out_2747407686181114080[11] = state[11]; + out_2747407686181114080[12] = state[12]; + out_2747407686181114080[13] = state[13]; + out_2747407686181114080[14] = state[14]; + out_2747407686181114080[15] = state[15]; + out_2747407686181114080[16] = state[16]; + out_2747407686181114080[17] = state[17]; } -void F_fun(double *state, double dt, double *out_1516035527731464330) { - out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[2] = 0; - out_1516035527731464330[3] = 0; - out_1516035527731464330[4] = 0; - out_1516035527731464330[5] = 0; - out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[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_1516035527731464330[9] = 0; - out_1516035527731464330[10] = 0; - out_1516035527731464330[11] = 0; - out_1516035527731464330[12] = 0; - out_1516035527731464330[13] = 0; - out_1516035527731464330[14] = 0; - out_1516035527731464330[15] = 0; - out_1516035527731464330[16] = 0; - out_1516035527731464330[17] = 0; - out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[20] = 0; - out_1516035527731464330[21] = 0; - out_1516035527731464330[22] = 0; - out_1516035527731464330[23] = 0; - out_1516035527731464330[24] = 0; - out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[27] = 0; - out_1516035527731464330[28] = 0; - out_1516035527731464330[29] = 0; - out_1516035527731464330[30] = 0; - out_1516035527731464330[31] = 0; - out_1516035527731464330[32] = 0; - out_1516035527731464330[33] = 0; - out_1516035527731464330[34] = 0; - out_1516035527731464330[35] = 0; - out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[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_1516035527731464330[39] = 0; - out_1516035527731464330[40] = 0; - out_1516035527731464330[41] = 0; - out_1516035527731464330[42] = 0; - out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[45] = 0; - out_1516035527731464330[46] = 0; - out_1516035527731464330[47] = 0; - out_1516035527731464330[48] = 0; - out_1516035527731464330[49] = 0; - out_1516035527731464330[50] = 0; - out_1516035527731464330[51] = 0; - out_1516035527731464330[52] = 0; - out_1516035527731464330[53] = 0; - out_1516035527731464330[54] = 0; - out_1516035527731464330[55] = 0; - out_1516035527731464330[56] = 0; - out_1516035527731464330[57] = 1; - out_1516035527731464330[58] = 0; - out_1516035527731464330[59] = 0; - out_1516035527731464330[60] = 0; - out_1516035527731464330[61] = 0; - out_1516035527731464330[62] = 0; - out_1516035527731464330[63] = 0; - out_1516035527731464330[64] = 0; - out_1516035527731464330[65] = 0; - out_1516035527731464330[66] = dt; - out_1516035527731464330[67] = 0; - out_1516035527731464330[68] = 0; - out_1516035527731464330[69] = 0; - out_1516035527731464330[70] = 0; - out_1516035527731464330[71] = 0; - out_1516035527731464330[72] = 0; - out_1516035527731464330[73] = 0; - out_1516035527731464330[74] = 0; - out_1516035527731464330[75] = 0; - out_1516035527731464330[76] = 1; - out_1516035527731464330[77] = 0; - out_1516035527731464330[78] = 0; - out_1516035527731464330[79] = 0; - out_1516035527731464330[80] = 0; - out_1516035527731464330[81] = 0; - out_1516035527731464330[82] = 0; - out_1516035527731464330[83] = 0; - out_1516035527731464330[84] = 0; - out_1516035527731464330[85] = dt; - out_1516035527731464330[86] = 0; - out_1516035527731464330[87] = 0; - out_1516035527731464330[88] = 0; - out_1516035527731464330[89] = 0; - out_1516035527731464330[90] = 0; - out_1516035527731464330[91] = 0; - out_1516035527731464330[92] = 0; - out_1516035527731464330[93] = 0; - out_1516035527731464330[94] = 0; - out_1516035527731464330[95] = 1; - out_1516035527731464330[96] = 0; - out_1516035527731464330[97] = 0; - out_1516035527731464330[98] = 0; - out_1516035527731464330[99] = 0; - out_1516035527731464330[100] = 0; - out_1516035527731464330[101] = 0; - out_1516035527731464330[102] = 0; - out_1516035527731464330[103] = 0; - out_1516035527731464330[104] = dt; - out_1516035527731464330[105] = 0; - out_1516035527731464330[106] = 0; - out_1516035527731464330[107] = 0; - out_1516035527731464330[108] = 0; - out_1516035527731464330[109] = 0; - out_1516035527731464330[110] = 0; - out_1516035527731464330[111] = 0; - out_1516035527731464330[112] = 0; - out_1516035527731464330[113] = 0; - out_1516035527731464330[114] = 1; - out_1516035527731464330[115] = 0; - out_1516035527731464330[116] = 0; - out_1516035527731464330[117] = 0; - out_1516035527731464330[118] = 0; - out_1516035527731464330[119] = 0; - out_1516035527731464330[120] = 0; - out_1516035527731464330[121] = 0; - out_1516035527731464330[122] = 0; - out_1516035527731464330[123] = 0; - out_1516035527731464330[124] = 0; - out_1516035527731464330[125] = 0; - out_1516035527731464330[126] = 0; - out_1516035527731464330[127] = 0; - out_1516035527731464330[128] = 0; - out_1516035527731464330[129] = 0; - out_1516035527731464330[130] = 0; - out_1516035527731464330[131] = 0; - out_1516035527731464330[132] = 0; - out_1516035527731464330[133] = 1; - out_1516035527731464330[134] = 0; - out_1516035527731464330[135] = 0; - out_1516035527731464330[136] = 0; - out_1516035527731464330[137] = 0; - out_1516035527731464330[138] = 0; - out_1516035527731464330[139] = 0; - out_1516035527731464330[140] = 0; - out_1516035527731464330[141] = 0; - out_1516035527731464330[142] = 0; - out_1516035527731464330[143] = 0; - out_1516035527731464330[144] = 0; - out_1516035527731464330[145] = 0; - out_1516035527731464330[146] = 0; - out_1516035527731464330[147] = 0; - out_1516035527731464330[148] = 0; - out_1516035527731464330[149] = 0; - out_1516035527731464330[150] = 0; - out_1516035527731464330[151] = 0; - out_1516035527731464330[152] = 1; - out_1516035527731464330[153] = 0; - out_1516035527731464330[154] = 0; - out_1516035527731464330[155] = 0; - out_1516035527731464330[156] = 0; - out_1516035527731464330[157] = 0; - out_1516035527731464330[158] = 0; - out_1516035527731464330[159] = 0; - out_1516035527731464330[160] = 0; - out_1516035527731464330[161] = 0; - out_1516035527731464330[162] = 0; - out_1516035527731464330[163] = 0; - out_1516035527731464330[164] = 0; - out_1516035527731464330[165] = 0; - out_1516035527731464330[166] = 0; - out_1516035527731464330[167] = 0; - out_1516035527731464330[168] = 0; - out_1516035527731464330[169] = 0; - out_1516035527731464330[170] = 0; - out_1516035527731464330[171] = 1; - out_1516035527731464330[172] = 0; - out_1516035527731464330[173] = 0; - out_1516035527731464330[174] = 0; - out_1516035527731464330[175] = 0; - out_1516035527731464330[176] = 0; - out_1516035527731464330[177] = 0; - out_1516035527731464330[178] = 0; - out_1516035527731464330[179] = 0; - out_1516035527731464330[180] = 0; - out_1516035527731464330[181] = 0; - out_1516035527731464330[182] = 0; - out_1516035527731464330[183] = 0; - out_1516035527731464330[184] = 0; - out_1516035527731464330[185] = 0; - out_1516035527731464330[186] = 0; - out_1516035527731464330[187] = 0; - out_1516035527731464330[188] = 0; - out_1516035527731464330[189] = 0; - out_1516035527731464330[190] = 1; - out_1516035527731464330[191] = 0; - out_1516035527731464330[192] = 0; - out_1516035527731464330[193] = 0; - out_1516035527731464330[194] = 0; - out_1516035527731464330[195] = 0; - out_1516035527731464330[196] = 0; - out_1516035527731464330[197] = 0; - out_1516035527731464330[198] = 0; - out_1516035527731464330[199] = 0; - out_1516035527731464330[200] = 0; - out_1516035527731464330[201] = 0; - out_1516035527731464330[202] = 0; - out_1516035527731464330[203] = 0; - out_1516035527731464330[204] = 0; - out_1516035527731464330[205] = 0; - out_1516035527731464330[206] = 0; - out_1516035527731464330[207] = 0; - out_1516035527731464330[208] = 0; - out_1516035527731464330[209] = 1; - out_1516035527731464330[210] = 0; - out_1516035527731464330[211] = 0; - out_1516035527731464330[212] = 0; - out_1516035527731464330[213] = 0; - out_1516035527731464330[214] = 0; - out_1516035527731464330[215] = 0; - out_1516035527731464330[216] = 0; - out_1516035527731464330[217] = 0; - out_1516035527731464330[218] = 0; - out_1516035527731464330[219] = 0; - out_1516035527731464330[220] = 0; - out_1516035527731464330[221] = 0; - out_1516035527731464330[222] = 0; - out_1516035527731464330[223] = 0; - out_1516035527731464330[224] = 0; - out_1516035527731464330[225] = 0; - out_1516035527731464330[226] = 0; - out_1516035527731464330[227] = 0; - out_1516035527731464330[228] = 1; - out_1516035527731464330[229] = 0; - out_1516035527731464330[230] = 0; - out_1516035527731464330[231] = 0; - out_1516035527731464330[232] = 0; - out_1516035527731464330[233] = 0; - out_1516035527731464330[234] = 0; - out_1516035527731464330[235] = 0; - out_1516035527731464330[236] = 0; - out_1516035527731464330[237] = 0; - out_1516035527731464330[238] = 0; - out_1516035527731464330[239] = 0; - out_1516035527731464330[240] = 0; - out_1516035527731464330[241] = 0; - out_1516035527731464330[242] = 0; - out_1516035527731464330[243] = 0; - out_1516035527731464330[244] = 0; - out_1516035527731464330[245] = 0; - out_1516035527731464330[246] = 0; - out_1516035527731464330[247] = 1; - out_1516035527731464330[248] = 0; - out_1516035527731464330[249] = 0; - out_1516035527731464330[250] = 0; - out_1516035527731464330[251] = 0; - out_1516035527731464330[252] = 0; - out_1516035527731464330[253] = 0; - out_1516035527731464330[254] = 0; - out_1516035527731464330[255] = 0; - out_1516035527731464330[256] = 0; - out_1516035527731464330[257] = 0; - out_1516035527731464330[258] = 0; - out_1516035527731464330[259] = 0; - out_1516035527731464330[260] = 0; - out_1516035527731464330[261] = 0; - out_1516035527731464330[262] = 0; - out_1516035527731464330[263] = 0; - out_1516035527731464330[264] = 0; - out_1516035527731464330[265] = 0; - out_1516035527731464330[266] = 1; - out_1516035527731464330[267] = 0; - out_1516035527731464330[268] = 0; - out_1516035527731464330[269] = 0; - out_1516035527731464330[270] = 0; - out_1516035527731464330[271] = 0; - out_1516035527731464330[272] = 0; - out_1516035527731464330[273] = 0; - out_1516035527731464330[274] = 0; - out_1516035527731464330[275] = 0; - out_1516035527731464330[276] = 0; - out_1516035527731464330[277] = 0; - out_1516035527731464330[278] = 0; - out_1516035527731464330[279] = 0; - out_1516035527731464330[280] = 0; - out_1516035527731464330[281] = 0; - out_1516035527731464330[282] = 0; - out_1516035527731464330[283] = 0; - out_1516035527731464330[284] = 0; - out_1516035527731464330[285] = 1; - out_1516035527731464330[286] = 0; - out_1516035527731464330[287] = 0; - out_1516035527731464330[288] = 0; - out_1516035527731464330[289] = 0; - out_1516035527731464330[290] = 0; - out_1516035527731464330[291] = 0; - out_1516035527731464330[292] = 0; - out_1516035527731464330[293] = 0; - out_1516035527731464330[294] = 0; - out_1516035527731464330[295] = 0; - out_1516035527731464330[296] = 0; - out_1516035527731464330[297] = 0; - out_1516035527731464330[298] = 0; - out_1516035527731464330[299] = 0; - out_1516035527731464330[300] = 0; - out_1516035527731464330[301] = 0; - out_1516035527731464330[302] = 0; - out_1516035527731464330[303] = 0; - out_1516035527731464330[304] = 1; - out_1516035527731464330[305] = 0; - out_1516035527731464330[306] = 0; - out_1516035527731464330[307] = 0; - out_1516035527731464330[308] = 0; - out_1516035527731464330[309] = 0; - out_1516035527731464330[310] = 0; - out_1516035527731464330[311] = 0; - out_1516035527731464330[312] = 0; - out_1516035527731464330[313] = 0; - out_1516035527731464330[314] = 0; - out_1516035527731464330[315] = 0; - out_1516035527731464330[316] = 0; - out_1516035527731464330[317] = 0; - out_1516035527731464330[318] = 0; - out_1516035527731464330[319] = 0; - out_1516035527731464330[320] = 0; - out_1516035527731464330[321] = 0; - out_1516035527731464330[322] = 0; - out_1516035527731464330[323] = 1; +void F_fun(double *state, double dt, double *out_5168326300746653192) { + out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[2] = 0; + out_5168326300746653192[3] = 0; + out_5168326300746653192[4] = 0; + out_5168326300746653192[5] = 0; + out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[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_5168326300746653192[9] = 0; + out_5168326300746653192[10] = 0; + out_5168326300746653192[11] = 0; + out_5168326300746653192[12] = 0; + out_5168326300746653192[13] = 0; + out_5168326300746653192[14] = 0; + out_5168326300746653192[15] = 0; + out_5168326300746653192[16] = 0; + out_5168326300746653192[17] = 0; + out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[20] = 0; + out_5168326300746653192[21] = 0; + out_5168326300746653192[22] = 0; + out_5168326300746653192[23] = 0; + out_5168326300746653192[24] = 0; + out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[27] = 0; + out_5168326300746653192[28] = 0; + out_5168326300746653192[29] = 0; + out_5168326300746653192[30] = 0; + out_5168326300746653192[31] = 0; + out_5168326300746653192[32] = 0; + out_5168326300746653192[33] = 0; + out_5168326300746653192[34] = 0; + out_5168326300746653192[35] = 0; + out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[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_5168326300746653192[39] = 0; + out_5168326300746653192[40] = 0; + out_5168326300746653192[41] = 0; + out_5168326300746653192[42] = 0; + out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[45] = 0; + out_5168326300746653192[46] = 0; + out_5168326300746653192[47] = 0; + out_5168326300746653192[48] = 0; + out_5168326300746653192[49] = 0; + out_5168326300746653192[50] = 0; + out_5168326300746653192[51] = 0; + out_5168326300746653192[52] = 0; + out_5168326300746653192[53] = 0; + out_5168326300746653192[54] = 0; + out_5168326300746653192[55] = 0; + out_5168326300746653192[56] = 0; + out_5168326300746653192[57] = 1; + out_5168326300746653192[58] = 0; + out_5168326300746653192[59] = 0; + out_5168326300746653192[60] = 0; + out_5168326300746653192[61] = 0; + out_5168326300746653192[62] = 0; + out_5168326300746653192[63] = 0; + out_5168326300746653192[64] = 0; + out_5168326300746653192[65] = 0; + out_5168326300746653192[66] = dt; + out_5168326300746653192[67] = 0; + out_5168326300746653192[68] = 0; + out_5168326300746653192[69] = 0; + out_5168326300746653192[70] = 0; + out_5168326300746653192[71] = 0; + out_5168326300746653192[72] = 0; + out_5168326300746653192[73] = 0; + out_5168326300746653192[74] = 0; + out_5168326300746653192[75] = 0; + out_5168326300746653192[76] = 1; + out_5168326300746653192[77] = 0; + out_5168326300746653192[78] = 0; + out_5168326300746653192[79] = 0; + out_5168326300746653192[80] = 0; + out_5168326300746653192[81] = 0; + out_5168326300746653192[82] = 0; + out_5168326300746653192[83] = 0; + out_5168326300746653192[84] = 0; + out_5168326300746653192[85] = dt; + out_5168326300746653192[86] = 0; + out_5168326300746653192[87] = 0; + out_5168326300746653192[88] = 0; + out_5168326300746653192[89] = 0; + out_5168326300746653192[90] = 0; + out_5168326300746653192[91] = 0; + out_5168326300746653192[92] = 0; + out_5168326300746653192[93] = 0; + out_5168326300746653192[94] = 0; + out_5168326300746653192[95] = 1; + out_5168326300746653192[96] = 0; + out_5168326300746653192[97] = 0; + out_5168326300746653192[98] = 0; + out_5168326300746653192[99] = 0; + out_5168326300746653192[100] = 0; + out_5168326300746653192[101] = 0; + out_5168326300746653192[102] = 0; + out_5168326300746653192[103] = 0; + out_5168326300746653192[104] = dt; + out_5168326300746653192[105] = 0; + out_5168326300746653192[106] = 0; + out_5168326300746653192[107] = 0; + out_5168326300746653192[108] = 0; + out_5168326300746653192[109] = 0; + out_5168326300746653192[110] = 0; + out_5168326300746653192[111] = 0; + out_5168326300746653192[112] = 0; + out_5168326300746653192[113] = 0; + out_5168326300746653192[114] = 1; + out_5168326300746653192[115] = 0; + out_5168326300746653192[116] = 0; + out_5168326300746653192[117] = 0; + out_5168326300746653192[118] = 0; + out_5168326300746653192[119] = 0; + out_5168326300746653192[120] = 0; + out_5168326300746653192[121] = 0; + out_5168326300746653192[122] = 0; + out_5168326300746653192[123] = 0; + out_5168326300746653192[124] = 0; + out_5168326300746653192[125] = 0; + out_5168326300746653192[126] = 0; + out_5168326300746653192[127] = 0; + out_5168326300746653192[128] = 0; + out_5168326300746653192[129] = 0; + out_5168326300746653192[130] = 0; + out_5168326300746653192[131] = 0; + out_5168326300746653192[132] = 0; + out_5168326300746653192[133] = 1; + out_5168326300746653192[134] = 0; + out_5168326300746653192[135] = 0; + out_5168326300746653192[136] = 0; + out_5168326300746653192[137] = 0; + out_5168326300746653192[138] = 0; + out_5168326300746653192[139] = 0; + out_5168326300746653192[140] = 0; + out_5168326300746653192[141] = 0; + out_5168326300746653192[142] = 0; + out_5168326300746653192[143] = 0; + out_5168326300746653192[144] = 0; + out_5168326300746653192[145] = 0; + out_5168326300746653192[146] = 0; + out_5168326300746653192[147] = 0; + out_5168326300746653192[148] = 0; + out_5168326300746653192[149] = 0; + out_5168326300746653192[150] = 0; + out_5168326300746653192[151] = 0; + out_5168326300746653192[152] = 1; + out_5168326300746653192[153] = 0; + out_5168326300746653192[154] = 0; + out_5168326300746653192[155] = 0; + out_5168326300746653192[156] = 0; + out_5168326300746653192[157] = 0; + out_5168326300746653192[158] = 0; + out_5168326300746653192[159] = 0; + out_5168326300746653192[160] = 0; + out_5168326300746653192[161] = 0; + out_5168326300746653192[162] = 0; + out_5168326300746653192[163] = 0; + out_5168326300746653192[164] = 0; + out_5168326300746653192[165] = 0; + out_5168326300746653192[166] = 0; + out_5168326300746653192[167] = 0; + out_5168326300746653192[168] = 0; + out_5168326300746653192[169] = 0; + out_5168326300746653192[170] = 0; + out_5168326300746653192[171] = 1; + out_5168326300746653192[172] = 0; + out_5168326300746653192[173] = 0; + out_5168326300746653192[174] = 0; + out_5168326300746653192[175] = 0; + out_5168326300746653192[176] = 0; + out_5168326300746653192[177] = 0; + out_5168326300746653192[178] = 0; + out_5168326300746653192[179] = 0; + out_5168326300746653192[180] = 0; + out_5168326300746653192[181] = 0; + out_5168326300746653192[182] = 0; + out_5168326300746653192[183] = 0; + out_5168326300746653192[184] = 0; + out_5168326300746653192[185] = 0; + out_5168326300746653192[186] = 0; + out_5168326300746653192[187] = 0; + out_5168326300746653192[188] = 0; + out_5168326300746653192[189] = 0; + out_5168326300746653192[190] = 1; + out_5168326300746653192[191] = 0; + out_5168326300746653192[192] = 0; + out_5168326300746653192[193] = 0; + out_5168326300746653192[194] = 0; + out_5168326300746653192[195] = 0; + out_5168326300746653192[196] = 0; + out_5168326300746653192[197] = 0; + out_5168326300746653192[198] = 0; + out_5168326300746653192[199] = 0; + out_5168326300746653192[200] = 0; + out_5168326300746653192[201] = 0; + out_5168326300746653192[202] = 0; + out_5168326300746653192[203] = 0; + out_5168326300746653192[204] = 0; + out_5168326300746653192[205] = 0; + out_5168326300746653192[206] = 0; + out_5168326300746653192[207] = 0; + out_5168326300746653192[208] = 0; + out_5168326300746653192[209] = 1; + out_5168326300746653192[210] = 0; + out_5168326300746653192[211] = 0; + out_5168326300746653192[212] = 0; + out_5168326300746653192[213] = 0; + out_5168326300746653192[214] = 0; + out_5168326300746653192[215] = 0; + out_5168326300746653192[216] = 0; + out_5168326300746653192[217] = 0; + out_5168326300746653192[218] = 0; + out_5168326300746653192[219] = 0; + out_5168326300746653192[220] = 0; + out_5168326300746653192[221] = 0; + out_5168326300746653192[222] = 0; + out_5168326300746653192[223] = 0; + out_5168326300746653192[224] = 0; + out_5168326300746653192[225] = 0; + out_5168326300746653192[226] = 0; + out_5168326300746653192[227] = 0; + out_5168326300746653192[228] = 1; + out_5168326300746653192[229] = 0; + out_5168326300746653192[230] = 0; + out_5168326300746653192[231] = 0; + out_5168326300746653192[232] = 0; + out_5168326300746653192[233] = 0; + out_5168326300746653192[234] = 0; + out_5168326300746653192[235] = 0; + out_5168326300746653192[236] = 0; + out_5168326300746653192[237] = 0; + out_5168326300746653192[238] = 0; + out_5168326300746653192[239] = 0; + out_5168326300746653192[240] = 0; + out_5168326300746653192[241] = 0; + out_5168326300746653192[242] = 0; + out_5168326300746653192[243] = 0; + out_5168326300746653192[244] = 0; + out_5168326300746653192[245] = 0; + out_5168326300746653192[246] = 0; + out_5168326300746653192[247] = 1; + out_5168326300746653192[248] = 0; + out_5168326300746653192[249] = 0; + out_5168326300746653192[250] = 0; + out_5168326300746653192[251] = 0; + out_5168326300746653192[252] = 0; + out_5168326300746653192[253] = 0; + out_5168326300746653192[254] = 0; + out_5168326300746653192[255] = 0; + out_5168326300746653192[256] = 0; + out_5168326300746653192[257] = 0; + out_5168326300746653192[258] = 0; + out_5168326300746653192[259] = 0; + out_5168326300746653192[260] = 0; + out_5168326300746653192[261] = 0; + out_5168326300746653192[262] = 0; + out_5168326300746653192[263] = 0; + out_5168326300746653192[264] = 0; + out_5168326300746653192[265] = 0; + out_5168326300746653192[266] = 1; + out_5168326300746653192[267] = 0; + out_5168326300746653192[268] = 0; + out_5168326300746653192[269] = 0; + out_5168326300746653192[270] = 0; + out_5168326300746653192[271] = 0; + out_5168326300746653192[272] = 0; + out_5168326300746653192[273] = 0; + out_5168326300746653192[274] = 0; + out_5168326300746653192[275] = 0; + out_5168326300746653192[276] = 0; + out_5168326300746653192[277] = 0; + out_5168326300746653192[278] = 0; + out_5168326300746653192[279] = 0; + out_5168326300746653192[280] = 0; + out_5168326300746653192[281] = 0; + out_5168326300746653192[282] = 0; + out_5168326300746653192[283] = 0; + out_5168326300746653192[284] = 0; + out_5168326300746653192[285] = 1; + out_5168326300746653192[286] = 0; + out_5168326300746653192[287] = 0; + out_5168326300746653192[288] = 0; + out_5168326300746653192[289] = 0; + out_5168326300746653192[290] = 0; + out_5168326300746653192[291] = 0; + out_5168326300746653192[292] = 0; + out_5168326300746653192[293] = 0; + out_5168326300746653192[294] = 0; + out_5168326300746653192[295] = 0; + out_5168326300746653192[296] = 0; + out_5168326300746653192[297] = 0; + out_5168326300746653192[298] = 0; + out_5168326300746653192[299] = 0; + out_5168326300746653192[300] = 0; + out_5168326300746653192[301] = 0; + out_5168326300746653192[302] = 0; + out_5168326300746653192[303] = 0; + out_5168326300746653192[304] = 1; + out_5168326300746653192[305] = 0; + out_5168326300746653192[306] = 0; + out_5168326300746653192[307] = 0; + out_5168326300746653192[308] = 0; + out_5168326300746653192[309] = 0; + out_5168326300746653192[310] = 0; + out_5168326300746653192[311] = 0; + out_5168326300746653192[312] = 0; + out_5168326300746653192[313] = 0; + out_5168326300746653192[314] = 0; + out_5168326300746653192[315] = 0; + out_5168326300746653192[316] = 0; + out_5168326300746653192[317] = 0; + out_5168326300746653192[318] = 0; + out_5168326300746653192[319] = 0; + out_5168326300746653192[320] = 0; + out_5168326300746653192[321] = 0; + out_5168326300746653192[322] = 0; + out_5168326300746653192[323] = 1; } -void h_4(double *state, double *unused, double *out_4492721189395603550) { - out_4492721189395603550[0] = state[6] + state[9]; - out_4492721189395603550[1] = state[7] + state[10]; - out_4492721189395603550[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_8670716112105694718) { + out_8670716112105694718[0] = state[6] + state[9]; + out_8670716112105694718[1] = state[7] + state[10]; + out_8670716112105694718[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_4858215238190826153) { - out_4858215238190826153[0] = 0; - out_4858215238190826153[1] = 0; - out_4858215238190826153[2] = 0; - out_4858215238190826153[3] = 0; - out_4858215238190826153[4] = 0; - out_4858215238190826153[5] = 0; - out_4858215238190826153[6] = 1; - out_4858215238190826153[7] = 0; - out_4858215238190826153[8] = 0; - out_4858215238190826153[9] = 1; - out_4858215238190826153[10] = 0; - out_4858215238190826153[11] = 0; - out_4858215238190826153[12] = 0; - out_4858215238190826153[13] = 0; - out_4858215238190826153[14] = 0; - out_4858215238190826153[15] = 0; - out_4858215238190826153[16] = 0; - out_4858215238190826153[17] = 0; - out_4858215238190826153[18] = 0; - out_4858215238190826153[19] = 0; - out_4858215238190826153[20] = 0; - out_4858215238190826153[21] = 0; - out_4858215238190826153[22] = 0; - out_4858215238190826153[23] = 0; - out_4858215238190826153[24] = 0; - out_4858215238190826153[25] = 1; - out_4858215238190826153[26] = 0; - out_4858215238190826153[27] = 0; - out_4858215238190826153[28] = 1; - out_4858215238190826153[29] = 0; - out_4858215238190826153[30] = 0; - out_4858215238190826153[31] = 0; - out_4858215238190826153[32] = 0; - out_4858215238190826153[33] = 0; - out_4858215238190826153[34] = 0; - out_4858215238190826153[35] = 0; - out_4858215238190826153[36] = 0; - out_4858215238190826153[37] = 0; - out_4858215238190826153[38] = 0; - out_4858215238190826153[39] = 0; - out_4858215238190826153[40] = 0; - out_4858215238190826153[41] = 0; - out_4858215238190826153[42] = 0; - out_4858215238190826153[43] = 0; - out_4858215238190826153[44] = 1; - out_4858215238190826153[45] = 0; - out_4858215238190826153[46] = 0; - out_4858215238190826153[47] = 1; - out_4858215238190826153[48] = 0; - out_4858215238190826153[49] = 0; - out_4858215238190826153[50] = 0; - out_4858215238190826153[51] = 0; - out_4858215238190826153[52] = 0; - out_4858215238190826153[53] = 0; +void H_4(double *state, double *unused, double *out_1470121245665226494) { + out_1470121245665226494[0] = 0; + out_1470121245665226494[1] = 0; + out_1470121245665226494[2] = 0; + out_1470121245665226494[3] = 0; + out_1470121245665226494[4] = 0; + out_1470121245665226494[5] = 0; + out_1470121245665226494[6] = 1; + out_1470121245665226494[7] = 0; + out_1470121245665226494[8] = 0; + out_1470121245665226494[9] = 1; + out_1470121245665226494[10] = 0; + out_1470121245665226494[11] = 0; + out_1470121245665226494[12] = 0; + out_1470121245665226494[13] = 0; + out_1470121245665226494[14] = 0; + out_1470121245665226494[15] = 0; + out_1470121245665226494[16] = 0; + out_1470121245665226494[17] = 0; + out_1470121245665226494[18] = 0; + out_1470121245665226494[19] = 0; + out_1470121245665226494[20] = 0; + out_1470121245665226494[21] = 0; + out_1470121245665226494[22] = 0; + out_1470121245665226494[23] = 0; + out_1470121245665226494[24] = 0; + out_1470121245665226494[25] = 1; + out_1470121245665226494[26] = 0; + out_1470121245665226494[27] = 0; + out_1470121245665226494[28] = 1; + out_1470121245665226494[29] = 0; + out_1470121245665226494[30] = 0; + out_1470121245665226494[31] = 0; + out_1470121245665226494[32] = 0; + out_1470121245665226494[33] = 0; + out_1470121245665226494[34] = 0; + out_1470121245665226494[35] = 0; + out_1470121245665226494[36] = 0; + out_1470121245665226494[37] = 0; + out_1470121245665226494[38] = 0; + out_1470121245665226494[39] = 0; + out_1470121245665226494[40] = 0; + out_1470121245665226494[41] = 0; + out_1470121245665226494[42] = 0; + out_1470121245665226494[43] = 0; + out_1470121245665226494[44] = 1; + out_1470121245665226494[45] = 0; + out_1470121245665226494[46] = 0; + out_1470121245665226494[47] = 1; + out_1470121245665226494[48] = 0; + out_1470121245665226494[49] = 0; + out_1470121245665226494[50] = 0; + out_1470121245665226494[51] = 0; + out_1470121245665226494[52] = 0; + out_1470121245665226494[53] = 0; } -void h_10(double *state, double *unused, double *out_558240635521768637) { - out_558240635521768637[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_558240635521768637[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_558240635521768637[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_6106389642402371296) { + out_6106389642402371296[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_6106389642402371296[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_6106389642402371296[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_7377176961528777036) { - out_7377176961528777036[0] = 0; - out_7377176961528777036[1] = 9.8100000000000005*cos(state[1]); - out_7377176961528777036[2] = 0; - out_7377176961528777036[3] = 0; - out_7377176961528777036[4] = -state[8]; - out_7377176961528777036[5] = state[7]; - out_7377176961528777036[6] = 0; - out_7377176961528777036[7] = state[5]; - out_7377176961528777036[8] = -state[4]; - out_7377176961528777036[9] = 0; - out_7377176961528777036[10] = 0; - out_7377176961528777036[11] = 0; - out_7377176961528777036[12] = 1; - out_7377176961528777036[13] = 0; - out_7377176961528777036[14] = 0; - out_7377176961528777036[15] = 1; - out_7377176961528777036[16] = 0; - out_7377176961528777036[17] = 0; - out_7377176961528777036[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_7377176961528777036[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_7377176961528777036[20] = 0; - out_7377176961528777036[21] = state[8]; - out_7377176961528777036[22] = 0; - out_7377176961528777036[23] = -state[6]; - out_7377176961528777036[24] = -state[5]; - out_7377176961528777036[25] = 0; - out_7377176961528777036[26] = state[3]; - out_7377176961528777036[27] = 0; - out_7377176961528777036[28] = 0; - out_7377176961528777036[29] = 0; - out_7377176961528777036[30] = 0; - out_7377176961528777036[31] = 1; - out_7377176961528777036[32] = 0; - out_7377176961528777036[33] = 0; - out_7377176961528777036[34] = 1; - out_7377176961528777036[35] = 0; - out_7377176961528777036[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_7377176961528777036[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_7377176961528777036[38] = 0; - out_7377176961528777036[39] = -state[7]; - out_7377176961528777036[40] = state[6]; - out_7377176961528777036[41] = 0; - out_7377176961528777036[42] = state[4]; - out_7377176961528777036[43] = -state[3]; - out_7377176961528777036[44] = 0; - out_7377176961528777036[45] = 0; - out_7377176961528777036[46] = 0; - out_7377176961528777036[47] = 0; - out_7377176961528777036[48] = 0; - out_7377176961528777036[49] = 0; - out_7377176961528777036[50] = 1; - out_7377176961528777036[51] = 0; - out_7377176961528777036[52] = 0; - out_7377176961528777036[53] = 1; +void H_10(double *state, double *unused, double *out_8155893514424523240) { + out_8155893514424523240[0] = 0; + out_8155893514424523240[1] = 9.8100000000000005*cos(state[1]); + out_8155893514424523240[2] = 0; + out_8155893514424523240[3] = 0; + out_8155893514424523240[4] = -state[8]; + out_8155893514424523240[5] = state[7]; + out_8155893514424523240[6] = 0; + out_8155893514424523240[7] = state[5]; + out_8155893514424523240[8] = -state[4]; + out_8155893514424523240[9] = 0; + out_8155893514424523240[10] = 0; + out_8155893514424523240[11] = 0; + out_8155893514424523240[12] = 1; + out_8155893514424523240[13] = 0; + out_8155893514424523240[14] = 0; + out_8155893514424523240[15] = 1; + out_8155893514424523240[16] = 0; + out_8155893514424523240[17] = 0; + out_8155893514424523240[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_8155893514424523240[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_8155893514424523240[20] = 0; + out_8155893514424523240[21] = state[8]; + out_8155893514424523240[22] = 0; + out_8155893514424523240[23] = -state[6]; + out_8155893514424523240[24] = -state[5]; + out_8155893514424523240[25] = 0; + out_8155893514424523240[26] = state[3]; + out_8155893514424523240[27] = 0; + out_8155893514424523240[28] = 0; + out_8155893514424523240[29] = 0; + out_8155893514424523240[30] = 0; + out_8155893514424523240[31] = 1; + out_8155893514424523240[32] = 0; + out_8155893514424523240[33] = 0; + out_8155893514424523240[34] = 1; + out_8155893514424523240[35] = 0; + out_8155893514424523240[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_8155893514424523240[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_8155893514424523240[38] = 0; + out_8155893514424523240[39] = -state[7]; + out_8155893514424523240[40] = state[6]; + out_8155893514424523240[41] = 0; + out_8155893514424523240[42] = state[4]; + out_8155893514424523240[43] = -state[3]; + out_8155893514424523240[44] = 0; + out_8155893514424523240[45] = 0; + out_8155893514424523240[46] = 0; + out_8155893514424523240[47] = 0; + out_8155893514424523240[48] = 0; + out_8155893514424523240[49] = 0; + out_8155893514424523240[50] = 1; + out_8155893514424523240[51] = 0; + out_8155893514424523240[52] = 0; + out_8155893514424523240[53] = 1; } -void h_13(double *state, double *unused, double *out_3864223786954470726) { - out_3864223786954470726[0] = state[3]; - out_3864223786954470726[1] = state[4]; - out_3864223786954470726[2] = state[5]; +void h_13(double *state, double *unused, double *out_8157156466131107868) { + out_8157156466131107868[0] = state[3]; + out_8157156466131107868[1] = state[4]; + out_8157156466131107868[2] = state[5]; } -void H_13(double *state, double *unused, double *out_1645941412858493352) { - out_1645941412858493352[0] = 0; - out_1645941412858493352[1] = 0; - out_1645941412858493352[2] = 0; - out_1645941412858493352[3] = 1; - out_1645941412858493352[4] = 0; - out_1645941412858493352[5] = 0; - out_1645941412858493352[6] = 0; - out_1645941412858493352[7] = 0; - out_1645941412858493352[8] = 0; - out_1645941412858493352[9] = 0; - out_1645941412858493352[10] = 0; - out_1645941412858493352[11] = 0; - out_1645941412858493352[12] = 0; - out_1645941412858493352[13] = 0; - out_1645941412858493352[14] = 0; - out_1645941412858493352[15] = 0; - out_1645941412858493352[16] = 0; - out_1645941412858493352[17] = 0; - out_1645941412858493352[18] = 0; - out_1645941412858493352[19] = 0; - out_1645941412858493352[20] = 0; - out_1645941412858493352[21] = 0; - out_1645941412858493352[22] = 1; - out_1645941412858493352[23] = 0; - out_1645941412858493352[24] = 0; - out_1645941412858493352[25] = 0; - out_1645941412858493352[26] = 0; - out_1645941412858493352[27] = 0; - out_1645941412858493352[28] = 0; - out_1645941412858493352[29] = 0; - out_1645941412858493352[30] = 0; - out_1645941412858493352[31] = 0; - out_1645941412858493352[32] = 0; - out_1645941412858493352[33] = 0; - out_1645941412858493352[34] = 0; - out_1645941412858493352[35] = 0; - out_1645941412858493352[36] = 0; - out_1645941412858493352[37] = 0; - out_1645941412858493352[38] = 0; - out_1645941412858493352[39] = 0; - out_1645941412858493352[40] = 0; - out_1645941412858493352[41] = 1; - out_1645941412858493352[42] = 0; - out_1645941412858493352[43] = 0; - out_1645941412858493352[44] = 0; - out_1645941412858493352[45] = 0; - out_1645941412858493352[46] = 0; - out_1645941412858493352[47] = 0; - out_1645941412858493352[48] = 0; - out_1645941412858493352[49] = 0; - out_1645941412858493352[50] = 0; - out_1645941412858493352[51] = 0; - out_1645941412858493352[52] = 0; - out_1645941412858493352[53] = 0; +void H_13(double *state, double *unused, double *out_1742152579667106307) { + out_1742152579667106307[0] = 0; + out_1742152579667106307[1] = 0; + out_1742152579667106307[2] = 0; + out_1742152579667106307[3] = 1; + out_1742152579667106307[4] = 0; + out_1742152579667106307[5] = 0; + out_1742152579667106307[6] = 0; + out_1742152579667106307[7] = 0; + out_1742152579667106307[8] = 0; + out_1742152579667106307[9] = 0; + out_1742152579667106307[10] = 0; + out_1742152579667106307[11] = 0; + out_1742152579667106307[12] = 0; + out_1742152579667106307[13] = 0; + out_1742152579667106307[14] = 0; + out_1742152579667106307[15] = 0; + out_1742152579667106307[16] = 0; + out_1742152579667106307[17] = 0; + out_1742152579667106307[18] = 0; + out_1742152579667106307[19] = 0; + out_1742152579667106307[20] = 0; + out_1742152579667106307[21] = 0; + out_1742152579667106307[22] = 1; + out_1742152579667106307[23] = 0; + out_1742152579667106307[24] = 0; + out_1742152579667106307[25] = 0; + out_1742152579667106307[26] = 0; + out_1742152579667106307[27] = 0; + out_1742152579667106307[28] = 0; + out_1742152579667106307[29] = 0; + out_1742152579667106307[30] = 0; + out_1742152579667106307[31] = 0; + out_1742152579667106307[32] = 0; + out_1742152579667106307[33] = 0; + out_1742152579667106307[34] = 0; + out_1742152579667106307[35] = 0; + out_1742152579667106307[36] = 0; + out_1742152579667106307[37] = 0; + out_1742152579667106307[38] = 0; + out_1742152579667106307[39] = 0; + out_1742152579667106307[40] = 0; + out_1742152579667106307[41] = 1; + out_1742152579667106307[42] = 0; + out_1742152579667106307[43] = 0; + out_1742152579667106307[44] = 0; + out_1742152579667106307[45] = 0; + out_1742152579667106307[46] = 0; + out_1742152579667106307[47] = 0; + out_1742152579667106307[48] = 0; + out_1742152579667106307[49] = 0; + out_1742152579667106307[50] = 0; + out_1742152579667106307[51] = 0; + out_1742152579667106307[52] = 0; + out_1742152579667106307[53] = 0; } -void h_14(double *state, double *unused, double *out_2242454366414003666) { - out_2242454366414003666[0] = state[6]; - out_2242454366414003666[1] = state[7]; - out_2242454366414003666[2] = state[8]; +void h_14(double *state, double *unused, double *out_568036792274749064) { + out_568036792274749064[0] = state[6]; + out_568036792274749064[1] = state[7]; + out_568036792274749064[2] = state[8]; } -void H_14(double *state, double *unused, double *out_7941003670486198449) { - out_7941003670486198449[0] = 0; - out_7941003670486198449[1] = 0; - out_7941003670486198449[2] = 0; - out_7941003670486198449[3] = 0; - out_7941003670486198449[4] = 0; - out_7941003670486198449[5] = 0; - out_7941003670486198449[6] = 1; - out_7941003670486198449[7] = 0; - out_7941003670486198449[8] = 0; - out_7941003670486198449[9] = 0; - out_7941003670486198449[10] = 0; - out_7941003670486198449[11] = 0; - out_7941003670486198449[12] = 0; - out_7941003670486198449[13] = 0; - out_7941003670486198449[14] = 0; - out_7941003670486198449[15] = 0; - out_7941003670486198449[16] = 0; - out_7941003670486198449[17] = 0; - out_7941003670486198449[18] = 0; - out_7941003670486198449[19] = 0; - out_7941003670486198449[20] = 0; - out_7941003670486198449[21] = 0; - out_7941003670486198449[22] = 0; - out_7941003670486198449[23] = 0; - out_7941003670486198449[24] = 0; - out_7941003670486198449[25] = 1; - out_7941003670486198449[26] = 0; - out_7941003670486198449[27] = 0; - out_7941003670486198449[28] = 0; - out_7941003670486198449[29] = 0; - out_7941003670486198449[30] = 0; - out_7941003670486198449[31] = 0; - out_7941003670486198449[32] = 0; - out_7941003670486198449[33] = 0; - out_7941003670486198449[34] = 0; - out_7941003670486198449[35] = 0; - out_7941003670486198449[36] = 0; - out_7941003670486198449[37] = 0; - out_7941003670486198449[38] = 0; - out_7941003670486198449[39] = 0; - out_7941003670486198449[40] = 0; - out_7941003670486198449[41] = 0; - out_7941003670486198449[42] = 0; - out_7941003670486198449[43] = 0; - out_7941003670486198449[44] = 1; - out_7941003670486198449[45] = 0; - out_7941003670486198449[46] = 0; - out_7941003670486198449[47] = 0; - out_7941003670486198449[48] = 0; - out_7941003670486198449[49] = 0; - out_7941003670486198449[50] = 0; - out_7941003670486198449[51] = 0; - out_7941003670486198449[52] = 0; - out_7941003670486198449[53] = 0; +void H_14(double *state, double *unused, double *out_4552909677960598790) { + out_4552909677960598790[0] = 0; + out_4552909677960598790[1] = 0; + out_4552909677960598790[2] = 0; + out_4552909677960598790[3] = 0; + out_4552909677960598790[4] = 0; + out_4552909677960598790[5] = 0; + out_4552909677960598790[6] = 1; + out_4552909677960598790[7] = 0; + out_4552909677960598790[8] = 0; + out_4552909677960598790[9] = 0; + out_4552909677960598790[10] = 0; + out_4552909677960598790[11] = 0; + out_4552909677960598790[12] = 0; + out_4552909677960598790[13] = 0; + out_4552909677960598790[14] = 0; + out_4552909677960598790[15] = 0; + out_4552909677960598790[16] = 0; + out_4552909677960598790[17] = 0; + out_4552909677960598790[18] = 0; + out_4552909677960598790[19] = 0; + out_4552909677960598790[20] = 0; + out_4552909677960598790[21] = 0; + out_4552909677960598790[22] = 0; + out_4552909677960598790[23] = 0; + out_4552909677960598790[24] = 0; + out_4552909677960598790[25] = 1; + out_4552909677960598790[26] = 0; + out_4552909677960598790[27] = 0; + out_4552909677960598790[28] = 0; + out_4552909677960598790[29] = 0; + out_4552909677960598790[30] = 0; + out_4552909677960598790[31] = 0; + out_4552909677960598790[32] = 0; + out_4552909677960598790[33] = 0; + out_4552909677960598790[34] = 0; + out_4552909677960598790[35] = 0; + out_4552909677960598790[36] = 0; + out_4552909677960598790[37] = 0; + out_4552909677960598790[38] = 0; + out_4552909677960598790[39] = 0; + out_4552909677960598790[40] = 0; + out_4552909677960598790[41] = 0; + out_4552909677960598790[42] = 0; + out_4552909677960598790[43] = 0; + out_4552909677960598790[44] = 1; + out_4552909677960598790[45] = 0; + out_4552909677960598790[46] = 0; + out_4552909677960598790[47] = 0; + out_4552909677960598790[48] = 0; + out_4552909677960598790[49] = 0; + out_4552909677960598790[50] = 0; + out_4552909677960598790[51] = 0; + out_4552909677960598790[52] = 0; + out_4552909677960598790[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_2526150130430510074) { - err_fun(nom_x, delta_x, out_2526150130430510074); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8259662850093450970) { + err_fun(nom_x, delta_x, out_8259662850093450970); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3515561648914426109) { - inv_err_fun(nom_x, true_x, out_3515561648914426109); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7477554158578246383) { + inv_err_fun(nom_x, true_x, out_7477554158578246383); } -void pose_H_mod_fun(double *state, double *out_6056162720073724257) { - H_mod_fun(state, out_6056162720073724257); +void pose_H_mod_fun(double *state, double *out_6622639630704307229) { + H_mod_fun(state, out_6622639630704307229); } -void pose_f_fun(double *state, double dt, double *out_8780284750928315681) { - f_fun(state, dt, out_8780284750928315681); +void pose_f_fun(double *state, double dt, double *out_2747407686181114080) { + f_fun(state, dt, out_2747407686181114080); } -void pose_F_fun(double *state, double dt, double *out_1516035527731464330) { - F_fun(state, dt, out_1516035527731464330); +void pose_F_fun(double *state, double dt, double *out_5168326300746653192) { + F_fun(state, dt, out_5168326300746653192); } -void pose_h_4(double *state, double *unused, double *out_4492721189395603550) { - h_4(state, unused, out_4492721189395603550); +void pose_h_4(double *state, double *unused, double *out_8670716112105694718) { + h_4(state, unused, out_8670716112105694718); } -void pose_H_4(double *state, double *unused, double *out_4858215238190826153) { - H_4(state, unused, out_4858215238190826153); +void pose_H_4(double *state, double *unused, double *out_1470121245665226494) { + H_4(state, unused, out_1470121245665226494); } -void pose_h_10(double *state, double *unused, double *out_558240635521768637) { - h_10(state, unused, out_558240635521768637); +void pose_h_10(double *state, double *unused, double *out_6106389642402371296) { + h_10(state, unused, out_6106389642402371296); } -void pose_H_10(double *state, double *unused, double *out_7377176961528777036) { - H_10(state, unused, out_7377176961528777036); +void pose_H_10(double *state, double *unused, double *out_8155893514424523240) { + H_10(state, unused, out_8155893514424523240); } -void pose_h_13(double *state, double *unused, double *out_3864223786954470726) { - h_13(state, unused, out_3864223786954470726); +void pose_h_13(double *state, double *unused, double *out_8157156466131107868) { + h_13(state, unused, out_8157156466131107868); } -void pose_H_13(double *state, double *unused, double *out_1645941412858493352) { - H_13(state, unused, out_1645941412858493352); +void pose_H_13(double *state, double *unused, double *out_1742152579667106307) { + H_13(state, unused, out_1742152579667106307); } -void pose_h_14(double *state, double *unused, double *out_2242454366414003666) { - h_14(state, unused, out_2242454366414003666); +void pose_h_14(double *state, double *unused, double *out_568036792274749064) { + h_14(state, unused, out_568036792274749064); } -void pose_H_14(double *state, double *unused, double *out_7941003670486198449) { - H_14(state, unused, out_7941003670486198449); +void pose_H_14(double *state, double *unused, double *out_4552909677960598790) { + H_14(state, unused, out_4552909677960598790); } 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 e7ae94715..1af60b6f8 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_2526150130430510074); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3515561648914426109); -void pose_H_mod_fun(double *state, double *out_6056162720073724257); -void pose_f_fun(double *state, double dt, double *out_8780284750928315681); -void pose_F_fun(double *state, double dt, double *out_1516035527731464330); -void pose_h_4(double *state, double *unused, double *out_4492721189395603550); -void pose_H_4(double *state, double *unused, double *out_4858215238190826153); -void pose_h_10(double *state, double *unused, double *out_558240635521768637); -void pose_H_10(double *state, double *unused, double *out_7377176961528777036); -void pose_h_13(double *state, double *unused, double *out_3864223786954470726); -void pose_H_13(double *state, double *unused, double *out_1645941412858493352); -void pose_h_14(double *state, double *unused, double *out_2242454366414003666); -void pose_H_14(double *state, double *unused, double *out_7941003670486198449); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8259662850093450970); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7477554158578246383); +void pose_H_mod_fun(double *state, double *out_6622639630704307229); +void pose_f_fun(double *state, double dt, double *out_2747407686181114080); +void pose_F_fun(double *state, double dt, double *out_5168326300746653192); +void pose_h_4(double *state, double *unused, double *out_8670716112105694718); +void pose_H_4(double *state, double *unused, double *out_1470121245665226494); +void pose_h_10(double *state, double *unused, double *out_6106389642402371296); +void pose_H_10(double *state, double *unused, double *out_8155893514424523240); +void pose_h_13(double *state, double *unused, double *out_8157156466131107868); +void pose_H_13(double *state, double *unused, double *out_1742152579667106307); +void pose_h_14(double *state, double *unused, double *out_568036792274749064); +void pose_H_14(double *state, double *unused, double *out_4552909677960598790); 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 288efafed..e8686ab72 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ