diff --git a/launch_env.sh b/launch_env.sh index 9bee47837a..aff451e541 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="11.11" + export AGNOS_VERSION="11.13" fi export STAGING_ROOT="/data/safe_staging" diff --git a/panda/board/jungle/obj/bootstub.panda_jungle.bin b/panda/board/jungle/obj/bootstub.panda_jungle.bin index ac539c9471..38b86458c3 100755 Binary files a/panda/board/jungle/obj/bootstub.panda_jungle.bin and b/panda/board/jungle/obj/bootstub.panda_jungle.bin differ diff --git a/panda/board/jungle/obj/bootstub.panda_jungle.elf b/panda/board/jungle/obj/bootstub.panda_jungle.elf index 05015477e9..331ff44963 100755 Binary files a/panda/board/jungle/obj/bootstub.panda_jungle.elf and b/panda/board/jungle/obj/bootstub.panda_jungle.elf differ diff --git a/panda/board/jungle/obj/bootstub.panda_jungle_h7.bin b/panda/board/jungle/obj/bootstub.panda_jungle_h7.bin index 2eb5de326c..487b1aa798 100755 Binary files a/panda/board/jungle/obj/bootstub.panda_jungle_h7.bin and b/panda/board/jungle/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/jungle/obj/bootstub.panda_jungle_h7.elf b/panda/board/jungle/obj/bootstub.panda_jungle_h7.elf index 15f14a9674..415ea41ca1 100755 Binary files a/panda/board/jungle/obj/bootstub.panda_jungle_h7.elf and b/panda/board/jungle/obj/bootstub.panda_jungle_h7.elf differ diff --git a/panda/board/jungle/obj/panda_jungle.bin b/panda/board/jungle/obj/panda_jungle.bin index ea902e09e4..9a148bccec 100755 Binary files a/panda/board/jungle/obj/panda_jungle.bin and b/panda/board/jungle/obj/panda_jungle.bin differ diff --git a/panda/board/jungle/obj/panda_jungle.bin.signed b/panda/board/jungle/obj/panda_jungle.bin.signed index 4bec5688d6..42ce869d80 100644 Binary files a/panda/board/jungle/obj/panda_jungle.bin.signed and b/panda/board/jungle/obj/panda_jungle.bin.signed differ diff --git a/panda/board/jungle/obj/panda_jungle.elf b/panda/board/jungle/obj/panda_jungle.elf index b33b4b2fd9..33e14cc1f7 100755 Binary files a/panda/board/jungle/obj/panda_jungle.elf and b/panda/board/jungle/obj/panda_jungle.elf differ diff --git a/panda/board/jungle/obj/panda_jungle_h7.bin b/panda/board/jungle/obj/panda_jungle_h7.bin index 6bceda62f7..e157745810 100755 Binary files a/panda/board/jungle/obj/panda_jungle_h7.bin and b/panda/board/jungle/obj/panda_jungle_h7.bin differ diff --git a/panda/board/jungle/obj/panda_jungle_h7.bin.signed b/panda/board/jungle/obj/panda_jungle_h7.bin.signed index 950a04a21d..8b03fb6ff6 100644 Binary files a/panda/board/jungle/obj/panda_jungle_h7.bin.signed and b/panda/board/jungle/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/jungle/obj/panda_jungle_h7.elf b/panda/board/jungle/obj/panda_jungle_h7.elf index bdc02c2084..d947ed5591 100755 Binary files a/panda/board/jungle/obj/panda_jungle_h7.elf and b/panda/board/jungle/obj/panda_jungle_h7.elf differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index ddac68a661..acacc1c41c 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.elf b/panda/board/obj/bootstub.panda.elf index a0e3c686e1..c9138a9f2b 100755 Binary files a/panda/board/obj/bootstub.panda.elf and b/panda/board/obj/bootstub.panda.elf differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index e575e25e72..4e34223567 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.elf b/panda/board/obj/bootstub.panda_h7.elf index 1d515cf269..60df3cffcd 100755 Binary files a/panda/board/obj/bootstub.panda_h7.elf and b/panda/board/obj/bootstub.panda_h7.elf differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index e99e6f9797..57c34db159 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[20]; -const uint8_t gitversion[20] = "DEV-6f01a19e-RELEASE"; +const uint8_t gitversion[20] = "DEV-7bab1879-RELEASE"; diff --git a/panda/board/obj/panda.bin b/panda/board/obj/panda.bin index 2a78d5ea87..0e1b13c07f 100755 Binary files a/panda/board/obj/panda.bin and b/panda/board/obj/panda.bin differ diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 0dd36bd64c..e9db17b708 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.elf b/panda/board/obj/panda.elf index d16b3e1432..a79d18acc2 100755 Binary files a/panda/board/obj/panda.elf and b/panda/board/obj/panda.elf differ diff --git a/panda/board/obj/panda_h7.bin b/panda/board/obj/panda_h7.bin index 9a32507287..d100067377 100755 Binary files a/panda/board/obj/panda_h7.bin and b/panda/board/obj/panda_h7.bin differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index 15ae23ca78..2887c27a0f 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.elf b/panda/board/obj/panda_h7.elf index 15981a4c54..6df5b35d94 100755 Binary files a/panda/board/obj/panda_h7.elf and b/panda/board/obj/panda_h7.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index 6f46d15ac2..6bdbb323bf 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-6f01a19e-RELEASE \ No newline at end of file +DEV-7bab1879-RELEASE \ No newline at end of file diff --git a/selfdrive/assets/assets.cc b/selfdrive/assets/assets.cc index 37a666242e..6d134ea477 100644 --- a/selfdrive/assets/assets.cc +++ b/selfdrive/assets/assets.cc @@ -37041,88 +37041,88 @@ static const unsigned char qt_resource_struct[] = { 0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0, // :/bootstrap-icons.svg 0x0,0x0,0x1,0x4,0x0,0x1,0x0,0x0,0x0,0x1,0x0,0x1,0xf9,0xf0, -0x0,0x0,0x1,0x95,0x8b,0x75,0xc6,0xf6, +0x0,0x0,0x1,0x95,0x8b,0x56,0x50,0x67, // :/main_es 0x0,0x0,0x2,0x4c,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x8,0x29,0xd4, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0x2c, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf6,0x19, // :/main_fr 0x0,0x0,0x1,0xbe,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x7,0x10,0xea, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0x18, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf6,0x5, // :/main_de 0x0,0x0,0x1,0xaa,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x6,0xb1,0x3d, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0xe, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf5,0xfb, // :/main_en 0x0,0x0,0x1,0xd2,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x7,0xa7,0x78, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0x4, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf5,0xf1, // :/main_ar 0x0,0x0,0x1,0x82,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x5,0xa1,0xd4, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0x2c, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf6,0x2d, // :/main_th 0x0,0x0,0x0,0x8e,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x0,0x5,0x8f, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0x36, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf6,0x2d, // :/main_tr 0x0,0x0,0x1,0x44,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x5,0x50,0xaf, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0x2c, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf6,0x23, // :/main_ja 0x0,0x0,0x1,0x96,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x6,0x41,0x40, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0x4a, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf6,0x4b, // :/main_ko 0x0,0x0,0x1,0xe6,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x7,0xab,0x14, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0x40, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf6,0x41, // :/main_pt-BR 0x0,0x0,0x0,0xa2,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x0,0x86,0x6d, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0x22, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf6,0xf, // :/offroad 0x0,0x0,0x1,0x30,0x0,0x2,0x0,0x0,0x0,0x7,0x0,0x0,0x0,0x16, 0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0, // :/main_zh-CHS 0x0,0x0,0x0,0xbc,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x1,0x2e,0xfa, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0x40, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf6,0x41, // :/main_zh-CHT 0x0,0x0,0x0,0xe8,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x1,0x94,0x2f, -0x0,0x0,0x1,0x95,0x8d,0x4f,0xb3,0x36, +0x0,0x0,0x1,0x95,0x8e,0xb,0xf6,0x37, // :/img_continue_triangle.svg 0x0,0x0,0x0,0x56,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x0,0x4,0xc6, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x28, +0x0,0x0,0x1,0x95,0x8b,0x56,0x46,0x3, // :/img_circled_slash.svg 0x0,0x0,0x1,0xfa,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x8,0x27,0x2c, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x1e, +0x0,0x0,0x1,0x95,0x8b,0x56,0x46,0x3, // :/img_eye_closed.svg 0x0,0x0,0x1,0x58,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x5,0x9b,0xfe, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x32, +0x0,0x0,0x1,0x95,0x8b,0x56,0x46,0xd, // :/img_circled_check.svg 0x0,0x0,0x0,0x26,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x0,0x3,0xb, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x1e, +0x0,0x0,0x1,0x95,0x8b,0x56,0x45,0xf9, // :/languages.json 0x0,0x0,0x2,0x2a,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x8,0x28,0x78, -0x0,0x0,0x1,0x95,0x8b,0x75,0xc2,0x46, +0x0,0x0,0x1,0x95,0x8b,0x56,0x4b,0x2b, // :/img_eye_open.svg 0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x0,0x0,0x0, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x32, +0x0,0x0,0x1,0x95,0x8b,0x56,0x46,0xd, // :/offroad/icon_checkmark.svg 0x0,0x0,0x3,0x6c,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x8,0xf1,0x6b, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x46, +0x0,0x0,0x1,0x95,0x8b,0x56,0x46,0x21, // :/offroad/icon_lock_closed.svg 0x0,0x0,0x2,0x60,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x8,0xbb,0xce, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x50, +0x0,0x0,0x1,0x95,0x8b,0x56,0x46,0x21, // :/offroad/icon_warning.png 0x0,0x0,0x3,0x6,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x8,0xcb,0xa6, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x64, +0x0,0x0,0x1,0x95,0x8b,0x56,0x46,0x35, // :/offroad/icon_wifi_strength_high.svg 0x0,0x0,0x2,0x8e,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x8,0xbe,0xae, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x6e, +0x0,0x0,0x1,0x95,0x8b,0x56,0x46,0x3f, // :/offroad/icon_wifi_strength_low.svg 0x0,0x0,0x3,0x96,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x8,0xf2,0x62, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x6e, +0x0,0x0,0x1,0x95,0x8b,0x56,0x46,0x3f, // :/offroad/icon_wifi_strength_full.svg 0x0,0x0,0x2,0xca,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x8,0xc5,0x2b, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x64, +0x0,0x0,0x1,0x95,0x8b,0x56,0x46,0x35, // :/offroad/icon_wifi_strength_medium.svg 0x0,0x0,0x3,0x2c,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x8,0xea,0xec, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x6e, +0x0,0x0,0x1,0x95,0x8b,0x56,0x46,0x3f, // :/icons/close.svg 0x0,0x0,0x3,0xd0,0x0,0x0,0x0,0x0,0x0,0x1,0x0,0x8,0xf8,0xe3, -0x0,0x0,0x1,0x95,0x8b,0x75,0xbd,0x14, +0x0,0x0,0x1,0x95,0x8b,0x56,0x45,0xef, }; diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 15764a16bf..c7ce1a4d65 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_3675669119552892075) { - out_3675669119552892075[0] = delta_x[0] + nom_x[0]; - out_3675669119552892075[1] = delta_x[1] + nom_x[1]; - out_3675669119552892075[2] = delta_x[2] + nom_x[2]; - out_3675669119552892075[3] = delta_x[3] + nom_x[3]; - out_3675669119552892075[4] = delta_x[4] + nom_x[4]; - out_3675669119552892075[5] = delta_x[5] + nom_x[5]; - out_3675669119552892075[6] = delta_x[6] + nom_x[6]; - out_3675669119552892075[7] = delta_x[7] + nom_x[7]; - out_3675669119552892075[8] = delta_x[8] + nom_x[8]; -} -void inv_err_fun(double *nom_x, double *true_x, double *out_1525509250774896015) { - out_1525509250774896015[0] = -nom_x[0] + true_x[0]; - out_1525509250774896015[1] = -nom_x[1] + true_x[1]; - out_1525509250774896015[2] = -nom_x[2] + true_x[2]; - out_1525509250774896015[3] = -nom_x[3] + true_x[3]; - out_1525509250774896015[4] = -nom_x[4] + true_x[4]; - out_1525509250774896015[5] = -nom_x[5] + true_x[5]; - out_1525509250774896015[6] = -nom_x[6] + true_x[6]; - out_1525509250774896015[7] = -nom_x[7] + true_x[7]; - out_1525509250774896015[8] = -nom_x[8] + true_x[8]; -} -void H_mod_fun(double *state, double *out_8908179234073782012) { - out_8908179234073782012[0] = 1.0; - out_8908179234073782012[1] = 0.0; - out_8908179234073782012[2] = 0.0; - out_8908179234073782012[3] = 0.0; - out_8908179234073782012[4] = 0.0; - out_8908179234073782012[5] = 0.0; - out_8908179234073782012[6] = 0.0; - out_8908179234073782012[7] = 0.0; - out_8908179234073782012[8] = 0.0; - out_8908179234073782012[9] = 0.0; - out_8908179234073782012[10] = 1.0; - out_8908179234073782012[11] = 0.0; - out_8908179234073782012[12] = 0.0; - out_8908179234073782012[13] = 0.0; - out_8908179234073782012[14] = 0.0; - out_8908179234073782012[15] = 0.0; - out_8908179234073782012[16] = 0.0; - out_8908179234073782012[17] = 0.0; - out_8908179234073782012[18] = 0.0; - out_8908179234073782012[19] = 0.0; - out_8908179234073782012[20] = 1.0; - out_8908179234073782012[21] = 0.0; - out_8908179234073782012[22] = 0.0; - out_8908179234073782012[23] = 0.0; - out_8908179234073782012[24] = 0.0; - out_8908179234073782012[25] = 0.0; - out_8908179234073782012[26] = 0.0; - out_8908179234073782012[27] = 0.0; - out_8908179234073782012[28] = 0.0; - out_8908179234073782012[29] = 0.0; - out_8908179234073782012[30] = 1.0; - out_8908179234073782012[31] = 0.0; - out_8908179234073782012[32] = 0.0; - out_8908179234073782012[33] = 0.0; - out_8908179234073782012[34] = 0.0; - out_8908179234073782012[35] = 0.0; - out_8908179234073782012[36] = 0.0; - out_8908179234073782012[37] = 0.0; - out_8908179234073782012[38] = 0.0; - out_8908179234073782012[39] = 0.0; - out_8908179234073782012[40] = 1.0; - out_8908179234073782012[41] = 0.0; - out_8908179234073782012[42] = 0.0; - out_8908179234073782012[43] = 0.0; - out_8908179234073782012[44] = 0.0; - out_8908179234073782012[45] = 0.0; - out_8908179234073782012[46] = 0.0; - out_8908179234073782012[47] = 0.0; - out_8908179234073782012[48] = 0.0; - out_8908179234073782012[49] = 0.0; - out_8908179234073782012[50] = 1.0; - out_8908179234073782012[51] = 0.0; - out_8908179234073782012[52] = 0.0; - out_8908179234073782012[53] = 0.0; - out_8908179234073782012[54] = 0.0; - out_8908179234073782012[55] = 0.0; - out_8908179234073782012[56] = 0.0; - out_8908179234073782012[57] = 0.0; - out_8908179234073782012[58] = 0.0; - out_8908179234073782012[59] = 0.0; - out_8908179234073782012[60] = 1.0; - out_8908179234073782012[61] = 0.0; - out_8908179234073782012[62] = 0.0; - out_8908179234073782012[63] = 0.0; - out_8908179234073782012[64] = 0.0; - out_8908179234073782012[65] = 0.0; - out_8908179234073782012[66] = 0.0; - out_8908179234073782012[67] = 0.0; - out_8908179234073782012[68] = 0.0; - out_8908179234073782012[69] = 0.0; - out_8908179234073782012[70] = 1.0; - out_8908179234073782012[71] = 0.0; - out_8908179234073782012[72] = 0.0; - out_8908179234073782012[73] = 0.0; - out_8908179234073782012[74] = 0.0; - out_8908179234073782012[75] = 0.0; - out_8908179234073782012[76] = 0.0; - out_8908179234073782012[77] = 0.0; - out_8908179234073782012[78] = 0.0; - out_8908179234073782012[79] = 0.0; - out_8908179234073782012[80] = 1.0; -} -void f_fun(double *state, double dt, double *out_2817676241617045765) { - out_2817676241617045765[0] = state[0]; - out_2817676241617045765[1] = state[1]; - out_2817676241617045765[2] = state[2]; - out_2817676241617045765[3] = state[3]; - out_2817676241617045765[4] = state[4]; - out_2817676241617045765[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*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_2817676241617045765[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_2817676241617045765[7] = state[7]; - out_2817676241617045765[8] = state[8]; -} -void F_fun(double *state, double dt, double *out_587119351607645481) { - out_587119351607645481[0] = 1; - out_587119351607645481[1] = 0; - out_587119351607645481[2] = 0; - out_587119351607645481[3] = 0; - out_587119351607645481[4] = 0; - out_587119351607645481[5] = 0; - out_587119351607645481[6] = 0; - out_587119351607645481[7] = 0; - out_587119351607645481[8] = 0; - out_587119351607645481[9] = 0; - out_587119351607645481[10] = 1; - out_587119351607645481[11] = 0; - out_587119351607645481[12] = 0; - out_587119351607645481[13] = 0; - out_587119351607645481[14] = 0; - out_587119351607645481[15] = 0; - out_587119351607645481[16] = 0; - out_587119351607645481[17] = 0; - out_587119351607645481[18] = 0; - out_587119351607645481[19] = 0; - out_587119351607645481[20] = 1; - out_587119351607645481[21] = 0; - out_587119351607645481[22] = 0; - out_587119351607645481[23] = 0; - out_587119351607645481[24] = 0; - out_587119351607645481[25] = 0; - out_587119351607645481[26] = 0; - out_587119351607645481[27] = 0; - out_587119351607645481[28] = 0; - out_587119351607645481[29] = 0; - out_587119351607645481[30] = 1; - out_587119351607645481[31] = 0; - out_587119351607645481[32] = 0; - out_587119351607645481[33] = 0; - out_587119351607645481[34] = 0; - out_587119351607645481[35] = 0; - out_587119351607645481[36] = 0; - out_587119351607645481[37] = 0; - out_587119351607645481[38] = 0; - out_587119351607645481[39] = 0; - out_587119351607645481[40] = 1; - out_587119351607645481[41] = 0; - out_587119351607645481[42] = 0; - out_587119351607645481[43] = 0; - out_587119351607645481[44] = 0; - out_587119351607645481[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_587119351607645481[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_587119351607645481[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_587119351607645481[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_587119351607645481[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_587119351607645481[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_587119351607645481[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_587119351607645481[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_587119351607645481[53] = -9.8000000000000007*dt; - out_587119351607645481[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_587119351607645481[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_587119351607645481[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_587119351607645481[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_587119351607645481[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_587119351607645481[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_587119351607645481[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_587119351607645481[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_587119351607645481[62] = 0; - out_587119351607645481[63] = 0; - out_587119351607645481[64] = 0; - out_587119351607645481[65] = 0; - out_587119351607645481[66] = 0; - out_587119351607645481[67] = 0; - out_587119351607645481[68] = 0; - out_587119351607645481[69] = 0; - out_587119351607645481[70] = 1; - out_587119351607645481[71] = 0; - out_587119351607645481[72] = 0; - out_587119351607645481[73] = 0; - out_587119351607645481[74] = 0; - out_587119351607645481[75] = 0; - out_587119351607645481[76] = 0; - out_587119351607645481[77] = 0; - out_587119351607645481[78] = 0; - out_587119351607645481[79] = 0; - out_587119351607645481[80] = 1; -} -void h_25(double *state, double *unused, double *out_2948496957738460434) { - out_2948496957738460434[0] = state[6]; -} -void H_25(double *state, double *unused, double *out_345376613723532367) { - out_345376613723532367[0] = 0; - out_345376613723532367[1] = 0; - out_345376613723532367[2] = 0; - out_345376613723532367[3] = 0; - out_345376613723532367[4] = 0; - out_345376613723532367[5] = 0; - out_345376613723532367[6] = 1; - out_345376613723532367[7] = 0; - out_345376613723532367[8] = 0; -} -void h_24(double *state, double *unused, double *out_1253312341000543437) { - out_1253312341000543437[0] = state[4]; - out_1253312341000543437[1] = state[5]; -} -void H_24(double *state, double *unused, double *out_8873302273916824024) { - out_8873302273916824024[0] = 0; - out_8873302273916824024[1] = 0; - out_8873302273916824024[2] = 0; - out_8873302273916824024[3] = 0; - out_8873302273916824024[4] = 1; - out_8873302273916824024[5] = 0; - out_8873302273916824024[6] = 0; - out_8873302273916824024[7] = 0; - out_8873302273916824024[8] = 0; - out_8873302273916824024[9] = 0; - out_8873302273916824024[10] = 0; - out_8873302273916824024[11] = 0; - out_8873302273916824024[12] = 0; - out_8873302273916824024[13] = 0; - out_8873302273916824024[14] = 1; - out_8873302273916824024[15] = 0; - out_8873302273916824024[16] = 0; - out_8873302273916824024[17] = 0; -} -void h_30(double *state, double *unused, double *out_4399128350367343601) { - out_4399128350367343601[0] = state[4]; -} -void H_30(double *state, double *unused, double *out_216037666580292297) { - out_216037666580292297[0] = 0; - out_216037666580292297[1] = 0; - out_216037666580292297[2] = 0; - out_216037666580292297[3] = 0; - out_216037666580292297[4] = 1; - out_216037666580292297[5] = 0; - out_216037666580292297[6] = 0; - out_216037666580292297[7] = 0; - out_216037666580292297[8] = 0; -} -void h_26(double *state, double *unused, double *out_1362342298923010916) { - out_1362342298923010916[0] = state[7]; -} -void H_26(double *state, double *unused, double *out_3396126705150523857) { - out_3396126705150523857[0] = 0; - out_3396126705150523857[1] = 0; - out_3396126705150523857[2] = 0; - out_3396126705150523857[3] = 0; - out_3396126705150523857[4] = 0; - out_3396126705150523857[5] = 0; - out_3396126705150523857[6] = 0; - out_3396126705150523857[7] = 1; - out_3396126705150523857[8] = 0; -} -void h_27(double *state, double *unused, double *out_9204321679697507688) { - out_9204321679697507688[0] = state[3]; -} -void H_27(double *state, double *unused, double *out_1958725645220132614) { - out_1958725645220132614[0] = 0; - out_1958725645220132614[1] = 0; - out_1958725645220132614[2] = 0; - out_1958725645220132614[3] = 1; - out_1958725645220132614[4] = 0; - out_1958725645220132614[5] = 0; - out_1958725645220132614[6] = 0; - out_1958725645220132614[7] = 0; - out_1958725645220132614[8] = 0; -} -void h_29(double *state, double *unused, double *out_602544917293502044) { - out_602544917293502044[0] = state[1]; -} -void H_29(double *state, double *unused, double *out_3672088372089683647) { - out_3672088372089683647[0] = 0; - out_3672088372089683647[1] = 1; - out_3672088372089683647[2] = 0; - out_3672088372089683647[3] = 0; - out_3672088372089683647[4] = 0; - out_3672088372089683647[5] = 0; - out_3672088372089683647[6] = 0; - out_3672088372089683647[7] = 0; - out_3672088372089683647[8] = 0; -} -void h_28(double *state, double *unused, double *out_4281120956433966857) { - out_4281120956433966857[0] = state[0]; -} -void H_28(double *state, double *unused, double *out_1708458100524357396) { - out_1708458100524357396[0] = 1; - out_1708458100524357396[1] = 0; - out_1708458100524357396[2] = 0; - out_1708458100524357396[3] = 0; - out_1708458100524357396[4] = 0; - out_1708458100524357396[5] = 0; - out_1708458100524357396[6] = 0; - out_1708458100524357396[7] = 0; - out_1708458100524357396[8] = 0; -} -void h_31(double *state, double *unused, double *out_5944889882572371594) { - out_5944889882572371594[0] = state[8]; -} -void H_31(double *state, double *unused, double *out_376022575600492795) { - out_376022575600492795[0] = 0; - out_376022575600492795[1] = 0; - out_376022575600492795[2] = 0; - out_376022575600492795[3] = 0; - out_376022575600492795[4] = 0; - out_376022575600492795[5] = 0; - out_376022575600492795[6] = 0; - out_376022575600492795[7] = 0; - out_376022575600492795[8] = 1; +void err_fun(double *nom_x, double *delta_x, double *out_7345504276725310336) { + out_7345504276725310336[0] = delta_x[0] + nom_x[0]; + out_7345504276725310336[1] = delta_x[1] + nom_x[1]; + out_7345504276725310336[2] = delta_x[2] + nom_x[2]; + out_7345504276725310336[3] = delta_x[3] + nom_x[3]; + out_7345504276725310336[4] = delta_x[4] + nom_x[4]; + out_7345504276725310336[5] = delta_x[5] + nom_x[5]; + out_7345504276725310336[6] = delta_x[6] + nom_x[6]; + out_7345504276725310336[7] = delta_x[7] + nom_x[7]; + out_7345504276725310336[8] = delta_x[8] + nom_x[8]; +} +void inv_err_fun(double *nom_x, double *true_x, double *out_1483419534327179826) { + out_1483419534327179826[0] = -nom_x[0] + true_x[0]; + out_1483419534327179826[1] = -nom_x[1] + true_x[1]; + out_1483419534327179826[2] = -nom_x[2] + true_x[2]; + out_1483419534327179826[3] = -nom_x[3] + true_x[3]; + out_1483419534327179826[4] = -nom_x[4] + true_x[4]; + out_1483419534327179826[5] = -nom_x[5] + true_x[5]; + out_1483419534327179826[6] = -nom_x[6] + true_x[6]; + out_1483419534327179826[7] = -nom_x[7] + true_x[7]; + out_1483419534327179826[8] = -nom_x[8] + true_x[8]; +} +void H_mod_fun(double *state, double *out_5980828091480380917) { + out_5980828091480380917[0] = 1.0; + out_5980828091480380917[1] = 0.0; + out_5980828091480380917[2] = 0.0; + out_5980828091480380917[3] = 0.0; + out_5980828091480380917[4] = 0.0; + out_5980828091480380917[5] = 0.0; + out_5980828091480380917[6] = 0.0; + out_5980828091480380917[7] = 0.0; + out_5980828091480380917[8] = 0.0; + out_5980828091480380917[9] = 0.0; + out_5980828091480380917[10] = 1.0; + out_5980828091480380917[11] = 0.0; + out_5980828091480380917[12] = 0.0; + out_5980828091480380917[13] = 0.0; + out_5980828091480380917[14] = 0.0; + out_5980828091480380917[15] = 0.0; + out_5980828091480380917[16] = 0.0; + out_5980828091480380917[17] = 0.0; + out_5980828091480380917[18] = 0.0; + out_5980828091480380917[19] = 0.0; + out_5980828091480380917[20] = 1.0; + out_5980828091480380917[21] = 0.0; + out_5980828091480380917[22] = 0.0; + out_5980828091480380917[23] = 0.0; + out_5980828091480380917[24] = 0.0; + out_5980828091480380917[25] = 0.0; + out_5980828091480380917[26] = 0.0; + out_5980828091480380917[27] = 0.0; + out_5980828091480380917[28] = 0.0; + out_5980828091480380917[29] = 0.0; + out_5980828091480380917[30] = 1.0; + out_5980828091480380917[31] = 0.0; + out_5980828091480380917[32] = 0.0; + out_5980828091480380917[33] = 0.0; + out_5980828091480380917[34] = 0.0; + out_5980828091480380917[35] = 0.0; + out_5980828091480380917[36] = 0.0; + out_5980828091480380917[37] = 0.0; + out_5980828091480380917[38] = 0.0; + out_5980828091480380917[39] = 0.0; + out_5980828091480380917[40] = 1.0; + out_5980828091480380917[41] = 0.0; + out_5980828091480380917[42] = 0.0; + out_5980828091480380917[43] = 0.0; + out_5980828091480380917[44] = 0.0; + out_5980828091480380917[45] = 0.0; + out_5980828091480380917[46] = 0.0; + out_5980828091480380917[47] = 0.0; + out_5980828091480380917[48] = 0.0; + out_5980828091480380917[49] = 0.0; + out_5980828091480380917[50] = 1.0; + out_5980828091480380917[51] = 0.0; + out_5980828091480380917[52] = 0.0; + out_5980828091480380917[53] = 0.0; + out_5980828091480380917[54] = 0.0; + out_5980828091480380917[55] = 0.0; + out_5980828091480380917[56] = 0.0; + out_5980828091480380917[57] = 0.0; + out_5980828091480380917[58] = 0.0; + out_5980828091480380917[59] = 0.0; + out_5980828091480380917[60] = 1.0; + out_5980828091480380917[61] = 0.0; + out_5980828091480380917[62] = 0.0; + out_5980828091480380917[63] = 0.0; + out_5980828091480380917[64] = 0.0; + out_5980828091480380917[65] = 0.0; + out_5980828091480380917[66] = 0.0; + out_5980828091480380917[67] = 0.0; + out_5980828091480380917[68] = 0.0; + out_5980828091480380917[69] = 0.0; + out_5980828091480380917[70] = 1.0; + out_5980828091480380917[71] = 0.0; + out_5980828091480380917[72] = 0.0; + out_5980828091480380917[73] = 0.0; + out_5980828091480380917[74] = 0.0; + out_5980828091480380917[75] = 0.0; + out_5980828091480380917[76] = 0.0; + out_5980828091480380917[77] = 0.0; + out_5980828091480380917[78] = 0.0; + out_5980828091480380917[79] = 0.0; + out_5980828091480380917[80] = 1.0; +} +void f_fun(double *state, double dt, double *out_6771744444959754452) { + out_6771744444959754452[0] = state[0]; + out_6771744444959754452[1] = state[1]; + out_6771744444959754452[2] = state[2]; + out_6771744444959754452[3] = state[3]; + out_6771744444959754452[4] = state[4]; + out_6771744444959754452[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*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_6771744444959754452[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_6771744444959754452[7] = state[7]; + out_6771744444959754452[8] = state[8]; +} +void F_fun(double *state, double dt, double *out_8079006821062657786) { + out_8079006821062657786[0] = 1; + out_8079006821062657786[1] = 0; + out_8079006821062657786[2] = 0; + out_8079006821062657786[3] = 0; + out_8079006821062657786[4] = 0; + out_8079006821062657786[5] = 0; + out_8079006821062657786[6] = 0; + out_8079006821062657786[7] = 0; + out_8079006821062657786[8] = 0; + out_8079006821062657786[9] = 0; + out_8079006821062657786[10] = 1; + out_8079006821062657786[11] = 0; + out_8079006821062657786[12] = 0; + out_8079006821062657786[13] = 0; + out_8079006821062657786[14] = 0; + out_8079006821062657786[15] = 0; + out_8079006821062657786[16] = 0; + out_8079006821062657786[17] = 0; + out_8079006821062657786[18] = 0; + out_8079006821062657786[19] = 0; + out_8079006821062657786[20] = 1; + out_8079006821062657786[21] = 0; + out_8079006821062657786[22] = 0; + out_8079006821062657786[23] = 0; + out_8079006821062657786[24] = 0; + out_8079006821062657786[25] = 0; + out_8079006821062657786[26] = 0; + out_8079006821062657786[27] = 0; + out_8079006821062657786[28] = 0; + out_8079006821062657786[29] = 0; + out_8079006821062657786[30] = 1; + out_8079006821062657786[31] = 0; + out_8079006821062657786[32] = 0; + out_8079006821062657786[33] = 0; + out_8079006821062657786[34] = 0; + out_8079006821062657786[35] = 0; + out_8079006821062657786[36] = 0; + out_8079006821062657786[37] = 0; + out_8079006821062657786[38] = 0; + out_8079006821062657786[39] = 0; + out_8079006821062657786[40] = 1; + out_8079006821062657786[41] = 0; + out_8079006821062657786[42] = 0; + out_8079006821062657786[43] = 0; + out_8079006821062657786[44] = 0; + out_8079006821062657786[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_8079006821062657786[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_8079006821062657786[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8079006821062657786[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8079006821062657786[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_8079006821062657786[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_8079006821062657786[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_8079006821062657786[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_8079006821062657786[53] = -9.8000000000000007*dt; + out_8079006821062657786[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_8079006821062657786[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_8079006821062657786[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8079006821062657786[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8079006821062657786[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_8079006821062657786[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_8079006821062657786[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_8079006821062657786[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8079006821062657786[62] = 0; + out_8079006821062657786[63] = 0; + out_8079006821062657786[64] = 0; + out_8079006821062657786[65] = 0; + out_8079006821062657786[66] = 0; + out_8079006821062657786[67] = 0; + out_8079006821062657786[68] = 0; + out_8079006821062657786[69] = 0; + out_8079006821062657786[70] = 1; + out_8079006821062657786[71] = 0; + out_8079006821062657786[72] = 0; + out_8079006821062657786[73] = 0; + out_8079006821062657786[74] = 0; + out_8079006821062657786[75] = 0; + out_8079006821062657786[76] = 0; + out_8079006821062657786[77] = 0; + out_8079006821062657786[78] = 0; + out_8079006821062657786[79] = 0; + out_8079006821062657786[80] = 1; +} +void h_25(double *state, double *unused, double *out_6013514099318955817) { + out_6013514099318955817[0] = state[6]; +} +void H_25(double *state, double *unused, double *out_8892173204326502465) { + out_8892173204326502465[0] = 0; + out_8892173204326502465[1] = 0; + out_8892173204326502465[2] = 0; + out_8892173204326502465[3] = 0; + out_8892173204326502465[4] = 0; + out_8892173204326502465[5] = 0; + out_8892173204326502465[6] = 1; + out_8892173204326502465[7] = 0; + out_8892173204326502465[8] = 0; +} +void h_24(double *state, double *unused, double *out_6766398854122891435) { + out_6766398854122891435[0] = state[4]; + out_6766398854122891435[1] = state[5]; +} +void H_24(double *state, double *unused, double *out_7204356768457398274) { + out_7204356768457398274[0] = 0; + out_7204356768457398274[1] = 0; + out_7204356768457398274[2] = 0; + out_7204356768457398274[3] = 0; + out_7204356768457398274[4] = 1; + out_7204356768457398274[5] = 0; + out_7204356768457398274[6] = 0; + out_7204356768457398274[7] = 0; + out_7204356768457398274[8] = 0; + out_7204356768457398274[9] = 0; + out_7204356768457398274[10] = 0; + out_7204356768457398274[11] = 0; + out_7204356768457398274[12] = 0; + out_7204356768457398274[13] = 0; + out_7204356768457398274[14] = 1; + out_7204356768457398274[15] = 0; + out_7204356768457398274[16] = 0; + out_7204356768457398274[17] = 0; +} +void h_30(double *state, double *unused, double *out_5662394748040244863) { + out_5662394748040244863[0] = state[4]; +} +void H_30(double *state, double *unused, double *out_5026874539255440953) { + out_5026874539255440953[0] = 0; + out_5026874539255440953[1] = 0; + out_5026874539255440953[2] = 0; + out_5026874539255440953[3] = 0; + out_5026874539255440953[4] = 1; + out_5026874539255440953[5] = 0; + out_5026874539255440953[6] = 0; + out_5026874539255440953[7] = 0; + out_5026874539255440953[8] = 0; +} +void h_26(double *state, double *unused, double *out_253236005693617473) { + out_253236005693617473[0] = state[7]; +} +void H_26(double *state, double *unused, double *out_5813067550508992927) { + out_5813067550508992927[0] = 0; + out_5813067550508992927[1] = 0; + out_5813067550508992927[2] = 0; + out_5813067550508992927[3] = 0; + out_5813067550508992927[4] = 0; + out_5813067550508992927[5] = 0; + out_5813067550508992927[6] = 0; + out_5813067550508992927[7] = 1; + out_5813067550508992927[8] = 0; +} +void h_27(double *state, double *unused, double *out_6177405252431548545) { + out_6177405252431548545[0] = state[3]; +} +void H_27(double *state, double *unused, double *out_2852111227455016042) { + out_2852111227455016042[0] = 0; + out_2852111227455016042[1] = 0; + out_2852111227455016042[2] = 0; + out_2852111227455016042[3] = 1; + out_2852111227455016042[4] = 0; + out_2852111227455016042[5] = 0; + out_2852111227455016042[6] = 0; + out_2852111227455016042[7] = 0; + out_2852111227455016042[8] = 0; +} +void h_29(double *state, double *unused, double *out_8631889293023980721) { + out_8631889293023980721[0] = state[1]; +} +void H_29(double *state, double *unused, double *out_5537105883569833137) { + out_5537105883569833137[0] = 0; + out_5537105883569833137[1] = 1; + out_5537105883569833137[2] = 0; + out_5537105883569833137[3] = 0; + out_5537105883569833137[4] = 0; + out_5537105883569833137[5] = 0; + out_5537105883569833137[6] = 0; + out_5537105883569833137[7] = 0; + out_5537105883569833137[8] = 0; +} +void h_28(double *state, double *unused, double *out_7267665553617157426) { + out_7267665553617157426[0] = state[0]; +} +void H_28(double *state, double *unused, double *out_454706866500302563) { + out_454706866500302563[0] = 1; + out_454706866500302563[1] = 0; + out_454706866500302563[2] = 0; + out_454706866500302563[3] = 0; + out_454706866500302563[4] = 0; + out_454706866500302563[5] = 0; + out_454706866500302563[6] = 0; + out_454706866500302563[7] = 0; + out_454706866500302563[8] = 0; +} +void h_31(double *state, double *unused, double *out_2626363377399586067) { + out_2626363377399586067[0] = state[8]; +} +void H_31(double *state, double *unused, double *out_5186859448275641451) { + out_5186859448275641451[0] = 0; + out_5186859448275641451[1] = 0; + out_5186859448275641451[2] = 0; + out_5186859448275641451[3] = 0; + out_5186859448275641451[4] = 0; + out_5186859448275641451[5] = 0; + out_5186859448275641451[6] = 0; + out_5186859448275641451[7] = 0; + out_5186859448275641451[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_3675669119552892075) { - err_fun(nom_x, delta_x, out_3675669119552892075); +void car_err_fun(double *nom_x, double *delta_x, double *out_7345504276725310336) { + err_fun(nom_x, delta_x, out_7345504276725310336); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1525509250774896015) { - inv_err_fun(nom_x, true_x, out_1525509250774896015); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1483419534327179826) { + inv_err_fun(nom_x, true_x, out_1483419534327179826); } -void car_H_mod_fun(double *state, double *out_8908179234073782012) { - H_mod_fun(state, out_8908179234073782012); +void car_H_mod_fun(double *state, double *out_5980828091480380917) { + H_mod_fun(state, out_5980828091480380917); } -void car_f_fun(double *state, double dt, double *out_2817676241617045765) { - f_fun(state, dt, out_2817676241617045765); +void car_f_fun(double *state, double dt, double *out_6771744444959754452) { + f_fun(state, dt, out_6771744444959754452); } -void car_F_fun(double *state, double dt, double *out_587119351607645481) { - F_fun(state, dt, out_587119351607645481); +void car_F_fun(double *state, double dt, double *out_8079006821062657786) { + F_fun(state, dt, out_8079006821062657786); } -void car_h_25(double *state, double *unused, double *out_2948496957738460434) { - h_25(state, unused, out_2948496957738460434); +void car_h_25(double *state, double *unused, double *out_6013514099318955817) { + h_25(state, unused, out_6013514099318955817); } -void car_H_25(double *state, double *unused, double *out_345376613723532367) { - H_25(state, unused, out_345376613723532367); +void car_H_25(double *state, double *unused, double *out_8892173204326502465) { + H_25(state, unused, out_8892173204326502465); } -void car_h_24(double *state, double *unused, double *out_1253312341000543437) { - h_24(state, unused, out_1253312341000543437); +void car_h_24(double *state, double *unused, double *out_6766398854122891435) { + h_24(state, unused, out_6766398854122891435); } -void car_H_24(double *state, double *unused, double *out_8873302273916824024) { - H_24(state, unused, out_8873302273916824024); +void car_H_24(double *state, double *unused, double *out_7204356768457398274) { + H_24(state, unused, out_7204356768457398274); } -void car_h_30(double *state, double *unused, double *out_4399128350367343601) { - h_30(state, unused, out_4399128350367343601); +void car_h_30(double *state, double *unused, double *out_5662394748040244863) { + h_30(state, unused, out_5662394748040244863); } -void car_H_30(double *state, double *unused, double *out_216037666580292297) { - H_30(state, unused, out_216037666580292297); +void car_H_30(double *state, double *unused, double *out_5026874539255440953) { + H_30(state, unused, out_5026874539255440953); } -void car_h_26(double *state, double *unused, double *out_1362342298923010916) { - h_26(state, unused, out_1362342298923010916); +void car_h_26(double *state, double *unused, double *out_253236005693617473) { + h_26(state, unused, out_253236005693617473); } -void car_H_26(double *state, double *unused, double *out_3396126705150523857) { - H_26(state, unused, out_3396126705150523857); +void car_H_26(double *state, double *unused, double *out_5813067550508992927) { + H_26(state, unused, out_5813067550508992927); } -void car_h_27(double *state, double *unused, double *out_9204321679697507688) { - h_27(state, unused, out_9204321679697507688); +void car_h_27(double *state, double *unused, double *out_6177405252431548545) { + h_27(state, unused, out_6177405252431548545); } -void car_H_27(double *state, double *unused, double *out_1958725645220132614) { - H_27(state, unused, out_1958725645220132614); +void car_H_27(double *state, double *unused, double *out_2852111227455016042) { + H_27(state, unused, out_2852111227455016042); } -void car_h_29(double *state, double *unused, double *out_602544917293502044) { - h_29(state, unused, out_602544917293502044); +void car_h_29(double *state, double *unused, double *out_8631889293023980721) { + h_29(state, unused, out_8631889293023980721); } -void car_H_29(double *state, double *unused, double *out_3672088372089683647) { - H_29(state, unused, out_3672088372089683647); +void car_H_29(double *state, double *unused, double *out_5537105883569833137) { + H_29(state, unused, out_5537105883569833137); } -void car_h_28(double *state, double *unused, double *out_4281120956433966857) { - h_28(state, unused, out_4281120956433966857); +void car_h_28(double *state, double *unused, double *out_7267665553617157426) { + h_28(state, unused, out_7267665553617157426); } -void car_H_28(double *state, double *unused, double *out_1708458100524357396) { - H_28(state, unused, out_1708458100524357396); +void car_H_28(double *state, double *unused, double *out_454706866500302563) { + H_28(state, unused, out_454706866500302563); } -void car_h_31(double *state, double *unused, double *out_5944889882572371594) { - h_31(state, unused, out_5944889882572371594); +void car_h_31(double *state, double *unused, double *out_2626363377399586067) { + h_31(state, unused, out_2626363377399586067); } -void car_H_31(double *state, double *unused, double *out_376022575600492795) { - H_31(state, unused, out_376022575600492795); +void car_H_31(double *state, double *unused, double *out_5186859448275641451) { + H_31(state, unused, out_5186859448275641451); } 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 8a7890c4c3..b89ec68e23 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_3675669119552892075); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1525509250774896015); -void car_H_mod_fun(double *state, double *out_8908179234073782012); -void car_f_fun(double *state, double dt, double *out_2817676241617045765); -void car_F_fun(double *state, double dt, double *out_587119351607645481); -void car_h_25(double *state, double *unused, double *out_2948496957738460434); -void car_H_25(double *state, double *unused, double *out_345376613723532367); -void car_h_24(double *state, double *unused, double *out_1253312341000543437); -void car_H_24(double *state, double *unused, double *out_8873302273916824024); -void car_h_30(double *state, double *unused, double *out_4399128350367343601); -void car_H_30(double *state, double *unused, double *out_216037666580292297); -void car_h_26(double *state, double *unused, double *out_1362342298923010916); -void car_H_26(double *state, double *unused, double *out_3396126705150523857); -void car_h_27(double *state, double *unused, double *out_9204321679697507688); -void car_H_27(double *state, double *unused, double *out_1958725645220132614); -void car_h_29(double *state, double *unused, double *out_602544917293502044); -void car_H_29(double *state, double *unused, double *out_3672088372089683647); -void car_h_28(double *state, double *unused, double *out_4281120956433966857); -void car_H_28(double *state, double *unused, double *out_1708458100524357396); -void car_h_31(double *state, double *unused, double *out_5944889882572371594); -void car_H_31(double *state, double *unused, double *out_376022575600492795); +void car_err_fun(double *nom_x, double *delta_x, double *out_7345504276725310336); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1483419534327179826); +void car_H_mod_fun(double *state, double *out_5980828091480380917); +void car_f_fun(double *state, double dt, double *out_6771744444959754452); +void car_F_fun(double *state, double dt, double *out_8079006821062657786); +void car_h_25(double *state, double *unused, double *out_6013514099318955817); +void car_H_25(double *state, double *unused, double *out_8892173204326502465); +void car_h_24(double *state, double *unused, double *out_6766398854122891435); +void car_H_24(double *state, double *unused, double *out_7204356768457398274); +void car_h_30(double *state, double *unused, double *out_5662394748040244863); +void car_H_30(double *state, double *unused, double *out_5026874539255440953); +void car_h_26(double *state, double *unused, double *out_253236005693617473); +void car_H_26(double *state, double *unused, double *out_5813067550508992927); +void car_h_27(double *state, double *unused, double *out_6177405252431548545); +void car_H_27(double *state, double *unused, double *out_2852111227455016042); +void car_h_29(double *state, double *unused, double *out_8631889293023980721); +void car_H_29(double *state, double *unused, double *out_5537105883569833137); +void car_h_28(double *state, double *unused, double *out_7267665553617157426); +void car_H_28(double *state, double *unused, double *out_454706866500302563); +void car_h_31(double *state, double *unused, double *out_2626363377399586067); +void car_H_31(double *state, double *unused, double *out_5186859448275641451); 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/libcar.so b/selfdrive/locationd/models/generated/libcar.so index 5c15cf8491..21b3defc8b 100755 Binary files a/selfdrive/locationd/models/generated/libcar.so and b/selfdrive/locationd/models/generated/libcar.so differ diff --git a/selfdrive/locationd/models/generated/libpose.so b/selfdrive/locationd/models/generated/libpose.so index 023223a6f6..9aca29a152 100755 Binary files a/selfdrive/locationd/models/generated/libpose.so and b/selfdrive/locationd/models/generated/libpose.so differ diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index b610be721e..490a0fb5a2 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_3867314615092444191) { - out_3867314615092444191[0] = delta_x[0] + nom_x[0]; - out_3867314615092444191[1] = delta_x[1] + nom_x[1]; - out_3867314615092444191[2] = delta_x[2] + nom_x[2]; - out_3867314615092444191[3] = delta_x[3] + nom_x[3]; - out_3867314615092444191[4] = delta_x[4] + nom_x[4]; - out_3867314615092444191[5] = delta_x[5] + nom_x[5]; - out_3867314615092444191[6] = delta_x[6] + nom_x[6]; - out_3867314615092444191[7] = delta_x[7] + nom_x[7]; - out_3867314615092444191[8] = delta_x[8] + nom_x[8]; - out_3867314615092444191[9] = delta_x[9] + nom_x[9]; - out_3867314615092444191[10] = delta_x[10] + nom_x[10]; - out_3867314615092444191[11] = delta_x[11] + nom_x[11]; - out_3867314615092444191[12] = delta_x[12] + nom_x[12]; - out_3867314615092444191[13] = delta_x[13] + nom_x[13]; - out_3867314615092444191[14] = delta_x[14] + nom_x[14]; - out_3867314615092444191[15] = delta_x[15] + nom_x[15]; - out_3867314615092444191[16] = delta_x[16] + nom_x[16]; - out_3867314615092444191[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7351456117776073154) { + out_7351456117776073154[0] = delta_x[0] + nom_x[0]; + out_7351456117776073154[1] = delta_x[1] + nom_x[1]; + out_7351456117776073154[2] = delta_x[2] + nom_x[2]; + out_7351456117776073154[3] = delta_x[3] + nom_x[3]; + out_7351456117776073154[4] = delta_x[4] + nom_x[4]; + out_7351456117776073154[5] = delta_x[5] + nom_x[5]; + out_7351456117776073154[6] = delta_x[6] + nom_x[6]; + out_7351456117776073154[7] = delta_x[7] + nom_x[7]; + out_7351456117776073154[8] = delta_x[8] + nom_x[8]; + out_7351456117776073154[9] = delta_x[9] + nom_x[9]; + out_7351456117776073154[10] = delta_x[10] + nom_x[10]; + out_7351456117776073154[11] = delta_x[11] + nom_x[11]; + out_7351456117776073154[12] = delta_x[12] + nom_x[12]; + out_7351456117776073154[13] = delta_x[13] + nom_x[13]; + out_7351456117776073154[14] = delta_x[14] + nom_x[14]; + out_7351456117776073154[15] = delta_x[15] + nom_x[15]; + out_7351456117776073154[16] = delta_x[16] + nom_x[16]; + out_7351456117776073154[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_357340291324325113) { - out_357340291324325113[0] = -nom_x[0] + true_x[0]; - out_357340291324325113[1] = -nom_x[1] + true_x[1]; - out_357340291324325113[2] = -nom_x[2] + true_x[2]; - out_357340291324325113[3] = -nom_x[3] + true_x[3]; - out_357340291324325113[4] = -nom_x[4] + true_x[4]; - out_357340291324325113[5] = -nom_x[5] + true_x[5]; - out_357340291324325113[6] = -nom_x[6] + true_x[6]; - out_357340291324325113[7] = -nom_x[7] + true_x[7]; - out_357340291324325113[8] = -nom_x[8] + true_x[8]; - out_357340291324325113[9] = -nom_x[9] + true_x[9]; - out_357340291324325113[10] = -nom_x[10] + true_x[10]; - out_357340291324325113[11] = -nom_x[11] + true_x[11]; - out_357340291324325113[12] = -nom_x[12] + true_x[12]; - out_357340291324325113[13] = -nom_x[13] + true_x[13]; - out_357340291324325113[14] = -nom_x[14] + true_x[14]; - out_357340291324325113[15] = -nom_x[15] + true_x[15]; - out_357340291324325113[16] = -nom_x[16] + true_x[16]; - out_357340291324325113[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_9109558368636035839) { + out_9109558368636035839[0] = -nom_x[0] + true_x[0]; + out_9109558368636035839[1] = -nom_x[1] + true_x[1]; + out_9109558368636035839[2] = -nom_x[2] + true_x[2]; + out_9109558368636035839[3] = -nom_x[3] + true_x[3]; + out_9109558368636035839[4] = -nom_x[4] + true_x[4]; + out_9109558368636035839[5] = -nom_x[5] + true_x[5]; + out_9109558368636035839[6] = -nom_x[6] + true_x[6]; + out_9109558368636035839[7] = -nom_x[7] + true_x[7]; + out_9109558368636035839[8] = -nom_x[8] + true_x[8]; + out_9109558368636035839[9] = -nom_x[9] + true_x[9]; + out_9109558368636035839[10] = -nom_x[10] + true_x[10]; + out_9109558368636035839[11] = -nom_x[11] + true_x[11]; + out_9109558368636035839[12] = -nom_x[12] + true_x[12]; + out_9109558368636035839[13] = -nom_x[13] + true_x[13]; + out_9109558368636035839[14] = -nom_x[14] + true_x[14]; + out_9109558368636035839[15] = -nom_x[15] + true_x[15]; + out_9109558368636035839[16] = -nom_x[16] + true_x[16]; + out_9109558368636035839[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_5335377228813010670) { - out_5335377228813010670[0] = 1.0; - out_5335377228813010670[1] = 0; - out_5335377228813010670[2] = 0; - out_5335377228813010670[3] = 0; - out_5335377228813010670[4] = 0; - out_5335377228813010670[5] = 0; - out_5335377228813010670[6] = 0; - out_5335377228813010670[7] = 0; - out_5335377228813010670[8] = 0; - out_5335377228813010670[9] = 0; - out_5335377228813010670[10] = 0; - out_5335377228813010670[11] = 0; - out_5335377228813010670[12] = 0; - out_5335377228813010670[13] = 0; - out_5335377228813010670[14] = 0; - out_5335377228813010670[15] = 0; - out_5335377228813010670[16] = 0; - out_5335377228813010670[17] = 0; - out_5335377228813010670[18] = 0; - out_5335377228813010670[19] = 1.0; - out_5335377228813010670[20] = 0; - out_5335377228813010670[21] = 0; - out_5335377228813010670[22] = 0; - out_5335377228813010670[23] = 0; - out_5335377228813010670[24] = 0; - out_5335377228813010670[25] = 0; - out_5335377228813010670[26] = 0; - out_5335377228813010670[27] = 0; - out_5335377228813010670[28] = 0; - out_5335377228813010670[29] = 0; - out_5335377228813010670[30] = 0; - out_5335377228813010670[31] = 0; - out_5335377228813010670[32] = 0; - out_5335377228813010670[33] = 0; - out_5335377228813010670[34] = 0; - out_5335377228813010670[35] = 0; - out_5335377228813010670[36] = 0; - out_5335377228813010670[37] = 0; - out_5335377228813010670[38] = 1.0; - out_5335377228813010670[39] = 0; - out_5335377228813010670[40] = 0; - out_5335377228813010670[41] = 0; - out_5335377228813010670[42] = 0; - out_5335377228813010670[43] = 0; - out_5335377228813010670[44] = 0; - out_5335377228813010670[45] = 0; - out_5335377228813010670[46] = 0; - out_5335377228813010670[47] = 0; - out_5335377228813010670[48] = 0; - out_5335377228813010670[49] = 0; - out_5335377228813010670[50] = 0; - out_5335377228813010670[51] = 0; - out_5335377228813010670[52] = 0; - out_5335377228813010670[53] = 0; - out_5335377228813010670[54] = 0; - out_5335377228813010670[55] = 0; - out_5335377228813010670[56] = 0; - out_5335377228813010670[57] = 1.0; - out_5335377228813010670[58] = 0; - out_5335377228813010670[59] = 0; - out_5335377228813010670[60] = 0; - out_5335377228813010670[61] = 0; - out_5335377228813010670[62] = 0; - out_5335377228813010670[63] = 0; - out_5335377228813010670[64] = 0; - out_5335377228813010670[65] = 0; - out_5335377228813010670[66] = 0; - out_5335377228813010670[67] = 0; - out_5335377228813010670[68] = 0; - out_5335377228813010670[69] = 0; - out_5335377228813010670[70] = 0; - out_5335377228813010670[71] = 0; - out_5335377228813010670[72] = 0; - out_5335377228813010670[73] = 0; - out_5335377228813010670[74] = 0; - out_5335377228813010670[75] = 0; - out_5335377228813010670[76] = 1.0; - out_5335377228813010670[77] = 0; - out_5335377228813010670[78] = 0; - out_5335377228813010670[79] = 0; - out_5335377228813010670[80] = 0; - out_5335377228813010670[81] = 0; - out_5335377228813010670[82] = 0; - out_5335377228813010670[83] = 0; - out_5335377228813010670[84] = 0; - out_5335377228813010670[85] = 0; - out_5335377228813010670[86] = 0; - out_5335377228813010670[87] = 0; - out_5335377228813010670[88] = 0; - out_5335377228813010670[89] = 0; - out_5335377228813010670[90] = 0; - out_5335377228813010670[91] = 0; - out_5335377228813010670[92] = 0; - out_5335377228813010670[93] = 0; - out_5335377228813010670[94] = 0; - out_5335377228813010670[95] = 1.0; - out_5335377228813010670[96] = 0; - out_5335377228813010670[97] = 0; - out_5335377228813010670[98] = 0; - out_5335377228813010670[99] = 0; - out_5335377228813010670[100] = 0; - out_5335377228813010670[101] = 0; - out_5335377228813010670[102] = 0; - out_5335377228813010670[103] = 0; - out_5335377228813010670[104] = 0; - out_5335377228813010670[105] = 0; - out_5335377228813010670[106] = 0; - out_5335377228813010670[107] = 0; - out_5335377228813010670[108] = 0; - out_5335377228813010670[109] = 0; - out_5335377228813010670[110] = 0; - out_5335377228813010670[111] = 0; - out_5335377228813010670[112] = 0; - out_5335377228813010670[113] = 0; - out_5335377228813010670[114] = 1.0; - out_5335377228813010670[115] = 0; - out_5335377228813010670[116] = 0; - out_5335377228813010670[117] = 0; - out_5335377228813010670[118] = 0; - out_5335377228813010670[119] = 0; - out_5335377228813010670[120] = 0; - out_5335377228813010670[121] = 0; - out_5335377228813010670[122] = 0; - out_5335377228813010670[123] = 0; - out_5335377228813010670[124] = 0; - out_5335377228813010670[125] = 0; - out_5335377228813010670[126] = 0; - out_5335377228813010670[127] = 0; - out_5335377228813010670[128] = 0; - out_5335377228813010670[129] = 0; - out_5335377228813010670[130] = 0; - out_5335377228813010670[131] = 0; - out_5335377228813010670[132] = 0; - out_5335377228813010670[133] = 1.0; - out_5335377228813010670[134] = 0; - out_5335377228813010670[135] = 0; - out_5335377228813010670[136] = 0; - out_5335377228813010670[137] = 0; - out_5335377228813010670[138] = 0; - out_5335377228813010670[139] = 0; - out_5335377228813010670[140] = 0; - out_5335377228813010670[141] = 0; - out_5335377228813010670[142] = 0; - out_5335377228813010670[143] = 0; - out_5335377228813010670[144] = 0; - out_5335377228813010670[145] = 0; - out_5335377228813010670[146] = 0; - out_5335377228813010670[147] = 0; - out_5335377228813010670[148] = 0; - out_5335377228813010670[149] = 0; - out_5335377228813010670[150] = 0; - out_5335377228813010670[151] = 0; - out_5335377228813010670[152] = 1.0; - out_5335377228813010670[153] = 0; - out_5335377228813010670[154] = 0; - out_5335377228813010670[155] = 0; - out_5335377228813010670[156] = 0; - out_5335377228813010670[157] = 0; - out_5335377228813010670[158] = 0; - out_5335377228813010670[159] = 0; - out_5335377228813010670[160] = 0; - out_5335377228813010670[161] = 0; - out_5335377228813010670[162] = 0; - out_5335377228813010670[163] = 0; - out_5335377228813010670[164] = 0; - out_5335377228813010670[165] = 0; - out_5335377228813010670[166] = 0; - out_5335377228813010670[167] = 0; - out_5335377228813010670[168] = 0; - out_5335377228813010670[169] = 0; - out_5335377228813010670[170] = 0; - out_5335377228813010670[171] = 1.0; - out_5335377228813010670[172] = 0; - out_5335377228813010670[173] = 0; - out_5335377228813010670[174] = 0; - out_5335377228813010670[175] = 0; - out_5335377228813010670[176] = 0; - out_5335377228813010670[177] = 0; - out_5335377228813010670[178] = 0; - out_5335377228813010670[179] = 0; - out_5335377228813010670[180] = 0; - out_5335377228813010670[181] = 0; - out_5335377228813010670[182] = 0; - out_5335377228813010670[183] = 0; - out_5335377228813010670[184] = 0; - out_5335377228813010670[185] = 0; - out_5335377228813010670[186] = 0; - out_5335377228813010670[187] = 0; - out_5335377228813010670[188] = 0; - out_5335377228813010670[189] = 0; - out_5335377228813010670[190] = 1.0; - out_5335377228813010670[191] = 0; - out_5335377228813010670[192] = 0; - out_5335377228813010670[193] = 0; - out_5335377228813010670[194] = 0; - out_5335377228813010670[195] = 0; - out_5335377228813010670[196] = 0; - out_5335377228813010670[197] = 0; - out_5335377228813010670[198] = 0; - out_5335377228813010670[199] = 0; - out_5335377228813010670[200] = 0; - out_5335377228813010670[201] = 0; - out_5335377228813010670[202] = 0; - out_5335377228813010670[203] = 0; - out_5335377228813010670[204] = 0; - out_5335377228813010670[205] = 0; - out_5335377228813010670[206] = 0; - out_5335377228813010670[207] = 0; - out_5335377228813010670[208] = 0; - out_5335377228813010670[209] = 1.0; - out_5335377228813010670[210] = 0; - out_5335377228813010670[211] = 0; - out_5335377228813010670[212] = 0; - out_5335377228813010670[213] = 0; - out_5335377228813010670[214] = 0; - out_5335377228813010670[215] = 0; - out_5335377228813010670[216] = 0; - out_5335377228813010670[217] = 0; - out_5335377228813010670[218] = 0; - out_5335377228813010670[219] = 0; - out_5335377228813010670[220] = 0; - out_5335377228813010670[221] = 0; - out_5335377228813010670[222] = 0; - out_5335377228813010670[223] = 0; - out_5335377228813010670[224] = 0; - out_5335377228813010670[225] = 0; - out_5335377228813010670[226] = 0; - out_5335377228813010670[227] = 0; - out_5335377228813010670[228] = 1.0; - out_5335377228813010670[229] = 0; - out_5335377228813010670[230] = 0; - out_5335377228813010670[231] = 0; - out_5335377228813010670[232] = 0; - out_5335377228813010670[233] = 0; - out_5335377228813010670[234] = 0; - out_5335377228813010670[235] = 0; - out_5335377228813010670[236] = 0; - out_5335377228813010670[237] = 0; - out_5335377228813010670[238] = 0; - out_5335377228813010670[239] = 0; - out_5335377228813010670[240] = 0; - out_5335377228813010670[241] = 0; - out_5335377228813010670[242] = 0; - out_5335377228813010670[243] = 0; - out_5335377228813010670[244] = 0; - out_5335377228813010670[245] = 0; - out_5335377228813010670[246] = 0; - out_5335377228813010670[247] = 1.0; - out_5335377228813010670[248] = 0; - out_5335377228813010670[249] = 0; - out_5335377228813010670[250] = 0; - out_5335377228813010670[251] = 0; - out_5335377228813010670[252] = 0; - out_5335377228813010670[253] = 0; - out_5335377228813010670[254] = 0; - out_5335377228813010670[255] = 0; - out_5335377228813010670[256] = 0; - out_5335377228813010670[257] = 0; - out_5335377228813010670[258] = 0; - out_5335377228813010670[259] = 0; - out_5335377228813010670[260] = 0; - out_5335377228813010670[261] = 0; - out_5335377228813010670[262] = 0; - out_5335377228813010670[263] = 0; - out_5335377228813010670[264] = 0; - out_5335377228813010670[265] = 0; - out_5335377228813010670[266] = 1.0; - out_5335377228813010670[267] = 0; - out_5335377228813010670[268] = 0; - out_5335377228813010670[269] = 0; - out_5335377228813010670[270] = 0; - out_5335377228813010670[271] = 0; - out_5335377228813010670[272] = 0; - out_5335377228813010670[273] = 0; - out_5335377228813010670[274] = 0; - out_5335377228813010670[275] = 0; - out_5335377228813010670[276] = 0; - out_5335377228813010670[277] = 0; - out_5335377228813010670[278] = 0; - out_5335377228813010670[279] = 0; - out_5335377228813010670[280] = 0; - out_5335377228813010670[281] = 0; - out_5335377228813010670[282] = 0; - out_5335377228813010670[283] = 0; - out_5335377228813010670[284] = 0; - out_5335377228813010670[285] = 1.0; - out_5335377228813010670[286] = 0; - out_5335377228813010670[287] = 0; - out_5335377228813010670[288] = 0; - out_5335377228813010670[289] = 0; - out_5335377228813010670[290] = 0; - out_5335377228813010670[291] = 0; - out_5335377228813010670[292] = 0; - out_5335377228813010670[293] = 0; - out_5335377228813010670[294] = 0; - out_5335377228813010670[295] = 0; - out_5335377228813010670[296] = 0; - out_5335377228813010670[297] = 0; - out_5335377228813010670[298] = 0; - out_5335377228813010670[299] = 0; - out_5335377228813010670[300] = 0; - out_5335377228813010670[301] = 0; - out_5335377228813010670[302] = 0; - out_5335377228813010670[303] = 0; - out_5335377228813010670[304] = 1.0; - out_5335377228813010670[305] = 0; - out_5335377228813010670[306] = 0; - out_5335377228813010670[307] = 0; - out_5335377228813010670[308] = 0; - out_5335377228813010670[309] = 0; - out_5335377228813010670[310] = 0; - out_5335377228813010670[311] = 0; - out_5335377228813010670[312] = 0; - out_5335377228813010670[313] = 0; - out_5335377228813010670[314] = 0; - out_5335377228813010670[315] = 0; - out_5335377228813010670[316] = 0; - out_5335377228813010670[317] = 0; - out_5335377228813010670[318] = 0; - out_5335377228813010670[319] = 0; - out_5335377228813010670[320] = 0; - out_5335377228813010670[321] = 0; - out_5335377228813010670[322] = 0; - out_5335377228813010670[323] = 1.0; +void H_mod_fun(double *state, double *out_595012964091947662) { + out_595012964091947662[0] = 1.0; + out_595012964091947662[1] = 0; + out_595012964091947662[2] = 0; + out_595012964091947662[3] = 0; + out_595012964091947662[4] = 0; + out_595012964091947662[5] = 0; + out_595012964091947662[6] = 0; + out_595012964091947662[7] = 0; + out_595012964091947662[8] = 0; + out_595012964091947662[9] = 0; + out_595012964091947662[10] = 0; + out_595012964091947662[11] = 0; + out_595012964091947662[12] = 0; + out_595012964091947662[13] = 0; + out_595012964091947662[14] = 0; + out_595012964091947662[15] = 0; + out_595012964091947662[16] = 0; + out_595012964091947662[17] = 0; + out_595012964091947662[18] = 0; + out_595012964091947662[19] = 1.0; + out_595012964091947662[20] = 0; + out_595012964091947662[21] = 0; + out_595012964091947662[22] = 0; + out_595012964091947662[23] = 0; + out_595012964091947662[24] = 0; + out_595012964091947662[25] = 0; + out_595012964091947662[26] = 0; + out_595012964091947662[27] = 0; + out_595012964091947662[28] = 0; + out_595012964091947662[29] = 0; + out_595012964091947662[30] = 0; + out_595012964091947662[31] = 0; + out_595012964091947662[32] = 0; + out_595012964091947662[33] = 0; + out_595012964091947662[34] = 0; + out_595012964091947662[35] = 0; + out_595012964091947662[36] = 0; + out_595012964091947662[37] = 0; + out_595012964091947662[38] = 1.0; + out_595012964091947662[39] = 0; + out_595012964091947662[40] = 0; + out_595012964091947662[41] = 0; + out_595012964091947662[42] = 0; + out_595012964091947662[43] = 0; + out_595012964091947662[44] = 0; + out_595012964091947662[45] = 0; + out_595012964091947662[46] = 0; + out_595012964091947662[47] = 0; + out_595012964091947662[48] = 0; + out_595012964091947662[49] = 0; + out_595012964091947662[50] = 0; + out_595012964091947662[51] = 0; + out_595012964091947662[52] = 0; + out_595012964091947662[53] = 0; + out_595012964091947662[54] = 0; + out_595012964091947662[55] = 0; + out_595012964091947662[56] = 0; + out_595012964091947662[57] = 1.0; + out_595012964091947662[58] = 0; + out_595012964091947662[59] = 0; + out_595012964091947662[60] = 0; + out_595012964091947662[61] = 0; + out_595012964091947662[62] = 0; + out_595012964091947662[63] = 0; + out_595012964091947662[64] = 0; + out_595012964091947662[65] = 0; + out_595012964091947662[66] = 0; + out_595012964091947662[67] = 0; + out_595012964091947662[68] = 0; + out_595012964091947662[69] = 0; + out_595012964091947662[70] = 0; + out_595012964091947662[71] = 0; + out_595012964091947662[72] = 0; + out_595012964091947662[73] = 0; + out_595012964091947662[74] = 0; + out_595012964091947662[75] = 0; + out_595012964091947662[76] = 1.0; + out_595012964091947662[77] = 0; + out_595012964091947662[78] = 0; + out_595012964091947662[79] = 0; + out_595012964091947662[80] = 0; + out_595012964091947662[81] = 0; + out_595012964091947662[82] = 0; + out_595012964091947662[83] = 0; + out_595012964091947662[84] = 0; + out_595012964091947662[85] = 0; + out_595012964091947662[86] = 0; + out_595012964091947662[87] = 0; + out_595012964091947662[88] = 0; + out_595012964091947662[89] = 0; + out_595012964091947662[90] = 0; + out_595012964091947662[91] = 0; + out_595012964091947662[92] = 0; + out_595012964091947662[93] = 0; + out_595012964091947662[94] = 0; + out_595012964091947662[95] = 1.0; + out_595012964091947662[96] = 0; + out_595012964091947662[97] = 0; + out_595012964091947662[98] = 0; + out_595012964091947662[99] = 0; + out_595012964091947662[100] = 0; + out_595012964091947662[101] = 0; + out_595012964091947662[102] = 0; + out_595012964091947662[103] = 0; + out_595012964091947662[104] = 0; + out_595012964091947662[105] = 0; + out_595012964091947662[106] = 0; + out_595012964091947662[107] = 0; + out_595012964091947662[108] = 0; + out_595012964091947662[109] = 0; + out_595012964091947662[110] = 0; + out_595012964091947662[111] = 0; + out_595012964091947662[112] = 0; + out_595012964091947662[113] = 0; + out_595012964091947662[114] = 1.0; + out_595012964091947662[115] = 0; + out_595012964091947662[116] = 0; + out_595012964091947662[117] = 0; + out_595012964091947662[118] = 0; + out_595012964091947662[119] = 0; + out_595012964091947662[120] = 0; + out_595012964091947662[121] = 0; + out_595012964091947662[122] = 0; + out_595012964091947662[123] = 0; + out_595012964091947662[124] = 0; + out_595012964091947662[125] = 0; + out_595012964091947662[126] = 0; + out_595012964091947662[127] = 0; + out_595012964091947662[128] = 0; + out_595012964091947662[129] = 0; + out_595012964091947662[130] = 0; + out_595012964091947662[131] = 0; + out_595012964091947662[132] = 0; + out_595012964091947662[133] = 1.0; + out_595012964091947662[134] = 0; + out_595012964091947662[135] = 0; + out_595012964091947662[136] = 0; + out_595012964091947662[137] = 0; + out_595012964091947662[138] = 0; + out_595012964091947662[139] = 0; + out_595012964091947662[140] = 0; + out_595012964091947662[141] = 0; + out_595012964091947662[142] = 0; + out_595012964091947662[143] = 0; + out_595012964091947662[144] = 0; + out_595012964091947662[145] = 0; + out_595012964091947662[146] = 0; + out_595012964091947662[147] = 0; + out_595012964091947662[148] = 0; + out_595012964091947662[149] = 0; + out_595012964091947662[150] = 0; + out_595012964091947662[151] = 0; + out_595012964091947662[152] = 1.0; + out_595012964091947662[153] = 0; + out_595012964091947662[154] = 0; + out_595012964091947662[155] = 0; + out_595012964091947662[156] = 0; + out_595012964091947662[157] = 0; + out_595012964091947662[158] = 0; + out_595012964091947662[159] = 0; + out_595012964091947662[160] = 0; + out_595012964091947662[161] = 0; + out_595012964091947662[162] = 0; + out_595012964091947662[163] = 0; + out_595012964091947662[164] = 0; + out_595012964091947662[165] = 0; + out_595012964091947662[166] = 0; + out_595012964091947662[167] = 0; + out_595012964091947662[168] = 0; + out_595012964091947662[169] = 0; + out_595012964091947662[170] = 0; + out_595012964091947662[171] = 1.0; + out_595012964091947662[172] = 0; + out_595012964091947662[173] = 0; + out_595012964091947662[174] = 0; + out_595012964091947662[175] = 0; + out_595012964091947662[176] = 0; + out_595012964091947662[177] = 0; + out_595012964091947662[178] = 0; + out_595012964091947662[179] = 0; + out_595012964091947662[180] = 0; + out_595012964091947662[181] = 0; + out_595012964091947662[182] = 0; + out_595012964091947662[183] = 0; + out_595012964091947662[184] = 0; + out_595012964091947662[185] = 0; + out_595012964091947662[186] = 0; + out_595012964091947662[187] = 0; + out_595012964091947662[188] = 0; + out_595012964091947662[189] = 0; + out_595012964091947662[190] = 1.0; + out_595012964091947662[191] = 0; + out_595012964091947662[192] = 0; + out_595012964091947662[193] = 0; + out_595012964091947662[194] = 0; + out_595012964091947662[195] = 0; + out_595012964091947662[196] = 0; + out_595012964091947662[197] = 0; + out_595012964091947662[198] = 0; + out_595012964091947662[199] = 0; + out_595012964091947662[200] = 0; + out_595012964091947662[201] = 0; + out_595012964091947662[202] = 0; + out_595012964091947662[203] = 0; + out_595012964091947662[204] = 0; + out_595012964091947662[205] = 0; + out_595012964091947662[206] = 0; + out_595012964091947662[207] = 0; + out_595012964091947662[208] = 0; + out_595012964091947662[209] = 1.0; + out_595012964091947662[210] = 0; + out_595012964091947662[211] = 0; + out_595012964091947662[212] = 0; + out_595012964091947662[213] = 0; + out_595012964091947662[214] = 0; + out_595012964091947662[215] = 0; + out_595012964091947662[216] = 0; + out_595012964091947662[217] = 0; + out_595012964091947662[218] = 0; + out_595012964091947662[219] = 0; + out_595012964091947662[220] = 0; + out_595012964091947662[221] = 0; + out_595012964091947662[222] = 0; + out_595012964091947662[223] = 0; + out_595012964091947662[224] = 0; + out_595012964091947662[225] = 0; + out_595012964091947662[226] = 0; + out_595012964091947662[227] = 0; + out_595012964091947662[228] = 1.0; + out_595012964091947662[229] = 0; + out_595012964091947662[230] = 0; + out_595012964091947662[231] = 0; + out_595012964091947662[232] = 0; + out_595012964091947662[233] = 0; + out_595012964091947662[234] = 0; + out_595012964091947662[235] = 0; + out_595012964091947662[236] = 0; + out_595012964091947662[237] = 0; + out_595012964091947662[238] = 0; + out_595012964091947662[239] = 0; + out_595012964091947662[240] = 0; + out_595012964091947662[241] = 0; + out_595012964091947662[242] = 0; + out_595012964091947662[243] = 0; + out_595012964091947662[244] = 0; + out_595012964091947662[245] = 0; + out_595012964091947662[246] = 0; + out_595012964091947662[247] = 1.0; + out_595012964091947662[248] = 0; + out_595012964091947662[249] = 0; + out_595012964091947662[250] = 0; + out_595012964091947662[251] = 0; + out_595012964091947662[252] = 0; + out_595012964091947662[253] = 0; + out_595012964091947662[254] = 0; + out_595012964091947662[255] = 0; + out_595012964091947662[256] = 0; + out_595012964091947662[257] = 0; + out_595012964091947662[258] = 0; + out_595012964091947662[259] = 0; + out_595012964091947662[260] = 0; + out_595012964091947662[261] = 0; + out_595012964091947662[262] = 0; + out_595012964091947662[263] = 0; + out_595012964091947662[264] = 0; + out_595012964091947662[265] = 0; + out_595012964091947662[266] = 1.0; + out_595012964091947662[267] = 0; + out_595012964091947662[268] = 0; + out_595012964091947662[269] = 0; + out_595012964091947662[270] = 0; + out_595012964091947662[271] = 0; + out_595012964091947662[272] = 0; + out_595012964091947662[273] = 0; + out_595012964091947662[274] = 0; + out_595012964091947662[275] = 0; + out_595012964091947662[276] = 0; + out_595012964091947662[277] = 0; + out_595012964091947662[278] = 0; + out_595012964091947662[279] = 0; + out_595012964091947662[280] = 0; + out_595012964091947662[281] = 0; + out_595012964091947662[282] = 0; + out_595012964091947662[283] = 0; + out_595012964091947662[284] = 0; + out_595012964091947662[285] = 1.0; + out_595012964091947662[286] = 0; + out_595012964091947662[287] = 0; + out_595012964091947662[288] = 0; + out_595012964091947662[289] = 0; + out_595012964091947662[290] = 0; + out_595012964091947662[291] = 0; + out_595012964091947662[292] = 0; + out_595012964091947662[293] = 0; + out_595012964091947662[294] = 0; + out_595012964091947662[295] = 0; + out_595012964091947662[296] = 0; + out_595012964091947662[297] = 0; + out_595012964091947662[298] = 0; + out_595012964091947662[299] = 0; + out_595012964091947662[300] = 0; + out_595012964091947662[301] = 0; + out_595012964091947662[302] = 0; + out_595012964091947662[303] = 0; + out_595012964091947662[304] = 1.0; + out_595012964091947662[305] = 0; + out_595012964091947662[306] = 0; + out_595012964091947662[307] = 0; + out_595012964091947662[308] = 0; + out_595012964091947662[309] = 0; + out_595012964091947662[310] = 0; + out_595012964091947662[311] = 0; + out_595012964091947662[312] = 0; + out_595012964091947662[313] = 0; + out_595012964091947662[314] = 0; + out_595012964091947662[315] = 0; + out_595012964091947662[316] = 0; + out_595012964091947662[317] = 0; + out_595012964091947662[318] = 0; + out_595012964091947662[319] = 0; + out_595012964091947662[320] = 0; + out_595012964091947662[321] = 0; + out_595012964091947662[322] = 0; + out_595012964091947662[323] = 1.0; } -void f_fun(double *state, double dt, double *out_8449822447276776350) { - out_8449822447276776350[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_8449822447276776350[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_8449822447276776350[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_8449822447276776350[3] = dt*state[12] + state[3]; - out_8449822447276776350[4] = dt*state[13] + state[4]; - out_8449822447276776350[5] = dt*state[14] + state[5]; - out_8449822447276776350[6] = state[6]; - out_8449822447276776350[7] = state[7]; - out_8449822447276776350[8] = state[8]; - out_8449822447276776350[9] = state[9]; - out_8449822447276776350[10] = state[10]; - out_8449822447276776350[11] = state[11]; - out_8449822447276776350[12] = state[12]; - out_8449822447276776350[13] = state[13]; - out_8449822447276776350[14] = state[14]; - out_8449822447276776350[15] = state[15]; - out_8449822447276776350[16] = state[16]; - out_8449822447276776350[17] = state[17]; +void f_fun(double *state, double dt, double *out_3792261713431509332) { + out_3792261713431509332[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_3792261713431509332[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_3792261713431509332[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_3792261713431509332[3] = dt*state[12] + state[3]; + out_3792261713431509332[4] = dt*state[13] + state[4]; + out_3792261713431509332[5] = dt*state[14] + state[5]; + out_3792261713431509332[6] = state[6]; + out_3792261713431509332[7] = state[7]; + out_3792261713431509332[8] = state[8]; + out_3792261713431509332[9] = state[9]; + out_3792261713431509332[10] = state[10]; + out_3792261713431509332[11] = state[11]; + out_3792261713431509332[12] = state[12]; + out_3792261713431509332[13] = state[13]; + out_3792261713431509332[14] = state[14]; + out_3792261713431509332[15] = state[15]; + out_3792261713431509332[16] = state[16]; + out_3792261713431509332[17] = state[17]; } -void F_fun(double *state, double dt, double *out_8918868180555199161) { - out_8918868180555199161[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_8918868180555199161[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_8918868180555199161[2] = 0; - out_8918868180555199161[3] = 0; - out_8918868180555199161[4] = 0; - out_8918868180555199161[5] = 0; - out_8918868180555199161[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_8918868180555199161[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_8918868180555199161[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_8918868180555199161[9] = 0; - out_8918868180555199161[10] = 0; - out_8918868180555199161[11] = 0; - out_8918868180555199161[12] = 0; - out_8918868180555199161[13] = 0; - out_8918868180555199161[14] = 0; - out_8918868180555199161[15] = 0; - out_8918868180555199161[16] = 0; - out_8918868180555199161[17] = 0; - out_8918868180555199161[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_8918868180555199161[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_8918868180555199161[20] = 0; - out_8918868180555199161[21] = 0; - out_8918868180555199161[22] = 0; - out_8918868180555199161[23] = 0; - out_8918868180555199161[24] = 0; - out_8918868180555199161[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_8918868180555199161[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_8918868180555199161[27] = 0; - out_8918868180555199161[28] = 0; - out_8918868180555199161[29] = 0; - out_8918868180555199161[30] = 0; - out_8918868180555199161[31] = 0; - out_8918868180555199161[32] = 0; - out_8918868180555199161[33] = 0; - out_8918868180555199161[34] = 0; - out_8918868180555199161[35] = 0; - out_8918868180555199161[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_8918868180555199161[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_8918868180555199161[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_8918868180555199161[39] = 0; - out_8918868180555199161[40] = 0; - out_8918868180555199161[41] = 0; - out_8918868180555199161[42] = 0; - out_8918868180555199161[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_8918868180555199161[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_8918868180555199161[45] = 0; - out_8918868180555199161[46] = 0; - out_8918868180555199161[47] = 0; - out_8918868180555199161[48] = 0; - out_8918868180555199161[49] = 0; - out_8918868180555199161[50] = 0; - out_8918868180555199161[51] = 0; - out_8918868180555199161[52] = 0; - out_8918868180555199161[53] = 0; - out_8918868180555199161[54] = 0; - out_8918868180555199161[55] = 0; - out_8918868180555199161[56] = 0; - out_8918868180555199161[57] = 1; - out_8918868180555199161[58] = 0; - out_8918868180555199161[59] = 0; - out_8918868180555199161[60] = 0; - out_8918868180555199161[61] = 0; - out_8918868180555199161[62] = 0; - out_8918868180555199161[63] = 0; - out_8918868180555199161[64] = 0; - out_8918868180555199161[65] = 0; - out_8918868180555199161[66] = dt; - out_8918868180555199161[67] = 0; - out_8918868180555199161[68] = 0; - out_8918868180555199161[69] = 0; - out_8918868180555199161[70] = 0; - out_8918868180555199161[71] = 0; - out_8918868180555199161[72] = 0; - out_8918868180555199161[73] = 0; - out_8918868180555199161[74] = 0; - out_8918868180555199161[75] = 0; - out_8918868180555199161[76] = 1; - out_8918868180555199161[77] = 0; - out_8918868180555199161[78] = 0; - out_8918868180555199161[79] = 0; - out_8918868180555199161[80] = 0; - out_8918868180555199161[81] = 0; - out_8918868180555199161[82] = 0; - out_8918868180555199161[83] = 0; - out_8918868180555199161[84] = 0; - out_8918868180555199161[85] = dt; - out_8918868180555199161[86] = 0; - out_8918868180555199161[87] = 0; - out_8918868180555199161[88] = 0; - out_8918868180555199161[89] = 0; - out_8918868180555199161[90] = 0; - out_8918868180555199161[91] = 0; - out_8918868180555199161[92] = 0; - out_8918868180555199161[93] = 0; - out_8918868180555199161[94] = 0; - out_8918868180555199161[95] = 1; - out_8918868180555199161[96] = 0; - out_8918868180555199161[97] = 0; - out_8918868180555199161[98] = 0; - out_8918868180555199161[99] = 0; - out_8918868180555199161[100] = 0; - out_8918868180555199161[101] = 0; - out_8918868180555199161[102] = 0; - out_8918868180555199161[103] = 0; - out_8918868180555199161[104] = dt; - out_8918868180555199161[105] = 0; - out_8918868180555199161[106] = 0; - out_8918868180555199161[107] = 0; - out_8918868180555199161[108] = 0; - out_8918868180555199161[109] = 0; - out_8918868180555199161[110] = 0; - out_8918868180555199161[111] = 0; - out_8918868180555199161[112] = 0; - out_8918868180555199161[113] = 0; - out_8918868180555199161[114] = 1; - out_8918868180555199161[115] = 0; - out_8918868180555199161[116] = 0; - out_8918868180555199161[117] = 0; - out_8918868180555199161[118] = 0; - out_8918868180555199161[119] = 0; - out_8918868180555199161[120] = 0; - out_8918868180555199161[121] = 0; - out_8918868180555199161[122] = 0; - out_8918868180555199161[123] = 0; - out_8918868180555199161[124] = 0; - out_8918868180555199161[125] = 0; - out_8918868180555199161[126] = 0; - out_8918868180555199161[127] = 0; - out_8918868180555199161[128] = 0; - out_8918868180555199161[129] = 0; - out_8918868180555199161[130] = 0; - out_8918868180555199161[131] = 0; - out_8918868180555199161[132] = 0; - out_8918868180555199161[133] = 1; - out_8918868180555199161[134] = 0; - out_8918868180555199161[135] = 0; - out_8918868180555199161[136] = 0; - out_8918868180555199161[137] = 0; - out_8918868180555199161[138] = 0; - out_8918868180555199161[139] = 0; - out_8918868180555199161[140] = 0; - out_8918868180555199161[141] = 0; - out_8918868180555199161[142] = 0; - out_8918868180555199161[143] = 0; - out_8918868180555199161[144] = 0; - out_8918868180555199161[145] = 0; - out_8918868180555199161[146] = 0; - out_8918868180555199161[147] = 0; - out_8918868180555199161[148] = 0; - out_8918868180555199161[149] = 0; - out_8918868180555199161[150] = 0; - out_8918868180555199161[151] = 0; - out_8918868180555199161[152] = 1; - out_8918868180555199161[153] = 0; - out_8918868180555199161[154] = 0; - out_8918868180555199161[155] = 0; - out_8918868180555199161[156] = 0; - out_8918868180555199161[157] = 0; - out_8918868180555199161[158] = 0; - out_8918868180555199161[159] = 0; - out_8918868180555199161[160] = 0; - out_8918868180555199161[161] = 0; - out_8918868180555199161[162] = 0; - out_8918868180555199161[163] = 0; - out_8918868180555199161[164] = 0; - out_8918868180555199161[165] = 0; - out_8918868180555199161[166] = 0; - out_8918868180555199161[167] = 0; - out_8918868180555199161[168] = 0; - out_8918868180555199161[169] = 0; - out_8918868180555199161[170] = 0; - out_8918868180555199161[171] = 1; - out_8918868180555199161[172] = 0; - out_8918868180555199161[173] = 0; - out_8918868180555199161[174] = 0; - out_8918868180555199161[175] = 0; - out_8918868180555199161[176] = 0; - out_8918868180555199161[177] = 0; - out_8918868180555199161[178] = 0; - out_8918868180555199161[179] = 0; - out_8918868180555199161[180] = 0; - out_8918868180555199161[181] = 0; - out_8918868180555199161[182] = 0; - out_8918868180555199161[183] = 0; - out_8918868180555199161[184] = 0; - out_8918868180555199161[185] = 0; - out_8918868180555199161[186] = 0; - out_8918868180555199161[187] = 0; - out_8918868180555199161[188] = 0; - out_8918868180555199161[189] = 0; - out_8918868180555199161[190] = 1; - out_8918868180555199161[191] = 0; - out_8918868180555199161[192] = 0; - out_8918868180555199161[193] = 0; - out_8918868180555199161[194] = 0; - out_8918868180555199161[195] = 0; - out_8918868180555199161[196] = 0; - out_8918868180555199161[197] = 0; - out_8918868180555199161[198] = 0; - out_8918868180555199161[199] = 0; - out_8918868180555199161[200] = 0; - out_8918868180555199161[201] = 0; - out_8918868180555199161[202] = 0; - out_8918868180555199161[203] = 0; - out_8918868180555199161[204] = 0; - out_8918868180555199161[205] = 0; - out_8918868180555199161[206] = 0; - out_8918868180555199161[207] = 0; - out_8918868180555199161[208] = 0; - out_8918868180555199161[209] = 1; - out_8918868180555199161[210] = 0; - out_8918868180555199161[211] = 0; - out_8918868180555199161[212] = 0; - out_8918868180555199161[213] = 0; - out_8918868180555199161[214] = 0; - out_8918868180555199161[215] = 0; - out_8918868180555199161[216] = 0; - out_8918868180555199161[217] = 0; - out_8918868180555199161[218] = 0; - out_8918868180555199161[219] = 0; - out_8918868180555199161[220] = 0; - out_8918868180555199161[221] = 0; - out_8918868180555199161[222] = 0; - out_8918868180555199161[223] = 0; - out_8918868180555199161[224] = 0; - out_8918868180555199161[225] = 0; - out_8918868180555199161[226] = 0; - out_8918868180555199161[227] = 0; - out_8918868180555199161[228] = 1; - out_8918868180555199161[229] = 0; - out_8918868180555199161[230] = 0; - out_8918868180555199161[231] = 0; - out_8918868180555199161[232] = 0; - out_8918868180555199161[233] = 0; - out_8918868180555199161[234] = 0; - out_8918868180555199161[235] = 0; - out_8918868180555199161[236] = 0; - out_8918868180555199161[237] = 0; - out_8918868180555199161[238] = 0; - out_8918868180555199161[239] = 0; - out_8918868180555199161[240] = 0; - out_8918868180555199161[241] = 0; - out_8918868180555199161[242] = 0; - out_8918868180555199161[243] = 0; - out_8918868180555199161[244] = 0; - out_8918868180555199161[245] = 0; - out_8918868180555199161[246] = 0; - out_8918868180555199161[247] = 1; - out_8918868180555199161[248] = 0; - out_8918868180555199161[249] = 0; - out_8918868180555199161[250] = 0; - out_8918868180555199161[251] = 0; - out_8918868180555199161[252] = 0; - out_8918868180555199161[253] = 0; - out_8918868180555199161[254] = 0; - out_8918868180555199161[255] = 0; - out_8918868180555199161[256] = 0; - out_8918868180555199161[257] = 0; - out_8918868180555199161[258] = 0; - out_8918868180555199161[259] = 0; - out_8918868180555199161[260] = 0; - out_8918868180555199161[261] = 0; - out_8918868180555199161[262] = 0; - out_8918868180555199161[263] = 0; - out_8918868180555199161[264] = 0; - out_8918868180555199161[265] = 0; - out_8918868180555199161[266] = 1; - out_8918868180555199161[267] = 0; - out_8918868180555199161[268] = 0; - out_8918868180555199161[269] = 0; - out_8918868180555199161[270] = 0; - out_8918868180555199161[271] = 0; - out_8918868180555199161[272] = 0; - out_8918868180555199161[273] = 0; - out_8918868180555199161[274] = 0; - out_8918868180555199161[275] = 0; - out_8918868180555199161[276] = 0; - out_8918868180555199161[277] = 0; - out_8918868180555199161[278] = 0; - out_8918868180555199161[279] = 0; - out_8918868180555199161[280] = 0; - out_8918868180555199161[281] = 0; - out_8918868180555199161[282] = 0; - out_8918868180555199161[283] = 0; - out_8918868180555199161[284] = 0; - out_8918868180555199161[285] = 1; - out_8918868180555199161[286] = 0; - out_8918868180555199161[287] = 0; - out_8918868180555199161[288] = 0; - out_8918868180555199161[289] = 0; - out_8918868180555199161[290] = 0; - out_8918868180555199161[291] = 0; - out_8918868180555199161[292] = 0; - out_8918868180555199161[293] = 0; - out_8918868180555199161[294] = 0; - out_8918868180555199161[295] = 0; - out_8918868180555199161[296] = 0; - out_8918868180555199161[297] = 0; - out_8918868180555199161[298] = 0; - out_8918868180555199161[299] = 0; - out_8918868180555199161[300] = 0; - out_8918868180555199161[301] = 0; - out_8918868180555199161[302] = 0; - out_8918868180555199161[303] = 0; - out_8918868180555199161[304] = 1; - out_8918868180555199161[305] = 0; - out_8918868180555199161[306] = 0; - out_8918868180555199161[307] = 0; - out_8918868180555199161[308] = 0; - out_8918868180555199161[309] = 0; - out_8918868180555199161[310] = 0; - out_8918868180555199161[311] = 0; - out_8918868180555199161[312] = 0; - out_8918868180555199161[313] = 0; - out_8918868180555199161[314] = 0; - out_8918868180555199161[315] = 0; - out_8918868180555199161[316] = 0; - out_8918868180555199161[317] = 0; - out_8918868180555199161[318] = 0; - out_8918868180555199161[319] = 0; - out_8918868180555199161[320] = 0; - out_8918868180555199161[321] = 0; - out_8918868180555199161[322] = 0; - out_8918868180555199161[323] = 1; +void F_fun(double *state, double dt, double *out_6928823179361542013) { + out_6928823179361542013[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_6928823179361542013[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_6928823179361542013[2] = 0; + out_6928823179361542013[3] = 0; + out_6928823179361542013[4] = 0; + out_6928823179361542013[5] = 0; + out_6928823179361542013[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_6928823179361542013[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_6928823179361542013[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_6928823179361542013[9] = 0; + out_6928823179361542013[10] = 0; + out_6928823179361542013[11] = 0; + out_6928823179361542013[12] = 0; + out_6928823179361542013[13] = 0; + out_6928823179361542013[14] = 0; + out_6928823179361542013[15] = 0; + out_6928823179361542013[16] = 0; + out_6928823179361542013[17] = 0; + out_6928823179361542013[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_6928823179361542013[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_6928823179361542013[20] = 0; + out_6928823179361542013[21] = 0; + out_6928823179361542013[22] = 0; + out_6928823179361542013[23] = 0; + out_6928823179361542013[24] = 0; + out_6928823179361542013[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_6928823179361542013[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_6928823179361542013[27] = 0; + out_6928823179361542013[28] = 0; + out_6928823179361542013[29] = 0; + out_6928823179361542013[30] = 0; + out_6928823179361542013[31] = 0; + out_6928823179361542013[32] = 0; + out_6928823179361542013[33] = 0; + out_6928823179361542013[34] = 0; + out_6928823179361542013[35] = 0; + out_6928823179361542013[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_6928823179361542013[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_6928823179361542013[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_6928823179361542013[39] = 0; + out_6928823179361542013[40] = 0; + out_6928823179361542013[41] = 0; + out_6928823179361542013[42] = 0; + out_6928823179361542013[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_6928823179361542013[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_6928823179361542013[45] = 0; + out_6928823179361542013[46] = 0; + out_6928823179361542013[47] = 0; + out_6928823179361542013[48] = 0; + out_6928823179361542013[49] = 0; + out_6928823179361542013[50] = 0; + out_6928823179361542013[51] = 0; + out_6928823179361542013[52] = 0; + out_6928823179361542013[53] = 0; + out_6928823179361542013[54] = 0; + out_6928823179361542013[55] = 0; + out_6928823179361542013[56] = 0; + out_6928823179361542013[57] = 1; + out_6928823179361542013[58] = 0; + out_6928823179361542013[59] = 0; + out_6928823179361542013[60] = 0; + out_6928823179361542013[61] = 0; + out_6928823179361542013[62] = 0; + out_6928823179361542013[63] = 0; + out_6928823179361542013[64] = 0; + out_6928823179361542013[65] = 0; + out_6928823179361542013[66] = dt; + out_6928823179361542013[67] = 0; + out_6928823179361542013[68] = 0; + out_6928823179361542013[69] = 0; + out_6928823179361542013[70] = 0; + out_6928823179361542013[71] = 0; + out_6928823179361542013[72] = 0; + out_6928823179361542013[73] = 0; + out_6928823179361542013[74] = 0; + out_6928823179361542013[75] = 0; + out_6928823179361542013[76] = 1; + out_6928823179361542013[77] = 0; + out_6928823179361542013[78] = 0; + out_6928823179361542013[79] = 0; + out_6928823179361542013[80] = 0; + out_6928823179361542013[81] = 0; + out_6928823179361542013[82] = 0; + out_6928823179361542013[83] = 0; + out_6928823179361542013[84] = 0; + out_6928823179361542013[85] = dt; + out_6928823179361542013[86] = 0; + out_6928823179361542013[87] = 0; + out_6928823179361542013[88] = 0; + out_6928823179361542013[89] = 0; + out_6928823179361542013[90] = 0; + out_6928823179361542013[91] = 0; + out_6928823179361542013[92] = 0; + out_6928823179361542013[93] = 0; + out_6928823179361542013[94] = 0; + out_6928823179361542013[95] = 1; + out_6928823179361542013[96] = 0; + out_6928823179361542013[97] = 0; + out_6928823179361542013[98] = 0; + out_6928823179361542013[99] = 0; + out_6928823179361542013[100] = 0; + out_6928823179361542013[101] = 0; + out_6928823179361542013[102] = 0; + out_6928823179361542013[103] = 0; + out_6928823179361542013[104] = dt; + out_6928823179361542013[105] = 0; + out_6928823179361542013[106] = 0; + out_6928823179361542013[107] = 0; + out_6928823179361542013[108] = 0; + out_6928823179361542013[109] = 0; + out_6928823179361542013[110] = 0; + out_6928823179361542013[111] = 0; + out_6928823179361542013[112] = 0; + out_6928823179361542013[113] = 0; + out_6928823179361542013[114] = 1; + out_6928823179361542013[115] = 0; + out_6928823179361542013[116] = 0; + out_6928823179361542013[117] = 0; + out_6928823179361542013[118] = 0; + out_6928823179361542013[119] = 0; + out_6928823179361542013[120] = 0; + out_6928823179361542013[121] = 0; + out_6928823179361542013[122] = 0; + out_6928823179361542013[123] = 0; + out_6928823179361542013[124] = 0; + out_6928823179361542013[125] = 0; + out_6928823179361542013[126] = 0; + out_6928823179361542013[127] = 0; + out_6928823179361542013[128] = 0; + out_6928823179361542013[129] = 0; + out_6928823179361542013[130] = 0; + out_6928823179361542013[131] = 0; + out_6928823179361542013[132] = 0; + out_6928823179361542013[133] = 1; + out_6928823179361542013[134] = 0; + out_6928823179361542013[135] = 0; + out_6928823179361542013[136] = 0; + out_6928823179361542013[137] = 0; + out_6928823179361542013[138] = 0; + out_6928823179361542013[139] = 0; + out_6928823179361542013[140] = 0; + out_6928823179361542013[141] = 0; + out_6928823179361542013[142] = 0; + out_6928823179361542013[143] = 0; + out_6928823179361542013[144] = 0; + out_6928823179361542013[145] = 0; + out_6928823179361542013[146] = 0; + out_6928823179361542013[147] = 0; + out_6928823179361542013[148] = 0; + out_6928823179361542013[149] = 0; + out_6928823179361542013[150] = 0; + out_6928823179361542013[151] = 0; + out_6928823179361542013[152] = 1; + out_6928823179361542013[153] = 0; + out_6928823179361542013[154] = 0; + out_6928823179361542013[155] = 0; + out_6928823179361542013[156] = 0; + out_6928823179361542013[157] = 0; + out_6928823179361542013[158] = 0; + out_6928823179361542013[159] = 0; + out_6928823179361542013[160] = 0; + out_6928823179361542013[161] = 0; + out_6928823179361542013[162] = 0; + out_6928823179361542013[163] = 0; + out_6928823179361542013[164] = 0; + out_6928823179361542013[165] = 0; + out_6928823179361542013[166] = 0; + out_6928823179361542013[167] = 0; + out_6928823179361542013[168] = 0; + out_6928823179361542013[169] = 0; + out_6928823179361542013[170] = 0; + out_6928823179361542013[171] = 1; + out_6928823179361542013[172] = 0; + out_6928823179361542013[173] = 0; + out_6928823179361542013[174] = 0; + out_6928823179361542013[175] = 0; + out_6928823179361542013[176] = 0; + out_6928823179361542013[177] = 0; + out_6928823179361542013[178] = 0; + out_6928823179361542013[179] = 0; + out_6928823179361542013[180] = 0; + out_6928823179361542013[181] = 0; + out_6928823179361542013[182] = 0; + out_6928823179361542013[183] = 0; + out_6928823179361542013[184] = 0; + out_6928823179361542013[185] = 0; + out_6928823179361542013[186] = 0; + out_6928823179361542013[187] = 0; + out_6928823179361542013[188] = 0; + out_6928823179361542013[189] = 0; + out_6928823179361542013[190] = 1; + out_6928823179361542013[191] = 0; + out_6928823179361542013[192] = 0; + out_6928823179361542013[193] = 0; + out_6928823179361542013[194] = 0; + out_6928823179361542013[195] = 0; + out_6928823179361542013[196] = 0; + out_6928823179361542013[197] = 0; + out_6928823179361542013[198] = 0; + out_6928823179361542013[199] = 0; + out_6928823179361542013[200] = 0; + out_6928823179361542013[201] = 0; + out_6928823179361542013[202] = 0; + out_6928823179361542013[203] = 0; + out_6928823179361542013[204] = 0; + out_6928823179361542013[205] = 0; + out_6928823179361542013[206] = 0; + out_6928823179361542013[207] = 0; + out_6928823179361542013[208] = 0; + out_6928823179361542013[209] = 1; + out_6928823179361542013[210] = 0; + out_6928823179361542013[211] = 0; + out_6928823179361542013[212] = 0; + out_6928823179361542013[213] = 0; + out_6928823179361542013[214] = 0; + out_6928823179361542013[215] = 0; + out_6928823179361542013[216] = 0; + out_6928823179361542013[217] = 0; + out_6928823179361542013[218] = 0; + out_6928823179361542013[219] = 0; + out_6928823179361542013[220] = 0; + out_6928823179361542013[221] = 0; + out_6928823179361542013[222] = 0; + out_6928823179361542013[223] = 0; + out_6928823179361542013[224] = 0; + out_6928823179361542013[225] = 0; + out_6928823179361542013[226] = 0; + out_6928823179361542013[227] = 0; + out_6928823179361542013[228] = 1; + out_6928823179361542013[229] = 0; + out_6928823179361542013[230] = 0; + out_6928823179361542013[231] = 0; + out_6928823179361542013[232] = 0; + out_6928823179361542013[233] = 0; + out_6928823179361542013[234] = 0; + out_6928823179361542013[235] = 0; + out_6928823179361542013[236] = 0; + out_6928823179361542013[237] = 0; + out_6928823179361542013[238] = 0; + out_6928823179361542013[239] = 0; + out_6928823179361542013[240] = 0; + out_6928823179361542013[241] = 0; + out_6928823179361542013[242] = 0; + out_6928823179361542013[243] = 0; + out_6928823179361542013[244] = 0; + out_6928823179361542013[245] = 0; + out_6928823179361542013[246] = 0; + out_6928823179361542013[247] = 1; + out_6928823179361542013[248] = 0; + out_6928823179361542013[249] = 0; + out_6928823179361542013[250] = 0; + out_6928823179361542013[251] = 0; + out_6928823179361542013[252] = 0; + out_6928823179361542013[253] = 0; + out_6928823179361542013[254] = 0; + out_6928823179361542013[255] = 0; + out_6928823179361542013[256] = 0; + out_6928823179361542013[257] = 0; + out_6928823179361542013[258] = 0; + out_6928823179361542013[259] = 0; + out_6928823179361542013[260] = 0; + out_6928823179361542013[261] = 0; + out_6928823179361542013[262] = 0; + out_6928823179361542013[263] = 0; + out_6928823179361542013[264] = 0; + out_6928823179361542013[265] = 0; + out_6928823179361542013[266] = 1; + out_6928823179361542013[267] = 0; + out_6928823179361542013[268] = 0; + out_6928823179361542013[269] = 0; + out_6928823179361542013[270] = 0; + out_6928823179361542013[271] = 0; + out_6928823179361542013[272] = 0; + out_6928823179361542013[273] = 0; + out_6928823179361542013[274] = 0; + out_6928823179361542013[275] = 0; + out_6928823179361542013[276] = 0; + out_6928823179361542013[277] = 0; + out_6928823179361542013[278] = 0; + out_6928823179361542013[279] = 0; + out_6928823179361542013[280] = 0; + out_6928823179361542013[281] = 0; + out_6928823179361542013[282] = 0; + out_6928823179361542013[283] = 0; + out_6928823179361542013[284] = 0; + out_6928823179361542013[285] = 1; + out_6928823179361542013[286] = 0; + out_6928823179361542013[287] = 0; + out_6928823179361542013[288] = 0; + out_6928823179361542013[289] = 0; + out_6928823179361542013[290] = 0; + out_6928823179361542013[291] = 0; + out_6928823179361542013[292] = 0; + out_6928823179361542013[293] = 0; + out_6928823179361542013[294] = 0; + out_6928823179361542013[295] = 0; + out_6928823179361542013[296] = 0; + out_6928823179361542013[297] = 0; + out_6928823179361542013[298] = 0; + out_6928823179361542013[299] = 0; + out_6928823179361542013[300] = 0; + out_6928823179361542013[301] = 0; + out_6928823179361542013[302] = 0; + out_6928823179361542013[303] = 0; + out_6928823179361542013[304] = 1; + out_6928823179361542013[305] = 0; + out_6928823179361542013[306] = 0; + out_6928823179361542013[307] = 0; + out_6928823179361542013[308] = 0; + out_6928823179361542013[309] = 0; + out_6928823179361542013[310] = 0; + out_6928823179361542013[311] = 0; + out_6928823179361542013[312] = 0; + out_6928823179361542013[313] = 0; + out_6928823179361542013[314] = 0; + out_6928823179361542013[315] = 0; + out_6928823179361542013[316] = 0; + out_6928823179361542013[317] = 0; + out_6928823179361542013[318] = 0; + out_6928823179361542013[319] = 0; + out_6928823179361542013[320] = 0; + out_6928823179361542013[321] = 0; + out_6928823179361542013[322] = 0; + out_6928823179361542013[323] = 1; } -void h_4(double *state, double *unused, double *out_6617669203308538531) { - out_6617669203308538531[0] = state[6] + state[9]; - out_6617669203308538531[1] = state[7] + state[10]; - out_6617669203308538531[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_7683085519451864677) { + out_7683085519451864677[0] = state[6] + state[9]; + out_7683085519451864677[1] = state[7] + state[10]; + out_7683085519451864677[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_1875678236267692381) { - out_1875678236267692381[0] = 0; - out_1875678236267692381[1] = 0; - out_1875678236267692381[2] = 0; - out_1875678236267692381[3] = 0; - out_1875678236267692381[4] = 0; - out_1875678236267692381[5] = 0; - out_1875678236267692381[6] = 1; - out_1875678236267692381[7] = 0; - out_1875678236267692381[8] = 0; - out_1875678236267692381[9] = 1; - out_1875678236267692381[10] = 0; - out_1875678236267692381[11] = 0; - out_1875678236267692381[12] = 0; - out_1875678236267692381[13] = 0; - out_1875678236267692381[14] = 0; - out_1875678236267692381[15] = 0; - out_1875678236267692381[16] = 0; - out_1875678236267692381[17] = 0; - out_1875678236267692381[18] = 0; - out_1875678236267692381[19] = 0; - out_1875678236267692381[20] = 0; - out_1875678236267692381[21] = 0; - out_1875678236267692381[22] = 0; - out_1875678236267692381[23] = 0; - out_1875678236267692381[24] = 0; - out_1875678236267692381[25] = 1; - out_1875678236267692381[26] = 0; - out_1875678236267692381[27] = 0; - out_1875678236267692381[28] = 1; - out_1875678236267692381[29] = 0; - out_1875678236267692381[30] = 0; - out_1875678236267692381[31] = 0; - out_1875678236267692381[32] = 0; - out_1875678236267692381[33] = 0; - out_1875678236267692381[34] = 0; - out_1875678236267692381[35] = 0; - out_1875678236267692381[36] = 0; - out_1875678236267692381[37] = 0; - out_1875678236267692381[38] = 0; - out_1875678236267692381[39] = 0; - out_1875678236267692381[40] = 0; - out_1875678236267692381[41] = 0; - out_1875678236267692381[42] = 0; - out_1875678236267692381[43] = 0; - out_1875678236267692381[44] = 1; - out_1875678236267692381[45] = 0; - out_1875678236267692381[46] = 0; - out_1875678236267692381[47] = 1; - out_1875678236267692381[48] = 0; - out_1875678236267692381[49] = 0; - out_1875678236267692381[50] = 0; - out_1875678236267692381[51] = 0; - out_1875678236267692381[52] = 0; - out_1875678236267692381[53] = 0; +void H_4(double *state, double *unused, double *out_3149865311834297615) { + out_3149865311834297615[0] = 0; + out_3149865311834297615[1] = 0; + out_3149865311834297615[2] = 0; + out_3149865311834297615[3] = 0; + out_3149865311834297615[4] = 0; + out_3149865311834297615[5] = 0; + out_3149865311834297615[6] = 1; + out_3149865311834297615[7] = 0; + out_3149865311834297615[8] = 0; + out_3149865311834297615[9] = 1; + out_3149865311834297615[10] = 0; + out_3149865311834297615[11] = 0; + out_3149865311834297615[12] = 0; + out_3149865311834297615[13] = 0; + out_3149865311834297615[14] = 0; + out_3149865311834297615[15] = 0; + out_3149865311834297615[16] = 0; + out_3149865311834297615[17] = 0; + out_3149865311834297615[18] = 0; + out_3149865311834297615[19] = 0; + out_3149865311834297615[20] = 0; + out_3149865311834297615[21] = 0; + out_3149865311834297615[22] = 0; + out_3149865311834297615[23] = 0; + out_3149865311834297615[24] = 0; + out_3149865311834297615[25] = 1; + out_3149865311834297615[26] = 0; + out_3149865311834297615[27] = 0; + out_3149865311834297615[28] = 1; + out_3149865311834297615[29] = 0; + out_3149865311834297615[30] = 0; + out_3149865311834297615[31] = 0; + out_3149865311834297615[32] = 0; + out_3149865311834297615[33] = 0; + out_3149865311834297615[34] = 0; + out_3149865311834297615[35] = 0; + out_3149865311834297615[36] = 0; + out_3149865311834297615[37] = 0; + out_3149865311834297615[38] = 0; + out_3149865311834297615[39] = 0; + out_3149865311834297615[40] = 0; + out_3149865311834297615[41] = 0; + out_3149865311834297615[42] = 0; + out_3149865311834297615[43] = 0; + out_3149865311834297615[44] = 1; + out_3149865311834297615[45] = 0; + out_3149865311834297615[46] = 0; + out_3149865311834297615[47] = 1; + out_3149865311834297615[48] = 0; + out_3149865311834297615[49] = 0; + out_3149865311834297615[50] = 0; + out_3149865311834297615[51] = 0; + out_3149865311834297615[52] = 0; + out_3149865311834297615[53] = 0; } -void h_10(double *state, double *unused, double *out_8387436857003783836) { - out_8387436857003783836[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_8387436857003783836[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_8387436857003783836[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_4760549729285226708) { + out_4760549729285226708[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_4760549729285226708[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_4760549729285226708[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_253999754152164366) { - out_253999754152164366[0] = 0; - out_253999754152164366[1] = 9.8100000000000005*cos(state[1]); - out_253999754152164366[2] = 0; - out_253999754152164366[3] = 0; - out_253999754152164366[4] = -state[8]; - out_253999754152164366[5] = state[7]; - out_253999754152164366[6] = 0; - out_253999754152164366[7] = state[5]; - out_253999754152164366[8] = -state[4]; - out_253999754152164366[9] = 0; - out_253999754152164366[10] = 0; - out_253999754152164366[11] = 0; - out_253999754152164366[12] = 1; - out_253999754152164366[13] = 0; - out_253999754152164366[14] = 0; - out_253999754152164366[15] = 1; - out_253999754152164366[16] = 0; - out_253999754152164366[17] = 0; - out_253999754152164366[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_253999754152164366[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_253999754152164366[20] = 0; - out_253999754152164366[21] = state[8]; - out_253999754152164366[22] = 0; - out_253999754152164366[23] = -state[6]; - out_253999754152164366[24] = -state[5]; - out_253999754152164366[25] = 0; - out_253999754152164366[26] = state[3]; - out_253999754152164366[27] = 0; - out_253999754152164366[28] = 0; - out_253999754152164366[29] = 0; - out_253999754152164366[30] = 0; - out_253999754152164366[31] = 1; - out_253999754152164366[32] = 0; - out_253999754152164366[33] = 0; - out_253999754152164366[34] = 1; - out_253999754152164366[35] = 0; - out_253999754152164366[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_253999754152164366[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_253999754152164366[38] = 0; - out_253999754152164366[39] = -state[7]; - out_253999754152164366[40] = state[6]; - out_253999754152164366[41] = 0; - out_253999754152164366[42] = state[4]; - out_253999754152164366[43] = -state[3]; - out_253999754152164366[44] = 0; - out_253999754152164366[45] = 0; - out_253999754152164366[46] = 0; - out_253999754152164366[47] = 0; - out_253999754152164366[48] = 0; - out_253999754152164366[49] = 0; - out_253999754152164366[50] = 1; - out_253999754152164366[51] = 0; - out_253999754152164366[52] = 0; - out_253999754152164366[53] = 1; +void H_10(double *state, double *unused, double *out_748214929053840095) { + out_748214929053840095[0] = 0; + out_748214929053840095[1] = 9.8100000000000005*cos(state[1]); + out_748214929053840095[2] = 0; + out_748214929053840095[3] = 0; + out_748214929053840095[4] = -state[8]; + out_748214929053840095[5] = state[7]; + out_748214929053840095[6] = 0; + out_748214929053840095[7] = state[5]; + out_748214929053840095[8] = -state[4]; + out_748214929053840095[9] = 0; + out_748214929053840095[10] = 0; + out_748214929053840095[11] = 0; + out_748214929053840095[12] = 1; + out_748214929053840095[13] = 0; + out_748214929053840095[14] = 0; + out_748214929053840095[15] = 1; + out_748214929053840095[16] = 0; + out_748214929053840095[17] = 0; + out_748214929053840095[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_748214929053840095[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_748214929053840095[20] = 0; + out_748214929053840095[21] = state[8]; + out_748214929053840095[22] = 0; + out_748214929053840095[23] = -state[6]; + out_748214929053840095[24] = -state[5]; + out_748214929053840095[25] = 0; + out_748214929053840095[26] = state[3]; + out_748214929053840095[27] = 0; + out_748214929053840095[28] = 0; + out_748214929053840095[29] = 0; + out_748214929053840095[30] = 0; + out_748214929053840095[31] = 1; + out_748214929053840095[32] = 0; + out_748214929053840095[33] = 0; + out_748214929053840095[34] = 1; + out_748214929053840095[35] = 0; + out_748214929053840095[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_748214929053840095[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_748214929053840095[38] = 0; + out_748214929053840095[39] = -state[7]; + out_748214929053840095[40] = state[6]; + out_748214929053840095[41] = 0; + out_748214929053840095[42] = state[4]; + out_748214929053840095[43] = -state[3]; + out_748214929053840095[44] = 0; + out_748214929053840095[45] = 0; + out_748214929053840095[46] = 0; + out_748214929053840095[47] = 0; + out_748214929053840095[48] = 0; + out_748214929053840095[49] = 0; + out_748214929053840095[50] = 1; + out_748214929053840095[51] = 0; + out_748214929053840095[52] = 0; + out_748214929053840095[53] = 1; } -void h_13(double *state, double *unused, double *out_3782328115152295086) { - out_3782328115152295086[0] = state[3]; - out_3782328115152295086[1] = state[4]; - out_3782328115152295086[2] = state[5]; +void h_13(double *state, double *unused, double *out_6328698996174999134) { + out_6328698996174999134[0] = state[3]; + out_6328698996174999134[1] = state[4]; + out_6328698996174999134[2] = state[5]; } -void H_13(double *state, double *unused, double *out_5709433699570216405) { - out_5709433699570216405[0] = 0; - out_5709433699570216405[1] = 0; - out_5709433699570216405[2] = 0; - out_5709433699570216405[3] = 1; - out_5709433699570216405[4] = 0; - out_5709433699570216405[5] = 0; - out_5709433699570216405[6] = 0; - out_5709433699570216405[7] = 0; - out_5709433699570216405[8] = 0; - out_5709433699570216405[9] = 0; - out_5709433699570216405[10] = 0; - out_5709433699570216405[11] = 0; - out_5709433699570216405[12] = 0; - out_5709433699570216405[13] = 0; - out_5709433699570216405[14] = 0; - out_5709433699570216405[15] = 0; - out_5709433699570216405[16] = 0; - out_5709433699570216405[17] = 0; - out_5709433699570216405[18] = 0; - out_5709433699570216405[19] = 0; - out_5709433699570216405[20] = 0; - out_5709433699570216405[21] = 0; - out_5709433699570216405[22] = 1; - out_5709433699570216405[23] = 0; - out_5709433699570216405[24] = 0; - out_5709433699570216405[25] = 0; - out_5709433699570216405[26] = 0; - out_5709433699570216405[27] = 0; - out_5709433699570216405[28] = 0; - out_5709433699570216405[29] = 0; - out_5709433699570216405[30] = 0; - out_5709433699570216405[31] = 0; - out_5709433699570216405[32] = 0; - out_5709433699570216405[33] = 0; - out_5709433699570216405[34] = 0; - out_5709433699570216405[35] = 0; - out_5709433699570216405[36] = 0; - out_5709433699570216405[37] = 0; - out_5709433699570216405[38] = 0; - out_5709433699570216405[39] = 0; - out_5709433699570216405[40] = 0; - out_5709433699570216405[41] = 1; - out_5709433699570216405[42] = 0; - out_5709433699570216405[43] = 0; - out_5709433699570216405[44] = 0; - out_5709433699570216405[45] = 0; - out_5709433699570216405[46] = 0; - out_5709433699570216405[47] = 0; - out_5709433699570216405[48] = 0; - out_5709433699570216405[49] = 0; - out_5709433699570216405[50] = 0; - out_5709433699570216405[51] = 0; - out_5709433699570216405[52] = 0; - out_5709433699570216405[53] = 0; +void H_13(double *state, double *unused, double *out_6362139137166630416) { + out_6362139137166630416[0] = 0; + out_6362139137166630416[1] = 0; + out_6362139137166630416[2] = 0; + out_6362139137166630416[3] = 1; + out_6362139137166630416[4] = 0; + out_6362139137166630416[5] = 0; + out_6362139137166630416[6] = 0; + out_6362139137166630416[7] = 0; + out_6362139137166630416[8] = 0; + out_6362139137166630416[9] = 0; + out_6362139137166630416[10] = 0; + out_6362139137166630416[11] = 0; + out_6362139137166630416[12] = 0; + out_6362139137166630416[13] = 0; + out_6362139137166630416[14] = 0; + out_6362139137166630416[15] = 0; + out_6362139137166630416[16] = 0; + out_6362139137166630416[17] = 0; + out_6362139137166630416[18] = 0; + out_6362139137166630416[19] = 0; + out_6362139137166630416[20] = 0; + out_6362139137166630416[21] = 0; + out_6362139137166630416[22] = 1; + out_6362139137166630416[23] = 0; + out_6362139137166630416[24] = 0; + out_6362139137166630416[25] = 0; + out_6362139137166630416[26] = 0; + out_6362139137166630416[27] = 0; + out_6362139137166630416[28] = 0; + out_6362139137166630416[29] = 0; + out_6362139137166630416[30] = 0; + out_6362139137166630416[31] = 0; + out_6362139137166630416[32] = 0; + out_6362139137166630416[33] = 0; + out_6362139137166630416[34] = 0; + out_6362139137166630416[35] = 0; + out_6362139137166630416[36] = 0; + out_6362139137166630416[37] = 0; + out_6362139137166630416[38] = 0; + out_6362139137166630416[39] = 0; + out_6362139137166630416[40] = 0; + out_6362139137166630416[41] = 1; + out_6362139137166630416[42] = 0; + out_6362139137166630416[43] = 0; + out_6362139137166630416[44] = 0; + out_6362139137166630416[45] = 0; + out_6362139137166630416[46] = 0; + out_6362139137166630416[47] = 0; + out_6362139137166630416[48] = 0; + out_6362139137166630416[49] = 0; + out_6362139137166630416[50] = 0; + out_6362139137166630416[51] = 0; + out_6362139137166630416[52] = 0; + out_6362139137166630416[53] = 0; } -void h_14(double *state, double *unused, double *out_8292081498849349712) { - out_8292081498849349712[0] = state[6]; - out_8292081498849349712[1] = state[7]; - out_8292081498849349712[2] = state[8]; +void h_14(double *state, double *unused, double *out_4490584305148798359) { + out_4490584305148798359[0] = state[6]; + out_4490584305148798359[1] = state[7]; + out_4490584305148798359[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4958466668563064677) { - out_4958466668563064677[0] = 0; - out_4958466668563064677[1] = 0; - out_4958466668563064677[2] = 0; - out_4958466668563064677[3] = 0; - out_4958466668563064677[4] = 0; - out_4958466668563064677[5] = 0; - out_4958466668563064677[6] = 1; - out_4958466668563064677[7] = 0; - out_4958466668563064677[8] = 0; - out_4958466668563064677[9] = 0; - out_4958466668563064677[10] = 0; - out_4958466668563064677[11] = 0; - out_4958466668563064677[12] = 0; - out_4958466668563064677[13] = 0; - out_4958466668563064677[14] = 0; - out_4958466668563064677[15] = 0; - out_4958466668563064677[16] = 0; - out_4958466668563064677[17] = 0; - out_4958466668563064677[18] = 0; - out_4958466668563064677[19] = 0; - out_4958466668563064677[20] = 0; - out_4958466668563064677[21] = 0; - out_4958466668563064677[22] = 0; - out_4958466668563064677[23] = 0; - out_4958466668563064677[24] = 0; - out_4958466668563064677[25] = 1; - out_4958466668563064677[26] = 0; - out_4958466668563064677[27] = 0; - out_4958466668563064677[28] = 0; - out_4958466668563064677[29] = 0; - out_4958466668563064677[30] = 0; - out_4958466668563064677[31] = 0; - out_4958466668563064677[32] = 0; - out_4958466668563064677[33] = 0; - out_4958466668563064677[34] = 0; - out_4958466668563064677[35] = 0; - out_4958466668563064677[36] = 0; - out_4958466668563064677[37] = 0; - out_4958466668563064677[38] = 0; - out_4958466668563064677[39] = 0; - out_4958466668563064677[40] = 0; - out_4958466668563064677[41] = 0; - out_4958466668563064677[42] = 0; - out_4958466668563064677[43] = 0; - out_4958466668563064677[44] = 1; - out_4958466668563064677[45] = 0; - out_4958466668563064677[46] = 0; - out_4958466668563064677[47] = 0; - out_4958466668563064677[48] = 0; - out_4958466668563064677[49] = 0; - out_4958466668563064677[50] = 0; - out_4958466668563064677[51] = 0; - out_4958466668563064677[52] = 0; - out_4958466668563064677[53] = 0; +void H_14(double *state, double *unused, double *out_7113106168173782144) { + out_7113106168173782144[0] = 0; + out_7113106168173782144[1] = 0; + out_7113106168173782144[2] = 0; + out_7113106168173782144[3] = 0; + out_7113106168173782144[4] = 0; + out_7113106168173782144[5] = 0; + out_7113106168173782144[6] = 1; + out_7113106168173782144[7] = 0; + out_7113106168173782144[8] = 0; + out_7113106168173782144[9] = 0; + out_7113106168173782144[10] = 0; + out_7113106168173782144[11] = 0; + out_7113106168173782144[12] = 0; + out_7113106168173782144[13] = 0; + out_7113106168173782144[14] = 0; + out_7113106168173782144[15] = 0; + out_7113106168173782144[16] = 0; + out_7113106168173782144[17] = 0; + out_7113106168173782144[18] = 0; + out_7113106168173782144[19] = 0; + out_7113106168173782144[20] = 0; + out_7113106168173782144[21] = 0; + out_7113106168173782144[22] = 0; + out_7113106168173782144[23] = 0; + out_7113106168173782144[24] = 0; + out_7113106168173782144[25] = 1; + out_7113106168173782144[26] = 0; + out_7113106168173782144[27] = 0; + out_7113106168173782144[28] = 0; + out_7113106168173782144[29] = 0; + out_7113106168173782144[30] = 0; + out_7113106168173782144[31] = 0; + out_7113106168173782144[32] = 0; + out_7113106168173782144[33] = 0; + out_7113106168173782144[34] = 0; + out_7113106168173782144[35] = 0; + out_7113106168173782144[36] = 0; + out_7113106168173782144[37] = 0; + out_7113106168173782144[38] = 0; + out_7113106168173782144[39] = 0; + out_7113106168173782144[40] = 0; + out_7113106168173782144[41] = 0; + out_7113106168173782144[42] = 0; + out_7113106168173782144[43] = 0; + out_7113106168173782144[44] = 1; + out_7113106168173782144[45] = 0; + out_7113106168173782144[46] = 0; + out_7113106168173782144[47] = 0; + out_7113106168173782144[48] = 0; + out_7113106168173782144[49] = 0; + out_7113106168173782144[50] = 0; + out_7113106168173782144[51] = 0; + out_7113106168173782144[52] = 0; + out_7113106168173782144[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_3867314615092444191) { - err_fun(nom_x, delta_x, out_3867314615092444191); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7351456117776073154) { + err_fun(nom_x, delta_x, out_7351456117776073154); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_357340291324325113) { - inv_err_fun(nom_x, true_x, out_357340291324325113); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_9109558368636035839) { + inv_err_fun(nom_x, true_x, out_9109558368636035839); } -void pose_H_mod_fun(double *state, double *out_5335377228813010670) { - H_mod_fun(state, out_5335377228813010670); +void pose_H_mod_fun(double *state, double *out_595012964091947662) { + H_mod_fun(state, out_595012964091947662); } -void pose_f_fun(double *state, double dt, double *out_8449822447276776350) { - f_fun(state, dt, out_8449822447276776350); +void pose_f_fun(double *state, double dt, double *out_3792261713431509332) { + f_fun(state, dt, out_3792261713431509332); } -void pose_F_fun(double *state, double dt, double *out_8918868180555199161) { - F_fun(state, dt, out_8918868180555199161); +void pose_F_fun(double *state, double dt, double *out_6928823179361542013) { + F_fun(state, dt, out_6928823179361542013); } -void pose_h_4(double *state, double *unused, double *out_6617669203308538531) { - h_4(state, unused, out_6617669203308538531); +void pose_h_4(double *state, double *unused, double *out_7683085519451864677) { + h_4(state, unused, out_7683085519451864677); } -void pose_H_4(double *state, double *unused, double *out_1875678236267692381) { - H_4(state, unused, out_1875678236267692381); +void pose_H_4(double *state, double *unused, double *out_3149865311834297615) { + H_4(state, unused, out_3149865311834297615); } -void pose_h_10(double *state, double *unused, double *out_8387436857003783836) { - h_10(state, unused, out_8387436857003783836); +void pose_h_10(double *state, double *unused, double *out_4760549729285226708) { + h_10(state, unused, out_4760549729285226708); } -void pose_H_10(double *state, double *unused, double *out_253999754152164366) { - H_10(state, unused, out_253999754152164366); +void pose_H_10(double *state, double *unused, double *out_748214929053840095) { + H_10(state, unused, out_748214929053840095); } -void pose_h_13(double *state, double *unused, double *out_3782328115152295086) { - h_13(state, unused, out_3782328115152295086); +void pose_h_13(double *state, double *unused, double *out_6328698996174999134) { + h_13(state, unused, out_6328698996174999134); } -void pose_H_13(double *state, double *unused, double *out_5709433699570216405) { - H_13(state, unused, out_5709433699570216405); +void pose_H_13(double *state, double *unused, double *out_6362139137166630416) { + H_13(state, unused, out_6362139137166630416); } -void pose_h_14(double *state, double *unused, double *out_8292081498849349712) { - h_14(state, unused, out_8292081498849349712); +void pose_h_14(double *state, double *unused, double *out_4490584305148798359) { + h_14(state, unused, out_4490584305148798359); } -void pose_H_14(double *state, double *unused, double *out_4958466668563064677) { - H_14(state, unused, out_4958466668563064677); +void pose_H_14(double *state, double *unused, double *out_7113106168173782144) { + H_14(state, unused, out_7113106168173782144); } 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 d5f5c9d82d..b1ee6c0f83 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_3867314615092444191); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_357340291324325113); -void pose_H_mod_fun(double *state, double *out_5335377228813010670); -void pose_f_fun(double *state, double dt, double *out_8449822447276776350); -void pose_F_fun(double *state, double dt, double *out_8918868180555199161); -void pose_h_4(double *state, double *unused, double *out_6617669203308538531); -void pose_H_4(double *state, double *unused, double *out_1875678236267692381); -void pose_h_10(double *state, double *unused, double *out_8387436857003783836); -void pose_H_10(double *state, double *unused, double *out_253999754152164366); -void pose_h_13(double *state, double *unused, double *out_3782328115152295086); -void pose_H_13(double *state, double *unused, double *out_5709433699570216405); -void pose_h_14(double *state, double *unused, double *out_8292081498849349712); -void pose_H_14(double *state, double *unused, double *out_4958466668563064677); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7351456117776073154); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_9109558368636035839); +void pose_H_mod_fun(double *state, double *out_595012964091947662); +void pose_f_fun(double *state, double dt, double *out_3792261713431509332); +void pose_F_fun(double *state, double dt, double *out_6928823179361542013); +void pose_h_4(double *state, double *unused, double *out_7683085519451864677); +void pose_H_4(double *state, double *unused, double *out_3149865311834297615); +void pose_h_10(double *state, double *unused, double *out_4760549729285226708); +void pose_H_10(double *state, double *unused, double *out_748214929053840095); +void pose_h_13(double *state, double *unused, double *out_6328698996174999134); +void pose_H_13(double *state, double *unused, double *out_6362139137166630416); +void pose_h_14(double *state, double *unused, double *out_4490584305148798359); +void pose_H_14(double *state, double *unused, double *out_7113106168173782144); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl b/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl index ad58c4bd0e..78b24e9042 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl and b/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl differ diff --git a/selfdrive/modeld/models/driving_policy_tinygrad.pkl b/selfdrive/modeld/models/driving_policy_tinygrad.pkl index da9c72d153..96017790f8 100644 Binary files a/selfdrive/modeld/models/driving_policy_tinygrad.pkl and b/selfdrive/modeld/models/driving_policy_tinygrad.pkl differ diff --git a/selfdrive/modeld/models/driving_vision_tinygrad.pkl b/selfdrive/modeld/models/driving_vision_tinygrad.pkl index 56169d0021..31ad19404e 100644 Binary files a/selfdrive/modeld/models/driving_vision_tinygrad.pkl and b/selfdrive/modeld/models/driving_vision_tinygrad.pkl differ diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index fdf1b77871..3778032bcd 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -274,7 +274,6 @@ class SelfdriveD: self.events.add(EventName.radarTempUnavailable) else: self.events.add(EventName.radarFault) - self.events.add(EventName.radarFault) if not self.sm.valid['pandaStates']: self.events.add(EventName.usbError) if CS.canTimeout: diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 1ea59c4856..c26cadbd97 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -187,7 +187,7 @@ class TestOnroad: def test_manager_starting_time(self): st = self.ts['managerState']['t'][0] - assert (st - self.manager_st) < 10, f"manager.py took {st - self.manager_st}s to publish the first 'managerState' msg" + assert (st - self.manager_st) < 15, f"manager.py took {st - self.manager_st}s to publish the first 'managerState' msg" def test_cloudlog_size(self): msgs = self.msgs['logMessage'] diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index c5a4b165e4..84db4fbae8 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 782465eb70..7b4fef4cbc 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/camerad/sensors/ox03c10_registers.h b/system/camerad/sensors/ox03c10_registers.h index 5c6282942b..bb7a1c5dd6 100644 --- a/system/camerad/sensors/ox03c10_registers.h +++ b/system/camerad/sensors/ox03c10_registers.h @@ -65,7 +65,6 @@ const struct i2c_random_wr_payload init_array_ox03c10[] = { {0x3008, 0x80}, // io_pad_sel // FSIN (frame sync) with external pulses - {0x3822, 0x33}, // wait for pulse before first frame {0x3009, 0x2}, {0x3015, 0x2}, {0x383E, 0x80}, @@ -73,6 +72,9 @@ const struct i2c_random_wr_payload init_array_ox03c10[] = { {0x3882, 0x8}, {0x3883, 0x0D}, {0x3836, 0x1F}, {0x3837, 0x40}, + // causes issues on some devices + //{0x3822, 0x33}, // wait for pulse before first frame + {0x3892, 0x44}, {0x3823, 0x41}, diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index 04e5fd3365..1a3cd382a9 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -56,28 +56,28 @@ }, { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-0d0d4d5a32e00b46fa36180b4a96337f2a53302d8bd0faee95f8fe1063d1e24a.img.xz", - "hash": "0d0d4d5a32e00b46fa36180b4a96337f2a53302d8bd0faee95f8fe1063d1e24a", - "hash_raw": "0d0d4d5a32e00b46fa36180b4a96337f2a53302d8bd0faee95f8fe1063d1e24a", + "url": "https://commadist.azureedge.net/agnosupdate/boot-9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892.img.xz", + "hash": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892", + "hash_raw": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892", "size": 18479104, "sparse": false, "full_check": true, "has_ab": true, - "ondevice_hash": "47b2096995578a5078e393c33108b42756009dbb361c43c508fc93cd8bda99cc" + "ondevice_hash": "41d31b862fec1b87879b508c405adb9d7b5c0a3324f7350bd904f451605b06cf" }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-ff2da7bc34a1ad8e2831a13a49685243957a51b762d8f28e14e98ac90e84d211.img.xz", - "hash": "fbda983c40e75d8c4784a45c687f27dd27f97f7f0e9a71b4981ec2092ff68a72", - "hash_raw": "ff2da7bc34a1ad8e2831a13a49685243957a51b762d8f28e14e98ac90e84d211", + "url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img.xz", + "hash": "c56256a64e6d7e16886e39a4263ffb686ed0f03d3a665c3552f54a39723f8824", + "hash_raw": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde", "size": 4404019200, "sparse": true, "full_check": false, "has_ab": true, - "ondevice_hash": "428c744458d5f35199aab96a0928876c9deeff3e753e95044291408ba244f0af", + "ondevice_hash": "ed2e11f52beb8559223bf9fb989fd4ef5d2ce66eeb11ae0053fff8e41903a533", "alt": { - "hash": "ff2da7bc34a1ad8e2831a13a49685243957a51b762d8f28e14e98ac90e84d211", - "url": "https://commadist.azureedge.net/agnosupdate/system-ff2da7bc34a1ad8e2831a13a49685243957a51b762d8f28e14e98ac90e84d211.img", + "hash": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde", + "url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img", "size": 4404019200 } } diff --git a/system/hardware/tici/all-partitions.json b/system/hardware/tici/all-partitions.json index 4d15e76b0e..643b1b2a26 100644 --- a/system/hardware/tici/all-partitions.json +++ b/system/hardware/tici/all-partitions.json @@ -339,62 +339,62 @@ }, { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-0d0d4d5a32e00b46fa36180b4a96337f2a53302d8bd0faee95f8fe1063d1e24a.img.xz", - "hash": "0d0d4d5a32e00b46fa36180b4a96337f2a53302d8bd0faee95f8fe1063d1e24a", - "hash_raw": "0d0d4d5a32e00b46fa36180b4a96337f2a53302d8bd0faee95f8fe1063d1e24a", + "url": "https://commadist.azureedge.net/agnosupdate/boot-9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892.img.xz", + "hash": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892", + "hash_raw": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892", "size": 18479104, "sparse": false, "full_check": true, "has_ab": true, - "ondevice_hash": "47b2096995578a5078e393c33108b42756009dbb361c43c508fc93cd8bda99cc" + "ondevice_hash": "41d31b862fec1b87879b508c405adb9d7b5c0a3324f7350bd904f451605b06cf" }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-ff2da7bc34a1ad8e2831a13a49685243957a51b762d8f28e14e98ac90e84d211.img.xz", - "hash": "fbda983c40e75d8c4784a45c687f27dd27f97f7f0e9a71b4981ec2092ff68a72", - "hash_raw": "ff2da7bc34a1ad8e2831a13a49685243957a51b762d8f28e14e98ac90e84d211", + "url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img.xz", + "hash": "c56256a64e6d7e16886e39a4263ffb686ed0f03d3a665c3552f54a39723f8824", + "hash_raw": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde", "size": 4404019200, "sparse": true, "full_check": false, "has_ab": true, - "ondevice_hash": "428c744458d5f35199aab96a0928876c9deeff3e753e95044291408ba244f0af", + "ondevice_hash": "ed2e11f52beb8559223bf9fb989fd4ef5d2ce66eeb11ae0053fff8e41903a533", "alt": { - "hash": "ff2da7bc34a1ad8e2831a13a49685243957a51b762d8f28e14e98ac90e84d211", - "url": "https://commadist.azureedge.net/agnosupdate/system-ff2da7bc34a1ad8e2831a13a49685243957a51b762d8f28e14e98ac90e84d211.img", + "hash": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde", + "url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img", "size": 4404019200 } }, { "name": "userdata_90", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-17f46ba0d0eec10cf14804c84d9bcf4f6fa864074f1b90ff2fb4873399d64b1a.img.xz", - "hash": "1442ae218daff960783bc493a32dd489fa787e4025c64fa3132f48b1e627647f", - "hash_raw": "17f46ba0d0eec10cf14804c84d9bcf4f6fa864074f1b90ff2fb4873399d64b1a", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-344c1f5da1ece113e0f07c1964a95646dbfe96f49cfd64ac7375430c9247ab33.img.xz", + "hash": "f94d635a74d1b7a7a6774f7673be0672219f253025406d0b2595ff3ef41b68f8", + "hash_raw": "344c1f5da1ece113e0f07c1964a95646dbfe96f49cfd64ac7375430c9247ab33", "size": 96636764160, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "a64fea9c3a7bec11b850a55d50ceb30a032742c90aadf0ce35cdbef9349bebba" + "ondevice_hash": "96e376ed61e8604adce6fea4b3a7302b966c1975f768f66e4a81428dce7e78f0" }, { "name": "userdata_89", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-69b2404839f87b1de5c05766550ca0217d5ed566c9f2ea41c46b5a301540a3ac.img.xz", - "hash": "332e6cf5c221690a01ebfd0582420e039c7796a8fb2d499e44a8b657dffe4af2", - "hash_raw": "69b2404839f87b1de5c05766550ca0217d5ed566c9f2ea41c46b5a301540a3ac", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-a2840d1aeac1b2229a82d91be1b1546b3ec294cb185b6ad5ca502848d1c9c8fb.img.xz", + "hash": "d86482e90ac0e08e31d0b699b6728043dc3db2ab0feaec99a6a393085824424a", + "hash_raw": "a2840d1aeac1b2229a82d91be1b1546b3ec294cb185b6ad5ca502848d1c9c8fb", "size": 95563022336, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "c560c76007fd8997e01e516ab16ff445048945a13bf830bb2a4c7f6e9d2fe5f6" + "ondevice_hash": "b6754cf40297e4a924315aa81d062ea108b86c93e6ad3c3c98559863a297e2b1" }, { "name": "userdata_30", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-ffc34d8753520e684448185db801c05bb81e23c54a499cc40a2e38b7009cdf36.img.xz", - "hash": "8bfb1fdec9c8f96c97bcce1efbc7d502447bdb3b481cc408b24b5feef66839fd", - "hash_raw": "ffc34d8753520e684448185db801c05bb81e23c54a499cc40a2e38b7009cdf36", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-1f4a795d4c7d052e94326e668be84929856fa2945ddbdf097e1ee722defeabd1.img.xz", + "hash": "2df53cb6d115bcb58998a7346e53030ae89683c215f1c28aaa1b524cb145ac65", + "hash_raw": "1f4a795d4c7d052e94326e668be84929856fa2945ddbdf097e1ee722defeabd1", "size": 32212254720, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "8f840157d73fa4469aa650d668e046c9750304af84248ab83453cbd285918859" + "ondevice_hash": "585e9a650d53be65600589ce0b6c71edf8e6678f65d16dc6ac2dd75f900bc6cc" } ] \ No newline at end of file