diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index ca37b2f7e..321a0fd09 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 2cb052921..8e1f2c5d5 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 2346a69c4..c07122e3c 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 740be5291..01eb6641b 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 0dcb72e5d..3b72fc8fb 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 bbdab3d90..cfda3ba62 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 2e7abf19a..871c1760b 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 2e7abf19a..871c1760b 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 d67210bd8..f3eefda40 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 bbdab3d90..cfda3ba62 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 e6de3dcfb..d8749877f 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-f06c82b2-DEBUG"; +const uint8_t gitversion[19] = "DEV-b12fdf18-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 10b6b5621..884a8870a 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 588fdde8c..a2711c732 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 8a0d03426..be05939cf 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 dfd62a550..935a91d2a 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 3787b1934..d1c03cdf6 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 e524f33d4..c403e9008 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 9d166adb9..7d7d972d1 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 e58c580ac..bc7795b1f 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 130234dd4..0052f859b 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 9c98a0b5f..0268aab02 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 731997681..adfa16193 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 44ae680fc..3723da971 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 42207f1a2..e7880d8c7 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 4d0c0d58f..b6da44fca 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 4b78eaf50..da3ec965d 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 5240b9da6..5be60edf7 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 17540bacc..058846831 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 5ecc83522..a0eaf6da3 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 5529997d7..2ae6647fd 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 a26636ade..f465f882d 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 61eedf377..cf25e96d9 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-f06c82b2-DEBUG \ No newline at end of file +DEV-b12fdf18-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 56108aa8b..e48f64f80 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_4387195160971747702) { - out_4387195160971747702[0] = delta_x[0] + nom_x[0]; - out_4387195160971747702[1] = delta_x[1] + nom_x[1]; - out_4387195160971747702[2] = delta_x[2] + nom_x[2]; - out_4387195160971747702[3] = delta_x[3] + nom_x[3]; - out_4387195160971747702[4] = delta_x[4] + nom_x[4]; - out_4387195160971747702[5] = delta_x[5] + nom_x[5]; - out_4387195160971747702[6] = delta_x[6] + nom_x[6]; - out_4387195160971747702[7] = delta_x[7] + nom_x[7]; - out_4387195160971747702[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1972702472511725479) { + out_1972702472511725479[0] = delta_x[0] + nom_x[0]; + out_1972702472511725479[1] = delta_x[1] + nom_x[1]; + out_1972702472511725479[2] = delta_x[2] + nom_x[2]; + out_1972702472511725479[3] = delta_x[3] + nom_x[3]; + out_1972702472511725479[4] = delta_x[4] + nom_x[4]; + out_1972702472511725479[5] = delta_x[5] + nom_x[5]; + out_1972702472511725479[6] = delta_x[6] + nom_x[6]; + out_1972702472511725479[7] = delta_x[7] + nom_x[7]; + out_1972702472511725479[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7707264064834051115) { - out_7707264064834051115[0] = -nom_x[0] + true_x[0]; - out_7707264064834051115[1] = -nom_x[1] + true_x[1]; - out_7707264064834051115[2] = -nom_x[2] + true_x[2]; - out_7707264064834051115[3] = -nom_x[3] + true_x[3]; - out_7707264064834051115[4] = -nom_x[4] + true_x[4]; - out_7707264064834051115[5] = -nom_x[5] + true_x[5]; - out_7707264064834051115[6] = -nom_x[6] + true_x[6]; - out_7707264064834051115[7] = -nom_x[7] + true_x[7]; - out_7707264064834051115[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3741555659818717473) { + out_3741555659818717473[0] = -nom_x[0] + true_x[0]; + out_3741555659818717473[1] = -nom_x[1] + true_x[1]; + out_3741555659818717473[2] = -nom_x[2] + true_x[2]; + out_3741555659818717473[3] = -nom_x[3] + true_x[3]; + out_3741555659818717473[4] = -nom_x[4] + true_x[4]; + out_3741555659818717473[5] = -nom_x[5] + true_x[5]; + out_3741555659818717473[6] = -nom_x[6] + true_x[6]; + out_3741555659818717473[7] = -nom_x[7] + true_x[7]; + out_3741555659818717473[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_7178605602671138983) { - out_7178605602671138983[0] = 1.0; - out_7178605602671138983[1] = 0.0; - out_7178605602671138983[2] = 0.0; - out_7178605602671138983[3] = 0.0; - out_7178605602671138983[4] = 0.0; - out_7178605602671138983[5] = 0.0; - out_7178605602671138983[6] = 0.0; - out_7178605602671138983[7] = 0.0; - out_7178605602671138983[8] = 0.0; - out_7178605602671138983[9] = 0.0; - out_7178605602671138983[10] = 1.0; - out_7178605602671138983[11] = 0.0; - out_7178605602671138983[12] = 0.0; - out_7178605602671138983[13] = 0.0; - out_7178605602671138983[14] = 0.0; - out_7178605602671138983[15] = 0.0; - out_7178605602671138983[16] = 0.0; - out_7178605602671138983[17] = 0.0; - out_7178605602671138983[18] = 0.0; - out_7178605602671138983[19] = 0.0; - out_7178605602671138983[20] = 1.0; - out_7178605602671138983[21] = 0.0; - out_7178605602671138983[22] = 0.0; - out_7178605602671138983[23] = 0.0; - out_7178605602671138983[24] = 0.0; - out_7178605602671138983[25] = 0.0; - out_7178605602671138983[26] = 0.0; - out_7178605602671138983[27] = 0.0; - out_7178605602671138983[28] = 0.0; - out_7178605602671138983[29] = 0.0; - out_7178605602671138983[30] = 1.0; - out_7178605602671138983[31] = 0.0; - out_7178605602671138983[32] = 0.0; - out_7178605602671138983[33] = 0.0; - out_7178605602671138983[34] = 0.0; - out_7178605602671138983[35] = 0.0; - out_7178605602671138983[36] = 0.0; - out_7178605602671138983[37] = 0.0; - out_7178605602671138983[38] = 0.0; - out_7178605602671138983[39] = 0.0; - out_7178605602671138983[40] = 1.0; - out_7178605602671138983[41] = 0.0; - out_7178605602671138983[42] = 0.0; - out_7178605602671138983[43] = 0.0; - out_7178605602671138983[44] = 0.0; - out_7178605602671138983[45] = 0.0; - out_7178605602671138983[46] = 0.0; - out_7178605602671138983[47] = 0.0; - out_7178605602671138983[48] = 0.0; - out_7178605602671138983[49] = 0.0; - out_7178605602671138983[50] = 1.0; - out_7178605602671138983[51] = 0.0; - out_7178605602671138983[52] = 0.0; - out_7178605602671138983[53] = 0.0; - out_7178605602671138983[54] = 0.0; - out_7178605602671138983[55] = 0.0; - out_7178605602671138983[56] = 0.0; - out_7178605602671138983[57] = 0.0; - out_7178605602671138983[58] = 0.0; - out_7178605602671138983[59] = 0.0; - out_7178605602671138983[60] = 1.0; - out_7178605602671138983[61] = 0.0; - out_7178605602671138983[62] = 0.0; - out_7178605602671138983[63] = 0.0; - out_7178605602671138983[64] = 0.0; - out_7178605602671138983[65] = 0.0; - out_7178605602671138983[66] = 0.0; - out_7178605602671138983[67] = 0.0; - out_7178605602671138983[68] = 0.0; - out_7178605602671138983[69] = 0.0; - out_7178605602671138983[70] = 1.0; - out_7178605602671138983[71] = 0.0; - out_7178605602671138983[72] = 0.0; - out_7178605602671138983[73] = 0.0; - out_7178605602671138983[74] = 0.0; - out_7178605602671138983[75] = 0.0; - out_7178605602671138983[76] = 0.0; - out_7178605602671138983[77] = 0.0; - out_7178605602671138983[78] = 0.0; - out_7178605602671138983[79] = 0.0; - out_7178605602671138983[80] = 1.0; +void H_mod_fun(double *state, double *out_4845788339544047954) { + out_4845788339544047954[0] = 1.0; + out_4845788339544047954[1] = 0.0; + out_4845788339544047954[2] = 0.0; + out_4845788339544047954[3] = 0.0; + out_4845788339544047954[4] = 0.0; + out_4845788339544047954[5] = 0.0; + out_4845788339544047954[6] = 0.0; + out_4845788339544047954[7] = 0.0; + out_4845788339544047954[8] = 0.0; + out_4845788339544047954[9] = 0.0; + out_4845788339544047954[10] = 1.0; + out_4845788339544047954[11] = 0.0; + out_4845788339544047954[12] = 0.0; + out_4845788339544047954[13] = 0.0; + out_4845788339544047954[14] = 0.0; + out_4845788339544047954[15] = 0.0; + out_4845788339544047954[16] = 0.0; + out_4845788339544047954[17] = 0.0; + out_4845788339544047954[18] = 0.0; + out_4845788339544047954[19] = 0.0; + out_4845788339544047954[20] = 1.0; + out_4845788339544047954[21] = 0.0; + out_4845788339544047954[22] = 0.0; + out_4845788339544047954[23] = 0.0; + out_4845788339544047954[24] = 0.0; + out_4845788339544047954[25] = 0.0; + out_4845788339544047954[26] = 0.0; + out_4845788339544047954[27] = 0.0; + out_4845788339544047954[28] = 0.0; + out_4845788339544047954[29] = 0.0; + out_4845788339544047954[30] = 1.0; + out_4845788339544047954[31] = 0.0; + out_4845788339544047954[32] = 0.0; + out_4845788339544047954[33] = 0.0; + out_4845788339544047954[34] = 0.0; + out_4845788339544047954[35] = 0.0; + out_4845788339544047954[36] = 0.0; + out_4845788339544047954[37] = 0.0; + out_4845788339544047954[38] = 0.0; + out_4845788339544047954[39] = 0.0; + out_4845788339544047954[40] = 1.0; + out_4845788339544047954[41] = 0.0; + out_4845788339544047954[42] = 0.0; + out_4845788339544047954[43] = 0.0; + out_4845788339544047954[44] = 0.0; + out_4845788339544047954[45] = 0.0; + out_4845788339544047954[46] = 0.0; + out_4845788339544047954[47] = 0.0; + out_4845788339544047954[48] = 0.0; + out_4845788339544047954[49] = 0.0; + out_4845788339544047954[50] = 1.0; + out_4845788339544047954[51] = 0.0; + out_4845788339544047954[52] = 0.0; + out_4845788339544047954[53] = 0.0; + out_4845788339544047954[54] = 0.0; + out_4845788339544047954[55] = 0.0; + out_4845788339544047954[56] = 0.0; + out_4845788339544047954[57] = 0.0; + out_4845788339544047954[58] = 0.0; + out_4845788339544047954[59] = 0.0; + out_4845788339544047954[60] = 1.0; + out_4845788339544047954[61] = 0.0; + out_4845788339544047954[62] = 0.0; + out_4845788339544047954[63] = 0.0; + out_4845788339544047954[64] = 0.0; + out_4845788339544047954[65] = 0.0; + out_4845788339544047954[66] = 0.0; + out_4845788339544047954[67] = 0.0; + out_4845788339544047954[68] = 0.0; + out_4845788339544047954[69] = 0.0; + out_4845788339544047954[70] = 1.0; + out_4845788339544047954[71] = 0.0; + out_4845788339544047954[72] = 0.0; + out_4845788339544047954[73] = 0.0; + out_4845788339544047954[74] = 0.0; + out_4845788339544047954[75] = 0.0; + out_4845788339544047954[76] = 0.0; + out_4845788339544047954[77] = 0.0; + out_4845788339544047954[78] = 0.0; + out_4845788339544047954[79] = 0.0; + out_4845788339544047954[80] = 1.0; } -void f_fun(double *state, double dt, double *out_2416599425795193412) { - out_2416599425795193412[0] = state[0]; - out_2416599425795193412[1] = state[1]; - out_2416599425795193412[2] = state[2]; - out_2416599425795193412[3] = state[3]; - out_2416599425795193412[4] = state[4]; - out_2416599425795193412[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_2416599425795193412[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_2416599425795193412[7] = state[7]; - out_2416599425795193412[8] = state[8]; +void f_fun(double *state, double dt, double *out_4185306989729419910) { + out_4185306989729419910[0] = state[0]; + out_4185306989729419910[1] = state[1]; + out_4185306989729419910[2] = state[2]; + out_4185306989729419910[3] = state[3]; + out_4185306989729419910[4] = state[4]; + out_4185306989729419910[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_4185306989729419910[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_4185306989729419910[7] = state[7]; + out_4185306989729419910[8] = state[8]; } -void F_fun(double *state, double dt, double *out_6761986232654706930) { - out_6761986232654706930[0] = 1; - out_6761986232654706930[1] = 0; - out_6761986232654706930[2] = 0; - out_6761986232654706930[3] = 0; - out_6761986232654706930[4] = 0; - out_6761986232654706930[5] = 0; - out_6761986232654706930[6] = 0; - out_6761986232654706930[7] = 0; - out_6761986232654706930[8] = 0; - out_6761986232654706930[9] = 0; - out_6761986232654706930[10] = 1; - out_6761986232654706930[11] = 0; - out_6761986232654706930[12] = 0; - out_6761986232654706930[13] = 0; - out_6761986232654706930[14] = 0; - out_6761986232654706930[15] = 0; - out_6761986232654706930[16] = 0; - out_6761986232654706930[17] = 0; - out_6761986232654706930[18] = 0; - out_6761986232654706930[19] = 0; - out_6761986232654706930[20] = 1; - out_6761986232654706930[21] = 0; - out_6761986232654706930[22] = 0; - out_6761986232654706930[23] = 0; - out_6761986232654706930[24] = 0; - out_6761986232654706930[25] = 0; - out_6761986232654706930[26] = 0; - out_6761986232654706930[27] = 0; - out_6761986232654706930[28] = 0; - out_6761986232654706930[29] = 0; - out_6761986232654706930[30] = 1; - out_6761986232654706930[31] = 0; - out_6761986232654706930[32] = 0; - out_6761986232654706930[33] = 0; - out_6761986232654706930[34] = 0; - out_6761986232654706930[35] = 0; - out_6761986232654706930[36] = 0; - out_6761986232654706930[37] = 0; - out_6761986232654706930[38] = 0; - out_6761986232654706930[39] = 0; - out_6761986232654706930[40] = 1; - out_6761986232654706930[41] = 0; - out_6761986232654706930[42] = 0; - out_6761986232654706930[43] = 0; - out_6761986232654706930[44] = 0; - out_6761986232654706930[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_6761986232654706930[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_6761986232654706930[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6761986232654706930[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6761986232654706930[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_6761986232654706930[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_6761986232654706930[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_6761986232654706930[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_6761986232654706930[53] = -9.8100000000000005*dt; - out_6761986232654706930[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_6761986232654706930[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_6761986232654706930[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6761986232654706930[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6761986232654706930[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_6761986232654706930[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_6761986232654706930[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_6761986232654706930[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6761986232654706930[62] = 0; - out_6761986232654706930[63] = 0; - out_6761986232654706930[64] = 0; - out_6761986232654706930[65] = 0; - out_6761986232654706930[66] = 0; - out_6761986232654706930[67] = 0; - out_6761986232654706930[68] = 0; - out_6761986232654706930[69] = 0; - out_6761986232654706930[70] = 1; - out_6761986232654706930[71] = 0; - out_6761986232654706930[72] = 0; - out_6761986232654706930[73] = 0; - out_6761986232654706930[74] = 0; - out_6761986232654706930[75] = 0; - out_6761986232654706930[76] = 0; - out_6761986232654706930[77] = 0; - out_6761986232654706930[78] = 0; - out_6761986232654706930[79] = 0; - out_6761986232654706930[80] = 1; +void F_fun(double *state, double dt, double *out_8252178874365212967) { + out_8252178874365212967[0] = 1; + out_8252178874365212967[1] = 0; + out_8252178874365212967[2] = 0; + out_8252178874365212967[3] = 0; + out_8252178874365212967[4] = 0; + out_8252178874365212967[5] = 0; + out_8252178874365212967[6] = 0; + out_8252178874365212967[7] = 0; + out_8252178874365212967[8] = 0; + out_8252178874365212967[9] = 0; + out_8252178874365212967[10] = 1; + out_8252178874365212967[11] = 0; + out_8252178874365212967[12] = 0; + out_8252178874365212967[13] = 0; + out_8252178874365212967[14] = 0; + out_8252178874365212967[15] = 0; + out_8252178874365212967[16] = 0; + out_8252178874365212967[17] = 0; + out_8252178874365212967[18] = 0; + out_8252178874365212967[19] = 0; + out_8252178874365212967[20] = 1; + out_8252178874365212967[21] = 0; + out_8252178874365212967[22] = 0; + out_8252178874365212967[23] = 0; + out_8252178874365212967[24] = 0; + out_8252178874365212967[25] = 0; + out_8252178874365212967[26] = 0; + out_8252178874365212967[27] = 0; + out_8252178874365212967[28] = 0; + out_8252178874365212967[29] = 0; + out_8252178874365212967[30] = 1; + out_8252178874365212967[31] = 0; + out_8252178874365212967[32] = 0; + out_8252178874365212967[33] = 0; + out_8252178874365212967[34] = 0; + out_8252178874365212967[35] = 0; + out_8252178874365212967[36] = 0; + out_8252178874365212967[37] = 0; + out_8252178874365212967[38] = 0; + out_8252178874365212967[39] = 0; + out_8252178874365212967[40] = 1; + out_8252178874365212967[41] = 0; + out_8252178874365212967[42] = 0; + out_8252178874365212967[43] = 0; + out_8252178874365212967[44] = 0; + out_8252178874365212967[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_8252178874365212967[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_8252178874365212967[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8252178874365212967[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8252178874365212967[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_8252178874365212967[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_8252178874365212967[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_8252178874365212967[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_8252178874365212967[53] = -9.8100000000000005*dt; + out_8252178874365212967[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_8252178874365212967[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_8252178874365212967[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8252178874365212967[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8252178874365212967[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_8252178874365212967[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_8252178874365212967[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_8252178874365212967[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8252178874365212967[62] = 0; + out_8252178874365212967[63] = 0; + out_8252178874365212967[64] = 0; + out_8252178874365212967[65] = 0; + out_8252178874365212967[66] = 0; + out_8252178874365212967[67] = 0; + out_8252178874365212967[68] = 0; + out_8252178874365212967[69] = 0; + out_8252178874365212967[70] = 1; + out_8252178874365212967[71] = 0; + out_8252178874365212967[72] = 0; + out_8252178874365212967[73] = 0; + out_8252178874365212967[74] = 0; + out_8252178874365212967[75] = 0; + out_8252178874365212967[76] = 0; + out_8252178874365212967[77] = 0; + out_8252178874365212967[78] = 0; + out_8252178874365212967[79] = 0; + out_8252178874365212967[80] = 1; } -void h_25(double *state, double *unused, double *out_319074315832850169) { - out_319074315832850169[0] = state[6]; +void h_25(double *state, double *unused, double *out_4739826667919645243) { + out_4739826667919645243[0] = state[6]; } -void H_25(double *state, double *unused, double *out_3655624952681647306) { - out_3655624952681647306[0] = 0; - out_3655624952681647306[1] = 0; - out_3655624952681647306[2] = 0; - out_3655624952681647306[3] = 0; - out_3655624952681647306[4] = 0; - out_3655624952681647306[5] = 0; - out_3655624952681647306[6] = 1; - out_3655624952681647306[7] = 0; - out_3655624952681647306[8] = 0; +void H_25(double *state, double *unused, double *out_2698629402266667542) { + out_2698629402266667542[0] = 0; + out_2698629402266667542[1] = 0; + out_2698629402266667542[2] = 0; + out_2698629402266667542[3] = 0; + out_2698629402266667542[4] = 0; + out_2698629402266667542[5] = 0; + out_2698629402266667542[6] = 1; + out_2698629402266667542[7] = 0; + out_2698629402266667542[8] = 0; } -void h_24(double *state, double *unused, double *out_5734853090591072423) { - out_5734853090591072423[0] = state[4]; - out_5734853090591072423[1] = state[5]; +void h_24(double *state, double *unused, double *out_6091047700113278923) { + out_6091047700113278923[0] = state[4]; + out_6091047700113278923[1] = state[5]; } -void H_24(double *state, double *unused, double *out_1429917168702778744) { - out_1429917168702778744[0] = 0; - out_1429917168702778744[1] = 0; - out_1429917168702778744[2] = 0; - out_1429917168702778744[3] = 0; - out_1429917168702778744[4] = 1; - out_1429917168702778744[5] = 0; - out_1429917168702778744[6] = 0; - out_1429917168702778744[7] = 0; - out_1429917168702778744[8] = 0; - out_1429917168702778744[9] = 0; - out_1429917168702778744[10] = 0; - out_1429917168702778744[11] = 0; - out_1429917168702778744[12] = 0; - out_1429917168702778744[13] = 0; - out_1429917168702778744[14] = 1; - out_1429917168702778744[15] = 0; - out_1429917168702778744[16] = 0; - out_1429917168702778744[17] = 0; +void H_24(double *state, double *unused, double *out_2121692102389320721) { + out_2121692102389320721[0] = 0; + out_2121692102389320721[1] = 0; + out_2121692102389320721[2] = 0; + out_2121692102389320721[3] = 0; + out_2121692102389320721[4] = 1; + out_2121692102389320721[5] = 0; + out_2121692102389320721[6] = 0; + out_2121692102389320721[7] = 0; + out_2121692102389320721[8] = 0; + out_2121692102389320721[9] = 0; + out_2121692102389320721[10] = 0; + out_2121692102389320721[11] = 0; + out_2121692102389320721[12] = 0; + out_2121692102389320721[13] = 0; + out_2121692102389320721[14] = 1; + out_2121692102389320721[15] = 0; + out_2121692102389320721[16] = 0; + out_2121692102389320721[17] = 0; } -void h_30(double *state, double *unused, double *out_8212013810717373528) { - out_8212013810717373528[0] = state[4]; +void h_30(double *state, double *unused, double *out_7687552658275130204) { + out_7687552658275130204[0] = state[4]; } -void H_30(double *state, double *unused, double *out_3784963899824887376) { - out_3784963899824887376[0] = 0; - out_3784963899824887376[1] = 0; - out_3784963899824887376[2] = 0; - out_3784963899824887376[3] = 0; - out_3784963899824887376[4] = 1; - out_3784963899824887376[5] = 0; - out_3784963899824887376[6] = 0; - out_3784963899824887376[7] = 0; - out_3784963899824887376[8] = 0; +void H_30(double *state, double *unused, double *out_2569290455123427472) { + out_2569290455123427472[0] = 0; + out_2569290455123427472[1] = 0; + out_2569290455123427472[2] = 0; + out_2569290455123427472[3] = 0; + out_2569290455123427472[4] = 1; + out_2569290455123427472[5] = 0; + out_2569290455123427472[6] = 0; + out_2569290455123427472[7] = 0; + out_2569290455123427472[8] = 0; } -void h_26(double *state, double *unused, double *out_5233056995331332443) { - out_5233056995331332443[0] = state[7]; +void h_26(double *state, double *unused, double *out_7946639312164568029) { + out_7946639312164568029[0] = state[7]; } -void H_26(double *state, double *unused, double *out_2998770888571335402) { - out_2998770888571335402[0] = 0; - out_2998770888571335402[1] = 0; - out_2998770888571335402[2] = 0; - out_2998770888571335402[3] = 0; - out_2998770888571335402[4] = 0; - out_2998770888571335402[5] = 0; - out_2998770888571335402[6] = 0; - out_2998770888571335402[7] = 1; - out_2998770888571335402[8] = 0; +void H_26(double *state, double *unused, double *out_1042873916607388682) { + out_1042873916607388682[0] = 0; + out_1042873916607388682[1] = 0; + out_1042873916607388682[2] = 0; + out_1042873916607388682[3] = 0; + out_1042873916607388682[4] = 0; + out_1042873916607388682[5] = 0; + out_1042873916607388682[6] = 0; + out_1042873916607388682[7] = 1; + out_1042873916607388682[8] = 0; } -void h_27(double *state, double *unused, double *out_3355860367277182854) { - out_3355860367277182854[0] = state[3]; +void h_27(double *state, double *unused, double *out_3665830151724581410) { + out_3665830151724581410[0] = state[3]; } -void H_27(double *state, double *unused, double *out_5959727211625312287) { - out_5959727211625312287[0] = 0; - out_5959727211625312287[1] = 0; - out_5959727211625312287[2] = 0; - out_5959727211625312287[3] = 1; - out_5959727211625312287[4] = 0; - out_5959727211625312287[5] = 0; - out_5959727211625312287[6] = 0; - out_5959727211625312287[7] = 0; - out_5959727211625312287[8] = 0; +void H_27(double *state, double *unused, double *out_394527143323002561) { + out_394527143323002561[0] = 0; + out_394527143323002561[1] = 0; + out_394527143323002561[2] = 0; + out_394527143323002561[3] = 1; + out_394527143323002561[4] = 0; + out_394527143323002561[5] = 0; + out_394527143323002561[6] = 0; + out_394527143323002561[7] = 0; + out_394527143323002561[8] = 0; } -void h_29(double *state, double *unused, double *out_7630324942183480437) { - out_7630324942183480437[0] = state[1]; +void h_29(double *state, double *unused, double *out_8290868542951607721) { + out_8290868542951607721[0] = state[1]; } -void H_29(double *state, double *unused, double *out_3274732555510495192) { - out_3274732555510495192[0] = 0; - out_3274732555510495192[1] = 1; - out_3274732555510495192[2] = 0; - out_3274732555510495192[3] = 0; - out_3274732555510495192[4] = 0; - out_3274732555510495192[5] = 0; - out_3274732555510495192[6] = 0; - out_3274732555510495192[7] = 0; - out_3274732555510495192[8] = 0; +void H_29(double *state, double *unused, double *out_3079521799437819656) { + out_3079521799437819656[0] = 0; + out_3079521799437819656[1] = 1; + out_3079521799437819656[2] = 0; + out_3079521799437819656[3] = 0; + out_3079521799437819656[4] = 0; + out_3079521799437819656[5] = 0; + out_3079521799437819656[6] = 0; + out_3079521799437819656[7] = 0; + out_3079521799437819656[8] = 0; } -void h_28(double *state, double *unused, double *out_4299370220178845289) { - out_4299370220178845289[0] = state[0]; +void h_28(double *state, double *unused, double *out_6846547824895893173) { + out_6846547824895893173[0] = state[0]; } -void H_28(double *state, double *unused, double *out_1311102283945168941) { - out_1311102283945168941[0] = 1; - out_1311102283945168941[1] = 0; - out_1311102283945168941[2] = 0; - out_1311102283945168941[3] = 0; - out_1311102283945168941[4] = 0; - out_1311102283945168941[5] = 0; - out_1311102283945168941[6] = 0; - out_1311102283945168941[7] = 0; - out_1311102283945168941[8] = 0; +void H_28(double *state, double *unused, double *out_2002877217631710918) { + out_2002877217631710918[0] = 1; + out_2002877217631710918[1] = 0; + out_2002877217631710918[2] = 0; + out_2002877217631710918[3] = 0; + out_2002877217631710918[4] = 0; + out_2002877217631710918[5] = 0; + out_2002877217631710918[6] = 0; + out_2002877217631710918[7] = 0; + out_2002877217631710918[8] = 0; } -void h_31(double *state, double *unused, double *out_594268378117356058) { - out_594268378117356058[0] = state[8]; +void h_31(double *state, double *unused, double *out_3141445982834403942) { + out_3141445982834403942[0] = state[8]; } -void H_31(double *state, double *unused, double *out_3624978990804686878) { - out_3624978990804686878[0] = 0; - out_3624978990804686878[1] = 0; - out_3624978990804686878[2] = 0; - out_3624978990804686878[3] = 0; - out_3624978990804686878[4] = 0; - out_3624978990804686878[5] = 0; - out_3624978990804686878[6] = 0; - out_3624978990804686878[7] = 0; - out_3624978990804686878[8] = 1; +void H_31(double *state, double *unused, double *out_2729275364143627970) { + out_2729275364143627970[0] = 0; + out_2729275364143627970[1] = 0; + out_2729275364143627970[2] = 0; + out_2729275364143627970[3] = 0; + out_2729275364143627970[4] = 0; + out_2729275364143627970[5] = 0; + out_2729275364143627970[6] = 0; + out_2729275364143627970[7] = 0; + out_2729275364143627970[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_4387195160971747702) { - err_fun(nom_x, delta_x, out_4387195160971747702); +void car_err_fun(double *nom_x, double *delta_x, double *out_1972702472511725479) { + err_fun(nom_x, delta_x, out_1972702472511725479); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7707264064834051115) { - inv_err_fun(nom_x, true_x, out_7707264064834051115); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3741555659818717473) { + inv_err_fun(nom_x, true_x, out_3741555659818717473); } -void car_H_mod_fun(double *state, double *out_7178605602671138983) { - H_mod_fun(state, out_7178605602671138983); +void car_H_mod_fun(double *state, double *out_4845788339544047954) { + H_mod_fun(state, out_4845788339544047954); } -void car_f_fun(double *state, double dt, double *out_2416599425795193412) { - f_fun(state, dt, out_2416599425795193412); +void car_f_fun(double *state, double dt, double *out_4185306989729419910) { + f_fun(state, dt, out_4185306989729419910); } -void car_F_fun(double *state, double dt, double *out_6761986232654706930) { - F_fun(state, dt, out_6761986232654706930); +void car_F_fun(double *state, double dt, double *out_8252178874365212967) { + F_fun(state, dt, out_8252178874365212967); } -void car_h_25(double *state, double *unused, double *out_319074315832850169) { - h_25(state, unused, out_319074315832850169); +void car_h_25(double *state, double *unused, double *out_4739826667919645243) { + h_25(state, unused, out_4739826667919645243); } -void car_H_25(double *state, double *unused, double *out_3655624952681647306) { - H_25(state, unused, out_3655624952681647306); +void car_H_25(double *state, double *unused, double *out_2698629402266667542) { + H_25(state, unused, out_2698629402266667542); } -void car_h_24(double *state, double *unused, double *out_5734853090591072423) { - h_24(state, unused, out_5734853090591072423); +void car_h_24(double *state, double *unused, double *out_6091047700113278923) { + h_24(state, unused, out_6091047700113278923); } -void car_H_24(double *state, double *unused, double *out_1429917168702778744) { - H_24(state, unused, out_1429917168702778744); +void car_H_24(double *state, double *unused, double *out_2121692102389320721) { + H_24(state, unused, out_2121692102389320721); } -void car_h_30(double *state, double *unused, double *out_8212013810717373528) { - h_30(state, unused, out_8212013810717373528); +void car_h_30(double *state, double *unused, double *out_7687552658275130204) { + h_30(state, unused, out_7687552658275130204); } -void car_H_30(double *state, double *unused, double *out_3784963899824887376) { - H_30(state, unused, out_3784963899824887376); +void car_H_30(double *state, double *unused, double *out_2569290455123427472) { + H_30(state, unused, out_2569290455123427472); } -void car_h_26(double *state, double *unused, double *out_5233056995331332443) { - h_26(state, unused, out_5233056995331332443); +void car_h_26(double *state, double *unused, double *out_7946639312164568029) { + h_26(state, unused, out_7946639312164568029); } -void car_H_26(double *state, double *unused, double *out_2998770888571335402) { - H_26(state, unused, out_2998770888571335402); +void car_H_26(double *state, double *unused, double *out_1042873916607388682) { + H_26(state, unused, out_1042873916607388682); } -void car_h_27(double *state, double *unused, double *out_3355860367277182854) { - h_27(state, unused, out_3355860367277182854); +void car_h_27(double *state, double *unused, double *out_3665830151724581410) { + h_27(state, unused, out_3665830151724581410); } -void car_H_27(double *state, double *unused, double *out_5959727211625312287) { - H_27(state, unused, out_5959727211625312287); +void car_H_27(double *state, double *unused, double *out_394527143323002561) { + H_27(state, unused, out_394527143323002561); } -void car_h_29(double *state, double *unused, double *out_7630324942183480437) { - h_29(state, unused, out_7630324942183480437); +void car_h_29(double *state, double *unused, double *out_8290868542951607721) { + h_29(state, unused, out_8290868542951607721); } -void car_H_29(double *state, double *unused, double *out_3274732555510495192) { - H_29(state, unused, out_3274732555510495192); +void car_H_29(double *state, double *unused, double *out_3079521799437819656) { + H_29(state, unused, out_3079521799437819656); } -void car_h_28(double *state, double *unused, double *out_4299370220178845289) { - h_28(state, unused, out_4299370220178845289); +void car_h_28(double *state, double *unused, double *out_6846547824895893173) { + h_28(state, unused, out_6846547824895893173); } -void car_H_28(double *state, double *unused, double *out_1311102283945168941) { - H_28(state, unused, out_1311102283945168941); +void car_H_28(double *state, double *unused, double *out_2002877217631710918) { + H_28(state, unused, out_2002877217631710918); } -void car_h_31(double *state, double *unused, double *out_594268378117356058) { - h_31(state, unused, out_594268378117356058); +void car_h_31(double *state, double *unused, double *out_3141445982834403942) { + h_31(state, unused, out_3141445982834403942); } -void car_H_31(double *state, double *unused, double *out_3624978990804686878) { - H_31(state, unused, out_3624978990804686878); +void car_H_31(double *state, double *unused, double *out_2729275364143627970) { + H_31(state, unused, out_2729275364143627970); } 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 73987e5d2..68696f76e 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_4387195160971747702); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7707264064834051115); -void car_H_mod_fun(double *state, double *out_7178605602671138983); -void car_f_fun(double *state, double dt, double *out_2416599425795193412); -void car_F_fun(double *state, double dt, double *out_6761986232654706930); -void car_h_25(double *state, double *unused, double *out_319074315832850169); -void car_H_25(double *state, double *unused, double *out_3655624952681647306); -void car_h_24(double *state, double *unused, double *out_5734853090591072423); -void car_H_24(double *state, double *unused, double *out_1429917168702778744); -void car_h_30(double *state, double *unused, double *out_8212013810717373528); -void car_H_30(double *state, double *unused, double *out_3784963899824887376); -void car_h_26(double *state, double *unused, double *out_5233056995331332443); -void car_H_26(double *state, double *unused, double *out_2998770888571335402); -void car_h_27(double *state, double *unused, double *out_3355860367277182854); -void car_H_27(double *state, double *unused, double *out_5959727211625312287); -void car_h_29(double *state, double *unused, double *out_7630324942183480437); -void car_H_29(double *state, double *unused, double *out_3274732555510495192); -void car_h_28(double *state, double *unused, double *out_4299370220178845289); -void car_H_28(double *state, double *unused, double *out_1311102283945168941); -void car_h_31(double *state, double *unused, double *out_594268378117356058); -void car_H_31(double *state, double *unused, double *out_3624978990804686878); +void car_err_fun(double *nom_x, double *delta_x, double *out_1972702472511725479); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3741555659818717473); +void car_H_mod_fun(double *state, double *out_4845788339544047954); +void car_f_fun(double *state, double dt, double *out_4185306989729419910); +void car_F_fun(double *state, double dt, double *out_8252178874365212967); +void car_h_25(double *state, double *unused, double *out_4739826667919645243); +void car_H_25(double *state, double *unused, double *out_2698629402266667542); +void car_h_24(double *state, double *unused, double *out_6091047700113278923); +void car_H_24(double *state, double *unused, double *out_2121692102389320721); +void car_h_30(double *state, double *unused, double *out_7687552658275130204); +void car_H_30(double *state, double *unused, double *out_2569290455123427472); +void car_h_26(double *state, double *unused, double *out_7946639312164568029); +void car_H_26(double *state, double *unused, double *out_1042873916607388682); +void car_h_27(double *state, double *unused, double *out_3665830151724581410); +void car_H_27(double *state, double *unused, double *out_394527143323002561); +void car_h_29(double *state, double *unused, double *out_8290868542951607721); +void car_H_29(double *state, double *unused, double *out_3079521799437819656); +void car_h_28(double *state, double *unused, double *out_6846547824895893173); +void car_H_28(double *state, double *unused, double *out_2002877217631710918); +void car_h_31(double *state, double *unused, double *out_3141445982834403942); +void car_H_31(double *state, double *unused, double *out_2729275364143627970); 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 08ba23662..2fa4f9fd9 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_8207359146711228947) { - out_8207359146711228947[0] = delta_x[0] + nom_x[0]; - out_8207359146711228947[1] = delta_x[1] + nom_x[1]; - out_8207359146711228947[2] = delta_x[2] + nom_x[2]; - out_8207359146711228947[3] = delta_x[3] + nom_x[3]; - out_8207359146711228947[4] = delta_x[4] + nom_x[4]; - out_8207359146711228947[5] = delta_x[5] + nom_x[5]; - out_8207359146711228947[6] = delta_x[6] + nom_x[6]; - out_8207359146711228947[7] = delta_x[7] + nom_x[7]; - out_8207359146711228947[8] = delta_x[8] + nom_x[8]; - out_8207359146711228947[9] = delta_x[9] + nom_x[9]; - out_8207359146711228947[10] = delta_x[10] + nom_x[10]; - out_8207359146711228947[11] = delta_x[11] + nom_x[11]; - out_8207359146711228947[12] = delta_x[12] + nom_x[12]; - out_8207359146711228947[13] = delta_x[13] + nom_x[13]; - out_8207359146711228947[14] = delta_x[14] + nom_x[14]; - out_8207359146711228947[15] = delta_x[15] + nom_x[15]; - out_8207359146711228947[16] = delta_x[16] + nom_x[16]; - out_8207359146711228947[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_867342344380239798) { + out_867342344380239798[0] = delta_x[0] + nom_x[0]; + out_867342344380239798[1] = delta_x[1] + nom_x[1]; + out_867342344380239798[2] = delta_x[2] + nom_x[2]; + out_867342344380239798[3] = delta_x[3] + nom_x[3]; + out_867342344380239798[4] = delta_x[4] + nom_x[4]; + out_867342344380239798[5] = delta_x[5] + nom_x[5]; + out_867342344380239798[6] = delta_x[6] + nom_x[6]; + out_867342344380239798[7] = delta_x[7] + nom_x[7]; + out_867342344380239798[8] = delta_x[8] + nom_x[8]; + out_867342344380239798[9] = delta_x[9] + nom_x[9]; + out_867342344380239798[10] = delta_x[10] + nom_x[10]; + out_867342344380239798[11] = delta_x[11] + nom_x[11]; + out_867342344380239798[12] = delta_x[12] + nom_x[12]; + out_867342344380239798[13] = delta_x[13] + nom_x[13]; + out_867342344380239798[14] = delta_x[14] + nom_x[14]; + out_867342344380239798[15] = delta_x[15] + nom_x[15]; + out_867342344380239798[16] = delta_x[16] + nom_x[16]; + out_867342344380239798[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8369219426901598353) { - out_8369219426901598353[0] = -nom_x[0] + true_x[0]; - out_8369219426901598353[1] = -nom_x[1] + true_x[1]; - out_8369219426901598353[2] = -nom_x[2] + true_x[2]; - out_8369219426901598353[3] = -nom_x[3] + true_x[3]; - out_8369219426901598353[4] = -nom_x[4] + true_x[4]; - out_8369219426901598353[5] = -nom_x[5] + true_x[5]; - out_8369219426901598353[6] = -nom_x[6] + true_x[6]; - out_8369219426901598353[7] = -nom_x[7] + true_x[7]; - out_8369219426901598353[8] = -nom_x[8] + true_x[8]; - out_8369219426901598353[9] = -nom_x[9] + true_x[9]; - out_8369219426901598353[10] = -nom_x[10] + true_x[10]; - out_8369219426901598353[11] = -nom_x[11] + true_x[11]; - out_8369219426901598353[12] = -nom_x[12] + true_x[12]; - out_8369219426901598353[13] = -nom_x[13] + true_x[13]; - out_8369219426901598353[14] = -nom_x[14] + true_x[14]; - out_8369219426901598353[15] = -nom_x[15] + true_x[15]; - out_8369219426901598353[16] = -nom_x[16] + true_x[16]; - out_8369219426901598353[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2946458972277190645) { + out_2946458972277190645[0] = -nom_x[0] + true_x[0]; + out_2946458972277190645[1] = -nom_x[1] + true_x[1]; + out_2946458972277190645[2] = -nom_x[2] + true_x[2]; + out_2946458972277190645[3] = -nom_x[3] + true_x[3]; + out_2946458972277190645[4] = -nom_x[4] + true_x[4]; + out_2946458972277190645[5] = -nom_x[5] + true_x[5]; + out_2946458972277190645[6] = -nom_x[6] + true_x[6]; + out_2946458972277190645[7] = -nom_x[7] + true_x[7]; + out_2946458972277190645[8] = -nom_x[8] + true_x[8]; + out_2946458972277190645[9] = -nom_x[9] + true_x[9]; + out_2946458972277190645[10] = -nom_x[10] + true_x[10]; + out_2946458972277190645[11] = -nom_x[11] + true_x[11]; + out_2946458972277190645[12] = -nom_x[12] + true_x[12]; + out_2946458972277190645[13] = -nom_x[13] + true_x[13]; + out_2946458972277190645[14] = -nom_x[14] + true_x[14]; + out_2946458972277190645[15] = -nom_x[15] + true_x[15]; + out_2946458972277190645[16] = -nom_x[16] + true_x[16]; + out_2946458972277190645[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_7202306802530511060) { - out_7202306802530511060[0] = 1.0; - out_7202306802530511060[1] = 0.0; - out_7202306802530511060[2] = 0.0; - out_7202306802530511060[3] = 0.0; - out_7202306802530511060[4] = 0.0; - out_7202306802530511060[5] = 0.0; - out_7202306802530511060[6] = 0.0; - out_7202306802530511060[7] = 0.0; - out_7202306802530511060[8] = 0.0; - out_7202306802530511060[9] = 0.0; - out_7202306802530511060[10] = 0.0; - out_7202306802530511060[11] = 0.0; - out_7202306802530511060[12] = 0.0; - out_7202306802530511060[13] = 0.0; - out_7202306802530511060[14] = 0.0; - out_7202306802530511060[15] = 0.0; - out_7202306802530511060[16] = 0.0; - out_7202306802530511060[17] = 0.0; - out_7202306802530511060[18] = 0.0; - out_7202306802530511060[19] = 1.0; - out_7202306802530511060[20] = 0.0; - out_7202306802530511060[21] = 0.0; - out_7202306802530511060[22] = 0.0; - out_7202306802530511060[23] = 0.0; - out_7202306802530511060[24] = 0.0; - out_7202306802530511060[25] = 0.0; - out_7202306802530511060[26] = 0.0; - out_7202306802530511060[27] = 0.0; - out_7202306802530511060[28] = 0.0; - out_7202306802530511060[29] = 0.0; - out_7202306802530511060[30] = 0.0; - out_7202306802530511060[31] = 0.0; - out_7202306802530511060[32] = 0.0; - out_7202306802530511060[33] = 0.0; - out_7202306802530511060[34] = 0.0; - out_7202306802530511060[35] = 0.0; - out_7202306802530511060[36] = 0.0; - out_7202306802530511060[37] = 0.0; - out_7202306802530511060[38] = 1.0; - out_7202306802530511060[39] = 0.0; - out_7202306802530511060[40] = 0.0; - out_7202306802530511060[41] = 0.0; - out_7202306802530511060[42] = 0.0; - out_7202306802530511060[43] = 0.0; - out_7202306802530511060[44] = 0.0; - out_7202306802530511060[45] = 0.0; - out_7202306802530511060[46] = 0.0; - out_7202306802530511060[47] = 0.0; - out_7202306802530511060[48] = 0.0; - out_7202306802530511060[49] = 0.0; - out_7202306802530511060[50] = 0.0; - out_7202306802530511060[51] = 0.0; - out_7202306802530511060[52] = 0.0; - out_7202306802530511060[53] = 0.0; - out_7202306802530511060[54] = 0.0; - out_7202306802530511060[55] = 0.0; - out_7202306802530511060[56] = 0.0; - out_7202306802530511060[57] = 1.0; - out_7202306802530511060[58] = 0.0; - out_7202306802530511060[59] = 0.0; - out_7202306802530511060[60] = 0.0; - out_7202306802530511060[61] = 0.0; - out_7202306802530511060[62] = 0.0; - out_7202306802530511060[63] = 0.0; - out_7202306802530511060[64] = 0.0; - out_7202306802530511060[65] = 0.0; - out_7202306802530511060[66] = 0.0; - out_7202306802530511060[67] = 0.0; - out_7202306802530511060[68] = 0.0; - out_7202306802530511060[69] = 0.0; - out_7202306802530511060[70] = 0.0; - out_7202306802530511060[71] = 0.0; - out_7202306802530511060[72] = 0.0; - out_7202306802530511060[73] = 0.0; - out_7202306802530511060[74] = 0.0; - out_7202306802530511060[75] = 0.0; - out_7202306802530511060[76] = 1.0; - out_7202306802530511060[77] = 0.0; - out_7202306802530511060[78] = 0.0; - out_7202306802530511060[79] = 0.0; - out_7202306802530511060[80] = 0.0; - out_7202306802530511060[81] = 0.0; - out_7202306802530511060[82] = 0.0; - out_7202306802530511060[83] = 0.0; - out_7202306802530511060[84] = 0.0; - out_7202306802530511060[85] = 0.0; - out_7202306802530511060[86] = 0.0; - out_7202306802530511060[87] = 0.0; - out_7202306802530511060[88] = 0.0; - out_7202306802530511060[89] = 0.0; - out_7202306802530511060[90] = 0.0; - out_7202306802530511060[91] = 0.0; - out_7202306802530511060[92] = 0.0; - out_7202306802530511060[93] = 0.0; - out_7202306802530511060[94] = 0.0; - out_7202306802530511060[95] = 1.0; - out_7202306802530511060[96] = 0.0; - out_7202306802530511060[97] = 0.0; - out_7202306802530511060[98] = 0.0; - out_7202306802530511060[99] = 0.0; - out_7202306802530511060[100] = 0.0; - out_7202306802530511060[101] = 0.0; - out_7202306802530511060[102] = 0.0; - out_7202306802530511060[103] = 0.0; - out_7202306802530511060[104] = 0.0; - out_7202306802530511060[105] = 0.0; - out_7202306802530511060[106] = 0.0; - out_7202306802530511060[107] = 0.0; - out_7202306802530511060[108] = 0.0; - out_7202306802530511060[109] = 0.0; - out_7202306802530511060[110] = 0.0; - out_7202306802530511060[111] = 0.0; - out_7202306802530511060[112] = 0.0; - out_7202306802530511060[113] = 0.0; - out_7202306802530511060[114] = 1.0; - out_7202306802530511060[115] = 0.0; - out_7202306802530511060[116] = 0.0; - out_7202306802530511060[117] = 0.0; - out_7202306802530511060[118] = 0.0; - out_7202306802530511060[119] = 0.0; - out_7202306802530511060[120] = 0.0; - out_7202306802530511060[121] = 0.0; - out_7202306802530511060[122] = 0.0; - out_7202306802530511060[123] = 0.0; - out_7202306802530511060[124] = 0.0; - out_7202306802530511060[125] = 0.0; - out_7202306802530511060[126] = 0.0; - out_7202306802530511060[127] = 0.0; - out_7202306802530511060[128] = 0.0; - out_7202306802530511060[129] = 0.0; - out_7202306802530511060[130] = 0.0; - out_7202306802530511060[131] = 0.0; - out_7202306802530511060[132] = 0.0; - out_7202306802530511060[133] = 1.0; - out_7202306802530511060[134] = 0.0; - out_7202306802530511060[135] = 0.0; - out_7202306802530511060[136] = 0.0; - out_7202306802530511060[137] = 0.0; - out_7202306802530511060[138] = 0.0; - out_7202306802530511060[139] = 0.0; - out_7202306802530511060[140] = 0.0; - out_7202306802530511060[141] = 0.0; - out_7202306802530511060[142] = 0.0; - out_7202306802530511060[143] = 0.0; - out_7202306802530511060[144] = 0.0; - out_7202306802530511060[145] = 0.0; - out_7202306802530511060[146] = 0.0; - out_7202306802530511060[147] = 0.0; - out_7202306802530511060[148] = 0.0; - out_7202306802530511060[149] = 0.0; - out_7202306802530511060[150] = 0.0; - out_7202306802530511060[151] = 0.0; - out_7202306802530511060[152] = 1.0; - out_7202306802530511060[153] = 0.0; - out_7202306802530511060[154] = 0.0; - out_7202306802530511060[155] = 0.0; - out_7202306802530511060[156] = 0.0; - out_7202306802530511060[157] = 0.0; - out_7202306802530511060[158] = 0.0; - out_7202306802530511060[159] = 0.0; - out_7202306802530511060[160] = 0.0; - out_7202306802530511060[161] = 0.0; - out_7202306802530511060[162] = 0.0; - out_7202306802530511060[163] = 0.0; - out_7202306802530511060[164] = 0.0; - out_7202306802530511060[165] = 0.0; - out_7202306802530511060[166] = 0.0; - out_7202306802530511060[167] = 0.0; - out_7202306802530511060[168] = 0.0; - out_7202306802530511060[169] = 0.0; - out_7202306802530511060[170] = 0.0; - out_7202306802530511060[171] = 1.0; - out_7202306802530511060[172] = 0.0; - out_7202306802530511060[173] = 0.0; - out_7202306802530511060[174] = 0.0; - out_7202306802530511060[175] = 0.0; - out_7202306802530511060[176] = 0.0; - out_7202306802530511060[177] = 0.0; - out_7202306802530511060[178] = 0.0; - out_7202306802530511060[179] = 0.0; - out_7202306802530511060[180] = 0.0; - out_7202306802530511060[181] = 0.0; - out_7202306802530511060[182] = 0.0; - out_7202306802530511060[183] = 0.0; - out_7202306802530511060[184] = 0.0; - out_7202306802530511060[185] = 0.0; - out_7202306802530511060[186] = 0.0; - out_7202306802530511060[187] = 0.0; - out_7202306802530511060[188] = 0.0; - out_7202306802530511060[189] = 0.0; - out_7202306802530511060[190] = 1.0; - out_7202306802530511060[191] = 0.0; - out_7202306802530511060[192] = 0.0; - out_7202306802530511060[193] = 0.0; - out_7202306802530511060[194] = 0.0; - out_7202306802530511060[195] = 0.0; - out_7202306802530511060[196] = 0.0; - out_7202306802530511060[197] = 0.0; - out_7202306802530511060[198] = 0.0; - out_7202306802530511060[199] = 0.0; - out_7202306802530511060[200] = 0.0; - out_7202306802530511060[201] = 0.0; - out_7202306802530511060[202] = 0.0; - out_7202306802530511060[203] = 0.0; - out_7202306802530511060[204] = 0.0; - out_7202306802530511060[205] = 0.0; - out_7202306802530511060[206] = 0.0; - out_7202306802530511060[207] = 0.0; - out_7202306802530511060[208] = 0.0; - out_7202306802530511060[209] = 1.0; - out_7202306802530511060[210] = 0.0; - out_7202306802530511060[211] = 0.0; - out_7202306802530511060[212] = 0.0; - out_7202306802530511060[213] = 0.0; - out_7202306802530511060[214] = 0.0; - out_7202306802530511060[215] = 0.0; - out_7202306802530511060[216] = 0.0; - out_7202306802530511060[217] = 0.0; - out_7202306802530511060[218] = 0.0; - out_7202306802530511060[219] = 0.0; - out_7202306802530511060[220] = 0.0; - out_7202306802530511060[221] = 0.0; - out_7202306802530511060[222] = 0.0; - out_7202306802530511060[223] = 0.0; - out_7202306802530511060[224] = 0.0; - out_7202306802530511060[225] = 0.0; - out_7202306802530511060[226] = 0.0; - out_7202306802530511060[227] = 0.0; - out_7202306802530511060[228] = 1.0; - out_7202306802530511060[229] = 0.0; - out_7202306802530511060[230] = 0.0; - out_7202306802530511060[231] = 0.0; - out_7202306802530511060[232] = 0.0; - out_7202306802530511060[233] = 0.0; - out_7202306802530511060[234] = 0.0; - out_7202306802530511060[235] = 0.0; - out_7202306802530511060[236] = 0.0; - out_7202306802530511060[237] = 0.0; - out_7202306802530511060[238] = 0.0; - out_7202306802530511060[239] = 0.0; - out_7202306802530511060[240] = 0.0; - out_7202306802530511060[241] = 0.0; - out_7202306802530511060[242] = 0.0; - out_7202306802530511060[243] = 0.0; - out_7202306802530511060[244] = 0.0; - out_7202306802530511060[245] = 0.0; - out_7202306802530511060[246] = 0.0; - out_7202306802530511060[247] = 1.0; - out_7202306802530511060[248] = 0.0; - out_7202306802530511060[249] = 0.0; - out_7202306802530511060[250] = 0.0; - out_7202306802530511060[251] = 0.0; - out_7202306802530511060[252] = 0.0; - out_7202306802530511060[253] = 0.0; - out_7202306802530511060[254] = 0.0; - out_7202306802530511060[255] = 0.0; - out_7202306802530511060[256] = 0.0; - out_7202306802530511060[257] = 0.0; - out_7202306802530511060[258] = 0.0; - out_7202306802530511060[259] = 0.0; - out_7202306802530511060[260] = 0.0; - out_7202306802530511060[261] = 0.0; - out_7202306802530511060[262] = 0.0; - out_7202306802530511060[263] = 0.0; - out_7202306802530511060[264] = 0.0; - out_7202306802530511060[265] = 0.0; - out_7202306802530511060[266] = 1.0; - out_7202306802530511060[267] = 0.0; - out_7202306802530511060[268] = 0.0; - out_7202306802530511060[269] = 0.0; - out_7202306802530511060[270] = 0.0; - out_7202306802530511060[271] = 0.0; - out_7202306802530511060[272] = 0.0; - out_7202306802530511060[273] = 0.0; - out_7202306802530511060[274] = 0.0; - out_7202306802530511060[275] = 0.0; - out_7202306802530511060[276] = 0.0; - out_7202306802530511060[277] = 0.0; - out_7202306802530511060[278] = 0.0; - out_7202306802530511060[279] = 0.0; - out_7202306802530511060[280] = 0.0; - out_7202306802530511060[281] = 0.0; - out_7202306802530511060[282] = 0.0; - out_7202306802530511060[283] = 0.0; - out_7202306802530511060[284] = 0.0; - out_7202306802530511060[285] = 1.0; - out_7202306802530511060[286] = 0.0; - out_7202306802530511060[287] = 0.0; - out_7202306802530511060[288] = 0.0; - out_7202306802530511060[289] = 0.0; - out_7202306802530511060[290] = 0.0; - out_7202306802530511060[291] = 0.0; - out_7202306802530511060[292] = 0.0; - out_7202306802530511060[293] = 0.0; - out_7202306802530511060[294] = 0.0; - out_7202306802530511060[295] = 0.0; - out_7202306802530511060[296] = 0.0; - out_7202306802530511060[297] = 0.0; - out_7202306802530511060[298] = 0.0; - out_7202306802530511060[299] = 0.0; - out_7202306802530511060[300] = 0.0; - out_7202306802530511060[301] = 0.0; - out_7202306802530511060[302] = 0.0; - out_7202306802530511060[303] = 0.0; - out_7202306802530511060[304] = 1.0; - out_7202306802530511060[305] = 0.0; - out_7202306802530511060[306] = 0.0; - out_7202306802530511060[307] = 0.0; - out_7202306802530511060[308] = 0.0; - out_7202306802530511060[309] = 0.0; - out_7202306802530511060[310] = 0.0; - out_7202306802530511060[311] = 0.0; - out_7202306802530511060[312] = 0.0; - out_7202306802530511060[313] = 0.0; - out_7202306802530511060[314] = 0.0; - out_7202306802530511060[315] = 0.0; - out_7202306802530511060[316] = 0.0; - out_7202306802530511060[317] = 0.0; - out_7202306802530511060[318] = 0.0; - out_7202306802530511060[319] = 0.0; - out_7202306802530511060[320] = 0.0; - out_7202306802530511060[321] = 0.0; - out_7202306802530511060[322] = 0.0; - out_7202306802530511060[323] = 1.0; +void H_mod_fun(double *state, double *out_8034808007713849348) { + out_8034808007713849348[0] = 1.0; + out_8034808007713849348[1] = 0.0; + out_8034808007713849348[2] = 0.0; + out_8034808007713849348[3] = 0.0; + out_8034808007713849348[4] = 0.0; + out_8034808007713849348[5] = 0.0; + out_8034808007713849348[6] = 0.0; + out_8034808007713849348[7] = 0.0; + out_8034808007713849348[8] = 0.0; + out_8034808007713849348[9] = 0.0; + out_8034808007713849348[10] = 0.0; + out_8034808007713849348[11] = 0.0; + out_8034808007713849348[12] = 0.0; + out_8034808007713849348[13] = 0.0; + out_8034808007713849348[14] = 0.0; + out_8034808007713849348[15] = 0.0; + out_8034808007713849348[16] = 0.0; + out_8034808007713849348[17] = 0.0; + out_8034808007713849348[18] = 0.0; + out_8034808007713849348[19] = 1.0; + out_8034808007713849348[20] = 0.0; + out_8034808007713849348[21] = 0.0; + out_8034808007713849348[22] = 0.0; + out_8034808007713849348[23] = 0.0; + out_8034808007713849348[24] = 0.0; + out_8034808007713849348[25] = 0.0; + out_8034808007713849348[26] = 0.0; + out_8034808007713849348[27] = 0.0; + out_8034808007713849348[28] = 0.0; + out_8034808007713849348[29] = 0.0; + out_8034808007713849348[30] = 0.0; + out_8034808007713849348[31] = 0.0; + out_8034808007713849348[32] = 0.0; + out_8034808007713849348[33] = 0.0; + out_8034808007713849348[34] = 0.0; + out_8034808007713849348[35] = 0.0; + out_8034808007713849348[36] = 0.0; + out_8034808007713849348[37] = 0.0; + out_8034808007713849348[38] = 1.0; + out_8034808007713849348[39] = 0.0; + out_8034808007713849348[40] = 0.0; + out_8034808007713849348[41] = 0.0; + out_8034808007713849348[42] = 0.0; + out_8034808007713849348[43] = 0.0; + out_8034808007713849348[44] = 0.0; + out_8034808007713849348[45] = 0.0; + out_8034808007713849348[46] = 0.0; + out_8034808007713849348[47] = 0.0; + out_8034808007713849348[48] = 0.0; + out_8034808007713849348[49] = 0.0; + out_8034808007713849348[50] = 0.0; + out_8034808007713849348[51] = 0.0; + out_8034808007713849348[52] = 0.0; + out_8034808007713849348[53] = 0.0; + out_8034808007713849348[54] = 0.0; + out_8034808007713849348[55] = 0.0; + out_8034808007713849348[56] = 0.0; + out_8034808007713849348[57] = 1.0; + out_8034808007713849348[58] = 0.0; + out_8034808007713849348[59] = 0.0; + out_8034808007713849348[60] = 0.0; + out_8034808007713849348[61] = 0.0; + out_8034808007713849348[62] = 0.0; + out_8034808007713849348[63] = 0.0; + out_8034808007713849348[64] = 0.0; + out_8034808007713849348[65] = 0.0; + out_8034808007713849348[66] = 0.0; + out_8034808007713849348[67] = 0.0; + out_8034808007713849348[68] = 0.0; + out_8034808007713849348[69] = 0.0; + out_8034808007713849348[70] = 0.0; + out_8034808007713849348[71] = 0.0; + out_8034808007713849348[72] = 0.0; + out_8034808007713849348[73] = 0.0; + out_8034808007713849348[74] = 0.0; + out_8034808007713849348[75] = 0.0; + out_8034808007713849348[76] = 1.0; + out_8034808007713849348[77] = 0.0; + out_8034808007713849348[78] = 0.0; + out_8034808007713849348[79] = 0.0; + out_8034808007713849348[80] = 0.0; + out_8034808007713849348[81] = 0.0; + out_8034808007713849348[82] = 0.0; + out_8034808007713849348[83] = 0.0; + out_8034808007713849348[84] = 0.0; + out_8034808007713849348[85] = 0.0; + out_8034808007713849348[86] = 0.0; + out_8034808007713849348[87] = 0.0; + out_8034808007713849348[88] = 0.0; + out_8034808007713849348[89] = 0.0; + out_8034808007713849348[90] = 0.0; + out_8034808007713849348[91] = 0.0; + out_8034808007713849348[92] = 0.0; + out_8034808007713849348[93] = 0.0; + out_8034808007713849348[94] = 0.0; + out_8034808007713849348[95] = 1.0; + out_8034808007713849348[96] = 0.0; + out_8034808007713849348[97] = 0.0; + out_8034808007713849348[98] = 0.0; + out_8034808007713849348[99] = 0.0; + out_8034808007713849348[100] = 0.0; + out_8034808007713849348[101] = 0.0; + out_8034808007713849348[102] = 0.0; + out_8034808007713849348[103] = 0.0; + out_8034808007713849348[104] = 0.0; + out_8034808007713849348[105] = 0.0; + out_8034808007713849348[106] = 0.0; + out_8034808007713849348[107] = 0.0; + out_8034808007713849348[108] = 0.0; + out_8034808007713849348[109] = 0.0; + out_8034808007713849348[110] = 0.0; + out_8034808007713849348[111] = 0.0; + out_8034808007713849348[112] = 0.0; + out_8034808007713849348[113] = 0.0; + out_8034808007713849348[114] = 1.0; + out_8034808007713849348[115] = 0.0; + out_8034808007713849348[116] = 0.0; + out_8034808007713849348[117] = 0.0; + out_8034808007713849348[118] = 0.0; + out_8034808007713849348[119] = 0.0; + out_8034808007713849348[120] = 0.0; + out_8034808007713849348[121] = 0.0; + out_8034808007713849348[122] = 0.0; + out_8034808007713849348[123] = 0.0; + out_8034808007713849348[124] = 0.0; + out_8034808007713849348[125] = 0.0; + out_8034808007713849348[126] = 0.0; + out_8034808007713849348[127] = 0.0; + out_8034808007713849348[128] = 0.0; + out_8034808007713849348[129] = 0.0; + out_8034808007713849348[130] = 0.0; + out_8034808007713849348[131] = 0.0; + out_8034808007713849348[132] = 0.0; + out_8034808007713849348[133] = 1.0; + out_8034808007713849348[134] = 0.0; + out_8034808007713849348[135] = 0.0; + out_8034808007713849348[136] = 0.0; + out_8034808007713849348[137] = 0.0; + out_8034808007713849348[138] = 0.0; + out_8034808007713849348[139] = 0.0; + out_8034808007713849348[140] = 0.0; + out_8034808007713849348[141] = 0.0; + out_8034808007713849348[142] = 0.0; + out_8034808007713849348[143] = 0.0; + out_8034808007713849348[144] = 0.0; + out_8034808007713849348[145] = 0.0; + out_8034808007713849348[146] = 0.0; + out_8034808007713849348[147] = 0.0; + out_8034808007713849348[148] = 0.0; + out_8034808007713849348[149] = 0.0; + out_8034808007713849348[150] = 0.0; + out_8034808007713849348[151] = 0.0; + out_8034808007713849348[152] = 1.0; + out_8034808007713849348[153] = 0.0; + out_8034808007713849348[154] = 0.0; + out_8034808007713849348[155] = 0.0; + out_8034808007713849348[156] = 0.0; + out_8034808007713849348[157] = 0.0; + out_8034808007713849348[158] = 0.0; + out_8034808007713849348[159] = 0.0; + out_8034808007713849348[160] = 0.0; + out_8034808007713849348[161] = 0.0; + out_8034808007713849348[162] = 0.0; + out_8034808007713849348[163] = 0.0; + out_8034808007713849348[164] = 0.0; + out_8034808007713849348[165] = 0.0; + out_8034808007713849348[166] = 0.0; + out_8034808007713849348[167] = 0.0; + out_8034808007713849348[168] = 0.0; + out_8034808007713849348[169] = 0.0; + out_8034808007713849348[170] = 0.0; + out_8034808007713849348[171] = 1.0; + out_8034808007713849348[172] = 0.0; + out_8034808007713849348[173] = 0.0; + out_8034808007713849348[174] = 0.0; + out_8034808007713849348[175] = 0.0; + out_8034808007713849348[176] = 0.0; + out_8034808007713849348[177] = 0.0; + out_8034808007713849348[178] = 0.0; + out_8034808007713849348[179] = 0.0; + out_8034808007713849348[180] = 0.0; + out_8034808007713849348[181] = 0.0; + out_8034808007713849348[182] = 0.0; + out_8034808007713849348[183] = 0.0; + out_8034808007713849348[184] = 0.0; + out_8034808007713849348[185] = 0.0; + out_8034808007713849348[186] = 0.0; + out_8034808007713849348[187] = 0.0; + out_8034808007713849348[188] = 0.0; + out_8034808007713849348[189] = 0.0; + out_8034808007713849348[190] = 1.0; + out_8034808007713849348[191] = 0.0; + out_8034808007713849348[192] = 0.0; + out_8034808007713849348[193] = 0.0; + out_8034808007713849348[194] = 0.0; + out_8034808007713849348[195] = 0.0; + out_8034808007713849348[196] = 0.0; + out_8034808007713849348[197] = 0.0; + out_8034808007713849348[198] = 0.0; + out_8034808007713849348[199] = 0.0; + out_8034808007713849348[200] = 0.0; + out_8034808007713849348[201] = 0.0; + out_8034808007713849348[202] = 0.0; + out_8034808007713849348[203] = 0.0; + out_8034808007713849348[204] = 0.0; + out_8034808007713849348[205] = 0.0; + out_8034808007713849348[206] = 0.0; + out_8034808007713849348[207] = 0.0; + out_8034808007713849348[208] = 0.0; + out_8034808007713849348[209] = 1.0; + out_8034808007713849348[210] = 0.0; + out_8034808007713849348[211] = 0.0; + out_8034808007713849348[212] = 0.0; + out_8034808007713849348[213] = 0.0; + out_8034808007713849348[214] = 0.0; + out_8034808007713849348[215] = 0.0; + out_8034808007713849348[216] = 0.0; + out_8034808007713849348[217] = 0.0; + out_8034808007713849348[218] = 0.0; + out_8034808007713849348[219] = 0.0; + out_8034808007713849348[220] = 0.0; + out_8034808007713849348[221] = 0.0; + out_8034808007713849348[222] = 0.0; + out_8034808007713849348[223] = 0.0; + out_8034808007713849348[224] = 0.0; + out_8034808007713849348[225] = 0.0; + out_8034808007713849348[226] = 0.0; + out_8034808007713849348[227] = 0.0; + out_8034808007713849348[228] = 1.0; + out_8034808007713849348[229] = 0.0; + out_8034808007713849348[230] = 0.0; + out_8034808007713849348[231] = 0.0; + out_8034808007713849348[232] = 0.0; + out_8034808007713849348[233] = 0.0; + out_8034808007713849348[234] = 0.0; + out_8034808007713849348[235] = 0.0; + out_8034808007713849348[236] = 0.0; + out_8034808007713849348[237] = 0.0; + out_8034808007713849348[238] = 0.0; + out_8034808007713849348[239] = 0.0; + out_8034808007713849348[240] = 0.0; + out_8034808007713849348[241] = 0.0; + out_8034808007713849348[242] = 0.0; + out_8034808007713849348[243] = 0.0; + out_8034808007713849348[244] = 0.0; + out_8034808007713849348[245] = 0.0; + out_8034808007713849348[246] = 0.0; + out_8034808007713849348[247] = 1.0; + out_8034808007713849348[248] = 0.0; + out_8034808007713849348[249] = 0.0; + out_8034808007713849348[250] = 0.0; + out_8034808007713849348[251] = 0.0; + out_8034808007713849348[252] = 0.0; + out_8034808007713849348[253] = 0.0; + out_8034808007713849348[254] = 0.0; + out_8034808007713849348[255] = 0.0; + out_8034808007713849348[256] = 0.0; + out_8034808007713849348[257] = 0.0; + out_8034808007713849348[258] = 0.0; + out_8034808007713849348[259] = 0.0; + out_8034808007713849348[260] = 0.0; + out_8034808007713849348[261] = 0.0; + out_8034808007713849348[262] = 0.0; + out_8034808007713849348[263] = 0.0; + out_8034808007713849348[264] = 0.0; + out_8034808007713849348[265] = 0.0; + out_8034808007713849348[266] = 1.0; + out_8034808007713849348[267] = 0.0; + out_8034808007713849348[268] = 0.0; + out_8034808007713849348[269] = 0.0; + out_8034808007713849348[270] = 0.0; + out_8034808007713849348[271] = 0.0; + out_8034808007713849348[272] = 0.0; + out_8034808007713849348[273] = 0.0; + out_8034808007713849348[274] = 0.0; + out_8034808007713849348[275] = 0.0; + out_8034808007713849348[276] = 0.0; + out_8034808007713849348[277] = 0.0; + out_8034808007713849348[278] = 0.0; + out_8034808007713849348[279] = 0.0; + out_8034808007713849348[280] = 0.0; + out_8034808007713849348[281] = 0.0; + out_8034808007713849348[282] = 0.0; + out_8034808007713849348[283] = 0.0; + out_8034808007713849348[284] = 0.0; + out_8034808007713849348[285] = 1.0; + out_8034808007713849348[286] = 0.0; + out_8034808007713849348[287] = 0.0; + out_8034808007713849348[288] = 0.0; + out_8034808007713849348[289] = 0.0; + out_8034808007713849348[290] = 0.0; + out_8034808007713849348[291] = 0.0; + out_8034808007713849348[292] = 0.0; + out_8034808007713849348[293] = 0.0; + out_8034808007713849348[294] = 0.0; + out_8034808007713849348[295] = 0.0; + out_8034808007713849348[296] = 0.0; + out_8034808007713849348[297] = 0.0; + out_8034808007713849348[298] = 0.0; + out_8034808007713849348[299] = 0.0; + out_8034808007713849348[300] = 0.0; + out_8034808007713849348[301] = 0.0; + out_8034808007713849348[302] = 0.0; + out_8034808007713849348[303] = 0.0; + out_8034808007713849348[304] = 1.0; + out_8034808007713849348[305] = 0.0; + out_8034808007713849348[306] = 0.0; + out_8034808007713849348[307] = 0.0; + out_8034808007713849348[308] = 0.0; + out_8034808007713849348[309] = 0.0; + out_8034808007713849348[310] = 0.0; + out_8034808007713849348[311] = 0.0; + out_8034808007713849348[312] = 0.0; + out_8034808007713849348[313] = 0.0; + out_8034808007713849348[314] = 0.0; + out_8034808007713849348[315] = 0.0; + out_8034808007713849348[316] = 0.0; + out_8034808007713849348[317] = 0.0; + out_8034808007713849348[318] = 0.0; + out_8034808007713849348[319] = 0.0; + out_8034808007713849348[320] = 0.0; + out_8034808007713849348[321] = 0.0; + out_8034808007713849348[322] = 0.0; + out_8034808007713849348[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4961001892384611064) { - out_4961001892384611064[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_4961001892384611064[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_4961001892384611064[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_4961001892384611064[3] = dt*state[12] + state[3]; - out_4961001892384611064[4] = dt*state[13] + state[4]; - out_4961001892384611064[5] = dt*state[14] + state[5]; - out_4961001892384611064[6] = state[6]; - out_4961001892384611064[7] = state[7]; - out_4961001892384611064[8] = state[8]; - out_4961001892384611064[9] = state[9]; - out_4961001892384611064[10] = state[10]; - out_4961001892384611064[11] = state[11]; - out_4961001892384611064[12] = state[12]; - out_4961001892384611064[13] = state[13]; - out_4961001892384611064[14] = state[14]; - out_4961001892384611064[15] = state[15]; - out_4961001892384611064[16] = state[16]; - out_4961001892384611064[17] = state[17]; +void f_fun(double *state, double dt, double *out_5448742405462655453) { + out_5448742405462655453[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_5448742405462655453[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_5448742405462655453[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_5448742405462655453[3] = dt*state[12] + state[3]; + out_5448742405462655453[4] = dt*state[13] + state[4]; + out_5448742405462655453[5] = dt*state[14] + state[5]; + out_5448742405462655453[6] = state[6]; + out_5448742405462655453[7] = state[7]; + out_5448742405462655453[8] = state[8]; + out_5448742405462655453[9] = state[9]; + out_5448742405462655453[10] = state[10]; + out_5448742405462655453[11] = state[11]; + out_5448742405462655453[12] = state[12]; + out_5448742405462655453[13] = state[13]; + out_5448742405462655453[14] = state[14]; + out_5448742405462655453[15] = state[15]; + out_5448742405462655453[16] = state[16]; + out_5448742405462655453[17] = state[17]; } -void F_fun(double *state, double dt, double *out_4383751150204030030) { - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[2] = 0; - out_4383751150204030030[3] = 0; - out_4383751150204030030[4] = 0; - out_4383751150204030030[5] = 0; - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[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_4383751150204030030[9] = 0; - out_4383751150204030030[10] = 0; - out_4383751150204030030[11] = 0; - out_4383751150204030030[12] = 0; - out_4383751150204030030[13] = 0; - out_4383751150204030030[14] = 0; - out_4383751150204030030[15] = 0; - out_4383751150204030030[16] = 0; - out_4383751150204030030[17] = 0; - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[20] = 0; - out_4383751150204030030[21] = 0; - out_4383751150204030030[22] = 0; - out_4383751150204030030[23] = 0; - out_4383751150204030030[24] = 0; - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[27] = 0; - out_4383751150204030030[28] = 0; - out_4383751150204030030[29] = 0; - out_4383751150204030030[30] = 0; - out_4383751150204030030[31] = 0; - out_4383751150204030030[32] = 0; - out_4383751150204030030[33] = 0; - out_4383751150204030030[34] = 0; - out_4383751150204030030[35] = 0; - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[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_4383751150204030030[39] = 0; - out_4383751150204030030[40] = 0; - out_4383751150204030030[41] = 0; - out_4383751150204030030[42] = 0; - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[45] = 0; - out_4383751150204030030[46] = 0; - out_4383751150204030030[47] = 0; - out_4383751150204030030[48] = 0; - out_4383751150204030030[49] = 0; - out_4383751150204030030[50] = 0; - out_4383751150204030030[51] = 0; - out_4383751150204030030[52] = 0; - out_4383751150204030030[53] = 0; - out_4383751150204030030[54] = 0; - out_4383751150204030030[55] = 0; - out_4383751150204030030[56] = 0; - out_4383751150204030030[57] = 1; - out_4383751150204030030[58] = 0; - out_4383751150204030030[59] = 0; - out_4383751150204030030[60] = 0; - out_4383751150204030030[61] = 0; - out_4383751150204030030[62] = 0; - out_4383751150204030030[63] = 0; - out_4383751150204030030[64] = 0; - out_4383751150204030030[65] = 0; - out_4383751150204030030[66] = dt; - out_4383751150204030030[67] = 0; - out_4383751150204030030[68] = 0; - out_4383751150204030030[69] = 0; - out_4383751150204030030[70] = 0; - out_4383751150204030030[71] = 0; - out_4383751150204030030[72] = 0; - out_4383751150204030030[73] = 0; - out_4383751150204030030[74] = 0; - out_4383751150204030030[75] = 0; - out_4383751150204030030[76] = 1; - out_4383751150204030030[77] = 0; - out_4383751150204030030[78] = 0; - out_4383751150204030030[79] = 0; - out_4383751150204030030[80] = 0; - out_4383751150204030030[81] = 0; - out_4383751150204030030[82] = 0; - out_4383751150204030030[83] = 0; - out_4383751150204030030[84] = 0; - out_4383751150204030030[85] = dt; - out_4383751150204030030[86] = 0; - out_4383751150204030030[87] = 0; - out_4383751150204030030[88] = 0; - out_4383751150204030030[89] = 0; - out_4383751150204030030[90] = 0; - out_4383751150204030030[91] = 0; - out_4383751150204030030[92] = 0; - out_4383751150204030030[93] = 0; - out_4383751150204030030[94] = 0; - out_4383751150204030030[95] = 1; - out_4383751150204030030[96] = 0; - out_4383751150204030030[97] = 0; - out_4383751150204030030[98] = 0; - out_4383751150204030030[99] = 0; - out_4383751150204030030[100] = 0; - out_4383751150204030030[101] = 0; - out_4383751150204030030[102] = 0; - out_4383751150204030030[103] = 0; - out_4383751150204030030[104] = dt; - out_4383751150204030030[105] = 0; - out_4383751150204030030[106] = 0; - out_4383751150204030030[107] = 0; - out_4383751150204030030[108] = 0; - out_4383751150204030030[109] = 0; - out_4383751150204030030[110] = 0; - out_4383751150204030030[111] = 0; - out_4383751150204030030[112] = 0; - out_4383751150204030030[113] = 0; - out_4383751150204030030[114] = 1; - out_4383751150204030030[115] = 0; - out_4383751150204030030[116] = 0; - out_4383751150204030030[117] = 0; - out_4383751150204030030[118] = 0; - out_4383751150204030030[119] = 0; - out_4383751150204030030[120] = 0; - out_4383751150204030030[121] = 0; - out_4383751150204030030[122] = 0; - out_4383751150204030030[123] = 0; - out_4383751150204030030[124] = 0; - out_4383751150204030030[125] = 0; - out_4383751150204030030[126] = 0; - out_4383751150204030030[127] = 0; - out_4383751150204030030[128] = 0; - out_4383751150204030030[129] = 0; - out_4383751150204030030[130] = 0; - out_4383751150204030030[131] = 0; - out_4383751150204030030[132] = 0; - out_4383751150204030030[133] = 1; - out_4383751150204030030[134] = 0; - out_4383751150204030030[135] = 0; - out_4383751150204030030[136] = 0; - out_4383751150204030030[137] = 0; - out_4383751150204030030[138] = 0; - out_4383751150204030030[139] = 0; - out_4383751150204030030[140] = 0; - out_4383751150204030030[141] = 0; - out_4383751150204030030[142] = 0; - out_4383751150204030030[143] = 0; - out_4383751150204030030[144] = 0; - out_4383751150204030030[145] = 0; - out_4383751150204030030[146] = 0; - out_4383751150204030030[147] = 0; - out_4383751150204030030[148] = 0; - out_4383751150204030030[149] = 0; - out_4383751150204030030[150] = 0; - out_4383751150204030030[151] = 0; - out_4383751150204030030[152] = 1; - out_4383751150204030030[153] = 0; - out_4383751150204030030[154] = 0; - out_4383751150204030030[155] = 0; - out_4383751150204030030[156] = 0; - out_4383751150204030030[157] = 0; - out_4383751150204030030[158] = 0; - out_4383751150204030030[159] = 0; - out_4383751150204030030[160] = 0; - out_4383751150204030030[161] = 0; - out_4383751150204030030[162] = 0; - out_4383751150204030030[163] = 0; - out_4383751150204030030[164] = 0; - out_4383751150204030030[165] = 0; - out_4383751150204030030[166] = 0; - out_4383751150204030030[167] = 0; - out_4383751150204030030[168] = 0; - out_4383751150204030030[169] = 0; - out_4383751150204030030[170] = 0; - out_4383751150204030030[171] = 1; - out_4383751150204030030[172] = 0; - out_4383751150204030030[173] = 0; - out_4383751150204030030[174] = 0; - out_4383751150204030030[175] = 0; - out_4383751150204030030[176] = 0; - out_4383751150204030030[177] = 0; - out_4383751150204030030[178] = 0; - out_4383751150204030030[179] = 0; - out_4383751150204030030[180] = 0; - out_4383751150204030030[181] = 0; - out_4383751150204030030[182] = 0; - out_4383751150204030030[183] = 0; - out_4383751150204030030[184] = 0; - out_4383751150204030030[185] = 0; - out_4383751150204030030[186] = 0; - out_4383751150204030030[187] = 0; - out_4383751150204030030[188] = 0; - out_4383751150204030030[189] = 0; - out_4383751150204030030[190] = 1; - out_4383751150204030030[191] = 0; - out_4383751150204030030[192] = 0; - out_4383751150204030030[193] = 0; - out_4383751150204030030[194] = 0; - out_4383751150204030030[195] = 0; - out_4383751150204030030[196] = 0; - out_4383751150204030030[197] = 0; - out_4383751150204030030[198] = 0; - out_4383751150204030030[199] = 0; - out_4383751150204030030[200] = 0; - out_4383751150204030030[201] = 0; - out_4383751150204030030[202] = 0; - out_4383751150204030030[203] = 0; - out_4383751150204030030[204] = 0; - out_4383751150204030030[205] = 0; - out_4383751150204030030[206] = 0; - out_4383751150204030030[207] = 0; - out_4383751150204030030[208] = 0; - out_4383751150204030030[209] = 1; - out_4383751150204030030[210] = 0; - out_4383751150204030030[211] = 0; - out_4383751150204030030[212] = 0; - out_4383751150204030030[213] = 0; - out_4383751150204030030[214] = 0; - out_4383751150204030030[215] = 0; - out_4383751150204030030[216] = 0; - out_4383751150204030030[217] = 0; - out_4383751150204030030[218] = 0; - out_4383751150204030030[219] = 0; - out_4383751150204030030[220] = 0; - out_4383751150204030030[221] = 0; - out_4383751150204030030[222] = 0; - out_4383751150204030030[223] = 0; - out_4383751150204030030[224] = 0; - out_4383751150204030030[225] = 0; - out_4383751150204030030[226] = 0; - out_4383751150204030030[227] = 0; - out_4383751150204030030[228] = 1; - out_4383751150204030030[229] = 0; - out_4383751150204030030[230] = 0; - out_4383751150204030030[231] = 0; - out_4383751150204030030[232] = 0; - out_4383751150204030030[233] = 0; - out_4383751150204030030[234] = 0; - out_4383751150204030030[235] = 0; - out_4383751150204030030[236] = 0; - out_4383751150204030030[237] = 0; - out_4383751150204030030[238] = 0; - out_4383751150204030030[239] = 0; - out_4383751150204030030[240] = 0; - out_4383751150204030030[241] = 0; - out_4383751150204030030[242] = 0; - out_4383751150204030030[243] = 0; - out_4383751150204030030[244] = 0; - out_4383751150204030030[245] = 0; - out_4383751150204030030[246] = 0; - out_4383751150204030030[247] = 1; - out_4383751150204030030[248] = 0; - out_4383751150204030030[249] = 0; - out_4383751150204030030[250] = 0; - out_4383751150204030030[251] = 0; - out_4383751150204030030[252] = 0; - out_4383751150204030030[253] = 0; - out_4383751150204030030[254] = 0; - out_4383751150204030030[255] = 0; - out_4383751150204030030[256] = 0; - out_4383751150204030030[257] = 0; - out_4383751150204030030[258] = 0; - out_4383751150204030030[259] = 0; - out_4383751150204030030[260] = 0; - out_4383751150204030030[261] = 0; - out_4383751150204030030[262] = 0; - out_4383751150204030030[263] = 0; - out_4383751150204030030[264] = 0; - out_4383751150204030030[265] = 0; - out_4383751150204030030[266] = 1; - out_4383751150204030030[267] = 0; - out_4383751150204030030[268] = 0; - out_4383751150204030030[269] = 0; - out_4383751150204030030[270] = 0; - out_4383751150204030030[271] = 0; - out_4383751150204030030[272] = 0; - out_4383751150204030030[273] = 0; - out_4383751150204030030[274] = 0; - out_4383751150204030030[275] = 0; - out_4383751150204030030[276] = 0; - out_4383751150204030030[277] = 0; - out_4383751150204030030[278] = 0; - out_4383751150204030030[279] = 0; - out_4383751150204030030[280] = 0; - out_4383751150204030030[281] = 0; - out_4383751150204030030[282] = 0; - out_4383751150204030030[283] = 0; - out_4383751150204030030[284] = 0; - out_4383751150204030030[285] = 1; - out_4383751150204030030[286] = 0; - out_4383751150204030030[287] = 0; - out_4383751150204030030[288] = 0; - out_4383751150204030030[289] = 0; - out_4383751150204030030[290] = 0; - out_4383751150204030030[291] = 0; - out_4383751150204030030[292] = 0; - out_4383751150204030030[293] = 0; - out_4383751150204030030[294] = 0; - out_4383751150204030030[295] = 0; - out_4383751150204030030[296] = 0; - out_4383751150204030030[297] = 0; - out_4383751150204030030[298] = 0; - out_4383751150204030030[299] = 0; - out_4383751150204030030[300] = 0; - out_4383751150204030030[301] = 0; - out_4383751150204030030[302] = 0; - out_4383751150204030030[303] = 0; - out_4383751150204030030[304] = 1; - out_4383751150204030030[305] = 0; - out_4383751150204030030[306] = 0; - out_4383751150204030030[307] = 0; - out_4383751150204030030[308] = 0; - out_4383751150204030030[309] = 0; - out_4383751150204030030[310] = 0; - out_4383751150204030030[311] = 0; - out_4383751150204030030[312] = 0; - out_4383751150204030030[313] = 0; - out_4383751150204030030[314] = 0; - out_4383751150204030030[315] = 0; - out_4383751150204030030[316] = 0; - out_4383751150204030030[317] = 0; - out_4383751150204030030[318] = 0; - out_4383751150204030030[319] = 0; - out_4383751150204030030[320] = 0; - out_4383751150204030030[321] = 0; - out_4383751150204030030[322] = 0; - out_4383751150204030030[323] = 1; +void F_fun(double *state, double dt, double *out_582733379073092242) { + out_582733379073092242[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_582733379073092242[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_582733379073092242[2] = 0; + out_582733379073092242[3] = 0; + out_582733379073092242[4] = 0; + out_582733379073092242[5] = 0; + out_582733379073092242[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_582733379073092242[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_582733379073092242[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_582733379073092242[9] = 0; + out_582733379073092242[10] = 0; + out_582733379073092242[11] = 0; + out_582733379073092242[12] = 0; + out_582733379073092242[13] = 0; + out_582733379073092242[14] = 0; + out_582733379073092242[15] = 0; + out_582733379073092242[16] = 0; + out_582733379073092242[17] = 0; + out_582733379073092242[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_582733379073092242[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_582733379073092242[20] = 0; + out_582733379073092242[21] = 0; + out_582733379073092242[22] = 0; + out_582733379073092242[23] = 0; + out_582733379073092242[24] = 0; + out_582733379073092242[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_582733379073092242[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_582733379073092242[27] = 0; + out_582733379073092242[28] = 0; + out_582733379073092242[29] = 0; + out_582733379073092242[30] = 0; + out_582733379073092242[31] = 0; + out_582733379073092242[32] = 0; + out_582733379073092242[33] = 0; + out_582733379073092242[34] = 0; + out_582733379073092242[35] = 0; + out_582733379073092242[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_582733379073092242[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_582733379073092242[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_582733379073092242[39] = 0; + out_582733379073092242[40] = 0; + out_582733379073092242[41] = 0; + out_582733379073092242[42] = 0; + out_582733379073092242[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_582733379073092242[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_582733379073092242[45] = 0; + out_582733379073092242[46] = 0; + out_582733379073092242[47] = 0; + out_582733379073092242[48] = 0; + out_582733379073092242[49] = 0; + out_582733379073092242[50] = 0; + out_582733379073092242[51] = 0; + out_582733379073092242[52] = 0; + out_582733379073092242[53] = 0; + out_582733379073092242[54] = 0; + out_582733379073092242[55] = 0; + out_582733379073092242[56] = 0; + out_582733379073092242[57] = 1; + out_582733379073092242[58] = 0; + out_582733379073092242[59] = 0; + out_582733379073092242[60] = 0; + out_582733379073092242[61] = 0; + out_582733379073092242[62] = 0; + out_582733379073092242[63] = 0; + out_582733379073092242[64] = 0; + out_582733379073092242[65] = 0; + out_582733379073092242[66] = dt; + out_582733379073092242[67] = 0; + out_582733379073092242[68] = 0; + out_582733379073092242[69] = 0; + out_582733379073092242[70] = 0; + out_582733379073092242[71] = 0; + out_582733379073092242[72] = 0; + out_582733379073092242[73] = 0; + out_582733379073092242[74] = 0; + out_582733379073092242[75] = 0; + out_582733379073092242[76] = 1; + out_582733379073092242[77] = 0; + out_582733379073092242[78] = 0; + out_582733379073092242[79] = 0; + out_582733379073092242[80] = 0; + out_582733379073092242[81] = 0; + out_582733379073092242[82] = 0; + out_582733379073092242[83] = 0; + out_582733379073092242[84] = 0; + out_582733379073092242[85] = dt; + out_582733379073092242[86] = 0; + out_582733379073092242[87] = 0; + out_582733379073092242[88] = 0; + out_582733379073092242[89] = 0; + out_582733379073092242[90] = 0; + out_582733379073092242[91] = 0; + out_582733379073092242[92] = 0; + out_582733379073092242[93] = 0; + out_582733379073092242[94] = 0; + out_582733379073092242[95] = 1; + out_582733379073092242[96] = 0; + out_582733379073092242[97] = 0; + out_582733379073092242[98] = 0; + out_582733379073092242[99] = 0; + out_582733379073092242[100] = 0; + out_582733379073092242[101] = 0; + out_582733379073092242[102] = 0; + out_582733379073092242[103] = 0; + out_582733379073092242[104] = dt; + out_582733379073092242[105] = 0; + out_582733379073092242[106] = 0; + out_582733379073092242[107] = 0; + out_582733379073092242[108] = 0; + out_582733379073092242[109] = 0; + out_582733379073092242[110] = 0; + out_582733379073092242[111] = 0; + out_582733379073092242[112] = 0; + out_582733379073092242[113] = 0; + out_582733379073092242[114] = 1; + out_582733379073092242[115] = 0; + out_582733379073092242[116] = 0; + out_582733379073092242[117] = 0; + out_582733379073092242[118] = 0; + out_582733379073092242[119] = 0; + out_582733379073092242[120] = 0; + out_582733379073092242[121] = 0; + out_582733379073092242[122] = 0; + out_582733379073092242[123] = 0; + out_582733379073092242[124] = 0; + out_582733379073092242[125] = 0; + out_582733379073092242[126] = 0; + out_582733379073092242[127] = 0; + out_582733379073092242[128] = 0; + out_582733379073092242[129] = 0; + out_582733379073092242[130] = 0; + out_582733379073092242[131] = 0; + out_582733379073092242[132] = 0; + out_582733379073092242[133] = 1; + out_582733379073092242[134] = 0; + out_582733379073092242[135] = 0; + out_582733379073092242[136] = 0; + out_582733379073092242[137] = 0; + out_582733379073092242[138] = 0; + out_582733379073092242[139] = 0; + out_582733379073092242[140] = 0; + out_582733379073092242[141] = 0; + out_582733379073092242[142] = 0; + out_582733379073092242[143] = 0; + out_582733379073092242[144] = 0; + out_582733379073092242[145] = 0; + out_582733379073092242[146] = 0; + out_582733379073092242[147] = 0; + out_582733379073092242[148] = 0; + out_582733379073092242[149] = 0; + out_582733379073092242[150] = 0; + out_582733379073092242[151] = 0; + out_582733379073092242[152] = 1; + out_582733379073092242[153] = 0; + out_582733379073092242[154] = 0; + out_582733379073092242[155] = 0; + out_582733379073092242[156] = 0; + out_582733379073092242[157] = 0; + out_582733379073092242[158] = 0; + out_582733379073092242[159] = 0; + out_582733379073092242[160] = 0; + out_582733379073092242[161] = 0; + out_582733379073092242[162] = 0; + out_582733379073092242[163] = 0; + out_582733379073092242[164] = 0; + out_582733379073092242[165] = 0; + out_582733379073092242[166] = 0; + out_582733379073092242[167] = 0; + out_582733379073092242[168] = 0; + out_582733379073092242[169] = 0; + out_582733379073092242[170] = 0; + out_582733379073092242[171] = 1; + out_582733379073092242[172] = 0; + out_582733379073092242[173] = 0; + out_582733379073092242[174] = 0; + out_582733379073092242[175] = 0; + out_582733379073092242[176] = 0; + out_582733379073092242[177] = 0; + out_582733379073092242[178] = 0; + out_582733379073092242[179] = 0; + out_582733379073092242[180] = 0; + out_582733379073092242[181] = 0; + out_582733379073092242[182] = 0; + out_582733379073092242[183] = 0; + out_582733379073092242[184] = 0; + out_582733379073092242[185] = 0; + out_582733379073092242[186] = 0; + out_582733379073092242[187] = 0; + out_582733379073092242[188] = 0; + out_582733379073092242[189] = 0; + out_582733379073092242[190] = 1; + out_582733379073092242[191] = 0; + out_582733379073092242[192] = 0; + out_582733379073092242[193] = 0; + out_582733379073092242[194] = 0; + out_582733379073092242[195] = 0; + out_582733379073092242[196] = 0; + out_582733379073092242[197] = 0; + out_582733379073092242[198] = 0; + out_582733379073092242[199] = 0; + out_582733379073092242[200] = 0; + out_582733379073092242[201] = 0; + out_582733379073092242[202] = 0; + out_582733379073092242[203] = 0; + out_582733379073092242[204] = 0; + out_582733379073092242[205] = 0; + out_582733379073092242[206] = 0; + out_582733379073092242[207] = 0; + out_582733379073092242[208] = 0; + out_582733379073092242[209] = 1; + out_582733379073092242[210] = 0; + out_582733379073092242[211] = 0; + out_582733379073092242[212] = 0; + out_582733379073092242[213] = 0; + out_582733379073092242[214] = 0; + out_582733379073092242[215] = 0; + out_582733379073092242[216] = 0; + out_582733379073092242[217] = 0; + out_582733379073092242[218] = 0; + out_582733379073092242[219] = 0; + out_582733379073092242[220] = 0; + out_582733379073092242[221] = 0; + out_582733379073092242[222] = 0; + out_582733379073092242[223] = 0; + out_582733379073092242[224] = 0; + out_582733379073092242[225] = 0; + out_582733379073092242[226] = 0; + out_582733379073092242[227] = 0; + out_582733379073092242[228] = 1; + out_582733379073092242[229] = 0; + out_582733379073092242[230] = 0; + out_582733379073092242[231] = 0; + out_582733379073092242[232] = 0; + out_582733379073092242[233] = 0; + out_582733379073092242[234] = 0; + out_582733379073092242[235] = 0; + out_582733379073092242[236] = 0; + out_582733379073092242[237] = 0; + out_582733379073092242[238] = 0; + out_582733379073092242[239] = 0; + out_582733379073092242[240] = 0; + out_582733379073092242[241] = 0; + out_582733379073092242[242] = 0; + out_582733379073092242[243] = 0; + out_582733379073092242[244] = 0; + out_582733379073092242[245] = 0; + out_582733379073092242[246] = 0; + out_582733379073092242[247] = 1; + out_582733379073092242[248] = 0; + out_582733379073092242[249] = 0; + out_582733379073092242[250] = 0; + out_582733379073092242[251] = 0; + out_582733379073092242[252] = 0; + out_582733379073092242[253] = 0; + out_582733379073092242[254] = 0; + out_582733379073092242[255] = 0; + out_582733379073092242[256] = 0; + out_582733379073092242[257] = 0; + out_582733379073092242[258] = 0; + out_582733379073092242[259] = 0; + out_582733379073092242[260] = 0; + out_582733379073092242[261] = 0; + out_582733379073092242[262] = 0; + out_582733379073092242[263] = 0; + out_582733379073092242[264] = 0; + out_582733379073092242[265] = 0; + out_582733379073092242[266] = 1; + out_582733379073092242[267] = 0; + out_582733379073092242[268] = 0; + out_582733379073092242[269] = 0; + out_582733379073092242[270] = 0; + out_582733379073092242[271] = 0; + out_582733379073092242[272] = 0; + out_582733379073092242[273] = 0; + out_582733379073092242[274] = 0; + out_582733379073092242[275] = 0; + out_582733379073092242[276] = 0; + out_582733379073092242[277] = 0; + out_582733379073092242[278] = 0; + out_582733379073092242[279] = 0; + out_582733379073092242[280] = 0; + out_582733379073092242[281] = 0; + out_582733379073092242[282] = 0; + out_582733379073092242[283] = 0; + out_582733379073092242[284] = 0; + out_582733379073092242[285] = 1; + out_582733379073092242[286] = 0; + out_582733379073092242[287] = 0; + out_582733379073092242[288] = 0; + out_582733379073092242[289] = 0; + out_582733379073092242[290] = 0; + out_582733379073092242[291] = 0; + out_582733379073092242[292] = 0; + out_582733379073092242[293] = 0; + out_582733379073092242[294] = 0; + out_582733379073092242[295] = 0; + out_582733379073092242[296] = 0; + out_582733379073092242[297] = 0; + out_582733379073092242[298] = 0; + out_582733379073092242[299] = 0; + out_582733379073092242[300] = 0; + out_582733379073092242[301] = 0; + out_582733379073092242[302] = 0; + out_582733379073092242[303] = 0; + out_582733379073092242[304] = 1; + out_582733379073092242[305] = 0; + out_582733379073092242[306] = 0; + out_582733379073092242[307] = 0; + out_582733379073092242[308] = 0; + out_582733379073092242[309] = 0; + out_582733379073092242[310] = 0; + out_582733379073092242[311] = 0; + out_582733379073092242[312] = 0; + out_582733379073092242[313] = 0; + out_582733379073092242[314] = 0; + out_582733379073092242[315] = 0; + out_582733379073092242[316] = 0; + out_582733379073092242[317] = 0; + out_582733379073092242[318] = 0; + out_582733379073092242[319] = 0; + out_582733379073092242[320] = 0; + out_582733379073092242[321] = 0; + out_582733379073092242[322] = 0; + out_582733379073092242[323] = 1; } -void h_4(double *state, double *unused, double *out_878843833946471253) { - out_878843833946471253[0] = state[6] + state[9]; - out_878843833946471253[1] = state[7] + state[10]; - out_878843833946471253[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_959952002277923346) { + out_959952002277923346[0] = state[6] + state[9]; + out_959952002277923346[1] = state[7] + state[10]; + out_959952002277923346[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_7970215192388091303) { - out_7970215192388091303[0] = 0; - out_7970215192388091303[1] = 0; - out_7970215192388091303[2] = 0; - out_7970215192388091303[3] = 0; - out_7970215192388091303[4] = 0; - out_7970215192388091303[5] = 0; - out_7970215192388091303[6] = 1; - out_7970215192388091303[7] = 0; - out_7970215192388091303[8] = 0; - out_7970215192388091303[9] = 1; - out_7970215192388091303[10] = 0; - out_7970215192388091303[11] = 0; - out_7970215192388091303[12] = 0; - out_7970215192388091303[13] = 0; - out_7970215192388091303[14] = 0; - out_7970215192388091303[15] = 0; - out_7970215192388091303[16] = 0; - out_7970215192388091303[17] = 0; - out_7970215192388091303[18] = 0; - out_7970215192388091303[19] = 0; - out_7970215192388091303[20] = 0; - out_7970215192388091303[21] = 0; - out_7970215192388091303[22] = 0; - out_7970215192388091303[23] = 0; - out_7970215192388091303[24] = 0; - out_7970215192388091303[25] = 1; - out_7970215192388091303[26] = 0; - out_7970215192388091303[27] = 0; - out_7970215192388091303[28] = 1; - out_7970215192388091303[29] = 0; - out_7970215192388091303[30] = 0; - out_7970215192388091303[31] = 0; - out_7970215192388091303[32] = 0; - out_7970215192388091303[33] = 0; - out_7970215192388091303[34] = 0; - out_7970215192388091303[35] = 0; - out_7970215192388091303[36] = 0; - out_7970215192388091303[37] = 0; - out_7970215192388091303[38] = 0; - out_7970215192388091303[39] = 0; - out_7970215192388091303[40] = 0; - out_7970215192388091303[41] = 0; - out_7970215192388091303[42] = 0; - out_7970215192388091303[43] = 0; - out_7970215192388091303[44] = 1; - out_7970215192388091303[45] = 0; - out_7970215192388091303[46] = 0; - out_7970215192388091303[47] = 1; - out_7970215192388091303[48] = 0; - out_7970215192388091303[49] = 0; - out_7970215192388091303[50] = 0; - out_7970215192388091303[51] = 0; - out_7970215192388091303[52] = 0; - out_7970215192388091303[53] = 0; +void H_4(double *state, double *unused, double *out_1467574399937880926) { + out_1467574399937880926[0] = 0; + out_1467574399937880926[1] = 0; + out_1467574399937880926[2] = 0; + out_1467574399937880926[3] = 0; + out_1467574399937880926[4] = 0; + out_1467574399937880926[5] = 0; + out_1467574399937880926[6] = 1; + out_1467574399937880926[7] = 0; + out_1467574399937880926[8] = 0; + out_1467574399937880926[9] = 1; + out_1467574399937880926[10] = 0; + out_1467574399937880926[11] = 0; + out_1467574399937880926[12] = 0; + out_1467574399937880926[13] = 0; + out_1467574399937880926[14] = 0; + out_1467574399937880926[15] = 0; + out_1467574399937880926[16] = 0; + out_1467574399937880926[17] = 0; + out_1467574399937880926[18] = 0; + out_1467574399937880926[19] = 0; + out_1467574399937880926[20] = 0; + out_1467574399937880926[21] = 0; + out_1467574399937880926[22] = 0; + out_1467574399937880926[23] = 0; + out_1467574399937880926[24] = 0; + out_1467574399937880926[25] = 1; + out_1467574399937880926[26] = 0; + out_1467574399937880926[27] = 0; + out_1467574399937880926[28] = 1; + out_1467574399937880926[29] = 0; + out_1467574399937880926[30] = 0; + out_1467574399937880926[31] = 0; + out_1467574399937880926[32] = 0; + out_1467574399937880926[33] = 0; + out_1467574399937880926[34] = 0; + out_1467574399937880926[35] = 0; + out_1467574399937880926[36] = 0; + out_1467574399937880926[37] = 0; + out_1467574399937880926[38] = 0; + out_1467574399937880926[39] = 0; + out_1467574399937880926[40] = 0; + out_1467574399937880926[41] = 0; + out_1467574399937880926[42] = 0; + out_1467574399937880926[43] = 0; + out_1467574399937880926[44] = 1; + out_1467574399937880926[45] = 0; + out_1467574399937880926[46] = 0; + out_1467574399937880926[47] = 1; + out_1467574399937880926[48] = 0; + out_1467574399937880926[49] = 0; + out_1467574399937880926[50] = 0; + out_1467574399937880926[51] = 0; + out_1467574399937880926[52] = 0; + out_1467574399937880926[53] = 0; } -void h_10(double *state, double *unused, double *out_1191517594273352576) { - out_1191517594273352576[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_1191517594273352576[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_1191517594273352576[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_315403293584823803) { + out_315403293584823803[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_315403293584823803[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_315403293584823803[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_7914634342859917414) { - out_7914634342859917414[0] = 0; - out_7914634342859917414[1] = 9.8100000000000005*cos(state[1]); - out_7914634342859917414[2] = 0; - out_7914634342859917414[3] = 0; - out_7914634342859917414[4] = -state[8]; - out_7914634342859917414[5] = state[7]; - out_7914634342859917414[6] = 0; - out_7914634342859917414[7] = state[5]; - out_7914634342859917414[8] = -state[4]; - out_7914634342859917414[9] = 0; - out_7914634342859917414[10] = 0; - out_7914634342859917414[11] = 0; - out_7914634342859917414[12] = 1; - out_7914634342859917414[13] = 0; - out_7914634342859917414[14] = 0; - out_7914634342859917414[15] = 1; - out_7914634342859917414[16] = 0; - out_7914634342859917414[17] = 0; - out_7914634342859917414[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_7914634342859917414[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_7914634342859917414[20] = 0; - out_7914634342859917414[21] = state[8]; - out_7914634342859917414[22] = 0; - out_7914634342859917414[23] = -state[6]; - out_7914634342859917414[24] = -state[5]; - out_7914634342859917414[25] = 0; - out_7914634342859917414[26] = state[3]; - out_7914634342859917414[27] = 0; - out_7914634342859917414[28] = 0; - out_7914634342859917414[29] = 0; - out_7914634342859917414[30] = 0; - out_7914634342859917414[31] = 1; - out_7914634342859917414[32] = 0; - out_7914634342859917414[33] = 0; - out_7914634342859917414[34] = 1; - out_7914634342859917414[35] = 0; - out_7914634342859917414[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_7914634342859917414[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_7914634342859917414[38] = 0; - out_7914634342859917414[39] = -state[7]; - out_7914634342859917414[40] = state[6]; - out_7914634342859917414[41] = 0; - out_7914634342859917414[42] = state[4]; - out_7914634342859917414[43] = -state[3]; - out_7914634342859917414[44] = 0; - out_7914634342859917414[45] = 0; - out_7914634342859917414[46] = 0; - out_7914634342859917414[47] = 0; - out_7914634342859917414[48] = 0; - out_7914634342859917414[49] = 0; - out_7914634342859917414[50] = 1; - out_7914634342859917414[51] = 0; - out_7914634342859917414[52] = 0; - out_7914634342859917414[53] = 1; +void H_10(double *state, double *unused, double *out_2334763542090568144) { + out_2334763542090568144[0] = 0; + out_2334763542090568144[1] = 9.8100000000000005*cos(state[1]); + out_2334763542090568144[2] = 0; + out_2334763542090568144[3] = 0; + out_2334763542090568144[4] = -state[8]; + out_2334763542090568144[5] = state[7]; + out_2334763542090568144[6] = 0; + out_2334763542090568144[7] = state[5]; + out_2334763542090568144[8] = -state[4]; + out_2334763542090568144[9] = 0; + out_2334763542090568144[10] = 0; + out_2334763542090568144[11] = 0; + out_2334763542090568144[12] = 1; + out_2334763542090568144[13] = 0; + out_2334763542090568144[14] = 0; + out_2334763542090568144[15] = 1; + out_2334763542090568144[16] = 0; + out_2334763542090568144[17] = 0; + out_2334763542090568144[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_2334763542090568144[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_2334763542090568144[20] = 0; + out_2334763542090568144[21] = state[8]; + out_2334763542090568144[22] = 0; + out_2334763542090568144[23] = -state[6]; + out_2334763542090568144[24] = -state[5]; + out_2334763542090568144[25] = 0; + out_2334763542090568144[26] = state[3]; + out_2334763542090568144[27] = 0; + out_2334763542090568144[28] = 0; + out_2334763542090568144[29] = 0; + out_2334763542090568144[30] = 0; + out_2334763542090568144[31] = 1; + out_2334763542090568144[32] = 0; + out_2334763542090568144[33] = 0; + out_2334763542090568144[34] = 1; + out_2334763542090568144[35] = 0; + out_2334763542090568144[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_2334763542090568144[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_2334763542090568144[38] = 0; + out_2334763542090568144[39] = -state[7]; + out_2334763542090568144[40] = state[6]; + out_2334763542090568144[41] = 0; + out_2334763542090568144[42] = state[4]; + out_2334763542090568144[43] = -state[3]; + out_2334763542090568144[44] = 0; + out_2334763542090568144[45] = 0; + out_2334763542090568144[46] = 0; + out_2334763542090568144[47] = 0; + out_2334763542090568144[48] = 0; + out_2334763542090568144[49] = 0; + out_2334763542090568144[50] = 1; + out_2334763542090568144[51] = 0; + out_2334763542090568144[52] = 0; + out_2334763542090568144[53] = 1; } -void h_13(double *state, double *unused, double *out_5300060524255047032) { - out_5300060524255047032[0] = state[3]; - out_5300060524255047032[1] = state[4]; - out_5300060524255047032[2] = state[5]; +void h_13(double *state, double *unused, double *out_5013407308002664142) { + out_5013407308002664142[0] = state[3]; + out_5013407308002664142[1] = state[4]; + out_5013407308002664142[2] = state[5]; } -void H_13(double *state, double *unused, double *out_7264255055989127512) { - out_7264255055989127512[0] = 0; - out_7264255055989127512[1] = 0; - out_7264255055989127512[2] = 0; - out_7264255055989127512[3] = 1; - out_7264255055989127512[4] = 0; - out_7264255055989127512[5] = 0; - out_7264255055989127512[6] = 0; - out_7264255055989127512[7] = 0; - out_7264255055989127512[8] = 0; - out_7264255055989127512[9] = 0; - out_7264255055989127512[10] = 0; - out_7264255055989127512[11] = 0; - out_7264255055989127512[12] = 0; - out_7264255055989127512[13] = 0; - out_7264255055989127512[14] = 0; - out_7264255055989127512[15] = 0; - out_7264255055989127512[16] = 0; - out_7264255055989127512[17] = 0; - out_7264255055989127512[18] = 0; - out_7264255055989127512[19] = 0; - out_7264255055989127512[20] = 0; - out_7264255055989127512[21] = 0; - out_7264255055989127512[22] = 1; - out_7264255055989127512[23] = 0; - out_7264255055989127512[24] = 0; - out_7264255055989127512[25] = 0; - out_7264255055989127512[26] = 0; - out_7264255055989127512[27] = 0; - out_7264255055989127512[28] = 0; - out_7264255055989127512[29] = 0; - out_7264255055989127512[30] = 0; - out_7264255055989127512[31] = 0; - out_7264255055989127512[32] = 0; - out_7264255055989127512[33] = 0; - out_7264255055989127512[34] = 0; - out_7264255055989127512[35] = 0; - out_7264255055989127512[36] = 0; - out_7264255055989127512[37] = 0; - out_7264255055989127512[38] = 0; - out_7264255055989127512[39] = 0; - out_7264255055989127512[40] = 0; - out_7264255055989127512[41] = 1; - out_7264255055989127512[42] = 0; - out_7264255055989127512[43] = 0; - out_7264255055989127512[44] = 0; - out_7264255055989127512[45] = 0; - out_7264255055989127512[46] = 0; - out_7264255055989127512[47] = 0; - out_7264255055989127512[48] = 0; - out_7264255055989127512[49] = 0; - out_7264255055989127512[50] = 0; - out_7264255055989127512[51] = 0; - out_7264255055989127512[52] = 0; - out_7264255055989127512[53] = 0; +void H_13(double *state, double *unused, double *out_4679848225270213727) { + out_4679848225270213727[0] = 0; + out_4679848225270213727[1] = 0; + out_4679848225270213727[2] = 0; + out_4679848225270213727[3] = 1; + out_4679848225270213727[4] = 0; + out_4679848225270213727[5] = 0; + out_4679848225270213727[6] = 0; + out_4679848225270213727[7] = 0; + out_4679848225270213727[8] = 0; + out_4679848225270213727[9] = 0; + out_4679848225270213727[10] = 0; + out_4679848225270213727[11] = 0; + out_4679848225270213727[12] = 0; + out_4679848225270213727[13] = 0; + out_4679848225270213727[14] = 0; + out_4679848225270213727[15] = 0; + out_4679848225270213727[16] = 0; + out_4679848225270213727[17] = 0; + out_4679848225270213727[18] = 0; + out_4679848225270213727[19] = 0; + out_4679848225270213727[20] = 0; + out_4679848225270213727[21] = 0; + out_4679848225270213727[22] = 1; + out_4679848225270213727[23] = 0; + out_4679848225270213727[24] = 0; + out_4679848225270213727[25] = 0; + out_4679848225270213727[26] = 0; + out_4679848225270213727[27] = 0; + out_4679848225270213727[28] = 0; + out_4679848225270213727[29] = 0; + out_4679848225270213727[30] = 0; + out_4679848225270213727[31] = 0; + out_4679848225270213727[32] = 0; + out_4679848225270213727[33] = 0; + out_4679848225270213727[34] = 0; + out_4679848225270213727[35] = 0; + out_4679848225270213727[36] = 0; + out_4679848225270213727[37] = 0; + out_4679848225270213727[38] = 0; + out_4679848225270213727[39] = 0; + out_4679848225270213727[40] = 0; + out_4679848225270213727[41] = 1; + out_4679848225270213727[42] = 0; + out_4679848225270213727[43] = 0; + out_4679848225270213727[44] = 0; + out_4679848225270213727[45] = 0; + out_4679848225270213727[46] = 0; + out_4679848225270213727[47] = 0; + out_4679848225270213727[48] = 0; + out_4679848225270213727[49] = 0; + out_4679848225270213727[50] = 0; + out_4679848225270213727[51] = 0; + out_4679848225270213727[52] = 0; + out_4679848225270213727[53] = 0; } -void h_14(double *state, double *unused, double *out_3965460404426586337) { - out_3965460404426586337[0] = state[6]; - out_3965460404426586337[1] = state[7]; - out_3965460404426586337[2] = state[8]; +void h_14(double *state, double *unused, double *out_6338305732819674146) { + out_6338305732819674146[0] = state[6]; + out_6338305732819674146[1] = state[7]; + out_6338305732819674146[2] = state[8]; } -void H_14(double *state, double *unused, double *out_6513288024981975784) { - out_6513288024981975784[0] = 0; - out_6513288024981975784[1] = 0; - out_6513288024981975784[2] = 0; - out_6513288024981975784[3] = 0; - out_6513288024981975784[4] = 0; - out_6513288024981975784[5] = 0; - out_6513288024981975784[6] = 1; - out_6513288024981975784[7] = 0; - out_6513288024981975784[8] = 0; - out_6513288024981975784[9] = 0; - out_6513288024981975784[10] = 0; - out_6513288024981975784[11] = 0; - out_6513288024981975784[12] = 0; - out_6513288024981975784[13] = 0; - out_6513288024981975784[14] = 0; - out_6513288024981975784[15] = 0; - out_6513288024981975784[16] = 0; - out_6513288024981975784[17] = 0; - out_6513288024981975784[18] = 0; - out_6513288024981975784[19] = 0; - out_6513288024981975784[20] = 0; - out_6513288024981975784[21] = 0; - out_6513288024981975784[22] = 0; - out_6513288024981975784[23] = 0; - out_6513288024981975784[24] = 0; - out_6513288024981975784[25] = 1; - out_6513288024981975784[26] = 0; - out_6513288024981975784[27] = 0; - out_6513288024981975784[28] = 0; - out_6513288024981975784[29] = 0; - out_6513288024981975784[30] = 0; - out_6513288024981975784[31] = 0; - out_6513288024981975784[32] = 0; - out_6513288024981975784[33] = 0; - out_6513288024981975784[34] = 0; - out_6513288024981975784[35] = 0; - out_6513288024981975784[36] = 0; - out_6513288024981975784[37] = 0; - out_6513288024981975784[38] = 0; - out_6513288024981975784[39] = 0; - out_6513288024981975784[40] = 0; - out_6513288024981975784[41] = 0; - out_6513288024981975784[42] = 0; - out_6513288024981975784[43] = 0; - out_6513288024981975784[44] = 1; - out_6513288024981975784[45] = 0; - out_6513288024981975784[46] = 0; - out_6513288024981975784[47] = 0; - out_6513288024981975784[48] = 0; - out_6513288024981975784[49] = 0; - out_6513288024981975784[50] = 0; - out_6513288024981975784[51] = 0; - out_6513288024981975784[52] = 0; - out_6513288024981975784[53] = 0; +void H_14(double *state, double *unused, double *out_1615214032357491370) { + out_1615214032357491370[0] = 0; + out_1615214032357491370[1] = 0; + out_1615214032357491370[2] = 0; + out_1615214032357491370[3] = 0; + out_1615214032357491370[4] = 0; + out_1615214032357491370[5] = 0; + out_1615214032357491370[6] = 1; + out_1615214032357491370[7] = 0; + out_1615214032357491370[8] = 0; + out_1615214032357491370[9] = 0; + out_1615214032357491370[10] = 0; + out_1615214032357491370[11] = 0; + out_1615214032357491370[12] = 0; + out_1615214032357491370[13] = 0; + out_1615214032357491370[14] = 0; + out_1615214032357491370[15] = 0; + out_1615214032357491370[16] = 0; + out_1615214032357491370[17] = 0; + out_1615214032357491370[18] = 0; + out_1615214032357491370[19] = 0; + out_1615214032357491370[20] = 0; + out_1615214032357491370[21] = 0; + out_1615214032357491370[22] = 0; + out_1615214032357491370[23] = 0; + out_1615214032357491370[24] = 0; + out_1615214032357491370[25] = 1; + out_1615214032357491370[26] = 0; + out_1615214032357491370[27] = 0; + out_1615214032357491370[28] = 0; + out_1615214032357491370[29] = 0; + out_1615214032357491370[30] = 0; + out_1615214032357491370[31] = 0; + out_1615214032357491370[32] = 0; + out_1615214032357491370[33] = 0; + out_1615214032357491370[34] = 0; + out_1615214032357491370[35] = 0; + out_1615214032357491370[36] = 0; + out_1615214032357491370[37] = 0; + out_1615214032357491370[38] = 0; + out_1615214032357491370[39] = 0; + out_1615214032357491370[40] = 0; + out_1615214032357491370[41] = 0; + out_1615214032357491370[42] = 0; + out_1615214032357491370[43] = 0; + out_1615214032357491370[44] = 1; + out_1615214032357491370[45] = 0; + out_1615214032357491370[46] = 0; + out_1615214032357491370[47] = 0; + out_1615214032357491370[48] = 0; + out_1615214032357491370[49] = 0; + out_1615214032357491370[50] = 0; + out_1615214032357491370[51] = 0; + out_1615214032357491370[52] = 0; + out_1615214032357491370[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_8207359146711228947) { - err_fun(nom_x, delta_x, out_8207359146711228947); +void pose_err_fun(double *nom_x, double *delta_x, double *out_867342344380239798) { + err_fun(nom_x, delta_x, out_867342344380239798); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8369219426901598353) { - inv_err_fun(nom_x, true_x, out_8369219426901598353); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2946458972277190645) { + inv_err_fun(nom_x, true_x, out_2946458972277190645); } -void pose_H_mod_fun(double *state, double *out_7202306802530511060) { - H_mod_fun(state, out_7202306802530511060); +void pose_H_mod_fun(double *state, double *out_8034808007713849348) { + H_mod_fun(state, out_8034808007713849348); } -void pose_f_fun(double *state, double dt, double *out_4961001892384611064) { - f_fun(state, dt, out_4961001892384611064); +void pose_f_fun(double *state, double dt, double *out_5448742405462655453) { + f_fun(state, dt, out_5448742405462655453); } -void pose_F_fun(double *state, double dt, double *out_4383751150204030030) { - F_fun(state, dt, out_4383751150204030030); +void pose_F_fun(double *state, double dt, double *out_582733379073092242) { + F_fun(state, dt, out_582733379073092242); } -void pose_h_4(double *state, double *unused, double *out_878843833946471253) { - h_4(state, unused, out_878843833946471253); +void pose_h_4(double *state, double *unused, double *out_959952002277923346) { + h_4(state, unused, out_959952002277923346); } -void pose_H_4(double *state, double *unused, double *out_7970215192388091303) { - H_4(state, unused, out_7970215192388091303); +void pose_H_4(double *state, double *unused, double *out_1467574399937880926) { + H_4(state, unused, out_1467574399937880926); } -void pose_h_10(double *state, double *unused, double *out_1191517594273352576) { - h_10(state, unused, out_1191517594273352576); +void pose_h_10(double *state, double *unused, double *out_315403293584823803) { + h_10(state, unused, out_315403293584823803); } -void pose_H_10(double *state, double *unused, double *out_7914634342859917414) { - H_10(state, unused, out_7914634342859917414); +void pose_H_10(double *state, double *unused, double *out_2334763542090568144) { + H_10(state, unused, out_2334763542090568144); } -void pose_h_13(double *state, double *unused, double *out_5300060524255047032) { - h_13(state, unused, out_5300060524255047032); +void pose_h_13(double *state, double *unused, double *out_5013407308002664142) { + h_13(state, unused, out_5013407308002664142); } -void pose_H_13(double *state, double *unused, double *out_7264255055989127512) { - H_13(state, unused, out_7264255055989127512); +void pose_H_13(double *state, double *unused, double *out_4679848225270213727) { + H_13(state, unused, out_4679848225270213727); } -void pose_h_14(double *state, double *unused, double *out_3965460404426586337) { - h_14(state, unused, out_3965460404426586337); +void pose_h_14(double *state, double *unused, double *out_6338305732819674146) { + h_14(state, unused, out_6338305732819674146); } -void pose_H_14(double *state, double *unused, double *out_6513288024981975784) { - H_14(state, unused, out_6513288024981975784); +void pose_H_14(double *state, double *unused, double *out_1615214032357491370) { + H_14(state, unused, out_1615214032357491370); } 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 106fa2f23..8543f3e81 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_8207359146711228947); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8369219426901598353); -void pose_H_mod_fun(double *state, double *out_7202306802530511060); -void pose_f_fun(double *state, double dt, double *out_4961001892384611064); -void pose_F_fun(double *state, double dt, double *out_4383751150204030030); -void pose_h_4(double *state, double *unused, double *out_878843833946471253); -void pose_H_4(double *state, double *unused, double *out_7970215192388091303); -void pose_h_10(double *state, double *unused, double *out_1191517594273352576); -void pose_H_10(double *state, double *unused, double *out_7914634342859917414); -void pose_h_13(double *state, double *unused, double *out_5300060524255047032); -void pose_H_13(double *state, double *unused, double *out_7264255055989127512); -void pose_h_14(double *state, double *unused, double *out_3965460404426586337); -void pose_H_14(double *state, double *unused, double *out_6513288024981975784); +void pose_err_fun(double *nom_x, double *delta_x, double *out_867342344380239798); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2946458972277190645); +void pose_H_mod_fun(double *state, double *out_8034808007713849348); +void pose_f_fun(double *state, double dt, double *out_5448742405462655453); +void pose_F_fun(double *state, double dt, double *out_582733379073092242); +void pose_h_4(double *state, double *unused, double *out_959952002277923346); +void pose_H_4(double *state, double *unused, double *out_1467574399937880926); +void pose_h_10(double *state, double *unused, double *out_315403293584823803); +void pose_H_10(double *state, double *unused, double *out_2334763542090568144); +void pose_h_13(double *state, double *unused, double *out_5013407308002664142); +void pose_H_13(double *state, double *unused, double *out_4679848225270213727); +void pose_h_14(double *state, double *unused, double *out_6338305732819674146); +void pose_H_14(double *state, double *unused, double *out_1615214032357491370); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 02a479a0a..4f47b968a 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ