diff --git a/common/libcommon.a b/common/libcommon.a index ce22a5c34..cb615e03b 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index ac3bb66f1..23ff8b9bf 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -380,7 +380,7 @@ inline static std::unordered_map keys = { {"MapsSelected", {PERSISTENT, STRING, "", "", 0}}, {"MapSpeedLimit", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}}, {"NavDesiresAllowed", {PERSISTENT, BOOL, "1", "0", 2}}, - {"NavLongitudinalAllowed", {PERSISTENT, BOOL, "0", "0", 2}}, + {"NavLongitudinalAllowed", {PERSISTENT, BOOL, "1", "0", 2}}, {"NavDestination", {PERSISTENT, STRING, "", ""}}, {"NavInstructionState", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON, "{}", "{}"}}, {"NextMapSpeedLimit", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index 29b795905..cc3fbfc93 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 44190fd61..ca37b2f7e 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 c8711e089..2cb052921 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 5acd8465a..2346a69c4 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 2b48a4ca5..740be5291 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 1ad369834..0dcb72e5d 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 f78f5ab16..bbdab3d90 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 8c4ecbdc7..2e7abf19a 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 8c4ecbdc7..2e7abf19a 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 64b1cc54c..d67210bd8 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 f78f5ab16..bbdab3d90 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 e29c0c3d0..e6de3dcfb 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-8e8225b6-DEBUG"; +const uint8_t gitversion[19] = "DEV-f06c82b2-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 0c64f8a71..10b6b5621 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 001e31245..588fdde8c 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 d4a062229..8a0d03426 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 4766d8e9a..dfd62a550 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 2f82c65fd..3787b1934 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 2d10af3c9..e524f33d4 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 0b0f5f7e4..9d166adb9 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 f8aca9717..e58c580ac 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 5217ac5a7..130234dd4 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 42d7a63c8..9c98a0b5f 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 86ed24b92..731997681 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 b8d1f17f9..44ae680fc 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 093eb4543..42207f1a2 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 0a516681a..4d0c0d58f 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 549bb6f88..4b78eaf50 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 8eb6a162f..5240b9da6 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 47147ada4..17540bacc 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 e3330d440..5ecc83522 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 c2ff5cede..5529997d7 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 2b47835aa..a26636ade 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 689135c5f..61eedf377 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-8e8225b6-DEBUG \ No newline at end of file +DEV-f06c82b2-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 9c42d6799..56108aa8b 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_2503888728252844227) { - out_2503888728252844227[0] = delta_x[0] + nom_x[0]; - out_2503888728252844227[1] = delta_x[1] + nom_x[1]; - out_2503888728252844227[2] = delta_x[2] + nom_x[2]; - out_2503888728252844227[3] = delta_x[3] + nom_x[3]; - out_2503888728252844227[4] = delta_x[4] + nom_x[4]; - out_2503888728252844227[5] = delta_x[5] + nom_x[5]; - out_2503888728252844227[6] = delta_x[6] + nom_x[6]; - out_2503888728252844227[7] = delta_x[7] + nom_x[7]; - out_2503888728252844227[8] = delta_x[8] + nom_x[8]; +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 inv_err_fun(double *nom_x, double *true_x, double *out_3671647430606809601) { - out_3671647430606809601[0] = -nom_x[0] + true_x[0]; - out_3671647430606809601[1] = -nom_x[1] + true_x[1]; - out_3671647430606809601[2] = -nom_x[2] + true_x[2]; - out_3671647430606809601[3] = -nom_x[3] + true_x[3]; - out_3671647430606809601[4] = -nom_x[4] + true_x[4]; - out_3671647430606809601[5] = -nom_x[5] + true_x[5]; - out_3671647430606809601[6] = -nom_x[6] + true_x[6]; - out_3671647430606809601[7] = -nom_x[7] + true_x[7]; - out_3671647430606809601[8] = -nom_x[8] + true_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 H_mod_fun(double *state, double *out_3172612914634759799) { - out_3172612914634759799[0] = 1.0; - out_3172612914634759799[1] = 0.0; - out_3172612914634759799[2] = 0.0; - out_3172612914634759799[3] = 0.0; - out_3172612914634759799[4] = 0.0; - out_3172612914634759799[5] = 0.0; - out_3172612914634759799[6] = 0.0; - out_3172612914634759799[7] = 0.0; - out_3172612914634759799[8] = 0.0; - out_3172612914634759799[9] = 0.0; - out_3172612914634759799[10] = 1.0; - out_3172612914634759799[11] = 0.0; - out_3172612914634759799[12] = 0.0; - out_3172612914634759799[13] = 0.0; - out_3172612914634759799[14] = 0.0; - out_3172612914634759799[15] = 0.0; - out_3172612914634759799[16] = 0.0; - out_3172612914634759799[17] = 0.0; - out_3172612914634759799[18] = 0.0; - out_3172612914634759799[19] = 0.0; - out_3172612914634759799[20] = 1.0; - out_3172612914634759799[21] = 0.0; - out_3172612914634759799[22] = 0.0; - out_3172612914634759799[23] = 0.0; - out_3172612914634759799[24] = 0.0; - out_3172612914634759799[25] = 0.0; - out_3172612914634759799[26] = 0.0; - out_3172612914634759799[27] = 0.0; - out_3172612914634759799[28] = 0.0; - out_3172612914634759799[29] = 0.0; - out_3172612914634759799[30] = 1.0; - out_3172612914634759799[31] = 0.0; - out_3172612914634759799[32] = 0.0; - out_3172612914634759799[33] = 0.0; - out_3172612914634759799[34] = 0.0; - out_3172612914634759799[35] = 0.0; - out_3172612914634759799[36] = 0.0; - out_3172612914634759799[37] = 0.0; - out_3172612914634759799[38] = 0.0; - out_3172612914634759799[39] = 0.0; - out_3172612914634759799[40] = 1.0; - out_3172612914634759799[41] = 0.0; - out_3172612914634759799[42] = 0.0; - out_3172612914634759799[43] = 0.0; - out_3172612914634759799[44] = 0.0; - out_3172612914634759799[45] = 0.0; - out_3172612914634759799[46] = 0.0; - out_3172612914634759799[47] = 0.0; - out_3172612914634759799[48] = 0.0; - out_3172612914634759799[49] = 0.0; - out_3172612914634759799[50] = 1.0; - out_3172612914634759799[51] = 0.0; - out_3172612914634759799[52] = 0.0; - out_3172612914634759799[53] = 0.0; - out_3172612914634759799[54] = 0.0; - out_3172612914634759799[55] = 0.0; - out_3172612914634759799[56] = 0.0; - out_3172612914634759799[57] = 0.0; - out_3172612914634759799[58] = 0.0; - out_3172612914634759799[59] = 0.0; - out_3172612914634759799[60] = 1.0; - out_3172612914634759799[61] = 0.0; - out_3172612914634759799[62] = 0.0; - out_3172612914634759799[63] = 0.0; - out_3172612914634759799[64] = 0.0; - out_3172612914634759799[65] = 0.0; - out_3172612914634759799[66] = 0.0; - out_3172612914634759799[67] = 0.0; - out_3172612914634759799[68] = 0.0; - out_3172612914634759799[69] = 0.0; - out_3172612914634759799[70] = 1.0; - out_3172612914634759799[71] = 0.0; - out_3172612914634759799[72] = 0.0; - out_3172612914634759799[73] = 0.0; - out_3172612914634759799[74] = 0.0; - out_3172612914634759799[75] = 0.0; - out_3172612914634759799[76] = 0.0; - out_3172612914634759799[77] = 0.0; - out_3172612914634759799[78] = 0.0; - out_3172612914634759799[79] = 0.0; - out_3172612914634759799[80] = 1.0; +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 f_fun(double *state, double dt, double *out_7316960543418142517) { - out_7316960543418142517[0] = state[0]; - out_7316960543418142517[1] = state[1]; - out_7316960543418142517[2] = state[2]; - out_7316960543418142517[3] = state[3]; - out_7316960543418142517[4] = state[4]; - out_7316960543418142517[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_7316960543418142517[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_7316960543418142517[7] = state[7]; - out_7316960543418142517[8] = state[8]; +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_1492910262171570578) { - out_1492910262171570578[0] = 1; - out_1492910262171570578[1] = 0; - out_1492910262171570578[2] = 0; - out_1492910262171570578[3] = 0; - out_1492910262171570578[4] = 0; - out_1492910262171570578[5] = 0; - out_1492910262171570578[6] = 0; - out_1492910262171570578[7] = 0; - out_1492910262171570578[8] = 0; - out_1492910262171570578[9] = 0; - out_1492910262171570578[10] = 1; - out_1492910262171570578[11] = 0; - out_1492910262171570578[12] = 0; - out_1492910262171570578[13] = 0; - out_1492910262171570578[14] = 0; - out_1492910262171570578[15] = 0; - out_1492910262171570578[16] = 0; - out_1492910262171570578[17] = 0; - out_1492910262171570578[18] = 0; - out_1492910262171570578[19] = 0; - out_1492910262171570578[20] = 1; - out_1492910262171570578[21] = 0; - out_1492910262171570578[22] = 0; - out_1492910262171570578[23] = 0; - out_1492910262171570578[24] = 0; - out_1492910262171570578[25] = 0; - out_1492910262171570578[26] = 0; - out_1492910262171570578[27] = 0; - out_1492910262171570578[28] = 0; - out_1492910262171570578[29] = 0; - out_1492910262171570578[30] = 1; - out_1492910262171570578[31] = 0; - out_1492910262171570578[32] = 0; - out_1492910262171570578[33] = 0; - out_1492910262171570578[34] = 0; - out_1492910262171570578[35] = 0; - out_1492910262171570578[36] = 0; - out_1492910262171570578[37] = 0; - out_1492910262171570578[38] = 0; - out_1492910262171570578[39] = 0; - out_1492910262171570578[40] = 1; - out_1492910262171570578[41] = 0; - out_1492910262171570578[42] = 0; - out_1492910262171570578[43] = 0; - out_1492910262171570578[44] = 0; - out_1492910262171570578[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_1492910262171570578[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_1492910262171570578[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_1492910262171570578[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_1492910262171570578[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_1492910262171570578[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_1492910262171570578[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_1492910262171570578[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_1492910262171570578[53] = -9.8100000000000005*dt; - out_1492910262171570578[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_1492910262171570578[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_1492910262171570578[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_1492910262171570578[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_1492910262171570578[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_1492910262171570578[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_1492910262171570578[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_1492910262171570578[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_1492910262171570578[62] = 0; - out_1492910262171570578[63] = 0; - out_1492910262171570578[64] = 0; - out_1492910262171570578[65] = 0; - out_1492910262171570578[66] = 0; - out_1492910262171570578[67] = 0; - out_1492910262171570578[68] = 0; - out_1492910262171570578[69] = 0; - out_1492910262171570578[70] = 1; - out_1492910262171570578[71] = 0; - out_1492910262171570578[72] = 0; - out_1492910262171570578[73] = 0; - out_1492910262171570578[74] = 0; - out_1492910262171570578[75] = 0; - out_1492910262171570578[76] = 0; - out_1492910262171570578[77] = 0; - out_1492910262171570578[78] = 0; - out_1492910262171570578[79] = 0; - out_1492910262171570578[80] = 1; +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 h_25(double *state, double *unused, double *out_595544251427276973) { - out_595544251427276973[0] = state[6]; +void h_25(double *state, double *unused, double *out_319074315832850169) { + out_319074315832850169[0] = state[6]; } -void H_25(double *state, double *unused, double *out_350435724010605349) { - out_350435724010605349[0] = 0; - out_350435724010605349[1] = 0; - out_350435724010605349[2] = 0; - out_350435724010605349[3] = 0; - out_350435724010605349[4] = 0; - out_350435724010605349[5] = 0; - out_350435724010605349[6] = 1; - out_350435724010605349[7] = 0; - out_350435724010605349[8] = 0; +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_24(double *state, double *unused, double *out_2954294579100300160) { - out_2954294579100300160[0] = state[4]; - out_2954294579100300160[1] = state[5]; +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_3207489874791717787) { - out_3207489874791717787[0] = 0; - out_3207489874791717787[1] = 0; - out_3207489874791717787[2] = 0; - out_3207489874791717787[3] = 0; - out_3207489874791717787[4] = 1; - out_3207489874791717787[5] = 0; - out_3207489874791717787[6] = 0; - out_3207489874791717787[7] = 0; - out_3207489874791717787[8] = 0; - out_3207489874791717787[9] = 0; - out_3207489874791717787[10] = 0; - out_3207489874791717787[11] = 0; - out_3207489874791717787[12] = 0; - out_3207489874791717787[13] = 0; - out_3207489874791717787[14] = 1; - out_3207489874791717787[15] = 0; - out_3207489874791717787[16] = 0; - out_3207489874791717787[17] = 0; +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_30(double *state, double *unused, double *out_4457227727855778625) { - out_4457227727855778625[0] = state[4]; +void h_30(double *state, double *unused, double *out_8212013810717373528) { + out_8212013810717373528[0] = state[4]; } -void H_30(double *state, double *unused, double *out_2167897234496643278) { - out_2167897234496643278[0] = 0; - out_2167897234496643278[1] = 0; - out_2167897234496643278[2] = 0; - out_2167897234496643278[3] = 0; - out_2167897234496643278[4] = 1; - out_2167897234496643278[5] = 0; - out_2167897234496643278[6] = 0; - out_2167897234496643278[7] = 0; - out_2167897234496643278[8] = 0; +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_26(double *state, double *unused, double *out_6778606859599551480) { - out_6778606859599551480[0] = state[7]; +void h_26(double *state, double *unused, double *out_5233056995331332443) { + out_5233056995331332443[0] = state[7]; } -void H_26(double *state, double *unused, double *out_4091939042884661573) { - out_4091939042884661573[0] = 0; - out_4091939042884661573[1] = 0; - out_4091939042884661573[2] = 0; - out_4091939042884661573[3] = 0; - out_4091939042884661573[4] = 0; - out_4091939042884661573[5] = 0; - out_4091939042884661573[6] = 0; - out_4091939042884661573[7] = 1; - out_4091939042884661573[8] = 0; +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_27(double *state, double *unused, double *out_2871073069040329107) { - out_2871073069040329107[0] = state[3]; +void h_27(double *state, double *unused, double *out_3355860367277182854) { + out_3355860367277182854[0] = state[3]; } -void H_27(double *state, double *unused, double *out_4391491305680586495) { - out_4391491305680586495[0] = 0; - out_4391491305680586495[1] = 0; - out_4391491305680586495[2] = 0; - out_4391491305680586495[3] = 1; - out_4391491305680586495[4] = 0; - out_4391491305680586495[5] = 0; - out_4391491305680586495[6] = 0; - out_4391491305680586495[7] = 0; - out_4391491305680586495[8] = 0; +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_29(double *state, double *unused, double *out_8987843078192927346) { - out_8987843078192927346[0] = state[1]; +void h_29(double *state, double *unused, double *out_7630324942183480437) { + out_7630324942183480437[0] = state[1]; } -void H_29(double *state, double *unused, double *out_2678128578811035462) { - out_2678128578811035462[0] = 0; - out_2678128578811035462[1] = 1; - out_2678128578811035462[2] = 0; - out_2678128578811035462[3] = 0; - out_2678128578811035462[4] = 0; - out_2678128578811035462[5] = 0; - out_2678128578811035462[6] = 0; - out_2678128578811035462[7] = 0; - out_2678128578811035462[8] = 0; +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_28(double *state, double *unused, double *out_3384751652918718147) { - out_3384751652918718147[0] = state[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_2404270438258495112) { - out_2404270438258495112[0] = 1; - out_2404270438258495112[1] = 0; - out_2404270438258495112[2] = 0; - out_2404270438258495112[3] = 0; - out_2404270438258495112[4] = 0; - out_2404270438258495112[5] = 0; - out_2404270438258495112[6] = 0; - out_2404270438258495112[7] = 0; - out_2404270438258495112[8] = 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_31(double *state, double *unused, double *out_1973413530269387393) { - out_1973413530269387393[0] = state[8]; +void h_31(double *state, double *unused, double *out_594268378117356058) { + out_594268378117356058[0] = state[8]; } -void H_31(double *state, double *unused, double *out_4718147145118013049) { - out_4718147145118013049[0] = 0; - out_4718147145118013049[1] = 0; - out_4718147145118013049[2] = 0; - out_4718147145118013049[3] = 0; - out_4718147145118013049[4] = 0; - out_4718147145118013049[5] = 0; - out_4718147145118013049[6] = 0; - out_4718147145118013049[7] = 0; - out_4718147145118013049[8] = 1; +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; } #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_2503888728252844227) { - err_fun(nom_x, delta_x, out_2503888728252844227); +void car_err_fun(double *nom_x, double *delta_x, double *out_4387195160971747702) { + err_fun(nom_x, delta_x, out_4387195160971747702); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3671647430606809601) { - inv_err_fun(nom_x, true_x, out_3671647430606809601); +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_H_mod_fun(double *state, double *out_3172612914634759799) { - H_mod_fun(state, out_3172612914634759799); +void car_H_mod_fun(double *state, double *out_7178605602671138983) { + H_mod_fun(state, out_7178605602671138983); } -void car_f_fun(double *state, double dt, double *out_7316960543418142517) { - f_fun(state, dt, out_7316960543418142517); +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_1492910262171570578) { - F_fun(state, dt, out_1492910262171570578); +void car_F_fun(double *state, double dt, double *out_6761986232654706930) { + F_fun(state, dt, out_6761986232654706930); } -void car_h_25(double *state, double *unused, double *out_595544251427276973) { - h_25(state, unused, out_595544251427276973); +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_350435724010605349) { - H_25(state, unused, out_350435724010605349); +void car_H_25(double *state, double *unused, double *out_3655624952681647306) { + H_25(state, unused, out_3655624952681647306); } -void car_h_24(double *state, double *unused, double *out_2954294579100300160) { - h_24(state, unused, out_2954294579100300160); +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_3207489874791717787) { - H_24(state, unused, out_3207489874791717787); +void car_H_24(double *state, double *unused, double *out_1429917168702778744) { + H_24(state, unused, out_1429917168702778744); } -void car_h_30(double *state, double *unused, double *out_4457227727855778625) { - h_30(state, unused, out_4457227727855778625); +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_2167897234496643278) { - H_30(state, unused, out_2167897234496643278); +void car_H_30(double *state, double *unused, double *out_3784963899824887376) { + H_30(state, unused, out_3784963899824887376); } -void car_h_26(double *state, double *unused, double *out_6778606859599551480) { - h_26(state, unused, out_6778606859599551480); +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_4091939042884661573) { - H_26(state, unused, out_4091939042884661573); +void car_H_26(double *state, double *unused, double *out_2998770888571335402) { + H_26(state, unused, out_2998770888571335402); } -void car_h_27(double *state, double *unused, double *out_2871073069040329107) { - h_27(state, unused, out_2871073069040329107); +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_4391491305680586495) { - H_27(state, unused, out_4391491305680586495); +void car_H_27(double *state, double *unused, double *out_5959727211625312287) { + H_27(state, unused, out_5959727211625312287); } -void car_h_29(double *state, double *unused, double *out_8987843078192927346) { - h_29(state, unused, out_8987843078192927346); +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_2678128578811035462) { - H_29(state, unused, out_2678128578811035462); +void car_H_29(double *state, double *unused, double *out_3274732555510495192) { + H_29(state, unused, out_3274732555510495192); } -void car_h_28(double *state, double *unused, double *out_3384751652918718147) { - h_28(state, unused, out_3384751652918718147); +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_2404270438258495112) { - H_28(state, unused, out_2404270438258495112); +void car_H_28(double *state, double *unused, double *out_1311102283945168941) { + H_28(state, unused, out_1311102283945168941); } -void car_h_31(double *state, double *unused, double *out_1973413530269387393) { - h_31(state, unused, out_1973413530269387393); +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_4718147145118013049) { - H_31(state, unused, out_4718147145118013049); +void car_H_31(double *state, double *unused, double *out_3624978990804686878) { + H_31(state, unused, out_3624978990804686878); } 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 a493c571e..73987e5d2 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_2503888728252844227); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3671647430606809601); -void car_H_mod_fun(double *state, double *out_3172612914634759799); -void car_f_fun(double *state, double dt, double *out_7316960543418142517); -void car_F_fun(double *state, double dt, double *out_1492910262171570578); -void car_h_25(double *state, double *unused, double *out_595544251427276973); -void car_H_25(double *state, double *unused, double *out_350435724010605349); -void car_h_24(double *state, double *unused, double *out_2954294579100300160); -void car_H_24(double *state, double *unused, double *out_3207489874791717787); -void car_h_30(double *state, double *unused, double *out_4457227727855778625); -void car_H_30(double *state, double *unused, double *out_2167897234496643278); -void car_h_26(double *state, double *unused, double *out_6778606859599551480); -void car_H_26(double *state, double *unused, double *out_4091939042884661573); -void car_h_27(double *state, double *unused, double *out_2871073069040329107); -void car_H_27(double *state, double *unused, double *out_4391491305680586495); -void car_h_29(double *state, double *unused, double *out_8987843078192927346); -void car_H_29(double *state, double *unused, double *out_2678128578811035462); -void car_h_28(double *state, double *unused, double *out_3384751652918718147); -void car_H_28(double *state, double *unused, double *out_2404270438258495112); -void car_h_31(double *state, double *unused, double *out_1973413530269387393); -void car_H_31(double *state, double *unused, double *out_4718147145118013049); +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_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 8fb6b422c..08ba23662 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_6989025868442055132) { - out_6989025868442055132[0] = delta_x[0] + nom_x[0]; - out_6989025868442055132[1] = delta_x[1] + nom_x[1]; - out_6989025868442055132[2] = delta_x[2] + nom_x[2]; - out_6989025868442055132[3] = delta_x[3] + nom_x[3]; - out_6989025868442055132[4] = delta_x[4] + nom_x[4]; - out_6989025868442055132[5] = delta_x[5] + nom_x[5]; - out_6989025868442055132[6] = delta_x[6] + nom_x[6]; - out_6989025868442055132[7] = delta_x[7] + nom_x[7]; - out_6989025868442055132[8] = delta_x[8] + nom_x[8]; - out_6989025868442055132[9] = delta_x[9] + nom_x[9]; - out_6989025868442055132[10] = delta_x[10] + nom_x[10]; - out_6989025868442055132[11] = delta_x[11] + nom_x[11]; - out_6989025868442055132[12] = delta_x[12] + nom_x[12]; - out_6989025868442055132[13] = delta_x[13] + nom_x[13]; - out_6989025868442055132[14] = delta_x[14] + nom_x[14]; - out_6989025868442055132[15] = delta_x[15] + nom_x[15]; - out_6989025868442055132[16] = delta_x[16] + nom_x[16]; - out_6989025868442055132[17] = delta_x[17] + nom_x[17]; +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 inv_err_fun(double *nom_x, double *true_x, double *out_3172938755845140042) { - out_3172938755845140042[0] = -nom_x[0] + true_x[0]; - out_3172938755845140042[1] = -nom_x[1] + true_x[1]; - out_3172938755845140042[2] = -nom_x[2] + true_x[2]; - out_3172938755845140042[3] = -nom_x[3] + true_x[3]; - out_3172938755845140042[4] = -nom_x[4] + true_x[4]; - out_3172938755845140042[5] = -nom_x[5] + true_x[5]; - out_3172938755845140042[6] = -nom_x[6] + true_x[6]; - out_3172938755845140042[7] = -nom_x[7] + true_x[7]; - out_3172938755845140042[8] = -nom_x[8] + true_x[8]; - out_3172938755845140042[9] = -nom_x[9] + true_x[9]; - out_3172938755845140042[10] = -nom_x[10] + true_x[10]; - out_3172938755845140042[11] = -nom_x[11] + true_x[11]; - out_3172938755845140042[12] = -nom_x[12] + true_x[12]; - out_3172938755845140042[13] = -nom_x[13] + true_x[13]; - out_3172938755845140042[14] = -nom_x[14] + true_x[14]; - out_3172938755845140042[15] = -nom_x[15] + true_x[15]; - out_3172938755845140042[16] = -nom_x[16] + true_x[16]; - out_3172938755845140042[17] = -nom_x[17] + true_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 H_mod_fun(double *state, double *out_3225867917449183727) { - out_3225867917449183727[0] = 1.0; - out_3225867917449183727[1] = 0.0; - out_3225867917449183727[2] = 0.0; - out_3225867917449183727[3] = 0.0; - out_3225867917449183727[4] = 0.0; - out_3225867917449183727[5] = 0.0; - out_3225867917449183727[6] = 0.0; - out_3225867917449183727[7] = 0.0; - out_3225867917449183727[8] = 0.0; - out_3225867917449183727[9] = 0.0; - out_3225867917449183727[10] = 0.0; - out_3225867917449183727[11] = 0.0; - out_3225867917449183727[12] = 0.0; - out_3225867917449183727[13] = 0.0; - out_3225867917449183727[14] = 0.0; - out_3225867917449183727[15] = 0.0; - out_3225867917449183727[16] = 0.0; - out_3225867917449183727[17] = 0.0; - out_3225867917449183727[18] = 0.0; - out_3225867917449183727[19] = 1.0; - out_3225867917449183727[20] = 0.0; - out_3225867917449183727[21] = 0.0; - out_3225867917449183727[22] = 0.0; - out_3225867917449183727[23] = 0.0; - out_3225867917449183727[24] = 0.0; - out_3225867917449183727[25] = 0.0; - out_3225867917449183727[26] = 0.0; - out_3225867917449183727[27] = 0.0; - out_3225867917449183727[28] = 0.0; - out_3225867917449183727[29] = 0.0; - out_3225867917449183727[30] = 0.0; - out_3225867917449183727[31] = 0.0; - out_3225867917449183727[32] = 0.0; - out_3225867917449183727[33] = 0.0; - out_3225867917449183727[34] = 0.0; - out_3225867917449183727[35] = 0.0; - out_3225867917449183727[36] = 0.0; - out_3225867917449183727[37] = 0.0; - out_3225867917449183727[38] = 1.0; - out_3225867917449183727[39] = 0.0; - out_3225867917449183727[40] = 0.0; - out_3225867917449183727[41] = 0.0; - out_3225867917449183727[42] = 0.0; - out_3225867917449183727[43] = 0.0; - out_3225867917449183727[44] = 0.0; - out_3225867917449183727[45] = 0.0; - out_3225867917449183727[46] = 0.0; - out_3225867917449183727[47] = 0.0; - out_3225867917449183727[48] = 0.0; - out_3225867917449183727[49] = 0.0; - out_3225867917449183727[50] = 0.0; - out_3225867917449183727[51] = 0.0; - out_3225867917449183727[52] = 0.0; - out_3225867917449183727[53] = 0.0; - out_3225867917449183727[54] = 0.0; - out_3225867917449183727[55] = 0.0; - out_3225867917449183727[56] = 0.0; - out_3225867917449183727[57] = 1.0; - out_3225867917449183727[58] = 0.0; - out_3225867917449183727[59] = 0.0; - out_3225867917449183727[60] = 0.0; - out_3225867917449183727[61] = 0.0; - out_3225867917449183727[62] = 0.0; - out_3225867917449183727[63] = 0.0; - out_3225867917449183727[64] = 0.0; - out_3225867917449183727[65] = 0.0; - out_3225867917449183727[66] = 0.0; - out_3225867917449183727[67] = 0.0; - out_3225867917449183727[68] = 0.0; - out_3225867917449183727[69] = 0.0; - out_3225867917449183727[70] = 0.0; - out_3225867917449183727[71] = 0.0; - out_3225867917449183727[72] = 0.0; - out_3225867917449183727[73] = 0.0; - out_3225867917449183727[74] = 0.0; - out_3225867917449183727[75] = 0.0; - out_3225867917449183727[76] = 1.0; - out_3225867917449183727[77] = 0.0; - out_3225867917449183727[78] = 0.0; - out_3225867917449183727[79] = 0.0; - out_3225867917449183727[80] = 0.0; - out_3225867917449183727[81] = 0.0; - out_3225867917449183727[82] = 0.0; - out_3225867917449183727[83] = 0.0; - out_3225867917449183727[84] = 0.0; - out_3225867917449183727[85] = 0.0; - out_3225867917449183727[86] = 0.0; - out_3225867917449183727[87] = 0.0; - out_3225867917449183727[88] = 0.0; - out_3225867917449183727[89] = 0.0; - out_3225867917449183727[90] = 0.0; - out_3225867917449183727[91] = 0.0; - out_3225867917449183727[92] = 0.0; - out_3225867917449183727[93] = 0.0; - out_3225867917449183727[94] = 0.0; - out_3225867917449183727[95] = 1.0; - out_3225867917449183727[96] = 0.0; - out_3225867917449183727[97] = 0.0; - out_3225867917449183727[98] = 0.0; - out_3225867917449183727[99] = 0.0; - out_3225867917449183727[100] = 0.0; - out_3225867917449183727[101] = 0.0; - out_3225867917449183727[102] = 0.0; - out_3225867917449183727[103] = 0.0; - out_3225867917449183727[104] = 0.0; - out_3225867917449183727[105] = 0.0; - out_3225867917449183727[106] = 0.0; - out_3225867917449183727[107] = 0.0; - out_3225867917449183727[108] = 0.0; - out_3225867917449183727[109] = 0.0; - out_3225867917449183727[110] = 0.0; - out_3225867917449183727[111] = 0.0; - out_3225867917449183727[112] = 0.0; - out_3225867917449183727[113] = 0.0; - out_3225867917449183727[114] = 1.0; - out_3225867917449183727[115] = 0.0; - out_3225867917449183727[116] = 0.0; - out_3225867917449183727[117] = 0.0; - out_3225867917449183727[118] = 0.0; - out_3225867917449183727[119] = 0.0; - out_3225867917449183727[120] = 0.0; - out_3225867917449183727[121] = 0.0; - out_3225867917449183727[122] = 0.0; - out_3225867917449183727[123] = 0.0; - out_3225867917449183727[124] = 0.0; - out_3225867917449183727[125] = 0.0; - out_3225867917449183727[126] = 0.0; - out_3225867917449183727[127] = 0.0; - out_3225867917449183727[128] = 0.0; - out_3225867917449183727[129] = 0.0; - out_3225867917449183727[130] = 0.0; - out_3225867917449183727[131] = 0.0; - out_3225867917449183727[132] = 0.0; - out_3225867917449183727[133] = 1.0; - out_3225867917449183727[134] = 0.0; - out_3225867917449183727[135] = 0.0; - out_3225867917449183727[136] = 0.0; - out_3225867917449183727[137] = 0.0; - out_3225867917449183727[138] = 0.0; - out_3225867917449183727[139] = 0.0; - out_3225867917449183727[140] = 0.0; - out_3225867917449183727[141] = 0.0; - out_3225867917449183727[142] = 0.0; - out_3225867917449183727[143] = 0.0; - out_3225867917449183727[144] = 0.0; - out_3225867917449183727[145] = 0.0; - out_3225867917449183727[146] = 0.0; - out_3225867917449183727[147] = 0.0; - out_3225867917449183727[148] = 0.0; - out_3225867917449183727[149] = 0.0; - out_3225867917449183727[150] = 0.0; - out_3225867917449183727[151] = 0.0; - out_3225867917449183727[152] = 1.0; - out_3225867917449183727[153] = 0.0; - out_3225867917449183727[154] = 0.0; - out_3225867917449183727[155] = 0.0; - out_3225867917449183727[156] = 0.0; - out_3225867917449183727[157] = 0.0; - out_3225867917449183727[158] = 0.0; - out_3225867917449183727[159] = 0.0; - out_3225867917449183727[160] = 0.0; - out_3225867917449183727[161] = 0.0; - out_3225867917449183727[162] = 0.0; - out_3225867917449183727[163] = 0.0; - out_3225867917449183727[164] = 0.0; - out_3225867917449183727[165] = 0.0; - out_3225867917449183727[166] = 0.0; - out_3225867917449183727[167] = 0.0; - out_3225867917449183727[168] = 0.0; - out_3225867917449183727[169] = 0.0; - out_3225867917449183727[170] = 0.0; - out_3225867917449183727[171] = 1.0; - out_3225867917449183727[172] = 0.0; - out_3225867917449183727[173] = 0.0; - out_3225867917449183727[174] = 0.0; - out_3225867917449183727[175] = 0.0; - out_3225867917449183727[176] = 0.0; - out_3225867917449183727[177] = 0.0; - out_3225867917449183727[178] = 0.0; - out_3225867917449183727[179] = 0.0; - out_3225867917449183727[180] = 0.0; - out_3225867917449183727[181] = 0.0; - out_3225867917449183727[182] = 0.0; - out_3225867917449183727[183] = 0.0; - out_3225867917449183727[184] = 0.0; - out_3225867917449183727[185] = 0.0; - out_3225867917449183727[186] = 0.0; - out_3225867917449183727[187] = 0.0; - out_3225867917449183727[188] = 0.0; - out_3225867917449183727[189] = 0.0; - out_3225867917449183727[190] = 1.0; - out_3225867917449183727[191] = 0.0; - out_3225867917449183727[192] = 0.0; - out_3225867917449183727[193] = 0.0; - out_3225867917449183727[194] = 0.0; - out_3225867917449183727[195] = 0.0; - out_3225867917449183727[196] = 0.0; - out_3225867917449183727[197] = 0.0; - out_3225867917449183727[198] = 0.0; - out_3225867917449183727[199] = 0.0; - out_3225867917449183727[200] = 0.0; - out_3225867917449183727[201] = 0.0; - out_3225867917449183727[202] = 0.0; - out_3225867917449183727[203] = 0.0; - out_3225867917449183727[204] = 0.0; - out_3225867917449183727[205] = 0.0; - out_3225867917449183727[206] = 0.0; - out_3225867917449183727[207] = 0.0; - out_3225867917449183727[208] = 0.0; - out_3225867917449183727[209] = 1.0; - out_3225867917449183727[210] = 0.0; - out_3225867917449183727[211] = 0.0; - out_3225867917449183727[212] = 0.0; - out_3225867917449183727[213] = 0.0; - out_3225867917449183727[214] = 0.0; - out_3225867917449183727[215] = 0.0; - out_3225867917449183727[216] = 0.0; - out_3225867917449183727[217] = 0.0; - out_3225867917449183727[218] = 0.0; - out_3225867917449183727[219] = 0.0; - out_3225867917449183727[220] = 0.0; - out_3225867917449183727[221] = 0.0; - out_3225867917449183727[222] = 0.0; - out_3225867917449183727[223] = 0.0; - out_3225867917449183727[224] = 0.0; - out_3225867917449183727[225] = 0.0; - out_3225867917449183727[226] = 0.0; - out_3225867917449183727[227] = 0.0; - out_3225867917449183727[228] = 1.0; - out_3225867917449183727[229] = 0.0; - out_3225867917449183727[230] = 0.0; - out_3225867917449183727[231] = 0.0; - out_3225867917449183727[232] = 0.0; - out_3225867917449183727[233] = 0.0; - out_3225867917449183727[234] = 0.0; - out_3225867917449183727[235] = 0.0; - out_3225867917449183727[236] = 0.0; - out_3225867917449183727[237] = 0.0; - out_3225867917449183727[238] = 0.0; - out_3225867917449183727[239] = 0.0; - out_3225867917449183727[240] = 0.0; - out_3225867917449183727[241] = 0.0; - out_3225867917449183727[242] = 0.0; - out_3225867917449183727[243] = 0.0; - out_3225867917449183727[244] = 0.0; - out_3225867917449183727[245] = 0.0; - out_3225867917449183727[246] = 0.0; - out_3225867917449183727[247] = 1.0; - out_3225867917449183727[248] = 0.0; - out_3225867917449183727[249] = 0.0; - out_3225867917449183727[250] = 0.0; - out_3225867917449183727[251] = 0.0; - out_3225867917449183727[252] = 0.0; - out_3225867917449183727[253] = 0.0; - out_3225867917449183727[254] = 0.0; - out_3225867917449183727[255] = 0.0; - out_3225867917449183727[256] = 0.0; - out_3225867917449183727[257] = 0.0; - out_3225867917449183727[258] = 0.0; - out_3225867917449183727[259] = 0.0; - out_3225867917449183727[260] = 0.0; - out_3225867917449183727[261] = 0.0; - out_3225867917449183727[262] = 0.0; - out_3225867917449183727[263] = 0.0; - out_3225867917449183727[264] = 0.0; - out_3225867917449183727[265] = 0.0; - out_3225867917449183727[266] = 1.0; - out_3225867917449183727[267] = 0.0; - out_3225867917449183727[268] = 0.0; - out_3225867917449183727[269] = 0.0; - out_3225867917449183727[270] = 0.0; - out_3225867917449183727[271] = 0.0; - out_3225867917449183727[272] = 0.0; - out_3225867917449183727[273] = 0.0; - out_3225867917449183727[274] = 0.0; - out_3225867917449183727[275] = 0.0; - out_3225867917449183727[276] = 0.0; - out_3225867917449183727[277] = 0.0; - out_3225867917449183727[278] = 0.0; - out_3225867917449183727[279] = 0.0; - out_3225867917449183727[280] = 0.0; - out_3225867917449183727[281] = 0.0; - out_3225867917449183727[282] = 0.0; - out_3225867917449183727[283] = 0.0; - out_3225867917449183727[284] = 0.0; - out_3225867917449183727[285] = 1.0; - out_3225867917449183727[286] = 0.0; - out_3225867917449183727[287] = 0.0; - out_3225867917449183727[288] = 0.0; - out_3225867917449183727[289] = 0.0; - out_3225867917449183727[290] = 0.0; - out_3225867917449183727[291] = 0.0; - out_3225867917449183727[292] = 0.0; - out_3225867917449183727[293] = 0.0; - out_3225867917449183727[294] = 0.0; - out_3225867917449183727[295] = 0.0; - out_3225867917449183727[296] = 0.0; - out_3225867917449183727[297] = 0.0; - out_3225867917449183727[298] = 0.0; - out_3225867917449183727[299] = 0.0; - out_3225867917449183727[300] = 0.0; - out_3225867917449183727[301] = 0.0; - out_3225867917449183727[302] = 0.0; - out_3225867917449183727[303] = 0.0; - out_3225867917449183727[304] = 1.0; - out_3225867917449183727[305] = 0.0; - out_3225867917449183727[306] = 0.0; - out_3225867917449183727[307] = 0.0; - out_3225867917449183727[308] = 0.0; - out_3225867917449183727[309] = 0.0; - out_3225867917449183727[310] = 0.0; - out_3225867917449183727[311] = 0.0; - out_3225867917449183727[312] = 0.0; - out_3225867917449183727[313] = 0.0; - out_3225867917449183727[314] = 0.0; - out_3225867917449183727[315] = 0.0; - out_3225867917449183727[316] = 0.0; - out_3225867917449183727[317] = 0.0; - out_3225867917449183727[318] = 0.0; - out_3225867917449183727[319] = 0.0; - out_3225867917449183727[320] = 0.0; - out_3225867917449183727[321] = 0.0; - out_3225867917449183727[322] = 0.0; - out_3225867917449183727[323] = 1.0; +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 f_fun(double *state, double dt, double *out_2533972772359230172) { - out_2533972772359230172[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_2533972772359230172[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_2533972772359230172[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_2533972772359230172[3] = dt*state[12] + state[3]; - out_2533972772359230172[4] = dt*state[13] + state[4]; - out_2533972772359230172[5] = dt*state[14] + state[5]; - out_2533972772359230172[6] = state[6]; - out_2533972772359230172[7] = state[7]; - out_2533972772359230172[8] = state[8]; - out_2533972772359230172[9] = state[9]; - out_2533972772359230172[10] = state[10]; - out_2533972772359230172[11] = state[11]; - out_2533972772359230172[12] = state[12]; - out_2533972772359230172[13] = state[13]; - out_2533972772359230172[14] = state[14]; - out_2533972772359230172[15] = state[15]; - out_2533972772359230172[16] = state[16]; - out_2533972772359230172[17] = state[17]; +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_3663800095301942537) { - out_3663800095301942537[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_3663800095301942537[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_3663800095301942537[2] = 0; - out_3663800095301942537[3] = 0; - out_3663800095301942537[4] = 0; - out_3663800095301942537[5] = 0; - out_3663800095301942537[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_3663800095301942537[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_3663800095301942537[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_3663800095301942537[9] = 0; - out_3663800095301942537[10] = 0; - out_3663800095301942537[11] = 0; - out_3663800095301942537[12] = 0; - out_3663800095301942537[13] = 0; - out_3663800095301942537[14] = 0; - out_3663800095301942537[15] = 0; - out_3663800095301942537[16] = 0; - out_3663800095301942537[17] = 0; - out_3663800095301942537[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_3663800095301942537[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_3663800095301942537[20] = 0; - out_3663800095301942537[21] = 0; - out_3663800095301942537[22] = 0; - out_3663800095301942537[23] = 0; - out_3663800095301942537[24] = 0; - out_3663800095301942537[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_3663800095301942537[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_3663800095301942537[27] = 0; - out_3663800095301942537[28] = 0; - out_3663800095301942537[29] = 0; - out_3663800095301942537[30] = 0; - out_3663800095301942537[31] = 0; - out_3663800095301942537[32] = 0; - out_3663800095301942537[33] = 0; - out_3663800095301942537[34] = 0; - out_3663800095301942537[35] = 0; - out_3663800095301942537[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_3663800095301942537[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_3663800095301942537[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_3663800095301942537[39] = 0; - out_3663800095301942537[40] = 0; - out_3663800095301942537[41] = 0; - out_3663800095301942537[42] = 0; - out_3663800095301942537[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_3663800095301942537[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_3663800095301942537[45] = 0; - out_3663800095301942537[46] = 0; - out_3663800095301942537[47] = 0; - out_3663800095301942537[48] = 0; - out_3663800095301942537[49] = 0; - out_3663800095301942537[50] = 0; - out_3663800095301942537[51] = 0; - out_3663800095301942537[52] = 0; - out_3663800095301942537[53] = 0; - out_3663800095301942537[54] = 0; - out_3663800095301942537[55] = 0; - out_3663800095301942537[56] = 0; - out_3663800095301942537[57] = 1; - out_3663800095301942537[58] = 0; - out_3663800095301942537[59] = 0; - out_3663800095301942537[60] = 0; - out_3663800095301942537[61] = 0; - out_3663800095301942537[62] = 0; - out_3663800095301942537[63] = 0; - out_3663800095301942537[64] = 0; - out_3663800095301942537[65] = 0; - out_3663800095301942537[66] = dt; - out_3663800095301942537[67] = 0; - out_3663800095301942537[68] = 0; - out_3663800095301942537[69] = 0; - out_3663800095301942537[70] = 0; - out_3663800095301942537[71] = 0; - out_3663800095301942537[72] = 0; - out_3663800095301942537[73] = 0; - out_3663800095301942537[74] = 0; - out_3663800095301942537[75] = 0; - out_3663800095301942537[76] = 1; - out_3663800095301942537[77] = 0; - out_3663800095301942537[78] = 0; - out_3663800095301942537[79] = 0; - out_3663800095301942537[80] = 0; - out_3663800095301942537[81] = 0; - out_3663800095301942537[82] = 0; - out_3663800095301942537[83] = 0; - out_3663800095301942537[84] = 0; - out_3663800095301942537[85] = dt; - out_3663800095301942537[86] = 0; - out_3663800095301942537[87] = 0; - out_3663800095301942537[88] = 0; - out_3663800095301942537[89] = 0; - out_3663800095301942537[90] = 0; - out_3663800095301942537[91] = 0; - out_3663800095301942537[92] = 0; - out_3663800095301942537[93] = 0; - out_3663800095301942537[94] = 0; - out_3663800095301942537[95] = 1; - out_3663800095301942537[96] = 0; - out_3663800095301942537[97] = 0; - out_3663800095301942537[98] = 0; - out_3663800095301942537[99] = 0; - out_3663800095301942537[100] = 0; - out_3663800095301942537[101] = 0; - out_3663800095301942537[102] = 0; - out_3663800095301942537[103] = 0; - out_3663800095301942537[104] = dt; - out_3663800095301942537[105] = 0; - out_3663800095301942537[106] = 0; - out_3663800095301942537[107] = 0; - out_3663800095301942537[108] = 0; - out_3663800095301942537[109] = 0; - out_3663800095301942537[110] = 0; - out_3663800095301942537[111] = 0; - out_3663800095301942537[112] = 0; - out_3663800095301942537[113] = 0; - out_3663800095301942537[114] = 1; - out_3663800095301942537[115] = 0; - out_3663800095301942537[116] = 0; - out_3663800095301942537[117] = 0; - out_3663800095301942537[118] = 0; - out_3663800095301942537[119] = 0; - out_3663800095301942537[120] = 0; - out_3663800095301942537[121] = 0; - out_3663800095301942537[122] = 0; - out_3663800095301942537[123] = 0; - out_3663800095301942537[124] = 0; - out_3663800095301942537[125] = 0; - out_3663800095301942537[126] = 0; - out_3663800095301942537[127] = 0; - out_3663800095301942537[128] = 0; - out_3663800095301942537[129] = 0; - out_3663800095301942537[130] = 0; - out_3663800095301942537[131] = 0; - out_3663800095301942537[132] = 0; - out_3663800095301942537[133] = 1; - out_3663800095301942537[134] = 0; - out_3663800095301942537[135] = 0; - out_3663800095301942537[136] = 0; - out_3663800095301942537[137] = 0; - out_3663800095301942537[138] = 0; - out_3663800095301942537[139] = 0; - out_3663800095301942537[140] = 0; - out_3663800095301942537[141] = 0; - out_3663800095301942537[142] = 0; - out_3663800095301942537[143] = 0; - out_3663800095301942537[144] = 0; - out_3663800095301942537[145] = 0; - out_3663800095301942537[146] = 0; - out_3663800095301942537[147] = 0; - out_3663800095301942537[148] = 0; - out_3663800095301942537[149] = 0; - out_3663800095301942537[150] = 0; - out_3663800095301942537[151] = 0; - out_3663800095301942537[152] = 1; - out_3663800095301942537[153] = 0; - out_3663800095301942537[154] = 0; - out_3663800095301942537[155] = 0; - out_3663800095301942537[156] = 0; - out_3663800095301942537[157] = 0; - out_3663800095301942537[158] = 0; - out_3663800095301942537[159] = 0; - out_3663800095301942537[160] = 0; - out_3663800095301942537[161] = 0; - out_3663800095301942537[162] = 0; - out_3663800095301942537[163] = 0; - out_3663800095301942537[164] = 0; - out_3663800095301942537[165] = 0; - out_3663800095301942537[166] = 0; - out_3663800095301942537[167] = 0; - out_3663800095301942537[168] = 0; - out_3663800095301942537[169] = 0; - out_3663800095301942537[170] = 0; - out_3663800095301942537[171] = 1; - out_3663800095301942537[172] = 0; - out_3663800095301942537[173] = 0; - out_3663800095301942537[174] = 0; - out_3663800095301942537[175] = 0; - out_3663800095301942537[176] = 0; - out_3663800095301942537[177] = 0; - out_3663800095301942537[178] = 0; - out_3663800095301942537[179] = 0; - out_3663800095301942537[180] = 0; - out_3663800095301942537[181] = 0; - out_3663800095301942537[182] = 0; - out_3663800095301942537[183] = 0; - out_3663800095301942537[184] = 0; - out_3663800095301942537[185] = 0; - out_3663800095301942537[186] = 0; - out_3663800095301942537[187] = 0; - out_3663800095301942537[188] = 0; - out_3663800095301942537[189] = 0; - out_3663800095301942537[190] = 1; - out_3663800095301942537[191] = 0; - out_3663800095301942537[192] = 0; - out_3663800095301942537[193] = 0; - out_3663800095301942537[194] = 0; - out_3663800095301942537[195] = 0; - out_3663800095301942537[196] = 0; - out_3663800095301942537[197] = 0; - out_3663800095301942537[198] = 0; - out_3663800095301942537[199] = 0; - out_3663800095301942537[200] = 0; - out_3663800095301942537[201] = 0; - out_3663800095301942537[202] = 0; - out_3663800095301942537[203] = 0; - out_3663800095301942537[204] = 0; - out_3663800095301942537[205] = 0; - out_3663800095301942537[206] = 0; - out_3663800095301942537[207] = 0; - out_3663800095301942537[208] = 0; - out_3663800095301942537[209] = 1; - out_3663800095301942537[210] = 0; - out_3663800095301942537[211] = 0; - out_3663800095301942537[212] = 0; - out_3663800095301942537[213] = 0; - out_3663800095301942537[214] = 0; - out_3663800095301942537[215] = 0; - out_3663800095301942537[216] = 0; - out_3663800095301942537[217] = 0; - out_3663800095301942537[218] = 0; - out_3663800095301942537[219] = 0; - out_3663800095301942537[220] = 0; - out_3663800095301942537[221] = 0; - out_3663800095301942537[222] = 0; - out_3663800095301942537[223] = 0; - out_3663800095301942537[224] = 0; - out_3663800095301942537[225] = 0; - out_3663800095301942537[226] = 0; - out_3663800095301942537[227] = 0; - out_3663800095301942537[228] = 1; - out_3663800095301942537[229] = 0; - out_3663800095301942537[230] = 0; - out_3663800095301942537[231] = 0; - out_3663800095301942537[232] = 0; - out_3663800095301942537[233] = 0; - out_3663800095301942537[234] = 0; - out_3663800095301942537[235] = 0; - out_3663800095301942537[236] = 0; - out_3663800095301942537[237] = 0; - out_3663800095301942537[238] = 0; - out_3663800095301942537[239] = 0; - out_3663800095301942537[240] = 0; - out_3663800095301942537[241] = 0; - out_3663800095301942537[242] = 0; - out_3663800095301942537[243] = 0; - out_3663800095301942537[244] = 0; - out_3663800095301942537[245] = 0; - out_3663800095301942537[246] = 0; - out_3663800095301942537[247] = 1; - out_3663800095301942537[248] = 0; - out_3663800095301942537[249] = 0; - out_3663800095301942537[250] = 0; - out_3663800095301942537[251] = 0; - out_3663800095301942537[252] = 0; - out_3663800095301942537[253] = 0; - out_3663800095301942537[254] = 0; - out_3663800095301942537[255] = 0; - out_3663800095301942537[256] = 0; - out_3663800095301942537[257] = 0; - out_3663800095301942537[258] = 0; - out_3663800095301942537[259] = 0; - out_3663800095301942537[260] = 0; - out_3663800095301942537[261] = 0; - out_3663800095301942537[262] = 0; - out_3663800095301942537[263] = 0; - out_3663800095301942537[264] = 0; - out_3663800095301942537[265] = 0; - out_3663800095301942537[266] = 1; - out_3663800095301942537[267] = 0; - out_3663800095301942537[268] = 0; - out_3663800095301942537[269] = 0; - out_3663800095301942537[270] = 0; - out_3663800095301942537[271] = 0; - out_3663800095301942537[272] = 0; - out_3663800095301942537[273] = 0; - out_3663800095301942537[274] = 0; - out_3663800095301942537[275] = 0; - out_3663800095301942537[276] = 0; - out_3663800095301942537[277] = 0; - out_3663800095301942537[278] = 0; - out_3663800095301942537[279] = 0; - out_3663800095301942537[280] = 0; - out_3663800095301942537[281] = 0; - out_3663800095301942537[282] = 0; - out_3663800095301942537[283] = 0; - out_3663800095301942537[284] = 0; - out_3663800095301942537[285] = 1; - out_3663800095301942537[286] = 0; - out_3663800095301942537[287] = 0; - out_3663800095301942537[288] = 0; - out_3663800095301942537[289] = 0; - out_3663800095301942537[290] = 0; - out_3663800095301942537[291] = 0; - out_3663800095301942537[292] = 0; - out_3663800095301942537[293] = 0; - out_3663800095301942537[294] = 0; - out_3663800095301942537[295] = 0; - out_3663800095301942537[296] = 0; - out_3663800095301942537[297] = 0; - out_3663800095301942537[298] = 0; - out_3663800095301942537[299] = 0; - out_3663800095301942537[300] = 0; - out_3663800095301942537[301] = 0; - out_3663800095301942537[302] = 0; - out_3663800095301942537[303] = 0; - out_3663800095301942537[304] = 1; - out_3663800095301942537[305] = 0; - out_3663800095301942537[306] = 0; - out_3663800095301942537[307] = 0; - out_3663800095301942537[308] = 0; - out_3663800095301942537[309] = 0; - out_3663800095301942537[310] = 0; - out_3663800095301942537[311] = 0; - out_3663800095301942537[312] = 0; - out_3663800095301942537[313] = 0; - out_3663800095301942537[314] = 0; - out_3663800095301942537[315] = 0; - out_3663800095301942537[316] = 0; - out_3663800095301942537[317] = 0; - out_3663800095301942537[318] = 0; - out_3663800095301942537[319] = 0; - out_3663800095301942537[320] = 0; - out_3663800095301942537[321] = 0; - out_3663800095301942537[322] = 0; - out_3663800095301942537[323] = 1; +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 h_4(double *state, double *unused, double *out_223446469702120852) { - out_223446469702120852[0] = state[6] + state[9]; - out_223446469702120852[1] = state[7] + state[10]; - out_223446469702120852[2] = state[8] + state[11]; +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_4324406006264935836) { - out_4324406006264935836[0] = 0; - out_4324406006264935836[1] = 0; - out_4324406006264935836[2] = 0; - out_4324406006264935836[3] = 0; - out_4324406006264935836[4] = 0; - out_4324406006264935836[5] = 0; - out_4324406006264935836[6] = 1; - out_4324406006264935836[7] = 0; - out_4324406006264935836[8] = 0; - out_4324406006264935836[9] = 1; - out_4324406006264935836[10] = 0; - out_4324406006264935836[11] = 0; - out_4324406006264935836[12] = 0; - out_4324406006264935836[13] = 0; - out_4324406006264935836[14] = 0; - out_4324406006264935836[15] = 0; - out_4324406006264935836[16] = 0; - out_4324406006264935836[17] = 0; - out_4324406006264935836[18] = 0; - out_4324406006264935836[19] = 0; - out_4324406006264935836[20] = 0; - out_4324406006264935836[21] = 0; - out_4324406006264935836[22] = 0; - out_4324406006264935836[23] = 0; - out_4324406006264935836[24] = 0; - out_4324406006264935836[25] = 1; - out_4324406006264935836[26] = 0; - out_4324406006264935836[27] = 0; - out_4324406006264935836[28] = 1; - out_4324406006264935836[29] = 0; - out_4324406006264935836[30] = 0; - out_4324406006264935836[31] = 0; - out_4324406006264935836[32] = 0; - out_4324406006264935836[33] = 0; - out_4324406006264935836[34] = 0; - out_4324406006264935836[35] = 0; - out_4324406006264935836[36] = 0; - out_4324406006264935836[37] = 0; - out_4324406006264935836[38] = 0; - out_4324406006264935836[39] = 0; - out_4324406006264935836[40] = 0; - out_4324406006264935836[41] = 0; - out_4324406006264935836[42] = 0; - out_4324406006264935836[43] = 0; - out_4324406006264935836[44] = 1; - out_4324406006264935836[45] = 0; - out_4324406006264935836[46] = 0; - out_4324406006264935836[47] = 1; - out_4324406006264935836[48] = 0; - out_4324406006264935836[49] = 0; - out_4324406006264935836[50] = 0; - out_4324406006264935836[51] = 0; - out_4324406006264935836[52] = 0; - out_4324406006264935836[53] = 0; +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_10(double *state, double *unused, double *out_2753848967308463071) { - out_2753848967308463071[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_2753848967308463071[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_2753848967308463071[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_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_1159288756680005657) { - out_1159288756680005657[0] = 0; - out_1159288756680005657[1] = 9.8100000000000005*cos(state[1]); - out_1159288756680005657[2] = 0; - out_1159288756680005657[3] = 0; - out_1159288756680005657[4] = -state[8]; - out_1159288756680005657[5] = state[7]; - out_1159288756680005657[6] = 0; - out_1159288756680005657[7] = state[5]; - out_1159288756680005657[8] = -state[4]; - out_1159288756680005657[9] = 0; - out_1159288756680005657[10] = 0; - out_1159288756680005657[11] = 0; - out_1159288756680005657[12] = 1; - out_1159288756680005657[13] = 0; - out_1159288756680005657[14] = 0; - out_1159288756680005657[15] = 1; - out_1159288756680005657[16] = 0; - out_1159288756680005657[17] = 0; - out_1159288756680005657[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_1159288756680005657[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_1159288756680005657[20] = 0; - out_1159288756680005657[21] = state[8]; - out_1159288756680005657[22] = 0; - out_1159288756680005657[23] = -state[6]; - out_1159288756680005657[24] = -state[5]; - out_1159288756680005657[25] = 0; - out_1159288756680005657[26] = state[3]; - out_1159288756680005657[27] = 0; - out_1159288756680005657[28] = 0; - out_1159288756680005657[29] = 0; - out_1159288756680005657[30] = 0; - out_1159288756680005657[31] = 1; - out_1159288756680005657[32] = 0; - out_1159288756680005657[33] = 0; - out_1159288756680005657[34] = 1; - out_1159288756680005657[35] = 0; - out_1159288756680005657[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_1159288756680005657[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_1159288756680005657[38] = 0; - out_1159288756680005657[39] = -state[7]; - out_1159288756680005657[40] = state[6]; - out_1159288756680005657[41] = 0; - out_1159288756680005657[42] = state[4]; - out_1159288756680005657[43] = -state[3]; - out_1159288756680005657[44] = 0; - out_1159288756680005657[45] = 0; - out_1159288756680005657[46] = 0; - out_1159288756680005657[47] = 0; - out_1159288756680005657[48] = 0; - out_1159288756680005657[49] = 0; - out_1159288756680005657[50] = 1; - out_1159288756680005657[51] = 0; - out_1159288756680005657[52] = 0; - out_1159288756680005657[53] = 1; +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_13(double *state, double *unused, double *out_4379566868646893753) { - out_4379566868646893753[0] = state[3]; - out_4379566868646893753[1] = state[4]; - out_4379566868646893753[2] = state[5]; +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_8158161469567459860) { - out_8158161469567459860[0] = 0; - out_8158161469567459860[1] = 0; - out_8158161469567459860[2] = 0; - out_8158161469567459860[3] = 1; - out_8158161469567459860[4] = 0; - out_8158161469567459860[5] = 0; - out_8158161469567459860[6] = 0; - out_8158161469567459860[7] = 0; - out_8158161469567459860[8] = 0; - out_8158161469567459860[9] = 0; - out_8158161469567459860[10] = 0; - out_8158161469567459860[11] = 0; - out_8158161469567459860[12] = 0; - out_8158161469567459860[13] = 0; - out_8158161469567459860[14] = 0; - out_8158161469567459860[15] = 0; - out_8158161469567459860[16] = 0; - out_8158161469567459860[17] = 0; - out_8158161469567459860[18] = 0; - out_8158161469567459860[19] = 0; - out_8158161469567459860[20] = 0; - out_8158161469567459860[21] = 0; - out_8158161469567459860[22] = 1; - out_8158161469567459860[23] = 0; - out_8158161469567459860[24] = 0; - out_8158161469567459860[25] = 0; - out_8158161469567459860[26] = 0; - out_8158161469567459860[27] = 0; - out_8158161469567459860[28] = 0; - out_8158161469567459860[29] = 0; - out_8158161469567459860[30] = 0; - out_8158161469567459860[31] = 0; - out_8158161469567459860[32] = 0; - out_8158161469567459860[33] = 0; - out_8158161469567459860[34] = 0; - out_8158161469567459860[35] = 0; - out_8158161469567459860[36] = 0; - out_8158161469567459860[37] = 0; - out_8158161469567459860[38] = 0; - out_8158161469567459860[39] = 0; - out_8158161469567459860[40] = 0; - out_8158161469567459860[41] = 1; - out_8158161469567459860[42] = 0; - out_8158161469567459860[43] = 0; - out_8158161469567459860[44] = 0; - out_8158161469567459860[45] = 0; - out_8158161469567459860[46] = 0; - out_8158161469567459860[47] = 0; - out_8158161469567459860[48] = 0; - out_8158161469567459860[49] = 0; - out_8158161469567459860[50] = 0; - out_8158161469567459860[51] = 0; - out_8158161469567459860[52] = 0; - out_8158161469567459860[53] = 0; +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_14(double *state, double *unused, double *out_8438977474921151403) { - out_8438977474921151403[0] = state[6]; - out_8438977474921151403[1] = state[7]; - out_8438977474921151403[2] = state[8]; +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_7407194438560308132) { - out_7407194438560308132[0] = 0; - out_7407194438560308132[1] = 0; - out_7407194438560308132[2] = 0; - out_7407194438560308132[3] = 0; - out_7407194438560308132[4] = 0; - out_7407194438560308132[5] = 0; - out_7407194438560308132[6] = 1; - out_7407194438560308132[7] = 0; - out_7407194438560308132[8] = 0; - out_7407194438560308132[9] = 0; - out_7407194438560308132[10] = 0; - out_7407194438560308132[11] = 0; - out_7407194438560308132[12] = 0; - out_7407194438560308132[13] = 0; - out_7407194438560308132[14] = 0; - out_7407194438560308132[15] = 0; - out_7407194438560308132[16] = 0; - out_7407194438560308132[17] = 0; - out_7407194438560308132[18] = 0; - out_7407194438560308132[19] = 0; - out_7407194438560308132[20] = 0; - out_7407194438560308132[21] = 0; - out_7407194438560308132[22] = 0; - out_7407194438560308132[23] = 0; - out_7407194438560308132[24] = 0; - out_7407194438560308132[25] = 1; - out_7407194438560308132[26] = 0; - out_7407194438560308132[27] = 0; - out_7407194438560308132[28] = 0; - out_7407194438560308132[29] = 0; - out_7407194438560308132[30] = 0; - out_7407194438560308132[31] = 0; - out_7407194438560308132[32] = 0; - out_7407194438560308132[33] = 0; - out_7407194438560308132[34] = 0; - out_7407194438560308132[35] = 0; - out_7407194438560308132[36] = 0; - out_7407194438560308132[37] = 0; - out_7407194438560308132[38] = 0; - out_7407194438560308132[39] = 0; - out_7407194438560308132[40] = 0; - out_7407194438560308132[41] = 0; - out_7407194438560308132[42] = 0; - out_7407194438560308132[43] = 0; - out_7407194438560308132[44] = 1; - out_7407194438560308132[45] = 0; - out_7407194438560308132[46] = 0; - out_7407194438560308132[47] = 0; - out_7407194438560308132[48] = 0; - out_7407194438560308132[49] = 0; - out_7407194438560308132[50] = 0; - out_7407194438560308132[51] = 0; - out_7407194438560308132[52] = 0; - out_7407194438560308132[53] = 0; +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; } #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_6989025868442055132) { - err_fun(nom_x, delta_x, out_6989025868442055132); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8207359146711228947) { + err_fun(nom_x, delta_x, out_8207359146711228947); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3172938755845140042) { - inv_err_fun(nom_x, true_x, out_3172938755845140042); +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_H_mod_fun(double *state, double *out_3225867917449183727) { - H_mod_fun(state, out_3225867917449183727); +void pose_H_mod_fun(double *state, double *out_7202306802530511060) { + H_mod_fun(state, out_7202306802530511060); } -void pose_f_fun(double *state, double dt, double *out_2533972772359230172) { - f_fun(state, dt, out_2533972772359230172); +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_3663800095301942537) { - F_fun(state, dt, out_3663800095301942537); +void pose_F_fun(double *state, double dt, double *out_4383751150204030030) { + F_fun(state, dt, out_4383751150204030030); } -void pose_h_4(double *state, double *unused, double *out_223446469702120852) { - h_4(state, unused, out_223446469702120852); +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_4324406006264935836) { - H_4(state, unused, out_4324406006264935836); +void pose_H_4(double *state, double *unused, double *out_7970215192388091303) { + H_4(state, unused, out_7970215192388091303); } -void pose_h_10(double *state, double *unused, double *out_2753848967308463071) { - h_10(state, unused, out_2753848967308463071); +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_1159288756680005657) { - H_10(state, unused, out_1159288756680005657); +void pose_H_10(double *state, double *unused, double *out_7914634342859917414) { + H_10(state, unused, out_7914634342859917414); } -void pose_h_13(double *state, double *unused, double *out_4379566868646893753) { - h_13(state, unused, out_4379566868646893753); +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_8158161469567459860) { - H_13(state, unused, out_8158161469567459860); +void pose_H_13(double *state, double *unused, double *out_7264255055989127512) { + H_13(state, unused, out_7264255055989127512); } -void pose_h_14(double *state, double *unused, double *out_8438977474921151403) { - h_14(state, unused, out_8438977474921151403); +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_7407194438560308132) { - H_14(state, unused, out_7407194438560308132); +void pose_H_14(double *state, double *unused, double *out_6513288024981975784) { + H_14(state, unused, out_6513288024981975784); } 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 7d7de395c..106fa2f23 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_6989025868442055132); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3172938755845140042); -void pose_H_mod_fun(double *state, double *out_3225867917449183727); -void pose_f_fun(double *state, double dt, double *out_2533972772359230172); -void pose_F_fun(double *state, double dt, double *out_3663800095301942537); -void pose_h_4(double *state, double *unused, double *out_223446469702120852); -void pose_H_4(double *state, double *unused, double *out_4324406006264935836); -void pose_h_10(double *state, double *unused, double *out_2753848967308463071); -void pose_H_10(double *state, double *unused, double *out_1159288756680005657); -void pose_h_13(double *state, double *unused, double *out_4379566868646893753); -void pose_H_13(double *state, double *unused, double *out_8158161469567459860); -void pose_h_14(double *state, double *unused, double *out_8438977474921151403); -void pose_H_14(double *state, double *unused, double *out_7407194438560308132); +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_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 3d28919ce..f5726b822 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 46d4d55d4..02a479a0a 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 447d80820..8316c7cf6 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 65f318fac..17fc208b1 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 8cad6fd44..6551b9a75 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 97c8dcdb8..0dee1b642 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ