diff --git a/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc b/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc index 96e2277ee..fb9b336c1 100644 --- a/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc +++ b/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc @@ -824,8 +824,6 @@ BO_ 736 MANUAL_SPEED_LIMIT_ASSIST: 32 XXX SG_ MSLA_ENABLED : 38|1@1+ (1,0) [0|1] "" XXX SG_ MAX_SPEED : 55|8@0+ (1,0) [0|255] "" XXX SG_ MAX_SPEED_COPY : 144|8@1+ (1,0) [0|255] "" XXX - SG_ EV_REGEN_STATE : 104|8@1+ (1,0) [0|255] "" XXX - SG_ EV_REGEN_STATE_2 : 184|8@1+ (1,0) [0|255] "" XXX BO_ 837 ADRV_0x345: 8 ADRV SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index fc980abed..a042644d3 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 71c95e779..9c7d325d9 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 98a41155b..c4ec50596 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 1cd2eda49..443e6db8f 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 86cea3daa..995aaccd7 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 ebae42aa4..7c7441932 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 0ab251ee7..2506bd33d 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 0ab251ee7..2506bd33d 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 e3e17e93e..2d3722cb4 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 ebae42aa4..7c7441932 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 9257bb2be..8a16f3bb0 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-1ddbc24a-DEBUG"; +const uint8_t gitversion[19] = "DEV-5f51e670-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 8af996c60..955be3d77 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 ad6651685..ae8acaae4 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 e23edf21c..85a45661e 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 93c1b1096..acbffabf6 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 7ebea8cb9..cbefa3246 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 7a4eaff8d..cb568d1af 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 d2b68ff38..50148a87b 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 9f5314e45..c673d7097 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 17997bb1f..e0a2071bc 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 299be005a..67a515a94 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 e6a10d97b..ab1b81b65 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 474d3b317..09d662eeb 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 e5ca7460e..8f48664d7 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 3cb961a94..37dc42f38 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 bdfed0b5a..cbcf0ea45 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 f97bc0139..feeb30bf8 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 8ffd4afef..dd02978b3 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 75f3269e0..5e9a25650 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 a9dd35af1..1545a03ff 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 f264de1ae..ac3dbb0d1 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 60009d6e4..9d2779368 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-1ddbc24a-DEBUG \ No newline at end of file +DEV-5f51e670-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 9831a7feb..0dd6ef222 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -279,10 +279,7 @@ class LongControl: output_accel = clip(a_target, 0.0, starpilot_toggles.startAccel) else: output_accel = starpilot_toggles.startAccel - self.pid.reset() - self.pid.i = a_target - self.last_a_target = 0.0 - self.integrator_hold_frames = 0 + self.reset() else: # LongCtrlState.pid error = a_target - CS.aEgo diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 94f9510e5..54bac82a1 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_5457591731668657994) { - out_5457591731668657994[0] = delta_x[0] + nom_x[0]; - out_5457591731668657994[1] = delta_x[1] + nom_x[1]; - out_5457591731668657994[2] = delta_x[2] + nom_x[2]; - out_5457591731668657994[3] = delta_x[3] + nom_x[3]; - out_5457591731668657994[4] = delta_x[4] + nom_x[4]; - out_5457591731668657994[5] = delta_x[5] + nom_x[5]; - out_5457591731668657994[6] = delta_x[6] + nom_x[6]; - out_5457591731668657994[7] = delta_x[7] + nom_x[7]; - out_5457591731668657994[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_2687587277675065552) { + out_2687587277675065552[0] = delta_x[0] + nom_x[0]; + out_2687587277675065552[1] = delta_x[1] + nom_x[1]; + out_2687587277675065552[2] = delta_x[2] + nom_x[2]; + out_2687587277675065552[3] = delta_x[3] + nom_x[3]; + out_2687587277675065552[4] = delta_x[4] + nom_x[4]; + out_2687587277675065552[5] = delta_x[5] + nom_x[5]; + out_2687587277675065552[6] = delta_x[6] + nom_x[6]; + out_2687587277675065552[7] = delta_x[7] + nom_x[7]; + out_2687587277675065552[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_625468362349040739) { - out_625468362349040739[0] = -nom_x[0] + true_x[0]; - out_625468362349040739[1] = -nom_x[1] + true_x[1]; - out_625468362349040739[2] = -nom_x[2] + true_x[2]; - out_625468362349040739[3] = -nom_x[3] + true_x[3]; - out_625468362349040739[4] = -nom_x[4] + true_x[4]; - out_625468362349040739[5] = -nom_x[5] + true_x[5]; - out_625468362349040739[6] = -nom_x[6] + true_x[6]; - out_625468362349040739[7] = -nom_x[7] + true_x[7]; - out_625468362349040739[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6222700140799525089) { + out_6222700140799525089[0] = -nom_x[0] + true_x[0]; + out_6222700140799525089[1] = -nom_x[1] + true_x[1]; + out_6222700140799525089[2] = -nom_x[2] + true_x[2]; + out_6222700140799525089[3] = -nom_x[3] + true_x[3]; + out_6222700140799525089[4] = -nom_x[4] + true_x[4]; + out_6222700140799525089[5] = -nom_x[5] + true_x[5]; + out_6222700140799525089[6] = -nom_x[6] + true_x[6]; + out_6222700140799525089[7] = -nom_x[7] + true_x[7]; + out_6222700140799525089[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_308877862146519903) { - out_308877862146519903[0] = 1.0; - out_308877862146519903[1] = 0.0; - out_308877862146519903[2] = 0.0; - out_308877862146519903[3] = 0.0; - out_308877862146519903[4] = 0.0; - out_308877862146519903[5] = 0.0; - out_308877862146519903[6] = 0.0; - out_308877862146519903[7] = 0.0; - out_308877862146519903[8] = 0.0; - out_308877862146519903[9] = 0.0; - out_308877862146519903[10] = 1.0; - out_308877862146519903[11] = 0.0; - out_308877862146519903[12] = 0.0; - out_308877862146519903[13] = 0.0; - out_308877862146519903[14] = 0.0; - out_308877862146519903[15] = 0.0; - out_308877862146519903[16] = 0.0; - out_308877862146519903[17] = 0.0; - out_308877862146519903[18] = 0.0; - out_308877862146519903[19] = 0.0; - out_308877862146519903[20] = 1.0; - out_308877862146519903[21] = 0.0; - out_308877862146519903[22] = 0.0; - out_308877862146519903[23] = 0.0; - out_308877862146519903[24] = 0.0; - out_308877862146519903[25] = 0.0; - out_308877862146519903[26] = 0.0; - out_308877862146519903[27] = 0.0; - out_308877862146519903[28] = 0.0; - out_308877862146519903[29] = 0.0; - out_308877862146519903[30] = 1.0; - out_308877862146519903[31] = 0.0; - out_308877862146519903[32] = 0.0; - out_308877862146519903[33] = 0.0; - out_308877862146519903[34] = 0.0; - out_308877862146519903[35] = 0.0; - out_308877862146519903[36] = 0.0; - out_308877862146519903[37] = 0.0; - out_308877862146519903[38] = 0.0; - out_308877862146519903[39] = 0.0; - out_308877862146519903[40] = 1.0; - out_308877862146519903[41] = 0.0; - out_308877862146519903[42] = 0.0; - out_308877862146519903[43] = 0.0; - out_308877862146519903[44] = 0.0; - out_308877862146519903[45] = 0.0; - out_308877862146519903[46] = 0.0; - out_308877862146519903[47] = 0.0; - out_308877862146519903[48] = 0.0; - out_308877862146519903[49] = 0.0; - out_308877862146519903[50] = 1.0; - out_308877862146519903[51] = 0.0; - out_308877862146519903[52] = 0.0; - out_308877862146519903[53] = 0.0; - out_308877862146519903[54] = 0.0; - out_308877862146519903[55] = 0.0; - out_308877862146519903[56] = 0.0; - out_308877862146519903[57] = 0.0; - out_308877862146519903[58] = 0.0; - out_308877862146519903[59] = 0.0; - out_308877862146519903[60] = 1.0; - out_308877862146519903[61] = 0.0; - out_308877862146519903[62] = 0.0; - out_308877862146519903[63] = 0.0; - out_308877862146519903[64] = 0.0; - out_308877862146519903[65] = 0.0; - out_308877862146519903[66] = 0.0; - out_308877862146519903[67] = 0.0; - out_308877862146519903[68] = 0.0; - out_308877862146519903[69] = 0.0; - out_308877862146519903[70] = 1.0; - out_308877862146519903[71] = 0.0; - out_308877862146519903[72] = 0.0; - out_308877862146519903[73] = 0.0; - out_308877862146519903[74] = 0.0; - out_308877862146519903[75] = 0.0; - out_308877862146519903[76] = 0.0; - out_308877862146519903[77] = 0.0; - out_308877862146519903[78] = 0.0; - out_308877862146519903[79] = 0.0; - out_308877862146519903[80] = 1.0; +void H_mod_fun(double *state, double *out_5736311190330935470) { + out_5736311190330935470[0] = 1.0; + out_5736311190330935470[1] = 0.0; + out_5736311190330935470[2] = 0.0; + out_5736311190330935470[3] = 0.0; + out_5736311190330935470[4] = 0.0; + out_5736311190330935470[5] = 0.0; + out_5736311190330935470[6] = 0.0; + out_5736311190330935470[7] = 0.0; + out_5736311190330935470[8] = 0.0; + out_5736311190330935470[9] = 0.0; + out_5736311190330935470[10] = 1.0; + out_5736311190330935470[11] = 0.0; + out_5736311190330935470[12] = 0.0; + out_5736311190330935470[13] = 0.0; + out_5736311190330935470[14] = 0.0; + out_5736311190330935470[15] = 0.0; + out_5736311190330935470[16] = 0.0; + out_5736311190330935470[17] = 0.0; + out_5736311190330935470[18] = 0.0; + out_5736311190330935470[19] = 0.0; + out_5736311190330935470[20] = 1.0; + out_5736311190330935470[21] = 0.0; + out_5736311190330935470[22] = 0.0; + out_5736311190330935470[23] = 0.0; + out_5736311190330935470[24] = 0.0; + out_5736311190330935470[25] = 0.0; + out_5736311190330935470[26] = 0.0; + out_5736311190330935470[27] = 0.0; + out_5736311190330935470[28] = 0.0; + out_5736311190330935470[29] = 0.0; + out_5736311190330935470[30] = 1.0; + out_5736311190330935470[31] = 0.0; + out_5736311190330935470[32] = 0.0; + out_5736311190330935470[33] = 0.0; + out_5736311190330935470[34] = 0.0; + out_5736311190330935470[35] = 0.0; + out_5736311190330935470[36] = 0.0; + out_5736311190330935470[37] = 0.0; + out_5736311190330935470[38] = 0.0; + out_5736311190330935470[39] = 0.0; + out_5736311190330935470[40] = 1.0; + out_5736311190330935470[41] = 0.0; + out_5736311190330935470[42] = 0.0; + out_5736311190330935470[43] = 0.0; + out_5736311190330935470[44] = 0.0; + out_5736311190330935470[45] = 0.0; + out_5736311190330935470[46] = 0.0; + out_5736311190330935470[47] = 0.0; + out_5736311190330935470[48] = 0.0; + out_5736311190330935470[49] = 0.0; + out_5736311190330935470[50] = 1.0; + out_5736311190330935470[51] = 0.0; + out_5736311190330935470[52] = 0.0; + out_5736311190330935470[53] = 0.0; + out_5736311190330935470[54] = 0.0; + out_5736311190330935470[55] = 0.0; + out_5736311190330935470[56] = 0.0; + out_5736311190330935470[57] = 0.0; + out_5736311190330935470[58] = 0.0; + out_5736311190330935470[59] = 0.0; + out_5736311190330935470[60] = 1.0; + out_5736311190330935470[61] = 0.0; + out_5736311190330935470[62] = 0.0; + out_5736311190330935470[63] = 0.0; + out_5736311190330935470[64] = 0.0; + out_5736311190330935470[65] = 0.0; + out_5736311190330935470[66] = 0.0; + out_5736311190330935470[67] = 0.0; + out_5736311190330935470[68] = 0.0; + out_5736311190330935470[69] = 0.0; + out_5736311190330935470[70] = 1.0; + out_5736311190330935470[71] = 0.0; + out_5736311190330935470[72] = 0.0; + out_5736311190330935470[73] = 0.0; + out_5736311190330935470[74] = 0.0; + out_5736311190330935470[75] = 0.0; + out_5736311190330935470[76] = 0.0; + out_5736311190330935470[77] = 0.0; + out_5736311190330935470[78] = 0.0; + out_5736311190330935470[79] = 0.0; + out_5736311190330935470[80] = 1.0; } -void f_fun(double *state, double dt, double *out_9068006394827680313) { - out_9068006394827680313[0] = state[0]; - out_9068006394827680313[1] = state[1]; - out_9068006394827680313[2] = state[2]; - out_9068006394827680313[3] = state[3]; - out_9068006394827680313[4] = state[4]; - out_9068006394827680313[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_9068006394827680313[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_9068006394827680313[7] = state[7]; - out_9068006394827680313[8] = state[8]; +void f_fun(double *state, double dt, double *out_8059955085909907268) { + out_8059955085909907268[0] = state[0]; + out_8059955085909907268[1] = state[1]; + out_8059955085909907268[2] = state[2]; + out_8059955085909907268[3] = state[3]; + out_8059955085909907268[4] = state[4]; + out_8059955085909907268[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_8059955085909907268[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_8059955085909907268[7] = state[7]; + out_8059955085909907268[8] = state[8]; } -void F_fun(double *state, double dt, double *out_203771605761279388) { - out_203771605761279388[0] = 1; - out_203771605761279388[1] = 0; - out_203771605761279388[2] = 0; - out_203771605761279388[3] = 0; - out_203771605761279388[4] = 0; - out_203771605761279388[5] = 0; - out_203771605761279388[6] = 0; - out_203771605761279388[7] = 0; - out_203771605761279388[8] = 0; - out_203771605761279388[9] = 0; - out_203771605761279388[10] = 1; - out_203771605761279388[11] = 0; - out_203771605761279388[12] = 0; - out_203771605761279388[13] = 0; - out_203771605761279388[14] = 0; - out_203771605761279388[15] = 0; - out_203771605761279388[16] = 0; - out_203771605761279388[17] = 0; - out_203771605761279388[18] = 0; - out_203771605761279388[19] = 0; - out_203771605761279388[20] = 1; - out_203771605761279388[21] = 0; - out_203771605761279388[22] = 0; - out_203771605761279388[23] = 0; - out_203771605761279388[24] = 0; - out_203771605761279388[25] = 0; - out_203771605761279388[26] = 0; - out_203771605761279388[27] = 0; - out_203771605761279388[28] = 0; - out_203771605761279388[29] = 0; - out_203771605761279388[30] = 1; - out_203771605761279388[31] = 0; - out_203771605761279388[32] = 0; - out_203771605761279388[33] = 0; - out_203771605761279388[34] = 0; - out_203771605761279388[35] = 0; - out_203771605761279388[36] = 0; - out_203771605761279388[37] = 0; - out_203771605761279388[38] = 0; - out_203771605761279388[39] = 0; - out_203771605761279388[40] = 1; - out_203771605761279388[41] = 0; - out_203771605761279388[42] = 0; - out_203771605761279388[43] = 0; - out_203771605761279388[44] = 0; - out_203771605761279388[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_203771605761279388[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_203771605761279388[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_203771605761279388[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_203771605761279388[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_203771605761279388[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_203771605761279388[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_203771605761279388[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_203771605761279388[53] = -9.8100000000000005*dt; - out_203771605761279388[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_203771605761279388[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_203771605761279388[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_203771605761279388[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_203771605761279388[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_203771605761279388[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_203771605761279388[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_203771605761279388[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_203771605761279388[62] = 0; - out_203771605761279388[63] = 0; - out_203771605761279388[64] = 0; - out_203771605761279388[65] = 0; - out_203771605761279388[66] = 0; - out_203771605761279388[67] = 0; - out_203771605761279388[68] = 0; - out_203771605761279388[69] = 0; - out_203771605761279388[70] = 1; - out_203771605761279388[71] = 0; - out_203771605761279388[72] = 0; - out_203771605761279388[73] = 0; - out_203771605761279388[74] = 0; - out_203771605761279388[75] = 0; - out_203771605761279388[76] = 0; - out_203771605761279388[77] = 0; - out_203771605761279388[78] = 0; - out_203771605761279388[79] = 0; - out_203771605761279388[80] = 1; +void F_fun(double *state, double dt, double *out_535217138807217635) { + out_535217138807217635[0] = 1; + out_535217138807217635[1] = 0; + out_535217138807217635[2] = 0; + out_535217138807217635[3] = 0; + out_535217138807217635[4] = 0; + out_535217138807217635[5] = 0; + out_535217138807217635[6] = 0; + out_535217138807217635[7] = 0; + out_535217138807217635[8] = 0; + out_535217138807217635[9] = 0; + out_535217138807217635[10] = 1; + out_535217138807217635[11] = 0; + out_535217138807217635[12] = 0; + out_535217138807217635[13] = 0; + out_535217138807217635[14] = 0; + out_535217138807217635[15] = 0; + out_535217138807217635[16] = 0; + out_535217138807217635[17] = 0; + out_535217138807217635[18] = 0; + out_535217138807217635[19] = 0; + out_535217138807217635[20] = 1; + out_535217138807217635[21] = 0; + out_535217138807217635[22] = 0; + out_535217138807217635[23] = 0; + out_535217138807217635[24] = 0; + out_535217138807217635[25] = 0; + out_535217138807217635[26] = 0; + out_535217138807217635[27] = 0; + out_535217138807217635[28] = 0; + out_535217138807217635[29] = 0; + out_535217138807217635[30] = 1; + out_535217138807217635[31] = 0; + out_535217138807217635[32] = 0; + out_535217138807217635[33] = 0; + out_535217138807217635[34] = 0; + out_535217138807217635[35] = 0; + out_535217138807217635[36] = 0; + out_535217138807217635[37] = 0; + out_535217138807217635[38] = 0; + out_535217138807217635[39] = 0; + out_535217138807217635[40] = 1; + out_535217138807217635[41] = 0; + out_535217138807217635[42] = 0; + out_535217138807217635[43] = 0; + out_535217138807217635[44] = 0; + out_535217138807217635[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_535217138807217635[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_535217138807217635[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_535217138807217635[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_535217138807217635[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_535217138807217635[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_535217138807217635[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_535217138807217635[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_535217138807217635[53] = -9.8100000000000005*dt; + out_535217138807217635[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_535217138807217635[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_535217138807217635[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_535217138807217635[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_535217138807217635[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_535217138807217635[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_535217138807217635[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_535217138807217635[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_535217138807217635[62] = 0; + out_535217138807217635[63] = 0; + out_535217138807217635[64] = 0; + out_535217138807217635[65] = 0; + out_535217138807217635[66] = 0; + out_535217138807217635[67] = 0; + out_535217138807217635[68] = 0; + out_535217138807217635[69] = 0; + out_535217138807217635[70] = 1; + out_535217138807217635[71] = 0; + out_535217138807217635[72] = 0; + out_535217138807217635[73] = 0; + out_535217138807217635[74] = 0; + out_535217138807217635[75] = 0; + out_535217138807217635[76] = 0; + out_535217138807217635[77] = 0; + out_535217138807217635[78] = 0; + out_535217138807217635[79] = 0; + out_535217138807217635[80] = 1; } -void h_25(double *state, double *unused, double *out_5131794037626248505) { - out_5131794037626248505[0] = state[6]; +void h_25(double *state, double *unused, double *out_7527307434081215851) { + out_7527307434081215851[0] = state[6]; } -void H_25(double *state, double *unused, double *out_640243059881962306) { - out_640243059881962306[0] = 0; - out_640243059881962306[1] = 0; - out_640243059881962306[2] = 0; - out_640243059881962306[3] = 0; - out_640243059881962306[4] = 0; - out_640243059881962306[5] = 0; - out_640243059881962306[6] = 1; - out_640243059881962306[7] = 0; - out_640243059881962306[8] = 0; +void H_25(double *state, double *unused, double *out_9187452233389124469) { + out_9187452233389124469[0] = 0; + out_9187452233389124469[1] = 0; + out_9187452233389124469[2] = 0; + out_9187452233389124469[3] = 0; + out_9187452233389124469[4] = 0; + out_9187452233389124469[5] = 0; + out_9187452233389124469[6] = 1; + out_9187452233389124469[7] = 0; + out_9187452233389124469[8] = 0; } -void h_24(double *state, double *unused, double *out_2029903564179396636) { - out_2029903564179396636[0] = state[4]; - out_2029903564179396636[1] = state[5]; +void h_24(double *state, double *unused, double *out_7476062049775574580) { + out_7476062049775574580[0] = state[4]; + out_7476062049775574580[1] = state[5]; } -void H_24(double *state, double *unused, double *out_2865950843860830868) { - out_2865950843860830868[0] = 0; - out_2865950843860830868[1] = 0; - out_2865950843860830868[2] = 0; - out_2865950843860830868[3] = 0; - out_2865950843860830868[4] = 1; - out_2865950843860830868[5] = 0; - out_2865950843860830868[6] = 0; - out_2865950843860830868[7] = 0; - out_2865950843860830868[8] = 0; - out_2865950843860830868[9] = 0; - out_2865950843860830868[10] = 0; - out_2865950843860830868[11] = 0; - out_2865950843860830868[12] = 0; - out_2865950843860830868[13] = 0; - out_2865950843860830868[14] = 1; - out_2865950843860830868[15] = 0; - out_2865950843860830868[16] = 0; - out_2865950843860830868[17] = 0; +void H_24(double *state, double *unused, double *out_5629526634586801333) { + out_5629526634586801333[0] = 0; + out_5629526634586801333[1] = 0; + out_5629526634586801333[2] = 0; + out_5629526634586801333[3] = 0; + out_5629526634586801333[4] = 1; + out_5629526634586801333[5] = 0; + out_5629526634586801333[6] = 0; + out_5629526634586801333[7] = 0; + out_5629526634586801333[8] = 0; + out_5629526634586801333[9] = 0; + out_5629526634586801333[10] = 0; + out_5629526634586801333[11] = 0; + out_5629526634586801333[12] = 0; + out_5629526634586801333[13] = 0; + out_5629526634586801333[14] = 1; + out_5629526634586801333[15] = 0; + out_5629526634586801333[16] = 0; + out_5629526634586801333[17] = 0; } -void h_30(double *state, double *unused, double *out_8651236171946894111) { - out_8651236171946894111[0] = state[4]; +void h_30(double *state, double *unused, double *out_1371860238240555722) { + out_1371860238240555722[0] = state[4]; } -void H_30(double *state, double *unused, double *out_510904112738722236) { - out_510904112738722236[0] = 0; - out_510904112738722236[1] = 0; - out_510904112738722236[2] = 0; - out_510904112738722236[3] = 0; - out_510904112738722236[4] = 1; - out_510904112738722236[5] = 0; - out_510904112738722236[6] = 0; - out_510904112738722236[7] = 0; - out_510904112738722236[8] = 0; +void H_30(double *state, double *unused, double *out_2270761891897507714) { + out_2270761891897507714[0] = 0; + out_2270761891897507714[1] = 0; + out_2270761891897507714[2] = 0; + out_2270761891897507714[3] = 0; + out_2270761891897507714[4] = 1; + out_2270761891897507714[5] = 0; + out_2270761891897507714[6] = 0; + out_2270761891897507714[7] = 0; + out_2270761891897507714[8] = 0; } -void h_26(double *state, double *unused, double *out_2252194654533044965) { - out_2252194654533044965[0] = state[7]; +void h_26(double *state, double *unused, double *out_6063283196188145091) { + out_6063283196188145091[0] = state[7]; } -void H_26(double *state, double *unused, double *out_3101260258992093918) { - out_3101260258992093918[0] = 0; - out_3101260258992093918[1] = 0; - out_3101260258992093918[2] = 0; - out_3101260258992093918[3] = 0; - out_3101260258992093918[4] = 0; - out_3101260258992093918[5] = 0; - out_3101260258992093918[6] = 0; - out_3101260258992093918[7] = 1; - out_3101260258992093918[8] = 0; +void H_26(double *state, double *unused, double *out_5517788521446370923) { + out_5517788521446370923[0] = 0; + out_5517788521446370923[1] = 0; + out_5517788521446370923[2] = 0; + out_5517788521446370923[3] = 0; + out_5517788521446370923[4] = 0; + out_5517788521446370923[5] = 0; + out_5517788521446370923[6] = 0; + out_5517788521446370923[7] = 1; + out_5517788521446370923[8] = 0; } -void h_27(double *state, double *unused, double *out_3283700363150443165) { - out_3283700363150443165[0] = state[3]; +void h_27(double *state, double *unused, double *out_8470817286982878286) { + out_8470817286982878286[0] = state[3]; } -void H_27(double *state, double *unused, double *out_1663859199061702675) { - out_1663859199061702675[0] = 0; - out_1663859199061702675[1] = 0; - out_1663859199061702675[2] = 0; - out_1663859199061702675[3] = 1; - out_1663859199061702675[4] = 0; - out_1663859199061702675[5] = 0; - out_1663859199061702675[6] = 0; - out_1663859199061702675[7] = 0; - out_1663859199061702675[8] = 0; +void H_27(double *state, double *unused, double *out_4445525203697932625) { + out_4445525203697932625[0] = 0; + out_4445525203697932625[1] = 0; + out_4445525203697932625[2] = 0; + out_4445525203697932625[3] = 1; + out_4445525203697932625[4] = 0; + out_4445525203697932625[5] = 0; + out_4445525203697932625[6] = 0; + out_4445525203697932625[7] = 0; + out_4445525203697932625[8] = 0; } -void h_29(double *state, double *unused, double *out_3008506300865937276) { - out_3008506300865937276[0] = state[1]; +void h_29(double *state, double *unused, double *out_7424449226025441418) { + out_7424449226025441418[0] = state[1]; } -void H_29(double *state, double *unused, double *out_1021135457053114420) { - out_1021135457053114420[0] = 0; - out_1021135457053114420[1] = 1; - out_1021135457053114420[2] = 0; - out_1021135457053114420[3] = 0; - out_1021135457053114420[4] = 0; - out_1021135457053114420[5] = 0; - out_1021135457053114420[6] = 0; - out_1021135457053114420[7] = 0; - out_1021135457053114420[8] = 0; +void H_29(double *state, double *unused, double *out_1760530547583115530) { + out_1760530547583115530[0] = 0; + out_1760530547583115530[1] = 1; + out_1760530547583115530[2] = 0; + out_1760530547583115530[3] = 0; + out_1760530547583115530[4] = 0; + out_1760530547583115530[5] = 0; + out_1760530547583115530[6] = 0; + out_1760530547583115530[7] = 0; + out_1760530547583115530[8] = 0; } -void h_28(double *state, double *unused, double *out_5663871735784661514) { - out_5663871735784661514[0] = state[0]; +void h_28(double *state, double *unused, double *out_2540783352297972517) { + out_2540783352297972517[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2984765728618440671) { - out_2984765728618440671[0] = 1; - out_2984765728618440671[1] = 0; - out_2984765728618440671[2] = 0; - out_2984765728618440671[3] = 0; - out_2984765728618440671[4] = 0; - out_2984765728618440671[5] = 0; - out_2984765728618440671[6] = 0; - out_2984765728618440671[7] = 0; - out_2984765728618440671[8] = 0; +void H_28(double *state, double *unused, double *out_6842929564652646104) { + out_6842929564652646104[0] = 1; + out_6842929564652646104[1] = 0; + out_6842929564652646104[2] = 0; + out_6842929564652646104[3] = 0; + out_6842929564652646104[4] = 0; + out_6842929564652646104[5] = 0; + out_6842929564652646104[6] = 0; + out_6842929564652646104[7] = 0; + out_6842929564652646104[8] = 0; } -void h_31(double *state, double *unused, double *out_7524465776080943032) { - out_7524465776080943032[0] = state[8]; +void h_31(double *state, double *unused, double *out_7802501496365721740) { + out_7802501496365721740[0] = state[8]; } -void H_31(double *state, double *unused, double *out_670889021758922734) { - out_670889021758922734[0] = 0; - out_670889021758922734[1] = 0; - out_670889021758922734[2] = 0; - out_670889021758922734[3] = 0; - out_670889021758922734[4] = 0; - out_670889021758922734[5] = 0; - out_670889021758922734[6] = 0; - out_670889021758922734[7] = 0; - out_670889021758922734[8] = 1; +void H_31(double *state, double *unused, double *out_9156806271512164041) { + out_9156806271512164041[0] = 0; + out_9156806271512164041[1] = 0; + out_9156806271512164041[2] = 0; + out_9156806271512164041[3] = 0; + out_9156806271512164041[4] = 0; + out_9156806271512164041[5] = 0; + out_9156806271512164041[6] = 0; + out_9156806271512164041[7] = 0; + out_9156806271512164041[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_5457591731668657994) { - err_fun(nom_x, delta_x, out_5457591731668657994); +void car_err_fun(double *nom_x, double *delta_x, double *out_2687587277675065552) { + err_fun(nom_x, delta_x, out_2687587277675065552); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_625468362349040739) { - inv_err_fun(nom_x, true_x, out_625468362349040739); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_6222700140799525089) { + inv_err_fun(nom_x, true_x, out_6222700140799525089); } -void car_H_mod_fun(double *state, double *out_308877862146519903) { - H_mod_fun(state, out_308877862146519903); +void car_H_mod_fun(double *state, double *out_5736311190330935470) { + H_mod_fun(state, out_5736311190330935470); } -void car_f_fun(double *state, double dt, double *out_9068006394827680313) { - f_fun(state, dt, out_9068006394827680313); +void car_f_fun(double *state, double dt, double *out_8059955085909907268) { + f_fun(state, dt, out_8059955085909907268); } -void car_F_fun(double *state, double dt, double *out_203771605761279388) { - F_fun(state, dt, out_203771605761279388); +void car_F_fun(double *state, double dt, double *out_535217138807217635) { + F_fun(state, dt, out_535217138807217635); } -void car_h_25(double *state, double *unused, double *out_5131794037626248505) { - h_25(state, unused, out_5131794037626248505); +void car_h_25(double *state, double *unused, double *out_7527307434081215851) { + h_25(state, unused, out_7527307434081215851); } -void car_H_25(double *state, double *unused, double *out_640243059881962306) { - H_25(state, unused, out_640243059881962306); +void car_H_25(double *state, double *unused, double *out_9187452233389124469) { + H_25(state, unused, out_9187452233389124469); } -void car_h_24(double *state, double *unused, double *out_2029903564179396636) { - h_24(state, unused, out_2029903564179396636); +void car_h_24(double *state, double *unused, double *out_7476062049775574580) { + h_24(state, unused, out_7476062049775574580); } -void car_H_24(double *state, double *unused, double *out_2865950843860830868) { - H_24(state, unused, out_2865950843860830868); +void car_H_24(double *state, double *unused, double *out_5629526634586801333) { + H_24(state, unused, out_5629526634586801333); } -void car_h_30(double *state, double *unused, double *out_8651236171946894111) { - h_30(state, unused, out_8651236171946894111); +void car_h_30(double *state, double *unused, double *out_1371860238240555722) { + h_30(state, unused, out_1371860238240555722); } -void car_H_30(double *state, double *unused, double *out_510904112738722236) { - H_30(state, unused, out_510904112738722236); +void car_H_30(double *state, double *unused, double *out_2270761891897507714) { + H_30(state, unused, out_2270761891897507714); } -void car_h_26(double *state, double *unused, double *out_2252194654533044965) { - h_26(state, unused, out_2252194654533044965); +void car_h_26(double *state, double *unused, double *out_6063283196188145091) { + h_26(state, unused, out_6063283196188145091); } -void car_H_26(double *state, double *unused, double *out_3101260258992093918) { - H_26(state, unused, out_3101260258992093918); +void car_H_26(double *state, double *unused, double *out_5517788521446370923) { + H_26(state, unused, out_5517788521446370923); } -void car_h_27(double *state, double *unused, double *out_3283700363150443165) { - h_27(state, unused, out_3283700363150443165); +void car_h_27(double *state, double *unused, double *out_8470817286982878286) { + h_27(state, unused, out_8470817286982878286); } -void car_H_27(double *state, double *unused, double *out_1663859199061702675) { - H_27(state, unused, out_1663859199061702675); +void car_H_27(double *state, double *unused, double *out_4445525203697932625) { + H_27(state, unused, out_4445525203697932625); } -void car_h_29(double *state, double *unused, double *out_3008506300865937276) { - h_29(state, unused, out_3008506300865937276); +void car_h_29(double *state, double *unused, double *out_7424449226025441418) { + h_29(state, unused, out_7424449226025441418); } -void car_H_29(double *state, double *unused, double *out_1021135457053114420) { - H_29(state, unused, out_1021135457053114420); +void car_H_29(double *state, double *unused, double *out_1760530547583115530) { + H_29(state, unused, out_1760530547583115530); } -void car_h_28(double *state, double *unused, double *out_5663871735784661514) { - h_28(state, unused, out_5663871735784661514); +void car_h_28(double *state, double *unused, double *out_2540783352297972517) { + h_28(state, unused, out_2540783352297972517); } -void car_H_28(double *state, double *unused, double *out_2984765728618440671) { - H_28(state, unused, out_2984765728618440671); +void car_H_28(double *state, double *unused, double *out_6842929564652646104) { + H_28(state, unused, out_6842929564652646104); } -void car_h_31(double *state, double *unused, double *out_7524465776080943032) { - h_31(state, unused, out_7524465776080943032); +void car_h_31(double *state, double *unused, double *out_7802501496365721740) { + h_31(state, unused, out_7802501496365721740); } -void car_H_31(double *state, double *unused, double *out_670889021758922734) { - H_31(state, unused, out_670889021758922734); +void car_H_31(double *state, double *unused, double *out_9156806271512164041) { + H_31(state, unused, out_9156806271512164041); } 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 dac997474..01b4b1286 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_5457591731668657994); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_625468362349040739); -void car_H_mod_fun(double *state, double *out_308877862146519903); -void car_f_fun(double *state, double dt, double *out_9068006394827680313); -void car_F_fun(double *state, double dt, double *out_203771605761279388); -void car_h_25(double *state, double *unused, double *out_5131794037626248505); -void car_H_25(double *state, double *unused, double *out_640243059881962306); -void car_h_24(double *state, double *unused, double *out_2029903564179396636); -void car_H_24(double *state, double *unused, double *out_2865950843860830868); -void car_h_30(double *state, double *unused, double *out_8651236171946894111); -void car_H_30(double *state, double *unused, double *out_510904112738722236); -void car_h_26(double *state, double *unused, double *out_2252194654533044965); -void car_H_26(double *state, double *unused, double *out_3101260258992093918); -void car_h_27(double *state, double *unused, double *out_3283700363150443165); -void car_H_27(double *state, double *unused, double *out_1663859199061702675); -void car_h_29(double *state, double *unused, double *out_3008506300865937276); -void car_H_29(double *state, double *unused, double *out_1021135457053114420); -void car_h_28(double *state, double *unused, double *out_5663871735784661514); -void car_H_28(double *state, double *unused, double *out_2984765728618440671); -void car_h_31(double *state, double *unused, double *out_7524465776080943032); -void car_H_31(double *state, double *unused, double *out_670889021758922734); +void car_err_fun(double *nom_x, double *delta_x, double *out_2687587277675065552); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_6222700140799525089); +void car_H_mod_fun(double *state, double *out_5736311190330935470); +void car_f_fun(double *state, double dt, double *out_8059955085909907268); +void car_F_fun(double *state, double dt, double *out_535217138807217635); +void car_h_25(double *state, double *unused, double *out_7527307434081215851); +void car_H_25(double *state, double *unused, double *out_9187452233389124469); +void car_h_24(double *state, double *unused, double *out_7476062049775574580); +void car_H_24(double *state, double *unused, double *out_5629526634586801333); +void car_h_30(double *state, double *unused, double *out_1371860238240555722); +void car_H_30(double *state, double *unused, double *out_2270761891897507714); +void car_h_26(double *state, double *unused, double *out_6063283196188145091); +void car_H_26(double *state, double *unused, double *out_5517788521446370923); +void car_h_27(double *state, double *unused, double *out_8470817286982878286); +void car_H_27(double *state, double *unused, double *out_4445525203697932625); +void car_h_29(double *state, double *unused, double *out_7424449226025441418); +void car_H_29(double *state, double *unused, double *out_1760530547583115530); +void car_h_28(double *state, double *unused, double *out_2540783352297972517); +void car_H_28(double *state, double *unused, double *out_6842929564652646104); +void car_h_31(double *state, double *unused, double *out_7802501496365721740); +void car_H_31(double *state, double *unused, double *out_9156806271512164041); 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 2818cff3a..b1ee67f33 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_3212432162194349052) { - out_3212432162194349052[0] = delta_x[0] + nom_x[0]; - out_3212432162194349052[1] = delta_x[1] + nom_x[1]; - out_3212432162194349052[2] = delta_x[2] + nom_x[2]; - out_3212432162194349052[3] = delta_x[3] + nom_x[3]; - out_3212432162194349052[4] = delta_x[4] + nom_x[4]; - out_3212432162194349052[5] = delta_x[5] + nom_x[5]; - out_3212432162194349052[6] = delta_x[6] + nom_x[6]; - out_3212432162194349052[7] = delta_x[7] + nom_x[7]; - out_3212432162194349052[8] = delta_x[8] + nom_x[8]; - out_3212432162194349052[9] = delta_x[9] + nom_x[9]; - out_3212432162194349052[10] = delta_x[10] + nom_x[10]; - out_3212432162194349052[11] = delta_x[11] + nom_x[11]; - out_3212432162194349052[12] = delta_x[12] + nom_x[12]; - out_3212432162194349052[13] = delta_x[13] + nom_x[13]; - out_3212432162194349052[14] = delta_x[14] + nom_x[14]; - out_3212432162194349052[15] = delta_x[15] + nom_x[15]; - out_3212432162194349052[16] = delta_x[16] + nom_x[16]; - out_3212432162194349052[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7389011504566069386) { + out_7389011504566069386[0] = delta_x[0] + nom_x[0]; + out_7389011504566069386[1] = delta_x[1] + nom_x[1]; + out_7389011504566069386[2] = delta_x[2] + nom_x[2]; + out_7389011504566069386[3] = delta_x[3] + nom_x[3]; + out_7389011504566069386[4] = delta_x[4] + nom_x[4]; + out_7389011504566069386[5] = delta_x[5] + nom_x[5]; + out_7389011504566069386[6] = delta_x[6] + nom_x[6]; + out_7389011504566069386[7] = delta_x[7] + nom_x[7]; + out_7389011504566069386[8] = delta_x[8] + nom_x[8]; + out_7389011504566069386[9] = delta_x[9] + nom_x[9]; + out_7389011504566069386[10] = delta_x[10] + nom_x[10]; + out_7389011504566069386[11] = delta_x[11] + nom_x[11]; + out_7389011504566069386[12] = delta_x[12] + nom_x[12]; + out_7389011504566069386[13] = delta_x[13] + nom_x[13]; + out_7389011504566069386[14] = delta_x[14] + nom_x[14]; + out_7389011504566069386[15] = delta_x[15] + nom_x[15]; + out_7389011504566069386[16] = delta_x[16] + nom_x[16]; + out_7389011504566069386[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8973271523308122061) { - out_8973271523308122061[0] = -nom_x[0] + true_x[0]; - out_8973271523308122061[1] = -nom_x[1] + true_x[1]; - out_8973271523308122061[2] = -nom_x[2] + true_x[2]; - out_8973271523308122061[3] = -nom_x[3] + true_x[3]; - out_8973271523308122061[4] = -nom_x[4] + true_x[4]; - out_8973271523308122061[5] = -nom_x[5] + true_x[5]; - out_8973271523308122061[6] = -nom_x[6] + true_x[6]; - out_8973271523308122061[7] = -nom_x[7] + true_x[7]; - out_8973271523308122061[8] = -nom_x[8] + true_x[8]; - out_8973271523308122061[9] = -nom_x[9] + true_x[9]; - out_8973271523308122061[10] = -nom_x[10] + true_x[10]; - out_8973271523308122061[11] = -nom_x[11] + true_x[11]; - out_8973271523308122061[12] = -nom_x[12] + true_x[12]; - out_8973271523308122061[13] = -nom_x[13] + true_x[13]; - out_8973271523308122061[14] = -nom_x[14] + true_x[14]; - out_8973271523308122061[15] = -nom_x[15] + true_x[15]; - out_8973271523308122061[16] = -nom_x[16] + true_x[16]; - out_8973271523308122061[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6795243368226050386) { + out_6795243368226050386[0] = -nom_x[0] + true_x[0]; + out_6795243368226050386[1] = -nom_x[1] + true_x[1]; + out_6795243368226050386[2] = -nom_x[2] + true_x[2]; + out_6795243368226050386[3] = -nom_x[3] + true_x[3]; + out_6795243368226050386[4] = -nom_x[4] + true_x[4]; + out_6795243368226050386[5] = -nom_x[5] + true_x[5]; + out_6795243368226050386[6] = -nom_x[6] + true_x[6]; + out_6795243368226050386[7] = -nom_x[7] + true_x[7]; + out_6795243368226050386[8] = -nom_x[8] + true_x[8]; + out_6795243368226050386[9] = -nom_x[9] + true_x[9]; + out_6795243368226050386[10] = -nom_x[10] + true_x[10]; + out_6795243368226050386[11] = -nom_x[11] + true_x[11]; + out_6795243368226050386[12] = -nom_x[12] + true_x[12]; + out_6795243368226050386[13] = -nom_x[13] + true_x[13]; + out_6795243368226050386[14] = -nom_x[14] + true_x[14]; + out_6795243368226050386[15] = -nom_x[15] + true_x[15]; + out_6795243368226050386[16] = -nom_x[16] + true_x[16]; + out_6795243368226050386[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_7731698092980355432) { - out_7731698092980355432[0] = 1.0; - out_7731698092980355432[1] = 0.0; - out_7731698092980355432[2] = 0.0; - out_7731698092980355432[3] = 0.0; - out_7731698092980355432[4] = 0.0; - out_7731698092980355432[5] = 0.0; - out_7731698092980355432[6] = 0.0; - out_7731698092980355432[7] = 0.0; - out_7731698092980355432[8] = 0.0; - out_7731698092980355432[9] = 0.0; - out_7731698092980355432[10] = 0.0; - out_7731698092980355432[11] = 0.0; - out_7731698092980355432[12] = 0.0; - out_7731698092980355432[13] = 0.0; - out_7731698092980355432[14] = 0.0; - out_7731698092980355432[15] = 0.0; - out_7731698092980355432[16] = 0.0; - out_7731698092980355432[17] = 0.0; - out_7731698092980355432[18] = 0.0; - out_7731698092980355432[19] = 1.0; - out_7731698092980355432[20] = 0.0; - out_7731698092980355432[21] = 0.0; - out_7731698092980355432[22] = 0.0; - out_7731698092980355432[23] = 0.0; - out_7731698092980355432[24] = 0.0; - out_7731698092980355432[25] = 0.0; - out_7731698092980355432[26] = 0.0; - out_7731698092980355432[27] = 0.0; - out_7731698092980355432[28] = 0.0; - out_7731698092980355432[29] = 0.0; - out_7731698092980355432[30] = 0.0; - out_7731698092980355432[31] = 0.0; - out_7731698092980355432[32] = 0.0; - out_7731698092980355432[33] = 0.0; - out_7731698092980355432[34] = 0.0; - out_7731698092980355432[35] = 0.0; - out_7731698092980355432[36] = 0.0; - out_7731698092980355432[37] = 0.0; - out_7731698092980355432[38] = 1.0; - out_7731698092980355432[39] = 0.0; - out_7731698092980355432[40] = 0.0; - out_7731698092980355432[41] = 0.0; - out_7731698092980355432[42] = 0.0; - out_7731698092980355432[43] = 0.0; - out_7731698092980355432[44] = 0.0; - out_7731698092980355432[45] = 0.0; - out_7731698092980355432[46] = 0.0; - out_7731698092980355432[47] = 0.0; - out_7731698092980355432[48] = 0.0; - out_7731698092980355432[49] = 0.0; - out_7731698092980355432[50] = 0.0; - out_7731698092980355432[51] = 0.0; - out_7731698092980355432[52] = 0.0; - out_7731698092980355432[53] = 0.0; - out_7731698092980355432[54] = 0.0; - out_7731698092980355432[55] = 0.0; - out_7731698092980355432[56] = 0.0; - out_7731698092980355432[57] = 1.0; - out_7731698092980355432[58] = 0.0; - out_7731698092980355432[59] = 0.0; - out_7731698092980355432[60] = 0.0; - out_7731698092980355432[61] = 0.0; - out_7731698092980355432[62] = 0.0; - out_7731698092980355432[63] = 0.0; - out_7731698092980355432[64] = 0.0; - out_7731698092980355432[65] = 0.0; - out_7731698092980355432[66] = 0.0; - out_7731698092980355432[67] = 0.0; - out_7731698092980355432[68] = 0.0; - out_7731698092980355432[69] = 0.0; - out_7731698092980355432[70] = 0.0; - out_7731698092980355432[71] = 0.0; - out_7731698092980355432[72] = 0.0; - out_7731698092980355432[73] = 0.0; - out_7731698092980355432[74] = 0.0; - out_7731698092980355432[75] = 0.0; - out_7731698092980355432[76] = 1.0; - out_7731698092980355432[77] = 0.0; - out_7731698092980355432[78] = 0.0; - out_7731698092980355432[79] = 0.0; - out_7731698092980355432[80] = 0.0; - out_7731698092980355432[81] = 0.0; - out_7731698092980355432[82] = 0.0; - out_7731698092980355432[83] = 0.0; - out_7731698092980355432[84] = 0.0; - out_7731698092980355432[85] = 0.0; - out_7731698092980355432[86] = 0.0; - out_7731698092980355432[87] = 0.0; - out_7731698092980355432[88] = 0.0; - out_7731698092980355432[89] = 0.0; - out_7731698092980355432[90] = 0.0; - out_7731698092980355432[91] = 0.0; - out_7731698092980355432[92] = 0.0; - out_7731698092980355432[93] = 0.0; - out_7731698092980355432[94] = 0.0; - out_7731698092980355432[95] = 1.0; - out_7731698092980355432[96] = 0.0; - out_7731698092980355432[97] = 0.0; - out_7731698092980355432[98] = 0.0; - out_7731698092980355432[99] = 0.0; - out_7731698092980355432[100] = 0.0; - out_7731698092980355432[101] = 0.0; - out_7731698092980355432[102] = 0.0; - out_7731698092980355432[103] = 0.0; - out_7731698092980355432[104] = 0.0; - out_7731698092980355432[105] = 0.0; - out_7731698092980355432[106] = 0.0; - out_7731698092980355432[107] = 0.0; - out_7731698092980355432[108] = 0.0; - out_7731698092980355432[109] = 0.0; - out_7731698092980355432[110] = 0.0; - out_7731698092980355432[111] = 0.0; - out_7731698092980355432[112] = 0.0; - out_7731698092980355432[113] = 0.0; - out_7731698092980355432[114] = 1.0; - out_7731698092980355432[115] = 0.0; - out_7731698092980355432[116] = 0.0; - out_7731698092980355432[117] = 0.0; - out_7731698092980355432[118] = 0.0; - out_7731698092980355432[119] = 0.0; - out_7731698092980355432[120] = 0.0; - out_7731698092980355432[121] = 0.0; - out_7731698092980355432[122] = 0.0; - out_7731698092980355432[123] = 0.0; - out_7731698092980355432[124] = 0.0; - out_7731698092980355432[125] = 0.0; - out_7731698092980355432[126] = 0.0; - out_7731698092980355432[127] = 0.0; - out_7731698092980355432[128] = 0.0; - out_7731698092980355432[129] = 0.0; - out_7731698092980355432[130] = 0.0; - out_7731698092980355432[131] = 0.0; - out_7731698092980355432[132] = 0.0; - out_7731698092980355432[133] = 1.0; - out_7731698092980355432[134] = 0.0; - out_7731698092980355432[135] = 0.0; - out_7731698092980355432[136] = 0.0; - out_7731698092980355432[137] = 0.0; - out_7731698092980355432[138] = 0.0; - out_7731698092980355432[139] = 0.0; - out_7731698092980355432[140] = 0.0; - out_7731698092980355432[141] = 0.0; - out_7731698092980355432[142] = 0.0; - out_7731698092980355432[143] = 0.0; - out_7731698092980355432[144] = 0.0; - out_7731698092980355432[145] = 0.0; - out_7731698092980355432[146] = 0.0; - out_7731698092980355432[147] = 0.0; - out_7731698092980355432[148] = 0.0; - out_7731698092980355432[149] = 0.0; - out_7731698092980355432[150] = 0.0; - out_7731698092980355432[151] = 0.0; - out_7731698092980355432[152] = 1.0; - out_7731698092980355432[153] = 0.0; - out_7731698092980355432[154] = 0.0; - out_7731698092980355432[155] = 0.0; - out_7731698092980355432[156] = 0.0; - out_7731698092980355432[157] = 0.0; - out_7731698092980355432[158] = 0.0; - out_7731698092980355432[159] = 0.0; - out_7731698092980355432[160] = 0.0; - out_7731698092980355432[161] = 0.0; - out_7731698092980355432[162] = 0.0; - out_7731698092980355432[163] = 0.0; - out_7731698092980355432[164] = 0.0; - out_7731698092980355432[165] = 0.0; - out_7731698092980355432[166] = 0.0; - out_7731698092980355432[167] = 0.0; - out_7731698092980355432[168] = 0.0; - out_7731698092980355432[169] = 0.0; - out_7731698092980355432[170] = 0.0; - out_7731698092980355432[171] = 1.0; - out_7731698092980355432[172] = 0.0; - out_7731698092980355432[173] = 0.0; - out_7731698092980355432[174] = 0.0; - out_7731698092980355432[175] = 0.0; - out_7731698092980355432[176] = 0.0; - out_7731698092980355432[177] = 0.0; - out_7731698092980355432[178] = 0.0; - out_7731698092980355432[179] = 0.0; - out_7731698092980355432[180] = 0.0; - out_7731698092980355432[181] = 0.0; - out_7731698092980355432[182] = 0.0; - out_7731698092980355432[183] = 0.0; - out_7731698092980355432[184] = 0.0; - out_7731698092980355432[185] = 0.0; - out_7731698092980355432[186] = 0.0; - out_7731698092980355432[187] = 0.0; - out_7731698092980355432[188] = 0.0; - out_7731698092980355432[189] = 0.0; - out_7731698092980355432[190] = 1.0; - out_7731698092980355432[191] = 0.0; - out_7731698092980355432[192] = 0.0; - out_7731698092980355432[193] = 0.0; - out_7731698092980355432[194] = 0.0; - out_7731698092980355432[195] = 0.0; - out_7731698092980355432[196] = 0.0; - out_7731698092980355432[197] = 0.0; - out_7731698092980355432[198] = 0.0; - out_7731698092980355432[199] = 0.0; - out_7731698092980355432[200] = 0.0; - out_7731698092980355432[201] = 0.0; - out_7731698092980355432[202] = 0.0; - out_7731698092980355432[203] = 0.0; - out_7731698092980355432[204] = 0.0; - out_7731698092980355432[205] = 0.0; - out_7731698092980355432[206] = 0.0; - out_7731698092980355432[207] = 0.0; - out_7731698092980355432[208] = 0.0; - out_7731698092980355432[209] = 1.0; - out_7731698092980355432[210] = 0.0; - out_7731698092980355432[211] = 0.0; - out_7731698092980355432[212] = 0.0; - out_7731698092980355432[213] = 0.0; - out_7731698092980355432[214] = 0.0; - out_7731698092980355432[215] = 0.0; - out_7731698092980355432[216] = 0.0; - out_7731698092980355432[217] = 0.0; - out_7731698092980355432[218] = 0.0; - out_7731698092980355432[219] = 0.0; - out_7731698092980355432[220] = 0.0; - out_7731698092980355432[221] = 0.0; - out_7731698092980355432[222] = 0.0; - out_7731698092980355432[223] = 0.0; - out_7731698092980355432[224] = 0.0; - out_7731698092980355432[225] = 0.0; - out_7731698092980355432[226] = 0.0; - out_7731698092980355432[227] = 0.0; - out_7731698092980355432[228] = 1.0; - out_7731698092980355432[229] = 0.0; - out_7731698092980355432[230] = 0.0; - out_7731698092980355432[231] = 0.0; - out_7731698092980355432[232] = 0.0; - out_7731698092980355432[233] = 0.0; - out_7731698092980355432[234] = 0.0; - out_7731698092980355432[235] = 0.0; - out_7731698092980355432[236] = 0.0; - out_7731698092980355432[237] = 0.0; - out_7731698092980355432[238] = 0.0; - out_7731698092980355432[239] = 0.0; - out_7731698092980355432[240] = 0.0; - out_7731698092980355432[241] = 0.0; - out_7731698092980355432[242] = 0.0; - out_7731698092980355432[243] = 0.0; - out_7731698092980355432[244] = 0.0; - out_7731698092980355432[245] = 0.0; - out_7731698092980355432[246] = 0.0; - out_7731698092980355432[247] = 1.0; - out_7731698092980355432[248] = 0.0; - out_7731698092980355432[249] = 0.0; - out_7731698092980355432[250] = 0.0; - out_7731698092980355432[251] = 0.0; - out_7731698092980355432[252] = 0.0; - out_7731698092980355432[253] = 0.0; - out_7731698092980355432[254] = 0.0; - out_7731698092980355432[255] = 0.0; - out_7731698092980355432[256] = 0.0; - out_7731698092980355432[257] = 0.0; - out_7731698092980355432[258] = 0.0; - out_7731698092980355432[259] = 0.0; - out_7731698092980355432[260] = 0.0; - out_7731698092980355432[261] = 0.0; - out_7731698092980355432[262] = 0.0; - out_7731698092980355432[263] = 0.0; - out_7731698092980355432[264] = 0.0; - out_7731698092980355432[265] = 0.0; - out_7731698092980355432[266] = 1.0; - out_7731698092980355432[267] = 0.0; - out_7731698092980355432[268] = 0.0; - out_7731698092980355432[269] = 0.0; - out_7731698092980355432[270] = 0.0; - out_7731698092980355432[271] = 0.0; - out_7731698092980355432[272] = 0.0; - out_7731698092980355432[273] = 0.0; - out_7731698092980355432[274] = 0.0; - out_7731698092980355432[275] = 0.0; - out_7731698092980355432[276] = 0.0; - out_7731698092980355432[277] = 0.0; - out_7731698092980355432[278] = 0.0; - out_7731698092980355432[279] = 0.0; - out_7731698092980355432[280] = 0.0; - out_7731698092980355432[281] = 0.0; - out_7731698092980355432[282] = 0.0; - out_7731698092980355432[283] = 0.0; - out_7731698092980355432[284] = 0.0; - out_7731698092980355432[285] = 1.0; - out_7731698092980355432[286] = 0.0; - out_7731698092980355432[287] = 0.0; - out_7731698092980355432[288] = 0.0; - out_7731698092980355432[289] = 0.0; - out_7731698092980355432[290] = 0.0; - out_7731698092980355432[291] = 0.0; - out_7731698092980355432[292] = 0.0; - out_7731698092980355432[293] = 0.0; - out_7731698092980355432[294] = 0.0; - out_7731698092980355432[295] = 0.0; - out_7731698092980355432[296] = 0.0; - out_7731698092980355432[297] = 0.0; - out_7731698092980355432[298] = 0.0; - out_7731698092980355432[299] = 0.0; - out_7731698092980355432[300] = 0.0; - out_7731698092980355432[301] = 0.0; - out_7731698092980355432[302] = 0.0; - out_7731698092980355432[303] = 0.0; - out_7731698092980355432[304] = 1.0; - out_7731698092980355432[305] = 0.0; - out_7731698092980355432[306] = 0.0; - out_7731698092980355432[307] = 0.0; - out_7731698092980355432[308] = 0.0; - out_7731698092980355432[309] = 0.0; - out_7731698092980355432[310] = 0.0; - out_7731698092980355432[311] = 0.0; - out_7731698092980355432[312] = 0.0; - out_7731698092980355432[313] = 0.0; - out_7731698092980355432[314] = 0.0; - out_7731698092980355432[315] = 0.0; - out_7731698092980355432[316] = 0.0; - out_7731698092980355432[317] = 0.0; - out_7731698092980355432[318] = 0.0; - out_7731698092980355432[319] = 0.0; - out_7731698092980355432[320] = 0.0; - out_7731698092980355432[321] = 0.0; - out_7731698092980355432[322] = 0.0; - out_7731698092980355432[323] = 1.0; +void H_mod_fun(double *state, double *out_6387810172757071512) { + out_6387810172757071512[0] = 1.0; + out_6387810172757071512[1] = 0.0; + out_6387810172757071512[2] = 0.0; + out_6387810172757071512[3] = 0.0; + out_6387810172757071512[4] = 0.0; + out_6387810172757071512[5] = 0.0; + out_6387810172757071512[6] = 0.0; + out_6387810172757071512[7] = 0.0; + out_6387810172757071512[8] = 0.0; + out_6387810172757071512[9] = 0.0; + out_6387810172757071512[10] = 0.0; + out_6387810172757071512[11] = 0.0; + out_6387810172757071512[12] = 0.0; + out_6387810172757071512[13] = 0.0; + out_6387810172757071512[14] = 0.0; + out_6387810172757071512[15] = 0.0; + out_6387810172757071512[16] = 0.0; + out_6387810172757071512[17] = 0.0; + out_6387810172757071512[18] = 0.0; + out_6387810172757071512[19] = 1.0; + out_6387810172757071512[20] = 0.0; + out_6387810172757071512[21] = 0.0; + out_6387810172757071512[22] = 0.0; + out_6387810172757071512[23] = 0.0; + out_6387810172757071512[24] = 0.0; + out_6387810172757071512[25] = 0.0; + out_6387810172757071512[26] = 0.0; + out_6387810172757071512[27] = 0.0; + out_6387810172757071512[28] = 0.0; + out_6387810172757071512[29] = 0.0; + out_6387810172757071512[30] = 0.0; + out_6387810172757071512[31] = 0.0; + out_6387810172757071512[32] = 0.0; + out_6387810172757071512[33] = 0.0; + out_6387810172757071512[34] = 0.0; + out_6387810172757071512[35] = 0.0; + out_6387810172757071512[36] = 0.0; + out_6387810172757071512[37] = 0.0; + out_6387810172757071512[38] = 1.0; + out_6387810172757071512[39] = 0.0; + out_6387810172757071512[40] = 0.0; + out_6387810172757071512[41] = 0.0; + out_6387810172757071512[42] = 0.0; + out_6387810172757071512[43] = 0.0; + out_6387810172757071512[44] = 0.0; + out_6387810172757071512[45] = 0.0; + out_6387810172757071512[46] = 0.0; + out_6387810172757071512[47] = 0.0; + out_6387810172757071512[48] = 0.0; + out_6387810172757071512[49] = 0.0; + out_6387810172757071512[50] = 0.0; + out_6387810172757071512[51] = 0.0; + out_6387810172757071512[52] = 0.0; + out_6387810172757071512[53] = 0.0; + out_6387810172757071512[54] = 0.0; + out_6387810172757071512[55] = 0.0; + out_6387810172757071512[56] = 0.0; + out_6387810172757071512[57] = 1.0; + out_6387810172757071512[58] = 0.0; + out_6387810172757071512[59] = 0.0; + out_6387810172757071512[60] = 0.0; + out_6387810172757071512[61] = 0.0; + out_6387810172757071512[62] = 0.0; + out_6387810172757071512[63] = 0.0; + out_6387810172757071512[64] = 0.0; + out_6387810172757071512[65] = 0.0; + out_6387810172757071512[66] = 0.0; + out_6387810172757071512[67] = 0.0; + out_6387810172757071512[68] = 0.0; + out_6387810172757071512[69] = 0.0; + out_6387810172757071512[70] = 0.0; + out_6387810172757071512[71] = 0.0; + out_6387810172757071512[72] = 0.0; + out_6387810172757071512[73] = 0.0; + out_6387810172757071512[74] = 0.0; + out_6387810172757071512[75] = 0.0; + out_6387810172757071512[76] = 1.0; + out_6387810172757071512[77] = 0.0; + out_6387810172757071512[78] = 0.0; + out_6387810172757071512[79] = 0.0; + out_6387810172757071512[80] = 0.0; + out_6387810172757071512[81] = 0.0; + out_6387810172757071512[82] = 0.0; + out_6387810172757071512[83] = 0.0; + out_6387810172757071512[84] = 0.0; + out_6387810172757071512[85] = 0.0; + out_6387810172757071512[86] = 0.0; + out_6387810172757071512[87] = 0.0; + out_6387810172757071512[88] = 0.0; + out_6387810172757071512[89] = 0.0; + out_6387810172757071512[90] = 0.0; + out_6387810172757071512[91] = 0.0; + out_6387810172757071512[92] = 0.0; + out_6387810172757071512[93] = 0.0; + out_6387810172757071512[94] = 0.0; + out_6387810172757071512[95] = 1.0; + out_6387810172757071512[96] = 0.0; + out_6387810172757071512[97] = 0.0; + out_6387810172757071512[98] = 0.0; + out_6387810172757071512[99] = 0.0; + out_6387810172757071512[100] = 0.0; + out_6387810172757071512[101] = 0.0; + out_6387810172757071512[102] = 0.0; + out_6387810172757071512[103] = 0.0; + out_6387810172757071512[104] = 0.0; + out_6387810172757071512[105] = 0.0; + out_6387810172757071512[106] = 0.0; + out_6387810172757071512[107] = 0.0; + out_6387810172757071512[108] = 0.0; + out_6387810172757071512[109] = 0.0; + out_6387810172757071512[110] = 0.0; + out_6387810172757071512[111] = 0.0; + out_6387810172757071512[112] = 0.0; + out_6387810172757071512[113] = 0.0; + out_6387810172757071512[114] = 1.0; + out_6387810172757071512[115] = 0.0; + out_6387810172757071512[116] = 0.0; + out_6387810172757071512[117] = 0.0; + out_6387810172757071512[118] = 0.0; + out_6387810172757071512[119] = 0.0; + out_6387810172757071512[120] = 0.0; + out_6387810172757071512[121] = 0.0; + out_6387810172757071512[122] = 0.0; + out_6387810172757071512[123] = 0.0; + out_6387810172757071512[124] = 0.0; + out_6387810172757071512[125] = 0.0; + out_6387810172757071512[126] = 0.0; + out_6387810172757071512[127] = 0.0; + out_6387810172757071512[128] = 0.0; + out_6387810172757071512[129] = 0.0; + out_6387810172757071512[130] = 0.0; + out_6387810172757071512[131] = 0.0; + out_6387810172757071512[132] = 0.0; + out_6387810172757071512[133] = 1.0; + out_6387810172757071512[134] = 0.0; + out_6387810172757071512[135] = 0.0; + out_6387810172757071512[136] = 0.0; + out_6387810172757071512[137] = 0.0; + out_6387810172757071512[138] = 0.0; + out_6387810172757071512[139] = 0.0; + out_6387810172757071512[140] = 0.0; + out_6387810172757071512[141] = 0.0; + out_6387810172757071512[142] = 0.0; + out_6387810172757071512[143] = 0.0; + out_6387810172757071512[144] = 0.0; + out_6387810172757071512[145] = 0.0; + out_6387810172757071512[146] = 0.0; + out_6387810172757071512[147] = 0.0; + out_6387810172757071512[148] = 0.0; + out_6387810172757071512[149] = 0.0; + out_6387810172757071512[150] = 0.0; + out_6387810172757071512[151] = 0.0; + out_6387810172757071512[152] = 1.0; + out_6387810172757071512[153] = 0.0; + out_6387810172757071512[154] = 0.0; + out_6387810172757071512[155] = 0.0; + out_6387810172757071512[156] = 0.0; + out_6387810172757071512[157] = 0.0; + out_6387810172757071512[158] = 0.0; + out_6387810172757071512[159] = 0.0; + out_6387810172757071512[160] = 0.0; + out_6387810172757071512[161] = 0.0; + out_6387810172757071512[162] = 0.0; + out_6387810172757071512[163] = 0.0; + out_6387810172757071512[164] = 0.0; + out_6387810172757071512[165] = 0.0; + out_6387810172757071512[166] = 0.0; + out_6387810172757071512[167] = 0.0; + out_6387810172757071512[168] = 0.0; + out_6387810172757071512[169] = 0.0; + out_6387810172757071512[170] = 0.0; + out_6387810172757071512[171] = 1.0; + out_6387810172757071512[172] = 0.0; + out_6387810172757071512[173] = 0.0; + out_6387810172757071512[174] = 0.0; + out_6387810172757071512[175] = 0.0; + out_6387810172757071512[176] = 0.0; + out_6387810172757071512[177] = 0.0; + out_6387810172757071512[178] = 0.0; + out_6387810172757071512[179] = 0.0; + out_6387810172757071512[180] = 0.0; + out_6387810172757071512[181] = 0.0; + out_6387810172757071512[182] = 0.0; + out_6387810172757071512[183] = 0.0; + out_6387810172757071512[184] = 0.0; + out_6387810172757071512[185] = 0.0; + out_6387810172757071512[186] = 0.0; + out_6387810172757071512[187] = 0.0; + out_6387810172757071512[188] = 0.0; + out_6387810172757071512[189] = 0.0; + out_6387810172757071512[190] = 1.0; + out_6387810172757071512[191] = 0.0; + out_6387810172757071512[192] = 0.0; + out_6387810172757071512[193] = 0.0; + out_6387810172757071512[194] = 0.0; + out_6387810172757071512[195] = 0.0; + out_6387810172757071512[196] = 0.0; + out_6387810172757071512[197] = 0.0; + out_6387810172757071512[198] = 0.0; + out_6387810172757071512[199] = 0.0; + out_6387810172757071512[200] = 0.0; + out_6387810172757071512[201] = 0.0; + out_6387810172757071512[202] = 0.0; + out_6387810172757071512[203] = 0.0; + out_6387810172757071512[204] = 0.0; + out_6387810172757071512[205] = 0.0; + out_6387810172757071512[206] = 0.0; + out_6387810172757071512[207] = 0.0; + out_6387810172757071512[208] = 0.0; + out_6387810172757071512[209] = 1.0; + out_6387810172757071512[210] = 0.0; + out_6387810172757071512[211] = 0.0; + out_6387810172757071512[212] = 0.0; + out_6387810172757071512[213] = 0.0; + out_6387810172757071512[214] = 0.0; + out_6387810172757071512[215] = 0.0; + out_6387810172757071512[216] = 0.0; + out_6387810172757071512[217] = 0.0; + out_6387810172757071512[218] = 0.0; + out_6387810172757071512[219] = 0.0; + out_6387810172757071512[220] = 0.0; + out_6387810172757071512[221] = 0.0; + out_6387810172757071512[222] = 0.0; + out_6387810172757071512[223] = 0.0; + out_6387810172757071512[224] = 0.0; + out_6387810172757071512[225] = 0.0; + out_6387810172757071512[226] = 0.0; + out_6387810172757071512[227] = 0.0; + out_6387810172757071512[228] = 1.0; + out_6387810172757071512[229] = 0.0; + out_6387810172757071512[230] = 0.0; + out_6387810172757071512[231] = 0.0; + out_6387810172757071512[232] = 0.0; + out_6387810172757071512[233] = 0.0; + out_6387810172757071512[234] = 0.0; + out_6387810172757071512[235] = 0.0; + out_6387810172757071512[236] = 0.0; + out_6387810172757071512[237] = 0.0; + out_6387810172757071512[238] = 0.0; + out_6387810172757071512[239] = 0.0; + out_6387810172757071512[240] = 0.0; + out_6387810172757071512[241] = 0.0; + out_6387810172757071512[242] = 0.0; + out_6387810172757071512[243] = 0.0; + out_6387810172757071512[244] = 0.0; + out_6387810172757071512[245] = 0.0; + out_6387810172757071512[246] = 0.0; + out_6387810172757071512[247] = 1.0; + out_6387810172757071512[248] = 0.0; + out_6387810172757071512[249] = 0.0; + out_6387810172757071512[250] = 0.0; + out_6387810172757071512[251] = 0.0; + out_6387810172757071512[252] = 0.0; + out_6387810172757071512[253] = 0.0; + out_6387810172757071512[254] = 0.0; + out_6387810172757071512[255] = 0.0; + out_6387810172757071512[256] = 0.0; + out_6387810172757071512[257] = 0.0; + out_6387810172757071512[258] = 0.0; + out_6387810172757071512[259] = 0.0; + out_6387810172757071512[260] = 0.0; + out_6387810172757071512[261] = 0.0; + out_6387810172757071512[262] = 0.0; + out_6387810172757071512[263] = 0.0; + out_6387810172757071512[264] = 0.0; + out_6387810172757071512[265] = 0.0; + out_6387810172757071512[266] = 1.0; + out_6387810172757071512[267] = 0.0; + out_6387810172757071512[268] = 0.0; + out_6387810172757071512[269] = 0.0; + out_6387810172757071512[270] = 0.0; + out_6387810172757071512[271] = 0.0; + out_6387810172757071512[272] = 0.0; + out_6387810172757071512[273] = 0.0; + out_6387810172757071512[274] = 0.0; + out_6387810172757071512[275] = 0.0; + out_6387810172757071512[276] = 0.0; + out_6387810172757071512[277] = 0.0; + out_6387810172757071512[278] = 0.0; + out_6387810172757071512[279] = 0.0; + out_6387810172757071512[280] = 0.0; + out_6387810172757071512[281] = 0.0; + out_6387810172757071512[282] = 0.0; + out_6387810172757071512[283] = 0.0; + out_6387810172757071512[284] = 0.0; + out_6387810172757071512[285] = 1.0; + out_6387810172757071512[286] = 0.0; + out_6387810172757071512[287] = 0.0; + out_6387810172757071512[288] = 0.0; + out_6387810172757071512[289] = 0.0; + out_6387810172757071512[290] = 0.0; + out_6387810172757071512[291] = 0.0; + out_6387810172757071512[292] = 0.0; + out_6387810172757071512[293] = 0.0; + out_6387810172757071512[294] = 0.0; + out_6387810172757071512[295] = 0.0; + out_6387810172757071512[296] = 0.0; + out_6387810172757071512[297] = 0.0; + out_6387810172757071512[298] = 0.0; + out_6387810172757071512[299] = 0.0; + out_6387810172757071512[300] = 0.0; + out_6387810172757071512[301] = 0.0; + out_6387810172757071512[302] = 0.0; + out_6387810172757071512[303] = 0.0; + out_6387810172757071512[304] = 1.0; + out_6387810172757071512[305] = 0.0; + out_6387810172757071512[306] = 0.0; + out_6387810172757071512[307] = 0.0; + out_6387810172757071512[308] = 0.0; + out_6387810172757071512[309] = 0.0; + out_6387810172757071512[310] = 0.0; + out_6387810172757071512[311] = 0.0; + out_6387810172757071512[312] = 0.0; + out_6387810172757071512[313] = 0.0; + out_6387810172757071512[314] = 0.0; + out_6387810172757071512[315] = 0.0; + out_6387810172757071512[316] = 0.0; + out_6387810172757071512[317] = 0.0; + out_6387810172757071512[318] = 0.0; + out_6387810172757071512[319] = 0.0; + out_6387810172757071512[320] = 0.0; + out_6387810172757071512[321] = 0.0; + out_6387810172757071512[322] = 0.0; + out_6387810172757071512[323] = 1.0; } -void f_fun(double *state, double dt, double *out_8923016311880444304) { - out_8923016311880444304[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_8923016311880444304[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_8923016311880444304[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_8923016311880444304[3] = dt*state[12] + state[3]; - out_8923016311880444304[4] = dt*state[13] + state[4]; - out_8923016311880444304[5] = dt*state[14] + state[5]; - out_8923016311880444304[6] = state[6]; - out_8923016311880444304[7] = state[7]; - out_8923016311880444304[8] = state[8]; - out_8923016311880444304[9] = state[9]; - out_8923016311880444304[10] = state[10]; - out_8923016311880444304[11] = state[11]; - out_8923016311880444304[12] = state[12]; - out_8923016311880444304[13] = state[13]; - out_8923016311880444304[14] = state[14]; - out_8923016311880444304[15] = state[15]; - out_8923016311880444304[16] = state[16]; - out_8923016311880444304[17] = state[17]; +void f_fun(double *state, double dt, double *out_5190191951972352761) { + out_5190191951972352761[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_5190191951972352761[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_5190191951972352761[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_5190191951972352761[3] = dt*state[12] + state[3]; + out_5190191951972352761[4] = dt*state[13] + state[4]; + out_5190191951972352761[5] = dt*state[14] + state[5]; + out_5190191951972352761[6] = state[6]; + out_5190191951972352761[7] = state[7]; + out_5190191951972352761[8] = state[8]; + out_5190191951972352761[9] = state[9]; + out_5190191951972352761[10] = state[10]; + out_5190191951972352761[11] = state[11]; + out_5190191951972352761[12] = state[12]; + out_5190191951972352761[13] = state[13]; + out_5190191951972352761[14] = state[14]; + out_5190191951972352761[15] = state[15]; + out_5190191951972352761[16] = state[16]; + out_5190191951972352761[17] = state[17]; } -void F_fun(double *state, double dt, double *out_210116950988546947) { - out_210116950988546947[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_210116950988546947[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_210116950988546947[2] = 0; - out_210116950988546947[3] = 0; - out_210116950988546947[4] = 0; - out_210116950988546947[5] = 0; - out_210116950988546947[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_210116950988546947[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_210116950988546947[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_210116950988546947[9] = 0; - out_210116950988546947[10] = 0; - out_210116950988546947[11] = 0; - out_210116950988546947[12] = 0; - out_210116950988546947[13] = 0; - out_210116950988546947[14] = 0; - out_210116950988546947[15] = 0; - out_210116950988546947[16] = 0; - out_210116950988546947[17] = 0; - out_210116950988546947[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_210116950988546947[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_210116950988546947[20] = 0; - out_210116950988546947[21] = 0; - out_210116950988546947[22] = 0; - out_210116950988546947[23] = 0; - out_210116950988546947[24] = 0; - out_210116950988546947[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_210116950988546947[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_210116950988546947[27] = 0; - out_210116950988546947[28] = 0; - out_210116950988546947[29] = 0; - out_210116950988546947[30] = 0; - out_210116950988546947[31] = 0; - out_210116950988546947[32] = 0; - out_210116950988546947[33] = 0; - out_210116950988546947[34] = 0; - out_210116950988546947[35] = 0; - out_210116950988546947[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_210116950988546947[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_210116950988546947[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_210116950988546947[39] = 0; - out_210116950988546947[40] = 0; - out_210116950988546947[41] = 0; - out_210116950988546947[42] = 0; - out_210116950988546947[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_210116950988546947[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_210116950988546947[45] = 0; - out_210116950988546947[46] = 0; - out_210116950988546947[47] = 0; - out_210116950988546947[48] = 0; - out_210116950988546947[49] = 0; - out_210116950988546947[50] = 0; - out_210116950988546947[51] = 0; - out_210116950988546947[52] = 0; - out_210116950988546947[53] = 0; - out_210116950988546947[54] = 0; - out_210116950988546947[55] = 0; - out_210116950988546947[56] = 0; - out_210116950988546947[57] = 1; - out_210116950988546947[58] = 0; - out_210116950988546947[59] = 0; - out_210116950988546947[60] = 0; - out_210116950988546947[61] = 0; - out_210116950988546947[62] = 0; - out_210116950988546947[63] = 0; - out_210116950988546947[64] = 0; - out_210116950988546947[65] = 0; - out_210116950988546947[66] = dt; - out_210116950988546947[67] = 0; - out_210116950988546947[68] = 0; - out_210116950988546947[69] = 0; - out_210116950988546947[70] = 0; - out_210116950988546947[71] = 0; - out_210116950988546947[72] = 0; - out_210116950988546947[73] = 0; - out_210116950988546947[74] = 0; - out_210116950988546947[75] = 0; - out_210116950988546947[76] = 1; - out_210116950988546947[77] = 0; - out_210116950988546947[78] = 0; - out_210116950988546947[79] = 0; - out_210116950988546947[80] = 0; - out_210116950988546947[81] = 0; - out_210116950988546947[82] = 0; - out_210116950988546947[83] = 0; - out_210116950988546947[84] = 0; - out_210116950988546947[85] = dt; - out_210116950988546947[86] = 0; - out_210116950988546947[87] = 0; - out_210116950988546947[88] = 0; - out_210116950988546947[89] = 0; - out_210116950988546947[90] = 0; - out_210116950988546947[91] = 0; - out_210116950988546947[92] = 0; - out_210116950988546947[93] = 0; - out_210116950988546947[94] = 0; - out_210116950988546947[95] = 1; - out_210116950988546947[96] = 0; - out_210116950988546947[97] = 0; - out_210116950988546947[98] = 0; - out_210116950988546947[99] = 0; - out_210116950988546947[100] = 0; - out_210116950988546947[101] = 0; - out_210116950988546947[102] = 0; - out_210116950988546947[103] = 0; - out_210116950988546947[104] = dt; - out_210116950988546947[105] = 0; - out_210116950988546947[106] = 0; - out_210116950988546947[107] = 0; - out_210116950988546947[108] = 0; - out_210116950988546947[109] = 0; - out_210116950988546947[110] = 0; - out_210116950988546947[111] = 0; - out_210116950988546947[112] = 0; - out_210116950988546947[113] = 0; - out_210116950988546947[114] = 1; - out_210116950988546947[115] = 0; - out_210116950988546947[116] = 0; - out_210116950988546947[117] = 0; - out_210116950988546947[118] = 0; - out_210116950988546947[119] = 0; - out_210116950988546947[120] = 0; - out_210116950988546947[121] = 0; - out_210116950988546947[122] = 0; - out_210116950988546947[123] = 0; - out_210116950988546947[124] = 0; - out_210116950988546947[125] = 0; - out_210116950988546947[126] = 0; - out_210116950988546947[127] = 0; - out_210116950988546947[128] = 0; - out_210116950988546947[129] = 0; - out_210116950988546947[130] = 0; - out_210116950988546947[131] = 0; - out_210116950988546947[132] = 0; - out_210116950988546947[133] = 1; - out_210116950988546947[134] = 0; - out_210116950988546947[135] = 0; - out_210116950988546947[136] = 0; - out_210116950988546947[137] = 0; - out_210116950988546947[138] = 0; - out_210116950988546947[139] = 0; - out_210116950988546947[140] = 0; - out_210116950988546947[141] = 0; - out_210116950988546947[142] = 0; - out_210116950988546947[143] = 0; - out_210116950988546947[144] = 0; - out_210116950988546947[145] = 0; - out_210116950988546947[146] = 0; - out_210116950988546947[147] = 0; - out_210116950988546947[148] = 0; - out_210116950988546947[149] = 0; - out_210116950988546947[150] = 0; - out_210116950988546947[151] = 0; - out_210116950988546947[152] = 1; - out_210116950988546947[153] = 0; - out_210116950988546947[154] = 0; - out_210116950988546947[155] = 0; - out_210116950988546947[156] = 0; - out_210116950988546947[157] = 0; - out_210116950988546947[158] = 0; - out_210116950988546947[159] = 0; - out_210116950988546947[160] = 0; - out_210116950988546947[161] = 0; - out_210116950988546947[162] = 0; - out_210116950988546947[163] = 0; - out_210116950988546947[164] = 0; - out_210116950988546947[165] = 0; - out_210116950988546947[166] = 0; - out_210116950988546947[167] = 0; - out_210116950988546947[168] = 0; - out_210116950988546947[169] = 0; - out_210116950988546947[170] = 0; - out_210116950988546947[171] = 1; - out_210116950988546947[172] = 0; - out_210116950988546947[173] = 0; - out_210116950988546947[174] = 0; - out_210116950988546947[175] = 0; - out_210116950988546947[176] = 0; - out_210116950988546947[177] = 0; - out_210116950988546947[178] = 0; - out_210116950988546947[179] = 0; - out_210116950988546947[180] = 0; - out_210116950988546947[181] = 0; - out_210116950988546947[182] = 0; - out_210116950988546947[183] = 0; - out_210116950988546947[184] = 0; - out_210116950988546947[185] = 0; - out_210116950988546947[186] = 0; - out_210116950988546947[187] = 0; - out_210116950988546947[188] = 0; - out_210116950988546947[189] = 0; - out_210116950988546947[190] = 1; - out_210116950988546947[191] = 0; - out_210116950988546947[192] = 0; - out_210116950988546947[193] = 0; - out_210116950988546947[194] = 0; - out_210116950988546947[195] = 0; - out_210116950988546947[196] = 0; - out_210116950988546947[197] = 0; - out_210116950988546947[198] = 0; - out_210116950988546947[199] = 0; - out_210116950988546947[200] = 0; - out_210116950988546947[201] = 0; - out_210116950988546947[202] = 0; - out_210116950988546947[203] = 0; - out_210116950988546947[204] = 0; - out_210116950988546947[205] = 0; - out_210116950988546947[206] = 0; - out_210116950988546947[207] = 0; - out_210116950988546947[208] = 0; - out_210116950988546947[209] = 1; - out_210116950988546947[210] = 0; - out_210116950988546947[211] = 0; - out_210116950988546947[212] = 0; - out_210116950988546947[213] = 0; - out_210116950988546947[214] = 0; - out_210116950988546947[215] = 0; - out_210116950988546947[216] = 0; - out_210116950988546947[217] = 0; - out_210116950988546947[218] = 0; - out_210116950988546947[219] = 0; - out_210116950988546947[220] = 0; - out_210116950988546947[221] = 0; - out_210116950988546947[222] = 0; - out_210116950988546947[223] = 0; - out_210116950988546947[224] = 0; - out_210116950988546947[225] = 0; - out_210116950988546947[226] = 0; - out_210116950988546947[227] = 0; - out_210116950988546947[228] = 1; - out_210116950988546947[229] = 0; - out_210116950988546947[230] = 0; - out_210116950988546947[231] = 0; - out_210116950988546947[232] = 0; - out_210116950988546947[233] = 0; - out_210116950988546947[234] = 0; - out_210116950988546947[235] = 0; - out_210116950988546947[236] = 0; - out_210116950988546947[237] = 0; - out_210116950988546947[238] = 0; - out_210116950988546947[239] = 0; - out_210116950988546947[240] = 0; - out_210116950988546947[241] = 0; - out_210116950988546947[242] = 0; - out_210116950988546947[243] = 0; - out_210116950988546947[244] = 0; - out_210116950988546947[245] = 0; - out_210116950988546947[246] = 0; - out_210116950988546947[247] = 1; - out_210116950988546947[248] = 0; - out_210116950988546947[249] = 0; - out_210116950988546947[250] = 0; - out_210116950988546947[251] = 0; - out_210116950988546947[252] = 0; - out_210116950988546947[253] = 0; - out_210116950988546947[254] = 0; - out_210116950988546947[255] = 0; - out_210116950988546947[256] = 0; - out_210116950988546947[257] = 0; - out_210116950988546947[258] = 0; - out_210116950988546947[259] = 0; - out_210116950988546947[260] = 0; - out_210116950988546947[261] = 0; - out_210116950988546947[262] = 0; - out_210116950988546947[263] = 0; - out_210116950988546947[264] = 0; - out_210116950988546947[265] = 0; - out_210116950988546947[266] = 1; - out_210116950988546947[267] = 0; - out_210116950988546947[268] = 0; - out_210116950988546947[269] = 0; - out_210116950988546947[270] = 0; - out_210116950988546947[271] = 0; - out_210116950988546947[272] = 0; - out_210116950988546947[273] = 0; - out_210116950988546947[274] = 0; - out_210116950988546947[275] = 0; - out_210116950988546947[276] = 0; - out_210116950988546947[277] = 0; - out_210116950988546947[278] = 0; - out_210116950988546947[279] = 0; - out_210116950988546947[280] = 0; - out_210116950988546947[281] = 0; - out_210116950988546947[282] = 0; - out_210116950988546947[283] = 0; - out_210116950988546947[284] = 0; - out_210116950988546947[285] = 1; - out_210116950988546947[286] = 0; - out_210116950988546947[287] = 0; - out_210116950988546947[288] = 0; - out_210116950988546947[289] = 0; - out_210116950988546947[290] = 0; - out_210116950988546947[291] = 0; - out_210116950988546947[292] = 0; - out_210116950988546947[293] = 0; - out_210116950988546947[294] = 0; - out_210116950988546947[295] = 0; - out_210116950988546947[296] = 0; - out_210116950988546947[297] = 0; - out_210116950988546947[298] = 0; - out_210116950988546947[299] = 0; - out_210116950988546947[300] = 0; - out_210116950988546947[301] = 0; - out_210116950988546947[302] = 0; - out_210116950988546947[303] = 0; - out_210116950988546947[304] = 1; - out_210116950988546947[305] = 0; - out_210116950988546947[306] = 0; - out_210116950988546947[307] = 0; - out_210116950988546947[308] = 0; - out_210116950988546947[309] = 0; - out_210116950988546947[310] = 0; - out_210116950988546947[311] = 0; - out_210116950988546947[312] = 0; - out_210116950988546947[313] = 0; - out_210116950988546947[314] = 0; - out_210116950988546947[315] = 0; - out_210116950988546947[316] = 0; - out_210116950988546947[317] = 0; - out_210116950988546947[318] = 0; - out_210116950988546947[319] = 0; - out_210116950988546947[320] = 0; - out_210116950988546947[321] = 0; - out_210116950988546947[322] = 0; - out_210116950988546947[323] = 1; +void F_fun(double *state, double dt, double *out_3425534515038953406) { + out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[2] = 0; + out_3425534515038953406[3] = 0; + out_3425534515038953406[4] = 0; + out_3425534515038953406[5] = 0; + out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[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_3425534515038953406[9] = 0; + out_3425534515038953406[10] = 0; + out_3425534515038953406[11] = 0; + out_3425534515038953406[12] = 0; + out_3425534515038953406[13] = 0; + out_3425534515038953406[14] = 0; + out_3425534515038953406[15] = 0; + out_3425534515038953406[16] = 0; + out_3425534515038953406[17] = 0; + out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[20] = 0; + out_3425534515038953406[21] = 0; + out_3425534515038953406[22] = 0; + out_3425534515038953406[23] = 0; + out_3425534515038953406[24] = 0; + out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[27] = 0; + out_3425534515038953406[28] = 0; + out_3425534515038953406[29] = 0; + out_3425534515038953406[30] = 0; + out_3425534515038953406[31] = 0; + out_3425534515038953406[32] = 0; + out_3425534515038953406[33] = 0; + out_3425534515038953406[34] = 0; + out_3425534515038953406[35] = 0; + out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[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_3425534515038953406[39] = 0; + out_3425534515038953406[40] = 0; + out_3425534515038953406[41] = 0; + out_3425534515038953406[42] = 0; + out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[45] = 0; + out_3425534515038953406[46] = 0; + out_3425534515038953406[47] = 0; + out_3425534515038953406[48] = 0; + out_3425534515038953406[49] = 0; + out_3425534515038953406[50] = 0; + out_3425534515038953406[51] = 0; + out_3425534515038953406[52] = 0; + out_3425534515038953406[53] = 0; + out_3425534515038953406[54] = 0; + out_3425534515038953406[55] = 0; + out_3425534515038953406[56] = 0; + out_3425534515038953406[57] = 1; + out_3425534515038953406[58] = 0; + out_3425534515038953406[59] = 0; + out_3425534515038953406[60] = 0; + out_3425534515038953406[61] = 0; + out_3425534515038953406[62] = 0; + out_3425534515038953406[63] = 0; + out_3425534515038953406[64] = 0; + out_3425534515038953406[65] = 0; + out_3425534515038953406[66] = dt; + out_3425534515038953406[67] = 0; + out_3425534515038953406[68] = 0; + out_3425534515038953406[69] = 0; + out_3425534515038953406[70] = 0; + out_3425534515038953406[71] = 0; + out_3425534515038953406[72] = 0; + out_3425534515038953406[73] = 0; + out_3425534515038953406[74] = 0; + out_3425534515038953406[75] = 0; + out_3425534515038953406[76] = 1; + out_3425534515038953406[77] = 0; + out_3425534515038953406[78] = 0; + out_3425534515038953406[79] = 0; + out_3425534515038953406[80] = 0; + out_3425534515038953406[81] = 0; + out_3425534515038953406[82] = 0; + out_3425534515038953406[83] = 0; + out_3425534515038953406[84] = 0; + out_3425534515038953406[85] = dt; + out_3425534515038953406[86] = 0; + out_3425534515038953406[87] = 0; + out_3425534515038953406[88] = 0; + out_3425534515038953406[89] = 0; + out_3425534515038953406[90] = 0; + out_3425534515038953406[91] = 0; + out_3425534515038953406[92] = 0; + out_3425534515038953406[93] = 0; + out_3425534515038953406[94] = 0; + out_3425534515038953406[95] = 1; + out_3425534515038953406[96] = 0; + out_3425534515038953406[97] = 0; + out_3425534515038953406[98] = 0; + out_3425534515038953406[99] = 0; + out_3425534515038953406[100] = 0; + out_3425534515038953406[101] = 0; + out_3425534515038953406[102] = 0; + out_3425534515038953406[103] = 0; + out_3425534515038953406[104] = dt; + out_3425534515038953406[105] = 0; + out_3425534515038953406[106] = 0; + out_3425534515038953406[107] = 0; + out_3425534515038953406[108] = 0; + out_3425534515038953406[109] = 0; + out_3425534515038953406[110] = 0; + out_3425534515038953406[111] = 0; + out_3425534515038953406[112] = 0; + out_3425534515038953406[113] = 0; + out_3425534515038953406[114] = 1; + out_3425534515038953406[115] = 0; + out_3425534515038953406[116] = 0; + out_3425534515038953406[117] = 0; + out_3425534515038953406[118] = 0; + out_3425534515038953406[119] = 0; + out_3425534515038953406[120] = 0; + out_3425534515038953406[121] = 0; + out_3425534515038953406[122] = 0; + out_3425534515038953406[123] = 0; + out_3425534515038953406[124] = 0; + out_3425534515038953406[125] = 0; + out_3425534515038953406[126] = 0; + out_3425534515038953406[127] = 0; + out_3425534515038953406[128] = 0; + out_3425534515038953406[129] = 0; + out_3425534515038953406[130] = 0; + out_3425534515038953406[131] = 0; + out_3425534515038953406[132] = 0; + out_3425534515038953406[133] = 1; + out_3425534515038953406[134] = 0; + out_3425534515038953406[135] = 0; + out_3425534515038953406[136] = 0; + out_3425534515038953406[137] = 0; + out_3425534515038953406[138] = 0; + out_3425534515038953406[139] = 0; + out_3425534515038953406[140] = 0; + out_3425534515038953406[141] = 0; + out_3425534515038953406[142] = 0; + out_3425534515038953406[143] = 0; + out_3425534515038953406[144] = 0; + out_3425534515038953406[145] = 0; + out_3425534515038953406[146] = 0; + out_3425534515038953406[147] = 0; + out_3425534515038953406[148] = 0; + out_3425534515038953406[149] = 0; + out_3425534515038953406[150] = 0; + out_3425534515038953406[151] = 0; + out_3425534515038953406[152] = 1; + out_3425534515038953406[153] = 0; + out_3425534515038953406[154] = 0; + out_3425534515038953406[155] = 0; + out_3425534515038953406[156] = 0; + out_3425534515038953406[157] = 0; + out_3425534515038953406[158] = 0; + out_3425534515038953406[159] = 0; + out_3425534515038953406[160] = 0; + out_3425534515038953406[161] = 0; + out_3425534515038953406[162] = 0; + out_3425534515038953406[163] = 0; + out_3425534515038953406[164] = 0; + out_3425534515038953406[165] = 0; + out_3425534515038953406[166] = 0; + out_3425534515038953406[167] = 0; + out_3425534515038953406[168] = 0; + out_3425534515038953406[169] = 0; + out_3425534515038953406[170] = 0; + out_3425534515038953406[171] = 1; + out_3425534515038953406[172] = 0; + out_3425534515038953406[173] = 0; + out_3425534515038953406[174] = 0; + out_3425534515038953406[175] = 0; + out_3425534515038953406[176] = 0; + out_3425534515038953406[177] = 0; + out_3425534515038953406[178] = 0; + out_3425534515038953406[179] = 0; + out_3425534515038953406[180] = 0; + out_3425534515038953406[181] = 0; + out_3425534515038953406[182] = 0; + out_3425534515038953406[183] = 0; + out_3425534515038953406[184] = 0; + out_3425534515038953406[185] = 0; + out_3425534515038953406[186] = 0; + out_3425534515038953406[187] = 0; + out_3425534515038953406[188] = 0; + out_3425534515038953406[189] = 0; + out_3425534515038953406[190] = 1; + out_3425534515038953406[191] = 0; + out_3425534515038953406[192] = 0; + out_3425534515038953406[193] = 0; + out_3425534515038953406[194] = 0; + out_3425534515038953406[195] = 0; + out_3425534515038953406[196] = 0; + out_3425534515038953406[197] = 0; + out_3425534515038953406[198] = 0; + out_3425534515038953406[199] = 0; + out_3425534515038953406[200] = 0; + out_3425534515038953406[201] = 0; + out_3425534515038953406[202] = 0; + out_3425534515038953406[203] = 0; + out_3425534515038953406[204] = 0; + out_3425534515038953406[205] = 0; + out_3425534515038953406[206] = 0; + out_3425534515038953406[207] = 0; + out_3425534515038953406[208] = 0; + out_3425534515038953406[209] = 1; + out_3425534515038953406[210] = 0; + out_3425534515038953406[211] = 0; + out_3425534515038953406[212] = 0; + out_3425534515038953406[213] = 0; + out_3425534515038953406[214] = 0; + out_3425534515038953406[215] = 0; + out_3425534515038953406[216] = 0; + out_3425534515038953406[217] = 0; + out_3425534515038953406[218] = 0; + out_3425534515038953406[219] = 0; + out_3425534515038953406[220] = 0; + out_3425534515038953406[221] = 0; + out_3425534515038953406[222] = 0; + out_3425534515038953406[223] = 0; + out_3425534515038953406[224] = 0; + out_3425534515038953406[225] = 0; + out_3425534515038953406[226] = 0; + out_3425534515038953406[227] = 0; + out_3425534515038953406[228] = 1; + out_3425534515038953406[229] = 0; + out_3425534515038953406[230] = 0; + out_3425534515038953406[231] = 0; + out_3425534515038953406[232] = 0; + out_3425534515038953406[233] = 0; + out_3425534515038953406[234] = 0; + out_3425534515038953406[235] = 0; + out_3425534515038953406[236] = 0; + out_3425534515038953406[237] = 0; + out_3425534515038953406[238] = 0; + out_3425534515038953406[239] = 0; + out_3425534515038953406[240] = 0; + out_3425534515038953406[241] = 0; + out_3425534515038953406[242] = 0; + out_3425534515038953406[243] = 0; + out_3425534515038953406[244] = 0; + out_3425534515038953406[245] = 0; + out_3425534515038953406[246] = 0; + out_3425534515038953406[247] = 1; + out_3425534515038953406[248] = 0; + out_3425534515038953406[249] = 0; + out_3425534515038953406[250] = 0; + out_3425534515038953406[251] = 0; + out_3425534515038953406[252] = 0; + out_3425534515038953406[253] = 0; + out_3425534515038953406[254] = 0; + out_3425534515038953406[255] = 0; + out_3425534515038953406[256] = 0; + out_3425534515038953406[257] = 0; + out_3425534515038953406[258] = 0; + out_3425534515038953406[259] = 0; + out_3425534515038953406[260] = 0; + out_3425534515038953406[261] = 0; + out_3425534515038953406[262] = 0; + out_3425534515038953406[263] = 0; + out_3425534515038953406[264] = 0; + out_3425534515038953406[265] = 0; + out_3425534515038953406[266] = 1; + out_3425534515038953406[267] = 0; + out_3425534515038953406[268] = 0; + out_3425534515038953406[269] = 0; + out_3425534515038953406[270] = 0; + out_3425534515038953406[271] = 0; + out_3425534515038953406[272] = 0; + out_3425534515038953406[273] = 0; + out_3425534515038953406[274] = 0; + out_3425534515038953406[275] = 0; + out_3425534515038953406[276] = 0; + out_3425534515038953406[277] = 0; + out_3425534515038953406[278] = 0; + out_3425534515038953406[279] = 0; + out_3425534515038953406[280] = 0; + out_3425534515038953406[281] = 0; + out_3425534515038953406[282] = 0; + out_3425534515038953406[283] = 0; + out_3425534515038953406[284] = 0; + out_3425534515038953406[285] = 1; + out_3425534515038953406[286] = 0; + out_3425534515038953406[287] = 0; + out_3425534515038953406[288] = 0; + out_3425534515038953406[289] = 0; + out_3425534515038953406[290] = 0; + out_3425534515038953406[291] = 0; + out_3425534515038953406[292] = 0; + out_3425534515038953406[293] = 0; + out_3425534515038953406[294] = 0; + out_3425534515038953406[295] = 0; + out_3425534515038953406[296] = 0; + out_3425534515038953406[297] = 0; + out_3425534515038953406[298] = 0; + out_3425534515038953406[299] = 0; + out_3425534515038953406[300] = 0; + out_3425534515038953406[301] = 0; + out_3425534515038953406[302] = 0; + out_3425534515038953406[303] = 0; + out_3425534515038953406[304] = 1; + out_3425534515038953406[305] = 0; + out_3425534515038953406[306] = 0; + out_3425534515038953406[307] = 0; + out_3425534515038953406[308] = 0; + out_3425534515038953406[309] = 0; + out_3425534515038953406[310] = 0; + out_3425534515038953406[311] = 0; + out_3425534515038953406[312] = 0; + out_3425534515038953406[313] = 0; + out_3425534515038953406[314] = 0; + out_3425534515038953406[315] = 0; + out_3425534515038953406[316] = 0; + out_3425534515038953406[317] = 0; + out_3425534515038953406[318] = 0; + out_3425534515038953406[319] = 0; + out_3425534515038953406[320] = 0; + out_3425534515038953406[321] = 0; + out_3425534515038953406[322] = 0; + out_3425534515038953406[323] = 1; } -void h_4(double *state, double *unused, double *out_7648624411257127562) { - out_7648624411257127562[0] = state[6] + state[9]; - out_7648624411257127562[1] = state[7] + state[10]; - out_7648624411257127562[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_7576152197490450413) { + out_7576152197490450413[0] = state[6] + state[9]; + out_7576152197490450413[1] = state[7] + state[10]; + out_7576152197490450413[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_3959890883136447860) { - out_3959890883136447860[0] = 0; - out_3959890883136447860[1] = 0; - out_3959890883136447860[2] = 0; - out_3959890883136447860[3] = 0; - out_3959890883136447860[4] = 0; - out_3959890883136447860[5] = 0; - out_3959890883136447860[6] = 1; - out_3959890883136447860[7] = 0; - out_3959890883136447860[8] = 0; - out_3959890883136447860[9] = 1; - out_3959890883136447860[10] = 0; - out_3959890883136447860[11] = 0; - out_3959890883136447860[12] = 0; - out_3959890883136447860[13] = 0; - out_3959890883136447860[14] = 0; - out_3959890883136447860[15] = 0; - out_3959890883136447860[16] = 0; - out_3959890883136447860[17] = 0; - out_3959890883136447860[18] = 0; - out_3959890883136447860[19] = 0; - out_3959890883136447860[20] = 0; - out_3959890883136447860[21] = 0; - out_3959890883136447860[22] = 0; - out_3959890883136447860[23] = 0; - out_3959890883136447860[24] = 0; - out_3959890883136447860[25] = 1; - out_3959890883136447860[26] = 0; - out_3959890883136447860[27] = 0; - out_3959890883136447860[28] = 1; - out_3959890883136447860[29] = 0; - out_3959890883136447860[30] = 0; - out_3959890883136447860[31] = 0; - out_3959890883136447860[32] = 0; - out_3959890883136447860[33] = 0; - out_3959890883136447860[34] = 0; - out_3959890883136447860[35] = 0; - out_3959890883136447860[36] = 0; - out_3959890883136447860[37] = 0; - out_3959890883136447860[38] = 0; - out_3959890883136447860[39] = 0; - out_3959890883136447860[40] = 0; - out_3959890883136447860[41] = 0; - out_3959890883136447860[42] = 0; - out_3959890883136447860[43] = 0; - out_3959890883136447860[44] = 1; - out_3959890883136447860[45] = 0; - out_3959890883136447860[46] = 0; - out_3959890883136447860[47] = 1; - out_3959890883136447860[48] = 0; - out_3959890883136447860[49] = 0; - out_3959890883136447860[50] = 0; - out_3959890883136447860[51] = 0; - out_3959890883136447860[52] = 0; - out_3959890883136447860[53] = 0; +void H_4(double *state, double *unused, double *out_3320737668869490966) { + out_3320737668869490966[0] = 0; + out_3320737668869490966[1] = 0; + out_3320737668869490966[2] = 0; + out_3320737668869490966[3] = 0; + out_3320737668869490966[4] = 0; + out_3320737668869490966[5] = 0; + out_3320737668869490966[6] = 1; + out_3320737668869490966[7] = 0; + out_3320737668869490966[8] = 0; + out_3320737668869490966[9] = 1; + out_3320737668869490966[10] = 0; + out_3320737668869490966[11] = 0; + out_3320737668869490966[12] = 0; + out_3320737668869490966[13] = 0; + out_3320737668869490966[14] = 0; + out_3320737668869490966[15] = 0; + out_3320737668869490966[16] = 0; + out_3320737668869490966[17] = 0; + out_3320737668869490966[18] = 0; + out_3320737668869490966[19] = 0; + out_3320737668869490966[20] = 0; + out_3320737668869490966[21] = 0; + out_3320737668869490966[22] = 0; + out_3320737668869490966[23] = 0; + out_3320737668869490966[24] = 0; + out_3320737668869490966[25] = 1; + out_3320737668869490966[26] = 0; + out_3320737668869490966[27] = 0; + out_3320737668869490966[28] = 1; + out_3320737668869490966[29] = 0; + out_3320737668869490966[30] = 0; + out_3320737668869490966[31] = 0; + out_3320737668869490966[32] = 0; + out_3320737668869490966[33] = 0; + out_3320737668869490966[34] = 0; + out_3320737668869490966[35] = 0; + out_3320737668869490966[36] = 0; + out_3320737668869490966[37] = 0; + out_3320737668869490966[38] = 0; + out_3320737668869490966[39] = 0; + out_3320737668869490966[40] = 0; + out_3320737668869490966[41] = 0; + out_3320737668869490966[42] = 0; + out_3320737668869490966[43] = 0; + out_3320737668869490966[44] = 1; + out_3320737668869490966[45] = 0; + out_3320737668869490966[46] = 0; + out_3320737668869490966[47] = 1; + out_3320737668869490966[48] = 0; + out_3320737668869490966[49] = 0; + out_3320737668869490966[50] = 0; + out_3320737668869490966[51] = 0; + out_3320737668869490966[52] = 0; + out_3320737668869490966[53] = 0; } -void h_10(double *state, double *unused, double *out_6099133755902511145) { - out_6099133755902511145[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_6099133755902511145[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_6099133755902511145[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_3130452913144701320) { + out_3130452913144701320[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_3130452913144701320[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_3130452913144701320[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_2569809615037315587) { - out_2569809615037315587[0] = 0; - out_2569809615037315587[1] = 9.8100000000000005*cos(state[1]); - out_2569809615037315587[2] = 0; - out_2569809615037315587[3] = 0; - out_2569809615037315587[4] = -state[8]; - out_2569809615037315587[5] = state[7]; - out_2569809615037315587[6] = 0; - out_2569809615037315587[7] = state[5]; - out_2569809615037315587[8] = -state[4]; - out_2569809615037315587[9] = 0; - out_2569809615037315587[10] = 0; - out_2569809615037315587[11] = 0; - out_2569809615037315587[12] = 1; - out_2569809615037315587[13] = 0; - out_2569809615037315587[14] = 0; - out_2569809615037315587[15] = 1; - out_2569809615037315587[16] = 0; - out_2569809615037315587[17] = 0; - out_2569809615037315587[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_2569809615037315587[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_2569809615037315587[20] = 0; - out_2569809615037315587[21] = state[8]; - out_2569809615037315587[22] = 0; - out_2569809615037315587[23] = -state[6]; - out_2569809615037315587[24] = -state[5]; - out_2569809615037315587[25] = 0; - out_2569809615037315587[26] = state[3]; - out_2569809615037315587[27] = 0; - out_2569809615037315587[28] = 0; - out_2569809615037315587[29] = 0; - out_2569809615037315587[30] = 0; - out_2569809615037315587[31] = 1; - out_2569809615037315587[32] = 0; - out_2569809615037315587[33] = 0; - out_2569809615037315587[34] = 1; - out_2569809615037315587[35] = 0; - out_2569809615037315587[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_2569809615037315587[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_2569809615037315587[38] = 0; - out_2569809615037315587[39] = -state[7]; - out_2569809615037315587[40] = state[6]; - out_2569809615037315587[41] = 0; - out_2569809615037315587[42] = state[4]; - out_2569809615037315587[43] = -state[3]; - out_2569809615037315587[44] = 0; - out_2569809615037315587[45] = 0; - out_2569809615037315587[46] = 0; - out_2569809615037315587[47] = 0; - out_2569809615037315587[48] = 0; - out_2569809615037315587[49] = 0; - out_2569809615037315587[50] = 1; - out_2569809615037315587[51] = 0; - out_2569809615037315587[52] = 0; - out_2569809615037315587[53] = 1; +void H_10(double *state, double *unused, double *out_2096220204218508864) { + out_2096220204218508864[0] = 0; + out_2096220204218508864[1] = 9.8100000000000005*cos(state[1]); + out_2096220204218508864[2] = 0; + out_2096220204218508864[3] = 0; + out_2096220204218508864[4] = -state[8]; + out_2096220204218508864[5] = state[7]; + out_2096220204218508864[6] = 0; + out_2096220204218508864[7] = state[5]; + out_2096220204218508864[8] = -state[4]; + out_2096220204218508864[9] = 0; + out_2096220204218508864[10] = 0; + out_2096220204218508864[11] = 0; + out_2096220204218508864[12] = 1; + out_2096220204218508864[13] = 0; + out_2096220204218508864[14] = 0; + out_2096220204218508864[15] = 1; + out_2096220204218508864[16] = 0; + out_2096220204218508864[17] = 0; + out_2096220204218508864[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_2096220204218508864[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_2096220204218508864[20] = 0; + out_2096220204218508864[21] = state[8]; + out_2096220204218508864[22] = 0; + out_2096220204218508864[23] = -state[6]; + out_2096220204218508864[24] = -state[5]; + out_2096220204218508864[25] = 0; + out_2096220204218508864[26] = state[3]; + out_2096220204218508864[27] = 0; + out_2096220204218508864[28] = 0; + out_2096220204218508864[29] = 0; + out_2096220204218508864[30] = 0; + out_2096220204218508864[31] = 1; + out_2096220204218508864[32] = 0; + out_2096220204218508864[33] = 0; + out_2096220204218508864[34] = 1; + out_2096220204218508864[35] = 0; + out_2096220204218508864[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_2096220204218508864[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_2096220204218508864[38] = 0; + out_2096220204218508864[39] = -state[7]; + out_2096220204218508864[40] = state[6]; + out_2096220204218508864[41] = 0; + out_2096220204218508864[42] = state[4]; + out_2096220204218508864[43] = -state[3]; + out_2096220204218508864[44] = 0; + out_2096220204218508864[45] = 0; + out_2096220204218508864[46] = 0; + out_2096220204218508864[47] = 0; + out_2096220204218508864[48] = 0; + out_2096220204218508864[49] = 0; + out_2096220204218508864[50] = 1; + out_2096220204218508864[51] = 0; + out_2096220204218508864[52] = 0; + out_2096220204218508864[53] = 1; } -void h_13(double *state, double *unused, double *out_2653699383913433253) { - out_2653699383913433253[0] = state[3]; - out_2653699383913433253[1] = state[4]; - out_2653699383913433253[2] = state[5]; +void h_13(double *state, double *unused, double *out_8767951916852656852) { + out_8767951916852656852[0] = state[3]; + out_8767951916852656852[1] = state[4]; + out_8767951916852656852[2] = state[5]; } -void H_13(double *state, double *unused, double *out_747617057804115059) { - out_747617057804115059[0] = 0; - out_747617057804115059[1] = 0; - out_747617057804115059[2] = 0; - out_747617057804115059[3] = 1; - out_747617057804115059[4] = 0; - out_747617057804115059[5] = 0; - out_747617057804115059[6] = 0; - out_747617057804115059[7] = 0; - out_747617057804115059[8] = 0; - out_747617057804115059[9] = 0; - out_747617057804115059[10] = 0; - out_747617057804115059[11] = 0; - out_747617057804115059[12] = 0; - out_747617057804115059[13] = 0; - out_747617057804115059[14] = 0; - out_747617057804115059[15] = 0; - out_747617057804115059[16] = 0; - out_747617057804115059[17] = 0; - out_747617057804115059[18] = 0; - out_747617057804115059[19] = 0; - out_747617057804115059[20] = 0; - out_747617057804115059[21] = 0; - out_747617057804115059[22] = 1; - out_747617057804115059[23] = 0; - out_747617057804115059[24] = 0; - out_747617057804115059[25] = 0; - out_747617057804115059[26] = 0; - out_747617057804115059[27] = 0; - out_747617057804115059[28] = 0; - out_747617057804115059[29] = 0; - out_747617057804115059[30] = 0; - out_747617057804115059[31] = 0; - out_747617057804115059[32] = 0; - out_747617057804115059[33] = 0; - out_747617057804115059[34] = 0; - out_747617057804115059[35] = 0; - out_747617057804115059[36] = 0; - out_747617057804115059[37] = 0; - out_747617057804115059[38] = 0; - out_747617057804115059[39] = 0; - out_747617057804115059[40] = 0; - out_747617057804115059[41] = 1; - out_747617057804115059[42] = 0; - out_747617057804115059[43] = 0; - out_747617057804115059[44] = 0; - out_747617057804115059[45] = 0; - out_747617057804115059[46] = 0; - out_747617057804115059[47] = 0; - out_747617057804115059[48] = 0; - out_747617057804115059[49] = 0; - out_747617057804115059[50] = 0; - out_747617057804115059[51] = 0; - out_747617057804115059[52] = 0; - out_747617057804115059[53] = 0; +void H_13(double *state, double *unused, double *out_108463843537158165) { + out_108463843537158165[0] = 0; + out_108463843537158165[1] = 0; + out_108463843537158165[2] = 0; + out_108463843537158165[3] = 1; + out_108463843537158165[4] = 0; + out_108463843537158165[5] = 0; + out_108463843537158165[6] = 0; + out_108463843537158165[7] = 0; + out_108463843537158165[8] = 0; + out_108463843537158165[9] = 0; + out_108463843537158165[10] = 0; + out_108463843537158165[11] = 0; + out_108463843537158165[12] = 0; + out_108463843537158165[13] = 0; + out_108463843537158165[14] = 0; + out_108463843537158165[15] = 0; + out_108463843537158165[16] = 0; + out_108463843537158165[17] = 0; + out_108463843537158165[18] = 0; + out_108463843537158165[19] = 0; + out_108463843537158165[20] = 0; + out_108463843537158165[21] = 0; + out_108463843537158165[22] = 1; + out_108463843537158165[23] = 0; + out_108463843537158165[24] = 0; + out_108463843537158165[25] = 0; + out_108463843537158165[26] = 0; + out_108463843537158165[27] = 0; + out_108463843537158165[28] = 0; + out_108463843537158165[29] = 0; + out_108463843537158165[30] = 0; + out_108463843537158165[31] = 0; + out_108463843537158165[32] = 0; + out_108463843537158165[33] = 0; + out_108463843537158165[34] = 0; + out_108463843537158165[35] = 0; + out_108463843537158165[36] = 0; + out_108463843537158165[37] = 0; + out_108463843537158165[38] = 0; + out_108463843537158165[39] = 0; + out_108463843537158165[40] = 0; + out_108463843537158165[41] = 1; + out_108463843537158165[42] = 0; + out_108463843537158165[43] = 0; + out_108463843537158165[44] = 0; + out_108463843537158165[45] = 0; + out_108463843537158165[46] = 0; + out_108463843537158165[47] = 0; + out_108463843537158165[48] = 0; + out_108463843537158165[49] = 0; + out_108463843537158165[50] = 0; + out_108463843537158165[51] = 0; + out_108463843537158165[52] = 0; + out_108463843537158165[53] = 0; } -void h_14(double *state, double *unused, double *out_7344654912082976031) { - out_7344654912082976031[0] = state[6]; - out_7344654912082976031[1] = state[7]; - out_7344654912082976031[2] = state[8]; +void h_14(double *state, double *unused, double *out_7181242506613011838) { + out_7181242506613011838[0] = state[6]; + out_7181242506613011838[1] = state[7]; + out_7181242506613011838[2] = state[8]; } -void H_14(double *state, double *unused, double *out_3349973203036669) { - out_3349973203036669[0] = 0; - out_3349973203036669[1] = 0; - out_3349973203036669[2] = 0; - out_3349973203036669[3] = 0; - out_3349973203036669[4] = 0; - out_3349973203036669[5] = 0; - out_3349973203036669[6] = 1; - out_3349973203036669[7] = 0; - out_3349973203036669[8] = 0; - out_3349973203036669[9] = 0; - out_3349973203036669[10] = 0; - out_3349973203036669[11] = 0; - out_3349973203036669[12] = 0; - out_3349973203036669[13] = 0; - out_3349973203036669[14] = 0; - out_3349973203036669[15] = 0; - out_3349973203036669[16] = 0; - out_3349973203036669[17] = 0; - out_3349973203036669[18] = 0; - out_3349973203036669[19] = 0; - out_3349973203036669[20] = 0; - out_3349973203036669[21] = 0; - out_3349973203036669[22] = 0; - out_3349973203036669[23] = 0; - out_3349973203036669[24] = 0; - out_3349973203036669[25] = 1; - out_3349973203036669[26] = 0; - out_3349973203036669[27] = 0; - out_3349973203036669[28] = 0; - out_3349973203036669[29] = 0; - out_3349973203036669[30] = 0; - out_3349973203036669[31] = 0; - out_3349973203036669[32] = 0; - out_3349973203036669[33] = 0; - out_3349973203036669[34] = 0; - out_3349973203036669[35] = 0; - out_3349973203036669[36] = 0; - out_3349973203036669[37] = 0; - out_3349973203036669[38] = 0; - out_3349973203036669[39] = 0; - out_3349973203036669[40] = 0; - out_3349973203036669[41] = 0; - out_3349973203036669[42] = 0; - out_3349973203036669[43] = 0; - out_3349973203036669[44] = 1; - out_3349973203036669[45] = 0; - out_3349973203036669[46] = 0; - out_3349973203036669[47] = 0; - out_3349973203036669[48] = 0; - out_3349973203036669[49] = 0; - out_3349973203036669[50] = 0; - out_3349973203036669[51] = 0; - out_3349973203036669[52] = 0; - out_3349973203036669[53] = 0; +void H_14(double *state, double *unused, double *out_6403526101164863262) { + out_6403526101164863262[0] = 0; + out_6403526101164863262[1] = 0; + out_6403526101164863262[2] = 0; + out_6403526101164863262[3] = 0; + out_6403526101164863262[4] = 0; + out_6403526101164863262[5] = 0; + out_6403526101164863262[6] = 1; + out_6403526101164863262[7] = 0; + out_6403526101164863262[8] = 0; + out_6403526101164863262[9] = 0; + out_6403526101164863262[10] = 0; + out_6403526101164863262[11] = 0; + out_6403526101164863262[12] = 0; + out_6403526101164863262[13] = 0; + out_6403526101164863262[14] = 0; + out_6403526101164863262[15] = 0; + out_6403526101164863262[16] = 0; + out_6403526101164863262[17] = 0; + out_6403526101164863262[18] = 0; + out_6403526101164863262[19] = 0; + out_6403526101164863262[20] = 0; + out_6403526101164863262[21] = 0; + out_6403526101164863262[22] = 0; + out_6403526101164863262[23] = 0; + out_6403526101164863262[24] = 0; + out_6403526101164863262[25] = 1; + out_6403526101164863262[26] = 0; + out_6403526101164863262[27] = 0; + out_6403526101164863262[28] = 0; + out_6403526101164863262[29] = 0; + out_6403526101164863262[30] = 0; + out_6403526101164863262[31] = 0; + out_6403526101164863262[32] = 0; + out_6403526101164863262[33] = 0; + out_6403526101164863262[34] = 0; + out_6403526101164863262[35] = 0; + out_6403526101164863262[36] = 0; + out_6403526101164863262[37] = 0; + out_6403526101164863262[38] = 0; + out_6403526101164863262[39] = 0; + out_6403526101164863262[40] = 0; + out_6403526101164863262[41] = 0; + out_6403526101164863262[42] = 0; + out_6403526101164863262[43] = 0; + out_6403526101164863262[44] = 1; + out_6403526101164863262[45] = 0; + out_6403526101164863262[46] = 0; + out_6403526101164863262[47] = 0; + out_6403526101164863262[48] = 0; + out_6403526101164863262[49] = 0; + out_6403526101164863262[50] = 0; + out_6403526101164863262[51] = 0; + out_6403526101164863262[52] = 0; + out_6403526101164863262[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_3212432162194349052) { - err_fun(nom_x, delta_x, out_3212432162194349052); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7389011504566069386) { + err_fun(nom_x, delta_x, out_7389011504566069386); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8973271523308122061) { - inv_err_fun(nom_x, true_x, out_8973271523308122061); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6795243368226050386) { + inv_err_fun(nom_x, true_x, out_6795243368226050386); } -void pose_H_mod_fun(double *state, double *out_7731698092980355432) { - H_mod_fun(state, out_7731698092980355432); +void pose_H_mod_fun(double *state, double *out_6387810172757071512) { + H_mod_fun(state, out_6387810172757071512); } -void pose_f_fun(double *state, double dt, double *out_8923016311880444304) { - f_fun(state, dt, out_8923016311880444304); +void pose_f_fun(double *state, double dt, double *out_5190191951972352761) { + f_fun(state, dt, out_5190191951972352761); } -void pose_F_fun(double *state, double dt, double *out_210116950988546947) { - F_fun(state, dt, out_210116950988546947); +void pose_F_fun(double *state, double dt, double *out_3425534515038953406) { + F_fun(state, dt, out_3425534515038953406); } -void pose_h_4(double *state, double *unused, double *out_7648624411257127562) { - h_4(state, unused, out_7648624411257127562); +void pose_h_4(double *state, double *unused, double *out_7576152197490450413) { + h_4(state, unused, out_7576152197490450413); } -void pose_H_4(double *state, double *unused, double *out_3959890883136447860) { - H_4(state, unused, out_3959890883136447860); +void pose_H_4(double *state, double *unused, double *out_3320737668869490966) { + H_4(state, unused, out_3320737668869490966); } -void pose_h_10(double *state, double *unused, double *out_6099133755902511145) { - h_10(state, unused, out_6099133755902511145); +void pose_h_10(double *state, double *unused, double *out_3130452913144701320) { + h_10(state, unused, out_3130452913144701320); } -void pose_H_10(double *state, double *unused, double *out_2569809615037315587) { - H_10(state, unused, out_2569809615037315587); +void pose_H_10(double *state, double *unused, double *out_2096220204218508864) { + H_10(state, unused, out_2096220204218508864); } -void pose_h_13(double *state, double *unused, double *out_2653699383913433253) { - h_13(state, unused, out_2653699383913433253); +void pose_h_13(double *state, double *unused, double *out_8767951916852656852) { + h_13(state, unused, out_8767951916852656852); } -void pose_H_13(double *state, double *unused, double *out_747617057804115059) { - H_13(state, unused, out_747617057804115059); +void pose_H_13(double *state, double *unused, double *out_108463843537158165) { + H_13(state, unused, out_108463843537158165); } -void pose_h_14(double *state, double *unused, double *out_7344654912082976031) { - h_14(state, unused, out_7344654912082976031); +void pose_h_14(double *state, double *unused, double *out_7181242506613011838) { + h_14(state, unused, out_7181242506613011838); } -void pose_H_14(double *state, double *unused, double *out_3349973203036669) { - H_14(state, unused, out_3349973203036669); +void pose_H_14(double *state, double *unused, double *out_6403526101164863262) { + H_14(state, unused, out_6403526101164863262); } 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 7d8159191..0e4053ac7 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_3212432162194349052); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8973271523308122061); -void pose_H_mod_fun(double *state, double *out_7731698092980355432); -void pose_f_fun(double *state, double dt, double *out_8923016311880444304); -void pose_F_fun(double *state, double dt, double *out_210116950988546947); -void pose_h_4(double *state, double *unused, double *out_7648624411257127562); -void pose_H_4(double *state, double *unused, double *out_3959890883136447860); -void pose_h_10(double *state, double *unused, double *out_6099133755902511145); -void pose_H_10(double *state, double *unused, double *out_2569809615037315587); -void pose_h_13(double *state, double *unused, double *out_2653699383913433253); -void pose_H_13(double *state, double *unused, double *out_747617057804115059); -void pose_h_14(double *state, double *unused, double *out_7344654912082976031); -void pose_H_14(double *state, double *unused, double *out_3349973203036669); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7389011504566069386); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6795243368226050386); +void pose_H_mod_fun(double *state, double *out_6387810172757071512); +void pose_f_fun(double *state, double dt, double *out_5190191951972352761); +void pose_F_fun(double *state, double dt, double *out_3425534515038953406); +void pose_h_4(double *state, double *unused, double *out_7576152197490450413); +void pose_H_4(double *state, double *unused, double *out_3320737668869490966); +void pose_h_10(double *state, double *unused, double *out_3130452913144701320); +void pose_H_10(double *state, double *unused, double *out_2096220204218508864); +void pose_h_13(double *state, double *unused, double *out_8767951916852656852); +void pose_H_13(double *state, double *unused, double *out_108463843537158165); +void pose_h_14(double *state, double *unused, double *out_7181242506613011838); +void pose_H_14(double *state, double *unused, double *out_6403526101164863262); 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 ccda39c52..e21ce7d02 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ