diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 5c10d0b22..615f3a9aa 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 e626c777b..079f1d6e0 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 ca109e506..8f5710c10 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 45c767c15..3e6d52c0e 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 d4248baa4..96417cfe1 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 689edce8f..0a8e4cd7b 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 40bd738a4..15495e734 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 40bd738a4..15495e734 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 2209872be..431b6e629 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 689edce8f..0a8e4cd7b 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 d3efc2a8a..96fd41a09 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-4254090c-DEBUG"; +const uint8_t gitversion[19] = "DEV-25633be9-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index fe44ee969..5ae77d288 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 c9d17bea4..b75551e54 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 f2f76af0d..a8a0ceb74 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 513bf77af..efa7bd221 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 70689eb3d..4509f1e04 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 beedef32e..9e5b539c3 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 94936d129..7d4414bdc 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 1a61fd33a..98bb99daa 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 736950f66..113467175 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 7ad868e22..0b48f1fb0 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 5ca99a3f7..db491cca5 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 16ed3fb16..0c2448b31 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 dd6036a7f..581f3cce8 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 463ee9ed0..fa3f90731 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 0d1c24a07..c9c1e06d8 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 a386295e5..782f85202 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 602e53e77..e1214a15a 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 d323db13e..45c1751b9 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 c939135b7..415c25078 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 c32283703..260a9d8c3 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 265e3df67..826584508 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-4254090c-DEBUG \ No newline at end of file +DEV-25633be9-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 79d930323..78da9f2fb 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -49,7 +49,7 @@ MIN_LATERAL_CONTROL_SPEED = 0.3 CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30 CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20 CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00 -CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.52 +CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.75 CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED = 12.0 CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE = 0.08 CIVIC_BOSCH_MODIFIED_B_FF_ONSET = 0.18 @@ -76,16 +76,16 @@ CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00 CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.03 CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.07 -CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.34 -CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.62 -CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.02 +CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.50 +CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.82 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.00 CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 3.20 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 4.40 -CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.02 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 4.40 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 6.20 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00 CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 2.00 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 3.00 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 2.80 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 4.20 BOLT_2022_2023_CARS = ( GM_CAR.CHEVROLET_BOLT_ACC_2022_2023, diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 217c6fac7..7bba92ac1 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -489,7 +489,7 @@ class TestLatControl: monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_lateral_testing_ground_active", lambda: True) variant_controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL) - assert variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 1.52) + assert variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 1.75) def test_modified_civic_b_torque_ff_scale_curve(self): steady_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.0, 12.0) @@ -539,11 +539,11 @@ class TestLatControl: assert variant_steady_right < base_steady_right assert variant_turn_in_right < base_turn_in_right - assert variant_turn_in_right > variant_steady_right + assert variant_turn_in_right >= variant_steady_right assert variant_turn_in_left > variant_steady_left assert variant_unwind_right < base_unwind_right assert variant_unwind_right_friction < base_unwind_right_friction - assert variant_turn_in_right_friction > base_turn_in_right_friction + assert variant_turn_in_right_friction >= base_turn_in_right_friction def test_modified_civic_a_variant_extra_torque_shaping_curve(self, monkeypatch): base_steady_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.0, 12.0) diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 7de48ab80..10a962e81 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_8796389704996072171) { - out_8796389704996072171[0] = delta_x[0] + nom_x[0]; - out_8796389704996072171[1] = delta_x[1] + nom_x[1]; - out_8796389704996072171[2] = delta_x[2] + nom_x[2]; - out_8796389704996072171[3] = delta_x[3] + nom_x[3]; - out_8796389704996072171[4] = delta_x[4] + nom_x[4]; - out_8796389704996072171[5] = delta_x[5] + nom_x[5]; - out_8796389704996072171[6] = delta_x[6] + nom_x[6]; - out_8796389704996072171[7] = delta_x[7] + nom_x[7]; - out_8796389704996072171[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_5325374827696631328) { + out_5325374827696631328[0] = delta_x[0] + nom_x[0]; + out_5325374827696631328[1] = delta_x[1] + nom_x[1]; + out_5325374827696631328[2] = delta_x[2] + nom_x[2]; + out_5325374827696631328[3] = delta_x[3] + nom_x[3]; + out_5325374827696631328[4] = delta_x[4] + nom_x[4]; + out_5325374827696631328[5] = delta_x[5] + nom_x[5]; + out_5325374827696631328[6] = delta_x[6] + nom_x[6]; + out_5325374827696631328[7] = delta_x[7] + nom_x[7]; + out_5325374827696631328[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8849207833158339340) { - out_8849207833158339340[0] = -nom_x[0] + true_x[0]; - out_8849207833158339340[1] = -nom_x[1] + true_x[1]; - out_8849207833158339340[2] = -nom_x[2] + true_x[2]; - out_8849207833158339340[3] = -nom_x[3] + true_x[3]; - out_8849207833158339340[4] = -nom_x[4] + true_x[4]; - out_8849207833158339340[5] = -nom_x[5] + true_x[5]; - out_8849207833158339340[6] = -nom_x[6] + true_x[6]; - out_8849207833158339340[7] = -nom_x[7] + true_x[7]; - out_8849207833158339340[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_263695042767338590) { + out_263695042767338590[0] = -nom_x[0] + true_x[0]; + out_263695042767338590[1] = -nom_x[1] + true_x[1]; + out_263695042767338590[2] = -nom_x[2] + true_x[2]; + out_263695042767338590[3] = -nom_x[3] + true_x[3]; + out_263695042767338590[4] = -nom_x[4] + true_x[4]; + out_263695042767338590[5] = -nom_x[5] + true_x[5]; + out_263695042767338590[6] = -nom_x[6] + true_x[6]; + out_263695042767338590[7] = -nom_x[7] + true_x[7]; + out_263695042767338590[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_1622786580378488408) { - out_1622786580378488408[0] = 1.0; - out_1622786580378488408[1] = 0.0; - out_1622786580378488408[2] = 0.0; - out_1622786580378488408[3] = 0.0; - out_1622786580378488408[4] = 0.0; - out_1622786580378488408[5] = 0.0; - out_1622786580378488408[6] = 0.0; - out_1622786580378488408[7] = 0.0; - out_1622786580378488408[8] = 0.0; - out_1622786580378488408[9] = 0.0; - out_1622786580378488408[10] = 1.0; - out_1622786580378488408[11] = 0.0; - out_1622786580378488408[12] = 0.0; - out_1622786580378488408[13] = 0.0; - out_1622786580378488408[14] = 0.0; - out_1622786580378488408[15] = 0.0; - out_1622786580378488408[16] = 0.0; - out_1622786580378488408[17] = 0.0; - out_1622786580378488408[18] = 0.0; - out_1622786580378488408[19] = 0.0; - out_1622786580378488408[20] = 1.0; - out_1622786580378488408[21] = 0.0; - out_1622786580378488408[22] = 0.0; - out_1622786580378488408[23] = 0.0; - out_1622786580378488408[24] = 0.0; - out_1622786580378488408[25] = 0.0; - out_1622786580378488408[26] = 0.0; - out_1622786580378488408[27] = 0.0; - out_1622786580378488408[28] = 0.0; - out_1622786580378488408[29] = 0.0; - out_1622786580378488408[30] = 1.0; - out_1622786580378488408[31] = 0.0; - out_1622786580378488408[32] = 0.0; - out_1622786580378488408[33] = 0.0; - out_1622786580378488408[34] = 0.0; - out_1622786580378488408[35] = 0.0; - out_1622786580378488408[36] = 0.0; - out_1622786580378488408[37] = 0.0; - out_1622786580378488408[38] = 0.0; - out_1622786580378488408[39] = 0.0; - out_1622786580378488408[40] = 1.0; - out_1622786580378488408[41] = 0.0; - out_1622786580378488408[42] = 0.0; - out_1622786580378488408[43] = 0.0; - out_1622786580378488408[44] = 0.0; - out_1622786580378488408[45] = 0.0; - out_1622786580378488408[46] = 0.0; - out_1622786580378488408[47] = 0.0; - out_1622786580378488408[48] = 0.0; - out_1622786580378488408[49] = 0.0; - out_1622786580378488408[50] = 1.0; - out_1622786580378488408[51] = 0.0; - out_1622786580378488408[52] = 0.0; - out_1622786580378488408[53] = 0.0; - out_1622786580378488408[54] = 0.0; - out_1622786580378488408[55] = 0.0; - out_1622786580378488408[56] = 0.0; - out_1622786580378488408[57] = 0.0; - out_1622786580378488408[58] = 0.0; - out_1622786580378488408[59] = 0.0; - out_1622786580378488408[60] = 1.0; - out_1622786580378488408[61] = 0.0; - out_1622786580378488408[62] = 0.0; - out_1622786580378488408[63] = 0.0; - out_1622786580378488408[64] = 0.0; - out_1622786580378488408[65] = 0.0; - out_1622786580378488408[66] = 0.0; - out_1622786580378488408[67] = 0.0; - out_1622786580378488408[68] = 0.0; - out_1622786580378488408[69] = 0.0; - out_1622786580378488408[70] = 1.0; - out_1622786580378488408[71] = 0.0; - out_1622786580378488408[72] = 0.0; - out_1622786580378488408[73] = 0.0; - out_1622786580378488408[74] = 0.0; - out_1622786580378488408[75] = 0.0; - out_1622786580378488408[76] = 0.0; - out_1622786580378488408[77] = 0.0; - out_1622786580378488408[78] = 0.0; - out_1622786580378488408[79] = 0.0; - out_1622786580378488408[80] = 1.0; +void H_mod_fun(double *state, double *out_2663737869404174494) { + out_2663737869404174494[0] = 1.0; + out_2663737869404174494[1] = 0.0; + out_2663737869404174494[2] = 0.0; + out_2663737869404174494[3] = 0.0; + out_2663737869404174494[4] = 0.0; + out_2663737869404174494[5] = 0.0; + out_2663737869404174494[6] = 0.0; + out_2663737869404174494[7] = 0.0; + out_2663737869404174494[8] = 0.0; + out_2663737869404174494[9] = 0.0; + out_2663737869404174494[10] = 1.0; + out_2663737869404174494[11] = 0.0; + out_2663737869404174494[12] = 0.0; + out_2663737869404174494[13] = 0.0; + out_2663737869404174494[14] = 0.0; + out_2663737869404174494[15] = 0.0; + out_2663737869404174494[16] = 0.0; + out_2663737869404174494[17] = 0.0; + out_2663737869404174494[18] = 0.0; + out_2663737869404174494[19] = 0.0; + out_2663737869404174494[20] = 1.0; + out_2663737869404174494[21] = 0.0; + out_2663737869404174494[22] = 0.0; + out_2663737869404174494[23] = 0.0; + out_2663737869404174494[24] = 0.0; + out_2663737869404174494[25] = 0.0; + out_2663737869404174494[26] = 0.0; + out_2663737869404174494[27] = 0.0; + out_2663737869404174494[28] = 0.0; + out_2663737869404174494[29] = 0.0; + out_2663737869404174494[30] = 1.0; + out_2663737869404174494[31] = 0.0; + out_2663737869404174494[32] = 0.0; + out_2663737869404174494[33] = 0.0; + out_2663737869404174494[34] = 0.0; + out_2663737869404174494[35] = 0.0; + out_2663737869404174494[36] = 0.0; + out_2663737869404174494[37] = 0.0; + out_2663737869404174494[38] = 0.0; + out_2663737869404174494[39] = 0.0; + out_2663737869404174494[40] = 1.0; + out_2663737869404174494[41] = 0.0; + out_2663737869404174494[42] = 0.0; + out_2663737869404174494[43] = 0.0; + out_2663737869404174494[44] = 0.0; + out_2663737869404174494[45] = 0.0; + out_2663737869404174494[46] = 0.0; + out_2663737869404174494[47] = 0.0; + out_2663737869404174494[48] = 0.0; + out_2663737869404174494[49] = 0.0; + out_2663737869404174494[50] = 1.0; + out_2663737869404174494[51] = 0.0; + out_2663737869404174494[52] = 0.0; + out_2663737869404174494[53] = 0.0; + out_2663737869404174494[54] = 0.0; + out_2663737869404174494[55] = 0.0; + out_2663737869404174494[56] = 0.0; + out_2663737869404174494[57] = 0.0; + out_2663737869404174494[58] = 0.0; + out_2663737869404174494[59] = 0.0; + out_2663737869404174494[60] = 1.0; + out_2663737869404174494[61] = 0.0; + out_2663737869404174494[62] = 0.0; + out_2663737869404174494[63] = 0.0; + out_2663737869404174494[64] = 0.0; + out_2663737869404174494[65] = 0.0; + out_2663737869404174494[66] = 0.0; + out_2663737869404174494[67] = 0.0; + out_2663737869404174494[68] = 0.0; + out_2663737869404174494[69] = 0.0; + out_2663737869404174494[70] = 1.0; + out_2663737869404174494[71] = 0.0; + out_2663737869404174494[72] = 0.0; + out_2663737869404174494[73] = 0.0; + out_2663737869404174494[74] = 0.0; + out_2663737869404174494[75] = 0.0; + out_2663737869404174494[76] = 0.0; + out_2663737869404174494[77] = 0.0; + out_2663737869404174494[78] = 0.0; + out_2663737869404174494[79] = 0.0; + out_2663737869404174494[80] = 1.0; } -void f_fun(double *state, double dt, double *out_1981095305174081329) { - out_1981095305174081329[0] = state[0]; - out_1981095305174081329[1] = state[1]; - out_1981095305174081329[2] = state[2]; - out_1981095305174081329[3] = state[3]; - out_1981095305174081329[4] = state[4]; - out_1981095305174081329[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_1981095305174081329[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_1981095305174081329[7] = state[7]; - out_1981095305174081329[8] = state[8]; +void f_fun(double *state, double dt, double *out_8230420991833863893) { + out_8230420991833863893[0] = state[0]; + out_8230420991833863893[1] = state[1]; + out_8230420991833863893[2] = state[2]; + out_8230420991833863893[3] = state[3]; + out_8230420991833863893[4] = state[4]; + out_8230420991833863893[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_8230420991833863893[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_8230420991833863893[7] = state[7]; + out_8230420991833863893[8] = state[8]; } -void F_fun(double *state, double dt, double *out_3561550856242897713) { - out_3561550856242897713[0] = 1; - out_3561550856242897713[1] = 0; - out_3561550856242897713[2] = 0; - out_3561550856242897713[3] = 0; - out_3561550856242897713[4] = 0; - out_3561550856242897713[5] = 0; - out_3561550856242897713[6] = 0; - out_3561550856242897713[7] = 0; - out_3561550856242897713[8] = 0; - out_3561550856242897713[9] = 0; - out_3561550856242897713[10] = 1; - out_3561550856242897713[11] = 0; - out_3561550856242897713[12] = 0; - out_3561550856242897713[13] = 0; - out_3561550856242897713[14] = 0; - out_3561550856242897713[15] = 0; - out_3561550856242897713[16] = 0; - out_3561550856242897713[17] = 0; - out_3561550856242897713[18] = 0; - out_3561550856242897713[19] = 0; - out_3561550856242897713[20] = 1; - out_3561550856242897713[21] = 0; - out_3561550856242897713[22] = 0; - out_3561550856242897713[23] = 0; - out_3561550856242897713[24] = 0; - out_3561550856242897713[25] = 0; - out_3561550856242897713[26] = 0; - out_3561550856242897713[27] = 0; - out_3561550856242897713[28] = 0; - out_3561550856242897713[29] = 0; - out_3561550856242897713[30] = 1; - out_3561550856242897713[31] = 0; - out_3561550856242897713[32] = 0; - out_3561550856242897713[33] = 0; - out_3561550856242897713[34] = 0; - out_3561550856242897713[35] = 0; - out_3561550856242897713[36] = 0; - out_3561550856242897713[37] = 0; - out_3561550856242897713[38] = 0; - out_3561550856242897713[39] = 0; - out_3561550856242897713[40] = 1; - out_3561550856242897713[41] = 0; - out_3561550856242897713[42] = 0; - out_3561550856242897713[43] = 0; - out_3561550856242897713[44] = 0; - out_3561550856242897713[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_3561550856242897713[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_3561550856242897713[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3561550856242897713[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3561550856242897713[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_3561550856242897713[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_3561550856242897713[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_3561550856242897713[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_3561550856242897713[53] = -9.8100000000000005*dt; - out_3561550856242897713[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_3561550856242897713[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_3561550856242897713[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3561550856242897713[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3561550856242897713[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_3561550856242897713[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_3561550856242897713[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_3561550856242897713[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3561550856242897713[62] = 0; - out_3561550856242897713[63] = 0; - out_3561550856242897713[64] = 0; - out_3561550856242897713[65] = 0; - out_3561550856242897713[66] = 0; - out_3561550856242897713[67] = 0; - out_3561550856242897713[68] = 0; - out_3561550856242897713[69] = 0; - out_3561550856242897713[70] = 1; - out_3561550856242897713[71] = 0; - out_3561550856242897713[72] = 0; - out_3561550856242897713[73] = 0; - out_3561550856242897713[74] = 0; - out_3561550856242897713[75] = 0; - out_3561550856242897713[76] = 0; - out_3561550856242897713[77] = 0; - out_3561550856242897713[78] = 0; - out_3561550856242897713[79] = 0; - out_3561550856242897713[80] = 1; +void F_fun(double *state, double dt, double *out_2756618938696411588) { + out_2756618938696411588[0] = 1; + out_2756618938696411588[1] = 0; + out_2756618938696411588[2] = 0; + out_2756618938696411588[3] = 0; + out_2756618938696411588[4] = 0; + out_2756618938696411588[5] = 0; + out_2756618938696411588[6] = 0; + out_2756618938696411588[7] = 0; + out_2756618938696411588[8] = 0; + out_2756618938696411588[9] = 0; + out_2756618938696411588[10] = 1; + out_2756618938696411588[11] = 0; + out_2756618938696411588[12] = 0; + out_2756618938696411588[13] = 0; + out_2756618938696411588[14] = 0; + out_2756618938696411588[15] = 0; + out_2756618938696411588[16] = 0; + out_2756618938696411588[17] = 0; + out_2756618938696411588[18] = 0; + out_2756618938696411588[19] = 0; + out_2756618938696411588[20] = 1; + out_2756618938696411588[21] = 0; + out_2756618938696411588[22] = 0; + out_2756618938696411588[23] = 0; + out_2756618938696411588[24] = 0; + out_2756618938696411588[25] = 0; + out_2756618938696411588[26] = 0; + out_2756618938696411588[27] = 0; + out_2756618938696411588[28] = 0; + out_2756618938696411588[29] = 0; + out_2756618938696411588[30] = 1; + out_2756618938696411588[31] = 0; + out_2756618938696411588[32] = 0; + out_2756618938696411588[33] = 0; + out_2756618938696411588[34] = 0; + out_2756618938696411588[35] = 0; + out_2756618938696411588[36] = 0; + out_2756618938696411588[37] = 0; + out_2756618938696411588[38] = 0; + out_2756618938696411588[39] = 0; + out_2756618938696411588[40] = 1; + out_2756618938696411588[41] = 0; + out_2756618938696411588[42] = 0; + out_2756618938696411588[43] = 0; + out_2756618938696411588[44] = 0; + out_2756618938696411588[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_2756618938696411588[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2756618938696411588[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2756618938696411588[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2756618938696411588[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_2756618938696411588[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2756618938696411588[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2756618938696411588[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2756618938696411588[53] = -9.8100000000000005*dt; + out_2756618938696411588[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_2756618938696411588[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2756618938696411588[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2756618938696411588[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2756618938696411588[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_2756618938696411588[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2756618938696411588[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_2756618938696411588[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2756618938696411588[62] = 0; + out_2756618938696411588[63] = 0; + out_2756618938696411588[64] = 0; + out_2756618938696411588[65] = 0; + out_2756618938696411588[66] = 0; + out_2756618938696411588[67] = 0; + out_2756618938696411588[68] = 0; + out_2756618938696411588[69] = 0; + out_2756618938696411588[70] = 1; + out_2756618938696411588[71] = 0; + out_2756618938696411588[72] = 0; + out_2756618938696411588[73] = 0; + out_2756618938696411588[74] = 0; + out_2756618938696411588[75] = 0; + out_2756618938696411588[76] = 0; + out_2756618938696411588[77] = 0; + out_2756618938696411588[78] = 0; + out_2756618938696411588[79] = 0; + out_2756618938696411588[80] = 1; } -void h_25(double *state, double *unused, double *out_3280789364307995853) { - out_3280789364307995853[0] = state[6]; +void h_25(double *state, double *unused, double *out_2585399580963523364) { + out_2585399580963523364[0] = state[6]; } -void H_25(double *state, double *unused, double *out_445738705382224348) { - out_445738705382224348[0] = 0; - out_445738705382224348[1] = 0; - out_445738705382224348[2] = 0; - out_445738705382224348[3] = 0; - out_445738705382224348[4] = 0; - out_445738705382224348[5] = 0; - out_445738705382224348[6] = 1; - out_445738705382224348[7] = 0; - out_445738705382224348[8] = 0; +void H_25(double *state, double *unused, double *out_4993569966627829866) { + out_4993569966627829866[0] = 0; + out_4993569966627829866[1] = 0; + out_4993569966627829866[2] = 0; + out_4993569966627829866[3] = 0; + out_4993569966627829866[4] = 0; + out_4993569966627829866[5] = 0; + out_4993569966627829866[6] = 1; + out_4993569966627829866[7] = 0; + out_4993569966627829866[8] = 0; } -void h_24(double *state, double *unused, double *out_5100775928185398157) { - out_5100775928185398157[0] = state[4]; - out_5100775928185398157[1] = state[5]; +void h_24(double *state, double *unused, double *out_7866369606921911129) { + out_7866369606921911129[0] = state[4]; + out_7866369606921911129[1] = state[5]; } -void H_24(double *state, double *unused, double *out_399257903401471051) { - out_399257903401471051[0] = 0; - out_399257903401471051[1] = 0; - out_399257903401471051[2] = 0; - out_399257903401471051[3] = 0; - out_399257903401471051[4] = 1; - out_399257903401471051[5] = 0; - out_399257903401471051[6] = 0; - out_399257903401471051[7] = 0; - out_399257903401471051[8] = 0; - out_399257903401471051[9] = 0; - out_399257903401471051[10] = 0; - out_399257903401471051[11] = 0; - out_399257903401471051[12] = 0; - out_399257903401471051[13] = 0; - out_399257903401471051[14] = 1; - out_399257903401471051[15] = 0; - out_399257903401471051[16] = 0; - out_399257903401471051[17] = 0; +void H_24(double *state, double *unused, double *out_124755101600123014) { + out_124755101600123014[0] = 0; + out_124755101600123014[1] = 0; + out_124755101600123014[2] = 0; + out_124755101600123014[3] = 0; + out_124755101600123014[4] = 1; + out_124755101600123014[5] = 0; + out_124755101600123014[6] = 0; + out_124755101600123014[7] = 0; + out_124755101600123014[8] = 0; + out_124755101600123014[9] = 0; + out_124755101600123014[10] = 0; + out_124755101600123014[11] = 0; + out_124755101600123014[12] = 0; + out_124755101600123014[13] = 0; + out_124755101600123014[14] = 1; + out_124755101600123014[15] = 0; + out_124755101600123014[16] = 0; + out_124755101600123014[17] = 0; } -void h_30(double *state, double *unused, double *out_3331749478418022440) { - out_3331749478418022440[0] = state[4]; +void h_30(double *state, double *unused, double *out_1052049947290498986) { + out_1052049947290498986[0] = state[4]; } -void H_30(double *state, double *unused, double *out_2072594253125024279) { - out_2072594253125024279[0] = 0; - out_2072594253125024279[1] = 0; - out_2072594253125024279[2] = 0; - out_2072594253125024279[3] = 0; - out_2072594253125024279[4] = 1; - out_2072594253125024279[5] = 0; - out_2072594253125024279[6] = 0; - out_2072594253125024279[7] = 0; - out_2072594253125024279[8] = 0; +void H_30(double *state, double *unused, double *out_465873636500221668) { + out_465873636500221668[0] = 0; + out_465873636500221668[1] = 0; + out_465873636500221668[2] = 0; + out_465873636500221668[3] = 0; + out_465873636500221668[4] = 1; + out_465873636500221668[5] = 0; + out_465873636500221668[6] = 0; + out_465873636500221668[7] = 0; + out_465873636500221668[8] = 0; } -void h_26(double *state, double *unused, double *out_1066202602863606572) { - out_1066202602863606572[0] = state[7]; +void h_26(double *state, double *unused, double *out_3137755351529974294) { + out_3137755351529974294[0] = state[7]; } -void H_26(double *state, double *unused, double *out_4187242024256280572) { - out_4187242024256280572[0] = 0; - out_4187242024256280572[1] = 0; - out_4187242024256280572[2] = 0; - out_4187242024256280572[3] = 0; - out_4187242024256280572[4] = 0; - out_4187242024256280572[5] = 0; - out_4187242024256280572[6] = 0; - out_4187242024256280572[7] = 1; - out_4187242024256280572[8] = 0; +void H_26(double *state, double *unused, double *out_1252066647753773642) { + out_1252066647753773642[0] = 0; + out_1252066647753773642[1] = 0; + out_1252066647753773642[2] = 0; + out_1252066647753773642[3] = 0; + out_1252066647753773642[4] = 0; + out_1252066647753773642[5] = 0; + out_1252066647753773642[6] = 0; + out_1252066647753773642[7] = 1; + out_1252066647753773642[8] = 0; } -void h_27(double *state, double *unused, double *out_3118252153105853293) { - out_3118252153105853293[0] = state[3]; +void h_27(double *state, double *unused, double *out_3292885906595191724) { + out_3292885906595191724[0] = state[3]; } -void H_27(double *state, double *unused, double *out_4296188324308967496) { - out_4296188324308967496[0] = 0; - out_4296188324308967496[1] = 0; - out_4296188324308967496[2] = 0; - out_4296188324308967496[3] = 1; - out_4296188324308967496[4] = 0; - out_4296188324308967496[5] = 0; - out_4296188324308967496[6] = 0; - out_4296188324308967496[7] = 0; - out_4296188324308967496[8] = 0; +void H_27(double *state, double *unused, double *out_2689467707684164885) { + out_2689467707684164885[0] = 0; + out_2689467707684164885[1] = 0; + out_2689467707684164885[2] = 0; + out_2689467707684164885[3] = 1; + out_2689467707684164885[4] = 0; + out_2689467707684164885[5] = 0; + out_2689467707684164885[6] = 0; + out_2689467707684164885[7] = 0; + out_2689467707684164885[8] = 0; } -void h_29(double *state, double *unused, double *out_8557656694253347387) { - out_8557656694253347387[0] = state[1]; +void h_29(double *state, double *unused, double *out_5346991570123350160) { + out_5346991570123350160[0] = state[1]; } -void H_29(double *state, double *unused, double *out_2582825597439416463) { - out_2582825597439416463[0] = 0; - out_2582825597439416463[1] = 1; - out_2582825597439416463[2] = 0; - out_2582825597439416463[3] = 0; - out_2582825597439416463[4] = 0; - out_2582825597439416463[5] = 0; - out_2582825597439416463[6] = 0; - out_2582825597439416463[7] = 0; - out_2582825597439416463[8] = 0; +void H_29(double *state, double *unused, double *out_976104980814613852) { + out_976104980814613852[0] = 0; + out_976104980814613852[1] = 1; + out_976104980814613852[2] = 0; + out_976104980814613852[3] = 0; + out_976104980814613852[4] = 0; + out_976104980814613852[5] = 0; + out_976104980814613852[6] = 0; + out_976104980814613852[7] = 0; + out_976104980814613852[8] = 0; } -void h_28(double *state, double *unused, double *out_4285995954181995030) { - out_4285995954181995030[0] = state[0]; +void h_28(double *state, double *unused, double *out_6565695485309518484) { + out_6565695485309518484[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2499573419630114111) { - out_2499573419630114111[0] = 1; - out_2499573419630114111[1] = 0; - out_2499573419630114111[2] = 0; - out_2499573419630114111[3] = 0; - out_2499573419630114111[4] = 0; - out_2499573419630114111[5] = 0; - out_2499573419630114111[6] = 0; - out_2499573419630114111[7] = 0; - out_2499573419630114111[8] = 0; +void H_28(double *state, double *unused, double *out_2939735252379940103) { + out_2939735252379940103[0] = 1; + out_2939735252379940103[1] = 0; + out_2939735252379940103[2] = 0; + out_2939735252379940103[3] = 0; + out_2939735252379940103[4] = 0; + out_2939735252379940103[5] = 0; + out_2939735252379940103[6] = 0; + out_2939735252379940103[7] = 0; + out_2939735252379940103[8] = 0; } -void h_31(double *state, double *unused, double *out_6669294028137815771) { - out_6669294028137815771[0] = state[8]; +void h_31(double *state, double *unused, double *out_7496068987004855670) { + out_7496068987004855670[0] = state[8]; } -void H_31(double *state, double *unused, double *out_415092743505263920) { - out_415092743505263920[0] = 0; - out_415092743505263920[1] = 0; - out_415092743505263920[2] = 0; - out_415092743505263920[3] = 0; - out_415092743505263920[4] = 0; - out_415092743505263920[5] = 0; - out_415092743505263920[6] = 0; - out_415092743505263920[7] = 0; - out_415092743505263920[8] = 1; +void H_31(double *state, double *unused, double *out_625858545520422166) { + out_625858545520422166[0] = 0; + out_625858545520422166[1] = 0; + out_625858545520422166[2] = 0; + out_625858545520422166[3] = 0; + out_625858545520422166[4] = 0; + out_625858545520422166[5] = 0; + out_625858545520422166[6] = 0; + out_625858545520422166[7] = 0; + out_625858545520422166[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_8796389704996072171) { - err_fun(nom_x, delta_x, out_8796389704996072171); +void car_err_fun(double *nom_x, double *delta_x, double *out_5325374827696631328) { + err_fun(nom_x, delta_x, out_5325374827696631328); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8849207833158339340) { - inv_err_fun(nom_x, true_x, out_8849207833158339340); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_263695042767338590) { + inv_err_fun(nom_x, true_x, out_263695042767338590); } -void car_H_mod_fun(double *state, double *out_1622786580378488408) { - H_mod_fun(state, out_1622786580378488408); +void car_H_mod_fun(double *state, double *out_2663737869404174494) { + H_mod_fun(state, out_2663737869404174494); } -void car_f_fun(double *state, double dt, double *out_1981095305174081329) { - f_fun(state, dt, out_1981095305174081329); +void car_f_fun(double *state, double dt, double *out_8230420991833863893) { + f_fun(state, dt, out_8230420991833863893); } -void car_F_fun(double *state, double dt, double *out_3561550856242897713) { - F_fun(state, dt, out_3561550856242897713); +void car_F_fun(double *state, double dt, double *out_2756618938696411588) { + F_fun(state, dt, out_2756618938696411588); } -void car_h_25(double *state, double *unused, double *out_3280789364307995853) { - h_25(state, unused, out_3280789364307995853); +void car_h_25(double *state, double *unused, double *out_2585399580963523364) { + h_25(state, unused, out_2585399580963523364); } -void car_H_25(double *state, double *unused, double *out_445738705382224348) { - H_25(state, unused, out_445738705382224348); +void car_H_25(double *state, double *unused, double *out_4993569966627829866) { + H_25(state, unused, out_4993569966627829866); } -void car_h_24(double *state, double *unused, double *out_5100775928185398157) { - h_24(state, unused, out_5100775928185398157); +void car_h_24(double *state, double *unused, double *out_7866369606921911129) { + h_24(state, unused, out_7866369606921911129); } -void car_H_24(double *state, double *unused, double *out_399257903401471051) { - H_24(state, unused, out_399257903401471051); +void car_H_24(double *state, double *unused, double *out_124755101600123014) { + H_24(state, unused, out_124755101600123014); } -void car_h_30(double *state, double *unused, double *out_3331749478418022440) { - h_30(state, unused, out_3331749478418022440); +void car_h_30(double *state, double *unused, double *out_1052049947290498986) { + h_30(state, unused, out_1052049947290498986); } -void car_H_30(double *state, double *unused, double *out_2072594253125024279) { - H_30(state, unused, out_2072594253125024279); +void car_H_30(double *state, double *unused, double *out_465873636500221668) { + H_30(state, unused, out_465873636500221668); } -void car_h_26(double *state, double *unused, double *out_1066202602863606572) { - h_26(state, unused, out_1066202602863606572); +void car_h_26(double *state, double *unused, double *out_3137755351529974294) { + h_26(state, unused, out_3137755351529974294); } -void car_H_26(double *state, double *unused, double *out_4187242024256280572) { - H_26(state, unused, out_4187242024256280572); +void car_H_26(double *state, double *unused, double *out_1252066647753773642) { + H_26(state, unused, out_1252066647753773642); } -void car_h_27(double *state, double *unused, double *out_3118252153105853293) { - h_27(state, unused, out_3118252153105853293); +void car_h_27(double *state, double *unused, double *out_3292885906595191724) { + h_27(state, unused, out_3292885906595191724); } -void car_H_27(double *state, double *unused, double *out_4296188324308967496) { - H_27(state, unused, out_4296188324308967496); +void car_H_27(double *state, double *unused, double *out_2689467707684164885) { + H_27(state, unused, out_2689467707684164885); } -void car_h_29(double *state, double *unused, double *out_8557656694253347387) { - h_29(state, unused, out_8557656694253347387); +void car_h_29(double *state, double *unused, double *out_5346991570123350160) { + h_29(state, unused, out_5346991570123350160); } -void car_H_29(double *state, double *unused, double *out_2582825597439416463) { - H_29(state, unused, out_2582825597439416463); +void car_H_29(double *state, double *unused, double *out_976104980814613852) { + H_29(state, unused, out_976104980814613852); } -void car_h_28(double *state, double *unused, double *out_4285995954181995030) { - h_28(state, unused, out_4285995954181995030); +void car_h_28(double *state, double *unused, double *out_6565695485309518484) { + h_28(state, unused, out_6565695485309518484); } -void car_H_28(double *state, double *unused, double *out_2499573419630114111) { - H_28(state, unused, out_2499573419630114111); +void car_H_28(double *state, double *unused, double *out_2939735252379940103) { + H_28(state, unused, out_2939735252379940103); } -void car_h_31(double *state, double *unused, double *out_6669294028137815771) { - h_31(state, unused, out_6669294028137815771); +void car_h_31(double *state, double *unused, double *out_7496068987004855670) { + h_31(state, unused, out_7496068987004855670); } -void car_H_31(double *state, double *unused, double *out_415092743505263920) { - H_31(state, unused, out_415092743505263920); +void car_H_31(double *state, double *unused, double *out_625858545520422166) { + H_31(state, unused, out_625858545520422166); } 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 5cc4e3315..442334395 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_8796389704996072171); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8849207833158339340); -void car_H_mod_fun(double *state, double *out_1622786580378488408); -void car_f_fun(double *state, double dt, double *out_1981095305174081329); -void car_F_fun(double *state, double dt, double *out_3561550856242897713); -void car_h_25(double *state, double *unused, double *out_3280789364307995853); -void car_H_25(double *state, double *unused, double *out_445738705382224348); -void car_h_24(double *state, double *unused, double *out_5100775928185398157); -void car_H_24(double *state, double *unused, double *out_399257903401471051); -void car_h_30(double *state, double *unused, double *out_3331749478418022440); -void car_H_30(double *state, double *unused, double *out_2072594253125024279); -void car_h_26(double *state, double *unused, double *out_1066202602863606572); -void car_H_26(double *state, double *unused, double *out_4187242024256280572); -void car_h_27(double *state, double *unused, double *out_3118252153105853293); -void car_H_27(double *state, double *unused, double *out_4296188324308967496); -void car_h_29(double *state, double *unused, double *out_8557656694253347387); -void car_H_29(double *state, double *unused, double *out_2582825597439416463); -void car_h_28(double *state, double *unused, double *out_4285995954181995030); -void car_H_28(double *state, double *unused, double *out_2499573419630114111); -void car_h_31(double *state, double *unused, double *out_6669294028137815771); -void car_H_31(double *state, double *unused, double *out_415092743505263920); +void car_err_fun(double *nom_x, double *delta_x, double *out_5325374827696631328); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_263695042767338590); +void car_H_mod_fun(double *state, double *out_2663737869404174494); +void car_f_fun(double *state, double dt, double *out_8230420991833863893); +void car_F_fun(double *state, double dt, double *out_2756618938696411588); +void car_h_25(double *state, double *unused, double *out_2585399580963523364); +void car_H_25(double *state, double *unused, double *out_4993569966627829866); +void car_h_24(double *state, double *unused, double *out_7866369606921911129); +void car_H_24(double *state, double *unused, double *out_124755101600123014); +void car_h_30(double *state, double *unused, double *out_1052049947290498986); +void car_H_30(double *state, double *unused, double *out_465873636500221668); +void car_h_26(double *state, double *unused, double *out_3137755351529974294); +void car_H_26(double *state, double *unused, double *out_1252066647753773642); +void car_h_27(double *state, double *unused, double *out_3292885906595191724); +void car_H_27(double *state, double *unused, double *out_2689467707684164885); +void car_h_29(double *state, double *unused, double *out_5346991570123350160); +void car_H_29(double *state, double *unused, double *out_976104980814613852); +void car_h_28(double *state, double *unused, double *out_6565695485309518484); +void car_H_28(double *state, double *unused, double *out_2939735252379940103); +void car_h_31(double *state, double *unused, double *out_7496068987004855670); +void car_H_31(double *state, double *unused, double *out_625858545520422166); 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 bb03e9348..84702ace9 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_2383575066684203465) { - out_2383575066684203465[0] = delta_x[0] + nom_x[0]; - out_2383575066684203465[1] = delta_x[1] + nom_x[1]; - out_2383575066684203465[2] = delta_x[2] + nom_x[2]; - out_2383575066684203465[3] = delta_x[3] + nom_x[3]; - out_2383575066684203465[4] = delta_x[4] + nom_x[4]; - out_2383575066684203465[5] = delta_x[5] + nom_x[5]; - out_2383575066684203465[6] = delta_x[6] + nom_x[6]; - out_2383575066684203465[7] = delta_x[7] + nom_x[7]; - out_2383575066684203465[8] = delta_x[8] + nom_x[8]; - out_2383575066684203465[9] = delta_x[9] + nom_x[9]; - out_2383575066684203465[10] = delta_x[10] + nom_x[10]; - out_2383575066684203465[11] = delta_x[11] + nom_x[11]; - out_2383575066684203465[12] = delta_x[12] + nom_x[12]; - out_2383575066684203465[13] = delta_x[13] + nom_x[13]; - out_2383575066684203465[14] = delta_x[14] + nom_x[14]; - out_2383575066684203465[15] = delta_x[15] + nom_x[15]; - out_2383575066684203465[16] = delta_x[16] + nom_x[16]; - out_2383575066684203465[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_4154280524256563890) { + out_4154280524256563890[0] = delta_x[0] + nom_x[0]; + out_4154280524256563890[1] = delta_x[1] + nom_x[1]; + out_4154280524256563890[2] = delta_x[2] + nom_x[2]; + out_4154280524256563890[3] = delta_x[3] + nom_x[3]; + out_4154280524256563890[4] = delta_x[4] + nom_x[4]; + out_4154280524256563890[5] = delta_x[5] + nom_x[5]; + out_4154280524256563890[6] = delta_x[6] + nom_x[6]; + out_4154280524256563890[7] = delta_x[7] + nom_x[7]; + out_4154280524256563890[8] = delta_x[8] + nom_x[8]; + out_4154280524256563890[9] = delta_x[9] + nom_x[9]; + out_4154280524256563890[10] = delta_x[10] + nom_x[10]; + out_4154280524256563890[11] = delta_x[11] + nom_x[11]; + out_4154280524256563890[12] = delta_x[12] + nom_x[12]; + out_4154280524256563890[13] = delta_x[13] + nom_x[13]; + out_4154280524256563890[14] = delta_x[14] + nom_x[14]; + out_4154280524256563890[15] = delta_x[15] + nom_x[15]; + out_4154280524256563890[16] = delta_x[16] + nom_x[16]; + out_4154280524256563890[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_2531792213831641268) { - out_2531792213831641268[0] = -nom_x[0] + true_x[0]; - out_2531792213831641268[1] = -nom_x[1] + true_x[1]; - out_2531792213831641268[2] = -nom_x[2] + true_x[2]; - out_2531792213831641268[3] = -nom_x[3] + true_x[3]; - out_2531792213831641268[4] = -nom_x[4] + true_x[4]; - out_2531792213831641268[5] = -nom_x[5] + true_x[5]; - out_2531792213831641268[6] = -nom_x[6] + true_x[6]; - out_2531792213831641268[7] = -nom_x[7] + true_x[7]; - out_2531792213831641268[8] = -nom_x[8] + true_x[8]; - out_2531792213831641268[9] = -nom_x[9] + true_x[9]; - out_2531792213831641268[10] = -nom_x[10] + true_x[10]; - out_2531792213831641268[11] = -nom_x[11] + true_x[11]; - out_2531792213831641268[12] = -nom_x[12] + true_x[12]; - out_2531792213831641268[13] = -nom_x[13] + true_x[13]; - out_2531792213831641268[14] = -nom_x[14] + true_x[14]; - out_2531792213831641268[15] = -nom_x[15] + true_x[15]; - out_2531792213831641268[16] = -nom_x[16] + true_x[16]; - out_2531792213831641268[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5487211563177375728) { + out_5487211563177375728[0] = -nom_x[0] + true_x[0]; + out_5487211563177375728[1] = -nom_x[1] + true_x[1]; + out_5487211563177375728[2] = -nom_x[2] + true_x[2]; + out_5487211563177375728[3] = -nom_x[3] + true_x[3]; + out_5487211563177375728[4] = -nom_x[4] + true_x[4]; + out_5487211563177375728[5] = -nom_x[5] + true_x[5]; + out_5487211563177375728[6] = -nom_x[6] + true_x[6]; + out_5487211563177375728[7] = -nom_x[7] + true_x[7]; + out_5487211563177375728[8] = -nom_x[8] + true_x[8]; + out_5487211563177375728[9] = -nom_x[9] + true_x[9]; + out_5487211563177375728[10] = -nom_x[10] + true_x[10]; + out_5487211563177375728[11] = -nom_x[11] + true_x[11]; + out_5487211563177375728[12] = -nom_x[12] + true_x[12]; + out_5487211563177375728[13] = -nom_x[13] + true_x[13]; + out_5487211563177375728[14] = -nom_x[14] + true_x[14]; + out_5487211563177375728[15] = -nom_x[15] + true_x[15]; + out_5487211563177375728[16] = -nom_x[16] + true_x[16]; + out_5487211563177375728[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_5270168453751144292) { - out_5270168453751144292[0] = 1.0; - out_5270168453751144292[1] = 0.0; - out_5270168453751144292[2] = 0.0; - out_5270168453751144292[3] = 0.0; - out_5270168453751144292[4] = 0.0; - out_5270168453751144292[5] = 0.0; - out_5270168453751144292[6] = 0.0; - out_5270168453751144292[7] = 0.0; - out_5270168453751144292[8] = 0.0; - out_5270168453751144292[9] = 0.0; - out_5270168453751144292[10] = 0.0; - out_5270168453751144292[11] = 0.0; - out_5270168453751144292[12] = 0.0; - out_5270168453751144292[13] = 0.0; - out_5270168453751144292[14] = 0.0; - out_5270168453751144292[15] = 0.0; - out_5270168453751144292[16] = 0.0; - out_5270168453751144292[17] = 0.0; - out_5270168453751144292[18] = 0.0; - out_5270168453751144292[19] = 1.0; - out_5270168453751144292[20] = 0.0; - out_5270168453751144292[21] = 0.0; - out_5270168453751144292[22] = 0.0; - out_5270168453751144292[23] = 0.0; - out_5270168453751144292[24] = 0.0; - out_5270168453751144292[25] = 0.0; - out_5270168453751144292[26] = 0.0; - out_5270168453751144292[27] = 0.0; - out_5270168453751144292[28] = 0.0; - out_5270168453751144292[29] = 0.0; - out_5270168453751144292[30] = 0.0; - out_5270168453751144292[31] = 0.0; - out_5270168453751144292[32] = 0.0; - out_5270168453751144292[33] = 0.0; - out_5270168453751144292[34] = 0.0; - out_5270168453751144292[35] = 0.0; - out_5270168453751144292[36] = 0.0; - out_5270168453751144292[37] = 0.0; - out_5270168453751144292[38] = 1.0; - out_5270168453751144292[39] = 0.0; - out_5270168453751144292[40] = 0.0; - out_5270168453751144292[41] = 0.0; - out_5270168453751144292[42] = 0.0; - out_5270168453751144292[43] = 0.0; - out_5270168453751144292[44] = 0.0; - out_5270168453751144292[45] = 0.0; - out_5270168453751144292[46] = 0.0; - out_5270168453751144292[47] = 0.0; - out_5270168453751144292[48] = 0.0; - out_5270168453751144292[49] = 0.0; - out_5270168453751144292[50] = 0.0; - out_5270168453751144292[51] = 0.0; - out_5270168453751144292[52] = 0.0; - out_5270168453751144292[53] = 0.0; - out_5270168453751144292[54] = 0.0; - out_5270168453751144292[55] = 0.0; - out_5270168453751144292[56] = 0.0; - out_5270168453751144292[57] = 1.0; - out_5270168453751144292[58] = 0.0; - out_5270168453751144292[59] = 0.0; - out_5270168453751144292[60] = 0.0; - out_5270168453751144292[61] = 0.0; - out_5270168453751144292[62] = 0.0; - out_5270168453751144292[63] = 0.0; - out_5270168453751144292[64] = 0.0; - out_5270168453751144292[65] = 0.0; - out_5270168453751144292[66] = 0.0; - out_5270168453751144292[67] = 0.0; - out_5270168453751144292[68] = 0.0; - out_5270168453751144292[69] = 0.0; - out_5270168453751144292[70] = 0.0; - out_5270168453751144292[71] = 0.0; - out_5270168453751144292[72] = 0.0; - out_5270168453751144292[73] = 0.0; - out_5270168453751144292[74] = 0.0; - out_5270168453751144292[75] = 0.0; - out_5270168453751144292[76] = 1.0; - out_5270168453751144292[77] = 0.0; - out_5270168453751144292[78] = 0.0; - out_5270168453751144292[79] = 0.0; - out_5270168453751144292[80] = 0.0; - out_5270168453751144292[81] = 0.0; - out_5270168453751144292[82] = 0.0; - out_5270168453751144292[83] = 0.0; - out_5270168453751144292[84] = 0.0; - out_5270168453751144292[85] = 0.0; - out_5270168453751144292[86] = 0.0; - out_5270168453751144292[87] = 0.0; - out_5270168453751144292[88] = 0.0; - out_5270168453751144292[89] = 0.0; - out_5270168453751144292[90] = 0.0; - out_5270168453751144292[91] = 0.0; - out_5270168453751144292[92] = 0.0; - out_5270168453751144292[93] = 0.0; - out_5270168453751144292[94] = 0.0; - out_5270168453751144292[95] = 1.0; - out_5270168453751144292[96] = 0.0; - out_5270168453751144292[97] = 0.0; - out_5270168453751144292[98] = 0.0; - out_5270168453751144292[99] = 0.0; - out_5270168453751144292[100] = 0.0; - out_5270168453751144292[101] = 0.0; - out_5270168453751144292[102] = 0.0; - out_5270168453751144292[103] = 0.0; - out_5270168453751144292[104] = 0.0; - out_5270168453751144292[105] = 0.0; - out_5270168453751144292[106] = 0.0; - out_5270168453751144292[107] = 0.0; - out_5270168453751144292[108] = 0.0; - out_5270168453751144292[109] = 0.0; - out_5270168453751144292[110] = 0.0; - out_5270168453751144292[111] = 0.0; - out_5270168453751144292[112] = 0.0; - out_5270168453751144292[113] = 0.0; - out_5270168453751144292[114] = 1.0; - out_5270168453751144292[115] = 0.0; - out_5270168453751144292[116] = 0.0; - out_5270168453751144292[117] = 0.0; - out_5270168453751144292[118] = 0.0; - out_5270168453751144292[119] = 0.0; - out_5270168453751144292[120] = 0.0; - out_5270168453751144292[121] = 0.0; - out_5270168453751144292[122] = 0.0; - out_5270168453751144292[123] = 0.0; - out_5270168453751144292[124] = 0.0; - out_5270168453751144292[125] = 0.0; - out_5270168453751144292[126] = 0.0; - out_5270168453751144292[127] = 0.0; - out_5270168453751144292[128] = 0.0; - out_5270168453751144292[129] = 0.0; - out_5270168453751144292[130] = 0.0; - out_5270168453751144292[131] = 0.0; - out_5270168453751144292[132] = 0.0; - out_5270168453751144292[133] = 1.0; - out_5270168453751144292[134] = 0.0; - out_5270168453751144292[135] = 0.0; - out_5270168453751144292[136] = 0.0; - out_5270168453751144292[137] = 0.0; - out_5270168453751144292[138] = 0.0; - out_5270168453751144292[139] = 0.0; - out_5270168453751144292[140] = 0.0; - out_5270168453751144292[141] = 0.0; - out_5270168453751144292[142] = 0.0; - out_5270168453751144292[143] = 0.0; - out_5270168453751144292[144] = 0.0; - out_5270168453751144292[145] = 0.0; - out_5270168453751144292[146] = 0.0; - out_5270168453751144292[147] = 0.0; - out_5270168453751144292[148] = 0.0; - out_5270168453751144292[149] = 0.0; - out_5270168453751144292[150] = 0.0; - out_5270168453751144292[151] = 0.0; - out_5270168453751144292[152] = 1.0; - out_5270168453751144292[153] = 0.0; - out_5270168453751144292[154] = 0.0; - out_5270168453751144292[155] = 0.0; - out_5270168453751144292[156] = 0.0; - out_5270168453751144292[157] = 0.0; - out_5270168453751144292[158] = 0.0; - out_5270168453751144292[159] = 0.0; - out_5270168453751144292[160] = 0.0; - out_5270168453751144292[161] = 0.0; - out_5270168453751144292[162] = 0.0; - out_5270168453751144292[163] = 0.0; - out_5270168453751144292[164] = 0.0; - out_5270168453751144292[165] = 0.0; - out_5270168453751144292[166] = 0.0; - out_5270168453751144292[167] = 0.0; - out_5270168453751144292[168] = 0.0; - out_5270168453751144292[169] = 0.0; - out_5270168453751144292[170] = 0.0; - out_5270168453751144292[171] = 1.0; - out_5270168453751144292[172] = 0.0; - out_5270168453751144292[173] = 0.0; - out_5270168453751144292[174] = 0.0; - out_5270168453751144292[175] = 0.0; - out_5270168453751144292[176] = 0.0; - out_5270168453751144292[177] = 0.0; - out_5270168453751144292[178] = 0.0; - out_5270168453751144292[179] = 0.0; - out_5270168453751144292[180] = 0.0; - out_5270168453751144292[181] = 0.0; - out_5270168453751144292[182] = 0.0; - out_5270168453751144292[183] = 0.0; - out_5270168453751144292[184] = 0.0; - out_5270168453751144292[185] = 0.0; - out_5270168453751144292[186] = 0.0; - out_5270168453751144292[187] = 0.0; - out_5270168453751144292[188] = 0.0; - out_5270168453751144292[189] = 0.0; - out_5270168453751144292[190] = 1.0; - out_5270168453751144292[191] = 0.0; - out_5270168453751144292[192] = 0.0; - out_5270168453751144292[193] = 0.0; - out_5270168453751144292[194] = 0.0; - out_5270168453751144292[195] = 0.0; - out_5270168453751144292[196] = 0.0; - out_5270168453751144292[197] = 0.0; - out_5270168453751144292[198] = 0.0; - out_5270168453751144292[199] = 0.0; - out_5270168453751144292[200] = 0.0; - out_5270168453751144292[201] = 0.0; - out_5270168453751144292[202] = 0.0; - out_5270168453751144292[203] = 0.0; - out_5270168453751144292[204] = 0.0; - out_5270168453751144292[205] = 0.0; - out_5270168453751144292[206] = 0.0; - out_5270168453751144292[207] = 0.0; - out_5270168453751144292[208] = 0.0; - out_5270168453751144292[209] = 1.0; - out_5270168453751144292[210] = 0.0; - out_5270168453751144292[211] = 0.0; - out_5270168453751144292[212] = 0.0; - out_5270168453751144292[213] = 0.0; - out_5270168453751144292[214] = 0.0; - out_5270168453751144292[215] = 0.0; - out_5270168453751144292[216] = 0.0; - out_5270168453751144292[217] = 0.0; - out_5270168453751144292[218] = 0.0; - out_5270168453751144292[219] = 0.0; - out_5270168453751144292[220] = 0.0; - out_5270168453751144292[221] = 0.0; - out_5270168453751144292[222] = 0.0; - out_5270168453751144292[223] = 0.0; - out_5270168453751144292[224] = 0.0; - out_5270168453751144292[225] = 0.0; - out_5270168453751144292[226] = 0.0; - out_5270168453751144292[227] = 0.0; - out_5270168453751144292[228] = 1.0; - out_5270168453751144292[229] = 0.0; - out_5270168453751144292[230] = 0.0; - out_5270168453751144292[231] = 0.0; - out_5270168453751144292[232] = 0.0; - out_5270168453751144292[233] = 0.0; - out_5270168453751144292[234] = 0.0; - out_5270168453751144292[235] = 0.0; - out_5270168453751144292[236] = 0.0; - out_5270168453751144292[237] = 0.0; - out_5270168453751144292[238] = 0.0; - out_5270168453751144292[239] = 0.0; - out_5270168453751144292[240] = 0.0; - out_5270168453751144292[241] = 0.0; - out_5270168453751144292[242] = 0.0; - out_5270168453751144292[243] = 0.0; - out_5270168453751144292[244] = 0.0; - out_5270168453751144292[245] = 0.0; - out_5270168453751144292[246] = 0.0; - out_5270168453751144292[247] = 1.0; - out_5270168453751144292[248] = 0.0; - out_5270168453751144292[249] = 0.0; - out_5270168453751144292[250] = 0.0; - out_5270168453751144292[251] = 0.0; - out_5270168453751144292[252] = 0.0; - out_5270168453751144292[253] = 0.0; - out_5270168453751144292[254] = 0.0; - out_5270168453751144292[255] = 0.0; - out_5270168453751144292[256] = 0.0; - out_5270168453751144292[257] = 0.0; - out_5270168453751144292[258] = 0.0; - out_5270168453751144292[259] = 0.0; - out_5270168453751144292[260] = 0.0; - out_5270168453751144292[261] = 0.0; - out_5270168453751144292[262] = 0.0; - out_5270168453751144292[263] = 0.0; - out_5270168453751144292[264] = 0.0; - out_5270168453751144292[265] = 0.0; - out_5270168453751144292[266] = 1.0; - out_5270168453751144292[267] = 0.0; - out_5270168453751144292[268] = 0.0; - out_5270168453751144292[269] = 0.0; - out_5270168453751144292[270] = 0.0; - out_5270168453751144292[271] = 0.0; - out_5270168453751144292[272] = 0.0; - out_5270168453751144292[273] = 0.0; - out_5270168453751144292[274] = 0.0; - out_5270168453751144292[275] = 0.0; - out_5270168453751144292[276] = 0.0; - out_5270168453751144292[277] = 0.0; - out_5270168453751144292[278] = 0.0; - out_5270168453751144292[279] = 0.0; - out_5270168453751144292[280] = 0.0; - out_5270168453751144292[281] = 0.0; - out_5270168453751144292[282] = 0.0; - out_5270168453751144292[283] = 0.0; - out_5270168453751144292[284] = 0.0; - out_5270168453751144292[285] = 1.0; - out_5270168453751144292[286] = 0.0; - out_5270168453751144292[287] = 0.0; - out_5270168453751144292[288] = 0.0; - out_5270168453751144292[289] = 0.0; - out_5270168453751144292[290] = 0.0; - out_5270168453751144292[291] = 0.0; - out_5270168453751144292[292] = 0.0; - out_5270168453751144292[293] = 0.0; - out_5270168453751144292[294] = 0.0; - out_5270168453751144292[295] = 0.0; - out_5270168453751144292[296] = 0.0; - out_5270168453751144292[297] = 0.0; - out_5270168453751144292[298] = 0.0; - out_5270168453751144292[299] = 0.0; - out_5270168453751144292[300] = 0.0; - out_5270168453751144292[301] = 0.0; - out_5270168453751144292[302] = 0.0; - out_5270168453751144292[303] = 0.0; - out_5270168453751144292[304] = 1.0; - out_5270168453751144292[305] = 0.0; - out_5270168453751144292[306] = 0.0; - out_5270168453751144292[307] = 0.0; - out_5270168453751144292[308] = 0.0; - out_5270168453751144292[309] = 0.0; - out_5270168453751144292[310] = 0.0; - out_5270168453751144292[311] = 0.0; - out_5270168453751144292[312] = 0.0; - out_5270168453751144292[313] = 0.0; - out_5270168453751144292[314] = 0.0; - out_5270168453751144292[315] = 0.0; - out_5270168453751144292[316] = 0.0; - out_5270168453751144292[317] = 0.0; - out_5270168453751144292[318] = 0.0; - out_5270168453751144292[319] = 0.0; - out_5270168453751144292[320] = 0.0; - out_5270168453751144292[321] = 0.0; - out_5270168453751144292[322] = 0.0; - out_5270168453751144292[323] = 1.0; +void H_mod_fun(double *state, double *out_6762039440372011872) { + out_6762039440372011872[0] = 1.0; + out_6762039440372011872[1] = 0.0; + out_6762039440372011872[2] = 0.0; + out_6762039440372011872[3] = 0.0; + out_6762039440372011872[4] = 0.0; + out_6762039440372011872[5] = 0.0; + out_6762039440372011872[6] = 0.0; + out_6762039440372011872[7] = 0.0; + out_6762039440372011872[8] = 0.0; + out_6762039440372011872[9] = 0.0; + out_6762039440372011872[10] = 0.0; + out_6762039440372011872[11] = 0.0; + out_6762039440372011872[12] = 0.0; + out_6762039440372011872[13] = 0.0; + out_6762039440372011872[14] = 0.0; + out_6762039440372011872[15] = 0.0; + out_6762039440372011872[16] = 0.0; + out_6762039440372011872[17] = 0.0; + out_6762039440372011872[18] = 0.0; + out_6762039440372011872[19] = 1.0; + out_6762039440372011872[20] = 0.0; + out_6762039440372011872[21] = 0.0; + out_6762039440372011872[22] = 0.0; + out_6762039440372011872[23] = 0.0; + out_6762039440372011872[24] = 0.0; + out_6762039440372011872[25] = 0.0; + out_6762039440372011872[26] = 0.0; + out_6762039440372011872[27] = 0.0; + out_6762039440372011872[28] = 0.0; + out_6762039440372011872[29] = 0.0; + out_6762039440372011872[30] = 0.0; + out_6762039440372011872[31] = 0.0; + out_6762039440372011872[32] = 0.0; + out_6762039440372011872[33] = 0.0; + out_6762039440372011872[34] = 0.0; + out_6762039440372011872[35] = 0.0; + out_6762039440372011872[36] = 0.0; + out_6762039440372011872[37] = 0.0; + out_6762039440372011872[38] = 1.0; + out_6762039440372011872[39] = 0.0; + out_6762039440372011872[40] = 0.0; + out_6762039440372011872[41] = 0.0; + out_6762039440372011872[42] = 0.0; + out_6762039440372011872[43] = 0.0; + out_6762039440372011872[44] = 0.0; + out_6762039440372011872[45] = 0.0; + out_6762039440372011872[46] = 0.0; + out_6762039440372011872[47] = 0.0; + out_6762039440372011872[48] = 0.0; + out_6762039440372011872[49] = 0.0; + out_6762039440372011872[50] = 0.0; + out_6762039440372011872[51] = 0.0; + out_6762039440372011872[52] = 0.0; + out_6762039440372011872[53] = 0.0; + out_6762039440372011872[54] = 0.0; + out_6762039440372011872[55] = 0.0; + out_6762039440372011872[56] = 0.0; + out_6762039440372011872[57] = 1.0; + out_6762039440372011872[58] = 0.0; + out_6762039440372011872[59] = 0.0; + out_6762039440372011872[60] = 0.0; + out_6762039440372011872[61] = 0.0; + out_6762039440372011872[62] = 0.0; + out_6762039440372011872[63] = 0.0; + out_6762039440372011872[64] = 0.0; + out_6762039440372011872[65] = 0.0; + out_6762039440372011872[66] = 0.0; + out_6762039440372011872[67] = 0.0; + out_6762039440372011872[68] = 0.0; + out_6762039440372011872[69] = 0.0; + out_6762039440372011872[70] = 0.0; + out_6762039440372011872[71] = 0.0; + out_6762039440372011872[72] = 0.0; + out_6762039440372011872[73] = 0.0; + out_6762039440372011872[74] = 0.0; + out_6762039440372011872[75] = 0.0; + out_6762039440372011872[76] = 1.0; + out_6762039440372011872[77] = 0.0; + out_6762039440372011872[78] = 0.0; + out_6762039440372011872[79] = 0.0; + out_6762039440372011872[80] = 0.0; + out_6762039440372011872[81] = 0.0; + out_6762039440372011872[82] = 0.0; + out_6762039440372011872[83] = 0.0; + out_6762039440372011872[84] = 0.0; + out_6762039440372011872[85] = 0.0; + out_6762039440372011872[86] = 0.0; + out_6762039440372011872[87] = 0.0; + out_6762039440372011872[88] = 0.0; + out_6762039440372011872[89] = 0.0; + out_6762039440372011872[90] = 0.0; + out_6762039440372011872[91] = 0.0; + out_6762039440372011872[92] = 0.0; + out_6762039440372011872[93] = 0.0; + out_6762039440372011872[94] = 0.0; + out_6762039440372011872[95] = 1.0; + out_6762039440372011872[96] = 0.0; + out_6762039440372011872[97] = 0.0; + out_6762039440372011872[98] = 0.0; + out_6762039440372011872[99] = 0.0; + out_6762039440372011872[100] = 0.0; + out_6762039440372011872[101] = 0.0; + out_6762039440372011872[102] = 0.0; + out_6762039440372011872[103] = 0.0; + out_6762039440372011872[104] = 0.0; + out_6762039440372011872[105] = 0.0; + out_6762039440372011872[106] = 0.0; + out_6762039440372011872[107] = 0.0; + out_6762039440372011872[108] = 0.0; + out_6762039440372011872[109] = 0.0; + out_6762039440372011872[110] = 0.0; + out_6762039440372011872[111] = 0.0; + out_6762039440372011872[112] = 0.0; + out_6762039440372011872[113] = 0.0; + out_6762039440372011872[114] = 1.0; + out_6762039440372011872[115] = 0.0; + out_6762039440372011872[116] = 0.0; + out_6762039440372011872[117] = 0.0; + out_6762039440372011872[118] = 0.0; + out_6762039440372011872[119] = 0.0; + out_6762039440372011872[120] = 0.0; + out_6762039440372011872[121] = 0.0; + out_6762039440372011872[122] = 0.0; + out_6762039440372011872[123] = 0.0; + out_6762039440372011872[124] = 0.0; + out_6762039440372011872[125] = 0.0; + out_6762039440372011872[126] = 0.0; + out_6762039440372011872[127] = 0.0; + out_6762039440372011872[128] = 0.0; + out_6762039440372011872[129] = 0.0; + out_6762039440372011872[130] = 0.0; + out_6762039440372011872[131] = 0.0; + out_6762039440372011872[132] = 0.0; + out_6762039440372011872[133] = 1.0; + out_6762039440372011872[134] = 0.0; + out_6762039440372011872[135] = 0.0; + out_6762039440372011872[136] = 0.0; + out_6762039440372011872[137] = 0.0; + out_6762039440372011872[138] = 0.0; + out_6762039440372011872[139] = 0.0; + out_6762039440372011872[140] = 0.0; + out_6762039440372011872[141] = 0.0; + out_6762039440372011872[142] = 0.0; + out_6762039440372011872[143] = 0.0; + out_6762039440372011872[144] = 0.0; + out_6762039440372011872[145] = 0.0; + out_6762039440372011872[146] = 0.0; + out_6762039440372011872[147] = 0.0; + out_6762039440372011872[148] = 0.0; + out_6762039440372011872[149] = 0.0; + out_6762039440372011872[150] = 0.0; + out_6762039440372011872[151] = 0.0; + out_6762039440372011872[152] = 1.0; + out_6762039440372011872[153] = 0.0; + out_6762039440372011872[154] = 0.0; + out_6762039440372011872[155] = 0.0; + out_6762039440372011872[156] = 0.0; + out_6762039440372011872[157] = 0.0; + out_6762039440372011872[158] = 0.0; + out_6762039440372011872[159] = 0.0; + out_6762039440372011872[160] = 0.0; + out_6762039440372011872[161] = 0.0; + out_6762039440372011872[162] = 0.0; + out_6762039440372011872[163] = 0.0; + out_6762039440372011872[164] = 0.0; + out_6762039440372011872[165] = 0.0; + out_6762039440372011872[166] = 0.0; + out_6762039440372011872[167] = 0.0; + out_6762039440372011872[168] = 0.0; + out_6762039440372011872[169] = 0.0; + out_6762039440372011872[170] = 0.0; + out_6762039440372011872[171] = 1.0; + out_6762039440372011872[172] = 0.0; + out_6762039440372011872[173] = 0.0; + out_6762039440372011872[174] = 0.0; + out_6762039440372011872[175] = 0.0; + out_6762039440372011872[176] = 0.0; + out_6762039440372011872[177] = 0.0; + out_6762039440372011872[178] = 0.0; + out_6762039440372011872[179] = 0.0; + out_6762039440372011872[180] = 0.0; + out_6762039440372011872[181] = 0.0; + out_6762039440372011872[182] = 0.0; + out_6762039440372011872[183] = 0.0; + out_6762039440372011872[184] = 0.0; + out_6762039440372011872[185] = 0.0; + out_6762039440372011872[186] = 0.0; + out_6762039440372011872[187] = 0.0; + out_6762039440372011872[188] = 0.0; + out_6762039440372011872[189] = 0.0; + out_6762039440372011872[190] = 1.0; + out_6762039440372011872[191] = 0.0; + out_6762039440372011872[192] = 0.0; + out_6762039440372011872[193] = 0.0; + out_6762039440372011872[194] = 0.0; + out_6762039440372011872[195] = 0.0; + out_6762039440372011872[196] = 0.0; + out_6762039440372011872[197] = 0.0; + out_6762039440372011872[198] = 0.0; + out_6762039440372011872[199] = 0.0; + out_6762039440372011872[200] = 0.0; + out_6762039440372011872[201] = 0.0; + out_6762039440372011872[202] = 0.0; + out_6762039440372011872[203] = 0.0; + out_6762039440372011872[204] = 0.0; + out_6762039440372011872[205] = 0.0; + out_6762039440372011872[206] = 0.0; + out_6762039440372011872[207] = 0.0; + out_6762039440372011872[208] = 0.0; + out_6762039440372011872[209] = 1.0; + out_6762039440372011872[210] = 0.0; + out_6762039440372011872[211] = 0.0; + out_6762039440372011872[212] = 0.0; + out_6762039440372011872[213] = 0.0; + out_6762039440372011872[214] = 0.0; + out_6762039440372011872[215] = 0.0; + out_6762039440372011872[216] = 0.0; + out_6762039440372011872[217] = 0.0; + out_6762039440372011872[218] = 0.0; + out_6762039440372011872[219] = 0.0; + out_6762039440372011872[220] = 0.0; + out_6762039440372011872[221] = 0.0; + out_6762039440372011872[222] = 0.0; + out_6762039440372011872[223] = 0.0; + out_6762039440372011872[224] = 0.0; + out_6762039440372011872[225] = 0.0; + out_6762039440372011872[226] = 0.0; + out_6762039440372011872[227] = 0.0; + out_6762039440372011872[228] = 1.0; + out_6762039440372011872[229] = 0.0; + out_6762039440372011872[230] = 0.0; + out_6762039440372011872[231] = 0.0; + out_6762039440372011872[232] = 0.0; + out_6762039440372011872[233] = 0.0; + out_6762039440372011872[234] = 0.0; + out_6762039440372011872[235] = 0.0; + out_6762039440372011872[236] = 0.0; + out_6762039440372011872[237] = 0.0; + out_6762039440372011872[238] = 0.0; + out_6762039440372011872[239] = 0.0; + out_6762039440372011872[240] = 0.0; + out_6762039440372011872[241] = 0.0; + out_6762039440372011872[242] = 0.0; + out_6762039440372011872[243] = 0.0; + out_6762039440372011872[244] = 0.0; + out_6762039440372011872[245] = 0.0; + out_6762039440372011872[246] = 0.0; + out_6762039440372011872[247] = 1.0; + out_6762039440372011872[248] = 0.0; + out_6762039440372011872[249] = 0.0; + out_6762039440372011872[250] = 0.0; + out_6762039440372011872[251] = 0.0; + out_6762039440372011872[252] = 0.0; + out_6762039440372011872[253] = 0.0; + out_6762039440372011872[254] = 0.0; + out_6762039440372011872[255] = 0.0; + out_6762039440372011872[256] = 0.0; + out_6762039440372011872[257] = 0.0; + out_6762039440372011872[258] = 0.0; + out_6762039440372011872[259] = 0.0; + out_6762039440372011872[260] = 0.0; + out_6762039440372011872[261] = 0.0; + out_6762039440372011872[262] = 0.0; + out_6762039440372011872[263] = 0.0; + out_6762039440372011872[264] = 0.0; + out_6762039440372011872[265] = 0.0; + out_6762039440372011872[266] = 1.0; + out_6762039440372011872[267] = 0.0; + out_6762039440372011872[268] = 0.0; + out_6762039440372011872[269] = 0.0; + out_6762039440372011872[270] = 0.0; + out_6762039440372011872[271] = 0.0; + out_6762039440372011872[272] = 0.0; + out_6762039440372011872[273] = 0.0; + out_6762039440372011872[274] = 0.0; + out_6762039440372011872[275] = 0.0; + out_6762039440372011872[276] = 0.0; + out_6762039440372011872[277] = 0.0; + out_6762039440372011872[278] = 0.0; + out_6762039440372011872[279] = 0.0; + out_6762039440372011872[280] = 0.0; + out_6762039440372011872[281] = 0.0; + out_6762039440372011872[282] = 0.0; + out_6762039440372011872[283] = 0.0; + out_6762039440372011872[284] = 0.0; + out_6762039440372011872[285] = 1.0; + out_6762039440372011872[286] = 0.0; + out_6762039440372011872[287] = 0.0; + out_6762039440372011872[288] = 0.0; + out_6762039440372011872[289] = 0.0; + out_6762039440372011872[290] = 0.0; + out_6762039440372011872[291] = 0.0; + out_6762039440372011872[292] = 0.0; + out_6762039440372011872[293] = 0.0; + out_6762039440372011872[294] = 0.0; + out_6762039440372011872[295] = 0.0; + out_6762039440372011872[296] = 0.0; + out_6762039440372011872[297] = 0.0; + out_6762039440372011872[298] = 0.0; + out_6762039440372011872[299] = 0.0; + out_6762039440372011872[300] = 0.0; + out_6762039440372011872[301] = 0.0; + out_6762039440372011872[302] = 0.0; + out_6762039440372011872[303] = 0.0; + out_6762039440372011872[304] = 1.0; + out_6762039440372011872[305] = 0.0; + out_6762039440372011872[306] = 0.0; + out_6762039440372011872[307] = 0.0; + out_6762039440372011872[308] = 0.0; + out_6762039440372011872[309] = 0.0; + out_6762039440372011872[310] = 0.0; + out_6762039440372011872[311] = 0.0; + out_6762039440372011872[312] = 0.0; + out_6762039440372011872[313] = 0.0; + out_6762039440372011872[314] = 0.0; + out_6762039440372011872[315] = 0.0; + out_6762039440372011872[316] = 0.0; + out_6762039440372011872[317] = 0.0; + out_6762039440372011872[318] = 0.0; + out_6762039440372011872[319] = 0.0; + out_6762039440372011872[320] = 0.0; + out_6762039440372011872[321] = 0.0; + out_6762039440372011872[322] = 0.0; + out_6762039440372011872[323] = 1.0; } -void f_fun(double *state, double dt, double *out_5870118476100851027) { - out_5870118476100851027[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_5870118476100851027[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_5870118476100851027[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_5870118476100851027[3] = dt*state[12] + state[3]; - out_5870118476100851027[4] = dt*state[13] + state[4]; - out_5870118476100851027[5] = dt*state[14] + state[5]; - out_5870118476100851027[6] = state[6]; - out_5870118476100851027[7] = state[7]; - out_5870118476100851027[8] = state[8]; - out_5870118476100851027[9] = state[9]; - out_5870118476100851027[10] = state[10]; - out_5870118476100851027[11] = state[11]; - out_5870118476100851027[12] = state[12]; - out_5870118476100851027[13] = state[13]; - out_5870118476100851027[14] = state[14]; - out_5870118476100851027[15] = state[15]; - out_5870118476100851027[16] = state[16]; - out_5870118476100851027[17] = state[17]; +void f_fun(double *state, double dt, double *out_2487690271541209027) { + out_2487690271541209027[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_2487690271541209027[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_2487690271541209027[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_2487690271541209027[3] = dt*state[12] + state[3]; + out_2487690271541209027[4] = dt*state[13] + state[4]; + out_2487690271541209027[5] = dt*state[14] + state[5]; + out_2487690271541209027[6] = state[6]; + out_2487690271541209027[7] = state[7]; + out_2487690271541209027[8] = state[8]; + out_2487690271541209027[9] = state[9]; + out_2487690271541209027[10] = state[10]; + out_2487690271541209027[11] = state[11]; + out_2487690271541209027[12] = state[12]; + out_2487690271541209027[13] = state[13]; + out_2487690271541209027[14] = state[14]; + out_2487690271541209027[15] = state[15]; + out_2487690271541209027[16] = state[16]; + out_2487690271541209027[17] = state[17]; } -void F_fun(double *state, double dt, double *out_6643036112755764466) { - out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[2] = 0; - out_6643036112755764466[3] = 0; - out_6643036112755764466[4] = 0; - out_6643036112755764466[5] = 0; - out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[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_6643036112755764466[9] = 0; - out_6643036112755764466[10] = 0; - out_6643036112755764466[11] = 0; - out_6643036112755764466[12] = 0; - out_6643036112755764466[13] = 0; - out_6643036112755764466[14] = 0; - out_6643036112755764466[15] = 0; - out_6643036112755764466[16] = 0; - out_6643036112755764466[17] = 0; - out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[20] = 0; - out_6643036112755764466[21] = 0; - out_6643036112755764466[22] = 0; - out_6643036112755764466[23] = 0; - out_6643036112755764466[24] = 0; - out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[27] = 0; - out_6643036112755764466[28] = 0; - out_6643036112755764466[29] = 0; - out_6643036112755764466[30] = 0; - out_6643036112755764466[31] = 0; - out_6643036112755764466[32] = 0; - out_6643036112755764466[33] = 0; - out_6643036112755764466[34] = 0; - out_6643036112755764466[35] = 0; - out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[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_6643036112755764466[39] = 0; - out_6643036112755764466[40] = 0; - out_6643036112755764466[41] = 0; - out_6643036112755764466[42] = 0; - out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[45] = 0; - out_6643036112755764466[46] = 0; - out_6643036112755764466[47] = 0; - out_6643036112755764466[48] = 0; - out_6643036112755764466[49] = 0; - out_6643036112755764466[50] = 0; - out_6643036112755764466[51] = 0; - out_6643036112755764466[52] = 0; - out_6643036112755764466[53] = 0; - out_6643036112755764466[54] = 0; - out_6643036112755764466[55] = 0; - out_6643036112755764466[56] = 0; - out_6643036112755764466[57] = 1; - out_6643036112755764466[58] = 0; - out_6643036112755764466[59] = 0; - out_6643036112755764466[60] = 0; - out_6643036112755764466[61] = 0; - out_6643036112755764466[62] = 0; - out_6643036112755764466[63] = 0; - out_6643036112755764466[64] = 0; - out_6643036112755764466[65] = 0; - out_6643036112755764466[66] = dt; - out_6643036112755764466[67] = 0; - out_6643036112755764466[68] = 0; - out_6643036112755764466[69] = 0; - out_6643036112755764466[70] = 0; - out_6643036112755764466[71] = 0; - out_6643036112755764466[72] = 0; - out_6643036112755764466[73] = 0; - out_6643036112755764466[74] = 0; - out_6643036112755764466[75] = 0; - out_6643036112755764466[76] = 1; - out_6643036112755764466[77] = 0; - out_6643036112755764466[78] = 0; - out_6643036112755764466[79] = 0; - out_6643036112755764466[80] = 0; - out_6643036112755764466[81] = 0; - out_6643036112755764466[82] = 0; - out_6643036112755764466[83] = 0; - out_6643036112755764466[84] = 0; - out_6643036112755764466[85] = dt; - out_6643036112755764466[86] = 0; - out_6643036112755764466[87] = 0; - out_6643036112755764466[88] = 0; - out_6643036112755764466[89] = 0; - out_6643036112755764466[90] = 0; - out_6643036112755764466[91] = 0; - out_6643036112755764466[92] = 0; - out_6643036112755764466[93] = 0; - out_6643036112755764466[94] = 0; - out_6643036112755764466[95] = 1; - out_6643036112755764466[96] = 0; - out_6643036112755764466[97] = 0; - out_6643036112755764466[98] = 0; - out_6643036112755764466[99] = 0; - out_6643036112755764466[100] = 0; - out_6643036112755764466[101] = 0; - out_6643036112755764466[102] = 0; - out_6643036112755764466[103] = 0; - out_6643036112755764466[104] = dt; - out_6643036112755764466[105] = 0; - out_6643036112755764466[106] = 0; - out_6643036112755764466[107] = 0; - out_6643036112755764466[108] = 0; - out_6643036112755764466[109] = 0; - out_6643036112755764466[110] = 0; - out_6643036112755764466[111] = 0; - out_6643036112755764466[112] = 0; - out_6643036112755764466[113] = 0; - out_6643036112755764466[114] = 1; - out_6643036112755764466[115] = 0; - out_6643036112755764466[116] = 0; - out_6643036112755764466[117] = 0; - out_6643036112755764466[118] = 0; - out_6643036112755764466[119] = 0; - out_6643036112755764466[120] = 0; - out_6643036112755764466[121] = 0; - out_6643036112755764466[122] = 0; - out_6643036112755764466[123] = 0; - out_6643036112755764466[124] = 0; - out_6643036112755764466[125] = 0; - out_6643036112755764466[126] = 0; - out_6643036112755764466[127] = 0; - out_6643036112755764466[128] = 0; - out_6643036112755764466[129] = 0; - out_6643036112755764466[130] = 0; - out_6643036112755764466[131] = 0; - out_6643036112755764466[132] = 0; - out_6643036112755764466[133] = 1; - out_6643036112755764466[134] = 0; - out_6643036112755764466[135] = 0; - out_6643036112755764466[136] = 0; - out_6643036112755764466[137] = 0; - out_6643036112755764466[138] = 0; - out_6643036112755764466[139] = 0; - out_6643036112755764466[140] = 0; - out_6643036112755764466[141] = 0; - out_6643036112755764466[142] = 0; - out_6643036112755764466[143] = 0; - out_6643036112755764466[144] = 0; - out_6643036112755764466[145] = 0; - out_6643036112755764466[146] = 0; - out_6643036112755764466[147] = 0; - out_6643036112755764466[148] = 0; - out_6643036112755764466[149] = 0; - out_6643036112755764466[150] = 0; - out_6643036112755764466[151] = 0; - out_6643036112755764466[152] = 1; - out_6643036112755764466[153] = 0; - out_6643036112755764466[154] = 0; - out_6643036112755764466[155] = 0; - out_6643036112755764466[156] = 0; - out_6643036112755764466[157] = 0; - out_6643036112755764466[158] = 0; - out_6643036112755764466[159] = 0; - out_6643036112755764466[160] = 0; - out_6643036112755764466[161] = 0; - out_6643036112755764466[162] = 0; - out_6643036112755764466[163] = 0; - out_6643036112755764466[164] = 0; - out_6643036112755764466[165] = 0; - out_6643036112755764466[166] = 0; - out_6643036112755764466[167] = 0; - out_6643036112755764466[168] = 0; - out_6643036112755764466[169] = 0; - out_6643036112755764466[170] = 0; - out_6643036112755764466[171] = 1; - out_6643036112755764466[172] = 0; - out_6643036112755764466[173] = 0; - out_6643036112755764466[174] = 0; - out_6643036112755764466[175] = 0; - out_6643036112755764466[176] = 0; - out_6643036112755764466[177] = 0; - out_6643036112755764466[178] = 0; - out_6643036112755764466[179] = 0; - out_6643036112755764466[180] = 0; - out_6643036112755764466[181] = 0; - out_6643036112755764466[182] = 0; - out_6643036112755764466[183] = 0; - out_6643036112755764466[184] = 0; - out_6643036112755764466[185] = 0; - out_6643036112755764466[186] = 0; - out_6643036112755764466[187] = 0; - out_6643036112755764466[188] = 0; - out_6643036112755764466[189] = 0; - out_6643036112755764466[190] = 1; - out_6643036112755764466[191] = 0; - out_6643036112755764466[192] = 0; - out_6643036112755764466[193] = 0; - out_6643036112755764466[194] = 0; - out_6643036112755764466[195] = 0; - out_6643036112755764466[196] = 0; - out_6643036112755764466[197] = 0; - out_6643036112755764466[198] = 0; - out_6643036112755764466[199] = 0; - out_6643036112755764466[200] = 0; - out_6643036112755764466[201] = 0; - out_6643036112755764466[202] = 0; - out_6643036112755764466[203] = 0; - out_6643036112755764466[204] = 0; - out_6643036112755764466[205] = 0; - out_6643036112755764466[206] = 0; - out_6643036112755764466[207] = 0; - out_6643036112755764466[208] = 0; - out_6643036112755764466[209] = 1; - out_6643036112755764466[210] = 0; - out_6643036112755764466[211] = 0; - out_6643036112755764466[212] = 0; - out_6643036112755764466[213] = 0; - out_6643036112755764466[214] = 0; - out_6643036112755764466[215] = 0; - out_6643036112755764466[216] = 0; - out_6643036112755764466[217] = 0; - out_6643036112755764466[218] = 0; - out_6643036112755764466[219] = 0; - out_6643036112755764466[220] = 0; - out_6643036112755764466[221] = 0; - out_6643036112755764466[222] = 0; - out_6643036112755764466[223] = 0; - out_6643036112755764466[224] = 0; - out_6643036112755764466[225] = 0; - out_6643036112755764466[226] = 0; - out_6643036112755764466[227] = 0; - out_6643036112755764466[228] = 1; - out_6643036112755764466[229] = 0; - out_6643036112755764466[230] = 0; - out_6643036112755764466[231] = 0; - out_6643036112755764466[232] = 0; - out_6643036112755764466[233] = 0; - out_6643036112755764466[234] = 0; - out_6643036112755764466[235] = 0; - out_6643036112755764466[236] = 0; - out_6643036112755764466[237] = 0; - out_6643036112755764466[238] = 0; - out_6643036112755764466[239] = 0; - out_6643036112755764466[240] = 0; - out_6643036112755764466[241] = 0; - out_6643036112755764466[242] = 0; - out_6643036112755764466[243] = 0; - out_6643036112755764466[244] = 0; - out_6643036112755764466[245] = 0; - out_6643036112755764466[246] = 0; - out_6643036112755764466[247] = 1; - out_6643036112755764466[248] = 0; - out_6643036112755764466[249] = 0; - out_6643036112755764466[250] = 0; - out_6643036112755764466[251] = 0; - out_6643036112755764466[252] = 0; - out_6643036112755764466[253] = 0; - out_6643036112755764466[254] = 0; - out_6643036112755764466[255] = 0; - out_6643036112755764466[256] = 0; - out_6643036112755764466[257] = 0; - out_6643036112755764466[258] = 0; - out_6643036112755764466[259] = 0; - out_6643036112755764466[260] = 0; - out_6643036112755764466[261] = 0; - out_6643036112755764466[262] = 0; - out_6643036112755764466[263] = 0; - out_6643036112755764466[264] = 0; - out_6643036112755764466[265] = 0; - out_6643036112755764466[266] = 1; - out_6643036112755764466[267] = 0; - out_6643036112755764466[268] = 0; - out_6643036112755764466[269] = 0; - out_6643036112755764466[270] = 0; - out_6643036112755764466[271] = 0; - out_6643036112755764466[272] = 0; - out_6643036112755764466[273] = 0; - out_6643036112755764466[274] = 0; - out_6643036112755764466[275] = 0; - out_6643036112755764466[276] = 0; - out_6643036112755764466[277] = 0; - out_6643036112755764466[278] = 0; - out_6643036112755764466[279] = 0; - out_6643036112755764466[280] = 0; - out_6643036112755764466[281] = 0; - out_6643036112755764466[282] = 0; - out_6643036112755764466[283] = 0; - out_6643036112755764466[284] = 0; - out_6643036112755764466[285] = 1; - out_6643036112755764466[286] = 0; - out_6643036112755764466[287] = 0; - out_6643036112755764466[288] = 0; - out_6643036112755764466[289] = 0; - out_6643036112755764466[290] = 0; - out_6643036112755764466[291] = 0; - out_6643036112755764466[292] = 0; - out_6643036112755764466[293] = 0; - out_6643036112755764466[294] = 0; - out_6643036112755764466[295] = 0; - out_6643036112755764466[296] = 0; - out_6643036112755764466[297] = 0; - out_6643036112755764466[298] = 0; - out_6643036112755764466[299] = 0; - out_6643036112755764466[300] = 0; - out_6643036112755764466[301] = 0; - out_6643036112755764466[302] = 0; - out_6643036112755764466[303] = 0; - out_6643036112755764466[304] = 1; - out_6643036112755764466[305] = 0; - out_6643036112755764466[306] = 0; - out_6643036112755764466[307] = 0; - out_6643036112755764466[308] = 0; - out_6643036112755764466[309] = 0; - out_6643036112755764466[310] = 0; - out_6643036112755764466[311] = 0; - out_6643036112755764466[312] = 0; - out_6643036112755764466[313] = 0; - out_6643036112755764466[314] = 0; - out_6643036112755764466[315] = 0; - out_6643036112755764466[316] = 0; - out_6643036112755764466[317] = 0; - out_6643036112755764466[318] = 0; - out_6643036112755764466[319] = 0; - out_6643036112755764466[320] = 0; - out_6643036112755764466[321] = 0; - out_6643036112755764466[322] = 0; - out_6643036112755764466[323] = 1; +void F_fun(double *state, double dt, double *out_950182517979020623) { + out_950182517979020623[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_950182517979020623[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_950182517979020623[2] = 0; + out_950182517979020623[3] = 0; + out_950182517979020623[4] = 0; + out_950182517979020623[5] = 0; + out_950182517979020623[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_950182517979020623[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_950182517979020623[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_950182517979020623[9] = 0; + out_950182517979020623[10] = 0; + out_950182517979020623[11] = 0; + out_950182517979020623[12] = 0; + out_950182517979020623[13] = 0; + out_950182517979020623[14] = 0; + out_950182517979020623[15] = 0; + out_950182517979020623[16] = 0; + out_950182517979020623[17] = 0; + out_950182517979020623[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_950182517979020623[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_950182517979020623[20] = 0; + out_950182517979020623[21] = 0; + out_950182517979020623[22] = 0; + out_950182517979020623[23] = 0; + out_950182517979020623[24] = 0; + out_950182517979020623[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_950182517979020623[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_950182517979020623[27] = 0; + out_950182517979020623[28] = 0; + out_950182517979020623[29] = 0; + out_950182517979020623[30] = 0; + out_950182517979020623[31] = 0; + out_950182517979020623[32] = 0; + out_950182517979020623[33] = 0; + out_950182517979020623[34] = 0; + out_950182517979020623[35] = 0; + out_950182517979020623[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_950182517979020623[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_950182517979020623[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_950182517979020623[39] = 0; + out_950182517979020623[40] = 0; + out_950182517979020623[41] = 0; + out_950182517979020623[42] = 0; + out_950182517979020623[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_950182517979020623[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_950182517979020623[45] = 0; + out_950182517979020623[46] = 0; + out_950182517979020623[47] = 0; + out_950182517979020623[48] = 0; + out_950182517979020623[49] = 0; + out_950182517979020623[50] = 0; + out_950182517979020623[51] = 0; + out_950182517979020623[52] = 0; + out_950182517979020623[53] = 0; + out_950182517979020623[54] = 0; + out_950182517979020623[55] = 0; + out_950182517979020623[56] = 0; + out_950182517979020623[57] = 1; + out_950182517979020623[58] = 0; + out_950182517979020623[59] = 0; + out_950182517979020623[60] = 0; + out_950182517979020623[61] = 0; + out_950182517979020623[62] = 0; + out_950182517979020623[63] = 0; + out_950182517979020623[64] = 0; + out_950182517979020623[65] = 0; + out_950182517979020623[66] = dt; + out_950182517979020623[67] = 0; + out_950182517979020623[68] = 0; + out_950182517979020623[69] = 0; + out_950182517979020623[70] = 0; + out_950182517979020623[71] = 0; + out_950182517979020623[72] = 0; + out_950182517979020623[73] = 0; + out_950182517979020623[74] = 0; + out_950182517979020623[75] = 0; + out_950182517979020623[76] = 1; + out_950182517979020623[77] = 0; + out_950182517979020623[78] = 0; + out_950182517979020623[79] = 0; + out_950182517979020623[80] = 0; + out_950182517979020623[81] = 0; + out_950182517979020623[82] = 0; + out_950182517979020623[83] = 0; + out_950182517979020623[84] = 0; + out_950182517979020623[85] = dt; + out_950182517979020623[86] = 0; + out_950182517979020623[87] = 0; + out_950182517979020623[88] = 0; + out_950182517979020623[89] = 0; + out_950182517979020623[90] = 0; + out_950182517979020623[91] = 0; + out_950182517979020623[92] = 0; + out_950182517979020623[93] = 0; + out_950182517979020623[94] = 0; + out_950182517979020623[95] = 1; + out_950182517979020623[96] = 0; + out_950182517979020623[97] = 0; + out_950182517979020623[98] = 0; + out_950182517979020623[99] = 0; + out_950182517979020623[100] = 0; + out_950182517979020623[101] = 0; + out_950182517979020623[102] = 0; + out_950182517979020623[103] = 0; + out_950182517979020623[104] = dt; + out_950182517979020623[105] = 0; + out_950182517979020623[106] = 0; + out_950182517979020623[107] = 0; + out_950182517979020623[108] = 0; + out_950182517979020623[109] = 0; + out_950182517979020623[110] = 0; + out_950182517979020623[111] = 0; + out_950182517979020623[112] = 0; + out_950182517979020623[113] = 0; + out_950182517979020623[114] = 1; + out_950182517979020623[115] = 0; + out_950182517979020623[116] = 0; + out_950182517979020623[117] = 0; + out_950182517979020623[118] = 0; + out_950182517979020623[119] = 0; + out_950182517979020623[120] = 0; + out_950182517979020623[121] = 0; + out_950182517979020623[122] = 0; + out_950182517979020623[123] = 0; + out_950182517979020623[124] = 0; + out_950182517979020623[125] = 0; + out_950182517979020623[126] = 0; + out_950182517979020623[127] = 0; + out_950182517979020623[128] = 0; + out_950182517979020623[129] = 0; + out_950182517979020623[130] = 0; + out_950182517979020623[131] = 0; + out_950182517979020623[132] = 0; + out_950182517979020623[133] = 1; + out_950182517979020623[134] = 0; + out_950182517979020623[135] = 0; + out_950182517979020623[136] = 0; + out_950182517979020623[137] = 0; + out_950182517979020623[138] = 0; + out_950182517979020623[139] = 0; + out_950182517979020623[140] = 0; + out_950182517979020623[141] = 0; + out_950182517979020623[142] = 0; + out_950182517979020623[143] = 0; + out_950182517979020623[144] = 0; + out_950182517979020623[145] = 0; + out_950182517979020623[146] = 0; + out_950182517979020623[147] = 0; + out_950182517979020623[148] = 0; + out_950182517979020623[149] = 0; + out_950182517979020623[150] = 0; + out_950182517979020623[151] = 0; + out_950182517979020623[152] = 1; + out_950182517979020623[153] = 0; + out_950182517979020623[154] = 0; + out_950182517979020623[155] = 0; + out_950182517979020623[156] = 0; + out_950182517979020623[157] = 0; + out_950182517979020623[158] = 0; + out_950182517979020623[159] = 0; + out_950182517979020623[160] = 0; + out_950182517979020623[161] = 0; + out_950182517979020623[162] = 0; + out_950182517979020623[163] = 0; + out_950182517979020623[164] = 0; + out_950182517979020623[165] = 0; + out_950182517979020623[166] = 0; + out_950182517979020623[167] = 0; + out_950182517979020623[168] = 0; + out_950182517979020623[169] = 0; + out_950182517979020623[170] = 0; + out_950182517979020623[171] = 1; + out_950182517979020623[172] = 0; + out_950182517979020623[173] = 0; + out_950182517979020623[174] = 0; + out_950182517979020623[175] = 0; + out_950182517979020623[176] = 0; + out_950182517979020623[177] = 0; + out_950182517979020623[178] = 0; + out_950182517979020623[179] = 0; + out_950182517979020623[180] = 0; + out_950182517979020623[181] = 0; + out_950182517979020623[182] = 0; + out_950182517979020623[183] = 0; + out_950182517979020623[184] = 0; + out_950182517979020623[185] = 0; + out_950182517979020623[186] = 0; + out_950182517979020623[187] = 0; + out_950182517979020623[188] = 0; + out_950182517979020623[189] = 0; + out_950182517979020623[190] = 1; + out_950182517979020623[191] = 0; + out_950182517979020623[192] = 0; + out_950182517979020623[193] = 0; + out_950182517979020623[194] = 0; + out_950182517979020623[195] = 0; + out_950182517979020623[196] = 0; + out_950182517979020623[197] = 0; + out_950182517979020623[198] = 0; + out_950182517979020623[199] = 0; + out_950182517979020623[200] = 0; + out_950182517979020623[201] = 0; + out_950182517979020623[202] = 0; + out_950182517979020623[203] = 0; + out_950182517979020623[204] = 0; + out_950182517979020623[205] = 0; + out_950182517979020623[206] = 0; + out_950182517979020623[207] = 0; + out_950182517979020623[208] = 0; + out_950182517979020623[209] = 1; + out_950182517979020623[210] = 0; + out_950182517979020623[211] = 0; + out_950182517979020623[212] = 0; + out_950182517979020623[213] = 0; + out_950182517979020623[214] = 0; + out_950182517979020623[215] = 0; + out_950182517979020623[216] = 0; + out_950182517979020623[217] = 0; + out_950182517979020623[218] = 0; + out_950182517979020623[219] = 0; + out_950182517979020623[220] = 0; + out_950182517979020623[221] = 0; + out_950182517979020623[222] = 0; + out_950182517979020623[223] = 0; + out_950182517979020623[224] = 0; + out_950182517979020623[225] = 0; + out_950182517979020623[226] = 0; + out_950182517979020623[227] = 0; + out_950182517979020623[228] = 1; + out_950182517979020623[229] = 0; + out_950182517979020623[230] = 0; + out_950182517979020623[231] = 0; + out_950182517979020623[232] = 0; + out_950182517979020623[233] = 0; + out_950182517979020623[234] = 0; + out_950182517979020623[235] = 0; + out_950182517979020623[236] = 0; + out_950182517979020623[237] = 0; + out_950182517979020623[238] = 0; + out_950182517979020623[239] = 0; + out_950182517979020623[240] = 0; + out_950182517979020623[241] = 0; + out_950182517979020623[242] = 0; + out_950182517979020623[243] = 0; + out_950182517979020623[244] = 0; + out_950182517979020623[245] = 0; + out_950182517979020623[246] = 0; + out_950182517979020623[247] = 1; + out_950182517979020623[248] = 0; + out_950182517979020623[249] = 0; + out_950182517979020623[250] = 0; + out_950182517979020623[251] = 0; + out_950182517979020623[252] = 0; + out_950182517979020623[253] = 0; + out_950182517979020623[254] = 0; + out_950182517979020623[255] = 0; + out_950182517979020623[256] = 0; + out_950182517979020623[257] = 0; + out_950182517979020623[258] = 0; + out_950182517979020623[259] = 0; + out_950182517979020623[260] = 0; + out_950182517979020623[261] = 0; + out_950182517979020623[262] = 0; + out_950182517979020623[263] = 0; + out_950182517979020623[264] = 0; + out_950182517979020623[265] = 0; + out_950182517979020623[266] = 1; + out_950182517979020623[267] = 0; + out_950182517979020623[268] = 0; + out_950182517979020623[269] = 0; + out_950182517979020623[270] = 0; + out_950182517979020623[271] = 0; + out_950182517979020623[272] = 0; + out_950182517979020623[273] = 0; + out_950182517979020623[274] = 0; + out_950182517979020623[275] = 0; + out_950182517979020623[276] = 0; + out_950182517979020623[277] = 0; + out_950182517979020623[278] = 0; + out_950182517979020623[279] = 0; + out_950182517979020623[280] = 0; + out_950182517979020623[281] = 0; + out_950182517979020623[282] = 0; + out_950182517979020623[283] = 0; + out_950182517979020623[284] = 0; + out_950182517979020623[285] = 1; + out_950182517979020623[286] = 0; + out_950182517979020623[287] = 0; + out_950182517979020623[288] = 0; + out_950182517979020623[289] = 0; + out_950182517979020623[290] = 0; + out_950182517979020623[291] = 0; + out_950182517979020623[292] = 0; + out_950182517979020623[293] = 0; + out_950182517979020623[294] = 0; + out_950182517979020623[295] = 0; + out_950182517979020623[296] = 0; + out_950182517979020623[297] = 0; + out_950182517979020623[298] = 0; + out_950182517979020623[299] = 0; + out_950182517979020623[300] = 0; + out_950182517979020623[301] = 0; + out_950182517979020623[302] = 0; + out_950182517979020623[303] = 0; + out_950182517979020623[304] = 1; + out_950182517979020623[305] = 0; + out_950182517979020623[306] = 0; + out_950182517979020623[307] = 0; + out_950182517979020623[308] = 0; + out_950182517979020623[309] = 0; + out_950182517979020623[310] = 0; + out_950182517979020623[311] = 0; + out_950182517979020623[312] = 0; + out_950182517979020623[313] = 0; + out_950182517979020623[314] = 0; + out_950182517979020623[315] = 0; + out_950182517979020623[316] = 0; + out_950182517979020623[317] = 0; + out_950182517979020623[318] = 0; + out_950182517979020623[319] = 0; + out_950182517979020623[320] = 0; + out_950182517979020623[321] = 0; + out_950182517979020623[322] = 0; + out_950182517979020623[323] = 1; } -void h_4(double *state, double *unused, double *out_3825151169355500496) { - out_3825151169355500496[0] = state[6] + state[9]; - out_3825151169355500496[1] = state[7] + state[10]; - out_3825151169355500496[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_1064982969821053038) { + out_1064982969821053038[0] = state[6] + state[9]; + out_1064982969821053038[1] = state[7] + state[10]; + out_1064982969821053038[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_4438379387875418186) { - out_4438379387875418186[0] = 0; - out_4438379387875418186[1] = 0; - out_4438379387875418186[2] = 0; - out_4438379387875418186[3] = 0; - out_4438379387875418186[4] = 0; - out_4438379387875418186[5] = 0; - out_4438379387875418186[6] = 1; - out_4438379387875418186[7] = 0; - out_4438379387875418186[8] = 0; - out_4438379387875418186[9] = 1; - out_4438379387875418186[10] = 0; - out_4438379387875418186[11] = 0; - out_4438379387875418186[12] = 0; - out_4438379387875418186[13] = 0; - out_4438379387875418186[14] = 0; - out_4438379387875418186[15] = 0; - out_4438379387875418186[16] = 0; - out_4438379387875418186[17] = 0; - out_4438379387875418186[18] = 0; - out_4438379387875418186[19] = 0; - out_4438379387875418186[20] = 0; - out_4438379387875418186[21] = 0; - out_4438379387875418186[22] = 0; - out_4438379387875418186[23] = 0; - out_4438379387875418186[24] = 0; - out_4438379387875418186[25] = 1; - out_4438379387875418186[26] = 0; - out_4438379387875418186[27] = 0; - out_4438379387875418186[28] = 1; - out_4438379387875418186[29] = 0; - out_4438379387875418186[30] = 0; - out_4438379387875418186[31] = 0; - out_4438379387875418186[32] = 0; - out_4438379387875418186[33] = 0; - out_4438379387875418186[34] = 0; - out_4438379387875418186[35] = 0; - out_4438379387875418186[36] = 0; - out_4438379387875418186[37] = 0; - out_4438379387875418186[38] = 0; - out_4438379387875418186[39] = 0; - out_4438379387875418186[40] = 0; - out_4438379387875418186[41] = 0; - out_4438379387875418186[42] = 0; - out_4438379387875418186[43] = 0; - out_4438379387875418186[44] = 1; - out_4438379387875418186[45] = 0; - out_4438379387875418186[46] = 0; - out_4438379387875418186[47] = 1; - out_4438379387875418186[48] = 0; - out_4438379387875418186[49] = 0; - out_4438379387875418186[50] = 0; - out_4438379387875418186[51] = 0; - out_4438379387875418186[52] = 0; - out_4438379387875418186[53] = 0; +void H_4(double *state, double *unused, double *out_1976156791710977266) { + out_1976156791710977266[0] = 0; + out_1976156791710977266[1] = 0; + out_1976156791710977266[2] = 0; + out_1976156791710977266[3] = 0; + out_1976156791710977266[4] = 0; + out_1976156791710977266[5] = 0; + out_1976156791710977266[6] = 1; + out_1976156791710977266[7] = 0; + out_1976156791710977266[8] = 0; + out_1976156791710977266[9] = 1; + out_1976156791710977266[10] = 0; + out_1976156791710977266[11] = 0; + out_1976156791710977266[12] = 0; + out_1976156791710977266[13] = 0; + out_1976156791710977266[14] = 0; + out_1976156791710977266[15] = 0; + out_1976156791710977266[16] = 0; + out_1976156791710977266[17] = 0; + out_1976156791710977266[18] = 0; + out_1976156791710977266[19] = 0; + out_1976156791710977266[20] = 0; + out_1976156791710977266[21] = 0; + out_1976156791710977266[22] = 0; + out_1976156791710977266[23] = 0; + out_1976156791710977266[24] = 0; + out_1976156791710977266[25] = 1; + out_1976156791710977266[26] = 0; + out_1976156791710977266[27] = 0; + out_1976156791710977266[28] = 1; + out_1976156791710977266[29] = 0; + out_1976156791710977266[30] = 0; + out_1976156791710977266[31] = 0; + out_1976156791710977266[32] = 0; + out_1976156791710977266[33] = 0; + out_1976156791710977266[34] = 0; + out_1976156791710977266[35] = 0; + out_1976156791710977266[36] = 0; + out_1976156791710977266[37] = 0; + out_1976156791710977266[38] = 0; + out_1976156791710977266[39] = 0; + out_1976156791710977266[40] = 0; + out_1976156791710977266[41] = 0; + out_1976156791710977266[42] = 0; + out_1976156791710977266[43] = 0; + out_1976156791710977266[44] = 1; + out_1976156791710977266[45] = 0; + out_1976156791710977266[46] = 0; + out_1976156791710977266[47] = 1; + out_1976156791710977266[48] = 0; + out_1976156791710977266[49] = 0; + out_1976156791710977266[50] = 0; + out_1976156791710977266[51] = 0; + out_1976156791710977266[52] = 0; + out_1976156791710977266[53] = 0; } -void h_10(double *state, double *unused, double *out_6180343716098328505) { - out_6180343716098328505[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_6180343716098328505[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_6180343716098328505[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_9039008373436645978) { + out_9039008373436645978[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_9039008373436645978[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_9039008373436645978[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_6436902649839043686) { - out_6436902649839043686[0] = 0; - out_6436902649839043686[1] = 9.8100000000000005*cos(state[1]); - out_6436902649839043686[2] = 0; - out_6436902649839043686[3] = 0; - out_6436902649839043686[4] = -state[8]; - out_6436902649839043686[5] = state[7]; - out_6436902649839043686[6] = 0; - out_6436902649839043686[7] = state[5]; - out_6436902649839043686[8] = -state[4]; - out_6436902649839043686[9] = 0; - out_6436902649839043686[10] = 0; - out_6436902649839043686[11] = 0; - out_6436902649839043686[12] = 1; - out_6436902649839043686[13] = 0; - out_6436902649839043686[14] = 0; - out_6436902649839043686[15] = 1; - out_6436902649839043686[16] = 0; - out_6436902649839043686[17] = 0; - out_6436902649839043686[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_6436902649839043686[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_6436902649839043686[20] = 0; - out_6436902649839043686[21] = state[8]; - out_6436902649839043686[22] = 0; - out_6436902649839043686[23] = -state[6]; - out_6436902649839043686[24] = -state[5]; - out_6436902649839043686[25] = 0; - out_6436902649839043686[26] = state[3]; - out_6436902649839043686[27] = 0; - out_6436902649839043686[28] = 0; - out_6436902649839043686[29] = 0; - out_6436902649839043686[30] = 0; - out_6436902649839043686[31] = 1; - out_6436902649839043686[32] = 0; - out_6436902649839043686[33] = 0; - out_6436902649839043686[34] = 1; - out_6436902649839043686[35] = 0; - out_6436902649839043686[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_6436902649839043686[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_6436902649839043686[38] = 0; - out_6436902649839043686[39] = -state[7]; - out_6436902649839043686[40] = state[6]; - out_6436902649839043686[41] = 0; - out_6436902649839043686[42] = state[4]; - out_6436902649839043686[43] = -state[3]; - out_6436902649839043686[44] = 0; - out_6436902649839043686[45] = 0; - out_6436902649839043686[46] = 0; - out_6436902649839043686[47] = 0; - out_6436902649839043686[48] = 0; - out_6436902649839043686[49] = 0; - out_6436902649839043686[50] = 1; - out_6436902649839043686[51] = 0; - out_6436902649839043686[52] = 0; - out_6436902649839043686[53] = 1; +void H_10(double *state, double *unused, double *out_6832717040485250243) { + out_6832717040485250243[0] = 0; + out_6832717040485250243[1] = 9.8100000000000005*cos(state[1]); + out_6832717040485250243[2] = 0; + out_6832717040485250243[3] = 0; + out_6832717040485250243[4] = -state[8]; + out_6832717040485250243[5] = state[7]; + out_6832717040485250243[6] = 0; + out_6832717040485250243[7] = state[5]; + out_6832717040485250243[8] = -state[4]; + out_6832717040485250243[9] = 0; + out_6832717040485250243[10] = 0; + out_6832717040485250243[11] = 0; + out_6832717040485250243[12] = 1; + out_6832717040485250243[13] = 0; + out_6832717040485250243[14] = 0; + out_6832717040485250243[15] = 1; + out_6832717040485250243[16] = 0; + out_6832717040485250243[17] = 0; + out_6832717040485250243[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_6832717040485250243[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_6832717040485250243[20] = 0; + out_6832717040485250243[21] = state[8]; + out_6832717040485250243[22] = 0; + out_6832717040485250243[23] = -state[6]; + out_6832717040485250243[24] = -state[5]; + out_6832717040485250243[25] = 0; + out_6832717040485250243[26] = state[3]; + out_6832717040485250243[27] = 0; + out_6832717040485250243[28] = 0; + out_6832717040485250243[29] = 0; + out_6832717040485250243[30] = 0; + out_6832717040485250243[31] = 1; + out_6832717040485250243[32] = 0; + out_6832717040485250243[33] = 0; + out_6832717040485250243[34] = 1; + out_6832717040485250243[35] = 0; + out_6832717040485250243[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_6832717040485250243[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_6832717040485250243[38] = 0; + out_6832717040485250243[39] = -state[7]; + out_6832717040485250243[40] = state[6]; + out_6832717040485250243[41] = 0; + out_6832717040485250243[42] = state[4]; + out_6832717040485250243[43] = -state[3]; + out_6832717040485250243[44] = 0; + out_6832717040485250243[45] = 0; + out_6832717040485250243[46] = 0; + out_6832717040485250243[47] = 0; + out_6832717040485250243[48] = 0; + out_6832717040485250243[49] = 0; + out_6832717040485250243[50] = 1; + out_6832717040485250243[51] = 0; + out_6832717040485250243[52] = 0; + out_6832717040485250243[53] = 1; } -void h_13(double *state, double *unused, double *out_5615804281251360017) { - out_5615804281251360017[0] = state[3]; - out_5615804281251360017[1] = state[4]; - out_5615804281251360017[2] = state[5]; +void h_13(double *state, double *unused, double *out_6939466162668529956) { + out_6939466162668529956[0] = state[3]; + out_6939466162668529956[1] = state[4]; + out_6939466162668529956[2] = state[5]; } -void H_13(double *state, double *unused, double *out_8272134851177942210) { - out_8272134851177942210[0] = 0; - out_8272134851177942210[1] = 0; - out_8272134851177942210[2] = 0; - out_8272134851177942210[3] = 1; - out_8272134851177942210[4] = 0; - out_8272134851177942210[5] = 0; - out_8272134851177942210[6] = 0; - out_8272134851177942210[7] = 0; - out_8272134851177942210[8] = 0; - out_8272134851177942210[9] = 0; - out_8272134851177942210[10] = 0; - out_8272134851177942210[11] = 0; - out_8272134851177942210[12] = 0; - out_8272134851177942210[13] = 0; - out_8272134851177942210[14] = 0; - out_8272134851177942210[15] = 0; - out_8272134851177942210[16] = 0; - out_8272134851177942210[17] = 0; - out_8272134851177942210[18] = 0; - out_8272134851177942210[19] = 0; - out_8272134851177942210[20] = 0; - out_8272134851177942210[21] = 0; - out_8272134851177942210[22] = 1; - out_8272134851177942210[23] = 0; - out_8272134851177942210[24] = 0; - out_8272134851177942210[25] = 0; - out_8272134851177942210[26] = 0; - out_8272134851177942210[27] = 0; - out_8272134851177942210[28] = 0; - out_8272134851177942210[29] = 0; - out_8272134851177942210[30] = 0; - out_8272134851177942210[31] = 0; - out_8272134851177942210[32] = 0; - out_8272134851177942210[33] = 0; - out_8272134851177942210[34] = 0; - out_8272134851177942210[35] = 0; - out_8272134851177942210[36] = 0; - out_8272134851177942210[37] = 0; - out_8272134851177942210[38] = 0; - out_8272134851177942210[39] = 0; - out_8272134851177942210[40] = 0; - out_8272134851177942210[41] = 1; - out_8272134851177942210[42] = 0; - out_8272134851177942210[43] = 0; - out_8272134851177942210[44] = 0; - out_8272134851177942210[45] = 0; - out_8272134851177942210[46] = 0; - out_8272134851177942210[47] = 0; - out_8272134851177942210[48] = 0; - out_8272134851177942210[49] = 0; - out_8272134851177942210[50] = 0; - out_8272134851177942210[51] = 0; - out_8272134851177942210[52] = 0; - out_8272134851177942210[53] = 0; +void H_13(double *state, double *unused, double *out_2540758711392821370) { + out_2540758711392821370[0] = 0; + out_2540758711392821370[1] = 0; + out_2540758711392821370[2] = 0; + out_2540758711392821370[3] = 1; + out_2540758711392821370[4] = 0; + out_2540758711392821370[5] = 0; + out_2540758711392821370[6] = 0; + out_2540758711392821370[7] = 0; + out_2540758711392821370[8] = 0; + out_2540758711392821370[9] = 0; + out_2540758711392821370[10] = 0; + out_2540758711392821370[11] = 0; + out_2540758711392821370[12] = 0; + out_2540758711392821370[13] = 0; + out_2540758711392821370[14] = 0; + out_2540758711392821370[15] = 0; + out_2540758711392821370[16] = 0; + out_2540758711392821370[17] = 0; + out_2540758711392821370[18] = 0; + out_2540758711392821370[19] = 0; + out_2540758711392821370[20] = 0; + out_2540758711392821370[21] = 0; + out_2540758711392821370[22] = 1; + out_2540758711392821370[23] = 0; + out_2540758711392821370[24] = 0; + out_2540758711392821370[25] = 0; + out_2540758711392821370[26] = 0; + out_2540758711392821370[27] = 0; + out_2540758711392821370[28] = 0; + out_2540758711392821370[29] = 0; + out_2540758711392821370[30] = 0; + out_2540758711392821370[31] = 0; + out_2540758711392821370[32] = 0; + out_2540758711392821370[33] = 0; + out_2540758711392821370[34] = 0; + out_2540758711392821370[35] = 0; + out_2540758711392821370[36] = 0; + out_2540758711392821370[37] = 0; + out_2540758711392821370[38] = 0; + out_2540758711392821370[39] = 0; + out_2540758711392821370[40] = 0; + out_2540758711392821370[41] = 1; + out_2540758711392821370[42] = 0; + out_2540758711392821370[43] = 0; + out_2540758711392821370[44] = 0; + out_2540758711392821370[45] = 0; + out_2540758711392821370[46] = 0; + out_2540758711392821370[47] = 0; + out_2540758711392821370[48] = 0; + out_2540758711392821370[49] = 0; + out_2540758711392821370[50] = 0; + out_2540758711392821370[51] = 0; + out_2540758711392821370[52] = 0; + out_2540758711392821370[53] = 0; } -void h_14(double *state, double *unused, double *out_1282457731118130678) { - out_1282457731118130678[0] = state[6]; - out_1282457731118130678[1] = state[7]; - out_1282457731118130678[2] = state[8]; +void h_14(double *state, double *unused, double *out_542665533068743617) { + out_542665533068743617[0] = state[6]; + out_542665533068743617[1] = state[7]; + out_542665533068743617[2] = state[8]; } -void H_14(double *state, double *unused, double *out_7521167820170790482) { - out_7521167820170790482[0] = 0; - out_7521167820170790482[1] = 0; - out_7521167820170790482[2] = 0; - out_7521167820170790482[3] = 0; - out_7521167820170790482[4] = 0; - out_7521167820170790482[5] = 0; - out_7521167820170790482[6] = 1; - out_7521167820170790482[7] = 0; - out_7521167820170790482[8] = 0; - out_7521167820170790482[9] = 0; - out_7521167820170790482[10] = 0; - out_7521167820170790482[11] = 0; - out_7521167820170790482[12] = 0; - out_7521167820170790482[13] = 0; - out_7521167820170790482[14] = 0; - out_7521167820170790482[15] = 0; - out_7521167820170790482[16] = 0; - out_7521167820170790482[17] = 0; - out_7521167820170790482[18] = 0; - out_7521167820170790482[19] = 0; - out_7521167820170790482[20] = 0; - out_7521167820170790482[21] = 0; - out_7521167820170790482[22] = 0; - out_7521167820170790482[23] = 0; - out_7521167820170790482[24] = 0; - out_7521167820170790482[25] = 1; - out_7521167820170790482[26] = 0; - out_7521167820170790482[27] = 0; - out_7521167820170790482[28] = 0; - out_7521167820170790482[29] = 0; - out_7521167820170790482[30] = 0; - out_7521167820170790482[31] = 0; - out_7521167820170790482[32] = 0; - out_7521167820170790482[33] = 0; - out_7521167820170790482[34] = 0; - out_7521167820170790482[35] = 0; - out_7521167820170790482[36] = 0; - out_7521167820170790482[37] = 0; - out_7521167820170790482[38] = 0; - out_7521167820170790482[39] = 0; - out_7521167820170790482[40] = 0; - out_7521167820170790482[41] = 0; - out_7521167820170790482[42] = 0; - out_7521167820170790482[43] = 0; - out_7521167820170790482[44] = 1; - out_7521167820170790482[45] = 0; - out_7521167820170790482[46] = 0; - out_7521167820170790482[47] = 0; - out_7521167820170790482[48] = 0; - out_7521167820170790482[49] = 0; - out_7521167820170790482[50] = 0; - out_7521167820170790482[51] = 0; - out_7521167820170790482[52] = 0; - out_7521167820170790482[53] = 0; +void H_14(double *state, double *unused, double *out_1106631640584395030) { + out_1106631640584395030[0] = 0; + out_1106631640584395030[1] = 0; + out_1106631640584395030[2] = 0; + out_1106631640584395030[3] = 0; + out_1106631640584395030[4] = 0; + out_1106631640584395030[5] = 0; + out_1106631640584395030[6] = 1; + out_1106631640584395030[7] = 0; + out_1106631640584395030[8] = 0; + out_1106631640584395030[9] = 0; + out_1106631640584395030[10] = 0; + out_1106631640584395030[11] = 0; + out_1106631640584395030[12] = 0; + out_1106631640584395030[13] = 0; + out_1106631640584395030[14] = 0; + out_1106631640584395030[15] = 0; + out_1106631640584395030[16] = 0; + out_1106631640584395030[17] = 0; + out_1106631640584395030[18] = 0; + out_1106631640584395030[19] = 0; + out_1106631640584395030[20] = 0; + out_1106631640584395030[21] = 0; + out_1106631640584395030[22] = 0; + out_1106631640584395030[23] = 0; + out_1106631640584395030[24] = 0; + out_1106631640584395030[25] = 1; + out_1106631640584395030[26] = 0; + out_1106631640584395030[27] = 0; + out_1106631640584395030[28] = 0; + out_1106631640584395030[29] = 0; + out_1106631640584395030[30] = 0; + out_1106631640584395030[31] = 0; + out_1106631640584395030[32] = 0; + out_1106631640584395030[33] = 0; + out_1106631640584395030[34] = 0; + out_1106631640584395030[35] = 0; + out_1106631640584395030[36] = 0; + out_1106631640584395030[37] = 0; + out_1106631640584395030[38] = 0; + out_1106631640584395030[39] = 0; + out_1106631640584395030[40] = 0; + out_1106631640584395030[41] = 0; + out_1106631640584395030[42] = 0; + out_1106631640584395030[43] = 0; + out_1106631640584395030[44] = 1; + out_1106631640584395030[45] = 0; + out_1106631640584395030[46] = 0; + out_1106631640584395030[47] = 0; + out_1106631640584395030[48] = 0; + out_1106631640584395030[49] = 0; + out_1106631640584395030[50] = 0; + out_1106631640584395030[51] = 0; + out_1106631640584395030[52] = 0; + out_1106631640584395030[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_2383575066684203465) { - err_fun(nom_x, delta_x, out_2383575066684203465); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4154280524256563890) { + err_fun(nom_x, delta_x, out_4154280524256563890); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2531792213831641268) { - inv_err_fun(nom_x, true_x, out_2531792213831641268); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5487211563177375728) { + inv_err_fun(nom_x, true_x, out_5487211563177375728); } -void pose_H_mod_fun(double *state, double *out_5270168453751144292) { - H_mod_fun(state, out_5270168453751144292); +void pose_H_mod_fun(double *state, double *out_6762039440372011872) { + H_mod_fun(state, out_6762039440372011872); } -void pose_f_fun(double *state, double dt, double *out_5870118476100851027) { - f_fun(state, dt, out_5870118476100851027); +void pose_f_fun(double *state, double dt, double *out_2487690271541209027) { + f_fun(state, dt, out_2487690271541209027); } -void pose_F_fun(double *state, double dt, double *out_6643036112755764466) { - F_fun(state, dt, out_6643036112755764466); +void pose_F_fun(double *state, double dt, double *out_950182517979020623) { + F_fun(state, dt, out_950182517979020623); } -void pose_h_4(double *state, double *unused, double *out_3825151169355500496) { - h_4(state, unused, out_3825151169355500496); +void pose_h_4(double *state, double *unused, double *out_1064982969821053038) { + h_4(state, unused, out_1064982969821053038); } -void pose_H_4(double *state, double *unused, double *out_4438379387875418186) { - H_4(state, unused, out_4438379387875418186); +void pose_H_4(double *state, double *unused, double *out_1976156791710977266) { + H_4(state, unused, out_1976156791710977266); } -void pose_h_10(double *state, double *unused, double *out_6180343716098328505) { - h_10(state, unused, out_6180343716098328505); +void pose_h_10(double *state, double *unused, double *out_9039008373436645978) { + h_10(state, unused, out_9039008373436645978); } -void pose_H_10(double *state, double *unused, double *out_6436902649839043686) { - H_10(state, unused, out_6436902649839043686); +void pose_H_10(double *state, double *unused, double *out_6832717040485250243) { + H_10(state, unused, out_6832717040485250243); } -void pose_h_13(double *state, double *unused, double *out_5615804281251360017) { - h_13(state, unused, out_5615804281251360017); +void pose_h_13(double *state, double *unused, double *out_6939466162668529956) { + h_13(state, unused, out_6939466162668529956); } -void pose_H_13(double *state, double *unused, double *out_8272134851177942210) { - H_13(state, unused, out_8272134851177942210); +void pose_H_13(double *state, double *unused, double *out_2540758711392821370) { + H_13(state, unused, out_2540758711392821370); } -void pose_h_14(double *state, double *unused, double *out_1282457731118130678) { - h_14(state, unused, out_1282457731118130678); +void pose_h_14(double *state, double *unused, double *out_542665533068743617) { + h_14(state, unused, out_542665533068743617); } -void pose_H_14(double *state, double *unused, double *out_7521167820170790482) { - H_14(state, unused, out_7521167820170790482); +void pose_H_14(double *state, double *unused, double *out_1106631640584395030) { + H_14(state, unused, out_1106631640584395030); } 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 5dadeaf60..b33fb04e2 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_2383575066684203465); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2531792213831641268); -void pose_H_mod_fun(double *state, double *out_5270168453751144292); -void pose_f_fun(double *state, double dt, double *out_5870118476100851027); -void pose_F_fun(double *state, double dt, double *out_6643036112755764466); -void pose_h_4(double *state, double *unused, double *out_3825151169355500496); -void pose_H_4(double *state, double *unused, double *out_4438379387875418186); -void pose_h_10(double *state, double *unused, double *out_6180343716098328505); -void pose_H_10(double *state, double *unused, double *out_6436902649839043686); -void pose_h_13(double *state, double *unused, double *out_5615804281251360017); -void pose_H_13(double *state, double *unused, double *out_8272134851177942210); -void pose_h_14(double *state, double *unused, double *out_1282457731118130678); -void pose_H_14(double *state, double *unused, double *out_7521167820170790482); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4154280524256563890); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5487211563177375728); +void pose_H_mod_fun(double *state, double *out_6762039440372011872); +void pose_f_fun(double *state, double dt, double *out_2487690271541209027); +void pose_F_fun(double *state, double dt, double *out_950182517979020623); +void pose_h_4(double *state, double *unused, double *out_1064982969821053038); +void pose_H_4(double *state, double *unused, double *out_1976156791710977266); +void pose_h_10(double *state, double *unused, double *out_9039008373436645978); +void pose_H_10(double *state, double *unused, double *out_6832717040485250243); +void pose_h_13(double *state, double *unused, double *out_6939466162668529956); +void pose_H_13(double *state, double *unused, double *out_2540758711392821370); +void pose_h_14(double *state, double *unused, double *out_542665533068743617); +void pose_H_14(double *state, double *unused, double *out_1106631640584395030); 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 7ff571faf..b6f773bf7 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/starpilot/ui/qt/offroad/starpilot_settings.cc b/starpilot/ui/qt/offroad/starpilot_settings.cc index 4aae12fcd..63935ee4b 100644 --- a/starpilot/ui/qt/offroad/starpilot_settings.cc +++ b/starpilot/ui/qt/offroad/starpilot_settings.cc @@ -14,6 +14,8 @@ #include "starpilot/ui/qt/offroad/wheel_settings.h" #include "system/hardware/hw.h" +#include + bool nnffLogFileExists(const QString &carFingerprint) { static QStringList models; static QMap substitutes; @@ -409,82 +411,35 @@ void StarPilotSettingsWindow::updateVariables() { float currentVEgoStartingStock = params.getFloat("VEgoStartingStock"); float currentVEgoStoppingStock = params.getFloat("VEgoStoppingStock"); - if (currentDelayStock != steerActuatorDelay && steerActuatorDelay != 0) { - if (params.getFloat("SteerDelay") == currentDelayStock || currentDelayStock == 0) { - params.putFloatNonBlocking("SteerDelay", steerActuatorDelay); - } - params.putFloatNonBlocking("SteerDelayStock", steerActuatorDelay); - } + auto nearlyEqual = [](float a, float b) { + return std::fabs(a - b) <= 1e-6f; + }; - if (currentFrictionStock != friction && friction != 0) { - if (params.getFloat("SteerFriction") == currentFrictionStock || currentFrictionStock == 0) { - params.putFloatNonBlocking("SteerFriction", friction); + auto syncStockParam = [&](const char *key, const char *stockKey, float currentStock, float liveValue) { + if (nearlyEqual(currentStock, liveValue) || nearlyEqual(liveValue, 0.0f)) { + return; } - params.putFloatNonBlocking("SteerFrictionStock", friction); - } - if (currentKPStock != steerKp && steerKp != 0) { - if (params.getFloat("SteerKP") == currentKPStock || currentKPStock == 0) { - params.putFloatNonBlocking("SteerKP", steerKp); + float currentValue = params.getFloat(key); + bool shouldUpdateLiveValue = nearlyEqual(currentValue, currentStock) || nearlyEqual(currentValue, 0.0f); + if (shouldUpdateLiveValue) { + params.putFloatNonBlocking(key, liveValue); } - params.putFloatNonBlocking("SteerKPStock", steerKp); - } - if (currentLatAccelStock != latAccelFactor && latAccelFactor != 0) { - if (params.getFloat("SteerLatAccel") == currentLatAccelStock || currentLatAccelStock == 0) { - params.putFloatNonBlocking("SteerLatAccel", latAccelFactor); - } - params.putFloatNonBlocking("SteerLatAccelStock", latAccelFactor); - } + params.putFloatNonBlocking(stockKey, liveValue); + }; - if (currentLongDelayStock != longitudinalActuatorDelay && longitudinalActuatorDelay != 0) { - if (params.getFloat("LongitudinalActuatorDelay") == currentLongDelayStock || currentLongDelayStock == 0) { - params.putFloatNonBlocking("LongitudinalActuatorDelay", longitudinalActuatorDelay); - } - params.putFloatNonBlocking("LongitudinalActuatorDelayStock", longitudinalActuatorDelay); - } - - if (currentStartAccelStock != startAccel && startAccel != 0) { - if (params.getFloat("StartAccel") == currentStartAccelStock || currentStartAccelStock == 0) { - params.putFloatNonBlocking("StartAccel", startAccel); - } - params.putFloatNonBlocking("StartAccelStock", startAccel); - } - - if (currentSteerRatioStock != steerRatio && steerRatio != 0) { - if (params.getFloat("SteerRatio") == currentSteerRatioStock || currentSteerRatioStock == 0) { - params.putFloatNonBlocking("SteerRatio", steerRatio); - } - params.putFloatNonBlocking("SteerRatioStock", steerRatio); - } - - if (currentStopAccelStock != stopAccel && stopAccel != 0) { - if (params.getFloat("StopAccel") == currentStopAccelStock || currentStopAccelStock == 0) { - params.putFloatNonBlocking("StopAccel", stopAccel); - } - params.putFloatNonBlocking("StopAccelStock", stopAccel); - } - - if (currentStoppingDecelRateStock != stoppingDecelRate && stoppingDecelRate != 0) { - if (params.getFloat("StoppingDecelRate") == currentStoppingDecelRateStock || currentStoppingDecelRateStock == 0) { - params.putFloatNonBlocking("StoppingDecelRate", stoppingDecelRate); - } - params.putFloatNonBlocking("StoppingDecelRateStock", stoppingDecelRate); - } - - if (currentVEgoStartingStock != vEgoStarting && vEgoStarting != 0) { - if (params.getFloat("VEgoStarting") == currentVEgoStartingStock || currentVEgoStartingStock == 0) { - params.putFloatNonBlocking("VEgoStarting", vEgoStarting); - } - params.putFloatNonBlocking("VEgoStartingStock", vEgoStarting); - } - - if (currentVEgoStoppingStock != vEgoStopping && vEgoStopping != 0) { - if (params.getFloat("VEgoStopping") == currentVEgoStoppingStock || currentVEgoStoppingStock == 0) { - params.putFloatNonBlocking("VEgoStopping", vEgoStopping); - } - params.putFloatNonBlocking("VEgoStoppingStock", vEgoStopping); - } + syncStockParam("SteerDelay", "SteerDelayStock", currentDelayStock, steerActuatorDelay); + syncStockParam("SteerFriction", "SteerFrictionStock", currentFrictionStock, friction); + syncStockParam("SteerKP", "SteerKPStock", currentKPStock, steerKp); + syncStockParam("SteerLatAccel", "SteerLatAccelStock", currentLatAccelStock, latAccelFactor); + syncStockParam("LongitudinalActuatorDelay", "LongitudinalActuatorDelayStock", currentLongDelayStock, longitudinalActuatorDelay); + syncStockParam("StartAccel", "StartAccelStock", currentStartAccelStock, startAccel); + syncStockParam("SteerRatio", "SteerRatioStock", currentSteerRatioStock, steerRatio); + syncStockParam("StopAccel", "StopAccelStock", currentStopAccelStock, stopAccel); + syncStockParam("StoppingDecelRate", "StoppingDecelRateStock", currentStoppingDecelRateStock, stoppingDecelRate); + syncStockParam("VEgoStarting", "VEgoStartingStock", currentVEgoStartingStock, vEgoStarting); + syncStockParam("VEgoStopping", "VEgoStoppingStock", currentVEgoStoppingStock, vEgoStopping); if (Hardware::PC() && (carMake == "mock" || carFingerprint == "MOCK")) { applyDesktopVehicleFallback();