diff --git a/common/libcommon.a b/common/libcommon.a index adc109b15..ed7391637 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 7c8ffd252..cc0951a6d 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 0b35476db..1f1bc25a2 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 c9b78fdc2..ef8bf1258 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 7881a3aad..6935c5000 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 906710f45..c3abedc83 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 54bdd29e7..107740384 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 b1c903545..38317fd3e 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 e4eddfbbf..5d4456eac 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 e4eddfbbf..5d4456eac 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 e7346af5e..cab12158b 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 b1c903545..38317fd3e 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 b94aee601..a572bac90 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-3d0c3a8a-DEBUG"; +const uint8_t gitversion[19] = "DEV-0f8142a1-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 35e5bd11c..d2f58d053 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 c524942cd..6b200b62c 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 1dc4e82de..12071f14d 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 24ed4f8a1..1f356f98c 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 ad7157e3b..8fb28f083 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 d8167a3b8..6a273dbe1 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 c03587224..52d3cd5ab 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 196adcb98..d103f9265 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 d7f3151fb..256f8d4d9 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 b81ff2bb3..0f1cc1367 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 9974818d9..ca65ece5b 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 3fabe4432..5cc5d2763 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 d832aa82f..c190919a8 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 573b8b51d..f88c07b69 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 a60f6bbe6..b7e217e50 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 dcb55faa0..ed384ac21 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 019e04fb3..314666276 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 f74770607..83c3a853a 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 d2f6b96c0..fb2c2bff2 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 776be5d2b..e41955090 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 33c7014ef..f8833cc44 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-3d0c3a8a-DEBUG \ No newline at end of file +DEV-0f8142a1-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 074b99bce..4cc7a135a 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_1263767819732456823) { - out_1263767819732456823[0] = delta_x[0] + nom_x[0]; - out_1263767819732456823[1] = delta_x[1] + nom_x[1]; - out_1263767819732456823[2] = delta_x[2] + nom_x[2]; - out_1263767819732456823[3] = delta_x[3] + nom_x[3]; - out_1263767819732456823[4] = delta_x[4] + nom_x[4]; - out_1263767819732456823[5] = delta_x[5] + nom_x[5]; - out_1263767819732456823[6] = delta_x[6] + nom_x[6]; - out_1263767819732456823[7] = delta_x[7] + nom_x[7]; - out_1263767819732456823[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_7019737380087612192) { + out_7019737380087612192[0] = delta_x[0] + nom_x[0]; + out_7019737380087612192[1] = delta_x[1] + nom_x[1]; + out_7019737380087612192[2] = delta_x[2] + nom_x[2]; + out_7019737380087612192[3] = delta_x[3] + nom_x[3]; + out_7019737380087612192[4] = delta_x[4] + nom_x[4]; + out_7019737380087612192[5] = delta_x[5] + nom_x[5]; + out_7019737380087612192[6] = delta_x[6] + nom_x[6]; + out_7019737380087612192[7] = delta_x[7] + nom_x[7]; + out_7019737380087612192[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3529295667634364975) { - out_3529295667634364975[0] = -nom_x[0] + true_x[0]; - out_3529295667634364975[1] = -nom_x[1] + true_x[1]; - out_3529295667634364975[2] = -nom_x[2] + true_x[2]; - out_3529295667634364975[3] = -nom_x[3] + true_x[3]; - out_3529295667634364975[4] = -nom_x[4] + true_x[4]; - out_3529295667634364975[5] = -nom_x[5] + true_x[5]; - out_3529295667634364975[6] = -nom_x[6] + true_x[6]; - out_3529295667634364975[7] = -nom_x[7] + true_x[7]; - out_3529295667634364975[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5837591879726777447) { + out_5837591879726777447[0] = -nom_x[0] + true_x[0]; + out_5837591879726777447[1] = -nom_x[1] + true_x[1]; + out_5837591879726777447[2] = -nom_x[2] + true_x[2]; + out_5837591879726777447[3] = -nom_x[3] + true_x[3]; + out_5837591879726777447[4] = -nom_x[4] + true_x[4]; + out_5837591879726777447[5] = -nom_x[5] + true_x[5]; + out_5837591879726777447[6] = -nom_x[6] + true_x[6]; + out_5837591879726777447[7] = -nom_x[7] + true_x[7]; + out_5837591879726777447[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_5031874340838454569) { - out_5031874340838454569[0] = 1.0; - out_5031874340838454569[1] = 0.0; - out_5031874340838454569[2] = 0.0; - out_5031874340838454569[3] = 0.0; - out_5031874340838454569[4] = 0.0; - out_5031874340838454569[5] = 0.0; - out_5031874340838454569[6] = 0.0; - out_5031874340838454569[7] = 0.0; - out_5031874340838454569[8] = 0.0; - out_5031874340838454569[9] = 0.0; - out_5031874340838454569[10] = 1.0; - out_5031874340838454569[11] = 0.0; - out_5031874340838454569[12] = 0.0; - out_5031874340838454569[13] = 0.0; - out_5031874340838454569[14] = 0.0; - out_5031874340838454569[15] = 0.0; - out_5031874340838454569[16] = 0.0; - out_5031874340838454569[17] = 0.0; - out_5031874340838454569[18] = 0.0; - out_5031874340838454569[19] = 0.0; - out_5031874340838454569[20] = 1.0; - out_5031874340838454569[21] = 0.0; - out_5031874340838454569[22] = 0.0; - out_5031874340838454569[23] = 0.0; - out_5031874340838454569[24] = 0.0; - out_5031874340838454569[25] = 0.0; - out_5031874340838454569[26] = 0.0; - out_5031874340838454569[27] = 0.0; - out_5031874340838454569[28] = 0.0; - out_5031874340838454569[29] = 0.0; - out_5031874340838454569[30] = 1.0; - out_5031874340838454569[31] = 0.0; - out_5031874340838454569[32] = 0.0; - out_5031874340838454569[33] = 0.0; - out_5031874340838454569[34] = 0.0; - out_5031874340838454569[35] = 0.0; - out_5031874340838454569[36] = 0.0; - out_5031874340838454569[37] = 0.0; - out_5031874340838454569[38] = 0.0; - out_5031874340838454569[39] = 0.0; - out_5031874340838454569[40] = 1.0; - out_5031874340838454569[41] = 0.0; - out_5031874340838454569[42] = 0.0; - out_5031874340838454569[43] = 0.0; - out_5031874340838454569[44] = 0.0; - out_5031874340838454569[45] = 0.0; - out_5031874340838454569[46] = 0.0; - out_5031874340838454569[47] = 0.0; - out_5031874340838454569[48] = 0.0; - out_5031874340838454569[49] = 0.0; - out_5031874340838454569[50] = 1.0; - out_5031874340838454569[51] = 0.0; - out_5031874340838454569[52] = 0.0; - out_5031874340838454569[53] = 0.0; - out_5031874340838454569[54] = 0.0; - out_5031874340838454569[55] = 0.0; - out_5031874340838454569[56] = 0.0; - out_5031874340838454569[57] = 0.0; - out_5031874340838454569[58] = 0.0; - out_5031874340838454569[59] = 0.0; - out_5031874340838454569[60] = 1.0; - out_5031874340838454569[61] = 0.0; - out_5031874340838454569[62] = 0.0; - out_5031874340838454569[63] = 0.0; - out_5031874340838454569[64] = 0.0; - out_5031874340838454569[65] = 0.0; - out_5031874340838454569[66] = 0.0; - out_5031874340838454569[67] = 0.0; - out_5031874340838454569[68] = 0.0; - out_5031874340838454569[69] = 0.0; - out_5031874340838454569[70] = 1.0; - out_5031874340838454569[71] = 0.0; - out_5031874340838454569[72] = 0.0; - out_5031874340838454569[73] = 0.0; - out_5031874340838454569[74] = 0.0; - out_5031874340838454569[75] = 0.0; - out_5031874340838454569[76] = 0.0; - out_5031874340838454569[77] = 0.0; - out_5031874340838454569[78] = 0.0; - out_5031874340838454569[79] = 0.0; - out_5031874340838454569[80] = 1.0; +void H_mod_fun(double *state, double *out_7212341887433844410) { + out_7212341887433844410[0] = 1.0; + out_7212341887433844410[1] = 0.0; + out_7212341887433844410[2] = 0.0; + out_7212341887433844410[3] = 0.0; + out_7212341887433844410[4] = 0.0; + out_7212341887433844410[5] = 0.0; + out_7212341887433844410[6] = 0.0; + out_7212341887433844410[7] = 0.0; + out_7212341887433844410[8] = 0.0; + out_7212341887433844410[9] = 0.0; + out_7212341887433844410[10] = 1.0; + out_7212341887433844410[11] = 0.0; + out_7212341887433844410[12] = 0.0; + out_7212341887433844410[13] = 0.0; + out_7212341887433844410[14] = 0.0; + out_7212341887433844410[15] = 0.0; + out_7212341887433844410[16] = 0.0; + out_7212341887433844410[17] = 0.0; + out_7212341887433844410[18] = 0.0; + out_7212341887433844410[19] = 0.0; + out_7212341887433844410[20] = 1.0; + out_7212341887433844410[21] = 0.0; + out_7212341887433844410[22] = 0.0; + out_7212341887433844410[23] = 0.0; + out_7212341887433844410[24] = 0.0; + out_7212341887433844410[25] = 0.0; + out_7212341887433844410[26] = 0.0; + out_7212341887433844410[27] = 0.0; + out_7212341887433844410[28] = 0.0; + out_7212341887433844410[29] = 0.0; + out_7212341887433844410[30] = 1.0; + out_7212341887433844410[31] = 0.0; + out_7212341887433844410[32] = 0.0; + out_7212341887433844410[33] = 0.0; + out_7212341887433844410[34] = 0.0; + out_7212341887433844410[35] = 0.0; + out_7212341887433844410[36] = 0.0; + out_7212341887433844410[37] = 0.0; + out_7212341887433844410[38] = 0.0; + out_7212341887433844410[39] = 0.0; + out_7212341887433844410[40] = 1.0; + out_7212341887433844410[41] = 0.0; + out_7212341887433844410[42] = 0.0; + out_7212341887433844410[43] = 0.0; + out_7212341887433844410[44] = 0.0; + out_7212341887433844410[45] = 0.0; + out_7212341887433844410[46] = 0.0; + out_7212341887433844410[47] = 0.0; + out_7212341887433844410[48] = 0.0; + out_7212341887433844410[49] = 0.0; + out_7212341887433844410[50] = 1.0; + out_7212341887433844410[51] = 0.0; + out_7212341887433844410[52] = 0.0; + out_7212341887433844410[53] = 0.0; + out_7212341887433844410[54] = 0.0; + out_7212341887433844410[55] = 0.0; + out_7212341887433844410[56] = 0.0; + out_7212341887433844410[57] = 0.0; + out_7212341887433844410[58] = 0.0; + out_7212341887433844410[59] = 0.0; + out_7212341887433844410[60] = 1.0; + out_7212341887433844410[61] = 0.0; + out_7212341887433844410[62] = 0.0; + out_7212341887433844410[63] = 0.0; + out_7212341887433844410[64] = 0.0; + out_7212341887433844410[65] = 0.0; + out_7212341887433844410[66] = 0.0; + out_7212341887433844410[67] = 0.0; + out_7212341887433844410[68] = 0.0; + out_7212341887433844410[69] = 0.0; + out_7212341887433844410[70] = 1.0; + out_7212341887433844410[71] = 0.0; + out_7212341887433844410[72] = 0.0; + out_7212341887433844410[73] = 0.0; + out_7212341887433844410[74] = 0.0; + out_7212341887433844410[75] = 0.0; + out_7212341887433844410[76] = 0.0; + out_7212341887433844410[77] = 0.0; + out_7212341887433844410[78] = 0.0; + out_7212341887433844410[79] = 0.0; + out_7212341887433844410[80] = 1.0; } -void f_fun(double *state, double dt, double *out_1529561136835295758) { - out_1529561136835295758[0] = state[0]; - out_1529561136835295758[1] = state[1]; - out_1529561136835295758[2] = state[2]; - out_1529561136835295758[3] = state[3]; - out_1529561136835295758[4] = state[4]; - out_1529561136835295758[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_1529561136835295758[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_1529561136835295758[7] = state[7]; - out_1529561136835295758[8] = state[8]; +void f_fun(double *state, double dt, double *out_884422573903963140) { + out_884422573903963140[0] = state[0]; + out_884422573903963140[1] = state[1]; + out_884422573903963140[2] = state[2]; + out_884422573903963140[3] = state[3]; + out_884422573903963140[4] = state[4]; + out_884422573903963140[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_884422573903963140[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_884422573903963140[7] = state[7]; + out_884422573903963140[8] = state[8]; } -void F_fun(double *state, double dt, double *out_665605705060988729) { - out_665605705060988729[0] = 1; - out_665605705060988729[1] = 0; - out_665605705060988729[2] = 0; - out_665605705060988729[3] = 0; - out_665605705060988729[4] = 0; - out_665605705060988729[5] = 0; - out_665605705060988729[6] = 0; - out_665605705060988729[7] = 0; - out_665605705060988729[8] = 0; - out_665605705060988729[9] = 0; - out_665605705060988729[10] = 1; - out_665605705060988729[11] = 0; - out_665605705060988729[12] = 0; - out_665605705060988729[13] = 0; - out_665605705060988729[14] = 0; - out_665605705060988729[15] = 0; - out_665605705060988729[16] = 0; - out_665605705060988729[17] = 0; - out_665605705060988729[18] = 0; - out_665605705060988729[19] = 0; - out_665605705060988729[20] = 1; - out_665605705060988729[21] = 0; - out_665605705060988729[22] = 0; - out_665605705060988729[23] = 0; - out_665605705060988729[24] = 0; - out_665605705060988729[25] = 0; - out_665605705060988729[26] = 0; - out_665605705060988729[27] = 0; - out_665605705060988729[28] = 0; - out_665605705060988729[29] = 0; - out_665605705060988729[30] = 1; - out_665605705060988729[31] = 0; - out_665605705060988729[32] = 0; - out_665605705060988729[33] = 0; - out_665605705060988729[34] = 0; - out_665605705060988729[35] = 0; - out_665605705060988729[36] = 0; - out_665605705060988729[37] = 0; - out_665605705060988729[38] = 0; - out_665605705060988729[39] = 0; - out_665605705060988729[40] = 1; - out_665605705060988729[41] = 0; - out_665605705060988729[42] = 0; - out_665605705060988729[43] = 0; - out_665605705060988729[44] = 0; - out_665605705060988729[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_665605705060988729[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_665605705060988729[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_665605705060988729[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_665605705060988729[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_665605705060988729[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_665605705060988729[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_665605705060988729[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_665605705060988729[53] = -9.8100000000000005*dt; - out_665605705060988729[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_665605705060988729[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_665605705060988729[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_665605705060988729[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_665605705060988729[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_665605705060988729[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_665605705060988729[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_665605705060988729[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_665605705060988729[62] = 0; - out_665605705060988729[63] = 0; - out_665605705060988729[64] = 0; - out_665605705060988729[65] = 0; - out_665605705060988729[66] = 0; - out_665605705060988729[67] = 0; - out_665605705060988729[68] = 0; - out_665605705060988729[69] = 0; - out_665605705060988729[70] = 1; - out_665605705060988729[71] = 0; - out_665605705060988729[72] = 0; - out_665605705060988729[73] = 0; - out_665605705060988729[74] = 0; - out_665605705060988729[75] = 0; - out_665605705060988729[76] = 0; - out_665605705060988729[77] = 0; - out_665605705060988729[78] = 0; - out_665605705060988729[79] = 0; - out_665605705060988729[80] = 1; +void F_fun(double *state, double dt, double *out_7930257990036637257) { + out_7930257990036637257[0] = 1; + out_7930257990036637257[1] = 0; + out_7930257990036637257[2] = 0; + out_7930257990036637257[3] = 0; + out_7930257990036637257[4] = 0; + out_7930257990036637257[5] = 0; + out_7930257990036637257[6] = 0; + out_7930257990036637257[7] = 0; + out_7930257990036637257[8] = 0; + out_7930257990036637257[9] = 0; + out_7930257990036637257[10] = 1; + out_7930257990036637257[11] = 0; + out_7930257990036637257[12] = 0; + out_7930257990036637257[13] = 0; + out_7930257990036637257[14] = 0; + out_7930257990036637257[15] = 0; + out_7930257990036637257[16] = 0; + out_7930257990036637257[17] = 0; + out_7930257990036637257[18] = 0; + out_7930257990036637257[19] = 0; + out_7930257990036637257[20] = 1; + out_7930257990036637257[21] = 0; + out_7930257990036637257[22] = 0; + out_7930257990036637257[23] = 0; + out_7930257990036637257[24] = 0; + out_7930257990036637257[25] = 0; + out_7930257990036637257[26] = 0; + out_7930257990036637257[27] = 0; + out_7930257990036637257[28] = 0; + out_7930257990036637257[29] = 0; + out_7930257990036637257[30] = 1; + out_7930257990036637257[31] = 0; + out_7930257990036637257[32] = 0; + out_7930257990036637257[33] = 0; + out_7930257990036637257[34] = 0; + out_7930257990036637257[35] = 0; + out_7930257990036637257[36] = 0; + out_7930257990036637257[37] = 0; + out_7930257990036637257[38] = 0; + out_7930257990036637257[39] = 0; + out_7930257990036637257[40] = 1; + out_7930257990036637257[41] = 0; + out_7930257990036637257[42] = 0; + out_7930257990036637257[43] = 0; + out_7930257990036637257[44] = 0; + out_7930257990036637257[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_7930257990036637257[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_7930257990036637257[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7930257990036637257[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7930257990036637257[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_7930257990036637257[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_7930257990036637257[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_7930257990036637257[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_7930257990036637257[53] = -9.8100000000000005*dt; + out_7930257990036637257[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_7930257990036637257[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_7930257990036637257[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7930257990036637257[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7930257990036637257[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_7930257990036637257[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_7930257990036637257[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_7930257990036637257[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7930257990036637257[62] = 0; + out_7930257990036637257[63] = 0; + out_7930257990036637257[64] = 0; + out_7930257990036637257[65] = 0; + out_7930257990036637257[66] = 0; + out_7930257990036637257[67] = 0; + out_7930257990036637257[68] = 0; + out_7930257990036637257[69] = 0; + out_7930257990036637257[70] = 1; + out_7930257990036637257[71] = 0; + out_7930257990036637257[72] = 0; + out_7930257990036637257[73] = 0; + out_7930257990036637257[74] = 0; + out_7930257990036637257[75] = 0; + out_7930257990036637257[76] = 0; + out_7930257990036637257[77] = 0; + out_7930257990036637257[78] = 0; + out_7930257990036637257[79] = 0; + out_7930257990036637257[80] = 1; } -void h_25(double *state, double *unused, double *out_2720961962516974745) { - out_2720961962516974745[0] = state[6]; +void h_25(double *state, double *unused, double *out_7492620826943930634) { + out_7492620826943930634[0] = state[6]; } -void H_25(double *state, double *unused, double *out_5863506002035943510) { - out_5863506002035943510[0] = 0; - out_5863506002035943510[1] = 0; - out_5863506002035943510[2] = 0; - out_5863506002035943510[3] = 0; - out_5863506002035943510[4] = 0; - out_5863506002035943510[5] = 0; - out_5863506002035943510[6] = 1; - out_5863506002035943510[7] = 0; - out_5863506002035943510[8] = 0; +void H_25(double *state, double *unused, double *out_6439571343347838097) { + out_6439571343347838097[0] = 0; + out_6439571343347838097[1] = 0; + out_6439571343347838097[2] = 0; + out_6439571343347838097[3] = 0; + out_6439571343347838097[4] = 0; + out_6439571343347838097[5] = 0; + out_6439571343347838097[6] = 1; + out_6439571343347838097[7] = 0; + out_6439571343347838097[8] = 0; } -void h_24(double *state, double *unused, double *out_1485787421513289501) { - out_1485787421513289501[0] = state[4]; - out_1485787421513289501[1] = state[5]; +void h_24(double *state, double *unused, double *out_4985338018763469281) { + out_4985338018763469281[0] = state[4]; + out_4985338018763469281[1] = state[5]; } -void H_24(double *state, double *unused, double *out_1554885631640983383) { - out_1554885631640983383[0] = 0; - out_1554885631640983383[1] = 0; - out_1554885631640983383[2] = 0; - out_1554885631640983383[3] = 0; - out_1554885631640983383[4] = 1; - out_1554885631640983383[5] = 0; - out_1554885631640983383[6] = 0; - out_1554885631640983383[7] = 0; - out_1554885631640983383[8] = 0; - out_1554885631640983383[9] = 0; - out_1554885631640983383[10] = 0; - out_1554885631640983383[11] = 0; - out_1554885631640983383[12] = 0; - out_1554885631640983383[13] = 0; - out_1554885631640983383[14] = 1; - out_1554885631640983383[15] = 0; - out_1554885631640983383[16] = 0; - out_1554885631640983383[17] = 0; +void H_24(double *state, double *unused, double *out_7852599591386246934) { + out_7852599591386246934[0] = 0; + out_7852599591386246934[1] = 0; + out_7852599591386246934[2] = 0; + out_7852599591386246934[3] = 0; + out_7852599591386246934[4] = 1; + out_7852599591386246934[5] = 0; + out_7852599591386246934[6] = 0; + out_7852599591386246934[7] = 0; + out_7852599591386246934[8] = 0; + out_7852599591386246934[9] = 0; + out_7852599591386246934[10] = 0; + out_7852599591386246934[11] = 0; + out_7852599591386246934[12] = 0; + out_7852599591386246934[13] = 0; + out_7852599591386246934[14] = 1; + out_7852599591386246934[15] = 0; + out_7852599591386246934[16] = 0; + out_7852599591386246934[17] = 0; } -void h_30(double *state, double *unused, double *out_2878148630868103890) { - out_2878148630868103890[0] = state[4]; +void h_30(double *state, double *unused, double *out_3190857989944948541) { + out_3190857989944948541[0] = state[4]; } -void H_30(double *state, double *unused, double *out_5992844949179183580) { - out_5992844949179183580[0] = 0; - out_5992844949179183580[1] = 0; - out_5992844949179183580[2] = 0; - out_5992844949179183580[3] = 0; - out_5992844949179183580[4] = 1; - out_5992844949179183580[5] = 0; - out_5992844949179183580[6] = 0; - out_5992844949179183580[7] = 0; - out_5992844949179183580[8] = 0; +void H_30(double *state, double *unused, double *out_8957904301855086724) { + out_8957904301855086724[0] = 0; + out_8957904301855086724[1] = 0; + out_8957904301855086724[2] = 0; + out_8957904301855086724[3] = 0; + out_8957904301855086724[4] = 1; + out_8957904301855086724[5] = 0; + out_8957904301855086724[6] = 0; + out_8957904301855086724[7] = 0; + out_8957904301855086724[8] = 0; } -void h_26(double *state, double *unused, double *out_158637420576228795) { - out_158637420576228795[0] = state[7]; +void h_26(double *state, double *unused, double *out_1810815277715897104) { + out_1810815277715897104[0] = state[7]; } -void H_26(double *state, double *unused, double *out_8841734752799551882) { - out_8841734752799551882[0] = 0; - out_8841734752799551882[1] = 0; - out_8841734752799551882[2] = 0; - out_8841734752799551882[3] = 0; - out_8841734752799551882[4] = 0; - out_8841734752799551882[5] = 0; - out_8841734752799551882[6] = 0; - out_8841734752799551882[7] = 1; - out_8841734752799551882[8] = 0; +void H_26(double *state, double *unused, double *out_2698068024473781873) { + out_2698068024473781873[0] = 0; + out_2698068024473781873[1] = 0; + out_2698068024473781873[2] = 0; + out_2698068024473781873[3] = 0; + out_2698068024473781873[4] = 0; + out_2698068024473781873[5] = 0; + out_2698068024473781873[6] = 0; + out_2698068024473781873[7] = 1; + out_2698068024473781873[8] = 0; } -void h_27(double *state, double *unused, double *out_2280711305143870900) { - out_2280711305143870900[0] = state[3]; +void h_27(double *state, double *unused, double *out_1225216092924761692) { + out_1225216092924761692[0] = state[3]; } -void H_27(double *state, double *unused, double *out_8167608260979608491) { - out_8167608260979608491[0] = 0; - out_8167608260979608491[1] = 0; - out_8167608260979608491[2] = 0; - out_8167608260979608491[3] = 1; - out_8167608260979608491[4] = 0; - out_8167608260979608491[5] = 0; - out_8167608260979608491[6] = 0; - out_8167608260979608491[7] = 0; - out_8167608260979608491[8] = 0; +void H_27(double *state, double *unused, double *out_6783140990054661813) { + out_6783140990054661813[0] = 0; + out_6783140990054661813[1] = 0; + out_6783140990054661813[2] = 0; + out_6783140990054661813[3] = 1; + out_6783140990054661813[4] = 0; + out_6783140990054661813[5] = 0; + out_6783140990054661813[6] = 0; + out_6783140990054661813[7] = 0; + out_6783140990054661813[8] = 0; } -void h_29(double *state, double *unused, double *out_6142394781572372552) { - out_6142394781572372552[0] = state[1]; +void h_29(double *state, double *unused, double *out_8192531257605794186) { + out_8192531257605794186[0] = state[1]; } -void H_29(double *state, double *unused, double *out_5482613604864791396) { - out_5482613604864791396[0] = 0; - out_5482613604864791396[1] = 1; - out_5482613604864791396[2] = 0; - out_5482613604864791396[3] = 0; - out_5482613604864791396[4] = 0; - out_5482613604864791396[5] = 0; - out_5482613604864791396[6] = 0; - out_5482613604864791396[7] = 0; - out_5482613604864791396[8] = 0; +void H_29(double *state, double *unused, double *out_8978608427540072708) { + out_8978608427540072708[0] = 0; + out_8978608427540072708[1] = 1; + out_8978608427540072708[2] = 0; + out_8978608427540072708[3] = 0; + out_8978608427540072708[4] = 0; + out_8978608427540072708[5] = 0; + out_8978608427540072708[6] = 0; + out_8978608427540072708[7] = 0; + out_8978608427540072708[8] = 0; } -void h_28(double *state, double *unused, double *out_6701257866862969865) { - out_6701257866862969865[0] = state[0]; +void h_28(double *state, double *unused, double *out_6973827342419625862) { + out_6973827342419625862[0] = state[0]; } -void H_28(double *state, double *unused, double *out_7881731451775229646) { - out_7881731451775229646[0] = 1; - out_7881731451775229646[1] = 0; - out_7881731451775229646[2] = 0; - out_7881731451775229646[3] = 0; - out_7881731451775229646[4] = 0; - out_7881731451775229646[5] = 0; - out_7881731451775229646[6] = 0; - out_7881731451775229646[7] = 0; - out_7881731451775229646[8] = 0; +void H_28(double *state, double *unused, double *out_4385736629099948334) { + out_4385736629099948334[0] = 1; + out_4385736629099948334[1] = 0; + out_4385736629099948334[2] = 0; + out_4385736629099948334[3] = 0; + out_4385736629099948334[4] = 0; + out_4385736629099948334[5] = 0; + out_4385736629099948334[6] = 0; + out_4385736629099948334[7] = 0; + out_4385736629099948334[8] = 0; } -void h_31(double *state, double *unused, double *out_4049873263833376191) { - out_4049873263833376191[0] = state[8]; +void h_31(double *state, double *unused, double *out_7638140651164585605) { + out_7638140651164585605[0] = state[8]; } -void H_31(double *state, double *unused, double *out_5832860040158983082) { - out_5832860040158983082[0] = 0; - out_5832860040158983082[1] = 0; - out_5832860040158983082[2] = 0; - out_5832860040158983082[3] = 0; - out_5832860040158983082[4] = 0; - out_5832860040158983082[5] = 0; - out_5832860040158983082[6] = 0; - out_5832860040158983082[7] = 0; - out_5832860040158983082[8] = 1; +void H_31(double *state, double *unused, double *out_2071859922240430397) { + out_2071859922240430397[0] = 0; + out_2071859922240430397[1] = 0; + out_2071859922240430397[2] = 0; + out_2071859922240430397[3] = 0; + out_2071859922240430397[4] = 0; + out_2071859922240430397[5] = 0; + out_2071859922240430397[6] = 0; + out_2071859922240430397[7] = 0; + out_2071859922240430397[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_1263767819732456823) { - err_fun(nom_x, delta_x, out_1263767819732456823); +void car_err_fun(double *nom_x, double *delta_x, double *out_7019737380087612192) { + err_fun(nom_x, delta_x, out_7019737380087612192); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3529295667634364975) { - inv_err_fun(nom_x, true_x, out_3529295667634364975); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5837591879726777447) { + inv_err_fun(nom_x, true_x, out_5837591879726777447); } -void car_H_mod_fun(double *state, double *out_5031874340838454569) { - H_mod_fun(state, out_5031874340838454569); +void car_H_mod_fun(double *state, double *out_7212341887433844410) { + H_mod_fun(state, out_7212341887433844410); } -void car_f_fun(double *state, double dt, double *out_1529561136835295758) { - f_fun(state, dt, out_1529561136835295758); +void car_f_fun(double *state, double dt, double *out_884422573903963140) { + f_fun(state, dt, out_884422573903963140); } -void car_F_fun(double *state, double dt, double *out_665605705060988729) { - F_fun(state, dt, out_665605705060988729); +void car_F_fun(double *state, double dt, double *out_7930257990036637257) { + F_fun(state, dt, out_7930257990036637257); } -void car_h_25(double *state, double *unused, double *out_2720961962516974745) { - h_25(state, unused, out_2720961962516974745); +void car_h_25(double *state, double *unused, double *out_7492620826943930634) { + h_25(state, unused, out_7492620826943930634); } -void car_H_25(double *state, double *unused, double *out_5863506002035943510) { - H_25(state, unused, out_5863506002035943510); +void car_H_25(double *state, double *unused, double *out_6439571343347838097) { + H_25(state, unused, out_6439571343347838097); } -void car_h_24(double *state, double *unused, double *out_1485787421513289501) { - h_24(state, unused, out_1485787421513289501); +void car_h_24(double *state, double *unused, double *out_4985338018763469281) { + h_24(state, unused, out_4985338018763469281); } -void car_H_24(double *state, double *unused, double *out_1554885631640983383) { - H_24(state, unused, out_1554885631640983383); +void car_H_24(double *state, double *unused, double *out_7852599591386246934) { + H_24(state, unused, out_7852599591386246934); } -void car_h_30(double *state, double *unused, double *out_2878148630868103890) { - h_30(state, unused, out_2878148630868103890); +void car_h_30(double *state, double *unused, double *out_3190857989944948541) { + h_30(state, unused, out_3190857989944948541); } -void car_H_30(double *state, double *unused, double *out_5992844949179183580) { - H_30(state, unused, out_5992844949179183580); +void car_H_30(double *state, double *unused, double *out_8957904301855086724) { + H_30(state, unused, out_8957904301855086724); } -void car_h_26(double *state, double *unused, double *out_158637420576228795) { - h_26(state, unused, out_158637420576228795); +void car_h_26(double *state, double *unused, double *out_1810815277715897104) { + h_26(state, unused, out_1810815277715897104); } -void car_H_26(double *state, double *unused, double *out_8841734752799551882) { - H_26(state, unused, out_8841734752799551882); +void car_H_26(double *state, double *unused, double *out_2698068024473781873) { + H_26(state, unused, out_2698068024473781873); } -void car_h_27(double *state, double *unused, double *out_2280711305143870900) { - h_27(state, unused, out_2280711305143870900); +void car_h_27(double *state, double *unused, double *out_1225216092924761692) { + h_27(state, unused, out_1225216092924761692); } -void car_H_27(double *state, double *unused, double *out_8167608260979608491) { - H_27(state, unused, out_8167608260979608491); +void car_H_27(double *state, double *unused, double *out_6783140990054661813) { + H_27(state, unused, out_6783140990054661813); } -void car_h_29(double *state, double *unused, double *out_6142394781572372552) { - h_29(state, unused, out_6142394781572372552); +void car_h_29(double *state, double *unused, double *out_8192531257605794186) { + h_29(state, unused, out_8192531257605794186); } -void car_H_29(double *state, double *unused, double *out_5482613604864791396) { - H_29(state, unused, out_5482613604864791396); +void car_H_29(double *state, double *unused, double *out_8978608427540072708) { + H_29(state, unused, out_8978608427540072708); } -void car_h_28(double *state, double *unused, double *out_6701257866862969865) { - h_28(state, unused, out_6701257866862969865); +void car_h_28(double *state, double *unused, double *out_6973827342419625862) { + h_28(state, unused, out_6973827342419625862); } -void car_H_28(double *state, double *unused, double *out_7881731451775229646) { - H_28(state, unused, out_7881731451775229646); +void car_H_28(double *state, double *unused, double *out_4385736629099948334) { + H_28(state, unused, out_4385736629099948334); } -void car_h_31(double *state, double *unused, double *out_4049873263833376191) { - h_31(state, unused, out_4049873263833376191); +void car_h_31(double *state, double *unused, double *out_7638140651164585605) { + h_31(state, unused, out_7638140651164585605); } -void car_H_31(double *state, double *unused, double *out_5832860040158983082) { - H_31(state, unused, out_5832860040158983082); +void car_H_31(double *state, double *unused, double *out_2071859922240430397) { + H_31(state, unused, out_2071859922240430397); } 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 7afaee74a..b65619b4e 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_1263767819732456823); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3529295667634364975); -void car_H_mod_fun(double *state, double *out_5031874340838454569); -void car_f_fun(double *state, double dt, double *out_1529561136835295758); -void car_F_fun(double *state, double dt, double *out_665605705060988729); -void car_h_25(double *state, double *unused, double *out_2720961962516974745); -void car_H_25(double *state, double *unused, double *out_5863506002035943510); -void car_h_24(double *state, double *unused, double *out_1485787421513289501); -void car_H_24(double *state, double *unused, double *out_1554885631640983383); -void car_h_30(double *state, double *unused, double *out_2878148630868103890); -void car_H_30(double *state, double *unused, double *out_5992844949179183580); -void car_h_26(double *state, double *unused, double *out_158637420576228795); -void car_H_26(double *state, double *unused, double *out_8841734752799551882); -void car_h_27(double *state, double *unused, double *out_2280711305143870900); -void car_H_27(double *state, double *unused, double *out_8167608260979608491); -void car_h_29(double *state, double *unused, double *out_6142394781572372552); -void car_H_29(double *state, double *unused, double *out_5482613604864791396); -void car_h_28(double *state, double *unused, double *out_6701257866862969865); -void car_H_28(double *state, double *unused, double *out_7881731451775229646); -void car_h_31(double *state, double *unused, double *out_4049873263833376191); -void car_H_31(double *state, double *unused, double *out_5832860040158983082); +void car_err_fun(double *nom_x, double *delta_x, double *out_7019737380087612192); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5837591879726777447); +void car_H_mod_fun(double *state, double *out_7212341887433844410); +void car_f_fun(double *state, double dt, double *out_884422573903963140); +void car_F_fun(double *state, double dt, double *out_7930257990036637257); +void car_h_25(double *state, double *unused, double *out_7492620826943930634); +void car_H_25(double *state, double *unused, double *out_6439571343347838097); +void car_h_24(double *state, double *unused, double *out_4985338018763469281); +void car_H_24(double *state, double *unused, double *out_7852599591386246934); +void car_h_30(double *state, double *unused, double *out_3190857989944948541); +void car_H_30(double *state, double *unused, double *out_8957904301855086724); +void car_h_26(double *state, double *unused, double *out_1810815277715897104); +void car_H_26(double *state, double *unused, double *out_2698068024473781873); +void car_h_27(double *state, double *unused, double *out_1225216092924761692); +void car_H_27(double *state, double *unused, double *out_6783140990054661813); +void car_h_29(double *state, double *unused, double *out_8192531257605794186); +void car_H_29(double *state, double *unused, double *out_8978608427540072708); +void car_h_28(double *state, double *unused, double *out_6973827342419625862); +void car_H_28(double *state, double *unused, double *out_4385736629099948334); +void car_h_31(double *state, double *unused, double *out_7638140651164585605); +void car_H_31(double *state, double *unused, double *out_2071859922240430397); 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 08116cb2e..c1451b31c 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_8550489212526755506) { - out_8550489212526755506[0] = delta_x[0] + nom_x[0]; - out_8550489212526755506[1] = delta_x[1] + nom_x[1]; - out_8550489212526755506[2] = delta_x[2] + nom_x[2]; - out_8550489212526755506[3] = delta_x[3] + nom_x[3]; - out_8550489212526755506[4] = delta_x[4] + nom_x[4]; - out_8550489212526755506[5] = delta_x[5] + nom_x[5]; - out_8550489212526755506[6] = delta_x[6] + nom_x[6]; - out_8550489212526755506[7] = delta_x[7] + nom_x[7]; - out_8550489212526755506[8] = delta_x[8] + nom_x[8]; - out_8550489212526755506[9] = delta_x[9] + nom_x[9]; - out_8550489212526755506[10] = delta_x[10] + nom_x[10]; - out_8550489212526755506[11] = delta_x[11] + nom_x[11]; - out_8550489212526755506[12] = delta_x[12] + nom_x[12]; - out_8550489212526755506[13] = delta_x[13] + nom_x[13]; - out_8550489212526755506[14] = delta_x[14] + nom_x[14]; - out_8550489212526755506[15] = delta_x[15] + nom_x[15]; - out_8550489212526755506[16] = delta_x[16] + nom_x[16]; - out_8550489212526755506[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_937271495952840532) { + out_937271495952840532[0] = delta_x[0] + nom_x[0]; + out_937271495952840532[1] = delta_x[1] + nom_x[1]; + out_937271495952840532[2] = delta_x[2] + nom_x[2]; + out_937271495952840532[3] = delta_x[3] + nom_x[3]; + out_937271495952840532[4] = delta_x[4] + nom_x[4]; + out_937271495952840532[5] = delta_x[5] + nom_x[5]; + out_937271495952840532[6] = delta_x[6] + nom_x[6]; + out_937271495952840532[7] = delta_x[7] + nom_x[7]; + out_937271495952840532[8] = delta_x[8] + nom_x[8]; + out_937271495952840532[9] = delta_x[9] + nom_x[9]; + out_937271495952840532[10] = delta_x[10] + nom_x[10]; + out_937271495952840532[11] = delta_x[11] + nom_x[11]; + out_937271495952840532[12] = delta_x[12] + nom_x[12]; + out_937271495952840532[13] = delta_x[13] + nom_x[13]; + out_937271495952840532[14] = delta_x[14] + nom_x[14]; + out_937271495952840532[15] = delta_x[15] + nom_x[15]; + out_937271495952840532[16] = delta_x[16] + nom_x[16]; + out_937271495952840532[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_2197167056715094306) { - out_2197167056715094306[0] = -nom_x[0] + true_x[0]; - out_2197167056715094306[1] = -nom_x[1] + true_x[1]; - out_2197167056715094306[2] = -nom_x[2] + true_x[2]; - out_2197167056715094306[3] = -nom_x[3] + true_x[3]; - out_2197167056715094306[4] = -nom_x[4] + true_x[4]; - out_2197167056715094306[5] = -nom_x[5] + true_x[5]; - out_2197167056715094306[6] = -nom_x[6] + true_x[6]; - out_2197167056715094306[7] = -nom_x[7] + true_x[7]; - out_2197167056715094306[8] = -nom_x[8] + true_x[8]; - out_2197167056715094306[9] = -nom_x[9] + true_x[9]; - out_2197167056715094306[10] = -nom_x[10] + true_x[10]; - out_2197167056715094306[11] = -nom_x[11] + true_x[11]; - out_2197167056715094306[12] = -nom_x[12] + true_x[12]; - out_2197167056715094306[13] = -nom_x[13] + true_x[13]; - out_2197167056715094306[14] = -nom_x[14] + true_x[14]; - out_2197167056715094306[15] = -nom_x[15] + true_x[15]; - out_2197167056715094306[16] = -nom_x[16] + true_x[16]; - out_2197167056715094306[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_712402659692030222) { + out_712402659692030222[0] = -nom_x[0] + true_x[0]; + out_712402659692030222[1] = -nom_x[1] + true_x[1]; + out_712402659692030222[2] = -nom_x[2] + true_x[2]; + out_712402659692030222[3] = -nom_x[3] + true_x[3]; + out_712402659692030222[4] = -nom_x[4] + true_x[4]; + out_712402659692030222[5] = -nom_x[5] + true_x[5]; + out_712402659692030222[6] = -nom_x[6] + true_x[6]; + out_712402659692030222[7] = -nom_x[7] + true_x[7]; + out_712402659692030222[8] = -nom_x[8] + true_x[8]; + out_712402659692030222[9] = -nom_x[9] + true_x[9]; + out_712402659692030222[10] = -nom_x[10] + true_x[10]; + out_712402659692030222[11] = -nom_x[11] + true_x[11]; + out_712402659692030222[12] = -nom_x[12] + true_x[12]; + out_712402659692030222[13] = -nom_x[13] + true_x[13]; + out_712402659692030222[14] = -nom_x[14] + true_x[14]; + out_712402659692030222[15] = -nom_x[15] + true_x[15]; + out_712402659692030222[16] = -nom_x[16] + true_x[16]; + out_712402659692030222[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_1273572215837011735) { - out_1273572215837011735[0] = 1.0; - out_1273572215837011735[1] = 0.0; - out_1273572215837011735[2] = 0.0; - out_1273572215837011735[3] = 0.0; - out_1273572215837011735[4] = 0.0; - out_1273572215837011735[5] = 0.0; - out_1273572215837011735[6] = 0.0; - out_1273572215837011735[7] = 0.0; - out_1273572215837011735[8] = 0.0; - out_1273572215837011735[9] = 0.0; - out_1273572215837011735[10] = 0.0; - out_1273572215837011735[11] = 0.0; - out_1273572215837011735[12] = 0.0; - out_1273572215837011735[13] = 0.0; - out_1273572215837011735[14] = 0.0; - out_1273572215837011735[15] = 0.0; - out_1273572215837011735[16] = 0.0; - out_1273572215837011735[17] = 0.0; - out_1273572215837011735[18] = 0.0; - out_1273572215837011735[19] = 1.0; - out_1273572215837011735[20] = 0.0; - out_1273572215837011735[21] = 0.0; - out_1273572215837011735[22] = 0.0; - out_1273572215837011735[23] = 0.0; - out_1273572215837011735[24] = 0.0; - out_1273572215837011735[25] = 0.0; - out_1273572215837011735[26] = 0.0; - out_1273572215837011735[27] = 0.0; - out_1273572215837011735[28] = 0.0; - out_1273572215837011735[29] = 0.0; - out_1273572215837011735[30] = 0.0; - out_1273572215837011735[31] = 0.0; - out_1273572215837011735[32] = 0.0; - out_1273572215837011735[33] = 0.0; - out_1273572215837011735[34] = 0.0; - out_1273572215837011735[35] = 0.0; - out_1273572215837011735[36] = 0.0; - out_1273572215837011735[37] = 0.0; - out_1273572215837011735[38] = 1.0; - out_1273572215837011735[39] = 0.0; - out_1273572215837011735[40] = 0.0; - out_1273572215837011735[41] = 0.0; - out_1273572215837011735[42] = 0.0; - out_1273572215837011735[43] = 0.0; - out_1273572215837011735[44] = 0.0; - out_1273572215837011735[45] = 0.0; - out_1273572215837011735[46] = 0.0; - out_1273572215837011735[47] = 0.0; - out_1273572215837011735[48] = 0.0; - out_1273572215837011735[49] = 0.0; - out_1273572215837011735[50] = 0.0; - out_1273572215837011735[51] = 0.0; - out_1273572215837011735[52] = 0.0; - out_1273572215837011735[53] = 0.0; - out_1273572215837011735[54] = 0.0; - out_1273572215837011735[55] = 0.0; - out_1273572215837011735[56] = 0.0; - out_1273572215837011735[57] = 1.0; - out_1273572215837011735[58] = 0.0; - out_1273572215837011735[59] = 0.0; - out_1273572215837011735[60] = 0.0; - out_1273572215837011735[61] = 0.0; - out_1273572215837011735[62] = 0.0; - out_1273572215837011735[63] = 0.0; - out_1273572215837011735[64] = 0.0; - out_1273572215837011735[65] = 0.0; - out_1273572215837011735[66] = 0.0; - out_1273572215837011735[67] = 0.0; - out_1273572215837011735[68] = 0.0; - out_1273572215837011735[69] = 0.0; - out_1273572215837011735[70] = 0.0; - out_1273572215837011735[71] = 0.0; - out_1273572215837011735[72] = 0.0; - out_1273572215837011735[73] = 0.0; - out_1273572215837011735[74] = 0.0; - out_1273572215837011735[75] = 0.0; - out_1273572215837011735[76] = 1.0; - out_1273572215837011735[77] = 0.0; - out_1273572215837011735[78] = 0.0; - out_1273572215837011735[79] = 0.0; - out_1273572215837011735[80] = 0.0; - out_1273572215837011735[81] = 0.0; - out_1273572215837011735[82] = 0.0; - out_1273572215837011735[83] = 0.0; - out_1273572215837011735[84] = 0.0; - out_1273572215837011735[85] = 0.0; - out_1273572215837011735[86] = 0.0; - out_1273572215837011735[87] = 0.0; - out_1273572215837011735[88] = 0.0; - out_1273572215837011735[89] = 0.0; - out_1273572215837011735[90] = 0.0; - out_1273572215837011735[91] = 0.0; - out_1273572215837011735[92] = 0.0; - out_1273572215837011735[93] = 0.0; - out_1273572215837011735[94] = 0.0; - out_1273572215837011735[95] = 1.0; - out_1273572215837011735[96] = 0.0; - out_1273572215837011735[97] = 0.0; - out_1273572215837011735[98] = 0.0; - out_1273572215837011735[99] = 0.0; - out_1273572215837011735[100] = 0.0; - out_1273572215837011735[101] = 0.0; - out_1273572215837011735[102] = 0.0; - out_1273572215837011735[103] = 0.0; - out_1273572215837011735[104] = 0.0; - out_1273572215837011735[105] = 0.0; - out_1273572215837011735[106] = 0.0; - out_1273572215837011735[107] = 0.0; - out_1273572215837011735[108] = 0.0; - out_1273572215837011735[109] = 0.0; - out_1273572215837011735[110] = 0.0; - out_1273572215837011735[111] = 0.0; - out_1273572215837011735[112] = 0.0; - out_1273572215837011735[113] = 0.0; - out_1273572215837011735[114] = 1.0; - out_1273572215837011735[115] = 0.0; - out_1273572215837011735[116] = 0.0; - out_1273572215837011735[117] = 0.0; - out_1273572215837011735[118] = 0.0; - out_1273572215837011735[119] = 0.0; - out_1273572215837011735[120] = 0.0; - out_1273572215837011735[121] = 0.0; - out_1273572215837011735[122] = 0.0; - out_1273572215837011735[123] = 0.0; - out_1273572215837011735[124] = 0.0; - out_1273572215837011735[125] = 0.0; - out_1273572215837011735[126] = 0.0; - out_1273572215837011735[127] = 0.0; - out_1273572215837011735[128] = 0.0; - out_1273572215837011735[129] = 0.0; - out_1273572215837011735[130] = 0.0; - out_1273572215837011735[131] = 0.0; - out_1273572215837011735[132] = 0.0; - out_1273572215837011735[133] = 1.0; - out_1273572215837011735[134] = 0.0; - out_1273572215837011735[135] = 0.0; - out_1273572215837011735[136] = 0.0; - out_1273572215837011735[137] = 0.0; - out_1273572215837011735[138] = 0.0; - out_1273572215837011735[139] = 0.0; - out_1273572215837011735[140] = 0.0; - out_1273572215837011735[141] = 0.0; - out_1273572215837011735[142] = 0.0; - out_1273572215837011735[143] = 0.0; - out_1273572215837011735[144] = 0.0; - out_1273572215837011735[145] = 0.0; - out_1273572215837011735[146] = 0.0; - out_1273572215837011735[147] = 0.0; - out_1273572215837011735[148] = 0.0; - out_1273572215837011735[149] = 0.0; - out_1273572215837011735[150] = 0.0; - out_1273572215837011735[151] = 0.0; - out_1273572215837011735[152] = 1.0; - out_1273572215837011735[153] = 0.0; - out_1273572215837011735[154] = 0.0; - out_1273572215837011735[155] = 0.0; - out_1273572215837011735[156] = 0.0; - out_1273572215837011735[157] = 0.0; - out_1273572215837011735[158] = 0.0; - out_1273572215837011735[159] = 0.0; - out_1273572215837011735[160] = 0.0; - out_1273572215837011735[161] = 0.0; - out_1273572215837011735[162] = 0.0; - out_1273572215837011735[163] = 0.0; - out_1273572215837011735[164] = 0.0; - out_1273572215837011735[165] = 0.0; - out_1273572215837011735[166] = 0.0; - out_1273572215837011735[167] = 0.0; - out_1273572215837011735[168] = 0.0; - out_1273572215837011735[169] = 0.0; - out_1273572215837011735[170] = 0.0; - out_1273572215837011735[171] = 1.0; - out_1273572215837011735[172] = 0.0; - out_1273572215837011735[173] = 0.0; - out_1273572215837011735[174] = 0.0; - out_1273572215837011735[175] = 0.0; - out_1273572215837011735[176] = 0.0; - out_1273572215837011735[177] = 0.0; - out_1273572215837011735[178] = 0.0; - out_1273572215837011735[179] = 0.0; - out_1273572215837011735[180] = 0.0; - out_1273572215837011735[181] = 0.0; - out_1273572215837011735[182] = 0.0; - out_1273572215837011735[183] = 0.0; - out_1273572215837011735[184] = 0.0; - out_1273572215837011735[185] = 0.0; - out_1273572215837011735[186] = 0.0; - out_1273572215837011735[187] = 0.0; - out_1273572215837011735[188] = 0.0; - out_1273572215837011735[189] = 0.0; - out_1273572215837011735[190] = 1.0; - out_1273572215837011735[191] = 0.0; - out_1273572215837011735[192] = 0.0; - out_1273572215837011735[193] = 0.0; - out_1273572215837011735[194] = 0.0; - out_1273572215837011735[195] = 0.0; - out_1273572215837011735[196] = 0.0; - out_1273572215837011735[197] = 0.0; - out_1273572215837011735[198] = 0.0; - out_1273572215837011735[199] = 0.0; - out_1273572215837011735[200] = 0.0; - out_1273572215837011735[201] = 0.0; - out_1273572215837011735[202] = 0.0; - out_1273572215837011735[203] = 0.0; - out_1273572215837011735[204] = 0.0; - out_1273572215837011735[205] = 0.0; - out_1273572215837011735[206] = 0.0; - out_1273572215837011735[207] = 0.0; - out_1273572215837011735[208] = 0.0; - out_1273572215837011735[209] = 1.0; - out_1273572215837011735[210] = 0.0; - out_1273572215837011735[211] = 0.0; - out_1273572215837011735[212] = 0.0; - out_1273572215837011735[213] = 0.0; - out_1273572215837011735[214] = 0.0; - out_1273572215837011735[215] = 0.0; - out_1273572215837011735[216] = 0.0; - out_1273572215837011735[217] = 0.0; - out_1273572215837011735[218] = 0.0; - out_1273572215837011735[219] = 0.0; - out_1273572215837011735[220] = 0.0; - out_1273572215837011735[221] = 0.0; - out_1273572215837011735[222] = 0.0; - out_1273572215837011735[223] = 0.0; - out_1273572215837011735[224] = 0.0; - out_1273572215837011735[225] = 0.0; - out_1273572215837011735[226] = 0.0; - out_1273572215837011735[227] = 0.0; - out_1273572215837011735[228] = 1.0; - out_1273572215837011735[229] = 0.0; - out_1273572215837011735[230] = 0.0; - out_1273572215837011735[231] = 0.0; - out_1273572215837011735[232] = 0.0; - out_1273572215837011735[233] = 0.0; - out_1273572215837011735[234] = 0.0; - out_1273572215837011735[235] = 0.0; - out_1273572215837011735[236] = 0.0; - out_1273572215837011735[237] = 0.0; - out_1273572215837011735[238] = 0.0; - out_1273572215837011735[239] = 0.0; - out_1273572215837011735[240] = 0.0; - out_1273572215837011735[241] = 0.0; - out_1273572215837011735[242] = 0.0; - out_1273572215837011735[243] = 0.0; - out_1273572215837011735[244] = 0.0; - out_1273572215837011735[245] = 0.0; - out_1273572215837011735[246] = 0.0; - out_1273572215837011735[247] = 1.0; - out_1273572215837011735[248] = 0.0; - out_1273572215837011735[249] = 0.0; - out_1273572215837011735[250] = 0.0; - out_1273572215837011735[251] = 0.0; - out_1273572215837011735[252] = 0.0; - out_1273572215837011735[253] = 0.0; - out_1273572215837011735[254] = 0.0; - out_1273572215837011735[255] = 0.0; - out_1273572215837011735[256] = 0.0; - out_1273572215837011735[257] = 0.0; - out_1273572215837011735[258] = 0.0; - out_1273572215837011735[259] = 0.0; - out_1273572215837011735[260] = 0.0; - out_1273572215837011735[261] = 0.0; - out_1273572215837011735[262] = 0.0; - out_1273572215837011735[263] = 0.0; - out_1273572215837011735[264] = 0.0; - out_1273572215837011735[265] = 0.0; - out_1273572215837011735[266] = 1.0; - out_1273572215837011735[267] = 0.0; - out_1273572215837011735[268] = 0.0; - out_1273572215837011735[269] = 0.0; - out_1273572215837011735[270] = 0.0; - out_1273572215837011735[271] = 0.0; - out_1273572215837011735[272] = 0.0; - out_1273572215837011735[273] = 0.0; - out_1273572215837011735[274] = 0.0; - out_1273572215837011735[275] = 0.0; - out_1273572215837011735[276] = 0.0; - out_1273572215837011735[277] = 0.0; - out_1273572215837011735[278] = 0.0; - out_1273572215837011735[279] = 0.0; - out_1273572215837011735[280] = 0.0; - out_1273572215837011735[281] = 0.0; - out_1273572215837011735[282] = 0.0; - out_1273572215837011735[283] = 0.0; - out_1273572215837011735[284] = 0.0; - out_1273572215837011735[285] = 1.0; - out_1273572215837011735[286] = 0.0; - out_1273572215837011735[287] = 0.0; - out_1273572215837011735[288] = 0.0; - out_1273572215837011735[289] = 0.0; - out_1273572215837011735[290] = 0.0; - out_1273572215837011735[291] = 0.0; - out_1273572215837011735[292] = 0.0; - out_1273572215837011735[293] = 0.0; - out_1273572215837011735[294] = 0.0; - out_1273572215837011735[295] = 0.0; - out_1273572215837011735[296] = 0.0; - out_1273572215837011735[297] = 0.0; - out_1273572215837011735[298] = 0.0; - out_1273572215837011735[299] = 0.0; - out_1273572215837011735[300] = 0.0; - out_1273572215837011735[301] = 0.0; - out_1273572215837011735[302] = 0.0; - out_1273572215837011735[303] = 0.0; - out_1273572215837011735[304] = 1.0; - out_1273572215837011735[305] = 0.0; - out_1273572215837011735[306] = 0.0; - out_1273572215837011735[307] = 0.0; - out_1273572215837011735[308] = 0.0; - out_1273572215837011735[309] = 0.0; - out_1273572215837011735[310] = 0.0; - out_1273572215837011735[311] = 0.0; - out_1273572215837011735[312] = 0.0; - out_1273572215837011735[313] = 0.0; - out_1273572215837011735[314] = 0.0; - out_1273572215837011735[315] = 0.0; - out_1273572215837011735[316] = 0.0; - out_1273572215837011735[317] = 0.0; - out_1273572215837011735[318] = 0.0; - out_1273572215837011735[319] = 0.0; - out_1273572215837011735[320] = 0.0; - out_1273572215837011735[321] = 0.0; - out_1273572215837011735[322] = 0.0; - out_1273572215837011735[323] = 1.0; +void H_mod_fun(double *state, double *out_4304163029800659692) { + out_4304163029800659692[0] = 1.0; + out_4304163029800659692[1] = 0.0; + out_4304163029800659692[2] = 0.0; + out_4304163029800659692[3] = 0.0; + out_4304163029800659692[4] = 0.0; + out_4304163029800659692[5] = 0.0; + out_4304163029800659692[6] = 0.0; + out_4304163029800659692[7] = 0.0; + out_4304163029800659692[8] = 0.0; + out_4304163029800659692[9] = 0.0; + out_4304163029800659692[10] = 0.0; + out_4304163029800659692[11] = 0.0; + out_4304163029800659692[12] = 0.0; + out_4304163029800659692[13] = 0.0; + out_4304163029800659692[14] = 0.0; + out_4304163029800659692[15] = 0.0; + out_4304163029800659692[16] = 0.0; + out_4304163029800659692[17] = 0.0; + out_4304163029800659692[18] = 0.0; + out_4304163029800659692[19] = 1.0; + out_4304163029800659692[20] = 0.0; + out_4304163029800659692[21] = 0.0; + out_4304163029800659692[22] = 0.0; + out_4304163029800659692[23] = 0.0; + out_4304163029800659692[24] = 0.0; + out_4304163029800659692[25] = 0.0; + out_4304163029800659692[26] = 0.0; + out_4304163029800659692[27] = 0.0; + out_4304163029800659692[28] = 0.0; + out_4304163029800659692[29] = 0.0; + out_4304163029800659692[30] = 0.0; + out_4304163029800659692[31] = 0.0; + out_4304163029800659692[32] = 0.0; + out_4304163029800659692[33] = 0.0; + out_4304163029800659692[34] = 0.0; + out_4304163029800659692[35] = 0.0; + out_4304163029800659692[36] = 0.0; + out_4304163029800659692[37] = 0.0; + out_4304163029800659692[38] = 1.0; + out_4304163029800659692[39] = 0.0; + out_4304163029800659692[40] = 0.0; + out_4304163029800659692[41] = 0.0; + out_4304163029800659692[42] = 0.0; + out_4304163029800659692[43] = 0.0; + out_4304163029800659692[44] = 0.0; + out_4304163029800659692[45] = 0.0; + out_4304163029800659692[46] = 0.0; + out_4304163029800659692[47] = 0.0; + out_4304163029800659692[48] = 0.0; + out_4304163029800659692[49] = 0.0; + out_4304163029800659692[50] = 0.0; + out_4304163029800659692[51] = 0.0; + out_4304163029800659692[52] = 0.0; + out_4304163029800659692[53] = 0.0; + out_4304163029800659692[54] = 0.0; + out_4304163029800659692[55] = 0.0; + out_4304163029800659692[56] = 0.0; + out_4304163029800659692[57] = 1.0; + out_4304163029800659692[58] = 0.0; + out_4304163029800659692[59] = 0.0; + out_4304163029800659692[60] = 0.0; + out_4304163029800659692[61] = 0.0; + out_4304163029800659692[62] = 0.0; + out_4304163029800659692[63] = 0.0; + out_4304163029800659692[64] = 0.0; + out_4304163029800659692[65] = 0.0; + out_4304163029800659692[66] = 0.0; + out_4304163029800659692[67] = 0.0; + out_4304163029800659692[68] = 0.0; + out_4304163029800659692[69] = 0.0; + out_4304163029800659692[70] = 0.0; + out_4304163029800659692[71] = 0.0; + out_4304163029800659692[72] = 0.0; + out_4304163029800659692[73] = 0.0; + out_4304163029800659692[74] = 0.0; + out_4304163029800659692[75] = 0.0; + out_4304163029800659692[76] = 1.0; + out_4304163029800659692[77] = 0.0; + out_4304163029800659692[78] = 0.0; + out_4304163029800659692[79] = 0.0; + out_4304163029800659692[80] = 0.0; + out_4304163029800659692[81] = 0.0; + out_4304163029800659692[82] = 0.0; + out_4304163029800659692[83] = 0.0; + out_4304163029800659692[84] = 0.0; + out_4304163029800659692[85] = 0.0; + out_4304163029800659692[86] = 0.0; + out_4304163029800659692[87] = 0.0; + out_4304163029800659692[88] = 0.0; + out_4304163029800659692[89] = 0.0; + out_4304163029800659692[90] = 0.0; + out_4304163029800659692[91] = 0.0; + out_4304163029800659692[92] = 0.0; + out_4304163029800659692[93] = 0.0; + out_4304163029800659692[94] = 0.0; + out_4304163029800659692[95] = 1.0; + out_4304163029800659692[96] = 0.0; + out_4304163029800659692[97] = 0.0; + out_4304163029800659692[98] = 0.0; + out_4304163029800659692[99] = 0.0; + out_4304163029800659692[100] = 0.0; + out_4304163029800659692[101] = 0.0; + out_4304163029800659692[102] = 0.0; + out_4304163029800659692[103] = 0.0; + out_4304163029800659692[104] = 0.0; + out_4304163029800659692[105] = 0.0; + out_4304163029800659692[106] = 0.0; + out_4304163029800659692[107] = 0.0; + out_4304163029800659692[108] = 0.0; + out_4304163029800659692[109] = 0.0; + out_4304163029800659692[110] = 0.0; + out_4304163029800659692[111] = 0.0; + out_4304163029800659692[112] = 0.0; + out_4304163029800659692[113] = 0.0; + out_4304163029800659692[114] = 1.0; + out_4304163029800659692[115] = 0.0; + out_4304163029800659692[116] = 0.0; + out_4304163029800659692[117] = 0.0; + out_4304163029800659692[118] = 0.0; + out_4304163029800659692[119] = 0.0; + out_4304163029800659692[120] = 0.0; + out_4304163029800659692[121] = 0.0; + out_4304163029800659692[122] = 0.0; + out_4304163029800659692[123] = 0.0; + out_4304163029800659692[124] = 0.0; + out_4304163029800659692[125] = 0.0; + out_4304163029800659692[126] = 0.0; + out_4304163029800659692[127] = 0.0; + out_4304163029800659692[128] = 0.0; + out_4304163029800659692[129] = 0.0; + out_4304163029800659692[130] = 0.0; + out_4304163029800659692[131] = 0.0; + out_4304163029800659692[132] = 0.0; + out_4304163029800659692[133] = 1.0; + out_4304163029800659692[134] = 0.0; + out_4304163029800659692[135] = 0.0; + out_4304163029800659692[136] = 0.0; + out_4304163029800659692[137] = 0.0; + out_4304163029800659692[138] = 0.0; + out_4304163029800659692[139] = 0.0; + out_4304163029800659692[140] = 0.0; + out_4304163029800659692[141] = 0.0; + out_4304163029800659692[142] = 0.0; + out_4304163029800659692[143] = 0.0; + out_4304163029800659692[144] = 0.0; + out_4304163029800659692[145] = 0.0; + out_4304163029800659692[146] = 0.0; + out_4304163029800659692[147] = 0.0; + out_4304163029800659692[148] = 0.0; + out_4304163029800659692[149] = 0.0; + out_4304163029800659692[150] = 0.0; + out_4304163029800659692[151] = 0.0; + out_4304163029800659692[152] = 1.0; + out_4304163029800659692[153] = 0.0; + out_4304163029800659692[154] = 0.0; + out_4304163029800659692[155] = 0.0; + out_4304163029800659692[156] = 0.0; + out_4304163029800659692[157] = 0.0; + out_4304163029800659692[158] = 0.0; + out_4304163029800659692[159] = 0.0; + out_4304163029800659692[160] = 0.0; + out_4304163029800659692[161] = 0.0; + out_4304163029800659692[162] = 0.0; + out_4304163029800659692[163] = 0.0; + out_4304163029800659692[164] = 0.0; + out_4304163029800659692[165] = 0.0; + out_4304163029800659692[166] = 0.0; + out_4304163029800659692[167] = 0.0; + out_4304163029800659692[168] = 0.0; + out_4304163029800659692[169] = 0.0; + out_4304163029800659692[170] = 0.0; + out_4304163029800659692[171] = 1.0; + out_4304163029800659692[172] = 0.0; + out_4304163029800659692[173] = 0.0; + out_4304163029800659692[174] = 0.0; + out_4304163029800659692[175] = 0.0; + out_4304163029800659692[176] = 0.0; + out_4304163029800659692[177] = 0.0; + out_4304163029800659692[178] = 0.0; + out_4304163029800659692[179] = 0.0; + out_4304163029800659692[180] = 0.0; + out_4304163029800659692[181] = 0.0; + out_4304163029800659692[182] = 0.0; + out_4304163029800659692[183] = 0.0; + out_4304163029800659692[184] = 0.0; + out_4304163029800659692[185] = 0.0; + out_4304163029800659692[186] = 0.0; + out_4304163029800659692[187] = 0.0; + out_4304163029800659692[188] = 0.0; + out_4304163029800659692[189] = 0.0; + out_4304163029800659692[190] = 1.0; + out_4304163029800659692[191] = 0.0; + out_4304163029800659692[192] = 0.0; + out_4304163029800659692[193] = 0.0; + out_4304163029800659692[194] = 0.0; + out_4304163029800659692[195] = 0.0; + out_4304163029800659692[196] = 0.0; + out_4304163029800659692[197] = 0.0; + out_4304163029800659692[198] = 0.0; + out_4304163029800659692[199] = 0.0; + out_4304163029800659692[200] = 0.0; + out_4304163029800659692[201] = 0.0; + out_4304163029800659692[202] = 0.0; + out_4304163029800659692[203] = 0.0; + out_4304163029800659692[204] = 0.0; + out_4304163029800659692[205] = 0.0; + out_4304163029800659692[206] = 0.0; + out_4304163029800659692[207] = 0.0; + out_4304163029800659692[208] = 0.0; + out_4304163029800659692[209] = 1.0; + out_4304163029800659692[210] = 0.0; + out_4304163029800659692[211] = 0.0; + out_4304163029800659692[212] = 0.0; + out_4304163029800659692[213] = 0.0; + out_4304163029800659692[214] = 0.0; + out_4304163029800659692[215] = 0.0; + out_4304163029800659692[216] = 0.0; + out_4304163029800659692[217] = 0.0; + out_4304163029800659692[218] = 0.0; + out_4304163029800659692[219] = 0.0; + out_4304163029800659692[220] = 0.0; + out_4304163029800659692[221] = 0.0; + out_4304163029800659692[222] = 0.0; + out_4304163029800659692[223] = 0.0; + out_4304163029800659692[224] = 0.0; + out_4304163029800659692[225] = 0.0; + out_4304163029800659692[226] = 0.0; + out_4304163029800659692[227] = 0.0; + out_4304163029800659692[228] = 1.0; + out_4304163029800659692[229] = 0.0; + out_4304163029800659692[230] = 0.0; + out_4304163029800659692[231] = 0.0; + out_4304163029800659692[232] = 0.0; + out_4304163029800659692[233] = 0.0; + out_4304163029800659692[234] = 0.0; + out_4304163029800659692[235] = 0.0; + out_4304163029800659692[236] = 0.0; + out_4304163029800659692[237] = 0.0; + out_4304163029800659692[238] = 0.0; + out_4304163029800659692[239] = 0.0; + out_4304163029800659692[240] = 0.0; + out_4304163029800659692[241] = 0.0; + out_4304163029800659692[242] = 0.0; + out_4304163029800659692[243] = 0.0; + out_4304163029800659692[244] = 0.0; + out_4304163029800659692[245] = 0.0; + out_4304163029800659692[246] = 0.0; + out_4304163029800659692[247] = 1.0; + out_4304163029800659692[248] = 0.0; + out_4304163029800659692[249] = 0.0; + out_4304163029800659692[250] = 0.0; + out_4304163029800659692[251] = 0.0; + out_4304163029800659692[252] = 0.0; + out_4304163029800659692[253] = 0.0; + out_4304163029800659692[254] = 0.0; + out_4304163029800659692[255] = 0.0; + out_4304163029800659692[256] = 0.0; + out_4304163029800659692[257] = 0.0; + out_4304163029800659692[258] = 0.0; + out_4304163029800659692[259] = 0.0; + out_4304163029800659692[260] = 0.0; + out_4304163029800659692[261] = 0.0; + out_4304163029800659692[262] = 0.0; + out_4304163029800659692[263] = 0.0; + out_4304163029800659692[264] = 0.0; + out_4304163029800659692[265] = 0.0; + out_4304163029800659692[266] = 1.0; + out_4304163029800659692[267] = 0.0; + out_4304163029800659692[268] = 0.0; + out_4304163029800659692[269] = 0.0; + out_4304163029800659692[270] = 0.0; + out_4304163029800659692[271] = 0.0; + out_4304163029800659692[272] = 0.0; + out_4304163029800659692[273] = 0.0; + out_4304163029800659692[274] = 0.0; + out_4304163029800659692[275] = 0.0; + out_4304163029800659692[276] = 0.0; + out_4304163029800659692[277] = 0.0; + out_4304163029800659692[278] = 0.0; + out_4304163029800659692[279] = 0.0; + out_4304163029800659692[280] = 0.0; + out_4304163029800659692[281] = 0.0; + out_4304163029800659692[282] = 0.0; + out_4304163029800659692[283] = 0.0; + out_4304163029800659692[284] = 0.0; + out_4304163029800659692[285] = 1.0; + out_4304163029800659692[286] = 0.0; + out_4304163029800659692[287] = 0.0; + out_4304163029800659692[288] = 0.0; + out_4304163029800659692[289] = 0.0; + out_4304163029800659692[290] = 0.0; + out_4304163029800659692[291] = 0.0; + out_4304163029800659692[292] = 0.0; + out_4304163029800659692[293] = 0.0; + out_4304163029800659692[294] = 0.0; + out_4304163029800659692[295] = 0.0; + out_4304163029800659692[296] = 0.0; + out_4304163029800659692[297] = 0.0; + out_4304163029800659692[298] = 0.0; + out_4304163029800659692[299] = 0.0; + out_4304163029800659692[300] = 0.0; + out_4304163029800659692[301] = 0.0; + out_4304163029800659692[302] = 0.0; + out_4304163029800659692[303] = 0.0; + out_4304163029800659692[304] = 1.0; + out_4304163029800659692[305] = 0.0; + out_4304163029800659692[306] = 0.0; + out_4304163029800659692[307] = 0.0; + out_4304163029800659692[308] = 0.0; + out_4304163029800659692[309] = 0.0; + out_4304163029800659692[310] = 0.0; + out_4304163029800659692[311] = 0.0; + out_4304163029800659692[312] = 0.0; + out_4304163029800659692[313] = 0.0; + out_4304163029800659692[314] = 0.0; + out_4304163029800659692[315] = 0.0; + out_4304163029800659692[316] = 0.0; + out_4304163029800659692[317] = 0.0; + out_4304163029800659692[318] = 0.0; + out_4304163029800659692[319] = 0.0; + out_4304163029800659692[320] = 0.0; + out_4304163029800659692[321] = 0.0; + out_4304163029800659692[322] = 0.0; + out_4304163029800659692[323] = 1.0; } -void f_fun(double *state, double dt, double *out_58843541046312828) { - out_58843541046312828[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_58843541046312828[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_58843541046312828[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_58843541046312828[3] = dt*state[12] + state[3]; - out_58843541046312828[4] = dt*state[13] + state[4]; - out_58843541046312828[5] = dt*state[14] + state[5]; - out_58843541046312828[6] = state[6]; - out_58843541046312828[7] = state[7]; - out_58843541046312828[8] = state[8]; - out_58843541046312828[9] = state[9]; - out_58843541046312828[10] = state[10]; - out_58843541046312828[11] = state[11]; - out_58843541046312828[12] = state[12]; - out_58843541046312828[13] = state[13]; - out_58843541046312828[14] = state[14]; - out_58843541046312828[15] = state[15]; - out_58843541046312828[16] = state[16]; - out_58843541046312828[17] = state[17]; +void f_fun(double *state, double dt, double *out_7809694961382882469) { + out_7809694961382882469[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_7809694961382882469[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_7809694961382882469[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_7809694961382882469[3] = dt*state[12] + state[3]; + out_7809694961382882469[4] = dt*state[13] + state[4]; + out_7809694961382882469[5] = dt*state[14] + state[5]; + out_7809694961382882469[6] = state[6]; + out_7809694961382882469[7] = state[7]; + out_7809694961382882469[8] = state[8]; + out_7809694961382882469[9] = state[9]; + out_7809694961382882469[10] = state[10]; + out_7809694961382882469[11] = state[11]; + out_7809694961382882469[12] = state[12]; + out_7809694961382882469[13] = state[13]; + out_7809694961382882469[14] = state[14]; + out_7809694961382882469[15] = state[15]; + out_7809694961382882469[16] = state[16]; + out_7809694961382882469[17] = state[17]; } -void F_fun(double *state, double dt, double *out_4952539587807327652) { - out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[2] = 0; - out_4952539587807327652[3] = 0; - out_4952539587807327652[4] = 0; - out_4952539587807327652[5] = 0; - out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[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_4952539587807327652[9] = 0; - out_4952539587807327652[10] = 0; - out_4952539587807327652[11] = 0; - out_4952539587807327652[12] = 0; - out_4952539587807327652[13] = 0; - out_4952539587807327652[14] = 0; - out_4952539587807327652[15] = 0; - out_4952539587807327652[16] = 0; - out_4952539587807327652[17] = 0; - out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[20] = 0; - out_4952539587807327652[21] = 0; - out_4952539587807327652[22] = 0; - out_4952539587807327652[23] = 0; - out_4952539587807327652[24] = 0; - out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[27] = 0; - out_4952539587807327652[28] = 0; - out_4952539587807327652[29] = 0; - out_4952539587807327652[30] = 0; - out_4952539587807327652[31] = 0; - out_4952539587807327652[32] = 0; - out_4952539587807327652[33] = 0; - out_4952539587807327652[34] = 0; - out_4952539587807327652[35] = 0; - out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[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_4952539587807327652[39] = 0; - out_4952539587807327652[40] = 0; - out_4952539587807327652[41] = 0; - out_4952539587807327652[42] = 0; - out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[45] = 0; - out_4952539587807327652[46] = 0; - out_4952539587807327652[47] = 0; - out_4952539587807327652[48] = 0; - out_4952539587807327652[49] = 0; - out_4952539587807327652[50] = 0; - out_4952539587807327652[51] = 0; - out_4952539587807327652[52] = 0; - out_4952539587807327652[53] = 0; - out_4952539587807327652[54] = 0; - out_4952539587807327652[55] = 0; - out_4952539587807327652[56] = 0; - out_4952539587807327652[57] = 1; - out_4952539587807327652[58] = 0; - out_4952539587807327652[59] = 0; - out_4952539587807327652[60] = 0; - out_4952539587807327652[61] = 0; - out_4952539587807327652[62] = 0; - out_4952539587807327652[63] = 0; - out_4952539587807327652[64] = 0; - out_4952539587807327652[65] = 0; - out_4952539587807327652[66] = dt; - out_4952539587807327652[67] = 0; - out_4952539587807327652[68] = 0; - out_4952539587807327652[69] = 0; - out_4952539587807327652[70] = 0; - out_4952539587807327652[71] = 0; - out_4952539587807327652[72] = 0; - out_4952539587807327652[73] = 0; - out_4952539587807327652[74] = 0; - out_4952539587807327652[75] = 0; - out_4952539587807327652[76] = 1; - out_4952539587807327652[77] = 0; - out_4952539587807327652[78] = 0; - out_4952539587807327652[79] = 0; - out_4952539587807327652[80] = 0; - out_4952539587807327652[81] = 0; - out_4952539587807327652[82] = 0; - out_4952539587807327652[83] = 0; - out_4952539587807327652[84] = 0; - out_4952539587807327652[85] = dt; - out_4952539587807327652[86] = 0; - out_4952539587807327652[87] = 0; - out_4952539587807327652[88] = 0; - out_4952539587807327652[89] = 0; - out_4952539587807327652[90] = 0; - out_4952539587807327652[91] = 0; - out_4952539587807327652[92] = 0; - out_4952539587807327652[93] = 0; - out_4952539587807327652[94] = 0; - out_4952539587807327652[95] = 1; - out_4952539587807327652[96] = 0; - out_4952539587807327652[97] = 0; - out_4952539587807327652[98] = 0; - out_4952539587807327652[99] = 0; - out_4952539587807327652[100] = 0; - out_4952539587807327652[101] = 0; - out_4952539587807327652[102] = 0; - out_4952539587807327652[103] = 0; - out_4952539587807327652[104] = dt; - out_4952539587807327652[105] = 0; - out_4952539587807327652[106] = 0; - out_4952539587807327652[107] = 0; - out_4952539587807327652[108] = 0; - out_4952539587807327652[109] = 0; - out_4952539587807327652[110] = 0; - out_4952539587807327652[111] = 0; - out_4952539587807327652[112] = 0; - out_4952539587807327652[113] = 0; - out_4952539587807327652[114] = 1; - out_4952539587807327652[115] = 0; - out_4952539587807327652[116] = 0; - out_4952539587807327652[117] = 0; - out_4952539587807327652[118] = 0; - out_4952539587807327652[119] = 0; - out_4952539587807327652[120] = 0; - out_4952539587807327652[121] = 0; - out_4952539587807327652[122] = 0; - out_4952539587807327652[123] = 0; - out_4952539587807327652[124] = 0; - out_4952539587807327652[125] = 0; - out_4952539587807327652[126] = 0; - out_4952539587807327652[127] = 0; - out_4952539587807327652[128] = 0; - out_4952539587807327652[129] = 0; - out_4952539587807327652[130] = 0; - out_4952539587807327652[131] = 0; - out_4952539587807327652[132] = 0; - out_4952539587807327652[133] = 1; - out_4952539587807327652[134] = 0; - out_4952539587807327652[135] = 0; - out_4952539587807327652[136] = 0; - out_4952539587807327652[137] = 0; - out_4952539587807327652[138] = 0; - out_4952539587807327652[139] = 0; - out_4952539587807327652[140] = 0; - out_4952539587807327652[141] = 0; - out_4952539587807327652[142] = 0; - out_4952539587807327652[143] = 0; - out_4952539587807327652[144] = 0; - out_4952539587807327652[145] = 0; - out_4952539587807327652[146] = 0; - out_4952539587807327652[147] = 0; - out_4952539587807327652[148] = 0; - out_4952539587807327652[149] = 0; - out_4952539587807327652[150] = 0; - out_4952539587807327652[151] = 0; - out_4952539587807327652[152] = 1; - out_4952539587807327652[153] = 0; - out_4952539587807327652[154] = 0; - out_4952539587807327652[155] = 0; - out_4952539587807327652[156] = 0; - out_4952539587807327652[157] = 0; - out_4952539587807327652[158] = 0; - out_4952539587807327652[159] = 0; - out_4952539587807327652[160] = 0; - out_4952539587807327652[161] = 0; - out_4952539587807327652[162] = 0; - out_4952539587807327652[163] = 0; - out_4952539587807327652[164] = 0; - out_4952539587807327652[165] = 0; - out_4952539587807327652[166] = 0; - out_4952539587807327652[167] = 0; - out_4952539587807327652[168] = 0; - out_4952539587807327652[169] = 0; - out_4952539587807327652[170] = 0; - out_4952539587807327652[171] = 1; - out_4952539587807327652[172] = 0; - out_4952539587807327652[173] = 0; - out_4952539587807327652[174] = 0; - out_4952539587807327652[175] = 0; - out_4952539587807327652[176] = 0; - out_4952539587807327652[177] = 0; - out_4952539587807327652[178] = 0; - out_4952539587807327652[179] = 0; - out_4952539587807327652[180] = 0; - out_4952539587807327652[181] = 0; - out_4952539587807327652[182] = 0; - out_4952539587807327652[183] = 0; - out_4952539587807327652[184] = 0; - out_4952539587807327652[185] = 0; - out_4952539587807327652[186] = 0; - out_4952539587807327652[187] = 0; - out_4952539587807327652[188] = 0; - out_4952539587807327652[189] = 0; - out_4952539587807327652[190] = 1; - out_4952539587807327652[191] = 0; - out_4952539587807327652[192] = 0; - out_4952539587807327652[193] = 0; - out_4952539587807327652[194] = 0; - out_4952539587807327652[195] = 0; - out_4952539587807327652[196] = 0; - out_4952539587807327652[197] = 0; - out_4952539587807327652[198] = 0; - out_4952539587807327652[199] = 0; - out_4952539587807327652[200] = 0; - out_4952539587807327652[201] = 0; - out_4952539587807327652[202] = 0; - out_4952539587807327652[203] = 0; - out_4952539587807327652[204] = 0; - out_4952539587807327652[205] = 0; - out_4952539587807327652[206] = 0; - out_4952539587807327652[207] = 0; - out_4952539587807327652[208] = 0; - out_4952539587807327652[209] = 1; - out_4952539587807327652[210] = 0; - out_4952539587807327652[211] = 0; - out_4952539587807327652[212] = 0; - out_4952539587807327652[213] = 0; - out_4952539587807327652[214] = 0; - out_4952539587807327652[215] = 0; - out_4952539587807327652[216] = 0; - out_4952539587807327652[217] = 0; - out_4952539587807327652[218] = 0; - out_4952539587807327652[219] = 0; - out_4952539587807327652[220] = 0; - out_4952539587807327652[221] = 0; - out_4952539587807327652[222] = 0; - out_4952539587807327652[223] = 0; - out_4952539587807327652[224] = 0; - out_4952539587807327652[225] = 0; - out_4952539587807327652[226] = 0; - out_4952539587807327652[227] = 0; - out_4952539587807327652[228] = 1; - out_4952539587807327652[229] = 0; - out_4952539587807327652[230] = 0; - out_4952539587807327652[231] = 0; - out_4952539587807327652[232] = 0; - out_4952539587807327652[233] = 0; - out_4952539587807327652[234] = 0; - out_4952539587807327652[235] = 0; - out_4952539587807327652[236] = 0; - out_4952539587807327652[237] = 0; - out_4952539587807327652[238] = 0; - out_4952539587807327652[239] = 0; - out_4952539587807327652[240] = 0; - out_4952539587807327652[241] = 0; - out_4952539587807327652[242] = 0; - out_4952539587807327652[243] = 0; - out_4952539587807327652[244] = 0; - out_4952539587807327652[245] = 0; - out_4952539587807327652[246] = 0; - out_4952539587807327652[247] = 1; - out_4952539587807327652[248] = 0; - out_4952539587807327652[249] = 0; - out_4952539587807327652[250] = 0; - out_4952539587807327652[251] = 0; - out_4952539587807327652[252] = 0; - out_4952539587807327652[253] = 0; - out_4952539587807327652[254] = 0; - out_4952539587807327652[255] = 0; - out_4952539587807327652[256] = 0; - out_4952539587807327652[257] = 0; - out_4952539587807327652[258] = 0; - out_4952539587807327652[259] = 0; - out_4952539587807327652[260] = 0; - out_4952539587807327652[261] = 0; - out_4952539587807327652[262] = 0; - out_4952539587807327652[263] = 0; - out_4952539587807327652[264] = 0; - out_4952539587807327652[265] = 0; - out_4952539587807327652[266] = 1; - out_4952539587807327652[267] = 0; - out_4952539587807327652[268] = 0; - out_4952539587807327652[269] = 0; - out_4952539587807327652[270] = 0; - out_4952539587807327652[271] = 0; - out_4952539587807327652[272] = 0; - out_4952539587807327652[273] = 0; - out_4952539587807327652[274] = 0; - out_4952539587807327652[275] = 0; - out_4952539587807327652[276] = 0; - out_4952539587807327652[277] = 0; - out_4952539587807327652[278] = 0; - out_4952539587807327652[279] = 0; - out_4952539587807327652[280] = 0; - out_4952539587807327652[281] = 0; - out_4952539587807327652[282] = 0; - out_4952539587807327652[283] = 0; - out_4952539587807327652[284] = 0; - out_4952539587807327652[285] = 1; - out_4952539587807327652[286] = 0; - out_4952539587807327652[287] = 0; - out_4952539587807327652[288] = 0; - out_4952539587807327652[289] = 0; - out_4952539587807327652[290] = 0; - out_4952539587807327652[291] = 0; - out_4952539587807327652[292] = 0; - out_4952539587807327652[293] = 0; - out_4952539587807327652[294] = 0; - out_4952539587807327652[295] = 0; - out_4952539587807327652[296] = 0; - out_4952539587807327652[297] = 0; - out_4952539587807327652[298] = 0; - out_4952539587807327652[299] = 0; - out_4952539587807327652[300] = 0; - out_4952539587807327652[301] = 0; - out_4952539587807327652[302] = 0; - out_4952539587807327652[303] = 0; - out_4952539587807327652[304] = 1; - out_4952539587807327652[305] = 0; - out_4952539587807327652[306] = 0; - out_4952539587807327652[307] = 0; - out_4952539587807327652[308] = 0; - out_4952539587807327652[309] = 0; - out_4952539587807327652[310] = 0; - out_4952539587807327652[311] = 0; - out_4952539587807327652[312] = 0; - out_4952539587807327652[313] = 0; - out_4952539587807327652[314] = 0; - out_4952539587807327652[315] = 0; - out_4952539587807327652[316] = 0; - out_4952539587807327652[317] = 0; - out_4952539587807327652[318] = 0; - out_4952539587807327652[319] = 0; - out_4952539587807327652[320] = 0; - out_4952539587807327652[321] = 0; - out_4952539587807327652[322] = 0; - out_4952539587807327652[323] = 1; +void F_fun(double *state, double dt, double *out_4402047608279739822) { + out_4402047608279739822[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_4402047608279739822[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_4402047608279739822[2] = 0; + out_4402047608279739822[3] = 0; + out_4402047608279739822[4] = 0; + out_4402047608279739822[5] = 0; + out_4402047608279739822[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_4402047608279739822[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_4402047608279739822[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_4402047608279739822[9] = 0; + out_4402047608279739822[10] = 0; + out_4402047608279739822[11] = 0; + out_4402047608279739822[12] = 0; + out_4402047608279739822[13] = 0; + out_4402047608279739822[14] = 0; + out_4402047608279739822[15] = 0; + out_4402047608279739822[16] = 0; + out_4402047608279739822[17] = 0; + out_4402047608279739822[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_4402047608279739822[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_4402047608279739822[20] = 0; + out_4402047608279739822[21] = 0; + out_4402047608279739822[22] = 0; + out_4402047608279739822[23] = 0; + out_4402047608279739822[24] = 0; + out_4402047608279739822[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_4402047608279739822[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_4402047608279739822[27] = 0; + out_4402047608279739822[28] = 0; + out_4402047608279739822[29] = 0; + out_4402047608279739822[30] = 0; + out_4402047608279739822[31] = 0; + out_4402047608279739822[32] = 0; + out_4402047608279739822[33] = 0; + out_4402047608279739822[34] = 0; + out_4402047608279739822[35] = 0; + out_4402047608279739822[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_4402047608279739822[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_4402047608279739822[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_4402047608279739822[39] = 0; + out_4402047608279739822[40] = 0; + out_4402047608279739822[41] = 0; + out_4402047608279739822[42] = 0; + out_4402047608279739822[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_4402047608279739822[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_4402047608279739822[45] = 0; + out_4402047608279739822[46] = 0; + out_4402047608279739822[47] = 0; + out_4402047608279739822[48] = 0; + out_4402047608279739822[49] = 0; + out_4402047608279739822[50] = 0; + out_4402047608279739822[51] = 0; + out_4402047608279739822[52] = 0; + out_4402047608279739822[53] = 0; + out_4402047608279739822[54] = 0; + out_4402047608279739822[55] = 0; + out_4402047608279739822[56] = 0; + out_4402047608279739822[57] = 1; + out_4402047608279739822[58] = 0; + out_4402047608279739822[59] = 0; + out_4402047608279739822[60] = 0; + out_4402047608279739822[61] = 0; + out_4402047608279739822[62] = 0; + out_4402047608279739822[63] = 0; + out_4402047608279739822[64] = 0; + out_4402047608279739822[65] = 0; + out_4402047608279739822[66] = dt; + out_4402047608279739822[67] = 0; + out_4402047608279739822[68] = 0; + out_4402047608279739822[69] = 0; + out_4402047608279739822[70] = 0; + out_4402047608279739822[71] = 0; + out_4402047608279739822[72] = 0; + out_4402047608279739822[73] = 0; + out_4402047608279739822[74] = 0; + out_4402047608279739822[75] = 0; + out_4402047608279739822[76] = 1; + out_4402047608279739822[77] = 0; + out_4402047608279739822[78] = 0; + out_4402047608279739822[79] = 0; + out_4402047608279739822[80] = 0; + out_4402047608279739822[81] = 0; + out_4402047608279739822[82] = 0; + out_4402047608279739822[83] = 0; + out_4402047608279739822[84] = 0; + out_4402047608279739822[85] = dt; + out_4402047608279739822[86] = 0; + out_4402047608279739822[87] = 0; + out_4402047608279739822[88] = 0; + out_4402047608279739822[89] = 0; + out_4402047608279739822[90] = 0; + out_4402047608279739822[91] = 0; + out_4402047608279739822[92] = 0; + out_4402047608279739822[93] = 0; + out_4402047608279739822[94] = 0; + out_4402047608279739822[95] = 1; + out_4402047608279739822[96] = 0; + out_4402047608279739822[97] = 0; + out_4402047608279739822[98] = 0; + out_4402047608279739822[99] = 0; + out_4402047608279739822[100] = 0; + out_4402047608279739822[101] = 0; + out_4402047608279739822[102] = 0; + out_4402047608279739822[103] = 0; + out_4402047608279739822[104] = dt; + out_4402047608279739822[105] = 0; + out_4402047608279739822[106] = 0; + out_4402047608279739822[107] = 0; + out_4402047608279739822[108] = 0; + out_4402047608279739822[109] = 0; + out_4402047608279739822[110] = 0; + out_4402047608279739822[111] = 0; + out_4402047608279739822[112] = 0; + out_4402047608279739822[113] = 0; + out_4402047608279739822[114] = 1; + out_4402047608279739822[115] = 0; + out_4402047608279739822[116] = 0; + out_4402047608279739822[117] = 0; + out_4402047608279739822[118] = 0; + out_4402047608279739822[119] = 0; + out_4402047608279739822[120] = 0; + out_4402047608279739822[121] = 0; + out_4402047608279739822[122] = 0; + out_4402047608279739822[123] = 0; + out_4402047608279739822[124] = 0; + out_4402047608279739822[125] = 0; + out_4402047608279739822[126] = 0; + out_4402047608279739822[127] = 0; + out_4402047608279739822[128] = 0; + out_4402047608279739822[129] = 0; + out_4402047608279739822[130] = 0; + out_4402047608279739822[131] = 0; + out_4402047608279739822[132] = 0; + out_4402047608279739822[133] = 1; + out_4402047608279739822[134] = 0; + out_4402047608279739822[135] = 0; + out_4402047608279739822[136] = 0; + out_4402047608279739822[137] = 0; + out_4402047608279739822[138] = 0; + out_4402047608279739822[139] = 0; + out_4402047608279739822[140] = 0; + out_4402047608279739822[141] = 0; + out_4402047608279739822[142] = 0; + out_4402047608279739822[143] = 0; + out_4402047608279739822[144] = 0; + out_4402047608279739822[145] = 0; + out_4402047608279739822[146] = 0; + out_4402047608279739822[147] = 0; + out_4402047608279739822[148] = 0; + out_4402047608279739822[149] = 0; + out_4402047608279739822[150] = 0; + out_4402047608279739822[151] = 0; + out_4402047608279739822[152] = 1; + out_4402047608279739822[153] = 0; + out_4402047608279739822[154] = 0; + out_4402047608279739822[155] = 0; + out_4402047608279739822[156] = 0; + out_4402047608279739822[157] = 0; + out_4402047608279739822[158] = 0; + out_4402047608279739822[159] = 0; + out_4402047608279739822[160] = 0; + out_4402047608279739822[161] = 0; + out_4402047608279739822[162] = 0; + out_4402047608279739822[163] = 0; + out_4402047608279739822[164] = 0; + out_4402047608279739822[165] = 0; + out_4402047608279739822[166] = 0; + out_4402047608279739822[167] = 0; + out_4402047608279739822[168] = 0; + out_4402047608279739822[169] = 0; + out_4402047608279739822[170] = 0; + out_4402047608279739822[171] = 1; + out_4402047608279739822[172] = 0; + out_4402047608279739822[173] = 0; + out_4402047608279739822[174] = 0; + out_4402047608279739822[175] = 0; + out_4402047608279739822[176] = 0; + out_4402047608279739822[177] = 0; + out_4402047608279739822[178] = 0; + out_4402047608279739822[179] = 0; + out_4402047608279739822[180] = 0; + out_4402047608279739822[181] = 0; + out_4402047608279739822[182] = 0; + out_4402047608279739822[183] = 0; + out_4402047608279739822[184] = 0; + out_4402047608279739822[185] = 0; + out_4402047608279739822[186] = 0; + out_4402047608279739822[187] = 0; + out_4402047608279739822[188] = 0; + out_4402047608279739822[189] = 0; + out_4402047608279739822[190] = 1; + out_4402047608279739822[191] = 0; + out_4402047608279739822[192] = 0; + out_4402047608279739822[193] = 0; + out_4402047608279739822[194] = 0; + out_4402047608279739822[195] = 0; + out_4402047608279739822[196] = 0; + out_4402047608279739822[197] = 0; + out_4402047608279739822[198] = 0; + out_4402047608279739822[199] = 0; + out_4402047608279739822[200] = 0; + out_4402047608279739822[201] = 0; + out_4402047608279739822[202] = 0; + out_4402047608279739822[203] = 0; + out_4402047608279739822[204] = 0; + out_4402047608279739822[205] = 0; + out_4402047608279739822[206] = 0; + out_4402047608279739822[207] = 0; + out_4402047608279739822[208] = 0; + out_4402047608279739822[209] = 1; + out_4402047608279739822[210] = 0; + out_4402047608279739822[211] = 0; + out_4402047608279739822[212] = 0; + out_4402047608279739822[213] = 0; + out_4402047608279739822[214] = 0; + out_4402047608279739822[215] = 0; + out_4402047608279739822[216] = 0; + out_4402047608279739822[217] = 0; + out_4402047608279739822[218] = 0; + out_4402047608279739822[219] = 0; + out_4402047608279739822[220] = 0; + out_4402047608279739822[221] = 0; + out_4402047608279739822[222] = 0; + out_4402047608279739822[223] = 0; + out_4402047608279739822[224] = 0; + out_4402047608279739822[225] = 0; + out_4402047608279739822[226] = 0; + out_4402047608279739822[227] = 0; + out_4402047608279739822[228] = 1; + out_4402047608279739822[229] = 0; + out_4402047608279739822[230] = 0; + out_4402047608279739822[231] = 0; + out_4402047608279739822[232] = 0; + out_4402047608279739822[233] = 0; + out_4402047608279739822[234] = 0; + out_4402047608279739822[235] = 0; + out_4402047608279739822[236] = 0; + out_4402047608279739822[237] = 0; + out_4402047608279739822[238] = 0; + out_4402047608279739822[239] = 0; + out_4402047608279739822[240] = 0; + out_4402047608279739822[241] = 0; + out_4402047608279739822[242] = 0; + out_4402047608279739822[243] = 0; + out_4402047608279739822[244] = 0; + out_4402047608279739822[245] = 0; + out_4402047608279739822[246] = 0; + out_4402047608279739822[247] = 1; + out_4402047608279739822[248] = 0; + out_4402047608279739822[249] = 0; + out_4402047608279739822[250] = 0; + out_4402047608279739822[251] = 0; + out_4402047608279739822[252] = 0; + out_4402047608279739822[253] = 0; + out_4402047608279739822[254] = 0; + out_4402047608279739822[255] = 0; + out_4402047608279739822[256] = 0; + out_4402047608279739822[257] = 0; + out_4402047608279739822[258] = 0; + out_4402047608279739822[259] = 0; + out_4402047608279739822[260] = 0; + out_4402047608279739822[261] = 0; + out_4402047608279739822[262] = 0; + out_4402047608279739822[263] = 0; + out_4402047608279739822[264] = 0; + out_4402047608279739822[265] = 0; + out_4402047608279739822[266] = 1; + out_4402047608279739822[267] = 0; + out_4402047608279739822[268] = 0; + out_4402047608279739822[269] = 0; + out_4402047608279739822[270] = 0; + out_4402047608279739822[271] = 0; + out_4402047608279739822[272] = 0; + out_4402047608279739822[273] = 0; + out_4402047608279739822[274] = 0; + out_4402047608279739822[275] = 0; + out_4402047608279739822[276] = 0; + out_4402047608279739822[277] = 0; + out_4402047608279739822[278] = 0; + out_4402047608279739822[279] = 0; + out_4402047608279739822[280] = 0; + out_4402047608279739822[281] = 0; + out_4402047608279739822[282] = 0; + out_4402047608279739822[283] = 0; + out_4402047608279739822[284] = 0; + out_4402047608279739822[285] = 1; + out_4402047608279739822[286] = 0; + out_4402047608279739822[287] = 0; + out_4402047608279739822[288] = 0; + out_4402047608279739822[289] = 0; + out_4402047608279739822[290] = 0; + out_4402047608279739822[291] = 0; + out_4402047608279739822[292] = 0; + out_4402047608279739822[293] = 0; + out_4402047608279739822[294] = 0; + out_4402047608279739822[295] = 0; + out_4402047608279739822[296] = 0; + out_4402047608279739822[297] = 0; + out_4402047608279739822[298] = 0; + out_4402047608279739822[299] = 0; + out_4402047608279739822[300] = 0; + out_4402047608279739822[301] = 0; + out_4402047608279739822[302] = 0; + out_4402047608279739822[303] = 0; + out_4402047608279739822[304] = 1; + out_4402047608279739822[305] = 0; + out_4402047608279739822[306] = 0; + out_4402047608279739822[307] = 0; + out_4402047608279739822[308] = 0; + out_4402047608279739822[309] = 0; + out_4402047608279739822[310] = 0; + out_4402047608279739822[311] = 0; + out_4402047608279739822[312] = 0; + out_4402047608279739822[313] = 0; + out_4402047608279739822[314] = 0; + out_4402047608279739822[315] = 0; + out_4402047608279739822[316] = 0; + out_4402047608279739822[317] = 0; + out_4402047608279739822[318] = 0; + out_4402047608279739822[319] = 0; + out_4402047608279739822[320] = 0; + out_4402047608279739822[321] = 0; + out_4402047608279739822[322] = 0; + out_4402047608279739822[323] = 1; } -void h_4(double *state, double *unused, double *out_105029621528743882) { - out_105029621528743882[0] = state[6] + state[9]; - out_105029621528743882[1] = state[7] + state[10]; - out_105029621528743882[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_1761018668225147827) { + out_1761018668225147827[0] = state[6] + state[9]; + out_1761018668225147827[1] = state[7] + state[10]; + out_1761018668225147827[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_7121654022588970456) { - out_7121654022588970456[0] = 0; - out_7121654022588970456[1] = 0; - out_7121654022588970456[2] = 0; - out_7121654022588970456[3] = 0; - out_7121654022588970456[4] = 0; - out_7121654022588970456[5] = 0; - out_7121654022588970456[6] = 1; - out_7121654022588970456[7] = 0; - out_7121654022588970456[8] = 0; - out_7121654022588970456[9] = 1; - out_7121654022588970456[10] = 0; - out_7121654022588970456[11] = 0; - out_7121654022588970456[12] = 0; - out_7121654022588970456[13] = 0; - out_7121654022588970456[14] = 0; - out_7121654022588970456[15] = 0; - out_7121654022588970456[16] = 0; - out_7121654022588970456[17] = 0; - out_7121654022588970456[18] = 0; - out_7121654022588970456[19] = 0; - out_7121654022588970456[20] = 0; - out_7121654022588970456[21] = 0; - out_7121654022588970456[22] = 0; - out_7121654022588970456[23] = 0; - out_7121654022588970456[24] = 0; - out_7121654022588970456[25] = 1; - out_7121654022588970456[26] = 0; - out_7121654022588970456[27] = 0; - out_7121654022588970456[28] = 1; - out_7121654022588970456[29] = 0; - out_7121654022588970456[30] = 0; - out_7121654022588970456[31] = 0; - out_7121654022588970456[32] = 0; - out_7121654022588970456[33] = 0; - out_7121654022588970456[34] = 0; - out_7121654022588970456[35] = 0; - out_7121654022588970456[36] = 0; - out_7121654022588970456[37] = 0; - out_7121654022588970456[38] = 0; - out_7121654022588970456[39] = 0; - out_7121654022588970456[40] = 0; - out_7121654022588970456[41] = 0; - out_7121654022588970456[42] = 0; - out_7121654022588970456[43] = 0; - out_7121654022588970456[44] = 1; - out_7121654022588970456[45] = 0; - out_7121654022588970456[46] = 0; - out_7121654022588970456[47] = 1; - out_7121654022588970456[48] = 0; - out_7121654022588970456[49] = 0; - out_7121654022588970456[50] = 0; - out_7121654022588970456[51] = 0; - out_7121654022588970456[52] = 0; - out_7121654022588970456[53] = 0; +void H_4(double *state, double *unused, double *out_6123861755706956553) { + out_6123861755706956553[0] = 0; + out_6123861755706956553[1] = 0; + out_6123861755706956553[2] = 0; + out_6123861755706956553[3] = 0; + out_6123861755706956553[4] = 0; + out_6123861755706956553[5] = 0; + out_6123861755706956553[6] = 1; + out_6123861755706956553[7] = 0; + out_6123861755706956553[8] = 0; + out_6123861755706956553[9] = 1; + out_6123861755706956553[10] = 0; + out_6123861755706956553[11] = 0; + out_6123861755706956553[12] = 0; + out_6123861755706956553[13] = 0; + out_6123861755706956553[14] = 0; + out_6123861755706956553[15] = 0; + out_6123861755706956553[16] = 0; + out_6123861755706956553[17] = 0; + out_6123861755706956553[18] = 0; + out_6123861755706956553[19] = 0; + out_6123861755706956553[20] = 0; + out_6123861755706956553[21] = 0; + out_6123861755706956553[22] = 0; + out_6123861755706956553[23] = 0; + out_6123861755706956553[24] = 0; + out_6123861755706956553[25] = 1; + out_6123861755706956553[26] = 0; + out_6123861755706956553[27] = 0; + out_6123861755706956553[28] = 1; + out_6123861755706956553[29] = 0; + out_6123861755706956553[30] = 0; + out_6123861755706956553[31] = 0; + out_6123861755706956553[32] = 0; + out_6123861755706956553[33] = 0; + out_6123861755706956553[34] = 0; + out_6123861755706956553[35] = 0; + out_6123861755706956553[36] = 0; + out_6123861755706956553[37] = 0; + out_6123861755706956553[38] = 0; + out_6123861755706956553[39] = 0; + out_6123861755706956553[40] = 0; + out_6123861755706956553[41] = 0; + out_6123861755706956553[42] = 0; + out_6123861755706956553[43] = 0; + out_6123861755706956553[44] = 1; + out_6123861755706956553[45] = 0; + out_6123861755706956553[46] = 0; + out_6123861755706956553[47] = 1; + out_6123861755706956553[48] = 0; + out_6123861755706956553[49] = 0; + out_6123861755706956553[50] = 0; + out_6123861755706956553[51] = 0; + out_6123861755706956553[52] = 0; + out_6123861755706956553[53] = 0; } -void h_10(double *state, double *unused, double *out_7355867801776776353) { - out_7355867801776776353[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_7355867801776776353[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_7355867801776776353[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_5906261859060682989) { + out_5906261859060682989[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_5906261859060682989[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_5906261859060682989[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_6146846088544597929) { - out_6146846088544597929[0] = 0; - out_6146846088544597929[1] = 9.8100000000000005*cos(state[1]); - out_6146846088544597929[2] = 0; - out_6146846088544597929[3] = 0; - out_6146846088544597929[4] = -state[8]; - out_6146846088544597929[5] = state[7]; - out_6146846088544597929[6] = 0; - out_6146846088544597929[7] = state[5]; - out_6146846088544597929[8] = -state[4]; - out_6146846088544597929[9] = 0; - out_6146846088544597929[10] = 0; - out_6146846088544597929[11] = 0; - out_6146846088544597929[12] = 1; - out_6146846088544597929[13] = 0; - out_6146846088544597929[14] = 0; - out_6146846088544597929[15] = 1; - out_6146846088544597929[16] = 0; - out_6146846088544597929[17] = 0; - out_6146846088544597929[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_6146846088544597929[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_6146846088544597929[20] = 0; - out_6146846088544597929[21] = state[8]; - out_6146846088544597929[22] = 0; - out_6146846088544597929[23] = -state[6]; - out_6146846088544597929[24] = -state[5]; - out_6146846088544597929[25] = 0; - out_6146846088544597929[26] = state[3]; - out_6146846088544597929[27] = 0; - out_6146846088544597929[28] = 0; - out_6146846088544597929[29] = 0; - out_6146846088544597929[30] = 0; - out_6146846088544597929[31] = 1; - out_6146846088544597929[32] = 0; - out_6146846088544597929[33] = 0; - out_6146846088544597929[34] = 1; - out_6146846088544597929[35] = 0; - out_6146846088544597929[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_6146846088544597929[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_6146846088544597929[38] = 0; - out_6146846088544597929[39] = -state[7]; - out_6146846088544597929[40] = state[6]; - out_6146846088544597929[41] = 0; - out_6146846088544597929[42] = state[4]; - out_6146846088544597929[43] = -state[3]; - out_6146846088544597929[44] = 0; - out_6146846088544597929[45] = 0; - out_6146846088544597929[46] = 0; - out_6146846088544597929[47] = 0; - out_6146846088544597929[48] = 0; - out_6146846088544597929[49] = 0; - out_6146846088544597929[50] = 1; - out_6146846088544597929[51] = 0; - out_6146846088544597929[52] = 0; - out_6146846088544597929[53] = 1; +void H_10(double *state, double *unused, double *out_8717187080084033317) { + out_8717187080084033317[0] = 0; + out_8717187080084033317[1] = 9.8100000000000005*cos(state[1]); + out_8717187080084033317[2] = 0; + out_8717187080084033317[3] = 0; + out_8717187080084033317[4] = -state[8]; + out_8717187080084033317[5] = state[7]; + out_8717187080084033317[6] = 0; + out_8717187080084033317[7] = state[5]; + out_8717187080084033317[8] = -state[4]; + out_8717187080084033317[9] = 0; + out_8717187080084033317[10] = 0; + out_8717187080084033317[11] = 0; + out_8717187080084033317[12] = 1; + out_8717187080084033317[13] = 0; + out_8717187080084033317[14] = 0; + out_8717187080084033317[15] = 1; + out_8717187080084033317[16] = 0; + out_8717187080084033317[17] = 0; + out_8717187080084033317[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_8717187080084033317[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_8717187080084033317[20] = 0; + out_8717187080084033317[21] = state[8]; + out_8717187080084033317[22] = 0; + out_8717187080084033317[23] = -state[6]; + out_8717187080084033317[24] = -state[5]; + out_8717187080084033317[25] = 0; + out_8717187080084033317[26] = state[3]; + out_8717187080084033317[27] = 0; + out_8717187080084033317[28] = 0; + out_8717187080084033317[29] = 0; + out_8717187080084033317[30] = 0; + out_8717187080084033317[31] = 1; + out_8717187080084033317[32] = 0; + out_8717187080084033317[33] = 0; + out_8717187080084033317[34] = 1; + out_8717187080084033317[35] = 0; + out_8717187080084033317[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_8717187080084033317[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_8717187080084033317[38] = 0; + out_8717187080084033317[39] = -state[7]; + out_8717187080084033317[40] = state[6]; + out_8717187080084033317[41] = 0; + out_8717187080084033317[42] = state[4]; + out_8717187080084033317[43] = -state[3]; + out_8717187080084033317[44] = 0; + out_8717187080084033317[45] = 0; + out_8717187080084033317[46] = 0; + out_8717187080084033317[47] = 0; + out_8717187080084033317[48] = 0; + out_8717187080084033317[49] = 0; + out_8717187080084033317[50] = 1; + out_8717187080084033317[51] = 0; + out_8717187080084033317[52] = 0; + out_8717187080084033317[53] = 1; } -void h_13(double *state, double *unused, double *out_2344100091734301077) { - out_2344100091734301077[0] = state[3]; - out_2344100091734301077[1] = state[4]; - out_2344100091734301077[2] = state[5]; +void h_13(double *state, double *unused, double *out_5437403947989237748) { + out_5437403947989237748[0] = state[3]; + out_5437403947989237748[1] = state[4]; + out_5437403947989237748[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3909380197256637655) { - out_3909380197256637655[0] = 0; - out_3909380197256637655[1] = 0; - out_3909380197256637655[2] = 0; - out_3909380197256637655[3] = 1; - out_3909380197256637655[4] = 0; - out_3909380197256637655[5] = 0; - out_3909380197256637655[6] = 0; - out_3909380197256637655[7] = 0; - out_3909380197256637655[8] = 0; - out_3909380197256637655[9] = 0; - out_3909380197256637655[10] = 0; - out_3909380197256637655[11] = 0; - out_3909380197256637655[12] = 0; - out_3909380197256637655[13] = 0; - out_3909380197256637655[14] = 0; - out_3909380197256637655[15] = 0; - out_3909380197256637655[16] = 0; - out_3909380197256637655[17] = 0; - out_3909380197256637655[18] = 0; - out_3909380197256637655[19] = 0; - out_3909380197256637655[20] = 0; - out_3909380197256637655[21] = 0; - out_3909380197256637655[22] = 1; - out_3909380197256637655[23] = 0; - out_3909380197256637655[24] = 0; - out_3909380197256637655[25] = 0; - out_3909380197256637655[26] = 0; - out_3909380197256637655[27] = 0; - out_3909380197256637655[28] = 0; - out_3909380197256637655[29] = 0; - out_3909380197256637655[30] = 0; - out_3909380197256637655[31] = 0; - out_3909380197256637655[32] = 0; - out_3909380197256637655[33] = 0; - out_3909380197256637655[34] = 0; - out_3909380197256637655[35] = 0; - out_3909380197256637655[36] = 0; - out_3909380197256637655[37] = 0; - out_3909380197256637655[38] = 0; - out_3909380197256637655[39] = 0; - out_3909380197256637655[40] = 0; - out_3909380197256637655[41] = 1; - out_3909380197256637655[42] = 0; - out_3909380197256637655[43] = 0; - out_3909380197256637655[44] = 0; - out_3909380197256637655[45] = 0; - out_3909380197256637655[46] = 0; - out_3909380197256637655[47] = 0; - out_3909380197256637655[48] = 0; - out_3909380197256637655[49] = 0; - out_3909380197256637655[50] = 0; - out_3909380197256637655[51] = 0; - out_3909380197256637655[52] = 0; - out_3909380197256637655[53] = 0; +void H_13(double *state, double *unused, double *out_8489126854700071039) { + out_8489126854700071039[0] = 0; + out_8489126854700071039[1] = 0; + out_8489126854700071039[2] = 0; + out_8489126854700071039[3] = 1; + out_8489126854700071039[4] = 0; + out_8489126854700071039[5] = 0; + out_8489126854700071039[6] = 0; + out_8489126854700071039[7] = 0; + out_8489126854700071039[8] = 0; + out_8489126854700071039[9] = 0; + out_8489126854700071039[10] = 0; + out_8489126854700071039[11] = 0; + out_8489126854700071039[12] = 0; + out_8489126854700071039[13] = 0; + out_8489126854700071039[14] = 0; + out_8489126854700071039[15] = 0; + out_8489126854700071039[16] = 0; + out_8489126854700071039[17] = 0; + out_8489126854700071039[18] = 0; + out_8489126854700071039[19] = 0; + out_8489126854700071039[20] = 0; + out_8489126854700071039[21] = 0; + out_8489126854700071039[22] = 1; + out_8489126854700071039[23] = 0; + out_8489126854700071039[24] = 0; + out_8489126854700071039[25] = 0; + out_8489126854700071039[26] = 0; + out_8489126854700071039[27] = 0; + out_8489126854700071039[28] = 0; + out_8489126854700071039[29] = 0; + out_8489126854700071039[30] = 0; + out_8489126854700071039[31] = 0; + out_8489126854700071039[32] = 0; + out_8489126854700071039[33] = 0; + out_8489126854700071039[34] = 0; + out_8489126854700071039[35] = 0; + out_8489126854700071039[36] = 0; + out_8489126854700071039[37] = 0; + out_8489126854700071039[38] = 0; + out_8489126854700071039[39] = 0; + out_8489126854700071039[40] = 0; + out_8489126854700071039[41] = 1; + out_8489126854700071039[42] = 0; + out_8489126854700071039[43] = 0; + out_8489126854700071039[44] = 0; + out_8489126854700071039[45] = 0; + out_8489126854700071039[46] = 0; + out_8489126854700071039[47] = 0; + out_8489126854700071039[48] = 0; + out_8489126854700071039[49] = 0; + out_8489126854700071039[50] = 0; + out_8489126854700071039[51] = 0; + out_8489126854700071039[52] = 0; + out_8489126854700071039[53] = 0; } -void h_14(double *state, double *unused, double *out_589344124113930337) { - out_589344124113930337[0] = state[6]; - out_589344124113930337[1] = state[7]; - out_589344124113930337[2] = state[8]; +void h_14(double *state, double *unused, double *out_2973157686650314108) { + out_2973157686650314108[0] = state[6]; + out_2973157686650314108[1] = state[7]; + out_2973157686650314108[2] = state[8]; } -void H_14(double *state, double *unused, double *out_3158413166249485927) { - out_3158413166249485927[0] = 0; - out_3158413166249485927[1] = 0; - out_3158413166249485927[2] = 0; - out_3158413166249485927[3] = 0; - out_3158413166249485927[4] = 0; - out_3158413166249485927[5] = 0; - out_3158413166249485927[6] = 1; - out_3158413166249485927[7] = 0; - out_3158413166249485927[8] = 0; - out_3158413166249485927[9] = 0; - out_3158413166249485927[10] = 0; - out_3158413166249485927[11] = 0; - out_3158413166249485927[12] = 0; - out_3158413166249485927[13] = 0; - out_3158413166249485927[14] = 0; - out_3158413166249485927[15] = 0; - out_3158413166249485927[16] = 0; - out_3158413166249485927[17] = 0; - out_3158413166249485927[18] = 0; - out_3158413166249485927[19] = 0; - out_3158413166249485927[20] = 0; - out_3158413166249485927[21] = 0; - out_3158413166249485927[22] = 0; - out_3158413166249485927[23] = 0; - out_3158413166249485927[24] = 0; - out_3158413166249485927[25] = 1; - out_3158413166249485927[26] = 0; - out_3158413166249485927[27] = 0; - out_3158413166249485927[28] = 0; - out_3158413166249485927[29] = 0; - out_3158413166249485927[30] = 0; - out_3158413166249485927[31] = 0; - out_3158413166249485927[32] = 0; - out_3158413166249485927[33] = 0; - out_3158413166249485927[34] = 0; - out_3158413166249485927[35] = 0; - out_3158413166249485927[36] = 0; - out_3158413166249485927[37] = 0; - out_3158413166249485927[38] = 0; - out_3158413166249485927[39] = 0; - out_3158413166249485927[40] = 0; - out_3158413166249485927[41] = 0; - out_3158413166249485927[42] = 0; - out_3158413166249485927[43] = 0; - out_3158413166249485927[44] = 1; - out_3158413166249485927[45] = 0; - out_3158413166249485927[46] = 0; - out_3158413166249485927[47] = 0; - out_3158413166249485927[48] = 0; - out_3158413166249485927[49] = 0; - out_3158413166249485927[50] = 0; - out_3158413166249485927[51] = 0; - out_3158413166249485927[52] = 0; - out_3158413166249485927[53] = 0; +void H_14(double *state, double *unused, double *out_9206650188002328849) { + out_9206650188002328849[0] = 0; + out_9206650188002328849[1] = 0; + out_9206650188002328849[2] = 0; + out_9206650188002328849[3] = 0; + out_9206650188002328849[4] = 0; + out_9206650188002328849[5] = 0; + out_9206650188002328849[6] = 1; + out_9206650188002328849[7] = 0; + out_9206650188002328849[8] = 0; + out_9206650188002328849[9] = 0; + out_9206650188002328849[10] = 0; + out_9206650188002328849[11] = 0; + out_9206650188002328849[12] = 0; + out_9206650188002328849[13] = 0; + out_9206650188002328849[14] = 0; + out_9206650188002328849[15] = 0; + out_9206650188002328849[16] = 0; + out_9206650188002328849[17] = 0; + out_9206650188002328849[18] = 0; + out_9206650188002328849[19] = 0; + out_9206650188002328849[20] = 0; + out_9206650188002328849[21] = 0; + out_9206650188002328849[22] = 0; + out_9206650188002328849[23] = 0; + out_9206650188002328849[24] = 0; + out_9206650188002328849[25] = 1; + out_9206650188002328849[26] = 0; + out_9206650188002328849[27] = 0; + out_9206650188002328849[28] = 0; + out_9206650188002328849[29] = 0; + out_9206650188002328849[30] = 0; + out_9206650188002328849[31] = 0; + out_9206650188002328849[32] = 0; + out_9206650188002328849[33] = 0; + out_9206650188002328849[34] = 0; + out_9206650188002328849[35] = 0; + out_9206650188002328849[36] = 0; + out_9206650188002328849[37] = 0; + out_9206650188002328849[38] = 0; + out_9206650188002328849[39] = 0; + out_9206650188002328849[40] = 0; + out_9206650188002328849[41] = 0; + out_9206650188002328849[42] = 0; + out_9206650188002328849[43] = 0; + out_9206650188002328849[44] = 1; + out_9206650188002328849[45] = 0; + out_9206650188002328849[46] = 0; + out_9206650188002328849[47] = 0; + out_9206650188002328849[48] = 0; + out_9206650188002328849[49] = 0; + out_9206650188002328849[50] = 0; + out_9206650188002328849[51] = 0; + out_9206650188002328849[52] = 0; + out_9206650188002328849[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_8550489212526755506) { - err_fun(nom_x, delta_x, out_8550489212526755506); +void pose_err_fun(double *nom_x, double *delta_x, double *out_937271495952840532) { + err_fun(nom_x, delta_x, out_937271495952840532); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2197167056715094306) { - inv_err_fun(nom_x, true_x, out_2197167056715094306); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_712402659692030222) { + inv_err_fun(nom_x, true_x, out_712402659692030222); } -void pose_H_mod_fun(double *state, double *out_1273572215837011735) { - H_mod_fun(state, out_1273572215837011735); +void pose_H_mod_fun(double *state, double *out_4304163029800659692) { + H_mod_fun(state, out_4304163029800659692); } -void pose_f_fun(double *state, double dt, double *out_58843541046312828) { - f_fun(state, dt, out_58843541046312828); +void pose_f_fun(double *state, double dt, double *out_7809694961382882469) { + f_fun(state, dt, out_7809694961382882469); } -void pose_F_fun(double *state, double dt, double *out_4952539587807327652) { - F_fun(state, dt, out_4952539587807327652); +void pose_F_fun(double *state, double dt, double *out_4402047608279739822) { + F_fun(state, dt, out_4402047608279739822); } -void pose_h_4(double *state, double *unused, double *out_105029621528743882) { - h_4(state, unused, out_105029621528743882); +void pose_h_4(double *state, double *unused, double *out_1761018668225147827) { + h_4(state, unused, out_1761018668225147827); } -void pose_H_4(double *state, double *unused, double *out_7121654022588970456) { - H_4(state, unused, out_7121654022588970456); +void pose_H_4(double *state, double *unused, double *out_6123861755706956553) { + H_4(state, unused, out_6123861755706956553); } -void pose_h_10(double *state, double *unused, double *out_7355867801776776353) { - h_10(state, unused, out_7355867801776776353); +void pose_h_10(double *state, double *unused, double *out_5906261859060682989) { + h_10(state, unused, out_5906261859060682989); } -void pose_H_10(double *state, double *unused, double *out_6146846088544597929) { - H_10(state, unused, out_6146846088544597929); +void pose_H_10(double *state, double *unused, double *out_8717187080084033317) { + H_10(state, unused, out_8717187080084033317); } -void pose_h_13(double *state, double *unused, double *out_2344100091734301077) { - h_13(state, unused, out_2344100091734301077); +void pose_h_13(double *state, double *unused, double *out_5437403947989237748) { + h_13(state, unused, out_5437403947989237748); } -void pose_H_13(double *state, double *unused, double *out_3909380197256637655) { - H_13(state, unused, out_3909380197256637655); +void pose_H_13(double *state, double *unused, double *out_8489126854700071039) { + H_13(state, unused, out_8489126854700071039); } -void pose_h_14(double *state, double *unused, double *out_589344124113930337) { - h_14(state, unused, out_589344124113930337); +void pose_h_14(double *state, double *unused, double *out_2973157686650314108) { + h_14(state, unused, out_2973157686650314108); } -void pose_H_14(double *state, double *unused, double *out_3158413166249485927) { - H_14(state, unused, out_3158413166249485927); +void pose_H_14(double *state, double *unused, double *out_9206650188002328849) { + H_14(state, unused, out_9206650188002328849); } 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 2ec0f34a5..d5e9211c4 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_8550489212526755506); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2197167056715094306); -void pose_H_mod_fun(double *state, double *out_1273572215837011735); -void pose_f_fun(double *state, double dt, double *out_58843541046312828); -void pose_F_fun(double *state, double dt, double *out_4952539587807327652); -void pose_h_4(double *state, double *unused, double *out_105029621528743882); -void pose_H_4(double *state, double *unused, double *out_7121654022588970456); -void pose_h_10(double *state, double *unused, double *out_7355867801776776353); -void pose_H_10(double *state, double *unused, double *out_6146846088544597929); -void pose_h_13(double *state, double *unused, double *out_2344100091734301077); -void pose_H_13(double *state, double *unused, double *out_3909380197256637655); -void pose_h_14(double *state, double *unused, double *out_589344124113930337); -void pose_H_14(double *state, double *unused, double *out_3158413166249485927); +void pose_err_fun(double *nom_x, double *delta_x, double *out_937271495952840532); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_712402659692030222); +void pose_H_mod_fun(double *state, double *out_4304163029800659692); +void pose_f_fun(double *state, double dt, double *out_7809694961382882469); +void pose_F_fun(double *state, double dt, double *out_4402047608279739822); +void pose_h_4(double *state, double *unused, double *out_1761018668225147827); +void pose_H_4(double *state, double *unused, double *out_6123861755706956553); +void pose_h_10(double *state, double *unused, double *out_5906261859060682989); +void pose_H_10(double *state, double *unused, double *out_8717187080084033317); +void pose_h_13(double *state, double *unused, double *out_5437403947989237748); +void pose_H_13(double *state, double *unused, double *out_8489126854700071039); +void pose_h_14(double *state, double *unused, double *out_2973157686650314108); +void pose_H_14(double *state, double *unused, double *out_9206650188002328849); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index aaf633288..8013f1a67 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 4e383def0..d383bbb6c 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 6af45e0a0..d4e4802fe 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 228ab0b16..370c190ad 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index c4720d2de..00c9f4c98 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index a50a8ca1a..f47b3908f 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ