diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 16899bf2c..595e3b7e4 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 576c51d49..36d5ee6d2 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 6eb332c18..40bd36ef9 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 d709df9a8..1bf155e71 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 c678b5d10..14f4fcbb5 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 f2b5c671b..e6520d6a4 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 0c2dcc81d..18e52f7ea 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 0c2dcc81d..18e52f7ea 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 c59188511..f06cc17d2 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 f2b5c671b..e6520d6a4 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 6822c59f8..32d1b5df7 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-75dcc077-DEBUG"; +const uint8_t gitversion[19] = "DEV-9d8a055a-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 78777d42a..646456850 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 925de2168..10c244af3 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 b5ed3c1b4..a24c29993 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 c509202b2..7b4547ad1 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 1d6290e4a..dcb5eb78f 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 f750de6bf..47b59e3ab 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 315b4edb7..7b4753217 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 b716cd504..673171001 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 a20afa9fd..66bd7d420 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 5708e4058..9773d34c5 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 6d104c490..eccab39e8 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 417ee7255..2b9d23363 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 84f5c264d..1fbcaf127 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 02cece1f3..163467550 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 867e76990..12e67d70f 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 54195979f..f8525e80c 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 fd8f4d151..c268a2f67 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 08986a200..09433b7cf 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 676798c23..f5c6cdf65 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 3f8b44788..c3783ff75 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 ca84cb69d..e67a37ad2 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-75dcc077-DEBUG \ No newline at end of file +DEV-9d8a055a-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 0ae251c36..732a0b140 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_5591921399563932369) { - out_5591921399563932369[0] = delta_x[0] + nom_x[0]; - out_5591921399563932369[1] = delta_x[1] + nom_x[1]; - out_5591921399563932369[2] = delta_x[2] + nom_x[2]; - out_5591921399563932369[3] = delta_x[3] + nom_x[3]; - out_5591921399563932369[4] = delta_x[4] + nom_x[4]; - out_5591921399563932369[5] = delta_x[5] + nom_x[5]; - out_5591921399563932369[6] = delta_x[6] + nom_x[6]; - out_5591921399563932369[7] = delta_x[7] + nom_x[7]; - out_5591921399563932369[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_587611173581676283) { + out_587611173581676283[0] = delta_x[0] + nom_x[0]; + out_587611173581676283[1] = delta_x[1] + nom_x[1]; + out_587611173581676283[2] = delta_x[2] + nom_x[2]; + out_587611173581676283[3] = delta_x[3] + nom_x[3]; + out_587611173581676283[4] = delta_x[4] + nom_x[4]; + out_587611173581676283[5] = delta_x[5] + nom_x[5]; + out_587611173581676283[6] = delta_x[6] + nom_x[6]; + out_587611173581676283[7] = delta_x[7] + nom_x[7]; + out_587611173581676283[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_806607420895539823) { - out_806607420895539823[0] = -nom_x[0] + true_x[0]; - out_806607420895539823[1] = -nom_x[1] + true_x[1]; - out_806607420895539823[2] = -nom_x[2] + true_x[2]; - out_806607420895539823[3] = -nom_x[3] + true_x[3]; - out_806607420895539823[4] = -nom_x[4] + true_x[4]; - out_806607420895539823[5] = -nom_x[5] + true_x[5]; - out_806607420895539823[6] = -nom_x[6] + true_x[6]; - out_806607420895539823[7] = -nom_x[7] + true_x[7]; - out_806607420895539823[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8120728011522269919) { + out_8120728011522269919[0] = -nom_x[0] + true_x[0]; + out_8120728011522269919[1] = -nom_x[1] + true_x[1]; + out_8120728011522269919[2] = -nom_x[2] + true_x[2]; + out_8120728011522269919[3] = -nom_x[3] + true_x[3]; + out_8120728011522269919[4] = -nom_x[4] + true_x[4]; + out_8120728011522269919[5] = -nom_x[5] + true_x[5]; + out_8120728011522269919[6] = -nom_x[6] + true_x[6]; + out_8120728011522269919[7] = -nom_x[7] + true_x[7]; + out_8120728011522269919[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_5676707820504431668) { - out_5676707820504431668[0] = 1.0; - out_5676707820504431668[1] = 0.0; - out_5676707820504431668[2] = 0.0; - out_5676707820504431668[3] = 0.0; - out_5676707820504431668[4] = 0.0; - out_5676707820504431668[5] = 0.0; - out_5676707820504431668[6] = 0.0; - out_5676707820504431668[7] = 0.0; - out_5676707820504431668[8] = 0.0; - out_5676707820504431668[9] = 0.0; - out_5676707820504431668[10] = 1.0; - out_5676707820504431668[11] = 0.0; - out_5676707820504431668[12] = 0.0; - out_5676707820504431668[13] = 0.0; - out_5676707820504431668[14] = 0.0; - out_5676707820504431668[15] = 0.0; - out_5676707820504431668[16] = 0.0; - out_5676707820504431668[17] = 0.0; - out_5676707820504431668[18] = 0.0; - out_5676707820504431668[19] = 0.0; - out_5676707820504431668[20] = 1.0; - out_5676707820504431668[21] = 0.0; - out_5676707820504431668[22] = 0.0; - out_5676707820504431668[23] = 0.0; - out_5676707820504431668[24] = 0.0; - out_5676707820504431668[25] = 0.0; - out_5676707820504431668[26] = 0.0; - out_5676707820504431668[27] = 0.0; - out_5676707820504431668[28] = 0.0; - out_5676707820504431668[29] = 0.0; - out_5676707820504431668[30] = 1.0; - out_5676707820504431668[31] = 0.0; - out_5676707820504431668[32] = 0.0; - out_5676707820504431668[33] = 0.0; - out_5676707820504431668[34] = 0.0; - out_5676707820504431668[35] = 0.0; - out_5676707820504431668[36] = 0.0; - out_5676707820504431668[37] = 0.0; - out_5676707820504431668[38] = 0.0; - out_5676707820504431668[39] = 0.0; - out_5676707820504431668[40] = 1.0; - out_5676707820504431668[41] = 0.0; - out_5676707820504431668[42] = 0.0; - out_5676707820504431668[43] = 0.0; - out_5676707820504431668[44] = 0.0; - out_5676707820504431668[45] = 0.0; - out_5676707820504431668[46] = 0.0; - out_5676707820504431668[47] = 0.0; - out_5676707820504431668[48] = 0.0; - out_5676707820504431668[49] = 0.0; - out_5676707820504431668[50] = 1.0; - out_5676707820504431668[51] = 0.0; - out_5676707820504431668[52] = 0.0; - out_5676707820504431668[53] = 0.0; - out_5676707820504431668[54] = 0.0; - out_5676707820504431668[55] = 0.0; - out_5676707820504431668[56] = 0.0; - out_5676707820504431668[57] = 0.0; - out_5676707820504431668[58] = 0.0; - out_5676707820504431668[59] = 0.0; - out_5676707820504431668[60] = 1.0; - out_5676707820504431668[61] = 0.0; - out_5676707820504431668[62] = 0.0; - out_5676707820504431668[63] = 0.0; - out_5676707820504431668[64] = 0.0; - out_5676707820504431668[65] = 0.0; - out_5676707820504431668[66] = 0.0; - out_5676707820504431668[67] = 0.0; - out_5676707820504431668[68] = 0.0; - out_5676707820504431668[69] = 0.0; - out_5676707820504431668[70] = 1.0; - out_5676707820504431668[71] = 0.0; - out_5676707820504431668[72] = 0.0; - out_5676707820504431668[73] = 0.0; - out_5676707820504431668[74] = 0.0; - out_5676707820504431668[75] = 0.0; - out_5676707820504431668[76] = 0.0; - out_5676707820504431668[77] = 0.0; - out_5676707820504431668[78] = 0.0; - out_5676707820504431668[79] = 0.0; - out_5676707820504431668[80] = 1.0; +void H_mod_fun(double *state, double *out_4001675034436284807) { + out_4001675034436284807[0] = 1.0; + out_4001675034436284807[1] = 0.0; + out_4001675034436284807[2] = 0.0; + out_4001675034436284807[3] = 0.0; + out_4001675034436284807[4] = 0.0; + out_4001675034436284807[5] = 0.0; + out_4001675034436284807[6] = 0.0; + out_4001675034436284807[7] = 0.0; + out_4001675034436284807[8] = 0.0; + out_4001675034436284807[9] = 0.0; + out_4001675034436284807[10] = 1.0; + out_4001675034436284807[11] = 0.0; + out_4001675034436284807[12] = 0.0; + out_4001675034436284807[13] = 0.0; + out_4001675034436284807[14] = 0.0; + out_4001675034436284807[15] = 0.0; + out_4001675034436284807[16] = 0.0; + out_4001675034436284807[17] = 0.0; + out_4001675034436284807[18] = 0.0; + out_4001675034436284807[19] = 0.0; + out_4001675034436284807[20] = 1.0; + out_4001675034436284807[21] = 0.0; + out_4001675034436284807[22] = 0.0; + out_4001675034436284807[23] = 0.0; + out_4001675034436284807[24] = 0.0; + out_4001675034436284807[25] = 0.0; + out_4001675034436284807[26] = 0.0; + out_4001675034436284807[27] = 0.0; + out_4001675034436284807[28] = 0.0; + out_4001675034436284807[29] = 0.0; + out_4001675034436284807[30] = 1.0; + out_4001675034436284807[31] = 0.0; + out_4001675034436284807[32] = 0.0; + out_4001675034436284807[33] = 0.0; + out_4001675034436284807[34] = 0.0; + out_4001675034436284807[35] = 0.0; + out_4001675034436284807[36] = 0.0; + out_4001675034436284807[37] = 0.0; + out_4001675034436284807[38] = 0.0; + out_4001675034436284807[39] = 0.0; + out_4001675034436284807[40] = 1.0; + out_4001675034436284807[41] = 0.0; + out_4001675034436284807[42] = 0.0; + out_4001675034436284807[43] = 0.0; + out_4001675034436284807[44] = 0.0; + out_4001675034436284807[45] = 0.0; + out_4001675034436284807[46] = 0.0; + out_4001675034436284807[47] = 0.0; + out_4001675034436284807[48] = 0.0; + out_4001675034436284807[49] = 0.0; + out_4001675034436284807[50] = 1.0; + out_4001675034436284807[51] = 0.0; + out_4001675034436284807[52] = 0.0; + out_4001675034436284807[53] = 0.0; + out_4001675034436284807[54] = 0.0; + out_4001675034436284807[55] = 0.0; + out_4001675034436284807[56] = 0.0; + out_4001675034436284807[57] = 0.0; + out_4001675034436284807[58] = 0.0; + out_4001675034436284807[59] = 0.0; + out_4001675034436284807[60] = 1.0; + out_4001675034436284807[61] = 0.0; + out_4001675034436284807[62] = 0.0; + out_4001675034436284807[63] = 0.0; + out_4001675034436284807[64] = 0.0; + out_4001675034436284807[65] = 0.0; + out_4001675034436284807[66] = 0.0; + out_4001675034436284807[67] = 0.0; + out_4001675034436284807[68] = 0.0; + out_4001675034436284807[69] = 0.0; + out_4001675034436284807[70] = 1.0; + out_4001675034436284807[71] = 0.0; + out_4001675034436284807[72] = 0.0; + out_4001675034436284807[73] = 0.0; + out_4001675034436284807[74] = 0.0; + out_4001675034436284807[75] = 0.0; + out_4001675034436284807[76] = 0.0; + out_4001675034436284807[77] = 0.0; + out_4001675034436284807[78] = 0.0; + out_4001675034436284807[79] = 0.0; + out_4001675034436284807[80] = 1.0; } -void f_fun(double *state, double dt, double *out_4844917589263296834) { - out_4844917589263296834[0] = state[0]; - out_4844917589263296834[1] = state[1]; - out_4844917589263296834[2] = state[2]; - out_4844917589263296834[3] = state[3]; - out_4844917589263296834[4] = state[4]; - out_4844917589263296834[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_4844917589263296834[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_4844917589263296834[7] = state[7]; - out_4844917589263296834[8] = state[8]; +void f_fun(double *state, double dt, double *out_6995360374056238489) { + out_6995360374056238489[0] = state[0]; + out_6995360374056238489[1] = state[1]; + out_6995360374056238489[2] = state[2]; + out_6995360374056238489[3] = state[3]; + out_6995360374056238489[4] = state[4]; + out_6995360374056238489[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_6995360374056238489[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_6995360374056238489[7] = state[7]; + out_6995360374056238489[8] = state[8]; } -void F_fun(double *state, double dt, double *out_4636534629313466670) { - out_4636534629313466670[0] = 1; - out_4636534629313466670[1] = 0; - out_4636534629313466670[2] = 0; - out_4636534629313466670[3] = 0; - out_4636534629313466670[4] = 0; - out_4636534629313466670[5] = 0; - out_4636534629313466670[6] = 0; - out_4636534629313466670[7] = 0; - out_4636534629313466670[8] = 0; - out_4636534629313466670[9] = 0; - out_4636534629313466670[10] = 1; - out_4636534629313466670[11] = 0; - out_4636534629313466670[12] = 0; - out_4636534629313466670[13] = 0; - out_4636534629313466670[14] = 0; - out_4636534629313466670[15] = 0; - out_4636534629313466670[16] = 0; - out_4636534629313466670[17] = 0; - out_4636534629313466670[18] = 0; - out_4636534629313466670[19] = 0; - out_4636534629313466670[20] = 1; - out_4636534629313466670[21] = 0; - out_4636534629313466670[22] = 0; - out_4636534629313466670[23] = 0; - out_4636534629313466670[24] = 0; - out_4636534629313466670[25] = 0; - out_4636534629313466670[26] = 0; - out_4636534629313466670[27] = 0; - out_4636534629313466670[28] = 0; - out_4636534629313466670[29] = 0; - out_4636534629313466670[30] = 1; - out_4636534629313466670[31] = 0; - out_4636534629313466670[32] = 0; - out_4636534629313466670[33] = 0; - out_4636534629313466670[34] = 0; - out_4636534629313466670[35] = 0; - out_4636534629313466670[36] = 0; - out_4636534629313466670[37] = 0; - out_4636534629313466670[38] = 0; - out_4636534629313466670[39] = 0; - out_4636534629313466670[40] = 1; - out_4636534629313466670[41] = 0; - out_4636534629313466670[42] = 0; - out_4636534629313466670[43] = 0; - out_4636534629313466670[44] = 0; - out_4636534629313466670[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_4636534629313466670[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_4636534629313466670[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4636534629313466670[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4636534629313466670[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_4636534629313466670[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_4636534629313466670[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_4636534629313466670[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_4636534629313466670[53] = -9.8100000000000005*dt; - out_4636534629313466670[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_4636534629313466670[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_4636534629313466670[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4636534629313466670[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4636534629313466670[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_4636534629313466670[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_4636534629313466670[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_4636534629313466670[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4636534629313466670[62] = 0; - out_4636534629313466670[63] = 0; - out_4636534629313466670[64] = 0; - out_4636534629313466670[65] = 0; - out_4636534629313466670[66] = 0; - out_4636534629313466670[67] = 0; - out_4636534629313466670[68] = 0; - out_4636534629313466670[69] = 0; - out_4636534629313466670[70] = 1; - out_4636534629313466670[71] = 0; - out_4636534629313466670[72] = 0; - out_4636534629313466670[73] = 0; - out_4636534629313466670[74] = 0; - out_4636534629313466670[75] = 0; - out_4636534629313466670[76] = 0; - out_4636534629313466670[77] = 0; - out_4636534629313466670[78] = 0; - out_4636534629313466670[79] = 0; - out_4636534629313466670[80] = 1; +void F_fun(double *state, double dt, double *out_5620828310005666299) { + out_5620828310005666299[0] = 1; + out_5620828310005666299[1] = 0; + out_5620828310005666299[2] = 0; + out_5620828310005666299[3] = 0; + out_5620828310005666299[4] = 0; + out_5620828310005666299[5] = 0; + out_5620828310005666299[6] = 0; + out_5620828310005666299[7] = 0; + out_5620828310005666299[8] = 0; + out_5620828310005666299[9] = 0; + out_5620828310005666299[10] = 1; + out_5620828310005666299[11] = 0; + out_5620828310005666299[12] = 0; + out_5620828310005666299[13] = 0; + out_5620828310005666299[14] = 0; + out_5620828310005666299[15] = 0; + out_5620828310005666299[16] = 0; + out_5620828310005666299[17] = 0; + out_5620828310005666299[18] = 0; + out_5620828310005666299[19] = 0; + out_5620828310005666299[20] = 1; + out_5620828310005666299[21] = 0; + out_5620828310005666299[22] = 0; + out_5620828310005666299[23] = 0; + out_5620828310005666299[24] = 0; + out_5620828310005666299[25] = 0; + out_5620828310005666299[26] = 0; + out_5620828310005666299[27] = 0; + out_5620828310005666299[28] = 0; + out_5620828310005666299[29] = 0; + out_5620828310005666299[30] = 1; + out_5620828310005666299[31] = 0; + out_5620828310005666299[32] = 0; + out_5620828310005666299[33] = 0; + out_5620828310005666299[34] = 0; + out_5620828310005666299[35] = 0; + out_5620828310005666299[36] = 0; + out_5620828310005666299[37] = 0; + out_5620828310005666299[38] = 0; + out_5620828310005666299[39] = 0; + out_5620828310005666299[40] = 1; + out_5620828310005666299[41] = 0; + out_5620828310005666299[42] = 0; + out_5620828310005666299[43] = 0; + out_5620828310005666299[44] = 0; + out_5620828310005666299[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_5620828310005666299[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_5620828310005666299[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_5620828310005666299[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_5620828310005666299[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_5620828310005666299[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_5620828310005666299[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_5620828310005666299[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_5620828310005666299[53] = -9.8100000000000005*dt; + out_5620828310005666299[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_5620828310005666299[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_5620828310005666299[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_5620828310005666299[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_5620828310005666299[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_5620828310005666299[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_5620828310005666299[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_5620828310005666299[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_5620828310005666299[62] = 0; + out_5620828310005666299[63] = 0; + out_5620828310005666299[64] = 0; + out_5620828310005666299[65] = 0; + out_5620828310005666299[66] = 0; + out_5620828310005666299[67] = 0; + out_5620828310005666299[68] = 0; + out_5620828310005666299[69] = 0; + out_5620828310005666299[70] = 1; + out_5620828310005666299[71] = 0; + out_5620828310005666299[72] = 0; + out_5620828310005666299[73] = 0; + out_5620828310005666299[74] = 0; + out_5620828310005666299[75] = 0; + out_5620828310005666299[76] = 0; + out_5620828310005666299[77] = 0; + out_5620828310005666299[78] = 0; + out_5620828310005666299[79] = 0; + out_5620828310005666299[80] = 1; } -void h_25(double *state, double *unused, double *out_5482647601852736049) { - out_5482647601852736049[0] = state[6]; +void h_25(double *state, double *unused, double *out_5932710635121359621) { + out_5932710635121359621[0] = state[6]; } -void H_25(double *state, double *unused, double *out_7866344428020455108) { - out_7866344428020455108[0] = 0; - out_7866344428020455108[1] = 0; - out_7866344428020455108[2] = 0; - out_7866344428020455108[3] = 0; - out_7866344428020455108[4] = 0; - out_7866344428020455108[5] = 0; - out_7866344428020455108[6] = 1; - out_7866344428020455108[7] = 0; - out_7866344428020455108[8] = 0; +void H_25(double *state, double *unused, double *out_4507009476636581519) { + out_4507009476636581519[0] = 0; + out_4507009476636581519[1] = 0; + out_4507009476636581519[2] = 0; + out_4507009476636581519[3] = 0; + out_4507009476636581519[4] = 0; + out_4507009476636581519[5] = 0; + out_4507009476636581519[6] = 1; + out_4507009476636581519[7] = 0; + out_4507009476636581519[8] = 0; } -void h_24(double *state, double *unused, double *out_6159121467458312716) { - out_6159121467458312716[0] = state[4]; - out_6159121467458312716[1] = state[5]; +void h_24(double *state, double *unused, double *out_8960081603047522700) { + out_8960081603047522700[0] = state[4]; + out_8960081603047522700[1] = state[5]; } -void H_24(double *state, double *unused, double *out_3137725052700380188) { - out_3137725052700380188[0] = 0; - out_3137725052700380188[1] = 0; - out_3137725052700380188[2] = 0; - out_3137725052700380188[3] = 0; - out_3137725052700380188[4] = 1; - out_3137725052700380188[5] = 0; - out_3137725052700380188[6] = 0; - out_3137725052700380188[7] = 0; - out_3137725052700380188[8] = 0; - out_3137725052700380188[9] = 0; - out_3137725052700380188[10] = 0; - out_3137725052700380188[11] = 0; - out_3137725052700380188[12] = 0; - out_3137725052700380188[13] = 0; - out_3137725052700380188[14] = 1; - out_3137725052700380188[15] = 0; - out_3137725052700380188[16] = 0; - out_3137725052700380188[17] = 0; +void H_24(double *state, double *unused, double *out_7364162790481451996) { + out_7364162790481451996[0] = 0; + out_7364162790481451996[1] = 0; + out_7364162790481451996[2] = 0; + out_7364162790481451996[3] = 0; + out_7364162790481451996[4] = 1; + out_7364162790481451996[5] = 0; + out_7364162790481451996[6] = 0; + out_7364162790481451996[7] = 0; + out_7364162790481451996[8] = 0; + out_7364162790481451996[9] = 0; + out_7364162790481451996[10] = 0; + out_7364162790481451996[11] = 0; + out_7364162790481451996[12] = 0; + out_7364162790481451996[13] = 0; + out_7364162790481451996[14] = 1; + out_7364162790481451996[15] = 0; + out_7364162790481451996[16] = 0; + out_7364162790481451996[17] = 0; } -void h_30(double *state, double *unused, double *out_5757841664137241938) { - out_5757841664137241938[0] = state[4]; +void h_30(double *state, double *unused, double *out_6293656255741178026) { + out_6293656255741178026[0] = state[4]; } -void H_30(double *state, double *unused, double *out_7995683375163695178) { - out_7995683375163695178[0] = 0; - out_7995683375163695178[1] = 0; - out_7995683375163695178[2] = 0; - out_7995683375163695178[3] = 0; - out_7995683375163695178[4] = 1; - out_7995683375163695178[5] = 0; - out_7995683375163695178[6] = 0; - out_7995683375163695178[7] = 0; - out_7995683375163695178[8] = 0; +void H_30(double *state, double *unused, double *out_7023044255581353342) { + out_7023044255581353342[0] = 0; + out_7023044255581353342[1] = 0; + out_7023044255581353342[2] = 0; + out_7023044255581353342[3] = 0; + out_7023044255581353342[4] = 1; + out_7023044255581353342[5] = 0; + out_7023044255581353342[6] = 0; + out_7023044255581353342[7] = 0; + out_7023044255581353342[8] = 0; } -void h_26(double *state, double *unused, double *out_6701351517038904373) { - out_6701351517038904373[0] = state[7]; +void h_26(double *state, double *unused, double *out_7151414550307527945) { + out_7151414550307527945[0] = state[7]; } -void H_26(double *state, double *unused, double *out_6838896326815040284) { - out_6838896326815040284[0] = 0; - out_6838896326815040284[1] = 0; - out_6838896326815040284[2] = 0; - out_6838896326815040284[3] = 0; - out_6838896326815040284[4] = 0; - out_6838896326815040284[5] = 0; - out_6838896326815040284[6] = 0; - out_6838896326815040284[7] = 1; - out_6838896326815040284[8] = 0; +void H_26(double *state, double *unused, double *out_765506157762525295) { + out_765506157762525295[0] = 0; + out_765506157762525295[1] = 0; + out_765506157762525295[2] = 0; + out_765506157762525295[3] = 0; + out_765506157762525295[4] = 0; + out_765506157762525295[5] = 0; + out_765506157762525295[6] = 0; + out_765506157762525295[7] = 1; + out_765506157762525295[8] = 0; } -void h_27(double *state, double *unused, double *out_7962423204195969922) { - out_7962423204195969922[0] = state[3]; +void h_27(double *state, double *unused, double *out_7512360170927346350) { + out_7512360170927346350[0] = state[3]; } -void H_27(double *state, double *unused, double *out_8276297386745431527) { - out_8276297386745431527[0] = 0; - out_8276297386745431527[1] = 0; - out_8276297386745431527[2] = 0; - out_8276297386745431527[3] = 1; - out_8276297386745431527[4] = 0; - out_8276297386745431527[5] = 0; - out_8276297386745431527[6] = 0; - out_8276297386745431527[7] = 0; - out_8276297386745431527[8] = 0; +void H_27(double *state, double *unused, double *out_9197807567381778253) { + out_9197807567381778253[0] = 0; + out_9197807567381778253[1] = 0; + out_9197807567381778253[2] = 0; + out_9197807567381778253[3] = 1; + out_9197807567381778253[4] = 0; + out_9197807567381778253[5] = 0; + out_9197807567381778253[6] = 0; + out_9197807567381778253[7] = 0; + out_9197807567381778253[8] = 0; } -void h_29(double *state, double *unused, double *out_2721055612692909253) { - out_2721055612692909253[0] = state[1]; +void h_29(double *state, double *unused, double *out_3171118645961532825) { + out_3171118645961532825[0] = state[1]; } -void H_29(double *state, double *unused, double *out_6562934659875880494) { - out_6562934659875880494[0] = 0; - out_6562934659875880494[1] = 1; - out_6562934659875880494[2] = 0; - out_6562934659875880494[3] = 0; - out_6562934659875880494[4] = 0; - out_6562934659875880494[5] = 0; - out_6562934659875880494[6] = 0; - out_6562934659875880494[7] = 0; - out_6562934659875880494[8] = 0; +void H_29(double *state, double *unused, double *out_6512812911266961158) { + out_6512812911266961158[0] = 0; + out_6512812911266961158[1] = 1; + out_6512812911266961158[2] = 0; + out_6512812911266961158[3] = 0; + out_6512812911266961158[4] = 0; + out_6512812911266961158[5] = 0; + out_6512812911266961158[6] = 0; + out_6512812911266961158[7] = 0; + out_6512812911266961158[8] = 0; } -void h_28(double *state, double *unused, double *out_8408093249132323059) { - out_8408093249132323059[0] = state[0]; +void h_28(double *state, double *unused, double *out_8329978581973507615) { + out_8329978581973507615[0] = state[0]; } -void H_28(double *state, double *unused, double *out_1480535642806349920) { - out_1480535642806349920[0] = 1; - out_1480535642806349920[1] = 0; - out_1480535642806349920[2] = 0; - out_1480535642806349920[3] = 0; - out_1480535642806349920[4] = 0; - out_1480535642806349920[5] = 0; - out_1480535642806349920[6] = 0; - out_1480535642806349920[7] = 0; - out_1480535642806349920[8] = 0; +void H_28(double *state, double *unused, double *out_2453174762388691756) { + out_2453174762388691756[0] = 1; + out_2453174762388691756[1] = 0; + out_2453174762388691756[2] = 0; + out_2453174762388691756[3] = 0; + out_2453174762388691756[4] = 0; + out_2453174762388691756[5] = 0; + out_2453174762388691756[6] = 0; + out_2453174762388691756[7] = 0; + out_2453174762388691756[8] = 0; } -void h_31(double *state, double *unused, double *out_4467435790593075575) { - out_4467435790593075575[0] = state[8]; +void h_31(double *state, double *unused, double *out_222736560719300508) { + out_222736560719300508[0] = state[8]; } -void H_31(double *state, double *unused, double *out_7835698466143494680) { - out_7835698466143494680[0] = 0; - out_7835698466143494680[1] = 0; - out_7835698466143494680[2] = 0; - out_7835698466143494680[3] = 0; - out_7835698466143494680[4] = 0; - out_7835698466143494680[5] = 0; - out_7835698466143494680[6] = 0; - out_7835698466143494680[7] = 0; - out_7835698466143494680[8] = 1; +void H_31(double *state, double *unused, double *out_4537655438513541947) { + out_4537655438513541947[0] = 0; + out_4537655438513541947[1] = 0; + out_4537655438513541947[2] = 0; + out_4537655438513541947[3] = 0; + out_4537655438513541947[4] = 0; + out_4537655438513541947[5] = 0; + out_4537655438513541947[6] = 0; + out_4537655438513541947[7] = 0; + out_4537655438513541947[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_5591921399563932369) { - err_fun(nom_x, delta_x, out_5591921399563932369); +void car_err_fun(double *nom_x, double *delta_x, double *out_587611173581676283) { + err_fun(nom_x, delta_x, out_587611173581676283); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_806607420895539823) { - inv_err_fun(nom_x, true_x, out_806607420895539823); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8120728011522269919) { + inv_err_fun(nom_x, true_x, out_8120728011522269919); } -void car_H_mod_fun(double *state, double *out_5676707820504431668) { - H_mod_fun(state, out_5676707820504431668); +void car_H_mod_fun(double *state, double *out_4001675034436284807) { + H_mod_fun(state, out_4001675034436284807); } -void car_f_fun(double *state, double dt, double *out_4844917589263296834) { - f_fun(state, dt, out_4844917589263296834); +void car_f_fun(double *state, double dt, double *out_6995360374056238489) { + f_fun(state, dt, out_6995360374056238489); } -void car_F_fun(double *state, double dt, double *out_4636534629313466670) { - F_fun(state, dt, out_4636534629313466670); +void car_F_fun(double *state, double dt, double *out_5620828310005666299) { + F_fun(state, dt, out_5620828310005666299); } -void car_h_25(double *state, double *unused, double *out_5482647601852736049) { - h_25(state, unused, out_5482647601852736049); +void car_h_25(double *state, double *unused, double *out_5932710635121359621) { + h_25(state, unused, out_5932710635121359621); } -void car_H_25(double *state, double *unused, double *out_7866344428020455108) { - H_25(state, unused, out_7866344428020455108); +void car_H_25(double *state, double *unused, double *out_4507009476636581519) { + H_25(state, unused, out_4507009476636581519); } -void car_h_24(double *state, double *unused, double *out_6159121467458312716) { - h_24(state, unused, out_6159121467458312716); +void car_h_24(double *state, double *unused, double *out_8960081603047522700) { + h_24(state, unused, out_8960081603047522700); } -void car_H_24(double *state, double *unused, double *out_3137725052700380188) { - H_24(state, unused, out_3137725052700380188); +void car_H_24(double *state, double *unused, double *out_7364162790481451996) { + H_24(state, unused, out_7364162790481451996); } -void car_h_30(double *state, double *unused, double *out_5757841664137241938) { - h_30(state, unused, out_5757841664137241938); +void car_h_30(double *state, double *unused, double *out_6293656255741178026) { + h_30(state, unused, out_6293656255741178026); } -void car_H_30(double *state, double *unused, double *out_7995683375163695178) { - H_30(state, unused, out_7995683375163695178); +void car_H_30(double *state, double *unused, double *out_7023044255581353342) { + H_30(state, unused, out_7023044255581353342); } -void car_h_26(double *state, double *unused, double *out_6701351517038904373) { - h_26(state, unused, out_6701351517038904373); +void car_h_26(double *state, double *unused, double *out_7151414550307527945) { + h_26(state, unused, out_7151414550307527945); } -void car_H_26(double *state, double *unused, double *out_6838896326815040284) { - H_26(state, unused, out_6838896326815040284); +void car_H_26(double *state, double *unused, double *out_765506157762525295) { + H_26(state, unused, out_765506157762525295); } -void car_h_27(double *state, double *unused, double *out_7962423204195969922) { - h_27(state, unused, out_7962423204195969922); +void car_h_27(double *state, double *unused, double *out_7512360170927346350) { + h_27(state, unused, out_7512360170927346350); } -void car_H_27(double *state, double *unused, double *out_8276297386745431527) { - H_27(state, unused, out_8276297386745431527); +void car_H_27(double *state, double *unused, double *out_9197807567381778253) { + H_27(state, unused, out_9197807567381778253); } -void car_h_29(double *state, double *unused, double *out_2721055612692909253) { - h_29(state, unused, out_2721055612692909253); +void car_h_29(double *state, double *unused, double *out_3171118645961532825) { + h_29(state, unused, out_3171118645961532825); } -void car_H_29(double *state, double *unused, double *out_6562934659875880494) { - H_29(state, unused, out_6562934659875880494); +void car_H_29(double *state, double *unused, double *out_6512812911266961158) { + H_29(state, unused, out_6512812911266961158); } -void car_h_28(double *state, double *unused, double *out_8408093249132323059) { - h_28(state, unused, out_8408093249132323059); +void car_h_28(double *state, double *unused, double *out_8329978581973507615) { + h_28(state, unused, out_8329978581973507615); } -void car_H_28(double *state, double *unused, double *out_1480535642806349920) { - H_28(state, unused, out_1480535642806349920); +void car_H_28(double *state, double *unused, double *out_2453174762388691756) { + H_28(state, unused, out_2453174762388691756); } -void car_h_31(double *state, double *unused, double *out_4467435790593075575) { - h_31(state, unused, out_4467435790593075575); +void car_h_31(double *state, double *unused, double *out_222736560719300508) { + h_31(state, unused, out_222736560719300508); } -void car_H_31(double *state, double *unused, double *out_7835698466143494680) { - H_31(state, unused, out_7835698466143494680); +void car_H_31(double *state, double *unused, double *out_4537655438513541947) { + H_31(state, unused, out_4537655438513541947); } 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 26814a1e8..803b6cd43 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_5591921399563932369); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_806607420895539823); -void car_H_mod_fun(double *state, double *out_5676707820504431668); -void car_f_fun(double *state, double dt, double *out_4844917589263296834); -void car_F_fun(double *state, double dt, double *out_4636534629313466670); -void car_h_25(double *state, double *unused, double *out_5482647601852736049); -void car_H_25(double *state, double *unused, double *out_7866344428020455108); -void car_h_24(double *state, double *unused, double *out_6159121467458312716); -void car_H_24(double *state, double *unused, double *out_3137725052700380188); -void car_h_30(double *state, double *unused, double *out_5757841664137241938); -void car_H_30(double *state, double *unused, double *out_7995683375163695178); -void car_h_26(double *state, double *unused, double *out_6701351517038904373); -void car_H_26(double *state, double *unused, double *out_6838896326815040284); -void car_h_27(double *state, double *unused, double *out_7962423204195969922); -void car_H_27(double *state, double *unused, double *out_8276297386745431527); -void car_h_29(double *state, double *unused, double *out_2721055612692909253); -void car_H_29(double *state, double *unused, double *out_6562934659875880494); -void car_h_28(double *state, double *unused, double *out_8408093249132323059); -void car_H_28(double *state, double *unused, double *out_1480535642806349920); -void car_h_31(double *state, double *unused, double *out_4467435790593075575); -void car_H_31(double *state, double *unused, double *out_7835698466143494680); +void car_err_fun(double *nom_x, double *delta_x, double *out_587611173581676283); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8120728011522269919); +void car_H_mod_fun(double *state, double *out_4001675034436284807); +void car_f_fun(double *state, double dt, double *out_6995360374056238489); +void car_F_fun(double *state, double dt, double *out_5620828310005666299); +void car_h_25(double *state, double *unused, double *out_5932710635121359621); +void car_H_25(double *state, double *unused, double *out_4507009476636581519); +void car_h_24(double *state, double *unused, double *out_8960081603047522700); +void car_H_24(double *state, double *unused, double *out_7364162790481451996); +void car_h_30(double *state, double *unused, double *out_6293656255741178026); +void car_H_30(double *state, double *unused, double *out_7023044255581353342); +void car_h_26(double *state, double *unused, double *out_7151414550307527945); +void car_H_26(double *state, double *unused, double *out_765506157762525295); +void car_h_27(double *state, double *unused, double *out_7512360170927346350); +void car_H_27(double *state, double *unused, double *out_9197807567381778253); +void car_h_29(double *state, double *unused, double *out_3171118645961532825); +void car_H_29(double *state, double *unused, double *out_6512812911266961158); +void car_h_28(double *state, double *unused, double *out_8329978581973507615); +void car_H_28(double *state, double *unused, double *out_2453174762388691756); +void car_h_31(double *state, double *unused, double *out_222736560719300508); +void car_H_31(double *state, double *unused, double *out_4537655438513541947); 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 7348afa4c..1292bbef8 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_9183838211698085113) { - out_9183838211698085113[0] = delta_x[0] + nom_x[0]; - out_9183838211698085113[1] = delta_x[1] + nom_x[1]; - out_9183838211698085113[2] = delta_x[2] + nom_x[2]; - out_9183838211698085113[3] = delta_x[3] + nom_x[3]; - out_9183838211698085113[4] = delta_x[4] + nom_x[4]; - out_9183838211698085113[5] = delta_x[5] + nom_x[5]; - out_9183838211698085113[6] = delta_x[6] + nom_x[6]; - out_9183838211698085113[7] = delta_x[7] + nom_x[7]; - out_9183838211698085113[8] = delta_x[8] + nom_x[8]; - out_9183838211698085113[9] = delta_x[9] + nom_x[9]; - out_9183838211698085113[10] = delta_x[10] + nom_x[10]; - out_9183838211698085113[11] = delta_x[11] + nom_x[11]; - out_9183838211698085113[12] = delta_x[12] + nom_x[12]; - out_9183838211698085113[13] = delta_x[13] + nom_x[13]; - out_9183838211698085113[14] = delta_x[14] + nom_x[14]; - out_9183838211698085113[15] = delta_x[15] + nom_x[15]; - out_9183838211698085113[16] = delta_x[16] + nom_x[16]; - out_9183838211698085113[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7704133318394013706) { + out_7704133318394013706[0] = delta_x[0] + nom_x[0]; + out_7704133318394013706[1] = delta_x[1] + nom_x[1]; + out_7704133318394013706[2] = delta_x[2] + nom_x[2]; + out_7704133318394013706[3] = delta_x[3] + nom_x[3]; + out_7704133318394013706[4] = delta_x[4] + nom_x[4]; + out_7704133318394013706[5] = delta_x[5] + nom_x[5]; + out_7704133318394013706[6] = delta_x[6] + nom_x[6]; + out_7704133318394013706[7] = delta_x[7] + nom_x[7]; + out_7704133318394013706[8] = delta_x[8] + nom_x[8]; + out_7704133318394013706[9] = delta_x[9] + nom_x[9]; + out_7704133318394013706[10] = delta_x[10] + nom_x[10]; + out_7704133318394013706[11] = delta_x[11] + nom_x[11]; + out_7704133318394013706[12] = delta_x[12] + nom_x[12]; + out_7704133318394013706[13] = delta_x[13] + nom_x[13]; + out_7704133318394013706[14] = delta_x[14] + nom_x[14]; + out_7704133318394013706[15] = delta_x[15] + nom_x[15]; + out_7704133318394013706[16] = delta_x[16] + nom_x[16]; + out_7704133318394013706[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7880707634176237912) { - out_7880707634176237912[0] = -nom_x[0] + true_x[0]; - out_7880707634176237912[1] = -nom_x[1] + true_x[1]; - out_7880707634176237912[2] = -nom_x[2] + true_x[2]; - out_7880707634176237912[3] = -nom_x[3] + true_x[3]; - out_7880707634176237912[4] = -nom_x[4] + true_x[4]; - out_7880707634176237912[5] = -nom_x[5] + true_x[5]; - out_7880707634176237912[6] = -nom_x[6] + true_x[6]; - out_7880707634176237912[7] = -nom_x[7] + true_x[7]; - out_7880707634176237912[8] = -nom_x[8] + true_x[8]; - out_7880707634176237912[9] = -nom_x[9] + true_x[9]; - out_7880707634176237912[10] = -nom_x[10] + true_x[10]; - out_7880707634176237912[11] = -nom_x[11] + true_x[11]; - out_7880707634176237912[12] = -nom_x[12] + true_x[12]; - out_7880707634176237912[13] = -nom_x[13] + true_x[13]; - out_7880707634176237912[14] = -nom_x[14] + true_x[14]; - out_7880707634176237912[15] = -nom_x[15] + true_x[15]; - out_7880707634176237912[16] = -nom_x[16] + true_x[16]; - out_7880707634176237912[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5732870200970735889) { + out_5732870200970735889[0] = -nom_x[0] + true_x[0]; + out_5732870200970735889[1] = -nom_x[1] + true_x[1]; + out_5732870200970735889[2] = -nom_x[2] + true_x[2]; + out_5732870200970735889[3] = -nom_x[3] + true_x[3]; + out_5732870200970735889[4] = -nom_x[4] + true_x[4]; + out_5732870200970735889[5] = -nom_x[5] + true_x[5]; + out_5732870200970735889[6] = -nom_x[6] + true_x[6]; + out_5732870200970735889[7] = -nom_x[7] + true_x[7]; + out_5732870200970735889[8] = -nom_x[8] + true_x[8]; + out_5732870200970735889[9] = -nom_x[9] + true_x[9]; + out_5732870200970735889[10] = -nom_x[10] + true_x[10]; + out_5732870200970735889[11] = -nom_x[11] + true_x[11]; + out_5732870200970735889[12] = -nom_x[12] + true_x[12]; + out_5732870200970735889[13] = -nom_x[13] + true_x[13]; + out_5732870200970735889[14] = -nom_x[14] + true_x[14]; + out_5732870200970735889[15] = -nom_x[15] + true_x[15]; + out_5732870200970735889[16] = -nom_x[16] + true_x[16]; + out_5732870200970735889[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_4757006295873358620) { - out_4757006295873358620[0] = 1.0; - out_4757006295873358620[1] = 0.0; - out_4757006295873358620[2] = 0.0; - out_4757006295873358620[3] = 0.0; - out_4757006295873358620[4] = 0.0; - out_4757006295873358620[5] = 0.0; - out_4757006295873358620[6] = 0.0; - out_4757006295873358620[7] = 0.0; - out_4757006295873358620[8] = 0.0; - out_4757006295873358620[9] = 0.0; - out_4757006295873358620[10] = 0.0; - out_4757006295873358620[11] = 0.0; - out_4757006295873358620[12] = 0.0; - out_4757006295873358620[13] = 0.0; - out_4757006295873358620[14] = 0.0; - out_4757006295873358620[15] = 0.0; - out_4757006295873358620[16] = 0.0; - out_4757006295873358620[17] = 0.0; - out_4757006295873358620[18] = 0.0; - out_4757006295873358620[19] = 1.0; - out_4757006295873358620[20] = 0.0; - out_4757006295873358620[21] = 0.0; - out_4757006295873358620[22] = 0.0; - out_4757006295873358620[23] = 0.0; - out_4757006295873358620[24] = 0.0; - out_4757006295873358620[25] = 0.0; - out_4757006295873358620[26] = 0.0; - out_4757006295873358620[27] = 0.0; - out_4757006295873358620[28] = 0.0; - out_4757006295873358620[29] = 0.0; - out_4757006295873358620[30] = 0.0; - out_4757006295873358620[31] = 0.0; - out_4757006295873358620[32] = 0.0; - out_4757006295873358620[33] = 0.0; - out_4757006295873358620[34] = 0.0; - out_4757006295873358620[35] = 0.0; - out_4757006295873358620[36] = 0.0; - out_4757006295873358620[37] = 0.0; - out_4757006295873358620[38] = 1.0; - out_4757006295873358620[39] = 0.0; - out_4757006295873358620[40] = 0.0; - out_4757006295873358620[41] = 0.0; - out_4757006295873358620[42] = 0.0; - out_4757006295873358620[43] = 0.0; - out_4757006295873358620[44] = 0.0; - out_4757006295873358620[45] = 0.0; - out_4757006295873358620[46] = 0.0; - out_4757006295873358620[47] = 0.0; - out_4757006295873358620[48] = 0.0; - out_4757006295873358620[49] = 0.0; - out_4757006295873358620[50] = 0.0; - out_4757006295873358620[51] = 0.0; - out_4757006295873358620[52] = 0.0; - out_4757006295873358620[53] = 0.0; - out_4757006295873358620[54] = 0.0; - out_4757006295873358620[55] = 0.0; - out_4757006295873358620[56] = 0.0; - out_4757006295873358620[57] = 1.0; - out_4757006295873358620[58] = 0.0; - out_4757006295873358620[59] = 0.0; - out_4757006295873358620[60] = 0.0; - out_4757006295873358620[61] = 0.0; - out_4757006295873358620[62] = 0.0; - out_4757006295873358620[63] = 0.0; - out_4757006295873358620[64] = 0.0; - out_4757006295873358620[65] = 0.0; - out_4757006295873358620[66] = 0.0; - out_4757006295873358620[67] = 0.0; - out_4757006295873358620[68] = 0.0; - out_4757006295873358620[69] = 0.0; - out_4757006295873358620[70] = 0.0; - out_4757006295873358620[71] = 0.0; - out_4757006295873358620[72] = 0.0; - out_4757006295873358620[73] = 0.0; - out_4757006295873358620[74] = 0.0; - out_4757006295873358620[75] = 0.0; - out_4757006295873358620[76] = 1.0; - out_4757006295873358620[77] = 0.0; - out_4757006295873358620[78] = 0.0; - out_4757006295873358620[79] = 0.0; - out_4757006295873358620[80] = 0.0; - out_4757006295873358620[81] = 0.0; - out_4757006295873358620[82] = 0.0; - out_4757006295873358620[83] = 0.0; - out_4757006295873358620[84] = 0.0; - out_4757006295873358620[85] = 0.0; - out_4757006295873358620[86] = 0.0; - out_4757006295873358620[87] = 0.0; - out_4757006295873358620[88] = 0.0; - out_4757006295873358620[89] = 0.0; - out_4757006295873358620[90] = 0.0; - out_4757006295873358620[91] = 0.0; - out_4757006295873358620[92] = 0.0; - out_4757006295873358620[93] = 0.0; - out_4757006295873358620[94] = 0.0; - out_4757006295873358620[95] = 1.0; - out_4757006295873358620[96] = 0.0; - out_4757006295873358620[97] = 0.0; - out_4757006295873358620[98] = 0.0; - out_4757006295873358620[99] = 0.0; - out_4757006295873358620[100] = 0.0; - out_4757006295873358620[101] = 0.0; - out_4757006295873358620[102] = 0.0; - out_4757006295873358620[103] = 0.0; - out_4757006295873358620[104] = 0.0; - out_4757006295873358620[105] = 0.0; - out_4757006295873358620[106] = 0.0; - out_4757006295873358620[107] = 0.0; - out_4757006295873358620[108] = 0.0; - out_4757006295873358620[109] = 0.0; - out_4757006295873358620[110] = 0.0; - out_4757006295873358620[111] = 0.0; - out_4757006295873358620[112] = 0.0; - out_4757006295873358620[113] = 0.0; - out_4757006295873358620[114] = 1.0; - out_4757006295873358620[115] = 0.0; - out_4757006295873358620[116] = 0.0; - out_4757006295873358620[117] = 0.0; - out_4757006295873358620[118] = 0.0; - out_4757006295873358620[119] = 0.0; - out_4757006295873358620[120] = 0.0; - out_4757006295873358620[121] = 0.0; - out_4757006295873358620[122] = 0.0; - out_4757006295873358620[123] = 0.0; - out_4757006295873358620[124] = 0.0; - out_4757006295873358620[125] = 0.0; - out_4757006295873358620[126] = 0.0; - out_4757006295873358620[127] = 0.0; - out_4757006295873358620[128] = 0.0; - out_4757006295873358620[129] = 0.0; - out_4757006295873358620[130] = 0.0; - out_4757006295873358620[131] = 0.0; - out_4757006295873358620[132] = 0.0; - out_4757006295873358620[133] = 1.0; - out_4757006295873358620[134] = 0.0; - out_4757006295873358620[135] = 0.0; - out_4757006295873358620[136] = 0.0; - out_4757006295873358620[137] = 0.0; - out_4757006295873358620[138] = 0.0; - out_4757006295873358620[139] = 0.0; - out_4757006295873358620[140] = 0.0; - out_4757006295873358620[141] = 0.0; - out_4757006295873358620[142] = 0.0; - out_4757006295873358620[143] = 0.0; - out_4757006295873358620[144] = 0.0; - out_4757006295873358620[145] = 0.0; - out_4757006295873358620[146] = 0.0; - out_4757006295873358620[147] = 0.0; - out_4757006295873358620[148] = 0.0; - out_4757006295873358620[149] = 0.0; - out_4757006295873358620[150] = 0.0; - out_4757006295873358620[151] = 0.0; - out_4757006295873358620[152] = 1.0; - out_4757006295873358620[153] = 0.0; - out_4757006295873358620[154] = 0.0; - out_4757006295873358620[155] = 0.0; - out_4757006295873358620[156] = 0.0; - out_4757006295873358620[157] = 0.0; - out_4757006295873358620[158] = 0.0; - out_4757006295873358620[159] = 0.0; - out_4757006295873358620[160] = 0.0; - out_4757006295873358620[161] = 0.0; - out_4757006295873358620[162] = 0.0; - out_4757006295873358620[163] = 0.0; - out_4757006295873358620[164] = 0.0; - out_4757006295873358620[165] = 0.0; - out_4757006295873358620[166] = 0.0; - out_4757006295873358620[167] = 0.0; - out_4757006295873358620[168] = 0.0; - out_4757006295873358620[169] = 0.0; - out_4757006295873358620[170] = 0.0; - out_4757006295873358620[171] = 1.0; - out_4757006295873358620[172] = 0.0; - out_4757006295873358620[173] = 0.0; - out_4757006295873358620[174] = 0.0; - out_4757006295873358620[175] = 0.0; - out_4757006295873358620[176] = 0.0; - out_4757006295873358620[177] = 0.0; - out_4757006295873358620[178] = 0.0; - out_4757006295873358620[179] = 0.0; - out_4757006295873358620[180] = 0.0; - out_4757006295873358620[181] = 0.0; - out_4757006295873358620[182] = 0.0; - out_4757006295873358620[183] = 0.0; - out_4757006295873358620[184] = 0.0; - out_4757006295873358620[185] = 0.0; - out_4757006295873358620[186] = 0.0; - out_4757006295873358620[187] = 0.0; - out_4757006295873358620[188] = 0.0; - out_4757006295873358620[189] = 0.0; - out_4757006295873358620[190] = 1.0; - out_4757006295873358620[191] = 0.0; - out_4757006295873358620[192] = 0.0; - out_4757006295873358620[193] = 0.0; - out_4757006295873358620[194] = 0.0; - out_4757006295873358620[195] = 0.0; - out_4757006295873358620[196] = 0.0; - out_4757006295873358620[197] = 0.0; - out_4757006295873358620[198] = 0.0; - out_4757006295873358620[199] = 0.0; - out_4757006295873358620[200] = 0.0; - out_4757006295873358620[201] = 0.0; - out_4757006295873358620[202] = 0.0; - out_4757006295873358620[203] = 0.0; - out_4757006295873358620[204] = 0.0; - out_4757006295873358620[205] = 0.0; - out_4757006295873358620[206] = 0.0; - out_4757006295873358620[207] = 0.0; - out_4757006295873358620[208] = 0.0; - out_4757006295873358620[209] = 1.0; - out_4757006295873358620[210] = 0.0; - out_4757006295873358620[211] = 0.0; - out_4757006295873358620[212] = 0.0; - out_4757006295873358620[213] = 0.0; - out_4757006295873358620[214] = 0.0; - out_4757006295873358620[215] = 0.0; - out_4757006295873358620[216] = 0.0; - out_4757006295873358620[217] = 0.0; - out_4757006295873358620[218] = 0.0; - out_4757006295873358620[219] = 0.0; - out_4757006295873358620[220] = 0.0; - out_4757006295873358620[221] = 0.0; - out_4757006295873358620[222] = 0.0; - out_4757006295873358620[223] = 0.0; - out_4757006295873358620[224] = 0.0; - out_4757006295873358620[225] = 0.0; - out_4757006295873358620[226] = 0.0; - out_4757006295873358620[227] = 0.0; - out_4757006295873358620[228] = 1.0; - out_4757006295873358620[229] = 0.0; - out_4757006295873358620[230] = 0.0; - out_4757006295873358620[231] = 0.0; - out_4757006295873358620[232] = 0.0; - out_4757006295873358620[233] = 0.0; - out_4757006295873358620[234] = 0.0; - out_4757006295873358620[235] = 0.0; - out_4757006295873358620[236] = 0.0; - out_4757006295873358620[237] = 0.0; - out_4757006295873358620[238] = 0.0; - out_4757006295873358620[239] = 0.0; - out_4757006295873358620[240] = 0.0; - out_4757006295873358620[241] = 0.0; - out_4757006295873358620[242] = 0.0; - out_4757006295873358620[243] = 0.0; - out_4757006295873358620[244] = 0.0; - out_4757006295873358620[245] = 0.0; - out_4757006295873358620[246] = 0.0; - out_4757006295873358620[247] = 1.0; - out_4757006295873358620[248] = 0.0; - out_4757006295873358620[249] = 0.0; - out_4757006295873358620[250] = 0.0; - out_4757006295873358620[251] = 0.0; - out_4757006295873358620[252] = 0.0; - out_4757006295873358620[253] = 0.0; - out_4757006295873358620[254] = 0.0; - out_4757006295873358620[255] = 0.0; - out_4757006295873358620[256] = 0.0; - out_4757006295873358620[257] = 0.0; - out_4757006295873358620[258] = 0.0; - out_4757006295873358620[259] = 0.0; - out_4757006295873358620[260] = 0.0; - out_4757006295873358620[261] = 0.0; - out_4757006295873358620[262] = 0.0; - out_4757006295873358620[263] = 0.0; - out_4757006295873358620[264] = 0.0; - out_4757006295873358620[265] = 0.0; - out_4757006295873358620[266] = 1.0; - out_4757006295873358620[267] = 0.0; - out_4757006295873358620[268] = 0.0; - out_4757006295873358620[269] = 0.0; - out_4757006295873358620[270] = 0.0; - out_4757006295873358620[271] = 0.0; - out_4757006295873358620[272] = 0.0; - out_4757006295873358620[273] = 0.0; - out_4757006295873358620[274] = 0.0; - out_4757006295873358620[275] = 0.0; - out_4757006295873358620[276] = 0.0; - out_4757006295873358620[277] = 0.0; - out_4757006295873358620[278] = 0.0; - out_4757006295873358620[279] = 0.0; - out_4757006295873358620[280] = 0.0; - out_4757006295873358620[281] = 0.0; - out_4757006295873358620[282] = 0.0; - out_4757006295873358620[283] = 0.0; - out_4757006295873358620[284] = 0.0; - out_4757006295873358620[285] = 1.0; - out_4757006295873358620[286] = 0.0; - out_4757006295873358620[287] = 0.0; - out_4757006295873358620[288] = 0.0; - out_4757006295873358620[289] = 0.0; - out_4757006295873358620[290] = 0.0; - out_4757006295873358620[291] = 0.0; - out_4757006295873358620[292] = 0.0; - out_4757006295873358620[293] = 0.0; - out_4757006295873358620[294] = 0.0; - out_4757006295873358620[295] = 0.0; - out_4757006295873358620[296] = 0.0; - out_4757006295873358620[297] = 0.0; - out_4757006295873358620[298] = 0.0; - out_4757006295873358620[299] = 0.0; - out_4757006295873358620[300] = 0.0; - out_4757006295873358620[301] = 0.0; - out_4757006295873358620[302] = 0.0; - out_4757006295873358620[303] = 0.0; - out_4757006295873358620[304] = 1.0; - out_4757006295873358620[305] = 0.0; - out_4757006295873358620[306] = 0.0; - out_4757006295873358620[307] = 0.0; - out_4757006295873358620[308] = 0.0; - out_4757006295873358620[309] = 0.0; - out_4757006295873358620[310] = 0.0; - out_4757006295873358620[311] = 0.0; - out_4757006295873358620[312] = 0.0; - out_4757006295873358620[313] = 0.0; - out_4757006295873358620[314] = 0.0; - out_4757006295873358620[315] = 0.0; - out_4757006295873358620[316] = 0.0; - out_4757006295873358620[317] = 0.0; - out_4757006295873358620[318] = 0.0; - out_4757006295873358620[319] = 0.0; - out_4757006295873358620[320] = 0.0; - out_4757006295873358620[321] = 0.0; - out_4757006295873358620[322] = 0.0; - out_4757006295873358620[323] = 1.0; +void H_mod_fun(double *state, double *out_5613029472060395452) { + out_5613029472060395452[0] = 1.0; + out_5613029472060395452[1] = 0.0; + out_5613029472060395452[2] = 0.0; + out_5613029472060395452[3] = 0.0; + out_5613029472060395452[4] = 0.0; + out_5613029472060395452[5] = 0.0; + out_5613029472060395452[6] = 0.0; + out_5613029472060395452[7] = 0.0; + out_5613029472060395452[8] = 0.0; + out_5613029472060395452[9] = 0.0; + out_5613029472060395452[10] = 0.0; + out_5613029472060395452[11] = 0.0; + out_5613029472060395452[12] = 0.0; + out_5613029472060395452[13] = 0.0; + out_5613029472060395452[14] = 0.0; + out_5613029472060395452[15] = 0.0; + out_5613029472060395452[16] = 0.0; + out_5613029472060395452[17] = 0.0; + out_5613029472060395452[18] = 0.0; + out_5613029472060395452[19] = 1.0; + out_5613029472060395452[20] = 0.0; + out_5613029472060395452[21] = 0.0; + out_5613029472060395452[22] = 0.0; + out_5613029472060395452[23] = 0.0; + out_5613029472060395452[24] = 0.0; + out_5613029472060395452[25] = 0.0; + out_5613029472060395452[26] = 0.0; + out_5613029472060395452[27] = 0.0; + out_5613029472060395452[28] = 0.0; + out_5613029472060395452[29] = 0.0; + out_5613029472060395452[30] = 0.0; + out_5613029472060395452[31] = 0.0; + out_5613029472060395452[32] = 0.0; + out_5613029472060395452[33] = 0.0; + out_5613029472060395452[34] = 0.0; + out_5613029472060395452[35] = 0.0; + out_5613029472060395452[36] = 0.0; + out_5613029472060395452[37] = 0.0; + out_5613029472060395452[38] = 1.0; + out_5613029472060395452[39] = 0.0; + out_5613029472060395452[40] = 0.0; + out_5613029472060395452[41] = 0.0; + out_5613029472060395452[42] = 0.0; + out_5613029472060395452[43] = 0.0; + out_5613029472060395452[44] = 0.0; + out_5613029472060395452[45] = 0.0; + out_5613029472060395452[46] = 0.0; + out_5613029472060395452[47] = 0.0; + out_5613029472060395452[48] = 0.0; + out_5613029472060395452[49] = 0.0; + out_5613029472060395452[50] = 0.0; + out_5613029472060395452[51] = 0.0; + out_5613029472060395452[52] = 0.0; + out_5613029472060395452[53] = 0.0; + out_5613029472060395452[54] = 0.0; + out_5613029472060395452[55] = 0.0; + out_5613029472060395452[56] = 0.0; + out_5613029472060395452[57] = 1.0; + out_5613029472060395452[58] = 0.0; + out_5613029472060395452[59] = 0.0; + out_5613029472060395452[60] = 0.0; + out_5613029472060395452[61] = 0.0; + out_5613029472060395452[62] = 0.0; + out_5613029472060395452[63] = 0.0; + out_5613029472060395452[64] = 0.0; + out_5613029472060395452[65] = 0.0; + out_5613029472060395452[66] = 0.0; + out_5613029472060395452[67] = 0.0; + out_5613029472060395452[68] = 0.0; + out_5613029472060395452[69] = 0.0; + out_5613029472060395452[70] = 0.0; + out_5613029472060395452[71] = 0.0; + out_5613029472060395452[72] = 0.0; + out_5613029472060395452[73] = 0.0; + out_5613029472060395452[74] = 0.0; + out_5613029472060395452[75] = 0.0; + out_5613029472060395452[76] = 1.0; + out_5613029472060395452[77] = 0.0; + out_5613029472060395452[78] = 0.0; + out_5613029472060395452[79] = 0.0; + out_5613029472060395452[80] = 0.0; + out_5613029472060395452[81] = 0.0; + out_5613029472060395452[82] = 0.0; + out_5613029472060395452[83] = 0.0; + out_5613029472060395452[84] = 0.0; + out_5613029472060395452[85] = 0.0; + out_5613029472060395452[86] = 0.0; + out_5613029472060395452[87] = 0.0; + out_5613029472060395452[88] = 0.0; + out_5613029472060395452[89] = 0.0; + out_5613029472060395452[90] = 0.0; + out_5613029472060395452[91] = 0.0; + out_5613029472060395452[92] = 0.0; + out_5613029472060395452[93] = 0.0; + out_5613029472060395452[94] = 0.0; + out_5613029472060395452[95] = 1.0; + out_5613029472060395452[96] = 0.0; + out_5613029472060395452[97] = 0.0; + out_5613029472060395452[98] = 0.0; + out_5613029472060395452[99] = 0.0; + out_5613029472060395452[100] = 0.0; + out_5613029472060395452[101] = 0.0; + out_5613029472060395452[102] = 0.0; + out_5613029472060395452[103] = 0.0; + out_5613029472060395452[104] = 0.0; + out_5613029472060395452[105] = 0.0; + out_5613029472060395452[106] = 0.0; + out_5613029472060395452[107] = 0.0; + out_5613029472060395452[108] = 0.0; + out_5613029472060395452[109] = 0.0; + out_5613029472060395452[110] = 0.0; + out_5613029472060395452[111] = 0.0; + out_5613029472060395452[112] = 0.0; + out_5613029472060395452[113] = 0.0; + out_5613029472060395452[114] = 1.0; + out_5613029472060395452[115] = 0.0; + out_5613029472060395452[116] = 0.0; + out_5613029472060395452[117] = 0.0; + out_5613029472060395452[118] = 0.0; + out_5613029472060395452[119] = 0.0; + out_5613029472060395452[120] = 0.0; + out_5613029472060395452[121] = 0.0; + out_5613029472060395452[122] = 0.0; + out_5613029472060395452[123] = 0.0; + out_5613029472060395452[124] = 0.0; + out_5613029472060395452[125] = 0.0; + out_5613029472060395452[126] = 0.0; + out_5613029472060395452[127] = 0.0; + out_5613029472060395452[128] = 0.0; + out_5613029472060395452[129] = 0.0; + out_5613029472060395452[130] = 0.0; + out_5613029472060395452[131] = 0.0; + out_5613029472060395452[132] = 0.0; + out_5613029472060395452[133] = 1.0; + out_5613029472060395452[134] = 0.0; + out_5613029472060395452[135] = 0.0; + out_5613029472060395452[136] = 0.0; + out_5613029472060395452[137] = 0.0; + out_5613029472060395452[138] = 0.0; + out_5613029472060395452[139] = 0.0; + out_5613029472060395452[140] = 0.0; + out_5613029472060395452[141] = 0.0; + out_5613029472060395452[142] = 0.0; + out_5613029472060395452[143] = 0.0; + out_5613029472060395452[144] = 0.0; + out_5613029472060395452[145] = 0.0; + out_5613029472060395452[146] = 0.0; + out_5613029472060395452[147] = 0.0; + out_5613029472060395452[148] = 0.0; + out_5613029472060395452[149] = 0.0; + out_5613029472060395452[150] = 0.0; + out_5613029472060395452[151] = 0.0; + out_5613029472060395452[152] = 1.0; + out_5613029472060395452[153] = 0.0; + out_5613029472060395452[154] = 0.0; + out_5613029472060395452[155] = 0.0; + out_5613029472060395452[156] = 0.0; + out_5613029472060395452[157] = 0.0; + out_5613029472060395452[158] = 0.0; + out_5613029472060395452[159] = 0.0; + out_5613029472060395452[160] = 0.0; + out_5613029472060395452[161] = 0.0; + out_5613029472060395452[162] = 0.0; + out_5613029472060395452[163] = 0.0; + out_5613029472060395452[164] = 0.0; + out_5613029472060395452[165] = 0.0; + out_5613029472060395452[166] = 0.0; + out_5613029472060395452[167] = 0.0; + out_5613029472060395452[168] = 0.0; + out_5613029472060395452[169] = 0.0; + out_5613029472060395452[170] = 0.0; + out_5613029472060395452[171] = 1.0; + out_5613029472060395452[172] = 0.0; + out_5613029472060395452[173] = 0.0; + out_5613029472060395452[174] = 0.0; + out_5613029472060395452[175] = 0.0; + out_5613029472060395452[176] = 0.0; + out_5613029472060395452[177] = 0.0; + out_5613029472060395452[178] = 0.0; + out_5613029472060395452[179] = 0.0; + out_5613029472060395452[180] = 0.0; + out_5613029472060395452[181] = 0.0; + out_5613029472060395452[182] = 0.0; + out_5613029472060395452[183] = 0.0; + out_5613029472060395452[184] = 0.0; + out_5613029472060395452[185] = 0.0; + out_5613029472060395452[186] = 0.0; + out_5613029472060395452[187] = 0.0; + out_5613029472060395452[188] = 0.0; + out_5613029472060395452[189] = 0.0; + out_5613029472060395452[190] = 1.0; + out_5613029472060395452[191] = 0.0; + out_5613029472060395452[192] = 0.0; + out_5613029472060395452[193] = 0.0; + out_5613029472060395452[194] = 0.0; + out_5613029472060395452[195] = 0.0; + out_5613029472060395452[196] = 0.0; + out_5613029472060395452[197] = 0.0; + out_5613029472060395452[198] = 0.0; + out_5613029472060395452[199] = 0.0; + out_5613029472060395452[200] = 0.0; + out_5613029472060395452[201] = 0.0; + out_5613029472060395452[202] = 0.0; + out_5613029472060395452[203] = 0.0; + out_5613029472060395452[204] = 0.0; + out_5613029472060395452[205] = 0.0; + out_5613029472060395452[206] = 0.0; + out_5613029472060395452[207] = 0.0; + out_5613029472060395452[208] = 0.0; + out_5613029472060395452[209] = 1.0; + out_5613029472060395452[210] = 0.0; + out_5613029472060395452[211] = 0.0; + out_5613029472060395452[212] = 0.0; + out_5613029472060395452[213] = 0.0; + out_5613029472060395452[214] = 0.0; + out_5613029472060395452[215] = 0.0; + out_5613029472060395452[216] = 0.0; + out_5613029472060395452[217] = 0.0; + out_5613029472060395452[218] = 0.0; + out_5613029472060395452[219] = 0.0; + out_5613029472060395452[220] = 0.0; + out_5613029472060395452[221] = 0.0; + out_5613029472060395452[222] = 0.0; + out_5613029472060395452[223] = 0.0; + out_5613029472060395452[224] = 0.0; + out_5613029472060395452[225] = 0.0; + out_5613029472060395452[226] = 0.0; + out_5613029472060395452[227] = 0.0; + out_5613029472060395452[228] = 1.0; + out_5613029472060395452[229] = 0.0; + out_5613029472060395452[230] = 0.0; + out_5613029472060395452[231] = 0.0; + out_5613029472060395452[232] = 0.0; + out_5613029472060395452[233] = 0.0; + out_5613029472060395452[234] = 0.0; + out_5613029472060395452[235] = 0.0; + out_5613029472060395452[236] = 0.0; + out_5613029472060395452[237] = 0.0; + out_5613029472060395452[238] = 0.0; + out_5613029472060395452[239] = 0.0; + out_5613029472060395452[240] = 0.0; + out_5613029472060395452[241] = 0.0; + out_5613029472060395452[242] = 0.0; + out_5613029472060395452[243] = 0.0; + out_5613029472060395452[244] = 0.0; + out_5613029472060395452[245] = 0.0; + out_5613029472060395452[246] = 0.0; + out_5613029472060395452[247] = 1.0; + out_5613029472060395452[248] = 0.0; + out_5613029472060395452[249] = 0.0; + out_5613029472060395452[250] = 0.0; + out_5613029472060395452[251] = 0.0; + out_5613029472060395452[252] = 0.0; + out_5613029472060395452[253] = 0.0; + out_5613029472060395452[254] = 0.0; + out_5613029472060395452[255] = 0.0; + out_5613029472060395452[256] = 0.0; + out_5613029472060395452[257] = 0.0; + out_5613029472060395452[258] = 0.0; + out_5613029472060395452[259] = 0.0; + out_5613029472060395452[260] = 0.0; + out_5613029472060395452[261] = 0.0; + out_5613029472060395452[262] = 0.0; + out_5613029472060395452[263] = 0.0; + out_5613029472060395452[264] = 0.0; + out_5613029472060395452[265] = 0.0; + out_5613029472060395452[266] = 1.0; + out_5613029472060395452[267] = 0.0; + out_5613029472060395452[268] = 0.0; + out_5613029472060395452[269] = 0.0; + out_5613029472060395452[270] = 0.0; + out_5613029472060395452[271] = 0.0; + out_5613029472060395452[272] = 0.0; + out_5613029472060395452[273] = 0.0; + out_5613029472060395452[274] = 0.0; + out_5613029472060395452[275] = 0.0; + out_5613029472060395452[276] = 0.0; + out_5613029472060395452[277] = 0.0; + out_5613029472060395452[278] = 0.0; + out_5613029472060395452[279] = 0.0; + out_5613029472060395452[280] = 0.0; + out_5613029472060395452[281] = 0.0; + out_5613029472060395452[282] = 0.0; + out_5613029472060395452[283] = 0.0; + out_5613029472060395452[284] = 0.0; + out_5613029472060395452[285] = 1.0; + out_5613029472060395452[286] = 0.0; + out_5613029472060395452[287] = 0.0; + out_5613029472060395452[288] = 0.0; + out_5613029472060395452[289] = 0.0; + out_5613029472060395452[290] = 0.0; + out_5613029472060395452[291] = 0.0; + out_5613029472060395452[292] = 0.0; + out_5613029472060395452[293] = 0.0; + out_5613029472060395452[294] = 0.0; + out_5613029472060395452[295] = 0.0; + out_5613029472060395452[296] = 0.0; + out_5613029472060395452[297] = 0.0; + out_5613029472060395452[298] = 0.0; + out_5613029472060395452[299] = 0.0; + out_5613029472060395452[300] = 0.0; + out_5613029472060395452[301] = 0.0; + out_5613029472060395452[302] = 0.0; + out_5613029472060395452[303] = 0.0; + out_5613029472060395452[304] = 1.0; + out_5613029472060395452[305] = 0.0; + out_5613029472060395452[306] = 0.0; + out_5613029472060395452[307] = 0.0; + out_5613029472060395452[308] = 0.0; + out_5613029472060395452[309] = 0.0; + out_5613029472060395452[310] = 0.0; + out_5613029472060395452[311] = 0.0; + out_5613029472060395452[312] = 0.0; + out_5613029472060395452[313] = 0.0; + out_5613029472060395452[314] = 0.0; + out_5613029472060395452[315] = 0.0; + out_5613029472060395452[316] = 0.0; + out_5613029472060395452[317] = 0.0; + out_5613029472060395452[318] = 0.0; + out_5613029472060395452[319] = 0.0; + out_5613029472060395452[320] = 0.0; + out_5613029472060395452[321] = 0.0; + out_5613029472060395452[322] = 0.0; + out_5613029472060395452[323] = 1.0; } -void f_fun(double *state, double dt, double *out_9118920386528411329) { - out_9118920386528411329[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_9118920386528411329[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_9118920386528411329[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_9118920386528411329[3] = dt*state[12] + state[3]; - out_9118920386528411329[4] = dt*state[13] + state[4]; - out_9118920386528411329[5] = dt*state[14] + state[5]; - out_9118920386528411329[6] = state[6]; - out_9118920386528411329[7] = state[7]; - out_9118920386528411329[8] = state[8]; - out_9118920386528411329[9] = state[9]; - out_9118920386528411329[10] = state[10]; - out_9118920386528411329[11] = state[11]; - out_9118920386528411329[12] = state[12]; - out_9118920386528411329[13] = state[13]; - out_9118920386528411329[14] = state[14]; - out_9118920386528411329[15] = state[15]; - out_9118920386528411329[16] = state[16]; - out_9118920386528411329[17] = state[17]; +void f_fun(double *state, double dt, double *out_3560257805552383557) { + out_3560257805552383557[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_3560257805552383557[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_3560257805552383557[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_3560257805552383557[3] = dt*state[12] + state[3]; + out_3560257805552383557[4] = dt*state[13] + state[4]; + out_3560257805552383557[5] = dt*state[14] + state[5]; + out_3560257805552383557[6] = state[6]; + out_3560257805552383557[7] = state[7]; + out_3560257805552383557[8] = state[8]; + out_3560257805552383557[9] = state[9]; + out_3560257805552383557[10] = state[10]; + out_3560257805552383557[11] = state[11]; + out_3560257805552383557[12] = state[12]; + out_3560257805552383557[13] = state[13]; + out_3560257805552383557[14] = state[14]; + out_3560257805552383557[15] = state[15]; + out_3560257805552383557[16] = state[16]; + out_3560257805552383557[17] = state[17]; } -void F_fun(double *state, double dt, double *out_388070008261359487) { - out_388070008261359487[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_388070008261359487[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_388070008261359487[2] = 0; - out_388070008261359487[3] = 0; - out_388070008261359487[4] = 0; - out_388070008261359487[5] = 0; - out_388070008261359487[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_388070008261359487[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_388070008261359487[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_388070008261359487[9] = 0; - out_388070008261359487[10] = 0; - out_388070008261359487[11] = 0; - out_388070008261359487[12] = 0; - out_388070008261359487[13] = 0; - out_388070008261359487[14] = 0; - out_388070008261359487[15] = 0; - out_388070008261359487[16] = 0; - out_388070008261359487[17] = 0; - out_388070008261359487[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_388070008261359487[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_388070008261359487[20] = 0; - out_388070008261359487[21] = 0; - out_388070008261359487[22] = 0; - out_388070008261359487[23] = 0; - out_388070008261359487[24] = 0; - out_388070008261359487[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_388070008261359487[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_388070008261359487[27] = 0; - out_388070008261359487[28] = 0; - out_388070008261359487[29] = 0; - out_388070008261359487[30] = 0; - out_388070008261359487[31] = 0; - out_388070008261359487[32] = 0; - out_388070008261359487[33] = 0; - out_388070008261359487[34] = 0; - out_388070008261359487[35] = 0; - out_388070008261359487[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_388070008261359487[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_388070008261359487[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_388070008261359487[39] = 0; - out_388070008261359487[40] = 0; - out_388070008261359487[41] = 0; - out_388070008261359487[42] = 0; - out_388070008261359487[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_388070008261359487[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_388070008261359487[45] = 0; - out_388070008261359487[46] = 0; - out_388070008261359487[47] = 0; - out_388070008261359487[48] = 0; - out_388070008261359487[49] = 0; - out_388070008261359487[50] = 0; - out_388070008261359487[51] = 0; - out_388070008261359487[52] = 0; - out_388070008261359487[53] = 0; - out_388070008261359487[54] = 0; - out_388070008261359487[55] = 0; - out_388070008261359487[56] = 0; - out_388070008261359487[57] = 1; - out_388070008261359487[58] = 0; - out_388070008261359487[59] = 0; - out_388070008261359487[60] = 0; - out_388070008261359487[61] = 0; - out_388070008261359487[62] = 0; - out_388070008261359487[63] = 0; - out_388070008261359487[64] = 0; - out_388070008261359487[65] = 0; - out_388070008261359487[66] = dt; - out_388070008261359487[67] = 0; - out_388070008261359487[68] = 0; - out_388070008261359487[69] = 0; - out_388070008261359487[70] = 0; - out_388070008261359487[71] = 0; - out_388070008261359487[72] = 0; - out_388070008261359487[73] = 0; - out_388070008261359487[74] = 0; - out_388070008261359487[75] = 0; - out_388070008261359487[76] = 1; - out_388070008261359487[77] = 0; - out_388070008261359487[78] = 0; - out_388070008261359487[79] = 0; - out_388070008261359487[80] = 0; - out_388070008261359487[81] = 0; - out_388070008261359487[82] = 0; - out_388070008261359487[83] = 0; - out_388070008261359487[84] = 0; - out_388070008261359487[85] = dt; - out_388070008261359487[86] = 0; - out_388070008261359487[87] = 0; - out_388070008261359487[88] = 0; - out_388070008261359487[89] = 0; - out_388070008261359487[90] = 0; - out_388070008261359487[91] = 0; - out_388070008261359487[92] = 0; - out_388070008261359487[93] = 0; - out_388070008261359487[94] = 0; - out_388070008261359487[95] = 1; - out_388070008261359487[96] = 0; - out_388070008261359487[97] = 0; - out_388070008261359487[98] = 0; - out_388070008261359487[99] = 0; - out_388070008261359487[100] = 0; - out_388070008261359487[101] = 0; - out_388070008261359487[102] = 0; - out_388070008261359487[103] = 0; - out_388070008261359487[104] = dt; - out_388070008261359487[105] = 0; - out_388070008261359487[106] = 0; - out_388070008261359487[107] = 0; - out_388070008261359487[108] = 0; - out_388070008261359487[109] = 0; - out_388070008261359487[110] = 0; - out_388070008261359487[111] = 0; - out_388070008261359487[112] = 0; - out_388070008261359487[113] = 0; - out_388070008261359487[114] = 1; - out_388070008261359487[115] = 0; - out_388070008261359487[116] = 0; - out_388070008261359487[117] = 0; - out_388070008261359487[118] = 0; - out_388070008261359487[119] = 0; - out_388070008261359487[120] = 0; - out_388070008261359487[121] = 0; - out_388070008261359487[122] = 0; - out_388070008261359487[123] = 0; - out_388070008261359487[124] = 0; - out_388070008261359487[125] = 0; - out_388070008261359487[126] = 0; - out_388070008261359487[127] = 0; - out_388070008261359487[128] = 0; - out_388070008261359487[129] = 0; - out_388070008261359487[130] = 0; - out_388070008261359487[131] = 0; - out_388070008261359487[132] = 0; - out_388070008261359487[133] = 1; - out_388070008261359487[134] = 0; - out_388070008261359487[135] = 0; - out_388070008261359487[136] = 0; - out_388070008261359487[137] = 0; - out_388070008261359487[138] = 0; - out_388070008261359487[139] = 0; - out_388070008261359487[140] = 0; - out_388070008261359487[141] = 0; - out_388070008261359487[142] = 0; - out_388070008261359487[143] = 0; - out_388070008261359487[144] = 0; - out_388070008261359487[145] = 0; - out_388070008261359487[146] = 0; - out_388070008261359487[147] = 0; - out_388070008261359487[148] = 0; - out_388070008261359487[149] = 0; - out_388070008261359487[150] = 0; - out_388070008261359487[151] = 0; - out_388070008261359487[152] = 1; - out_388070008261359487[153] = 0; - out_388070008261359487[154] = 0; - out_388070008261359487[155] = 0; - out_388070008261359487[156] = 0; - out_388070008261359487[157] = 0; - out_388070008261359487[158] = 0; - out_388070008261359487[159] = 0; - out_388070008261359487[160] = 0; - out_388070008261359487[161] = 0; - out_388070008261359487[162] = 0; - out_388070008261359487[163] = 0; - out_388070008261359487[164] = 0; - out_388070008261359487[165] = 0; - out_388070008261359487[166] = 0; - out_388070008261359487[167] = 0; - out_388070008261359487[168] = 0; - out_388070008261359487[169] = 0; - out_388070008261359487[170] = 0; - out_388070008261359487[171] = 1; - out_388070008261359487[172] = 0; - out_388070008261359487[173] = 0; - out_388070008261359487[174] = 0; - out_388070008261359487[175] = 0; - out_388070008261359487[176] = 0; - out_388070008261359487[177] = 0; - out_388070008261359487[178] = 0; - out_388070008261359487[179] = 0; - out_388070008261359487[180] = 0; - out_388070008261359487[181] = 0; - out_388070008261359487[182] = 0; - out_388070008261359487[183] = 0; - out_388070008261359487[184] = 0; - out_388070008261359487[185] = 0; - out_388070008261359487[186] = 0; - out_388070008261359487[187] = 0; - out_388070008261359487[188] = 0; - out_388070008261359487[189] = 0; - out_388070008261359487[190] = 1; - out_388070008261359487[191] = 0; - out_388070008261359487[192] = 0; - out_388070008261359487[193] = 0; - out_388070008261359487[194] = 0; - out_388070008261359487[195] = 0; - out_388070008261359487[196] = 0; - out_388070008261359487[197] = 0; - out_388070008261359487[198] = 0; - out_388070008261359487[199] = 0; - out_388070008261359487[200] = 0; - out_388070008261359487[201] = 0; - out_388070008261359487[202] = 0; - out_388070008261359487[203] = 0; - out_388070008261359487[204] = 0; - out_388070008261359487[205] = 0; - out_388070008261359487[206] = 0; - out_388070008261359487[207] = 0; - out_388070008261359487[208] = 0; - out_388070008261359487[209] = 1; - out_388070008261359487[210] = 0; - out_388070008261359487[211] = 0; - out_388070008261359487[212] = 0; - out_388070008261359487[213] = 0; - out_388070008261359487[214] = 0; - out_388070008261359487[215] = 0; - out_388070008261359487[216] = 0; - out_388070008261359487[217] = 0; - out_388070008261359487[218] = 0; - out_388070008261359487[219] = 0; - out_388070008261359487[220] = 0; - out_388070008261359487[221] = 0; - out_388070008261359487[222] = 0; - out_388070008261359487[223] = 0; - out_388070008261359487[224] = 0; - out_388070008261359487[225] = 0; - out_388070008261359487[226] = 0; - out_388070008261359487[227] = 0; - out_388070008261359487[228] = 1; - out_388070008261359487[229] = 0; - out_388070008261359487[230] = 0; - out_388070008261359487[231] = 0; - out_388070008261359487[232] = 0; - out_388070008261359487[233] = 0; - out_388070008261359487[234] = 0; - out_388070008261359487[235] = 0; - out_388070008261359487[236] = 0; - out_388070008261359487[237] = 0; - out_388070008261359487[238] = 0; - out_388070008261359487[239] = 0; - out_388070008261359487[240] = 0; - out_388070008261359487[241] = 0; - out_388070008261359487[242] = 0; - out_388070008261359487[243] = 0; - out_388070008261359487[244] = 0; - out_388070008261359487[245] = 0; - out_388070008261359487[246] = 0; - out_388070008261359487[247] = 1; - out_388070008261359487[248] = 0; - out_388070008261359487[249] = 0; - out_388070008261359487[250] = 0; - out_388070008261359487[251] = 0; - out_388070008261359487[252] = 0; - out_388070008261359487[253] = 0; - out_388070008261359487[254] = 0; - out_388070008261359487[255] = 0; - out_388070008261359487[256] = 0; - out_388070008261359487[257] = 0; - out_388070008261359487[258] = 0; - out_388070008261359487[259] = 0; - out_388070008261359487[260] = 0; - out_388070008261359487[261] = 0; - out_388070008261359487[262] = 0; - out_388070008261359487[263] = 0; - out_388070008261359487[264] = 0; - out_388070008261359487[265] = 0; - out_388070008261359487[266] = 1; - out_388070008261359487[267] = 0; - out_388070008261359487[268] = 0; - out_388070008261359487[269] = 0; - out_388070008261359487[270] = 0; - out_388070008261359487[271] = 0; - out_388070008261359487[272] = 0; - out_388070008261359487[273] = 0; - out_388070008261359487[274] = 0; - out_388070008261359487[275] = 0; - out_388070008261359487[276] = 0; - out_388070008261359487[277] = 0; - out_388070008261359487[278] = 0; - out_388070008261359487[279] = 0; - out_388070008261359487[280] = 0; - out_388070008261359487[281] = 0; - out_388070008261359487[282] = 0; - out_388070008261359487[283] = 0; - out_388070008261359487[284] = 0; - out_388070008261359487[285] = 1; - out_388070008261359487[286] = 0; - out_388070008261359487[287] = 0; - out_388070008261359487[288] = 0; - out_388070008261359487[289] = 0; - out_388070008261359487[290] = 0; - out_388070008261359487[291] = 0; - out_388070008261359487[292] = 0; - out_388070008261359487[293] = 0; - out_388070008261359487[294] = 0; - out_388070008261359487[295] = 0; - out_388070008261359487[296] = 0; - out_388070008261359487[297] = 0; - out_388070008261359487[298] = 0; - out_388070008261359487[299] = 0; - out_388070008261359487[300] = 0; - out_388070008261359487[301] = 0; - out_388070008261359487[302] = 0; - out_388070008261359487[303] = 0; - out_388070008261359487[304] = 1; - out_388070008261359487[305] = 0; - out_388070008261359487[306] = 0; - out_388070008261359487[307] = 0; - out_388070008261359487[308] = 0; - out_388070008261359487[309] = 0; - out_388070008261359487[310] = 0; - out_388070008261359487[311] = 0; - out_388070008261359487[312] = 0; - out_388070008261359487[313] = 0; - out_388070008261359487[314] = 0; - out_388070008261359487[315] = 0; - out_388070008261359487[316] = 0; - out_388070008261359487[317] = 0; - out_388070008261359487[318] = 0; - out_388070008261359487[319] = 0; - out_388070008261359487[320] = 0; - out_388070008261359487[321] = 0; - out_388070008261359487[322] = 0; - out_388070008261359487[323] = 1; +void F_fun(double *state, double dt, double *out_8405700866591566947) { + out_8405700866591566947[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_8405700866591566947[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_8405700866591566947[2] = 0; + out_8405700866591566947[3] = 0; + out_8405700866591566947[4] = 0; + out_8405700866591566947[5] = 0; + out_8405700866591566947[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_8405700866591566947[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_8405700866591566947[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_8405700866591566947[9] = 0; + out_8405700866591566947[10] = 0; + out_8405700866591566947[11] = 0; + out_8405700866591566947[12] = 0; + out_8405700866591566947[13] = 0; + out_8405700866591566947[14] = 0; + out_8405700866591566947[15] = 0; + out_8405700866591566947[16] = 0; + out_8405700866591566947[17] = 0; + out_8405700866591566947[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_8405700866591566947[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_8405700866591566947[20] = 0; + out_8405700866591566947[21] = 0; + out_8405700866591566947[22] = 0; + out_8405700866591566947[23] = 0; + out_8405700866591566947[24] = 0; + out_8405700866591566947[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_8405700866591566947[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_8405700866591566947[27] = 0; + out_8405700866591566947[28] = 0; + out_8405700866591566947[29] = 0; + out_8405700866591566947[30] = 0; + out_8405700866591566947[31] = 0; + out_8405700866591566947[32] = 0; + out_8405700866591566947[33] = 0; + out_8405700866591566947[34] = 0; + out_8405700866591566947[35] = 0; + out_8405700866591566947[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_8405700866591566947[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_8405700866591566947[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_8405700866591566947[39] = 0; + out_8405700866591566947[40] = 0; + out_8405700866591566947[41] = 0; + out_8405700866591566947[42] = 0; + out_8405700866591566947[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_8405700866591566947[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_8405700866591566947[45] = 0; + out_8405700866591566947[46] = 0; + out_8405700866591566947[47] = 0; + out_8405700866591566947[48] = 0; + out_8405700866591566947[49] = 0; + out_8405700866591566947[50] = 0; + out_8405700866591566947[51] = 0; + out_8405700866591566947[52] = 0; + out_8405700866591566947[53] = 0; + out_8405700866591566947[54] = 0; + out_8405700866591566947[55] = 0; + out_8405700866591566947[56] = 0; + out_8405700866591566947[57] = 1; + out_8405700866591566947[58] = 0; + out_8405700866591566947[59] = 0; + out_8405700866591566947[60] = 0; + out_8405700866591566947[61] = 0; + out_8405700866591566947[62] = 0; + out_8405700866591566947[63] = 0; + out_8405700866591566947[64] = 0; + out_8405700866591566947[65] = 0; + out_8405700866591566947[66] = dt; + out_8405700866591566947[67] = 0; + out_8405700866591566947[68] = 0; + out_8405700866591566947[69] = 0; + out_8405700866591566947[70] = 0; + out_8405700866591566947[71] = 0; + out_8405700866591566947[72] = 0; + out_8405700866591566947[73] = 0; + out_8405700866591566947[74] = 0; + out_8405700866591566947[75] = 0; + out_8405700866591566947[76] = 1; + out_8405700866591566947[77] = 0; + out_8405700866591566947[78] = 0; + out_8405700866591566947[79] = 0; + out_8405700866591566947[80] = 0; + out_8405700866591566947[81] = 0; + out_8405700866591566947[82] = 0; + out_8405700866591566947[83] = 0; + out_8405700866591566947[84] = 0; + out_8405700866591566947[85] = dt; + out_8405700866591566947[86] = 0; + out_8405700866591566947[87] = 0; + out_8405700866591566947[88] = 0; + out_8405700866591566947[89] = 0; + out_8405700866591566947[90] = 0; + out_8405700866591566947[91] = 0; + out_8405700866591566947[92] = 0; + out_8405700866591566947[93] = 0; + out_8405700866591566947[94] = 0; + out_8405700866591566947[95] = 1; + out_8405700866591566947[96] = 0; + out_8405700866591566947[97] = 0; + out_8405700866591566947[98] = 0; + out_8405700866591566947[99] = 0; + out_8405700866591566947[100] = 0; + out_8405700866591566947[101] = 0; + out_8405700866591566947[102] = 0; + out_8405700866591566947[103] = 0; + out_8405700866591566947[104] = dt; + out_8405700866591566947[105] = 0; + out_8405700866591566947[106] = 0; + out_8405700866591566947[107] = 0; + out_8405700866591566947[108] = 0; + out_8405700866591566947[109] = 0; + out_8405700866591566947[110] = 0; + out_8405700866591566947[111] = 0; + out_8405700866591566947[112] = 0; + out_8405700866591566947[113] = 0; + out_8405700866591566947[114] = 1; + out_8405700866591566947[115] = 0; + out_8405700866591566947[116] = 0; + out_8405700866591566947[117] = 0; + out_8405700866591566947[118] = 0; + out_8405700866591566947[119] = 0; + out_8405700866591566947[120] = 0; + out_8405700866591566947[121] = 0; + out_8405700866591566947[122] = 0; + out_8405700866591566947[123] = 0; + out_8405700866591566947[124] = 0; + out_8405700866591566947[125] = 0; + out_8405700866591566947[126] = 0; + out_8405700866591566947[127] = 0; + out_8405700866591566947[128] = 0; + out_8405700866591566947[129] = 0; + out_8405700866591566947[130] = 0; + out_8405700866591566947[131] = 0; + out_8405700866591566947[132] = 0; + out_8405700866591566947[133] = 1; + out_8405700866591566947[134] = 0; + out_8405700866591566947[135] = 0; + out_8405700866591566947[136] = 0; + out_8405700866591566947[137] = 0; + out_8405700866591566947[138] = 0; + out_8405700866591566947[139] = 0; + out_8405700866591566947[140] = 0; + out_8405700866591566947[141] = 0; + out_8405700866591566947[142] = 0; + out_8405700866591566947[143] = 0; + out_8405700866591566947[144] = 0; + out_8405700866591566947[145] = 0; + out_8405700866591566947[146] = 0; + out_8405700866591566947[147] = 0; + out_8405700866591566947[148] = 0; + out_8405700866591566947[149] = 0; + out_8405700866591566947[150] = 0; + out_8405700866591566947[151] = 0; + out_8405700866591566947[152] = 1; + out_8405700866591566947[153] = 0; + out_8405700866591566947[154] = 0; + out_8405700866591566947[155] = 0; + out_8405700866591566947[156] = 0; + out_8405700866591566947[157] = 0; + out_8405700866591566947[158] = 0; + out_8405700866591566947[159] = 0; + out_8405700866591566947[160] = 0; + out_8405700866591566947[161] = 0; + out_8405700866591566947[162] = 0; + out_8405700866591566947[163] = 0; + out_8405700866591566947[164] = 0; + out_8405700866591566947[165] = 0; + out_8405700866591566947[166] = 0; + out_8405700866591566947[167] = 0; + out_8405700866591566947[168] = 0; + out_8405700866591566947[169] = 0; + out_8405700866591566947[170] = 0; + out_8405700866591566947[171] = 1; + out_8405700866591566947[172] = 0; + out_8405700866591566947[173] = 0; + out_8405700866591566947[174] = 0; + out_8405700866591566947[175] = 0; + out_8405700866591566947[176] = 0; + out_8405700866591566947[177] = 0; + out_8405700866591566947[178] = 0; + out_8405700866591566947[179] = 0; + out_8405700866591566947[180] = 0; + out_8405700866591566947[181] = 0; + out_8405700866591566947[182] = 0; + out_8405700866591566947[183] = 0; + out_8405700866591566947[184] = 0; + out_8405700866591566947[185] = 0; + out_8405700866591566947[186] = 0; + out_8405700866591566947[187] = 0; + out_8405700866591566947[188] = 0; + out_8405700866591566947[189] = 0; + out_8405700866591566947[190] = 1; + out_8405700866591566947[191] = 0; + out_8405700866591566947[192] = 0; + out_8405700866591566947[193] = 0; + out_8405700866591566947[194] = 0; + out_8405700866591566947[195] = 0; + out_8405700866591566947[196] = 0; + out_8405700866591566947[197] = 0; + out_8405700866591566947[198] = 0; + out_8405700866591566947[199] = 0; + out_8405700866591566947[200] = 0; + out_8405700866591566947[201] = 0; + out_8405700866591566947[202] = 0; + out_8405700866591566947[203] = 0; + out_8405700866591566947[204] = 0; + out_8405700866591566947[205] = 0; + out_8405700866591566947[206] = 0; + out_8405700866591566947[207] = 0; + out_8405700866591566947[208] = 0; + out_8405700866591566947[209] = 1; + out_8405700866591566947[210] = 0; + out_8405700866591566947[211] = 0; + out_8405700866591566947[212] = 0; + out_8405700866591566947[213] = 0; + out_8405700866591566947[214] = 0; + out_8405700866591566947[215] = 0; + out_8405700866591566947[216] = 0; + out_8405700866591566947[217] = 0; + out_8405700866591566947[218] = 0; + out_8405700866591566947[219] = 0; + out_8405700866591566947[220] = 0; + out_8405700866591566947[221] = 0; + out_8405700866591566947[222] = 0; + out_8405700866591566947[223] = 0; + out_8405700866591566947[224] = 0; + out_8405700866591566947[225] = 0; + out_8405700866591566947[226] = 0; + out_8405700866591566947[227] = 0; + out_8405700866591566947[228] = 1; + out_8405700866591566947[229] = 0; + out_8405700866591566947[230] = 0; + out_8405700866591566947[231] = 0; + out_8405700866591566947[232] = 0; + out_8405700866591566947[233] = 0; + out_8405700866591566947[234] = 0; + out_8405700866591566947[235] = 0; + out_8405700866591566947[236] = 0; + out_8405700866591566947[237] = 0; + out_8405700866591566947[238] = 0; + out_8405700866591566947[239] = 0; + out_8405700866591566947[240] = 0; + out_8405700866591566947[241] = 0; + out_8405700866591566947[242] = 0; + out_8405700866591566947[243] = 0; + out_8405700866591566947[244] = 0; + out_8405700866591566947[245] = 0; + out_8405700866591566947[246] = 0; + out_8405700866591566947[247] = 1; + out_8405700866591566947[248] = 0; + out_8405700866591566947[249] = 0; + out_8405700866591566947[250] = 0; + out_8405700866591566947[251] = 0; + out_8405700866591566947[252] = 0; + out_8405700866591566947[253] = 0; + out_8405700866591566947[254] = 0; + out_8405700866591566947[255] = 0; + out_8405700866591566947[256] = 0; + out_8405700866591566947[257] = 0; + out_8405700866591566947[258] = 0; + out_8405700866591566947[259] = 0; + out_8405700866591566947[260] = 0; + out_8405700866591566947[261] = 0; + out_8405700866591566947[262] = 0; + out_8405700866591566947[263] = 0; + out_8405700866591566947[264] = 0; + out_8405700866591566947[265] = 0; + out_8405700866591566947[266] = 1; + out_8405700866591566947[267] = 0; + out_8405700866591566947[268] = 0; + out_8405700866591566947[269] = 0; + out_8405700866591566947[270] = 0; + out_8405700866591566947[271] = 0; + out_8405700866591566947[272] = 0; + out_8405700866591566947[273] = 0; + out_8405700866591566947[274] = 0; + out_8405700866591566947[275] = 0; + out_8405700866591566947[276] = 0; + out_8405700866591566947[277] = 0; + out_8405700866591566947[278] = 0; + out_8405700866591566947[279] = 0; + out_8405700866591566947[280] = 0; + out_8405700866591566947[281] = 0; + out_8405700866591566947[282] = 0; + out_8405700866591566947[283] = 0; + out_8405700866591566947[284] = 0; + out_8405700866591566947[285] = 1; + out_8405700866591566947[286] = 0; + out_8405700866591566947[287] = 0; + out_8405700866591566947[288] = 0; + out_8405700866591566947[289] = 0; + out_8405700866591566947[290] = 0; + out_8405700866591566947[291] = 0; + out_8405700866591566947[292] = 0; + out_8405700866591566947[293] = 0; + out_8405700866591566947[294] = 0; + out_8405700866591566947[295] = 0; + out_8405700866591566947[296] = 0; + out_8405700866591566947[297] = 0; + out_8405700866591566947[298] = 0; + out_8405700866591566947[299] = 0; + out_8405700866591566947[300] = 0; + out_8405700866591566947[301] = 0; + out_8405700866591566947[302] = 0; + out_8405700866591566947[303] = 0; + out_8405700866591566947[304] = 1; + out_8405700866591566947[305] = 0; + out_8405700866591566947[306] = 0; + out_8405700866591566947[307] = 0; + out_8405700866591566947[308] = 0; + out_8405700866591566947[309] = 0; + out_8405700866591566947[310] = 0; + out_8405700866591566947[311] = 0; + out_8405700866591566947[312] = 0; + out_8405700866591566947[313] = 0; + out_8405700866591566947[314] = 0; + out_8405700866591566947[315] = 0; + out_8405700866591566947[316] = 0; + out_8405700866591566947[317] = 0; + out_8405700866591566947[318] = 0; + out_8405700866591566947[319] = 0; + out_8405700866591566947[320] = 0; + out_8405700866591566947[321] = 0; + out_8405700866591566947[322] = 0; + out_8405700866591566947[323] = 1; } -void h_4(double *state, double *unused, double *out_424143301988092518) { - out_424143301988092518[0] = state[6] + state[9]; - out_424143301988092518[1] = state[7] + state[10]; - out_424143301988092518[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_6714333113820678959) { + out_6714333113820678959[0] = state[6] + state[9]; + out_6714333113820678959[1] = state[7] + state[10]; + out_6714333113820678959[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_6650517199469134710) { - out_6650517199469134710[0] = 0; - out_6650517199469134710[1] = 0; - out_6650517199469134710[2] = 0; - out_6650517199469134710[3] = 0; - out_6650517199469134710[4] = 0; - out_6650517199469134710[5] = 0; - out_6650517199469134710[6] = 1; - out_6650517199469134710[7] = 0; - out_6650517199469134710[8] = 0; - out_6650517199469134710[9] = 1; - out_6650517199469134710[10] = 0; - out_6650517199469134710[11] = 0; - out_6650517199469134710[12] = 0; - out_6650517199469134710[13] = 0; - out_6650517199469134710[14] = 0; - out_6650517199469134710[15] = 0; - out_6650517199469134710[16] = 0; - out_6650517199469134710[17] = 0; - out_6650517199469134710[18] = 0; - out_6650517199469134710[19] = 0; - out_6650517199469134710[20] = 0; - out_6650517199469134710[21] = 0; - out_6650517199469134710[22] = 0; - out_6650517199469134710[23] = 0; - out_6650517199469134710[24] = 0; - out_6650517199469134710[25] = 1; - out_6650517199469134710[26] = 0; - out_6650517199469134710[27] = 0; - out_6650517199469134710[28] = 1; - out_6650517199469134710[29] = 0; - out_6650517199469134710[30] = 0; - out_6650517199469134710[31] = 0; - out_6650517199469134710[32] = 0; - out_6650517199469134710[33] = 0; - out_6650517199469134710[34] = 0; - out_6650517199469134710[35] = 0; - out_6650517199469134710[36] = 0; - out_6650517199469134710[37] = 0; - out_6650517199469134710[38] = 0; - out_6650517199469134710[39] = 0; - out_6650517199469134710[40] = 0; - out_6650517199469134710[41] = 0; - out_6650517199469134710[42] = 0; - out_6650517199469134710[43] = 0; - out_6650517199469134710[44] = 1; - out_6650517199469134710[45] = 0; - out_6650517199469134710[46] = 0; - out_6650517199469134710[47] = 1; - out_6650517199469134710[48] = 0; - out_6650517199469134710[49] = 0; - out_6650517199469134710[50] = 0; - out_6650517199469134710[51] = 0; - out_6650517199469134710[52] = 0; - out_6650517199469134710[53] = 0; +void H_4(double *state, double *unused, double *out_6985632794897197443) { + out_6985632794897197443[0] = 0; + out_6985632794897197443[1] = 0; + out_6985632794897197443[2] = 0; + out_6985632794897197443[3] = 0; + out_6985632794897197443[4] = 0; + out_6985632794897197443[5] = 0; + out_6985632794897197443[6] = 1; + out_6985632794897197443[7] = 0; + out_6985632794897197443[8] = 0; + out_6985632794897197443[9] = 1; + out_6985632794897197443[10] = 0; + out_6985632794897197443[11] = 0; + out_6985632794897197443[12] = 0; + out_6985632794897197443[13] = 0; + out_6985632794897197443[14] = 0; + out_6985632794897197443[15] = 0; + out_6985632794897197443[16] = 0; + out_6985632794897197443[17] = 0; + out_6985632794897197443[18] = 0; + out_6985632794897197443[19] = 0; + out_6985632794897197443[20] = 0; + out_6985632794897197443[21] = 0; + out_6985632794897197443[22] = 0; + out_6985632794897197443[23] = 0; + out_6985632794897197443[24] = 0; + out_6985632794897197443[25] = 1; + out_6985632794897197443[26] = 0; + out_6985632794897197443[27] = 0; + out_6985632794897197443[28] = 1; + out_6985632794897197443[29] = 0; + out_6985632794897197443[30] = 0; + out_6985632794897197443[31] = 0; + out_6985632794897197443[32] = 0; + out_6985632794897197443[33] = 0; + out_6985632794897197443[34] = 0; + out_6985632794897197443[35] = 0; + out_6985632794897197443[36] = 0; + out_6985632794897197443[37] = 0; + out_6985632794897197443[38] = 0; + out_6985632794897197443[39] = 0; + out_6985632794897197443[40] = 0; + out_6985632794897197443[41] = 0; + out_6985632794897197443[42] = 0; + out_6985632794897197443[43] = 0; + out_6985632794897197443[44] = 1; + out_6985632794897197443[45] = 0; + out_6985632794897197443[46] = 0; + out_6985632794897197443[47] = 1; + out_6985632794897197443[48] = 0; + out_6985632794897197443[49] = 0; + out_6985632794897197443[50] = 0; + out_6985632794897197443[51] = 0; + out_6985632794897197443[52] = 0; + out_6985632794897197443[53] = 0; } -void h_10(double *state, double *unused, double *out_5429944465818265103) { - out_5429944465818265103[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_5429944465818265103[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_5429944465818265103[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_1436740119579277140) { + out_1436740119579277140[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_1436740119579277140[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_1436740119579277140[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_6328749297458135702) { - out_6328749297458135702[0] = 0; - out_6328749297458135702[1] = 9.8100000000000005*cos(state[1]); - out_6328749297458135702[2] = 0; - out_6328749297458135702[3] = 0; - out_6328749297458135702[4] = -state[8]; - out_6328749297458135702[5] = state[7]; - out_6328749297458135702[6] = 0; - out_6328749297458135702[7] = state[5]; - out_6328749297458135702[8] = -state[4]; - out_6328749297458135702[9] = 0; - out_6328749297458135702[10] = 0; - out_6328749297458135702[11] = 0; - out_6328749297458135702[12] = 1; - out_6328749297458135702[13] = 0; - out_6328749297458135702[14] = 0; - out_6328749297458135702[15] = 1; - out_6328749297458135702[16] = 0; - out_6328749297458135702[17] = 0; - out_6328749297458135702[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_6328749297458135702[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_6328749297458135702[20] = 0; - out_6328749297458135702[21] = state[8]; - out_6328749297458135702[22] = 0; - out_6328749297458135702[23] = -state[6]; - out_6328749297458135702[24] = -state[5]; - out_6328749297458135702[25] = 0; - out_6328749297458135702[26] = state[3]; - out_6328749297458135702[27] = 0; - out_6328749297458135702[28] = 0; - out_6328749297458135702[29] = 0; - out_6328749297458135702[30] = 0; - out_6328749297458135702[31] = 1; - out_6328749297458135702[32] = 0; - out_6328749297458135702[33] = 0; - out_6328749297458135702[34] = 1; - out_6328749297458135702[35] = 0; - out_6328749297458135702[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_6328749297458135702[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_6328749297458135702[38] = 0; - out_6328749297458135702[39] = -state[7]; - out_6328749297458135702[40] = state[6]; - out_6328749297458135702[41] = 0; - out_6328749297458135702[42] = state[4]; - out_6328749297458135702[43] = -state[3]; - out_6328749297458135702[44] = 0; - out_6328749297458135702[45] = 0; - out_6328749297458135702[46] = 0; - out_6328749297458135702[47] = 0; - out_6328749297458135702[48] = 0; - out_6328749297458135702[49] = 0; - out_6328749297458135702[50] = 1; - out_6328749297458135702[51] = 0; - out_6328749297458135702[52] = 0; - out_6328749297458135702[53] = 1; +void H_10(double *state, double *unused, double *out_1336760473497929829) { + out_1336760473497929829[0] = 0; + out_1336760473497929829[1] = 9.8100000000000005*cos(state[1]); + out_1336760473497929829[2] = 0; + out_1336760473497929829[3] = 0; + out_1336760473497929829[4] = -state[8]; + out_1336760473497929829[5] = state[7]; + out_1336760473497929829[6] = 0; + out_1336760473497929829[7] = state[5]; + out_1336760473497929829[8] = -state[4]; + out_1336760473497929829[9] = 0; + out_1336760473497929829[10] = 0; + out_1336760473497929829[11] = 0; + out_1336760473497929829[12] = 1; + out_1336760473497929829[13] = 0; + out_1336760473497929829[14] = 0; + out_1336760473497929829[15] = 1; + out_1336760473497929829[16] = 0; + out_1336760473497929829[17] = 0; + out_1336760473497929829[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_1336760473497929829[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_1336760473497929829[20] = 0; + out_1336760473497929829[21] = state[8]; + out_1336760473497929829[22] = 0; + out_1336760473497929829[23] = -state[6]; + out_1336760473497929829[24] = -state[5]; + out_1336760473497929829[25] = 0; + out_1336760473497929829[26] = state[3]; + out_1336760473497929829[27] = 0; + out_1336760473497929829[28] = 0; + out_1336760473497929829[29] = 0; + out_1336760473497929829[30] = 0; + out_1336760473497929829[31] = 1; + out_1336760473497929829[32] = 0; + out_1336760473497929829[33] = 0; + out_1336760473497929829[34] = 1; + out_1336760473497929829[35] = 0; + out_1336760473497929829[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_1336760473497929829[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_1336760473497929829[38] = 0; + out_1336760473497929829[39] = -state[7]; + out_1336760473497929829[40] = state[6]; + out_1336760473497929829[41] = 0; + out_1336760473497929829[42] = state[4]; + out_1336760473497929829[43] = -state[3]; + out_1336760473497929829[44] = 0; + out_1336760473497929829[45] = 0; + out_1336760473497929829[46] = 0; + out_1336760473497929829[47] = 0; + out_1336760473497929829[48] = 0; + out_1336760473497929829[49] = 0; + out_1336760473497929829[50] = 1; + out_1336760473497929829[51] = 0; + out_1336760473497929829[52] = 0; + out_1336760473497929829[53] = 1; } -void h_13(double *state, double *unused, double *out_7408027604108256403) { - out_7408027604108256403[0] = state[3]; - out_7408027604108256403[1] = state[4]; - out_7408027604108256403[2] = state[5]; +void h_13(double *state, double *unused, double *out_8610784800470542827) { + out_8610784800470542827[0] = state[3]; + out_8610784800470542827[1] = state[4]; + out_8610784800470542827[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3438243374136801909) { - out_3438243374136801909[0] = 0; - out_3438243374136801909[1] = 0; - out_3438243374136801909[2] = 0; - out_3438243374136801909[3] = 1; - out_3438243374136801909[4] = 0; - out_3438243374136801909[5] = 0; - out_3438243374136801909[6] = 0; - out_3438243374136801909[7] = 0; - out_3438243374136801909[8] = 0; - out_3438243374136801909[9] = 0; - out_3438243374136801909[10] = 0; - out_3438243374136801909[11] = 0; - out_3438243374136801909[12] = 0; - out_3438243374136801909[13] = 0; - out_3438243374136801909[14] = 0; - out_3438243374136801909[15] = 0; - out_3438243374136801909[16] = 0; - out_3438243374136801909[17] = 0; - out_3438243374136801909[18] = 0; - out_3438243374136801909[19] = 0; - out_3438243374136801909[20] = 0; - out_3438243374136801909[21] = 0; - out_3438243374136801909[22] = 1; - out_3438243374136801909[23] = 0; - out_3438243374136801909[24] = 0; - out_3438243374136801909[25] = 0; - out_3438243374136801909[26] = 0; - out_3438243374136801909[27] = 0; - out_3438243374136801909[28] = 0; - out_3438243374136801909[29] = 0; - out_3438243374136801909[30] = 0; - out_3438243374136801909[31] = 0; - out_3438243374136801909[32] = 0; - out_3438243374136801909[33] = 0; - out_3438243374136801909[34] = 0; - out_3438243374136801909[35] = 0; - out_3438243374136801909[36] = 0; - out_3438243374136801909[37] = 0; - out_3438243374136801909[38] = 0; - out_3438243374136801909[39] = 0; - out_3438243374136801909[40] = 0; - out_3438243374136801909[41] = 1; - out_3438243374136801909[42] = 0; - out_3438243374136801909[43] = 0; - out_3438243374136801909[44] = 0; - out_3438243374136801909[45] = 0; - out_3438243374136801909[46] = 0; - out_3438243374136801909[47] = 0; - out_3438243374136801909[48] = 0; - out_3438243374136801909[49] = 0; - out_3438243374136801909[50] = 0; - out_3438243374136801909[51] = 0; - out_3438243374136801909[52] = 0; - out_3438243374136801909[53] = 0; +void H_13(double *state, double *unused, double *out_8248837453480021372) { + out_8248837453480021372[0] = 0; + out_8248837453480021372[1] = 0; + out_8248837453480021372[2] = 0; + out_8248837453480021372[3] = 1; + out_8248837453480021372[4] = 0; + out_8248837453480021372[5] = 0; + out_8248837453480021372[6] = 0; + out_8248837453480021372[7] = 0; + out_8248837453480021372[8] = 0; + out_8248837453480021372[9] = 0; + out_8248837453480021372[10] = 0; + out_8248837453480021372[11] = 0; + out_8248837453480021372[12] = 0; + out_8248837453480021372[13] = 0; + out_8248837453480021372[14] = 0; + out_8248837453480021372[15] = 0; + out_8248837453480021372[16] = 0; + out_8248837453480021372[17] = 0; + out_8248837453480021372[18] = 0; + out_8248837453480021372[19] = 0; + out_8248837453480021372[20] = 0; + out_8248837453480021372[21] = 0; + out_8248837453480021372[22] = 1; + out_8248837453480021372[23] = 0; + out_8248837453480021372[24] = 0; + out_8248837453480021372[25] = 0; + out_8248837453480021372[26] = 0; + out_8248837453480021372[27] = 0; + out_8248837453480021372[28] = 0; + out_8248837453480021372[29] = 0; + out_8248837453480021372[30] = 0; + out_8248837453480021372[31] = 0; + out_8248837453480021372[32] = 0; + out_8248837453480021372[33] = 0; + out_8248837453480021372[34] = 0; + out_8248837453480021372[35] = 0; + out_8248837453480021372[36] = 0; + out_8248837453480021372[37] = 0; + out_8248837453480021372[38] = 0; + out_8248837453480021372[39] = 0; + out_8248837453480021372[40] = 0; + out_8248837453480021372[41] = 1; + out_8248837453480021372[42] = 0; + out_8248837453480021372[43] = 0; + out_8248837453480021372[44] = 0; + out_8248837453480021372[45] = 0; + out_8248837453480021372[46] = 0; + out_8248837453480021372[47] = 0; + out_8248837453480021372[48] = 0; + out_8248837453480021372[49] = 0; + out_8248837453480021372[50] = 0; + out_8248837453480021372[51] = 0; + out_8248837453480021372[52] = 0; + out_8248837453480021372[53] = 0; } -void h_14(double *state, double *unused, double *out_1915359263866192008) { - out_1915359263866192008[0] = state[6]; - out_1915359263866192008[1] = state[7]; - out_1915359263866192008[2] = state[8]; +void h_14(double *state, double *unused, double *out_7905030664698657319) { + out_7905030664698657319[0] = state[6]; + out_7905030664698657319[1] = state[7]; + out_7905030664698657319[2] = state[8]; } -void H_14(double *state, double *unused, double *out_2687276343129650181) { - out_2687276343129650181[0] = 0; - out_2687276343129650181[1] = 0; - out_2687276343129650181[2] = 0; - out_2687276343129650181[3] = 0; - out_2687276343129650181[4] = 0; - out_2687276343129650181[5] = 0; - out_2687276343129650181[6] = 1; - out_2687276343129650181[7] = 0; - out_2687276343129650181[8] = 0; - out_2687276343129650181[9] = 0; - out_2687276343129650181[10] = 0; - out_2687276343129650181[11] = 0; - out_2687276343129650181[12] = 0; - out_2687276343129650181[13] = 0; - out_2687276343129650181[14] = 0; - out_2687276343129650181[15] = 0; - out_2687276343129650181[16] = 0; - out_2687276343129650181[17] = 0; - out_2687276343129650181[18] = 0; - out_2687276343129650181[19] = 0; - out_2687276343129650181[20] = 0; - out_2687276343129650181[21] = 0; - out_2687276343129650181[22] = 0; - out_2687276343129650181[23] = 0; - out_2687276343129650181[24] = 0; - out_2687276343129650181[25] = 1; - out_2687276343129650181[26] = 0; - out_2687276343129650181[27] = 0; - out_2687276343129650181[28] = 0; - out_2687276343129650181[29] = 0; - out_2687276343129650181[30] = 0; - out_2687276343129650181[31] = 0; - out_2687276343129650181[32] = 0; - out_2687276343129650181[33] = 0; - out_2687276343129650181[34] = 0; - out_2687276343129650181[35] = 0; - out_2687276343129650181[36] = 0; - out_2687276343129650181[37] = 0; - out_2687276343129650181[38] = 0; - out_2687276343129650181[39] = 0; - out_2687276343129650181[40] = 0; - out_2687276343129650181[41] = 0; - out_2687276343129650181[42] = 0; - out_2687276343129650181[43] = 0; - out_2687276343129650181[44] = 1; - out_2687276343129650181[45] = 0; - out_2687276343129650181[46] = 0; - out_2687276343129650181[47] = 0; - out_2687276343129650181[48] = 0; - out_2687276343129650181[49] = 0; - out_2687276343129650181[50] = 0; - out_2687276343129650181[51] = 0; - out_2687276343129650181[52] = 0; - out_2687276343129650181[53] = 0; +void H_14(double *state, double *unused, double *out_6550516268252313844) { + out_6550516268252313844[0] = 0; + out_6550516268252313844[1] = 0; + out_6550516268252313844[2] = 0; + out_6550516268252313844[3] = 0; + out_6550516268252313844[4] = 0; + out_6550516268252313844[5] = 0; + out_6550516268252313844[6] = 1; + out_6550516268252313844[7] = 0; + out_6550516268252313844[8] = 0; + out_6550516268252313844[9] = 0; + out_6550516268252313844[10] = 0; + out_6550516268252313844[11] = 0; + out_6550516268252313844[12] = 0; + out_6550516268252313844[13] = 0; + out_6550516268252313844[14] = 0; + out_6550516268252313844[15] = 0; + out_6550516268252313844[16] = 0; + out_6550516268252313844[17] = 0; + out_6550516268252313844[18] = 0; + out_6550516268252313844[19] = 0; + out_6550516268252313844[20] = 0; + out_6550516268252313844[21] = 0; + out_6550516268252313844[22] = 0; + out_6550516268252313844[23] = 0; + out_6550516268252313844[24] = 0; + out_6550516268252313844[25] = 1; + out_6550516268252313844[26] = 0; + out_6550516268252313844[27] = 0; + out_6550516268252313844[28] = 0; + out_6550516268252313844[29] = 0; + out_6550516268252313844[30] = 0; + out_6550516268252313844[31] = 0; + out_6550516268252313844[32] = 0; + out_6550516268252313844[33] = 0; + out_6550516268252313844[34] = 0; + out_6550516268252313844[35] = 0; + out_6550516268252313844[36] = 0; + out_6550516268252313844[37] = 0; + out_6550516268252313844[38] = 0; + out_6550516268252313844[39] = 0; + out_6550516268252313844[40] = 0; + out_6550516268252313844[41] = 0; + out_6550516268252313844[42] = 0; + out_6550516268252313844[43] = 0; + out_6550516268252313844[44] = 1; + out_6550516268252313844[45] = 0; + out_6550516268252313844[46] = 0; + out_6550516268252313844[47] = 0; + out_6550516268252313844[48] = 0; + out_6550516268252313844[49] = 0; + out_6550516268252313844[50] = 0; + out_6550516268252313844[51] = 0; + out_6550516268252313844[52] = 0; + out_6550516268252313844[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_9183838211698085113) { - err_fun(nom_x, delta_x, out_9183838211698085113); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7704133318394013706) { + err_fun(nom_x, delta_x, out_7704133318394013706); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7880707634176237912) { - inv_err_fun(nom_x, true_x, out_7880707634176237912); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5732870200970735889) { + inv_err_fun(nom_x, true_x, out_5732870200970735889); } -void pose_H_mod_fun(double *state, double *out_4757006295873358620) { - H_mod_fun(state, out_4757006295873358620); +void pose_H_mod_fun(double *state, double *out_5613029472060395452) { + H_mod_fun(state, out_5613029472060395452); } -void pose_f_fun(double *state, double dt, double *out_9118920386528411329) { - f_fun(state, dt, out_9118920386528411329); +void pose_f_fun(double *state, double dt, double *out_3560257805552383557) { + f_fun(state, dt, out_3560257805552383557); } -void pose_F_fun(double *state, double dt, double *out_388070008261359487) { - F_fun(state, dt, out_388070008261359487); +void pose_F_fun(double *state, double dt, double *out_8405700866591566947) { + F_fun(state, dt, out_8405700866591566947); } -void pose_h_4(double *state, double *unused, double *out_424143301988092518) { - h_4(state, unused, out_424143301988092518); +void pose_h_4(double *state, double *unused, double *out_6714333113820678959) { + h_4(state, unused, out_6714333113820678959); } -void pose_H_4(double *state, double *unused, double *out_6650517199469134710) { - H_4(state, unused, out_6650517199469134710); +void pose_H_4(double *state, double *unused, double *out_6985632794897197443) { + H_4(state, unused, out_6985632794897197443); } -void pose_h_10(double *state, double *unused, double *out_5429944465818265103) { - h_10(state, unused, out_5429944465818265103); +void pose_h_10(double *state, double *unused, double *out_1436740119579277140) { + h_10(state, unused, out_1436740119579277140); } -void pose_H_10(double *state, double *unused, double *out_6328749297458135702) { - H_10(state, unused, out_6328749297458135702); +void pose_H_10(double *state, double *unused, double *out_1336760473497929829) { + H_10(state, unused, out_1336760473497929829); } -void pose_h_13(double *state, double *unused, double *out_7408027604108256403) { - h_13(state, unused, out_7408027604108256403); +void pose_h_13(double *state, double *unused, double *out_8610784800470542827) { + h_13(state, unused, out_8610784800470542827); } -void pose_H_13(double *state, double *unused, double *out_3438243374136801909) { - H_13(state, unused, out_3438243374136801909); +void pose_H_13(double *state, double *unused, double *out_8248837453480021372) { + H_13(state, unused, out_8248837453480021372); } -void pose_h_14(double *state, double *unused, double *out_1915359263866192008) { - h_14(state, unused, out_1915359263866192008); +void pose_h_14(double *state, double *unused, double *out_7905030664698657319) { + h_14(state, unused, out_7905030664698657319); } -void pose_H_14(double *state, double *unused, double *out_2687276343129650181) { - H_14(state, unused, out_2687276343129650181); +void pose_H_14(double *state, double *unused, double *out_6550516268252313844) { + H_14(state, unused, out_6550516268252313844); } 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 ce528f8b4..f7b3c4f9c 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_9183838211698085113); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7880707634176237912); -void pose_H_mod_fun(double *state, double *out_4757006295873358620); -void pose_f_fun(double *state, double dt, double *out_9118920386528411329); -void pose_F_fun(double *state, double dt, double *out_388070008261359487); -void pose_h_4(double *state, double *unused, double *out_424143301988092518); -void pose_H_4(double *state, double *unused, double *out_6650517199469134710); -void pose_h_10(double *state, double *unused, double *out_5429944465818265103); -void pose_H_10(double *state, double *unused, double *out_6328749297458135702); -void pose_h_13(double *state, double *unused, double *out_7408027604108256403); -void pose_H_13(double *state, double *unused, double *out_3438243374136801909); -void pose_h_14(double *state, double *unused, double *out_1915359263866192008); -void pose_H_14(double *state, double *unused, double *out_2687276343129650181); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7704133318394013706); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5732870200970735889); +void pose_H_mod_fun(double *state, double *out_5613029472060395452); +void pose_f_fun(double *state, double dt, double *out_3560257805552383557); +void pose_F_fun(double *state, double dt, double *out_8405700866591566947); +void pose_h_4(double *state, double *unused, double *out_6714333113820678959); +void pose_H_4(double *state, double *unused, double *out_6985632794897197443); +void pose_h_10(double *state, double *unused, double *out_1436740119579277140); +void pose_H_10(double *state, double *unused, double *out_1336760473497929829); +void pose_h_13(double *state, double *unused, double *out_8610784800470542827); +void pose_H_13(double *state, double *unused, double *out_8248837453480021372); +void pose_h_14(double *state, double *unused, double *out_7905030664698657319); +void pose_H_14(double *state, double *unused, double *out_6550516268252313844); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 022883425..43b12cd78 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ