diff --git a/common/libcommon.a b/common/libcommon.a index 4f73c521f..62aa29761 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 199fe2c0d..b08ab3d2a 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index ee5912930..250578eef 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 b165e4819..c2420efd5 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 fae68cc76..59ad053ea 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 fc2c7403a..e052b4b36 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 d56104a6a..36a9f172a 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 0d03b02bd..da6a5545b 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 457c2ead4..b88a0f905 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 457c2ead4..b88a0f905 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 68c207be4..65e3a9322 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 0d03b02bd..da6a5545b 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 d02420ebf..25ae62b40 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-b78e4430-DEBUG"; +const uint8_t gitversion[19] = "DEV-7d8be707-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index a8442fcd2..3e7775481 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 4d53327fe..37be5913f 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 b732e2c8d..f9a7c99c3 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 a1c153cdb..b58d4cc14 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 a4f5cab0e..792675cc8 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 8d01bfb1e..3179d9183 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 3a570cb73..298620e3a 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 3b91f83b2..9ca537f3f 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 fbfe85b13..b939536a3 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 57c694d55..efea3b7ec 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 a694952eb..a41028e3f 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 dc097ea09..6fb5d4220 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 eb6bea684..703fb1632 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 2a954a650..0b12fe6d8 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 cef8bf9ab..b8968c3b4 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 cbbe3264e..170e73f72 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 11234b387..e46b8494b 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 91634a093..db3ee78db 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 32234aad3..8ce5c0bf7 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 57f101f00..7ed745330 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 245409de6..9f2838f54 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-b78e4430-DEBUG \ No newline at end of file +DEV-7d8be707-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 43e2970c5..c9f10efc5 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_7371356248284686223) { - out_7371356248284686223[0] = delta_x[0] + nom_x[0]; - out_7371356248284686223[1] = delta_x[1] + nom_x[1]; - out_7371356248284686223[2] = delta_x[2] + nom_x[2]; - out_7371356248284686223[3] = delta_x[3] + nom_x[3]; - out_7371356248284686223[4] = delta_x[4] + nom_x[4]; - out_7371356248284686223[5] = delta_x[5] + nom_x[5]; - out_7371356248284686223[6] = delta_x[6] + nom_x[6]; - out_7371356248284686223[7] = delta_x[7] + nom_x[7]; - out_7371356248284686223[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_3955673683859984289) { + out_3955673683859984289[0] = delta_x[0] + nom_x[0]; + out_3955673683859984289[1] = delta_x[1] + nom_x[1]; + out_3955673683859984289[2] = delta_x[2] + nom_x[2]; + out_3955673683859984289[3] = delta_x[3] + nom_x[3]; + out_3955673683859984289[4] = delta_x[4] + nom_x[4]; + out_3955673683859984289[5] = delta_x[5] + nom_x[5]; + out_3955673683859984289[6] = delta_x[6] + nom_x[6]; + out_3955673683859984289[7] = delta_x[7] + nom_x[7]; + out_3955673683859984289[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3965605218365756510) { - out_3965605218365756510[0] = -nom_x[0] + true_x[0]; - out_3965605218365756510[1] = -nom_x[1] + true_x[1]; - out_3965605218365756510[2] = -nom_x[2] + true_x[2]; - out_3965605218365756510[3] = -nom_x[3] + true_x[3]; - out_3965605218365756510[4] = -nom_x[4] + true_x[4]; - out_3965605218365756510[5] = -nom_x[5] + true_x[5]; - out_3965605218365756510[6] = -nom_x[6] + true_x[6]; - out_3965605218365756510[7] = -nom_x[7] + true_x[7]; - out_3965605218365756510[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8970258583381717138) { + out_8970258583381717138[0] = -nom_x[0] + true_x[0]; + out_8970258583381717138[1] = -nom_x[1] + true_x[1]; + out_8970258583381717138[2] = -nom_x[2] + true_x[2]; + out_8970258583381717138[3] = -nom_x[3] + true_x[3]; + out_8970258583381717138[4] = -nom_x[4] + true_x[4]; + out_8970258583381717138[5] = -nom_x[5] + true_x[5]; + out_8970258583381717138[6] = -nom_x[6] + true_x[6]; + out_8970258583381717138[7] = -nom_x[7] + true_x[7]; + out_8970258583381717138[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_2474093599384348200) { - out_2474093599384348200[0] = 1.0; - out_2474093599384348200[1] = 0.0; - out_2474093599384348200[2] = 0.0; - out_2474093599384348200[3] = 0.0; - out_2474093599384348200[4] = 0.0; - out_2474093599384348200[5] = 0.0; - out_2474093599384348200[6] = 0.0; - out_2474093599384348200[7] = 0.0; - out_2474093599384348200[8] = 0.0; - out_2474093599384348200[9] = 0.0; - out_2474093599384348200[10] = 1.0; - out_2474093599384348200[11] = 0.0; - out_2474093599384348200[12] = 0.0; - out_2474093599384348200[13] = 0.0; - out_2474093599384348200[14] = 0.0; - out_2474093599384348200[15] = 0.0; - out_2474093599384348200[16] = 0.0; - out_2474093599384348200[17] = 0.0; - out_2474093599384348200[18] = 0.0; - out_2474093599384348200[19] = 0.0; - out_2474093599384348200[20] = 1.0; - out_2474093599384348200[21] = 0.0; - out_2474093599384348200[22] = 0.0; - out_2474093599384348200[23] = 0.0; - out_2474093599384348200[24] = 0.0; - out_2474093599384348200[25] = 0.0; - out_2474093599384348200[26] = 0.0; - out_2474093599384348200[27] = 0.0; - out_2474093599384348200[28] = 0.0; - out_2474093599384348200[29] = 0.0; - out_2474093599384348200[30] = 1.0; - out_2474093599384348200[31] = 0.0; - out_2474093599384348200[32] = 0.0; - out_2474093599384348200[33] = 0.0; - out_2474093599384348200[34] = 0.0; - out_2474093599384348200[35] = 0.0; - out_2474093599384348200[36] = 0.0; - out_2474093599384348200[37] = 0.0; - out_2474093599384348200[38] = 0.0; - out_2474093599384348200[39] = 0.0; - out_2474093599384348200[40] = 1.0; - out_2474093599384348200[41] = 0.0; - out_2474093599384348200[42] = 0.0; - out_2474093599384348200[43] = 0.0; - out_2474093599384348200[44] = 0.0; - out_2474093599384348200[45] = 0.0; - out_2474093599384348200[46] = 0.0; - out_2474093599384348200[47] = 0.0; - out_2474093599384348200[48] = 0.0; - out_2474093599384348200[49] = 0.0; - out_2474093599384348200[50] = 1.0; - out_2474093599384348200[51] = 0.0; - out_2474093599384348200[52] = 0.0; - out_2474093599384348200[53] = 0.0; - out_2474093599384348200[54] = 0.0; - out_2474093599384348200[55] = 0.0; - out_2474093599384348200[56] = 0.0; - out_2474093599384348200[57] = 0.0; - out_2474093599384348200[58] = 0.0; - out_2474093599384348200[59] = 0.0; - out_2474093599384348200[60] = 1.0; - out_2474093599384348200[61] = 0.0; - out_2474093599384348200[62] = 0.0; - out_2474093599384348200[63] = 0.0; - out_2474093599384348200[64] = 0.0; - out_2474093599384348200[65] = 0.0; - out_2474093599384348200[66] = 0.0; - out_2474093599384348200[67] = 0.0; - out_2474093599384348200[68] = 0.0; - out_2474093599384348200[69] = 0.0; - out_2474093599384348200[70] = 1.0; - out_2474093599384348200[71] = 0.0; - out_2474093599384348200[72] = 0.0; - out_2474093599384348200[73] = 0.0; - out_2474093599384348200[74] = 0.0; - out_2474093599384348200[75] = 0.0; - out_2474093599384348200[76] = 0.0; - out_2474093599384348200[77] = 0.0; - out_2474093599384348200[78] = 0.0; - out_2474093599384348200[79] = 0.0; - out_2474093599384348200[80] = 1.0; +void H_mod_fun(double *state, double *out_4831252241531258142) { + out_4831252241531258142[0] = 1.0; + out_4831252241531258142[1] = 0.0; + out_4831252241531258142[2] = 0.0; + out_4831252241531258142[3] = 0.0; + out_4831252241531258142[4] = 0.0; + out_4831252241531258142[5] = 0.0; + out_4831252241531258142[6] = 0.0; + out_4831252241531258142[7] = 0.0; + out_4831252241531258142[8] = 0.0; + out_4831252241531258142[9] = 0.0; + out_4831252241531258142[10] = 1.0; + out_4831252241531258142[11] = 0.0; + out_4831252241531258142[12] = 0.0; + out_4831252241531258142[13] = 0.0; + out_4831252241531258142[14] = 0.0; + out_4831252241531258142[15] = 0.0; + out_4831252241531258142[16] = 0.0; + out_4831252241531258142[17] = 0.0; + out_4831252241531258142[18] = 0.0; + out_4831252241531258142[19] = 0.0; + out_4831252241531258142[20] = 1.0; + out_4831252241531258142[21] = 0.0; + out_4831252241531258142[22] = 0.0; + out_4831252241531258142[23] = 0.0; + out_4831252241531258142[24] = 0.0; + out_4831252241531258142[25] = 0.0; + out_4831252241531258142[26] = 0.0; + out_4831252241531258142[27] = 0.0; + out_4831252241531258142[28] = 0.0; + out_4831252241531258142[29] = 0.0; + out_4831252241531258142[30] = 1.0; + out_4831252241531258142[31] = 0.0; + out_4831252241531258142[32] = 0.0; + out_4831252241531258142[33] = 0.0; + out_4831252241531258142[34] = 0.0; + out_4831252241531258142[35] = 0.0; + out_4831252241531258142[36] = 0.0; + out_4831252241531258142[37] = 0.0; + out_4831252241531258142[38] = 0.0; + out_4831252241531258142[39] = 0.0; + out_4831252241531258142[40] = 1.0; + out_4831252241531258142[41] = 0.0; + out_4831252241531258142[42] = 0.0; + out_4831252241531258142[43] = 0.0; + out_4831252241531258142[44] = 0.0; + out_4831252241531258142[45] = 0.0; + out_4831252241531258142[46] = 0.0; + out_4831252241531258142[47] = 0.0; + out_4831252241531258142[48] = 0.0; + out_4831252241531258142[49] = 0.0; + out_4831252241531258142[50] = 1.0; + out_4831252241531258142[51] = 0.0; + out_4831252241531258142[52] = 0.0; + out_4831252241531258142[53] = 0.0; + out_4831252241531258142[54] = 0.0; + out_4831252241531258142[55] = 0.0; + out_4831252241531258142[56] = 0.0; + out_4831252241531258142[57] = 0.0; + out_4831252241531258142[58] = 0.0; + out_4831252241531258142[59] = 0.0; + out_4831252241531258142[60] = 1.0; + out_4831252241531258142[61] = 0.0; + out_4831252241531258142[62] = 0.0; + out_4831252241531258142[63] = 0.0; + out_4831252241531258142[64] = 0.0; + out_4831252241531258142[65] = 0.0; + out_4831252241531258142[66] = 0.0; + out_4831252241531258142[67] = 0.0; + out_4831252241531258142[68] = 0.0; + out_4831252241531258142[69] = 0.0; + out_4831252241531258142[70] = 1.0; + out_4831252241531258142[71] = 0.0; + out_4831252241531258142[72] = 0.0; + out_4831252241531258142[73] = 0.0; + out_4831252241531258142[74] = 0.0; + out_4831252241531258142[75] = 0.0; + out_4831252241531258142[76] = 0.0; + out_4831252241531258142[77] = 0.0; + out_4831252241531258142[78] = 0.0; + out_4831252241531258142[79] = 0.0; + out_4831252241531258142[80] = 1.0; } -void f_fun(double *state, double dt, double *out_2271815562821462998) { - out_2271815562821462998[0] = state[0]; - out_2271815562821462998[1] = state[1]; - out_2271815562821462998[2] = state[2]; - out_2271815562821462998[3] = state[3]; - out_2271815562821462998[4] = state[4]; - out_2271815562821462998[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_2271815562821462998[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_2271815562821462998[7] = state[7]; - out_2271815562821462998[8] = state[8]; +void f_fun(double *state, double dt, double *out_2508158103842919927) { + out_2508158103842919927[0] = state[0]; + out_2508158103842919927[1] = state[1]; + out_2508158103842919927[2] = state[2]; + out_2508158103842919927[3] = state[3]; + out_2508158103842919927[4] = state[4]; + out_2508158103842919927[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_2508158103842919927[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_2508158103842919927[7] = state[7]; + out_2508158103842919927[8] = state[8]; } -void F_fun(double *state, double dt, double *out_7600503497871632413) { - out_7600503497871632413[0] = 1; - out_7600503497871632413[1] = 0; - out_7600503497871632413[2] = 0; - out_7600503497871632413[3] = 0; - out_7600503497871632413[4] = 0; - out_7600503497871632413[5] = 0; - out_7600503497871632413[6] = 0; - out_7600503497871632413[7] = 0; - out_7600503497871632413[8] = 0; - out_7600503497871632413[9] = 0; - out_7600503497871632413[10] = 1; - out_7600503497871632413[11] = 0; - out_7600503497871632413[12] = 0; - out_7600503497871632413[13] = 0; - out_7600503497871632413[14] = 0; - out_7600503497871632413[15] = 0; - out_7600503497871632413[16] = 0; - out_7600503497871632413[17] = 0; - out_7600503497871632413[18] = 0; - out_7600503497871632413[19] = 0; - out_7600503497871632413[20] = 1; - out_7600503497871632413[21] = 0; - out_7600503497871632413[22] = 0; - out_7600503497871632413[23] = 0; - out_7600503497871632413[24] = 0; - out_7600503497871632413[25] = 0; - out_7600503497871632413[26] = 0; - out_7600503497871632413[27] = 0; - out_7600503497871632413[28] = 0; - out_7600503497871632413[29] = 0; - out_7600503497871632413[30] = 1; - out_7600503497871632413[31] = 0; - out_7600503497871632413[32] = 0; - out_7600503497871632413[33] = 0; - out_7600503497871632413[34] = 0; - out_7600503497871632413[35] = 0; - out_7600503497871632413[36] = 0; - out_7600503497871632413[37] = 0; - out_7600503497871632413[38] = 0; - out_7600503497871632413[39] = 0; - out_7600503497871632413[40] = 1; - out_7600503497871632413[41] = 0; - out_7600503497871632413[42] = 0; - out_7600503497871632413[43] = 0; - out_7600503497871632413[44] = 0; - out_7600503497871632413[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_7600503497871632413[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_7600503497871632413[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_7600503497871632413[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_7600503497871632413[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_7600503497871632413[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_7600503497871632413[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_7600503497871632413[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_7600503497871632413[53] = -9.8100000000000005*dt; - out_7600503497871632413[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_7600503497871632413[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_7600503497871632413[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7600503497871632413[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7600503497871632413[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_7600503497871632413[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_7600503497871632413[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_7600503497871632413[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7600503497871632413[62] = 0; - out_7600503497871632413[63] = 0; - out_7600503497871632413[64] = 0; - out_7600503497871632413[65] = 0; - out_7600503497871632413[66] = 0; - out_7600503497871632413[67] = 0; - out_7600503497871632413[68] = 0; - out_7600503497871632413[69] = 0; - out_7600503497871632413[70] = 1; - out_7600503497871632413[71] = 0; - out_7600503497871632413[72] = 0; - out_7600503497871632413[73] = 0; - out_7600503497871632413[74] = 0; - out_7600503497871632413[75] = 0; - out_7600503497871632413[76] = 0; - out_7600503497871632413[77] = 0; - out_7600503497871632413[78] = 0; - out_7600503497871632413[79] = 0; - out_7600503497871632413[80] = 1; +void F_fun(double *state, double dt, double *out_619119889307979727) { + out_619119889307979727[0] = 1; + out_619119889307979727[1] = 0; + out_619119889307979727[2] = 0; + out_619119889307979727[3] = 0; + out_619119889307979727[4] = 0; + out_619119889307979727[5] = 0; + out_619119889307979727[6] = 0; + out_619119889307979727[7] = 0; + out_619119889307979727[8] = 0; + out_619119889307979727[9] = 0; + out_619119889307979727[10] = 1; + out_619119889307979727[11] = 0; + out_619119889307979727[12] = 0; + out_619119889307979727[13] = 0; + out_619119889307979727[14] = 0; + out_619119889307979727[15] = 0; + out_619119889307979727[16] = 0; + out_619119889307979727[17] = 0; + out_619119889307979727[18] = 0; + out_619119889307979727[19] = 0; + out_619119889307979727[20] = 1; + out_619119889307979727[21] = 0; + out_619119889307979727[22] = 0; + out_619119889307979727[23] = 0; + out_619119889307979727[24] = 0; + out_619119889307979727[25] = 0; + out_619119889307979727[26] = 0; + out_619119889307979727[27] = 0; + out_619119889307979727[28] = 0; + out_619119889307979727[29] = 0; + out_619119889307979727[30] = 1; + out_619119889307979727[31] = 0; + out_619119889307979727[32] = 0; + out_619119889307979727[33] = 0; + out_619119889307979727[34] = 0; + out_619119889307979727[35] = 0; + out_619119889307979727[36] = 0; + out_619119889307979727[37] = 0; + out_619119889307979727[38] = 0; + out_619119889307979727[39] = 0; + out_619119889307979727[40] = 1; + out_619119889307979727[41] = 0; + out_619119889307979727[42] = 0; + out_619119889307979727[43] = 0; + out_619119889307979727[44] = 0; + out_619119889307979727[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_619119889307979727[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_619119889307979727[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_619119889307979727[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_619119889307979727[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_619119889307979727[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_619119889307979727[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_619119889307979727[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_619119889307979727[53] = -9.8100000000000005*dt; + out_619119889307979727[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_619119889307979727[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_619119889307979727[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_619119889307979727[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_619119889307979727[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_619119889307979727[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_619119889307979727[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_619119889307979727[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_619119889307979727[62] = 0; + out_619119889307979727[63] = 0; + out_619119889307979727[64] = 0; + out_619119889307979727[65] = 0; + out_619119889307979727[66] = 0; + out_619119889307979727[67] = 0; + out_619119889307979727[68] = 0; + out_619119889307979727[69] = 0; + out_619119889307979727[70] = 1; + out_619119889307979727[71] = 0; + out_619119889307979727[72] = 0; + out_619119889307979727[73] = 0; + out_619119889307979727[74] = 0; + out_619119889307979727[75] = 0; + out_619119889307979727[76] = 0; + out_619119889307979727[77] = 0; + out_619119889307979727[78] = 0; + out_619119889307979727[79] = 0; + out_619119889307979727[80] = 1; } -void h_25(double *state, double *unused, double *out_2245281936356175607) { - out_2245281936356175607[0] = state[6]; +void h_25(double *state, double *unused, double *out_7300303321148015441) { + out_7300303321148015441[0] = state[6]; } -void H_25(double *state, double *unused, double *out_7377785424569013040) { - out_7377785424569013040[0] = 0; - out_7377785424569013040[1] = 0; - out_7377785424569013040[2] = 0; - out_7377785424569013040[3] = 0; - out_7377785424569013040[4] = 0; - out_7377785424569013040[5] = 0; - out_7377785424569013040[6] = 1; - out_7377785424569013040[7] = 0; - out_7377785424569013040[8] = 0; +void H_25(double *state, double *unused, double *out_3882131319502775933) { + out_3882131319502775933[0] = 0; + out_3882131319502775933[1] = 0; + out_3882131319502775933[2] = 0; + out_3882131319502775933[3] = 0; + out_3882131319502775933[4] = 0; + out_3882131319502775933[5] = 0; + out_3882131319502775933[6] = 1; + out_3882131319502775933[7] = 0; + out_3882131319502775933[8] = 0; } -void h_24(double *state, double *unused, double *out_5229861498965331368) { - out_5229861498965331368[0] = state[4]; - out_5229861498965331368[1] = state[5]; +void h_24(double *state, double *unused, double *out_7158441455227347179) { + out_7158441455227347179[0] = state[4]; + out_7158441455227347179[1] = state[5]; } -void H_24(double *state, double *unused, double *out_4493386842549020475) { - out_4493386842549020475[0] = 0; - out_4493386842549020475[1] = 0; - out_4493386842549020475[2] = 0; - out_4493386842549020475[3] = 0; - out_4493386842549020475[4] = 1; - out_4493386842549020475[5] = 0; - out_4493386842549020475[6] = 0; - out_4493386842549020475[7] = 0; - out_4493386842549020475[8] = 0; - out_4493386842549020475[9] = 0; - out_4493386842549020475[10] = 0; - out_4493386842549020475[11] = 0; - out_4493386842549020475[12] = 0; - out_4493386842549020475[13] = 0; - out_4493386842549020475[14] = 1; - out_4493386842549020475[15] = 0; - out_4493386842549020475[16] = 0; - out_4493386842549020475[17] = 0; +void H_24(double *state, double *unused, double *out_1656423535523907371) { + out_1656423535523907371[0] = 0; + out_1656423535523907371[1] = 0; + out_1656423535523907371[2] = 0; + out_1656423535523907371[3] = 0; + out_1656423535523907371[4] = 1; + out_1656423535523907371[5] = 0; + out_1656423535523907371[6] = 0; + out_1656423535523907371[7] = 0; + out_1656423535523907371[8] = 0; + out_1656423535523907371[9] = 0; + out_1656423535523907371[10] = 0; + out_1656423535523907371[11] = 0; + out_1656423535523907371[12] = 0; + out_1656423535523907371[13] = 0; + out_1656423535523907371[14] = 1; + out_1656423535523907371[15] = 0; + out_1656423535523907371[16] = 0; + out_1656423535523907371[17] = 0; } -void h_30(double *state, double *unused, double *out_9036311057795238262) { - out_9036311057795238262[0] = state[4]; +void h_30(double *state, double *unused, double *out_7508991224307513825) { + out_7508991224307513825[0] = state[4]; } -void H_30(double *state, double *unused, double *out_8550625690633289949) { - out_8550625690633289949[0] = 0; - out_8550625690633289949[1] = 0; - out_8550625690633289949[2] = 0; - out_8550625690633289949[3] = 0; - out_8550625690633289949[4] = 1; - out_8550625690633289949[5] = 0; - out_8550625690633289949[6] = 0; - out_8550625690633289949[7] = 0; - out_8550625690633289949[8] = 0; +void H_30(double *state, double *unused, double *out_3034559021988840822) { + out_3034559021988840822[0] = 0; + out_3034559021988840822[1] = 0; + out_3034559021988840822[2] = 0; + out_3034559021988840822[3] = 0; + out_3034559021988840822[4] = 1; + out_3034559021988840822[5] = 0; + out_3034559021988840822[6] = 0; + out_3034559021988840822[7] = 0; + out_3034559021988840822[8] = 0; } -void h_26(double *state, double *unused, double *out_8466923163012650919) { - out_8466923163012650919[0] = state[7]; +void h_26(double *state, double *unused, double *out_5818543389571832865) { + out_5818543389571832865[0] = state[7]; } -void H_26(double *state, double *unused, double *out_7764432679379737975) { - out_7764432679379737975[0] = 0; - out_7764432679379737975[1] = 0; - out_7764432679379737975[2] = 0; - out_7764432679379737975[3] = 0; - out_7764432679379737975[4] = 0; - out_7764432679379737975[5] = 0; - out_7764432679379737975[6] = 0; - out_7764432679379737975[7] = 1; - out_7764432679379737975[8] = 0; +void H_26(double *state, double *unused, double *out_7623634638376832157) { + out_7623634638376832157[0] = 0; + out_7623634638376832157[1] = 0; + out_7623634638376832157[2] = 0; + out_7623634638376832157[3] = 0; + out_7623634638376832157[4] = 0; + out_7623634638376832157[5] = 0; + out_7623634638376832157[6] = 0; + out_7623634638376832157[7] = 1; + out_7623634638376832157[8] = 0; } -void h_27(double *state, double *unused, double *out_2036281904887484901) { - out_2036281904887484901[0] = state[3]; +void h_27(double *state, double *unused, double *out_4263517269703682756) { + out_4263517269703682756[0] = state[3]; } -void H_27(double *state, double *unused, double *out_6327031619449346732) { - out_6327031619449346732[0] = 0; - out_6327031619449346732[1] = 0; - out_6327031619449346732[2] = 0; - out_6327031619449346732[3] = 1; - out_6327031619449346732[4] = 0; - out_6327031619449346732[5] = 0; - out_6327031619449346732[6] = 0; - out_6327031619449346732[7] = 0; - out_6327031619449346732[8] = 0; +void H_27(double *state, double *unused, double *out_859795710188415911) { + out_859795710188415911[0] = 0; + out_859795710188415911[1] = 0; + out_859795710188415911[2] = 0; + out_859795710188415911[3] = 1; + out_859795710188415911[4] = 0; + out_859795710188415911[5] = 0; + out_859795710188415911[6] = 0; + out_859795710188415911[7] = 0; + out_859795710188415911[8] = 0; } -void h_29(double *state, double *unused, double *out_7562339341438508014) { - out_7562339341438508014[0] = state[1]; +void h_29(double *state, double *unused, double *out_2507317956646668180) { + out_2507317956646668180[0] = state[1]; } -void H_29(double *state, double *unused, double *out_8040394346318897765) { - out_8040394346318897765[0] = 0; - out_8040394346318897765[1] = 1; - out_8040394346318897765[2] = 0; - out_8040394346318897765[3] = 0; - out_8040394346318897765[4] = 0; - out_8040394346318897765[5] = 0; - out_8040394346318897765[6] = 0; - out_8040394346318897765[7] = 0; - out_8040394346318897765[8] = 0; +void H_29(double *state, double *unused, double *out_3544790366303233006) { + out_3544790366303233006[0] = 0; + out_3544790366303233006[1] = 1; + out_3544790366303233006[2] = 0; + out_3544790366303233006[3] = 0; + out_3544790366303233006[4] = 0; + out_3544790366303233006[5] = 0; + out_3544790366303233006[6] = 0; + out_3544790366303233006[7] = 0; + out_3544790366303233006[8] = 0; } -void h_28(double *state, double *unused, double *out_2481951949823838038) { - out_2481951949823838038[0] = state[0]; +void h_28(double *state, double *unused, double *out_8661469350985311768) { + out_8661469350985311768[0] = state[0]; } -void H_28(double *state, double *unused, double *out_5323950710321123277) { - out_5323950710321123277[0] = 1; - out_5323950710321123277[1] = 0; - out_5323950710321123277[2] = 0; - out_5323950710321123277[3] = 0; - out_5323950710321123277[4] = 0; - out_5323950710321123277[5] = 0; - out_5323950710321123277[6] = 0; - out_5323950710321123277[7] = 0; - out_5323950710321123277[8] = 0; +void H_28(double *state, double *unused, double *out_1537608650766297568) { + out_1537608650766297568[0] = 1; + out_1537608650766297568[1] = 0; + out_1537608650766297568[2] = 0; + out_1537608650766297568[3] = 0; + out_1537608650766297568[4] = 0; + out_1537608650766297568[5] = 0; + out_1537608650766297568[6] = 0; + out_1537608650766297568[7] = 0; + out_1537608650766297568[8] = 0; } -void h_31(double *state, double *unused, double *out_5123667467256710023) { - out_5123667467256710023[0] = state[8]; +void h_31(double *state, double *unused, double *out_7025109258863509552) { + out_7025109258863509552[0] = state[8]; } -void H_31(double *state, double *unused, double *out_7408431386445973468) { - out_7408431386445973468[0] = 0; - out_7408431386445973468[1] = 0; - out_7408431386445973468[2] = 0; - out_7408431386445973468[3] = 0; - out_7408431386445973468[4] = 0; - out_7408431386445973468[5] = 0; - out_7408431386445973468[6] = 0; - out_7408431386445973468[7] = 0; - out_7408431386445973468[8] = 1; +void H_31(double *state, double *unused, double *out_3851485357625815505) { + out_3851485357625815505[0] = 0; + out_3851485357625815505[1] = 0; + out_3851485357625815505[2] = 0; + out_3851485357625815505[3] = 0; + out_3851485357625815505[4] = 0; + out_3851485357625815505[5] = 0; + out_3851485357625815505[6] = 0; + out_3851485357625815505[7] = 0; + out_3851485357625815505[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_7371356248284686223) { - err_fun(nom_x, delta_x, out_7371356248284686223); +void car_err_fun(double *nom_x, double *delta_x, double *out_3955673683859984289) { + err_fun(nom_x, delta_x, out_3955673683859984289); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3965605218365756510) { - inv_err_fun(nom_x, true_x, out_3965605218365756510); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8970258583381717138) { + inv_err_fun(nom_x, true_x, out_8970258583381717138); } -void car_H_mod_fun(double *state, double *out_2474093599384348200) { - H_mod_fun(state, out_2474093599384348200); +void car_H_mod_fun(double *state, double *out_4831252241531258142) { + H_mod_fun(state, out_4831252241531258142); } -void car_f_fun(double *state, double dt, double *out_2271815562821462998) { - f_fun(state, dt, out_2271815562821462998); +void car_f_fun(double *state, double dt, double *out_2508158103842919927) { + f_fun(state, dt, out_2508158103842919927); } -void car_F_fun(double *state, double dt, double *out_7600503497871632413) { - F_fun(state, dt, out_7600503497871632413); +void car_F_fun(double *state, double dt, double *out_619119889307979727) { + F_fun(state, dt, out_619119889307979727); } -void car_h_25(double *state, double *unused, double *out_2245281936356175607) { - h_25(state, unused, out_2245281936356175607); +void car_h_25(double *state, double *unused, double *out_7300303321148015441) { + h_25(state, unused, out_7300303321148015441); } -void car_H_25(double *state, double *unused, double *out_7377785424569013040) { - H_25(state, unused, out_7377785424569013040); +void car_H_25(double *state, double *unused, double *out_3882131319502775933) { + H_25(state, unused, out_3882131319502775933); } -void car_h_24(double *state, double *unused, double *out_5229861498965331368) { - h_24(state, unused, out_5229861498965331368); +void car_h_24(double *state, double *unused, double *out_7158441455227347179) { + h_24(state, unused, out_7158441455227347179); } -void car_H_24(double *state, double *unused, double *out_4493386842549020475) { - H_24(state, unused, out_4493386842549020475); +void car_H_24(double *state, double *unused, double *out_1656423535523907371) { + H_24(state, unused, out_1656423535523907371); } -void car_h_30(double *state, double *unused, double *out_9036311057795238262) { - h_30(state, unused, out_9036311057795238262); +void car_h_30(double *state, double *unused, double *out_7508991224307513825) { + h_30(state, unused, out_7508991224307513825); } -void car_H_30(double *state, double *unused, double *out_8550625690633289949) { - H_30(state, unused, out_8550625690633289949); +void car_H_30(double *state, double *unused, double *out_3034559021988840822) { + H_30(state, unused, out_3034559021988840822); } -void car_h_26(double *state, double *unused, double *out_8466923163012650919) { - h_26(state, unused, out_8466923163012650919); +void car_h_26(double *state, double *unused, double *out_5818543389571832865) { + h_26(state, unused, out_5818543389571832865); } -void car_H_26(double *state, double *unused, double *out_7764432679379737975) { - H_26(state, unused, out_7764432679379737975); +void car_H_26(double *state, double *unused, double *out_7623634638376832157) { + H_26(state, unused, out_7623634638376832157); } -void car_h_27(double *state, double *unused, double *out_2036281904887484901) { - h_27(state, unused, out_2036281904887484901); +void car_h_27(double *state, double *unused, double *out_4263517269703682756) { + h_27(state, unused, out_4263517269703682756); } -void car_H_27(double *state, double *unused, double *out_6327031619449346732) { - H_27(state, unused, out_6327031619449346732); +void car_H_27(double *state, double *unused, double *out_859795710188415911) { + H_27(state, unused, out_859795710188415911); } -void car_h_29(double *state, double *unused, double *out_7562339341438508014) { - h_29(state, unused, out_7562339341438508014); +void car_h_29(double *state, double *unused, double *out_2507317956646668180) { + h_29(state, unused, out_2507317956646668180); } -void car_H_29(double *state, double *unused, double *out_8040394346318897765) { - H_29(state, unused, out_8040394346318897765); +void car_H_29(double *state, double *unused, double *out_3544790366303233006) { + H_29(state, unused, out_3544790366303233006); } -void car_h_28(double *state, double *unused, double *out_2481951949823838038) { - h_28(state, unused, out_2481951949823838038); +void car_h_28(double *state, double *unused, double *out_8661469350985311768) { + h_28(state, unused, out_8661469350985311768); } -void car_H_28(double *state, double *unused, double *out_5323950710321123277) { - H_28(state, unused, out_5323950710321123277); +void car_H_28(double *state, double *unused, double *out_1537608650766297568) { + H_28(state, unused, out_1537608650766297568); } -void car_h_31(double *state, double *unused, double *out_5123667467256710023) { - h_31(state, unused, out_5123667467256710023); +void car_h_31(double *state, double *unused, double *out_7025109258863509552) { + h_31(state, unused, out_7025109258863509552); } -void car_H_31(double *state, double *unused, double *out_7408431386445973468) { - H_31(state, unused, out_7408431386445973468); +void car_H_31(double *state, double *unused, double *out_3851485357625815505) { + H_31(state, unused, out_3851485357625815505); } 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 0836bbc23..c4033c327 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_7371356248284686223); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3965605218365756510); -void car_H_mod_fun(double *state, double *out_2474093599384348200); -void car_f_fun(double *state, double dt, double *out_2271815562821462998); -void car_F_fun(double *state, double dt, double *out_7600503497871632413); -void car_h_25(double *state, double *unused, double *out_2245281936356175607); -void car_H_25(double *state, double *unused, double *out_7377785424569013040); -void car_h_24(double *state, double *unused, double *out_5229861498965331368); -void car_H_24(double *state, double *unused, double *out_4493386842549020475); -void car_h_30(double *state, double *unused, double *out_9036311057795238262); -void car_H_30(double *state, double *unused, double *out_8550625690633289949); -void car_h_26(double *state, double *unused, double *out_8466923163012650919); -void car_H_26(double *state, double *unused, double *out_7764432679379737975); -void car_h_27(double *state, double *unused, double *out_2036281904887484901); -void car_H_27(double *state, double *unused, double *out_6327031619449346732); -void car_h_29(double *state, double *unused, double *out_7562339341438508014); -void car_H_29(double *state, double *unused, double *out_8040394346318897765); -void car_h_28(double *state, double *unused, double *out_2481951949823838038); -void car_H_28(double *state, double *unused, double *out_5323950710321123277); -void car_h_31(double *state, double *unused, double *out_5123667467256710023); -void car_H_31(double *state, double *unused, double *out_7408431386445973468); +void car_err_fun(double *nom_x, double *delta_x, double *out_3955673683859984289); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8970258583381717138); +void car_H_mod_fun(double *state, double *out_4831252241531258142); +void car_f_fun(double *state, double dt, double *out_2508158103842919927); +void car_F_fun(double *state, double dt, double *out_619119889307979727); +void car_h_25(double *state, double *unused, double *out_7300303321148015441); +void car_H_25(double *state, double *unused, double *out_3882131319502775933); +void car_h_24(double *state, double *unused, double *out_7158441455227347179); +void car_H_24(double *state, double *unused, double *out_1656423535523907371); +void car_h_30(double *state, double *unused, double *out_7508991224307513825); +void car_H_30(double *state, double *unused, double *out_3034559021988840822); +void car_h_26(double *state, double *unused, double *out_5818543389571832865); +void car_H_26(double *state, double *unused, double *out_7623634638376832157); +void car_h_27(double *state, double *unused, double *out_4263517269703682756); +void car_H_27(double *state, double *unused, double *out_859795710188415911); +void car_h_29(double *state, double *unused, double *out_2507317956646668180); +void car_H_29(double *state, double *unused, double *out_3544790366303233006); +void car_h_28(double *state, double *unused, double *out_8661469350985311768); +void car_H_28(double *state, double *unused, double *out_1537608650766297568); +void car_h_31(double *state, double *unused, double *out_7025109258863509552); +void car_H_31(double *state, double *unused, double *out_3851485357625815505); 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 d771745e5..ae4474137 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_8940459122146932400) { - out_8940459122146932400[0] = delta_x[0] + nom_x[0]; - out_8940459122146932400[1] = delta_x[1] + nom_x[1]; - out_8940459122146932400[2] = delta_x[2] + nom_x[2]; - out_8940459122146932400[3] = delta_x[3] + nom_x[3]; - out_8940459122146932400[4] = delta_x[4] + nom_x[4]; - out_8940459122146932400[5] = delta_x[5] + nom_x[5]; - out_8940459122146932400[6] = delta_x[6] + nom_x[6]; - out_8940459122146932400[7] = delta_x[7] + nom_x[7]; - out_8940459122146932400[8] = delta_x[8] + nom_x[8]; - out_8940459122146932400[9] = delta_x[9] + nom_x[9]; - out_8940459122146932400[10] = delta_x[10] + nom_x[10]; - out_8940459122146932400[11] = delta_x[11] + nom_x[11]; - out_8940459122146932400[12] = delta_x[12] + nom_x[12]; - out_8940459122146932400[13] = delta_x[13] + nom_x[13]; - out_8940459122146932400[14] = delta_x[14] + nom_x[14]; - out_8940459122146932400[15] = delta_x[15] + nom_x[15]; - out_8940459122146932400[16] = delta_x[16] + nom_x[16]; - out_8940459122146932400[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7536058918625145808) { + out_7536058918625145808[0] = delta_x[0] + nom_x[0]; + out_7536058918625145808[1] = delta_x[1] + nom_x[1]; + out_7536058918625145808[2] = delta_x[2] + nom_x[2]; + out_7536058918625145808[3] = delta_x[3] + nom_x[3]; + out_7536058918625145808[4] = delta_x[4] + nom_x[4]; + out_7536058918625145808[5] = delta_x[5] + nom_x[5]; + out_7536058918625145808[6] = delta_x[6] + nom_x[6]; + out_7536058918625145808[7] = delta_x[7] + nom_x[7]; + out_7536058918625145808[8] = delta_x[8] + nom_x[8]; + out_7536058918625145808[9] = delta_x[9] + nom_x[9]; + out_7536058918625145808[10] = delta_x[10] + nom_x[10]; + out_7536058918625145808[11] = delta_x[11] + nom_x[11]; + out_7536058918625145808[12] = delta_x[12] + nom_x[12]; + out_7536058918625145808[13] = delta_x[13] + nom_x[13]; + out_7536058918625145808[14] = delta_x[14] + nom_x[14]; + out_7536058918625145808[15] = delta_x[15] + nom_x[15]; + out_7536058918625145808[16] = delta_x[16] + nom_x[16]; + out_7536058918625145808[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3384987028755125218) { - out_3384987028755125218[0] = -nom_x[0] + true_x[0]; - out_3384987028755125218[1] = -nom_x[1] + true_x[1]; - out_3384987028755125218[2] = -nom_x[2] + true_x[2]; - out_3384987028755125218[3] = -nom_x[3] + true_x[3]; - out_3384987028755125218[4] = -nom_x[4] + true_x[4]; - out_3384987028755125218[5] = -nom_x[5] + true_x[5]; - out_3384987028755125218[6] = -nom_x[6] + true_x[6]; - out_3384987028755125218[7] = -nom_x[7] + true_x[7]; - out_3384987028755125218[8] = -nom_x[8] + true_x[8]; - out_3384987028755125218[9] = -nom_x[9] + true_x[9]; - out_3384987028755125218[10] = -nom_x[10] + true_x[10]; - out_3384987028755125218[11] = -nom_x[11] + true_x[11]; - out_3384987028755125218[12] = -nom_x[12] + true_x[12]; - out_3384987028755125218[13] = -nom_x[13] + true_x[13]; - out_3384987028755125218[14] = -nom_x[14] + true_x[14]; - out_3384987028755125218[15] = -nom_x[15] + true_x[15]; - out_3384987028755125218[16] = -nom_x[16] + true_x[16]; - out_3384987028755125218[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6294980525542920357) { + out_6294980525542920357[0] = -nom_x[0] + true_x[0]; + out_6294980525542920357[1] = -nom_x[1] + true_x[1]; + out_6294980525542920357[2] = -nom_x[2] + true_x[2]; + out_6294980525542920357[3] = -nom_x[3] + true_x[3]; + out_6294980525542920357[4] = -nom_x[4] + true_x[4]; + out_6294980525542920357[5] = -nom_x[5] + true_x[5]; + out_6294980525542920357[6] = -nom_x[6] + true_x[6]; + out_6294980525542920357[7] = -nom_x[7] + true_x[7]; + out_6294980525542920357[8] = -nom_x[8] + true_x[8]; + out_6294980525542920357[9] = -nom_x[9] + true_x[9]; + out_6294980525542920357[10] = -nom_x[10] + true_x[10]; + out_6294980525542920357[11] = -nom_x[11] + true_x[11]; + out_6294980525542920357[12] = -nom_x[12] + true_x[12]; + out_6294980525542920357[13] = -nom_x[13] + true_x[13]; + out_6294980525542920357[14] = -nom_x[14] + true_x[14]; + out_6294980525542920357[15] = -nom_x[15] + true_x[15]; + out_6294980525542920357[16] = -nom_x[16] + true_x[16]; + out_6294980525542920357[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_5245124330854285834) { - out_5245124330854285834[0] = 1.0; - out_5245124330854285834[1] = 0.0; - out_5245124330854285834[2] = 0.0; - out_5245124330854285834[3] = 0.0; - out_5245124330854285834[4] = 0.0; - out_5245124330854285834[5] = 0.0; - out_5245124330854285834[6] = 0.0; - out_5245124330854285834[7] = 0.0; - out_5245124330854285834[8] = 0.0; - out_5245124330854285834[9] = 0.0; - out_5245124330854285834[10] = 0.0; - out_5245124330854285834[11] = 0.0; - out_5245124330854285834[12] = 0.0; - out_5245124330854285834[13] = 0.0; - out_5245124330854285834[14] = 0.0; - out_5245124330854285834[15] = 0.0; - out_5245124330854285834[16] = 0.0; - out_5245124330854285834[17] = 0.0; - out_5245124330854285834[18] = 0.0; - out_5245124330854285834[19] = 1.0; - out_5245124330854285834[20] = 0.0; - out_5245124330854285834[21] = 0.0; - out_5245124330854285834[22] = 0.0; - out_5245124330854285834[23] = 0.0; - out_5245124330854285834[24] = 0.0; - out_5245124330854285834[25] = 0.0; - out_5245124330854285834[26] = 0.0; - out_5245124330854285834[27] = 0.0; - out_5245124330854285834[28] = 0.0; - out_5245124330854285834[29] = 0.0; - out_5245124330854285834[30] = 0.0; - out_5245124330854285834[31] = 0.0; - out_5245124330854285834[32] = 0.0; - out_5245124330854285834[33] = 0.0; - out_5245124330854285834[34] = 0.0; - out_5245124330854285834[35] = 0.0; - out_5245124330854285834[36] = 0.0; - out_5245124330854285834[37] = 0.0; - out_5245124330854285834[38] = 1.0; - out_5245124330854285834[39] = 0.0; - out_5245124330854285834[40] = 0.0; - out_5245124330854285834[41] = 0.0; - out_5245124330854285834[42] = 0.0; - out_5245124330854285834[43] = 0.0; - out_5245124330854285834[44] = 0.0; - out_5245124330854285834[45] = 0.0; - out_5245124330854285834[46] = 0.0; - out_5245124330854285834[47] = 0.0; - out_5245124330854285834[48] = 0.0; - out_5245124330854285834[49] = 0.0; - out_5245124330854285834[50] = 0.0; - out_5245124330854285834[51] = 0.0; - out_5245124330854285834[52] = 0.0; - out_5245124330854285834[53] = 0.0; - out_5245124330854285834[54] = 0.0; - out_5245124330854285834[55] = 0.0; - out_5245124330854285834[56] = 0.0; - out_5245124330854285834[57] = 1.0; - out_5245124330854285834[58] = 0.0; - out_5245124330854285834[59] = 0.0; - out_5245124330854285834[60] = 0.0; - out_5245124330854285834[61] = 0.0; - out_5245124330854285834[62] = 0.0; - out_5245124330854285834[63] = 0.0; - out_5245124330854285834[64] = 0.0; - out_5245124330854285834[65] = 0.0; - out_5245124330854285834[66] = 0.0; - out_5245124330854285834[67] = 0.0; - out_5245124330854285834[68] = 0.0; - out_5245124330854285834[69] = 0.0; - out_5245124330854285834[70] = 0.0; - out_5245124330854285834[71] = 0.0; - out_5245124330854285834[72] = 0.0; - out_5245124330854285834[73] = 0.0; - out_5245124330854285834[74] = 0.0; - out_5245124330854285834[75] = 0.0; - out_5245124330854285834[76] = 1.0; - out_5245124330854285834[77] = 0.0; - out_5245124330854285834[78] = 0.0; - out_5245124330854285834[79] = 0.0; - out_5245124330854285834[80] = 0.0; - out_5245124330854285834[81] = 0.0; - out_5245124330854285834[82] = 0.0; - out_5245124330854285834[83] = 0.0; - out_5245124330854285834[84] = 0.0; - out_5245124330854285834[85] = 0.0; - out_5245124330854285834[86] = 0.0; - out_5245124330854285834[87] = 0.0; - out_5245124330854285834[88] = 0.0; - out_5245124330854285834[89] = 0.0; - out_5245124330854285834[90] = 0.0; - out_5245124330854285834[91] = 0.0; - out_5245124330854285834[92] = 0.0; - out_5245124330854285834[93] = 0.0; - out_5245124330854285834[94] = 0.0; - out_5245124330854285834[95] = 1.0; - out_5245124330854285834[96] = 0.0; - out_5245124330854285834[97] = 0.0; - out_5245124330854285834[98] = 0.0; - out_5245124330854285834[99] = 0.0; - out_5245124330854285834[100] = 0.0; - out_5245124330854285834[101] = 0.0; - out_5245124330854285834[102] = 0.0; - out_5245124330854285834[103] = 0.0; - out_5245124330854285834[104] = 0.0; - out_5245124330854285834[105] = 0.0; - out_5245124330854285834[106] = 0.0; - out_5245124330854285834[107] = 0.0; - out_5245124330854285834[108] = 0.0; - out_5245124330854285834[109] = 0.0; - out_5245124330854285834[110] = 0.0; - out_5245124330854285834[111] = 0.0; - out_5245124330854285834[112] = 0.0; - out_5245124330854285834[113] = 0.0; - out_5245124330854285834[114] = 1.0; - out_5245124330854285834[115] = 0.0; - out_5245124330854285834[116] = 0.0; - out_5245124330854285834[117] = 0.0; - out_5245124330854285834[118] = 0.0; - out_5245124330854285834[119] = 0.0; - out_5245124330854285834[120] = 0.0; - out_5245124330854285834[121] = 0.0; - out_5245124330854285834[122] = 0.0; - out_5245124330854285834[123] = 0.0; - out_5245124330854285834[124] = 0.0; - out_5245124330854285834[125] = 0.0; - out_5245124330854285834[126] = 0.0; - out_5245124330854285834[127] = 0.0; - out_5245124330854285834[128] = 0.0; - out_5245124330854285834[129] = 0.0; - out_5245124330854285834[130] = 0.0; - out_5245124330854285834[131] = 0.0; - out_5245124330854285834[132] = 0.0; - out_5245124330854285834[133] = 1.0; - out_5245124330854285834[134] = 0.0; - out_5245124330854285834[135] = 0.0; - out_5245124330854285834[136] = 0.0; - out_5245124330854285834[137] = 0.0; - out_5245124330854285834[138] = 0.0; - out_5245124330854285834[139] = 0.0; - out_5245124330854285834[140] = 0.0; - out_5245124330854285834[141] = 0.0; - out_5245124330854285834[142] = 0.0; - out_5245124330854285834[143] = 0.0; - out_5245124330854285834[144] = 0.0; - out_5245124330854285834[145] = 0.0; - out_5245124330854285834[146] = 0.0; - out_5245124330854285834[147] = 0.0; - out_5245124330854285834[148] = 0.0; - out_5245124330854285834[149] = 0.0; - out_5245124330854285834[150] = 0.0; - out_5245124330854285834[151] = 0.0; - out_5245124330854285834[152] = 1.0; - out_5245124330854285834[153] = 0.0; - out_5245124330854285834[154] = 0.0; - out_5245124330854285834[155] = 0.0; - out_5245124330854285834[156] = 0.0; - out_5245124330854285834[157] = 0.0; - out_5245124330854285834[158] = 0.0; - out_5245124330854285834[159] = 0.0; - out_5245124330854285834[160] = 0.0; - out_5245124330854285834[161] = 0.0; - out_5245124330854285834[162] = 0.0; - out_5245124330854285834[163] = 0.0; - out_5245124330854285834[164] = 0.0; - out_5245124330854285834[165] = 0.0; - out_5245124330854285834[166] = 0.0; - out_5245124330854285834[167] = 0.0; - out_5245124330854285834[168] = 0.0; - out_5245124330854285834[169] = 0.0; - out_5245124330854285834[170] = 0.0; - out_5245124330854285834[171] = 1.0; - out_5245124330854285834[172] = 0.0; - out_5245124330854285834[173] = 0.0; - out_5245124330854285834[174] = 0.0; - out_5245124330854285834[175] = 0.0; - out_5245124330854285834[176] = 0.0; - out_5245124330854285834[177] = 0.0; - out_5245124330854285834[178] = 0.0; - out_5245124330854285834[179] = 0.0; - out_5245124330854285834[180] = 0.0; - out_5245124330854285834[181] = 0.0; - out_5245124330854285834[182] = 0.0; - out_5245124330854285834[183] = 0.0; - out_5245124330854285834[184] = 0.0; - out_5245124330854285834[185] = 0.0; - out_5245124330854285834[186] = 0.0; - out_5245124330854285834[187] = 0.0; - out_5245124330854285834[188] = 0.0; - out_5245124330854285834[189] = 0.0; - out_5245124330854285834[190] = 1.0; - out_5245124330854285834[191] = 0.0; - out_5245124330854285834[192] = 0.0; - out_5245124330854285834[193] = 0.0; - out_5245124330854285834[194] = 0.0; - out_5245124330854285834[195] = 0.0; - out_5245124330854285834[196] = 0.0; - out_5245124330854285834[197] = 0.0; - out_5245124330854285834[198] = 0.0; - out_5245124330854285834[199] = 0.0; - out_5245124330854285834[200] = 0.0; - out_5245124330854285834[201] = 0.0; - out_5245124330854285834[202] = 0.0; - out_5245124330854285834[203] = 0.0; - out_5245124330854285834[204] = 0.0; - out_5245124330854285834[205] = 0.0; - out_5245124330854285834[206] = 0.0; - out_5245124330854285834[207] = 0.0; - out_5245124330854285834[208] = 0.0; - out_5245124330854285834[209] = 1.0; - out_5245124330854285834[210] = 0.0; - out_5245124330854285834[211] = 0.0; - out_5245124330854285834[212] = 0.0; - out_5245124330854285834[213] = 0.0; - out_5245124330854285834[214] = 0.0; - out_5245124330854285834[215] = 0.0; - out_5245124330854285834[216] = 0.0; - out_5245124330854285834[217] = 0.0; - out_5245124330854285834[218] = 0.0; - out_5245124330854285834[219] = 0.0; - out_5245124330854285834[220] = 0.0; - out_5245124330854285834[221] = 0.0; - out_5245124330854285834[222] = 0.0; - out_5245124330854285834[223] = 0.0; - out_5245124330854285834[224] = 0.0; - out_5245124330854285834[225] = 0.0; - out_5245124330854285834[226] = 0.0; - out_5245124330854285834[227] = 0.0; - out_5245124330854285834[228] = 1.0; - out_5245124330854285834[229] = 0.0; - out_5245124330854285834[230] = 0.0; - out_5245124330854285834[231] = 0.0; - out_5245124330854285834[232] = 0.0; - out_5245124330854285834[233] = 0.0; - out_5245124330854285834[234] = 0.0; - out_5245124330854285834[235] = 0.0; - out_5245124330854285834[236] = 0.0; - out_5245124330854285834[237] = 0.0; - out_5245124330854285834[238] = 0.0; - out_5245124330854285834[239] = 0.0; - out_5245124330854285834[240] = 0.0; - out_5245124330854285834[241] = 0.0; - out_5245124330854285834[242] = 0.0; - out_5245124330854285834[243] = 0.0; - out_5245124330854285834[244] = 0.0; - out_5245124330854285834[245] = 0.0; - out_5245124330854285834[246] = 0.0; - out_5245124330854285834[247] = 1.0; - out_5245124330854285834[248] = 0.0; - out_5245124330854285834[249] = 0.0; - out_5245124330854285834[250] = 0.0; - out_5245124330854285834[251] = 0.0; - out_5245124330854285834[252] = 0.0; - out_5245124330854285834[253] = 0.0; - out_5245124330854285834[254] = 0.0; - out_5245124330854285834[255] = 0.0; - out_5245124330854285834[256] = 0.0; - out_5245124330854285834[257] = 0.0; - out_5245124330854285834[258] = 0.0; - out_5245124330854285834[259] = 0.0; - out_5245124330854285834[260] = 0.0; - out_5245124330854285834[261] = 0.0; - out_5245124330854285834[262] = 0.0; - out_5245124330854285834[263] = 0.0; - out_5245124330854285834[264] = 0.0; - out_5245124330854285834[265] = 0.0; - out_5245124330854285834[266] = 1.0; - out_5245124330854285834[267] = 0.0; - out_5245124330854285834[268] = 0.0; - out_5245124330854285834[269] = 0.0; - out_5245124330854285834[270] = 0.0; - out_5245124330854285834[271] = 0.0; - out_5245124330854285834[272] = 0.0; - out_5245124330854285834[273] = 0.0; - out_5245124330854285834[274] = 0.0; - out_5245124330854285834[275] = 0.0; - out_5245124330854285834[276] = 0.0; - out_5245124330854285834[277] = 0.0; - out_5245124330854285834[278] = 0.0; - out_5245124330854285834[279] = 0.0; - out_5245124330854285834[280] = 0.0; - out_5245124330854285834[281] = 0.0; - out_5245124330854285834[282] = 0.0; - out_5245124330854285834[283] = 0.0; - out_5245124330854285834[284] = 0.0; - out_5245124330854285834[285] = 1.0; - out_5245124330854285834[286] = 0.0; - out_5245124330854285834[287] = 0.0; - out_5245124330854285834[288] = 0.0; - out_5245124330854285834[289] = 0.0; - out_5245124330854285834[290] = 0.0; - out_5245124330854285834[291] = 0.0; - out_5245124330854285834[292] = 0.0; - out_5245124330854285834[293] = 0.0; - out_5245124330854285834[294] = 0.0; - out_5245124330854285834[295] = 0.0; - out_5245124330854285834[296] = 0.0; - out_5245124330854285834[297] = 0.0; - out_5245124330854285834[298] = 0.0; - out_5245124330854285834[299] = 0.0; - out_5245124330854285834[300] = 0.0; - out_5245124330854285834[301] = 0.0; - out_5245124330854285834[302] = 0.0; - out_5245124330854285834[303] = 0.0; - out_5245124330854285834[304] = 1.0; - out_5245124330854285834[305] = 0.0; - out_5245124330854285834[306] = 0.0; - out_5245124330854285834[307] = 0.0; - out_5245124330854285834[308] = 0.0; - out_5245124330854285834[309] = 0.0; - out_5245124330854285834[310] = 0.0; - out_5245124330854285834[311] = 0.0; - out_5245124330854285834[312] = 0.0; - out_5245124330854285834[313] = 0.0; - out_5245124330854285834[314] = 0.0; - out_5245124330854285834[315] = 0.0; - out_5245124330854285834[316] = 0.0; - out_5245124330854285834[317] = 0.0; - out_5245124330854285834[318] = 0.0; - out_5245124330854285834[319] = 0.0; - out_5245124330854285834[320] = 0.0; - out_5245124330854285834[321] = 0.0; - out_5245124330854285834[322] = 0.0; - out_5245124330854285834[323] = 1.0; +void H_mod_fun(double *state, double *out_1208523344993905378) { + out_1208523344993905378[0] = 1.0; + out_1208523344993905378[1] = 0.0; + out_1208523344993905378[2] = 0.0; + out_1208523344993905378[3] = 0.0; + out_1208523344993905378[4] = 0.0; + out_1208523344993905378[5] = 0.0; + out_1208523344993905378[6] = 0.0; + out_1208523344993905378[7] = 0.0; + out_1208523344993905378[8] = 0.0; + out_1208523344993905378[9] = 0.0; + out_1208523344993905378[10] = 0.0; + out_1208523344993905378[11] = 0.0; + out_1208523344993905378[12] = 0.0; + out_1208523344993905378[13] = 0.0; + out_1208523344993905378[14] = 0.0; + out_1208523344993905378[15] = 0.0; + out_1208523344993905378[16] = 0.0; + out_1208523344993905378[17] = 0.0; + out_1208523344993905378[18] = 0.0; + out_1208523344993905378[19] = 1.0; + out_1208523344993905378[20] = 0.0; + out_1208523344993905378[21] = 0.0; + out_1208523344993905378[22] = 0.0; + out_1208523344993905378[23] = 0.0; + out_1208523344993905378[24] = 0.0; + out_1208523344993905378[25] = 0.0; + out_1208523344993905378[26] = 0.0; + out_1208523344993905378[27] = 0.0; + out_1208523344993905378[28] = 0.0; + out_1208523344993905378[29] = 0.0; + out_1208523344993905378[30] = 0.0; + out_1208523344993905378[31] = 0.0; + out_1208523344993905378[32] = 0.0; + out_1208523344993905378[33] = 0.0; + out_1208523344993905378[34] = 0.0; + out_1208523344993905378[35] = 0.0; + out_1208523344993905378[36] = 0.0; + out_1208523344993905378[37] = 0.0; + out_1208523344993905378[38] = 1.0; + out_1208523344993905378[39] = 0.0; + out_1208523344993905378[40] = 0.0; + out_1208523344993905378[41] = 0.0; + out_1208523344993905378[42] = 0.0; + out_1208523344993905378[43] = 0.0; + out_1208523344993905378[44] = 0.0; + out_1208523344993905378[45] = 0.0; + out_1208523344993905378[46] = 0.0; + out_1208523344993905378[47] = 0.0; + out_1208523344993905378[48] = 0.0; + out_1208523344993905378[49] = 0.0; + out_1208523344993905378[50] = 0.0; + out_1208523344993905378[51] = 0.0; + out_1208523344993905378[52] = 0.0; + out_1208523344993905378[53] = 0.0; + out_1208523344993905378[54] = 0.0; + out_1208523344993905378[55] = 0.0; + out_1208523344993905378[56] = 0.0; + out_1208523344993905378[57] = 1.0; + out_1208523344993905378[58] = 0.0; + out_1208523344993905378[59] = 0.0; + out_1208523344993905378[60] = 0.0; + out_1208523344993905378[61] = 0.0; + out_1208523344993905378[62] = 0.0; + out_1208523344993905378[63] = 0.0; + out_1208523344993905378[64] = 0.0; + out_1208523344993905378[65] = 0.0; + out_1208523344993905378[66] = 0.0; + out_1208523344993905378[67] = 0.0; + out_1208523344993905378[68] = 0.0; + out_1208523344993905378[69] = 0.0; + out_1208523344993905378[70] = 0.0; + out_1208523344993905378[71] = 0.0; + out_1208523344993905378[72] = 0.0; + out_1208523344993905378[73] = 0.0; + out_1208523344993905378[74] = 0.0; + out_1208523344993905378[75] = 0.0; + out_1208523344993905378[76] = 1.0; + out_1208523344993905378[77] = 0.0; + out_1208523344993905378[78] = 0.0; + out_1208523344993905378[79] = 0.0; + out_1208523344993905378[80] = 0.0; + out_1208523344993905378[81] = 0.0; + out_1208523344993905378[82] = 0.0; + out_1208523344993905378[83] = 0.0; + out_1208523344993905378[84] = 0.0; + out_1208523344993905378[85] = 0.0; + out_1208523344993905378[86] = 0.0; + out_1208523344993905378[87] = 0.0; + out_1208523344993905378[88] = 0.0; + out_1208523344993905378[89] = 0.0; + out_1208523344993905378[90] = 0.0; + out_1208523344993905378[91] = 0.0; + out_1208523344993905378[92] = 0.0; + out_1208523344993905378[93] = 0.0; + out_1208523344993905378[94] = 0.0; + out_1208523344993905378[95] = 1.0; + out_1208523344993905378[96] = 0.0; + out_1208523344993905378[97] = 0.0; + out_1208523344993905378[98] = 0.0; + out_1208523344993905378[99] = 0.0; + out_1208523344993905378[100] = 0.0; + out_1208523344993905378[101] = 0.0; + out_1208523344993905378[102] = 0.0; + out_1208523344993905378[103] = 0.0; + out_1208523344993905378[104] = 0.0; + out_1208523344993905378[105] = 0.0; + out_1208523344993905378[106] = 0.0; + out_1208523344993905378[107] = 0.0; + out_1208523344993905378[108] = 0.0; + out_1208523344993905378[109] = 0.0; + out_1208523344993905378[110] = 0.0; + out_1208523344993905378[111] = 0.0; + out_1208523344993905378[112] = 0.0; + out_1208523344993905378[113] = 0.0; + out_1208523344993905378[114] = 1.0; + out_1208523344993905378[115] = 0.0; + out_1208523344993905378[116] = 0.0; + out_1208523344993905378[117] = 0.0; + out_1208523344993905378[118] = 0.0; + out_1208523344993905378[119] = 0.0; + out_1208523344993905378[120] = 0.0; + out_1208523344993905378[121] = 0.0; + out_1208523344993905378[122] = 0.0; + out_1208523344993905378[123] = 0.0; + out_1208523344993905378[124] = 0.0; + out_1208523344993905378[125] = 0.0; + out_1208523344993905378[126] = 0.0; + out_1208523344993905378[127] = 0.0; + out_1208523344993905378[128] = 0.0; + out_1208523344993905378[129] = 0.0; + out_1208523344993905378[130] = 0.0; + out_1208523344993905378[131] = 0.0; + out_1208523344993905378[132] = 0.0; + out_1208523344993905378[133] = 1.0; + out_1208523344993905378[134] = 0.0; + out_1208523344993905378[135] = 0.0; + out_1208523344993905378[136] = 0.0; + out_1208523344993905378[137] = 0.0; + out_1208523344993905378[138] = 0.0; + out_1208523344993905378[139] = 0.0; + out_1208523344993905378[140] = 0.0; + out_1208523344993905378[141] = 0.0; + out_1208523344993905378[142] = 0.0; + out_1208523344993905378[143] = 0.0; + out_1208523344993905378[144] = 0.0; + out_1208523344993905378[145] = 0.0; + out_1208523344993905378[146] = 0.0; + out_1208523344993905378[147] = 0.0; + out_1208523344993905378[148] = 0.0; + out_1208523344993905378[149] = 0.0; + out_1208523344993905378[150] = 0.0; + out_1208523344993905378[151] = 0.0; + out_1208523344993905378[152] = 1.0; + out_1208523344993905378[153] = 0.0; + out_1208523344993905378[154] = 0.0; + out_1208523344993905378[155] = 0.0; + out_1208523344993905378[156] = 0.0; + out_1208523344993905378[157] = 0.0; + out_1208523344993905378[158] = 0.0; + out_1208523344993905378[159] = 0.0; + out_1208523344993905378[160] = 0.0; + out_1208523344993905378[161] = 0.0; + out_1208523344993905378[162] = 0.0; + out_1208523344993905378[163] = 0.0; + out_1208523344993905378[164] = 0.0; + out_1208523344993905378[165] = 0.0; + out_1208523344993905378[166] = 0.0; + out_1208523344993905378[167] = 0.0; + out_1208523344993905378[168] = 0.0; + out_1208523344993905378[169] = 0.0; + out_1208523344993905378[170] = 0.0; + out_1208523344993905378[171] = 1.0; + out_1208523344993905378[172] = 0.0; + out_1208523344993905378[173] = 0.0; + out_1208523344993905378[174] = 0.0; + out_1208523344993905378[175] = 0.0; + out_1208523344993905378[176] = 0.0; + out_1208523344993905378[177] = 0.0; + out_1208523344993905378[178] = 0.0; + out_1208523344993905378[179] = 0.0; + out_1208523344993905378[180] = 0.0; + out_1208523344993905378[181] = 0.0; + out_1208523344993905378[182] = 0.0; + out_1208523344993905378[183] = 0.0; + out_1208523344993905378[184] = 0.0; + out_1208523344993905378[185] = 0.0; + out_1208523344993905378[186] = 0.0; + out_1208523344993905378[187] = 0.0; + out_1208523344993905378[188] = 0.0; + out_1208523344993905378[189] = 0.0; + out_1208523344993905378[190] = 1.0; + out_1208523344993905378[191] = 0.0; + out_1208523344993905378[192] = 0.0; + out_1208523344993905378[193] = 0.0; + out_1208523344993905378[194] = 0.0; + out_1208523344993905378[195] = 0.0; + out_1208523344993905378[196] = 0.0; + out_1208523344993905378[197] = 0.0; + out_1208523344993905378[198] = 0.0; + out_1208523344993905378[199] = 0.0; + out_1208523344993905378[200] = 0.0; + out_1208523344993905378[201] = 0.0; + out_1208523344993905378[202] = 0.0; + out_1208523344993905378[203] = 0.0; + out_1208523344993905378[204] = 0.0; + out_1208523344993905378[205] = 0.0; + out_1208523344993905378[206] = 0.0; + out_1208523344993905378[207] = 0.0; + out_1208523344993905378[208] = 0.0; + out_1208523344993905378[209] = 1.0; + out_1208523344993905378[210] = 0.0; + out_1208523344993905378[211] = 0.0; + out_1208523344993905378[212] = 0.0; + out_1208523344993905378[213] = 0.0; + out_1208523344993905378[214] = 0.0; + out_1208523344993905378[215] = 0.0; + out_1208523344993905378[216] = 0.0; + out_1208523344993905378[217] = 0.0; + out_1208523344993905378[218] = 0.0; + out_1208523344993905378[219] = 0.0; + out_1208523344993905378[220] = 0.0; + out_1208523344993905378[221] = 0.0; + out_1208523344993905378[222] = 0.0; + out_1208523344993905378[223] = 0.0; + out_1208523344993905378[224] = 0.0; + out_1208523344993905378[225] = 0.0; + out_1208523344993905378[226] = 0.0; + out_1208523344993905378[227] = 0.0; + out_1208523344993905378[228] = 1.0; + out_1208523344993905378[229] = 0.0; + out_1208523344993905378[230] = 0.0; + out_1208523344993905378[231] = 0.0; + out_1208523344993905378[232] = 0.0; + out_1208523344993905378[233] = 0.0; + out_1208523344993905378[234] = 0.0; + out_1208523344993905378[235] = 0.0; + out_1208523344993905378[236] = 0.0; + out_1208523344993905378[237] = 0.0; + out_1208523344993905378[238] = 0.0; + out_1208523344993905378[239] = 0.0; + out_1208523344993905378[240] = 0.0; + out_1208523344993905378[241] = 0.0; + out_1208523344993905378[242] = 0.0; + out_1208523344993905378[243] = 0.0; + out_1208523344993905378[244] = 0.0; + out_1208523344993905378[245] = 0.0; + out_1208523344993905378[246] = 0.0; + out_1208523344993905378[247] = 1.0; + out_1208523344993905378[248] = 0.0; + out_1208523344993905378[249] = 0.0; + out_1208523344993905378[250] = 0.0; + out_1208523344993905378[251] = 0.0; + out_1208523344993905378[252] = 0.0; + out_1208523344993905378[253] = 0.0; + out_1208523344993905378[254] = 0.0; + out_1208523344993905378[255] = 0.0; + out_1208523344993905378[256] = 0.0; + out_1208523344993905378[257] = 0.0; + out_1208523344993905378[258] = 0.0; + out_1208523344993905378[259] = 0.0; + out_1208523344993905378[260] = 0.0; + out_1208523344993905378[261] = 0.0; + out_1208523344993905378[262] = 0.0; + out_1208523344993905378[263] = 0.0; + out_1208523344993905378[264] = 0.0; + out_1208523344993905378[265] = 0.0; + out_1208523344993905378[266] = 1.0; + out_1208523344993905378[267] = 0.0; + out_1208523344993905378[268] = 0.0; + out_1208523344993905378[269] = 0.0; + out_1208523344993905378[270] = 0.0; + out_1208523344993905378[271] = 0.0; + out_1208523344993905378[272] = 0.0; + out_1208523344993905378[273] = 0.0; + out_1208523344993905378[274] = 0.0; + out_1208523344993905378[275] = 0.0; + out_1208523344993905378[276] = 0.0; + out_1208523344993905378[277] = 0.0; + out_1208523344993905378[278] = 0.0; + out_1208523344993905378[279] = 0.0; + out_1208523344993905378[280] = 0.0; + out_1208523344993905378[281] = 0.0; + out_1208523344993905378[282] = 0.0; + out_1208523344993905378[283] = 0.0; + out_1208523344993905378[284] = 0.0; + out_1208523344993905378[285] = 1.0; + out_1208523344993905378[286] = 0.0; + out_1208523344993905378[287] = 0.0; + out_1208523344993905378[288] = 0.0; + out_1208523344993905378[289] = 0.0; + out_1208523344993905378[290] = 0.0; + out_1208523344993905378[291] = 0.0; + out_1208523344993905378[292] = 0.0; + out_1208523344993905378[293] = 0.0; + out_1208523344993905378[294] = 0.0; + out_1208523344993905378[295] = 0.0; + out_1208523344993905378[296] = 0.0; + out_1208523344993905378[297] = 0.0; + out_1208523344993905378[298] = 0.0; + out_1208523344993905378[299] = 0.0; + out_1208523344993905378[300] = 0.0; + out_1208523344993905378[301] = 0.0; + out_1208523344993905378[302] = 0.0; + out_1208523344993905378[303] = 0.0; + out_1208523344993905378[304] = 1.0; + out_1208523344993905378[305] = 0.0; + out_1208523344993905378[306] = 0.0; + out_1208523344993905378[307] = 0.0; + out_1208523344993905378[308] = 0.0; + out_1208523344993905378[309] = 0.0; + out_1208523344993905378[310] = 0.0; + out_1208523344993905378[311] = 0.0; + out_1208523344993905378[312] = 0.0; + out_1208523344993905378[313] = 0.0; + out_1208523344993905378[314] = 0.0; + out_1208523344993905378[315] = 0.0; + out_1208523344993905378[316] = 0.0; + out_1208523344993905378[317] = 0.0; + out_1208523344993905378[318] = 0.0; + out_1208523344993905378[319] = 0.0; + out_1208523344993905378[320] = 0.0; + out_1208523344993905378[321] = 0.0; + out_1208523344993905378[322] = 0.0; + out_1208523344993905378[323] = 1.0; } -void f_fun(double *state, double dt, double *out_6830900184565569763) { - out_6830900184565569763[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_6830900184565569763[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_6830900184565569763[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_6830900184565569763[3] = dt*state[12] + state[3]; - out_6830900184565569763[4] = dt*state[13] + state[4]; - out_6830900184565569763[5] = dt*state[14] + state[5]; - out_6830900184565569763[6] = state[6]; - out_6830900184565569763[7] = state[7]; - out_6830900184565569763[8] = state[8]; - out_6830900184565569763[9] = state[9]; - out_6830900184565569763[10] = state[10]; - out_6830900184565569763[11] = state[11]; - out_6830900184565569763[12] = state[12]; - out_6830900184565569763[13] = state[13]; - out_6830900184565569763[14] = state[14]; - out_6830900184565569763[15] = state[15]; - out_6830900184565569763[16] = state[16]; - out_6830900184565569763[17] = state[17]; +void f_fun(double *state, double dt, double *out_828336908542161324) { + out_828336908542161324[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_828336908542161324[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_828336908542161324[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_828336908542161324[3] = dt*state[12] + state[3]; + out_828336908542161324[4] = dt*state[13] + state[4]; + out_828336908542161324[5] = dt*state[14] + state[5]; + out_828336908542161324[6] = state[6]; + out_828336908542161324[7] = state[7]; + out_828336908542161324[8] = state[8]; + out_828336908542161324[9] = state[9]; + out_828336908542161324[10] = state[10]; + out_828336908542161324[11] = state[11]; + out_828336908542161324[12] = state[12]; + out_828336908542161324[13] = state[13]; + out_828336908542161324[14] = state[14]; + out_828336908542161324[15] = state[15]; + out_828336908542161324[16] = state[16]; + out_828336908542161324[17] = state[17]; } -void F_fun(double *state, double dt, double *out_5118094281500581129) { - out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[2] = 0; - out_5118094281500581129[3] = 0; - out_5118094281500581129[4] = 0; - out_5118094281500581129[5] = 0; - out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[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_5118094281500581129[9] = 0; - out_5118094281500581129[10] = 0; - out_5118094281500581129[11] = 0; - out_5118094281500581129[12] = 0; - out_5118094281500581129[13] = 0; - out_5118094281500581129[14] = 0; - out_5118094281500581129[15] = 0; - out_5118094281500581129[16] = 0; - out_5118094281500581129[17] = 0; - out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[20] = 0; - out_5118094281500581129[21] = 0; - out_5118094281500581129[22] = 0; - out_5118094281500581129[23] = 0; - out_5118094281500581129[24] = 0; - out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[27] = 0; - out_5118094281500581129[28] = 0; - out_5118094281500581129[29] = 0; - out_5118094281500581129[30] = 0; - out_5118094281500581129[31] = 0; - out_5118094281500581129[32] = 0; - out_5118094281500581129[33] = 0; - out_5118094281500581129[34] = 0; - out_5118094281500581129[35] = 0; - out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[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_5118094281500581129[39] = 0; - out_5118094281500581129[40] = 0; - out_5118094281500581129[41] = 0; - out_5118094281500581129[42] = 0; - out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[45] = 0; - out_5118094281500581129[46] = 0; - out_5118094281500581129[47] = 0; - out_5118094281500581129[48] = 0; - out_5118094281500581129[49] = 0; - out_5118094281500581129[50] = 0; - out_5118094281500581129[51] = 0; - out_5118094281500581129[52] = 0; - out_5118094281500581129[53] = 0; - out_5118094281500581129[54] = 0; - out_5118094281500581129[55] = 0; - out_5118094281500581129[56] = 0; - out_5118094281500581129[57] = 1; - out_5118094281500581129[58] = 0; - out_5118094281500581129[59] = 0; - out_5118094281500581129[60] = 0; - out_5118094281500581129[61] = 0; - out_5118094281500581129[62] = 0; - out_5118094281500581129[63] = 0; - out_5118094281500581129[64] = 0; - out_5118094281500581129[65] = 0; - out_5118094281500581129[66] = dt; - out_5118094281500581129[67] = 0; - out_5118094281500581129[68] = 0; - out_5118094281500581129[69] = 0; - out_5118094281500581129[70] = 0; - out_5118094281500581129[71] = 0; - out_5118094281500581129[72] = 0; - out_5118094281500581129[73] = 0; - out_5118094281500581129[74] = 0; - out_5118094281500581129[75] = 0; - out_5118094281500581129[76] = 1; - out_5118094281500581129[77] = 0; - out_5118094281500581129[78] = 0; - out_5118094281500581129[79] = 0; - out_5118094281500581129[80] = 0; - out_5118094281500581129[81] = 0; - out_5118094281500581129[82] = 0; - out_5118094281500581129[83] = 0; - out_5118094281500581129[84] = 0; - out_5118094281500581129[85] = dt; - out_5118094281500581129[86] = 0; - out_5118094281500581129[87] = 0; - out_5118094281500581129[88] = 0; - out_5118094281500581129[89] = 0; - out_5118094281500581129[90] = 0; - out_5118094281500581129[91] = 0; - out_5118094281500581129[92] = 0; - out_5118094281500581129[93] = 0; - out_5118094281500581129[94] = 0; - out_5118094281500581129[95] = 1; - out_5118094281500581129[96] = 0; - out_5118094281500581129[97] = 0; - out_5118094281500581129[98] = 0; - out_5118094281500581129[99] = 0; - out_5118094281500581129[100] = 0; - out_5118094281500581129[101] = 0; - out_5118094281500581129[102] = 0; - out_5118094281500581129[103] = 0; - out_5118094281500581129[104] = dt; - out_5118094281500581129[105] = 0; - out_5118094281500581129[106] = 0; - out_5118094281500581129[107] = 0; - out_5118094281500581129[108] = 0; - out_5118094281500581129[109] = 0; - out_5118094281500581129[110] = 0; - out_5118094281500581129[111] = 0; - out_5118094281500581129[112] = 0; - out_5118094281500581129[113] = 0; - out_5118094281500581129[114] = 1; - out_5118094281500581129[115] = 0; - out_5118094281500581129[116] = 0; - out_5118094281500581129[117] = 0; - out_5118094281500581129[118] = 0; - out_5118094281500581129[119] = 0; - out_5118094281500581129[120] = 0; - out_5118094281500581129[121] = 0; - out_5118094281500581129[122] = 0; - out_5118094281500581129[123] = 0; - out_5118094281500581129[124] = 0; - out_5118094281500581129[125] = 0; - out_5118094281500581129[126] = 0; - out_5118094281500581129[127] = 0; - out_5118094281500581129[128] = 0; - out_5118094281500581129[129] = 0; - out_5118094281500581129[130] = 0; - out_5118094281500581129[131] = 0; - out_5118094281500581129[132] = 0; - out_5118094281500581129[133] = 1; - out_5118094281500581129[134] = 0; - out_5118094281500581129[135] = 0; - out_5118094281500581129[136] = 0; - out_5118094281500581129[137] = 0; - out_5118094281500581129[138] = 0; - out_5118094281500581129[139] = 0; - out_5118094281500581129[140] = 0; - out_5118094281500581129[141] = 0; - out_5118094281500581129[142] = 0; - out_5118094281500581129[143] = 0; - out_5118094281500581129[144] = 0; - out_5118094281500581129[145] = 0; - out_5118094281500581129[146] = 0; - out_5118094281500581129[147] = 0; - out_5118094281500581129[148] = 0; - out_5118094281500581129[149] = 0; - out_5118094281500581129[150] = 0; - out_5118094281500581129[151] = 0; - out_5118094281500581129[152] = 1; - out_5118094281500581129[153] = 0; - out_5118094281500581129[154] = 0; - out_5118094281500581129[155] = 0; - out_5118094281500581129[156] = 0; - out_5118094281500581129[157] = 0; - out_5118094281500581129[158] = 0; - out_5118094281500581129[159] = 0; - out_5118094281500581129[160] = 0; - out_5118094281500581129[161] = 0; - out_5118094281500581129[162] = 0; - out_5118094281500581129[163] = 0; - out_5118094281500581129[164] = 0; - out_5118094281500581129[165] = 0; - out_5118094281500581129[166] = 0; - out_5118094281500581129[167] = 0; - out_5118094281500581129[168] = 0; - out_5118094281500581129[169] = 0; - out_5118094281500581129[170] = 0; - out_5118094281500581129[171] = 1; - out_5118094281500581129[172] = 0; - out_5118094281500581129[173] = 0; - out_5118094281500581129[174] = 0; - out_5118094281500581129[175] = 0; - out_5118094281500581129[176] = 0; - out_5118094281500581129[177] = 0; - out_5118094281500581129[178] = 0; - out_5118094281500581129[179] = 0; - out_5118094281500581129[180] = 0; - out_5118094281500581129[181] = 0; - out_5118094281500581129[182] = 0; - out_5118094281500581129[183] = 0; - out_5118094281500581129[184] = 0; - out_5118094281500581129[185] = 0; - out_5118094281500581129[186] = 0; - out_5118094281500581129[187] = 0; - out_5118094281500581129[188] = 0; - out_5118094281500581129[189] = 0; - out_5118094281500581129[190] = 1; - out_5118094281500581129[191] = 0; - out_5118094281500581129[192] = 0; - out_5118094281500581129[193] = 0; - out_5118094281500581129[194] = 0; - out_5118094281500581129[195] = 0; - out_5118094281500581129[196] = 0; - out_5118094281500581129[197] = 0; - out_5118094281500581129[198] = 0; - out_5118094281500581129[199] = 0; - out_5118094281500581129[200] = 0; - out_5118094281500581129[201] = 0; - out_5118094281500581129[202] = 0; - out_5118094281500581129[203] = 0; - out_5118094281500581129[204] = 0; - out_5118094281500581129[205] = 0; - out_5118094281500581129[206] = 0; - out_5118094281500581129[207] = 0; - out_5118094281500581129[208] = 0; - out_5118094281500581129[209] = 1; - out_5118094281500581129[210] = 0; - out_5118094281500581129[211] = 0; - out_5118094281500581129[212] = 0; - out_5118094281500581129[213] = 0; - out_5118094281500581129[214] = 0; - out_5118094281500581129[215] = 0; - out_5118094281500581129[216] = 0; - out_5118094281500581129[217] = 0; - out_5118094281500581129[218] = 0; - out_5118094281500581129[219] = 0; - out_5118094281500581129[220] = 0; - out_5118094281500581129[221] = 0; - out_5118094281500581129[222] = 0; - out_5118094281500581129[223] = 0; - out_5118094281500581129[224] = 0; - out_5118094281500581129[225] = 0; - out_5118094281500581129[226] = 0; - out_5118094281500581129[227] = 0; - out_5118094281500581129[228] = 1; - out_5118094281500581129[229] = 0; - out_5118094281500581129[230] = 0; - out_5118094281500581129[231] = 0; - out_5118094281500581129[232] = 0; - out_5118094281500581129[233] = 0; - out_5118094281500581129[234] = 0; - out_5118094281500581129[235] = 0; - out_5118094281500581129[236] = 0; - out_5118094281500581129[237] = 0; - out_5118094281500581129[238] = 0; - out_5118094281500581129[239] = 0; - out_5118094281500581129[240] = 0; - out_5118094281500581129[241] = 0; - out_5118094281500581129[242] = 0; - out_5118094281500581129[243] = 0; - out_5118094281500581129[244] = 0; - out_5118094281500581129[245] = 0; - out_5118094281500581129[246] = 0; - out_5118094281500581129[247] = 1; - out_5118094281500581129[248] = 0; - out_5118094281500581129[249] = 0; - out_5118094281500581129[250] = 0; - out_5118094281500581129[251] = 0; - out_5118094281500581129[252] = 0; - out_5118094281500581129[253] = 0; - out_5118094281500581129[254] = 0; - out_5118094281500581129[255] = 0; - out_5118094281500581129[256] = 0; - out_5118094281500581129[257] = 0; - out_5118094281500581129[258] = 0; - out_5118094281500581129[259] = 0; - out_5118094281500581129[260] = 0; - out_5118094281500581129[261] = 0; - out_5118094281500581129[262] = 0; - out_5118094281500581129[263] = 0; - out_5118094281500581129[264] = 0; - out_5118094281500581129[265] = 0; - out_5118094281500581129[266] = 1; - out_5118094281500581129[267] = 0; - out_5118094281500581129[268] = 0; - out_5118094281500581129[269] = 0; - out_5118094281500581129[270] = 0; - out_5118094281500581129[271] = 0; - out_5118094281500581129[272] = 0; - out_5118094281500581129[273] = 0; - out_5118094281500581129[274] = 0; - out_5118094281500581129[275] = 0; - out_5118094281500581129[276] = 0; - out_5118094281500581129[277] = 0; - out_5118094281500581129[278] = 0; - out_5118094281500581129[279] = 0; - out_5118094281500581129[280] = 0; - out_5118094281500581129[281] = 0; - out_5118094281500581129[282] = 0; - out_5118094281500581129[283] = 0; - out_5118094281500581129[284] = 0; - out_5118094281500581129[285] = 1; - out_5118094281500581129[286] = 0; - out_5118094281500581129[287] = 0; - out_5118094281500581129[288] = 0; - out_5118094281500581129[289] = 0; - out_5118094281500581129[290] = 0; - out_5118094281500581129[291] = 0; - out_5118094281500581129[292] = 0; - out_5118094281500581129[293] = 0; - out_5118094281500581129[294] = 0; - out_5118094281500581129[295] = 0; - out_5118094281500581129[296] = 0; - out_5118094281500581129[297] = 0; - out_5118094281500581129[298] = 0; - out_5118094281500581129[299] = 0; - out_5118094281500581129[300] = 0; - out_5118094281500581129[301] = 0; - out_5118094281500581129[302] = 0; - out_5118094281500581129[303] = 0; - out_5118094281500581129[304] = 1; - out_5118094281500581129[305] = 0; - out_5118094281500581129[306] = 0; - out_5118094281500581129[307] = 0; - out_5118094281500581129[308] = 0; - out_5118094281500581129[309] = 0; - out_5118094281500581129[310] = 0; - out_5118094281500581129[311] = 0; - out_5118094281500581129[312] = 0; - out_5118094281500581129[313] = 0; - out_5118094281500581129[314] = 0; - out_5118094281500581129[315] = 0; - out_5118094281500581129[316] = 0; - out_5118094281500581129[317] = 0; - out_5118094281500581129[318] = 0; - out_5118094281500581129[319] = 0; - out_5118094281500581129[320] = 0; - out_5118094281500581129[321] = 0; - out_5118094281500581129[322] = 0; - out_5118094281500581129[323] = 1; +void F_fun(double *state, double dt, double *out_5703387388084628727) { + out_5703387388084628727[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_5703387388084628727[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_5703387388084628727[2] = 0; + out_5703387388084628727[3] = 0; + out_5703387388084628727[4] = 0; + out_5703387388084628727[5] = 0; + out_5703387388084628727[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_5703387388084628727[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_5703387388084628727[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_5703387388084628727[9] = 0; + out_5703387388084628727[10] = 0; + out_5703387388084628727[11] = 0; + out_5703387388084628727[12] = 0; + out_5703387388084628727[13] = 0; + out_5703387388084628727[14] = 0; + out_5703387388084628727[15] = 0; + out_5703387388084628727[16] = 0; + out_5703387388084628727[17] = 0; + out_5703387388084628727[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_5703387388084628727[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_5703387388084628727[20] = 0; + out_5703387388084628727[21] = 0; + out_5703387388084628727[22] = 0; + out_5703387388084628727[23] = 0; + out_5703387388084628727[24] = 0; + out_5703387388084628727[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_5703387388084628727[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_5703387388084628727[27] = 0; + out_5703387388084628727[28] = 0; + out_5703387388084628727[29] = 0; + out_5703387388084628727[30] = 0; + out_5703387388084628727[31] = 0; + out_5703387388084628727[32] = 0; + out_5703387388084628727[33] = 0; + out_5703387388084628727[34] = 0; + out_5703387388084628727[35] = 0; + out_5703387388084628727[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_5703387388084628727[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_5703387388084628727[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_5703387388084628727[39] = 0; + out_5703387388084628727[40] = 0; + out_5703387388084628727[41] = 0; + out_5703387388084628727[42] = 0; + out_5703387388084628727[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_5703387388084628727[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_5703387388084628727[45] = 0; + out_5703387388084628727[46] = 0; + out_5703387388084628727[47] = 0; + out_5703387388084628727[48] = 0; + out_5703387388084628727[49] = 0; + out_5703387388084628727[50] = 0; + out_5703387388084628727[51] = 0; + out_5703387388084628727[52] = 0; + out_5703387388084628727[53] = 0; + out_5703387388084628727[54] = 0; + out_5703387388084628727[55] = 0; + out_5703387388084628727[56] = 0; + out_5703387388084628727[57] = 1; + out_5703387388084628727[58] = 0; + out_5703387388084628727[59] = 0; + out_5703387388084628727[60] = 0; + out_5703387388084628727[61] = 0; + out_5703387388084628727[62] = 0; + out_5703387388084628727[63] = 0; + out_5703387388084628727[64] = 0; + out_5703387388084628727[65] = 0; + out_5703387388084628727[66] = dt; + out_5703387388084628727[67] = 0; + out_5703387388084628727[68] = 0; + out_5703387388084628727[69] = 0; + out_5703387388084628727[70] = 0; + out_5703387388084628727[71] = 0; + out_5703387388084628727[72] = 0; + out_5703387388084628727[73] = 0; + out_5703387388084628727[74] = 0; + out_5703387388084628727[75] = 0; + out_5703387388084628727[76] = 1; + out_5703387388084628727[77] = 0; + out_5703387388084628727[78] = 0; + out_5703387388084628727[79] = 0; + out_5703387388084628727[80] = 0; + out_5703387388084628727[81] = 0; + out_5703387388084628727[82] = 0; + out_5703387388084628727[83] = 0; + out_5703387388084628727[84] = 0; + out_5703387388084628727[85] = dt; + out_5703387388084628727[86] = 0; + out_5703387388084628727[87] = 0; + out_5703387388084628727[88] = 0; + out_5703387388084628727[89] = 0; + out_5703387388084628727[90] = 0; + out_5703387388084628727[91] = 0; + out_5703387388084628727[92] = 0; + out_5703387388084628727[93] = 0; + out_5703387388084628727[94] = 0; + out_5703387388084628727[95] = 1; + out_5703387388084628727[96] = 0; + out_5703387388084628727[97] = 0; + out_5703387388084628727[98] = 0; + out_5703387388084628727[99] = 0; + out_5703387388084628727[100] = 0; + out_5703387388084628727[101] = 0; + out_5703387388084628727[102] = 0; + out_5703387388084628727[103] = 0; + out_5703387388084628727[104] = dt; + out_5703387388084628727[105] = 0; + out_5703387388084628727[106] = 0; + out_5703387388084628727[107] = 0; + out_5703387388084628727[108] = 0; + out_5703387388084628727[109] = 0; + out_5703387388084628727[110] = 0; + out_5703387388084628727[111] = 0; + out_5703387388084628727[112] = 0; + out_5703387388084628727[113] = 0; + out_5703387388084628727[114] = 1; + out_5703387388084628727[115] = 0; + out_5703387388084628727[116] = 0; + out_5703387388084628727[117] = 0; + out_5703387388084628727[118] = 0; + out_5703387388084628727[119] = 0; + out_5703387388084628727[120] = 0; + out_5703387388084628727[121] = 0; + out_5703387388084628727[122] = 0; + out_5703387388084628727[123] = 0; + out_5703387388084628727[124] = 0; + out_5703387388084628727[125] = 0; + out_5703387388084628727[126] = 0; + out_5703387388084628727[127] = 0; + out_5703387388084628727[128] = 0; + out_5703387388084628727[129] = 0; + out_5703387388084628727[130] = 0; + out_5703387388084628727[131] = 0; + out_5703387388084628727[132] = 0; + out_5703387388084628727[133] = 1; + out_5703387388084628727[134] = 0; + out_5703387388084628727[135] = 0; + out_5703387388084628727[136] = 0; + out_5703387388084628727[137] = 0; + out_5703387388084628727[138] = 0; + out_5703387388084628727[139] = 0; + out_5703387388084628727[140] = 0; + out_5703387388084628727[141] = 0; + out_5703387388084628727[142] = 0; + out_5703387388084628727[143] = 0; + out_5703387388084628727[144] = 0; + out_5703387388084628727[145] = 0; + out_5703387388084628727[146] = 0; + out_5703387388084628727[147] = 0; + out_5703387388084628727[148] = 0; + out_5703387388084628727[149] = 0; + out_5703387388084628727[150] = 0; + out_5703387388084628727[151] = 0; + out_5703387388084628727[152] = 1; + out_5703387388084628727[153] = 0; + out_5703387388084628727[154] = 0; + out_5703387388084628727[155] = 0; + out_5703387388084628727[156] = 0; + out_5703387388084628727[157] = 0; + out_5703387388084628727[158] = 0; + out_5703387388084628727[159] = 0; + out_5703387388084628727[160] = 0; + out_5703387388084628727[161] = 0; + out_5703387388084628727[162] = 0; + out_5703387388084628727[163] = 0; + out_5703387388084628727[164] = 0; + out_5703387388084628727[165] = 0; + out_5703387388084628727[166] = 0; + out_5703387388084628727[167] = 0; + out_5703387388084628727[168] = 0; + out_5703387388084628727[169] = 0; + out_5703387388084628727[170] = 0; + out_5703387388084628727[171] = 1; + out_5703387388084628727[172] = 0; + out_5703387388084628727[173] = 0; + out_5703387388084628727[174] = 0; + out_5703387388084628727[175] = 0; + out_5703387388084628727[176] = 0; + out_5703387388084628727[177] = 0; + out_5703387388084628727[178] = 0; + out_5703387388084628727[179] = 0; + out_5703387388084628727[180] = 0; + out_5703387388084628727[181] = 0; + out_5703387388084628727[182] = 0; + out_5703387388084628727[183] = 0; + out_5703387388084628727[184] = 0; + out_5703387388084628727[185] = 0; + out_5703387388084628727[186] = 0; + out_5703387388084628727[187] = 0; + out_5703387388084628727[188] = 0; + out_5703387388084628727[189] = 0; + out_5703387388084628727[190] = 1; + out_5703387388084628727[191] = 0; + out_5703387388084628727[192] = 0; + out_5703387388084628727[193] = 0; + out_5703387388084628727[194] = 0; + out_5703387388084628727[195] = 0; + out_5703387388084628727[196] = 0; + out_5703387388084628727[197] = 0; + out_5703387388084628727[198] = 0; + out_5703387388084628727[199] = 0; + out_5703387388084628727[200] = 0; + out_5703387388084628727[201] = 0; + out_5703387388084628727[202] = 0; + out_5703387388084628727[203] = 0; + out_5703387388084628727[204] = 0; + out_5703387388084628727[205] = 0; + out_5703387388084628727[206] = 0; + out_5703387388084628727[207] = 0; + out_5703387388084628727[208] = 0; + out_5703387388084628727[209] = 1; + out_5703387388084628727[210] = 0; + out_5703387388084628727[211] = 0; + out_5703387388084628727[212] = 0; + out_5703387388084628727[213] = 0; + out_5703387388084628727[214] = 0; + out_5703387388084628727[215] = 0; + out_5703387388084628727[216] = 0; + out_5703387388084628727[217] = 0; + out_5703387388084628727[218] = 0; + out_5703387388084628727[219] = 0; + out_5703387388084628727[220] = 0; + out_5703387388084628727[221] = 0; + out_5703387388084628727[222] = 0; + out_5703387388084628727[223] = 0; + out_5703387388084628727[224] = 0; + out_5703387388084628727[225] = 0; + out_5703387388084628727[226] = 0; + out_5703387388084628727[227] = 0; + out_5703387388084628727[228] = 1; + out_5703387388084628727[229] = 0; + out_5703387388084628727[230] = 0; + out_5703387388084628727[231] = 0; + out_5703387388084628727[232] = 0; + out_5703387388084628727[233] = 0; + out_5703387388084628727[234] = 0; + out_5703387388084628727[235] = 0; + out_5703387388084628727[236] = 0; + out_5703387388084628727[237] = 0; + out_5703387388084628727[238] = 0; + out_5703387388084628727[239] = 0; + out_5703387388084628727[240] = 0; + out_5703387388084628727[241] = 0; + out_5703387388084628727[242] = 0; + out_5703387388084628727[243] = 0; + out_5703387388084628727[244] = 0; + out_5703387388084628727[245] = 0; + out_5703387388084628727[246] = 0; + out_5703387388084628727[247] = 1; + out_5703387388084628727[248] = 0; + out_5703387388084628727[249] = 0; + out_5703387388084628727[250] = 0; + out_5703387388084628727[251] = 0; + out_5703387388084628727[252] = 0; + out_5703387388084628727[253] = 0; + out_5703387388084628727[254] = 0; + out_5703387388084628727[255] = 0; + out_5703387388084628727[256] = 0; + out_5703387388084628727[257] = 0; + out_5703387388084628727[258] = 0; + out_5703387388084628727[259] = 0; + out_5703387388084628727[260] = 0; + out_5703387388084628727[261] = 0; + out_5703387388084628727[262] = 0; + out_5703387388084628727[263] = 0; + out_5703387388084628727[264] = 0; + out_5703387388084628727[265] = 0; + out_5703387388084628727[266] = 1; + out_5703387388084628727[267] = 0; + out_5703387388084628727[268] = 0; + out_5703387388084628727[269] = 0; + out_5703387388084628727[270] = 0; + out_5703387388084628727[271] = 0; + out_5703387388084628727[272] = 0; + out_5703387388084628727[273] = 0; + out_5703387388084628727[274] = 0; + out_5703387388084628727[275] = 0; + out_5703387388084628727[276] = 0; + out_5703387388084628727[277] = 0; + out_5703387388084628727[278] = 0; + out_5703387388084628727[279] = 0; + out_5703387388084628727[280] = 0; + out_5703387388084628727[281] = 0; + out_5703387388084628727[282] = 0; + out_5703387388084628727[283] = 0; + out_5703387388084628727[284] = 0; + out_5703387388084628727[285] = 1; + out_5703387388084628727[286] = 0; + out_5703387388084628727[287] = 0; + out_5703387388084628727[288] = 0; + out_5703387388084628727[289] = 0; + out_5703387388084628727[290] = 0; + out_5703387388084628727[291] = 0; + out_5703387388084628727[292] = 0; + out_5703387388084628727[293] = 0; + out_5703387388084628727[294] = 0; + out_5703387388084628727[295] = 0; + out_5703387388084628727[296] = 0; + out_5703387388084628727[297] = 0; + out_5703387388084628727[298] = 0; + out_5703387388084628727[299] = 0; + out_5703387388084628727[300] = 0; + out_5703387388084628727[301] = 0; + out_5703387388084628727[302] = 0; + out_5703387388084628727[303] = 0; + out_5703387388084628727[304] = 1; + out_5703387388084628727[305] = 0; + out_5703387388084628727[306] = 0; + out_5703387388084628727[307] = 0; + out_5703387388084628727[308] = 0; + out_5703387388084628727[309] = 0; + out_5703387388084628727[310] = 0; + out_5703387388084628727[311] = 0; + out_5703387388084628727[312] = 0; + out_5703387388084628727[313] = 0; + out_5703387388084628727[314] = 0; + out_5703387388084628727[315] = 0; + out_5703387388084628727[316] = 0; + out_5703387388084628727[317] = 0; + out_5703387388084628727[318] = 0; + out_5703387388084628727[319] = 0; + out_5703387388084628727[320] = 0; + out_5703387388084628727[321] = 0; + out_5703387388084628727[322] = 0; + out_5703387388084628727[323] = 1; } -void h_4(double *state, double *unused, double *out_4935070827926002458) { - out_4935070827926002458[0] = state[6] + state[9]; - out_4935070827926002458[1] = state[7] + state[10]; - out_4935070827926002458[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_2600666041609585625) { + out_2600666041609585625[0] = state[6] + state[9]; + out_2600666041609585625[1] = state[7] + state[10]; + out_2600666041609585625[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_6443071812737183938) { - out_6443071812737183938[0] = 0; - out_6443071812737183938[1] = 0; - out_6443071812737183938[2] = 0; - out_6443071812737183938[3] = 0; - out_6443071812737183938[4] = 0; - out_6443071812737183938[5] = 0; - out_6443071812737183938[6] = 1; - out_6443071812737183938[7] = 0; - out_6443071812737183938[8] = 0; - out_6443071812737183938[9] = 1; - out_6443071812737183938[10] = 0; - out_6443071812737183938[11] = 0; - out_6443071812737183938[12] = 0; - out_6443071812737183938[13] = 0; - out_6443071812737183938[14] = 0; - out_6443071812737183938[15] = 0; - out_6443071812737183938[16] = 0; - out_6443071812737183938[17] = 0; - out_6443071812737183938[18] = 0; - out_6443071812737183938[19] = 0; - out_6443071812737183938[20] = 0; - out_6443071812737183938[21] = 0; - out_6443071812737183938[22] = 0; - out_6443071812737183938[23] = 0; - out_6443071812737183938[24] = 0; - out_6443071812737183938[25] = 1; - out_6443071812737183938[26] = 0; - out_6443071812737183938[27] = 0; - out_6443071812737183938[28] = 1; - out_6443071812737183938[29] = 0; - out_6443071812737183938[30] = 0; - out_6443071812737183938[31] = 0; - out_6443071812737183938[32] = 0; - out_6443071812737183938[33] = 0; - out_6443071812737183938[34] = 0; - out_6443071812737183938[35] = 0; - out_6443071812737183938[36] = 0; - out_6443071812737183938[37] = 0; - out_6443071812737183938[38] = 0; - out_6443071812737183938[39] = 0; - out_6443071812737183938[40] = 0; - out_6443071812737183938[41] = 0; - out_6443071812737183938[42] = 0; - out_6443071812737183938[43] = 0; - out_6443071812737183938[44] = 1; - out_6443071812737183938[45] = 0; - out_6443071812737183938[46] = 0; - out_6443071812737183938[47] = 1; - out_6443071812737183938[48] = 0; - out_6443071812737183938[49] = 0; - out_6443071812737183938[50] = 0; - out_6443071812737183938[51] = 0; - out_6443071812737183938[52] = 0; - out_6443071812737183938[53] = 0; +void H_4(double *state, double *unused, double *out_2406470826876803482) { + out_2406470826876803482[0] = 0; + out_2406470826876803482[1] = 0; + out_2406470826876803482[2] = 0; + out_2406470826876803482[3] = 0; + out_2406470826876803482[4] = 0; + out_2406470826876803482[5] = 0; + out_2406470826876803482[6] = 1; + out_2406470826876803482[7] = 0; + out_2406470826876803482[8] = 0; + out_2406470826876803482[9] = 1; + out_2406470826876803482[10] = 0; + out_2406470826876803482[11] = 0; + out_2406470826876803482[12] = 0; + out_2406470826876803482[13] = 0; + out_2406470826876803482[14] = 0; + out_2406470826876803482[15] = 0; + out_2406470826876803482[16] = 0; + out_2406470826876803482[17] = 0; + out_2406470826876803482[18] = 0; + out_2406470826876803482[19] = 0; + out_2406470826876803482[20] = 0; + out_2406470826876803482[21] = 0; + out_2406470826876803482[22] = 0; + out_2406470826876803482[23] = 0; + out_2406470826876803482[24] = 0; + out_2406470826876803482[25] = 1; + out_2406470826876803482[26] = 0; + out_2406470826876803482[27] = 0; + out_2406470826876803482[28] = 1; + out_2406470826876803482[29] = 0; + out_2406470826876803482[30] = 0; + out_2406470826876803482[31] = 0; + out_2406470826876803482[32] = 0; + out_2406470826876803482[33] = 0; + out_2406470826876803482[34] = 0; + out_2406470826876803482[35] = 0; + out_2406470826876803482[36] = 0; + out_2406470826876803482[37] = 0; + out_2406470826876803482[38] = 0; + out_2406470826876803482[39] = 0; + out_2406470826876803482[40] = 0; + out_2406470826876803482[41] = 0; + out_2406470826876803482[42] = 0; + out_2406470826876803482[43] = 0; + out_2406470826876803482[44] = 1; + out_2406470826876803482[45] = 0; + out_2406470826876803482[46] = 0; + out_2406470826876803482[47] = 1; + out_2406470826876803482[48] = 0; + out_2406470826876803482[49] = 0; + out_2406470826876803482[50] = 0; + out_2406470826876803482[51] = 0; + out_2406470826876803482[52] = 0; + out_2406470826876803482[53] = 0; } -void h_10(double *state, double *unused, double *out_2021668210924663063) { - out_2021668210924663063[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_2021668210924663063[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_2021668210924663063[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_2042730394265084466) { + out_2042730394265084466[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_2042730394265084466[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_2042730394265084466[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_7830124823924821015) { - out_7830124823924821015[0] = 0; - out_7830124823924821015[1] = 9.8100000000000005*cos(state[1]); - out_7830124823924821015[2] = 0; - out_7830124823924821015[3] = 0; - out_7830124823924821015[4] = -state[8]; - out_7830124823924821015[5] = state[7]; - out_7830124823924821015[6] = 0; - out_7830124823924821015[7] = state[5]; - out_7830124823924821015[8] = -state[4]; - out_7830124823924821015[9] = 0; - out_7830124823924821015[10] = 0; - out_7830124823924821015[11] = 0; - out_7830124823924821015[12] = 1; - out_7830124823924821015[13] = 0; - out_7830124823924821015[14] = 0; - out_7830124823924821015[15] = 1; - out_7830124823924821015[16] = 0; - out_7830124823924821015[17] = 0; - out_7830124823924821015[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_7830124823924821015[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_7830124823924821015[20] = 0; - out_7830124823924821015[21] = state[8]; - out_7830124823924821015[22] = 0; - out_7830124823924821015[23] = -state[6]; - out_7830124823924821015[24] = -state[5]; - out_7830124823924821015[25] = 0; - out_7830124823924821015[26] = state[3]; - out_7830124823924821015[27] = 0; - out_7830124823924821015[28] = 0; - out_7830124823924821015[29] = 0; - out_7830124823924821015[30] = 0; - out_7830124823924821015[31] = 1; - out_7830124823924821015[32] = 0; - out_7830124823924821015[33] = 0; - out_7830124823924821015[34] = 1; - out_7830124823924821015[35] = 0; - out_7830124823924821015[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_7830124823924821015[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_7830124823924821015[38] = 0; - out_7830124823924821015[39] = -state[7]; - out_7830124823924821015[40] = state[6]; - out_7830124823924821015[41] = 0; - out_7830124823924821015[42] = state[4]; - out_7830124823924821015[43] = -state[3]; - out_7830124823924821015[44] = 0; - out_7830124823924821015[45] = 0; - out_7830124823924821015[46] = 0; - out_7830124823924821015[47] = 0; - out_7830124823924821015[48] = 0; - out_7830124823924821015[49] = 0; - out_7830124823924821015[50] = 1; - out_7830124823924821015[51] = 0; - out_7830124823924821015[52] = 0; - out_7830124823924821015[53] = 1; +void H_10(double *state, double *unused, double *out_6010400890176312817) { + out_6010400890176312817[0] = 0; + out_6010400890176312817[1] = 9.8100000000000005*cos(state[1]); + out_6010400890176312817[2] = 0; + out_6010400890176312817[3] = 0; + out_6010400890176312817[4] = -state[8]; + out_6010400890176312817[5] = state[7]; + out_6010400890176312817[6] = 0; + out_6010400890176312817[7] = state[5]; + out_6010400890176312817[8] = -state[4]; + out_6010400890176312817[9] = 0; + out_6010400890176312817[10] = 0; + out_6010400890176312817[11] = 0; + out_6010400890176312817[12] = 1; + out_6010400890176312817[13] = 0; + out_6010400890176312817[14] = 0; + out_6010400890176312817[15] = 1; + out_6010400890176312817[16] = 0; + out_6010400890176312817[17] = 0; + out_6010400890176312817[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_6010400890176312817[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_6010400890176312817[20] = 0; + out_6010400890176312817[21] = state[8]; + out_6010400890176312817[22] = 0; + out_6010400890176312817[23] = -state[6]; + out_6010400890176312817[24] = -state[5]; + out_6010400890176312817[25] = 0; + out_6010400890176312817[26] = state[3]; + out_6010400890176312817[27] = 0; + out_6010400890176312817[28] = 0; + out_6010400890176312817[29] = 0; + out_6010400890176312817[30] = 0; + out_6010400890176312817[31] = 1; + out_6010400890176312817[32] = 0; + out_6010400890176312817[33] = 0; + out_6010400890176312817[34] = 1; + out_6010400890176312817[35] = 0; + out_6010400890176312817[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_6010400890176312817[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_6010400890176312817[38] = 0; + out_6010400890176312817[39] = -state[7]; + out_6010400890176312817[40] = state[6]; + out_6010400890176312817[41] = 0; + out_6010400890176312817[42] = state[4]; + out_6010400890176312817[43] = -state[3]; + out_6010400890176312817[44] = 0; + out_6010400890176312817[45] = 0; + out_6010400890176312817[46] = 0; + out_6010400890176312817[47] = 0; + out_6010400890176312817[48] = 0; + out_6010400890176312817[49] = 0; + out_6010400890176312817[50] = 1; + out_6010400890176312817[51] = 0; + out_6010400890176312817[52] = 0; + out_6010400890176312817[53] = 1; } -void h_13(double *state, double *unused, double *out_7135402855206141384) { - out_7135402855206141384[0] = state[3]; - out_7135402855206141384[1] = state[4]; - out_7135402855206141384[2] = state[5]; +void h_13(double *state, double *unused, double *out_3259369495818676553) { + out_3259369495818676553[0] = state[3]; + out_3259369495818676553[1] = state[4]; + out_3259369495818676553[2] = state[5]; } -void H_13(double *state, double *unused, double *out_8791398435640034877) { - out_8791398435640034877[0] = 0; - out_8791398435640034877[1] = 0; - out_8791398435640034877[2] = 0; - out_8791398435640034877[3] = 1; - out_8791398435640034877[4] = 0; - out_8791398435640034877[5] = 0; - out_8791398435640034877[6] = 0; - out_8791398435640034877[7] = 0; - out_8791398435640034877[8] = 0; - out_8791398435640034877[9] = 0; - out_8791398435640034877[10] = 0; - out_8791398435640034877[11] = 0; - out_8791398435640034877[12] = 0; - out_8791398435640034877[13] = 0; - out_8791398435640034877[14] = 0; - out_8791398435640034877[15] = 0; - out_8791398435640034877[16] = 0; - out_8791398435640034877[17] = 0; - out_8791398435640034877[18] = 0; - out_8791398435640034877[19] = 0; - out_8791398435640034877[20] = 0; - out_8791398435640034877[21] = 0; - out_8791398435640034877[22] = 1; - out_8791398435640034877[23] = 0; - out_8791398435640034877[24] = 0; - out_8791398435640034877[25] = 0; - out_8791398435640034877[26] = 0; - out_8791398435640034877[27] = 0; - out_8791398435640034877[28] = 0; - out_8791398435640034877[29] = 0; - out_8791398435640034877[30] = 0; - out_8791398435640034877[31] = 0; - out_8791398435640034877[32] = 0; - out_8791398435640034877[33] = 0; - out_8791398435640034877[34] = 0; - out_8791398435640034877[35] = 0; - out_8791398435640034877[36] = 0; - out_8791398435640034877[37] = 0; - out_8791398435640034877[38] = 0; - out_8791398435640034877[39] = 0; - out_8791398435640034877[40] = 0; - out_8791398435640034877[41] = 1; - out_8791398435640034877[42] = 0; - out_8791398435640034877[43] = 0; - out_8791398435640034877[44] = 0; - out_8791398435640034877[45] = 0; - out_8791398435640034877[46] = 0; - out_8791398435640034877[47] = 0; - out_8791398435640034877[48] = 0; - out_8791398435640034877[49] = 0; - out_8791398435640034877[50] = 0; - out_8791398435640034877[51] = 0; - out_8791398435640034877[52] = 0; - out_8791398435640034877[53] = 0; +void H_13(double *state, double *unused, double *out_5618744652209136283) { + out_5618744652209136283[0] = 0; + out_5618744652209136283[1] = 0; + out_5618744652209136283[2] = 0; + out_5618744652209136283[3] = 1; + out_5618744652209136283[4] = 0; + out_5618744652209136283[5] = 0; + out_5618744652209136283[6] = 0; + out_5618744652209136283[7] = 0; + out_5618744652209136283[8] = 0; + out_5618744652209136283[9] = 0; + out_5618744652209136283[10] = 0; + out_5618744652209136283[11] = 0; + out_5618744652209136283[12] = 0; + out_5618744652209136283[13] = 0; + out_5618744652209136283[14] = 0; + out_5618744652209136283[15] = 0; + out_5618744652209136283[16] = 0; + out_5618744652209136283[17] = 0; + out_5618744652209136283[18] = 0; + out_5618744652209136283[19] = 0; + out_5618744652209136283[20] = 0; + out_5618744652209136283[21] = 0; + out_5618744652209136283[22] = 1; + out_5618744652209136283[23] = 0; + out_5618744652209136283[24] = 0; + out_5618744652209136283[25] = 0; + out_5618744652209136283[26] = 0; + out_5618744652209136283[27] = 0; + out_5618744652209136283[28] = 0; + out_5618744652209136283[29] = 0; + out_5618744652209136283[30] = 0; + out_5618744652209136283[31] = 0; + out_5618744652209136283[32] = 0; + out_5618744652209136283[33] = 0; + out_5618744652209136283[34] = 0; + out_5618744652209136283[35] = 0; + out_5618744652209136283[36] = 0; + out_5618744652209136283[37] = 0; + out_5618744652209136283[38] = 0; + out_5618744652209136283[39] = 0; + out_5618744652209136283[40] = 0; + out_5618744652209136283[41] = 1; + out_5618744652209136283[42] = 0; + out_5618744652209136283[43] = 0; + out_5618744652209136283[44] = 0; + out_5618744652209136283[45] = 0; + out_5618744652209136283[46] = 0; + out_5618744652209136283[47] = 0; + out_5618744652209136283[48] = 0; + out_5618744652209136283[49] = 0; + out_5618744652209136283[50] = 0; + out_5618744652209136283[51] = 0; + out_5618744652209136283[52] = 0; + out_5618744652209136283[53] = 0; } -void h_14(double *state, double *unused, double *out_7043440464612625298) { - out_7043440464612625298[0] = state[6]; - out_7043440464612625298[1] = state[7]; - out_7043440464612625298[2] = state[8]; +void h_14(double *state, double *unused, double *out_1539412177960834763) { + out_1539412177960834763[0] = state[6]; + out_1539412177960834763[1] = state[7]; + out_1539412177960834763[2] = state[8]; } -void H_14(double *state, double *unused, double *out_3360283380441811642) { - out_3360283380441811642[0] = 0; - out_3360283380441811642[1] = 0; - out_3360283380441811642[2] = 0; - out_3360283380441811642[3] = 0; - out_3360283380441811642[4] = 0; - out_3360283380441811642[5] = 0; - out_3360283380441811642[6] = 1; - out_3360283380441811642[7] = 0; - out_3360283380441811642[8] = 0; - out_3360283380441811642[9] = 0; - out_3360283380441811642[10] = 0; - out_3360283380441811642[11] = 0; - out_3360283380441811642[12] = 0; - out_3360283380441811642[13] = 0; - out_3360283380441811642[14] = 0; - out_3360283380441811642[15] = 0; - out_3360283380441811642[16] = 0; - out_3360283380441811642[17] = 0; - out_3360283380441811642[18] = 0; - out_3360283380441811642[19] = 0; - out_3360283380441811642[20] = 0; - out_3360283380441811642[21] = 0; - out_3360283380441811642[22] = 0; - out_3360283380441811642[23] = 0; - out_3360283380441811642[24] = 0; - out_3360283380441811642[25] = 1; - out_3360283380441811642[26] = 0; - out_3360283380441811642[27] = 0; - out_3360283380441811642[28] = 0; - out_3360283380441811642[29] = 0; - out_3360283380441811642[30] = 0; - out_3360283380441811642[31] = 0; - out_3360283380441811642[32] = 0; - out_3360283380441811642[33] = 0; - out_3360283380441811642[34] = 0; - out_3360283380441811642[35] = 0; - out_3360283380441811642[36] = 0; - out_3360283380441811642[37] = 0; - out_3360283380441811642[38] = 0; - out_3360283380441811642[39] = 0; - out_3360283380441811642[40] = 0; - out_3360283380441811642[41] = 0; - out_3360283380441811642[42] = 0; - out_3360283380441811642[43] = 0; - out_3360283380441811642[44] = 1; - out_3360283380441811642[45] = 0; - out_3360283380441811642[46] = 0; - out_3360283380441811642[47] = 0; - out_3360283380441811642[48] = 0; - out_3360283380441811642[49] = 0; - out_3360283380441811642[50] = 0; - out_3360283380441811642[51] = 0; - out_3360283380441811642[52] = 0; - out_3360283380441811642[53] = 0; +void H_14(double *state, double *unused, double *out_676317605418568814) { + out_676317605418568814[0] = 0; + out_676317605418568814[1] = 0; + out_676317605418568814[2] = 0; + out_676317605418568814[3] = 0; + out_676317605418568814[4] = 0; + out_676317605418568814[5] = 0; + out_676317605418568814[6] = 1; + out_676317605418568814[7] = 0; + out_676317605418568814[8] = 0; + out_676317605418568814[9] = 0; + out_676317605418568814[10] = 0; + out_676317605418568814[11] = 0; + out_676317605418568814[12] = 0; + out_676317605418568814[13] = 0; + out_676317605418568814[14] = 0; + out_676317605418568814[15] = 0; + out_676317605418568814[16] = 0; + out_676317605418568814[17] = 0; + out_676317605418568814[18] = 0; + out_676317605418568814[19] = 0; + out_676317605418568814[20] = 0; + out_676317605418568814[21] = 0; + out_676317605418568814[22] = 0; + out_676317605418568814[23] = 0; + out_676317605418568814[24] = 0; + out_676317605418568814[25] = 1; + out_676317605418568814[26] = 0; + out_676317605418568814[27] = 0; + out_676317605418568814[28] = 0; + out_676317605418568814[29] = 0; + out_676317605418568814[30] = 0; + out_676317605418568814[31] = 0; + out_676317605418568814[32] = 0; + out_676317605418568814[33] = 0; + out_676317605418568814[34] = 0; + out_676317605418568814[35] = 0; + out_676317605418568814[36] = 0; + out_676317605418568814[37] = 0; + out_676317605418568814[38] = 0; + out_676317605418568814[39] = 0; + out_676317605418568814[40] = 0; + out_676317605418568814[41] = 0; + out_676317605418568814[42] = 0; + out_676317605418568814[43] = 0; + out_676317605418568814[44] = 1; + out_676317605418568814[45] = 0; + out_676317605418568814[46] = 0; + out_676317605418568814[47] = 0; + out_676317605418568814[48] = 0; + out_676317605418568814[49] = 0; + out_676317605418568814[50] = 0; + out_676317605418568814[51] = 0; + out_676317605418568814[52] = 0; + out_676317605418568814[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_8940459122146932400) { - err_fun(nom_x, delta_x, out_8940459122146932400); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7536058918625145808) { + err_fun(nom_x, delta_x, out_7536058918625145808); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3384987028755125218) { - inv_err_fun(nom_x, true_x, out_3384987028755125218); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6294980525542920357) { + inv_err_fun(nom_x, true_x, out_6294980525542920357); } -void pose_H_mod_fun(double *state, double *out_5245124330854285834) { - H_mod_fun(state, out_5245124330854285834); +void pose_H_mod_fun(double *state, double *out_1208523344993905378) { + H_mod_fun(state, out_1208523344993905378); } -void pose_f_fun(double *state, double dt, double *out_6830900184565569763) { - f_fun(state, dt, out_6830900184565569763); +void pose_f_fun(double *state, double dt, double *out_828336908542161324) { + f_fun(state, dt, out_828336908542161324); } -void pose_F_fun(double *state, double dt, double *out_5118094281500581129) { - F_fun(state, dt, out_5118094281500581129); +void pose_F_fun(double *state, double dt, double *out_5703387388084628727) { + F_fun(state, dt, out_5703387388084628727); } -void pose_h_4(double *state, double *unused, double *out_4935070827926002458) { - h_4(state, unused, out_4935070827926002458); +void pose_h_4(double *state, double *unused, double *out_2600666041609585625) { + h_4(state, unused, out_2600666041609585625); } -void pose_H_4(double *state, double *unused, double *out_6443071812737183938) { - H_4(state, unused, out_6443071812737183938); +void pose_H_4(double *state, double *unused, double *out_2406470826876803482) { + H_4(state, unused, out_2406470826876803482); } -void pose_h_10(double *state, double *unused, double *out_2021668210924663063) { - h_10(state, unused, out_2021668210924663063); +void pose_h_10(double *state, double *unused, double *out_2042730394265084466) { + h_10(state, unused, out_2042730394265084466); } -void pose_H_10(double *state, double *unused, double *out_7830124823924821015) { - H_10(state, unused, out_7830124823924821015); +void pose_H_10(double *state, double *unused, double *out_6010400890176312817) { + H_10(state, unused, out_6010400890176312817); } -void pose_h_13(double *state, double *unused, double *out_7135402855206141384) { - h_13(state, unused, out_7135402855206141384); +void pose_h_13(double *state, double *unused, double *out_3259369495818676553) { + h_13(state, unused, out_3259369495818676553); } -void pose_H_13(double *state, double *unused, double *out_8791398435640034877) { - H_13(state, unused, out_8791398435640034877); +void pose_H_13(double *state, double *unused, double *out_5618744652209136283) { + H_13(state, unused, out_5618744652209136283); } -void pose_h_14(double *state, double *unused, double *out_7043440464612625298) { - h_14(state, unused, out_7043440464612625298); +void pose_h_14(double *state, double *unused, double *out_1539412177960834763) { + h_14(state, unused, out_1539412177960834763); } -void pose_H_14(double *state, double *unused, double *out_3360283380441811642) { - H_14(state, unused, out_3360283380441811642); +void pose_H_14(double *state, double *unused, double *out_676317605418568814) { + H_14(state, unused, out_676317605418568814); } 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 ee9159d12..46d481b6f 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_8940459122146932400); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3384987028755125218); -void pose_H_mod_fun(double *state, double *out_5245124330854285834); -void pose_f_fun(double *state, double dt, double *out_6830900184565569763); -void pose_F_fun(double *state, double dt, double *out_5118094281500581129); -void pose_h_4(double *state, double *unused, double *out_4935070827926002458); -void pose_H_4(double *state, double *unused, double *out_6443071812737183938); -void pose_h_10(double *state, double *unused, double *out_2021668210924663063); -void pose_H_10(double *state, double *unused, double *out_7830124823924821015); -void pose_h_13(double *state, double *unused, double *out_7135402855206141384); -void pose_H_13(double *state, double *unused, double *out_8791398435640034877); -void pose_h_14(double *state, double *unused, double *out_7043440464612625298); -void pose_H_14(double *state, double *unused, double *out_3360283380441811642); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7536058918625145808); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6294980525542920357); +void pose_H_mod_fun(double *state, double *out_1208523344993905378); +void pose_f_fun(double *state, double dt, double *out_828336908542161324); +void pose_F_fun(double *state, double dt, double *out_5703387388084628727); +void pose_h_4(double *state, double *unused, double *out_2600666041609585625); +void pose_H_4(double *state, double *unused, double *out_2406470826876803482); +void pose_h_10(double *state, double *unused, double *out_2042730394265084466); +void pose_H_10(double *state, double *unused, double *out_6010400890176312817); +void pose_h_13(double *state, double *unused, double *out_3259369495818676553); +void pose_H_13(double *state, double *unused, double *out_5618744652209136283); +void pose_h_14(double *state, double *unused, double *out_1539412177960834763); +void pose_H_14(double *state, double *unused, double *out_676317605418568814); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index a730076d4..2f7137186 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 49e849592..92f07382f 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 0194af73f..480d69085 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 890117fb4..9aba3f0af 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 31cd8ac54..b4baa918c 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index cacd5a9e3..62fb7bfed 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ