diff --git a/common/libcommon.a b/common/libcommon.a index 82ab0c0c0..5dc99c65b 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 1c988c8ef..ade4b70c0 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 484a13cda..8aabe9782 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/bootstub.elf b/panda/board/obj/body_h7/bootstub.elf index babce9260..cfb10fc67 100755 Binary files a/panda/board/obj/body_h7/bootstub.elf and b/panda/board/obj/body_h7/bootstub.elf differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index 8dd087f79..43ff4ffc1 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index d7cce1efa..69c4fad86 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index e76760f49..ecf35d2bd 100755 Binary files a/panda/board/obj/bootstub.body_h7.bin and b/panda/board/obj/bootstub.body_h7.bin differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index 804bc713f..c796f5eaf 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index 1cc7f8785..138851fa0 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote.bin b/panda/board/obj/bootstub.panda_h7_remote.bin index 1cc7f8785..138851fa0 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote.bin and b/panda/board/obj/bootstub.panda_h7_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_jungle_h7.bin b/panda/board/obj/bootstub.panda_jungle_h7.bin index c4ee4ceff..d9588a0b2 100755 Binary files a/panda/board/obj/bootstub.panda_jungle_h7.bin and b/panda/board/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_remote.bin b/panda/board/obj/bootstub.panda_remote.bin index 804bc713f..c796f5eaf 100755 Binary files a/panda/board/obj/bootstub.panda_remote.bin and b/panda/board/obj/bootstub.panda_remote.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index d6dd9c696..516eb068a 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-f20b1f2a-DEBUG"; +const uint8_t gitversion[19] = "DEV-301a45c7-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index d1c20f860..9aaa51526 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda/bootstub.elf b/panda/board/obj/panda/bootstub.elf index 1eb04fd9c..e2844bbad 100755 Binary files a/panda/board/obj/panda/bootstub.elf and b/panda/board/obj/panda/bootstub.elf differ diff --git a/panda/board/obj/panda/main.bin b/panda/board/obj/panda/main.bin index 9cc71242b..b5db35774 100755 Binary files a/panda/board/obj/panda/main.bin and b/panda/board/obj/panda/main.bin differ diff --git a/panda/board/obj/panda/main.elf b/panda/board/obj/panda/main.elf index 349e25f06..6f2d5be2c 100755 Binary files a/panda/board/obj/panda/main.elf and b/panda/board/obj/panda/main.elf differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index 4f9443c25..58068583e 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/bootstub.elf b/panda/board/obj/panda_h7/bootstub.elf index 285360a1a..40cc2325b 100755 Binary files a/panda/board/obj/panda_h7/bootstub.elf and b/panda/board/obj/panda_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index 63a354f56..fa45b7d95 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index d17eb9526..fc317c393 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index 5f5c0f601..de1e68881 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote/bootstub.elf b/panda/board/obj/panda_h7_remote/bootstub.elf index 5ea845a0f..f664f7f0a 100755 Binary files a/panda/board/obj/panda_h7_remote/bootstub.elf and b/panda/board/obj/panda_h7_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote/main.bin b/panda/board/obj/panda_h7_remote/main.bin index bfc9e3860..4195cccb0 100755 Binary files a/panda/board/obj/panda_h7_remote/main.bin and b/panda/board/obj/panda_h7_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_remote/main.elf b/panda/board/obj/panda_h7_remote/main.elf index 6d8b9a4ae..1a2194213 100755 Binary files a/panda/board/obj/panda_h7_remote/main.elf and b/panda/board/obj/panda_h7_remote/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index e64a8030d..0f96144cb 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/bootstub.elf b/panda/board/obj/panda_jungle_h7/bootstub.elf index ec9707e72..80d4878db 100755 Binary files a/panda/board/obj/panda_jungle_h7/bootstub.elf and b/panda/board/obj/panda_jungle_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index 904f67ea9..5bef145f9 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index cc0ad6e48..c8942cdea 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed index 81c01d854..6093e91a0 100644 Binary files a/panda/board/obj/panda_remote.bin.signed and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote/bootstub.elf b/panda/board/obj/panda_remote/bootstub.elf index bdaf8795f..a670e9d6c 100755 Binary files a/panda/board/obj/panda_remote/bootstub.elf and b/panda/board/obj/panda_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_remote/main.bin b/panda/board/obj/panda_remote/main.bin index 805ea08eb..d0187d8d4 100755 Binary files a/panda/board/obj/panda_remote/main.bin and b/panda/board/obj/panda_remote/main.bin differ diff --git a/panda/board/obj/panda_remote/main.elf b/panda/board/obj/panda_remote/main.elf index 6e2601ed4..c2be48e77 100755 Binary files a/panda/board/obj/panda_remote/main.elf and b/panda/board/obj/panda_remote/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index 28b50c240..7d5532868 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-f20b1f2a-DEBUG \ No newline at end of file +DEV-301a45c7-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 687b78dae..525813374 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_5063363100648596688) { - out_5063363100648596688[0] = delta_x[0] + nom_x[0]; - out_5063363100648596688[1] = delta_x[1] + nom_x[1]; - out_5063363100648596688[2] = delta_x[2] + nom_x[2]; - out_5063363100648596688[3] = delta_x[3] + nom_x[3]; - out_5063363100648596688[4] = delta_x[4] + nom_x[4]; - out_5063363100648596688[5] = delta_x[5] + nom_x[5]; - out_5063363100648596688[6] = delta_x[6] + nom_x[6]; - out_5063363100648596688[7] = delta_x[7] + nom_x[7]; - out_5063363100648596688[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_3959736040406304353) { + out_3959736040406304353[0] = delta_x[0] + nom_x[0]; + out_3959736040406304353[1] = delta_x[1] + nom_x[1]; + out_3959736040406304353[2] = delta_x[2] + nom_x[2]; + out_3959736040406304353[3] = delta_x[3] + nom_x[3]; + out_3959736040406304353[4] = delta_x[4] + nom_x[4]; + out_3959736040406304353[5] = delta_x[5] + nom_x[5]; + out_3959736040406304353[6] = delta_x[6] + nom_x[6]; + out_3959736040406304353[7] = delta_x[7] + nom_x[7]; + out_3959736040406304353[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1051973791362877167) { - out_1051973791362877167[0] = -nom_x[0] + true_x[0]; - out_1051973791362877167[1] = -nom_x[1] + true_x[1]; - out_1051973791362877167[2] = -nom_x[2] + true_x[2]; - out_1051973791362877167[3] = -nom_x[3] + true_x[3]; - out_1051973791362877167[4] = -nom_x[4] + true_x[4]; - out_1051973791362877167[5] = -nom_x[5] + true_x[5]; - out_1051973791362877167[6] = -nom_x[6] + true_x[6]; - out_1051973791362877167[7] = -nom_x[7] + true_x[7]; - out_1051973791362877167[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_7899142377712818401) { + out_7899142377712818401[0] = -nom_x[0] + true_x[0]; + out_7899142377712818401[1] = -nom_x[1] + true_x[1]; + out_7899142377712818401[2] = -nom_x[2] + true_x[2]; + out_7899142377712818401[3] = -nom_x[3] + true_x[3]; + out_7899142377712818401[4] = -nom_x[4] + true_x[4]; + out_7899142377712818401[5] = -nom_x[5] + true_x[5]; + out_7899142377712818401[6] = -nom_x[6] + true_x[6]; + out_7899142377712818401[7] = -nom_x[7] + true_x[7]; + out_7899142377712818401[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_5315869906856267240) { - out_5315869906856267240[0] = 1.0; - out_5315869906856267240[1] = 0.0; - out_5315869906856267240[2] = 0.0; - out_5315869906856267240[3] = 0.0; - out_5315869906856267240[4] = 0.0; - out_5315869906856267240[5] = 0.0; - out_5315869906856267240[6] = 0.0; - out_5315869906856267240[7] = 0.0; - out_5315869906856267240[8] = 0.0; - out_5315869906856267240[9] = 0.0; - out_5315869906856267240[10] = 1.0; - out_5315869906856267240[11] = 0.0; - out_5315869906856267240[12] = 0.0; - out_5315869906856267240[13] = 0.0; - out_5315869906856267240[14] = 0.0; - out_5315869906856267240[15] = 0.0; - out_5315869906856267240[16] = 0.0; - out_5315869906856267240[17] = 0.0; - out_5315869906856267240[18] = 0.0; - out_5315869906856267240[19] = 0.0; - out_5315869906856267240[20] = 1.0; - out_5315869906856267240[21] = 0.0; - out_5315869906856267240[22] = 0.0; - out_5315869906856267240[23] = 0.0; - out_5315869906856267240[24] = 0.0; - out_5315869906856267240[25] = 0.0; - out_5315869906856267240[26] = 0.0; - out_5315869906856267240[27] = 0.0; - out_5315869906856267240[28] = 0.0; - out_5315869906856267240[29] = 0.0; - out_5315869906856267240[30] = 1.0; - out_5315869906856267240[31] = 0.0; - out_5315869906856267240[32] = 0.0; - out_5315869906856267240[33] = 0.0; - out_5315869906856267240[34] = 0.0; - out_5315869906856267240[35] = 0.0; - out_5315869906856267240[36] = 0.0; - out_5315869906856267240[37] = 0.0; - out_5315869906856267240[38] = 0.0; - out_5315869906856267240[39] = 0.0; - out_5315869906856267240[40] = 1.0; - out_5315869906856267240[41] = 0.0; - out_5315869906856267240[42] = 0.0; - out_5315869906856267240[43] = 0.0; - out_5315869906856267240[44] = 0.0; - out_5315869906856267240[45] = 0.0; - out_5315869906856267240[46] = 0.0; - out_5315869906856267240[47] = 0.0; - out_5315869906856267240[48] = 0.0; - out_5315869906856267240[49] = 0.0; - out_5315869906856267240[50] = 1.0; - out_5315869906856267240[51] = 0.0; - out_5315869906856267240[52] = 0.0; - out_5315869906856267240[53] = 0.0; - out_5315869906856267240[54] = 0.0; - out_5315869906856267240[55] = 0.0; - out_5315869906856267240[56] = 0.0; - out_5315869906856267240[57] = 0.0; - out_5315869906856267240[58] = 0.0; - out_5315869906856267240[59] = 0.0; - out_5315869906856267240[60] = 1.0; - out_5315869906856267240[61] = 0.0; - out_5315869906856267240[62] = 0.0; - out_5315869906856267240[63] = 0.0; - out_5315869906856267240[64] = 0.0; - out_5315869906856267240[65] = 0.0; - out_5315869906856267240[66] = 0.0; - out_5315869906856267240[67] = 0.0; - out_5315869906856267240[68] = 0.0; - out_5315869906856267240[69] = 0.0; - out_5315869906856267240[70] = 1.0; - out_5315869906856267240[71] = 0.0; - out_5315869906856267240[72] = 0.0; - out_5315869906856267240[73] = 0.0; - out_5315869906856267240[74] = 0.0; - out_5315869906856267240[75] = 0.0; - out_5315869906856267240[76] = 0.0; - out_5315869906856267240[77] = 0.0; - out_5315869906856267240[78] = 0.0; - out_5315869906856267240[79] = 0.0; - out_5315869906856267240[80] = 1.0; +void H_mod_fun(double *state, double *out_8853016966935855995) { + out_8853016966935855995[0] = 1.0; + out_8853016966935855995[1] = 0.0; + out_8853016966935855995[2] = 0.0; + out_8853016966935855995[3] = 0.0; + out_8853016966935855995[4] = 0.0; + out_8853016966935855995[5] = 0.0; + out_8853016966935855995[6] = 0.0; + out_8853016966935855995[7] = 0.0; + out_8853016966935855995[8] = 0.0; + out_8853016966935855995[9] = 0.0; + out_8853016966935855995[10] = 1.0; + out_8853016966935855995[11] = 0.0; + out_8853016966935855995[12] = 0.0; + out_8853016966935855995[13] = 0.0; + out_8853016966935855995[14] = 0.0; + out_8853016966935855995[15] = 0.0; + out_8853016966935855995[16] = 0.0; + out_8853016966935855995[17] = 0.0; + out_8853016966935855995[18] = 0.0; + out_8853016966935855995[19] = 0.0; + out_8853016966935855995[20] = 1.0; + out_8853016966935855995[21] = 0.0; + out_8853016966935855995[22] = 0.0; + out_8853016966935855995[23] = 0.0; + out_8853016966935855995[24] = 0.0; + out_8853016966935855995[25] = 0.0; + out_8853016966935855995[26] = 0.0; + out_8853016966935855995[27] = 0.0; + out_8853016966935855995[28] = 0.0; + out_8853016966935855995[29] = 0.0; + out_8853016966935855995[30] = 1.0; + out_8853016966935855995[31] = 0.0; + out_8853016966935855995[32] = 0.0; + out_8853016966935855995[33] = 0.0; + out_8853016966935855995[34] = 0.0; + out_8853016966935855995[35] = 0.0; + out_8853016966935855995[36] = 0.0; + out_8853016966935855995[37] = 0.0; + out_8853016966935855995[38] = 0.0; + out_8853016966935855995[39] = 0.0; + out_8853016966935855995[40] = 1.0; + out_8853016966935855995[41] = 0.0; + out_8853016966935855995[42] = 0.0; + out_8853016966935855995[43] = 0.0; + out_8853016966935855995[44] = 0.0; + out_8853016966935855995[45] = 0.0; + out_8853016966935855995[46] = 0.0; + out_8853016966935855995[47] = 0.0; + out_8853016966935855995[48] = 0.0; + out_8853016966935855995[49] = 0.0; + out_8853016966935855995[50] = 1.0; + out_8853016966935855995[51] = 0.0; + out_8853016966935855995[52] = 0.0; + out_8853016966935855995[53] = 0.0; + out_8853016966935855995[54] = 0.0; + out_8853016966935855995[55] = 0.0; + out_8853016966935855995[56] = 0.0; + out_8853016966935855995[57] = 0.0; + out_8853016966935855995[58] = 0.0; + out_8853016966935855995[59] = 0.0; + out_8853016966935855995[60] = 1.0; + out_8853016966935855995[61] = 0.0; + out_8853016966935855995[62] = 0.0; + out_8853016966935855995[63] = 0.0; + out_8853016966935855995[64] = 0.0; + out_8853016966935855995[65] = 0.0; + out_8853016966935855995[66] = 0.0; + out_8853016966935855995[67] = 0.0; + out_8853016966935855995[68] = 0.0; + out_8853016966935855995[69] = 0.0; + out_8853016966935855995[70] = 1.0; + out_8853016966935855995[71] = 0.0; + out_8853016966935855995[72] = 0.0; + out_8853016966935855995[73] = 0.0; + out_8853016966935855995[74] = 0.0; + out_8853016966935855995[75] = 0.0; + out_8853016966935855995[76] = 0.0; + out_8853016966935855995[77] = 0.0; + out_8853016966935855995[78] = 0.0; + out_8853016966935855995[79] = 0.0; + out_8853016966935855995[80] = 1.0; } -void f_fun(double *state, double dt, double *out_2679807483918754558) { - out_2679807483918754558[0] = state[0]; - out_2679807483918754558[1] = state[1]; - out_2679807483918754558[2] = state[2]; - out_2679807483918754558[3] = state[3]; - out_2679807483918754558[4] = state[4]; - out_2679807483918754558[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5]; - out_2679807483918754558[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_2679807483918754558[7] = state[7]; - out_2679807483918754558[8] = state[8]; +void f_fun(double *state, double dt, double *out_8070155753173713130) { + out_8070155753173713130[0] = state[0]; + out_8070155753173713130[1] = state[1]; + out_8070155753173713130[2] = state[2]; + out_8070155753173713130[3] = state[3]; + out_8070155753173713130[4] = state[4]; + out_8070155753173713130[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5]; + out_8070155753173713130[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_8070155753173713130[7] = state[7]; + out_8070155753173713130[8] = state[8]; } -void F_fun(double *state, double dt, double *out_8053926087528305413) { - out_8053926087528305413[0] = 1; - out_8053926087528305413[1] = 0; - out_8053926087528305413[2] = 0; - out_8053926087528305413[3] = 0; - out_8053926087528305413[4] = 0; - out_8053926087528305413[5] = 0; - out_8053926087528305413[6] = 0; - out_8053926087528305413[7] = 0; - out_8053926087528305413[8] = 0; - out_8053926087528305413[9] = 0; - out_8053926087528305413[10] = 1; - out_8053926087528305413[11] = 0; - out_8053926087528305413[12] = 0; - out_8053926087528305413[13] = 0; - out_8053926087528305413[14] = 0; - out_8053926087528305413[15] = 0; - out_8053926087528305413[16] = 0; - out_8053926087528305413[17] = 0; - out_8053926087528305413[18] = 0; - out_8053926087528305413[19] = 0; - out_8053926087528305413[20] = 1; - out_8053926087528305413[21] = 0; - out_8053926087528305413[22] = 0; - out_8053926087528305413[23] = 0; - out_8053926087528305413[24] = 0; - out_8053926087528305413[25] = 0; - out_8053926087528305413[26] = 0; - out_8053926087528305413[27] = 0; - out_8053926087528305413[28] = 0; - out_8053926087528305413[29] = 0; - out_8053926087528305413[30] = 1; - out_8053926087528305413[31] = 0; - out_8053926087528305413[32] = 0; - out_8053926087528305413[33] = 0; - out_8053926087528305413[34] = 0; - out_8053926087528305413[35] = 0; - out_8053926087528305413[36] = 0; - out_8053926087528305413[37] = 0; - out_8053926087528305413[38] = 0; - out_8053926087528305413[39] = 0; - out_8053926087528305413[40] = 1; - out_8053926087528305413[41] = 0; - out_8053926087528305413[42] = 0; - out_8053926087528305413[43] = 0; - out_8053926087528305413[44] = 0; - out_8053926087528305413[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_8053926087528305413[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_8053926087528305413[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8053926087528305413[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8053926087528305413[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_8053926087528305413[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_8053926087528305413[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_8053926087528305413[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_8053926087528305413[53] = -9.8100000000000005*dt; - out_8053926087528305413[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_8053926087528305413[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_8053926087528305413[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8053926087528305413[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8053926087528305413[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_8053926087528305413[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_8053926087528305413[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_8053926087528305413[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8053926087528305413[62] = 0; - out_8053926087528305413[63] = 0; - out_8053926087528305413[64] = 0; - out_8053926087528305413[65] = 0; - out_8053926087528305413[66] = 0; - out_8053926087528305413[67] = 0; - out_8053926087528305413[68] = 0; - out_8053926087528305413[69] = 0; - out_8053926087528305413[70] = 1; - out_8053926087528305413[71] = 0; - out_8053926087528305413[72] = 0; - out_8053926087528305413[73] = 0; - out_8053926087528305413[74] = 0; - out_8053926087528305413[75] = 0; - out_8053926087528305413[76] = 0; - out_8053926087528305413[77] = 0; - out_8053926087528305413[78] = 0; - out_8053926087528305413[79] = 0; - out_8053926087528305413[80] = 1; +void F_fun(double *state, double dt, double *out_2319703520718919116) { + out_2319703520718919116[0] = 1; + out_2319703520718919116[1] = 0; + out_2319703520718919116[2] = 0; + out_2319703520718919116[3] = 0; + out_2319703520718919116[4] = 0; + out_2319703520718919116[5] = 0; + out_2319703520718919116[6] = 0; + out_2319703520718919116[7] = 0; + out_2319703520718919116[8] = 0; + out_2319703520718919116[9] = 0; + out_2319703520718919116[10] = 1; + out_2319703520718919116[11] = 0; + out_2319703520718919116[12] = 0; + out_2319703520718919116[13] = 0; + out_2319703520718919116[14] = 0; + out_2319703520718919116[15] = 0; + out_2319703520718919116[16] = 0; + out_2319703520718919116[17] = 0; + out_2319703520718919116[18] = 0; + out_2319703520718919116[19] = 0; + out_2319703520718919116[20] = 1; + out_2319703520718919116[21] = 0; + out_2319703520718919116[22] = 0; + out_2319703520718919116[23] = 0; + out_2319703520718919116[24] = 0; + out_2319703520718919116[25] = 0; + out_2319703520718919116[26] = 0; + out_2319703520718919116[27] = 0; + out_2319703520718919116[28] = 0; + out_2319703520718919116[29] = 0; + out_2319703520718919116[30] = 1; + out_2319703520718919116[31] = 0; + out_2319703520718919116[32] = 0; + out_2319703520718919116[33] = 0; + out_2319703520718919116[34] = 0; + out_2319703520718919116[35] = 0; + out_2319703520718919116[36] = 0; + out_2319703520718919116[37] = 0; + out_2319703520718919116[38] = 0; + out_2319703520718919116[39] = 0; + out_2319703520718919116[40] = 1; + out_2319703520718919116[41] = 0; + out_2319703520718919116[42] = 0; + out_2319703520718919116[43] = 0; + out_2319703520718919116[44] = 0; + out_2319703520718919116[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_2319703520718919116[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2319703520718919116[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2319703520718919116[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2319703520718919116[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_2319703520718919116[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2319703520718919116[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2319703520718919116[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2319703520718919116[53] = -9.8100000000000005*dt; + out_2319703520718919116[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_2319703520718919116[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2319703520718919116[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2319703520718919116[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2319703520718919116[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_2319703520718919116[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2319703520718919116[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_2319703520718919116[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2319703520718919116[62] = 0; + out_2319703520718919116[63] = 0; + out_2319703520718919116[64] = 0; + out_2319703520718919116[65] = 0; + out_2319703520718919116[66] = 0; + out_2319703520718919116[67] = 0; + out_2319703520718919116[68] = 0; + out_2319703520718919116[69] = 0; + out_2319703520718919116[70] = 1; + out_2319703520718919116[71] = 0; + out_2319703520718919116[72] = 0; + out_2319703520718919116[73] = 0; + out_2319703520718919116[74] = 0; + out_2319703520718919116[75] = 0; + out_2319703520718919116[76] = 0; + out_2319703520718919116[77] = 0; + out_2319703520718919116[78] = 0; + out_2319703520718919116[79] = 0; + out_2319703520718919116[80] = 1; } -void h_25(double *state, double *unused, double *out_5716369086896639497) { - out_5716369086896639497[0] = state[6]; +void h_25(double *state, double *unused, double *out_4616275488682323747) { + out_4616275488682323747[0] = state[6]; } -void H_25(double *state, double *unused, double *out_3877318319055969997) { - out_3877318319055969997[0] = 0; - out_3877318319055969997[1] = 0; - out_3877318319055969997[2] = 0; - out_3877318319055969997[3] = 0; - out_3877318319055969997[4] = 0; - out_3877318319055969997[5] = 0; - out_3877318319055969997[6] = 1; - out_3877318319055969997[7] = 0; - out_3877318319055969997[8] = 0; +void H_25(double *state, double *unused, double *out_7903896044907373786) { + out_7903896044907373786[0] = 0; + out_7903896044907373786[1] = 0; + out_7903896044907373786[2] = 0; + out_7903896044907373786[3] = 0; + out_7903896044907373786[4] = 0; + out_7903896044907373786[5] = 0; + out_7903896044907373786[6] = 1; + out_7903896044907373786[7] = 0; + out_7903896044907373786[8] = 0; } -void h_24(double *state, double *unused, double *out_8949486097258288203) { - out_8949486097258288203[0] = state[4]; - out_8949486097258288203[1] = state[5]; +void h_24(double *state, double *unused, double *out_1240821179762925085) { + out_1240821179762925085[0] = state[4]; + out_1240821179762925085[1] = state[5]; } -void H_24(double *state, double *unused, double *out_6049967918061469563) { - out_6049967918061469563[0] = 0; - out_6049967918061469563[1] = 0; - out_6049967918061469563[2] = 0; - out_6049967918061469563[3] = 0; - out_6049967918061469563[4] = 1; - out_6049967918061469563[5] = 0; - out_6049967918061469563[6] = 0; - out_6049967918061469563[7] = 0; - out_6049967918061469563[8] = 0; - out_6049967918061469563[9] = 0; - out_6049967918061469563[10] = 0; - out_6049967918061469563[11] = 0; - out_6049967918061469563[12] = 0; - out_6049967918061469563[13] = 0; - out_6049967918061469563[14] = 1; - out_6049967918061469563[15] = 0; - out_6049967918061469563[16] = 0; - out_6049967918061469563[17] = 0; +void H_24(double *state, double *unused, double *out_4345970446105050650) { + out_4345970446105050650[0] = 0; + out_4345970446105050650[1] = 0; + out_4345970446105050650[2] = 0; + out_4345970446105050650[3] = 0; + out_4345970446105050650[4] = 1; + out_4345970446105050650[5] = 0; + out_4345970446105050650[6] = 0; + out_4345970446105050650[7] = 0; + out_4345970446105050650[8] = 0; + out_4345970446105050650[9] = 0; + out_4345970446105050650[10] = 0; + out_4345970446105050650[11] = 0; + out_4345970446105050650[12] = 0; + out_4345970446105050650[13] = 0; + out_4345970446105050650[14] = 1; + out_4345970446105050650[15] = 0; + out_4345970446105050650[16] = 0; + out_4345970446105050650[17] = 0; } -void h_30(double *state, double *unused, double *out_5873555755247768642) { - out_5873555755247768642[0] = state[4]; +void h_30(double *state, double *unused, double *out_978825960428301397) { + out_978825960428301397[0] = state[4]; } -void H_30(double *state, double *unused, double *out_3039372022435646758) { - out_3039372022435646758[0] = 0; - out_3039372022435646758[1] = 0; - out_3039372022435646758[2] = 0; - out_3039372022435646758[3] = 0; - out_3039372022435646758[4] = 1; - out_3039372022435646758[5] = 0; - out_3039372022435646758[6] = 0; - out_3039372022435646758[7] = 0; - out_3039372022435646758[8] = 0; +void H_30(double *state, double *unused, double *out_8033234992050613856) { + out_8033234992050613856[0] = 0; + out_8033234992050613856[1] = 0; + out_8033234992050613856[2] = 0; + out_8033234992050613856[3] = 0; + out_8033234992050613856[4] = 1; + out_8033234992050613856[5] = 0; + out_8033234992050613856[6] = 0; + out_8033234992050613856[7] = 0; + out_8033234992050613856[8] = 0; } -void h_26(double *state, double *unused, double *out_8147827403163551441) { - out_8147827403163551441[0] = state[7]; +void h_26(double *state, double *unused, double *out_35316107526638962) { + out_35316107526638962[0] = state[7]; } -void H_26(double *state, double *unused, double *out_7618821637930026221) { - out_7618821637930026221[0] = 0; - out_7618821637930026221[1] = 0; - out_7618821637930026221[2] = 0; - out_7618821637930026221[3] = 0; - out_7618821637930026221[4] = 0; - out_7618821637930026221[5] = 0; - out_7618821637930026221[6] = 0; - out_7618821637930026221[7] = 1; - out_7618821637930026221[8] = 0; +void H_26(double *state, double *unused, double *out_6801344709928121606) { + out_6801344709928121606[0] = 0; + out_6801344709928121606[1] = 0; + out_6801344709928121606[2] = 0; + out_6801344709928121606[3] = 0; + out_6801344709928121606[4] = 0; + out_6801344709928121606[5] = 0; + out_6801344709928121606[6] = 0; + out_6801344709928121606[7] = 1; + out_6801344709928121606[8] = 0; } -void h_27(double *state, double *unused, double *out_3868275412420834157) { - out_3868275412420834157[0] = state[3]; +void h_27(double *state, double *unused, double *out_8256782726505873065) { + out_8256782726505873065[0] = state[3]; } -void H_27(double *state, double *unused, double *out_864608710635221847) { - out_864608710635221847[0] = 0; - out_864608710635221847[1] = 0; - out_864608710635221847[2] = 0; - out_864608710635221847[3] = 1; - out_864608710635221847[4] = 0; - out_864608710635221847[5] = 0; - out_864608710635221847[6] = 0; - out_864608710635221847[7] = 0; - out_864608710635221847[8] = 0; +void H_27(double *state, double *unused, double *out_8238745769858512849) { + out_8238745769858512849[0] = 0; + out_8238745769858512849[1] = 0; + out_8238745769858512849[2] = 0; + out_8238745769858512849[3] = 1; + out_8238745769858512849[4] = 0; + out_8238745769858512849[5] = 0; + out_8238745769858512849[6] = 0; + out_8238745769858512849[7] = 0; + out_8238745769858512849[8] = 0; } -void h_29(double *state, double *unused, double *out_2922753709018228498) { - out_2922753709018228498[0] = state[1]; +void h_29(double *state, double *unused, double *out_4022847307232544248) { + out_4022847307232544248[0] = state[1]; } -void H_29(double *state, double *unused, double *out_3549603366750038942) { - out_3549603366750038942[0] = 0; - out_3549603366750038942[1] = 1; - out_3549603366750038942[2] = 0; - out_3549603366750038942[3] = 0; - out_3549603366750038942[4] = 0; - out_3549603366750038942[5] = 0; - out_3549603366750038942[6] = 0; - out_3549603366750038942[7] = 0; - out_3549603366750038942[8] = 0; +void H_29(double *state, double *unused, double *out_7523003647736221672) { + out_7523003647736221672[0] = 0; + out_7523003647736221672[1] = 1; + out_7523003647736221672[2] = 0; + out_7523003647736221672[3] = 0; + out_7523003647736221672[4] = 0; + out_7523003647736221672[5] = 0; + out_7523003647736221672[6] = 0; + out_7523003647736221672[7] = 0; + out_7523003647736221672[8] = 0; } -void h_28(double *state, double *unused, double *out_2837524384607109846) { - out_2837524384607109846[0] = state[0]; +void h_28(double *state, double *unused, double *out_3255109458845027420) { + out_3255109458845027420[0] = state[0]; } -void H_28(double *state, double *unused, double *out_5931153033303859760) { - out_5931153033303859760[0] = 1; - out_5931153033303859760[1] = 0; - out_5931153033303859760[2] = 0; - out_5931153033303859760[3] = 0; - out_5931153033303859760[4] = 0; - out_5931153033303859760[5] = 0; - out_5931153033303859760[6] = 0; - out_5931153033303859760[7] = 0; - out_5931153033303859760[8] = 0; +void H_28(double *state, double *unused, double *out_5841341408903799370) { + out_5841341408903799370[0] = 1; + out_5841341408903799370[1] = 0; + out_5841341408903799370[2] = 0; + out_5841341408903799370[3] = 0; + out_5841341408903799370[4] = 0; + out_5841341408903799370[5] = 0; + out_5841341408903799370[6] = 0; + out_5841341408903799370[7] = 0; + out_5841341408903799370[8] = 0; } -void h_31(double *state, double *unused, double *out_7419317287452796635) { - out_7419317287452796635[0] = state[8]; +void h_31(double *state, double *unused, double *out_6638919472171716073) { + out_6638919472171716073[0] = state[8]; } -void H_31(double *state, double *unused, double *out_3846672357179009569) { - out_3846672357179009569[0] = 0; - out_3846672357179009569[1] = 0; - out_3846672357179009569[2] = 0; - out_3846672357179009569[3] = 0; - out_3846672357179009569[4] = 0; - out_3846672357179009569[5] = 0; - out_3846672357179009569[6] = 0; - out_3846672357179009569[7] = 0; - out_3846672357179009569[8] = 1; +void H_31(double *state, double *unused, double *out_7873250083030413358) { + out_7873250083030413358[0] = 0; + out_7873250083030413358[1] = 0; + out_7873250083030413358[2] = 0; + out_7873250083030413358[3] = 0; + out_7873250083030413358[4] = 0; + out_7873250083030413358[5] = 0; + out_7873250083030413358[6] = 0; + out_7873250083030413358[7] = 0; + out_7873250083030413358[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_5063363100648596688) { - err_fun(nom_x, delta_x, out_5063363100648596688); +void car_err_fun(double *nom_x, double *delta_x, double *out_3959736040406304353) { + err_fun(nom_x, delta_x, out_3959736040406304353); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1051973791362877167) { - inv_err_fun(nom_x, true_x, out_1051973791362877167); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_7899142377712818401) { + inv_err_fun(nom_x, true_x, out_7899142377712818401); } -void car_H_mod_fun(double *state, double *out_5315869906856267240) { - H_mod_fun(state, out_5315869906856267240); +void car_H_mod_fun(double *state, double *out_8853016966935855995) { + H_mod_fun(state, out_8853016966935855995); } -void car_f_fun(double *state, double dt, double *out_2679807483918754558) { - f_fun(state, dt, out_2679807483918754558); +void car_f_fun(double *state, double dt, double *out_8070155753173713130) { + f_fun(state, dt, out_8070155753173713130); } -void car_F_fun(double *state, double dt, double *out_8053926087528305413) { - F_fun(state, dt, out_8053926087528305413); +void car_F_fun(double *state, double dt, double *out_2319703520718919116) { + F_fun(state, dt, out_2319703520718919116); } -void car_h_25(double *state, double *unused, double *out_5716369086896639497) { - h_25(state, unused, out_5716369086896639497); +void car_h_25(double *state, double *unused, double *out_4616275488682323747) { + h_25(state, unused, out_4616275488682323747); } -void car_H_25(double *state, double *unused, double *out_3877318319055969997) { - H_25(state, unused, out_3877318319055969997); +void car_H_25(double *state, double *unused, double *out_7903896044907373786) { + H_25(state, unused, out_7903896044907373786); } -void car_h_24(double *state, double *unused, double *out_8949486097258288203) { - h_24(state, unused, out_8949486097258288203); +void car_h_24(double *state, double *unused, double *out_1240821179762925085) { + h_24(state, unused, out_1240821179762925085); } -void car_H_24(double *state, double *unused, double *out_6049967918061469563) { - H_24(state, unused, out_6049967918061469563); +void car_H_24(double *state, double *unused, double *out_4345970446105050650) { + H_24(state, unused, out_4345970446105050650); } -void car_h_30(double *state, double *unused, double *out_5873555755247768642) { - h_30(state, unused, out_5873555755247768642); +void car_h_30(double *state, double *unused, double *out_978825960428301397) { + h_30(state, unused, out_978825960428301397); } -void car_H_30(double *state, double *unused, double *out_3039372022435646758) { - H_30(state, unused, out_3039372022435646758); +void car_H_30(double *state, double *unused, double *out_8033234992050613856) { + H_30(state, unused, out_8033234992050613856); } -void car_h_26(double *state, double *unused, double *out_8147827403163551441) { - h_26(state, unused, out_8147827403163551441); +void car_h_26(double *state, double *unused, double *out_35316107526638962) { + h_26(state, unused, out_35316107526638962); } -void car_H_26(double *state, double *unused, double *out_7618821637930026221) { - H_26(state, unused, out_7618821637930026221); +void car_H_26(double *state, double *unused, double *out_6801344709928121606) { + H_26(state, unused, out_6801344709928121606); } -void car_h_27(double *state, double *unused, double *out_3868275412420834157) { - h_27(state, unused, out_3868275412420834157); +void car_h_27(double *state, double *unused, double *out_8256782726505873065) { + h_27(state, unused, out_8256782726505873065); } -void car_H_27(double *state, double *unused, double *out_864608710635221847) { - H_27(state, unused, out_864608710635221847); +void car_H_27(double *state, double *unused, double *out_8238745769858512849) { + H_27(state, unused, out_8238745769858512849); } -void car_h_29(double *state, double *unused, double *out_2922753709018228498) { - h_29(state, unused, out_2922753709018228498); +void car_h_29(double *state, double *unused, double *out_4022847307232544248) { + h_29(state, unused, out_4022847307232544248); } -void car_H_29(double *state, double *unused, double *out_3549603366750038942) { - H_29(state, unused, out_3549603366750038942); +void car_H_29(double *state, double *unused, double *out_7523003647736221672) { + H_29(state, unused, out_7523003647736221672); } -void car_h_28(double *state, double *unused, double *out_2837524384607109846) { - h_28(state, unused, out_2837524384607109846); +void car_h_28(double *state, double *unused, double *out_3255109458845027420) { + h_28(state, unused, out_3255109458845027420); } -void car_H_28(double *state, double *unused, double *out_5931153033303859760) { - H_28(state, unused, out_5931153033303859760); +void car_H_28(double *state, double *unused, double *out_5841341408903799370) { + H_28(state, unused, out_5841341408903799370); } -void car_h_31(double *state, double *unused, double *out_7419317287452796635) { - h_31(state, unused, out_7419317287452796635); +void car_h_31(double *state, double *unused, double *out_6638919472171716073) { + h_31(state, unused, out_6638919472171716073); } -void car_H_31(double *state, double *unused, double *out_3846672357179009569) { - H_31(state, unused, out_3846672357179009569); +void car_H_31(double *state, double *unused, double *out_7873250083030413358) { + H_31(state, unused, out_7873250083030413358); } 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 ba5ed14f4..f801f0e6b 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_5063363100648596688); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1051973791362877167); -void car_H_mod_fun(double *state, double *out_5315869906856267240); -void car_f_fun(double *state, double dt, double *out_2679807483918754558); -void car_F_fun(double *state, double dt, double *out_8053926087528305413); -void car_h_25(double *state, double *unused, double *out_5716369086896639497); -void car_H_25(double *state, double *unused, double *out_3877318319055969997); -void car_h_24(double *state, double *unused, double *out_8949486097258288203); -void car_H_24(double *state, double *unused, double *out_6049967918061469563); -void car_h_30(double *state, double *unused, double *out_5873555755247768642); -void car_H_30(double *state, double *unused, double *out_3039372022435646758); -void car_h_26(double *state, double *unused, double *out_8147827403163551441); -void car_H_26(double *state, double *unused, double *out_7618821637930026221); -void car_h_27(double *state, double *unused, double *out_3868275412420834157); -void car_H_27(double *state, double *unused, double *out_864608710635221847); -void car_h_29(double *state, double *unused, double *out_2922753709018228498); -void car_H_29(double *state, double *unused, double *out_3549603366750038942); -void car_h_28(double *state, double *unused, double *out_2837524384607109846); -void car_H_28(double *state, double *unused, double *out_5931153033303859760); -void car_h_31(double *state, double *unused, double *out_7419317287452796635); -void car_H_31(double *state, double *unused, double *out_3846672357179009569); +void car_err_fun(double *nom_x, double *delta_x, double *out_3959736040406304353); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_7899142377712818401); +void car_H_mod_fun(double *state, double *out_8853016966935855995); +void car_f_fun(double *state, double dt, double *out_8070155753173713130); +void car_F_fun(double *state, double dt, double *out_2319703520718919116); +void car_h_25(double *state, double *unused, double *out_4616275488682323747); +void car_H_25(double *state, double *unused, double *out_7903896044907373786); +void car_h_24(double *state, double *unused, double *out_1240821179762925085); +void car_H_24(double *state, double *unused, double *out_4345970446105050650); +void car_h_30(double *state, double *unused, double *out_978825960428301397); +void car_H_30(double *state, double *unused, double *out_8033234992050613856); +void car_h_26(double *state, double *unused, double *out_35316107526638962); +void car_H_26(double *state, double *unused, double *out_6801344709928121606); +void car_h_27(double *state, double *unused, double *out_8256782726505873065); +void car_H_27(double *state, double *unused, double *out_8238745769858512849); +void car_h_29(double *state, double *unused, double *out_4022847307232544248); +void car_H_29(double *state, double *unused, double *out_7523003647736221672); +void car_h_28(double *state, double *unused, double *out_3255109458845027420); +void car_H_28(double *state, double *unused, double *out_5841341408903799370); +void car_h_31(double *state, double *unused, double *out_6638919472171716073); +void car_H_31(double *state, double *unused, double *out_7873250083030413358); void car_predict(double *in_x, double *in_P, double *in_Q, double dt); void car_set_mass(double x); void car_set_rotational_inertia(double x); diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index 9bf6cd5f6..bd2fd7c40 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_3341780452292505997) { - out_3341780452292505997[0] = delta_x[0] + nom_x[0]; - out_3341780452292505997[1] = delta_x[1] + nom_x[1]; - out_3341780452292505997[2] = delta_x[2] + nom_x[2]; - out_3341780452292505997[3] = delta_x[3] + nom_x[3]; - out_3341780452292505997[4] = delta_x[4] + nom_x[4]; - out_3341780452292505997[5] = delta_x[5] + nom_x[5]; - out_3341780452292505997[6] = delta_x[6] + nom_x[6]; - out_3341780452292505997[7] = delta_x[7] + nom_x[7]; - out_3341780452292505997[8] = delta_x[8] + nom_x[8]; - out_3341780452292505997[9] = delta_x[9] + nom_x[9]; - out_3341780452292505997[10] = delta_x[10] + nom_x[10]; - out_3341780452292505997[11] = delta_x[11] + nom_x[11]; - out_3341780452292505997[12] = delta_x[12] + nom_x[12]; - out_3341780452292505997[13] = delta_x[13] + nom_x[13]; - out_3341780452292505997[14] = delta_x[14] + nom_x[14]; - out_3341780452292505997[15] = delta_x[15] + nom_x[15]; - out_3341780452292505997[16] = delta_x[16] + nom_x[16]; - out_3341780452292505997[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_3650099251769799718) { + out_3650099251769799718[0] = delta_x[0] + nom_x[0]; + out_3650099251769799718[1] = delta_x[1] + nom_x[1]; + out_3650099251769799718[2] = delta_x[2] + nom_x[2]; + out_3650099251769799718[3] = delta_x[3] + nom_x[3]; + out_3650099251769799718[4] = delta_x[4] + nom_x[4]; + out_3650099251769799718[5] = delta_x[5] + nom_x[5]; + out_3650099251769799718[6] = delta_x[6] + nom_x[6]; + out_3650099251769799718[7] = delta_x[7] + nom_x[7]; + out_3650099251769799718[8] = delta_x[8] + nom_x[8]; + out_3650099251769799718[9] = delta_x[9] + nom_x[9]; + out_3650099251769799718[10] = delta_x[10] + nom_x[10]; + out_3650099251769799718[11] = delta_x[11] + nom_x[11]; + out_3650099251769799718[12] = delta_x[12] + nom_x[12]; + out_3650099251769799718[13] = delta_x[13] + nom_x[13]; + out_3650099251769799718[14] = delta_x[14] + nom_x[14]; + out_3650099251769799718[15] = delta_x[15] + nom_x[15]; + out_3650099251769799718[16] = delta_x[16] + nom_x[16]; + out_3650099251769799718[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5409150910353518766) { - out_5409150910353518766[0] = -nom_x[0] + true_x[0]; - out_5409150910353518766[1] = -nom_x[1] + true_x[1]; - out_5409150910353518766[2] = -nom_x[2] + true_x[2]; - out_5409150910353518766[3] = -nom_x[3] + true_x[3]; - out_5409150910353518766[4] = -nom_x[4] + true_x[4]; - out_5409150910353518766[5] = -nom_x[5] + true_x[5]; - out_5409150910353518766[6] = -nom_x[6] + true_x[6]; - out_5409150910353518766[7] = -nom_x[7] + true_x[7]; - out_5409150910353518766[8] = -nom_x[8] + true_x[8]; - out_5409150910353518766[9] = -nom_x[9] + true_x[9]; - out_5409150910353518766[10] = -nom_x[10] + true_x[10]; - out_5409150910353518766[11] = -nom_x[11] + true_x[11]; - out_5409150910353518766[12] = -nom_x[12] + true_x[12]; - out_5409150910353518766[13] = -nom_x[13] + true_x[13]; - out_5409150910353518766[14] = -nom_x[14] + true_x[14]; - out_5409150910353518766[15] = -nom_x[15] + true_x[15]; - out_5409150910353518766[16] = -nom_x[16] + true_x[16]; - out_5409150910353518766[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_4549899801136771746) { + out_4549899801136771746[0] = -nom_x[0] + true_x[0]; + out_4549899801136771746[1] = -nom_x[1] + true_x[1]; + out_4549899801136771746[2] = -nom_x[2] + true_x[2]; + out_4549899801136771746[3] = -nom_x[3] + true_x[3]; + out_4549899801136771746[4] = -nom_x[4] + true_x[4]; + out_4549899801136771746[5] = -nom_x[5] + true_x[5]; + out_4549899801136771746[6] = -nom_x[6] + true_x[6]; + out_4549899801136771746[7] = -nom_x[7] + true_x[7]; + out_4549899801136771746[8] = -nom_x[8] + true_x[8]; + out_4549899801136771746[9] = -nom_x[9] + true_x[9]; + out_4549899801136771746[10] = -nom_x[10] + true_x[10]; + out_4549899801136771746[11] = -nom_x[11] + true_x[11]; + out_4549899801136771746[12] = -nom_x[12] + true_x[12]; + out_4549899801136771746[13] = -nom_x[13] + true_x[13]; + out_4549899801136771746[14] = -nom_x[14] + true_x[14]; + out_4549899801136771746[15] = -nom_x[15] + true_x[15]; + out_4549899801136771746[16] = -nom_x[16] + true_x[16]; + out_4549899801136771746[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_8300172317114018464) { - out_8300172317114018464[0] = 1.0; - out_8300172317114018464[1] = 0.0; - out_8300172317114018464[2] = 0.0; - out_8300172317114018464[3] = 0.0; - out_8300172317114018464[4] = 0.0; - out_8300172317114018464[5] = 0.0; - out_8300172317114018464[6] = 0.0; - out_8300172317114018464[7] = 0.0; - out_8300172317114018464[8] = 0.0; - out_8300172317114018464[9] = 0.0; - out_8300172317114018464[10] = 0.0; - out_8300172317114018464[11] = 0.0; - out_8300172317114018464[12] = 0.0; - out_8300172317114018464[13] = 0.0; - out_8300172317114018464[14] = 0.0; - out_8300172317114018464[15] = 0.0; - out_8300172317114018464[16] = 0.0; - out_8300172317114018464[17] = 0.0; - out_8300172317114018464[18] = 0.0; - out_8300172317114018464[19] = 1.0; - out_8300172317114018464[20] = 0.0; - out_8300172317114018464[21] = 0.0; - out_8300172317114018464[22] = 0.0; - out_8300172317114018464[23] = 0.0; - out_8300172317114018464[24] = 0.0; - out_8300172317114018464[25] = 0.0; - out_8300172317114018464[26] = 0.0; - out_8300172317114018464[27] = 0.0; - out_8300172317114018464[28] = 0.0; - out_8300172317114018464[29] = 0.0; - out_8300172317114018464[30] = 0.0; - out_8300172317114018464[31] = 0.0; - out_8300172317114018464[32] = 0.0; - out_8300172317114018464[33] = 0.0; - out_8300172317114018464[34] = 0.0; - out_8300172317114018464[35] = 0.0; - out_8300172317114018464[36] = 0.0; - out_8300172317114018464[37] = 0.0; - out_8300172317114018464[38] = 1.0; - out_8300172317114018464[39] = 0.0; - out_8300172317114018464[40] = 0.0; - out_8300172317114018464[41] = 0.0; - out_8300172317114018464[42] = 0.0; - out_8300172317114018464[43] = 0.0; - out_8300172317114018464[44] = 0.0; - out_8300172317114018464[45] = 0.0; - out_8300172317114018464[46] = 0.0; - out_8300172317114018464[47] = 0.0; - out_8300172317114018464[48] = 0.0; - out_8300172317114018464[49] = 0.0; - out_8300172317114018464[50] = 0.0; - out_8300172317114018464[51] = 0.0; - out_8300172317114018464[52] = 0.0; - out_8300172317114018464[53] = 0.0; - out_8300172317114018464[54] = 0.0; - out_8300172317114018464[55] = 0.0; - out_8300172317114018464[56] = 0.0; - out_8300172317114018464[57] = 1.0; - out_8300172317114018464[58] = 0.0; - out_8300172317114018464[59] = 0.0; - out_8300172317114018464[60] = 0.0; - out_8300172317114018464[61] = 0.0; - out_8300172317114018464[62] = 0.0; - out_8300172317114018464[63] = 0.0; - out_8300172317114018464[64] = 0.0; - out_8300172317114018464[65] = 0.0; - out_8300172317114018464[66] = 0.0; - out_8300172317114018464[67] = 0.0; - out_8300172317114018464[68] = 0.0; - out_8300172317114018464[69] = 0.0; - out_8300172317114018464[70] = 0.0; - out_8300172317114018464[71] = 0.0; - out_8300172317114018464[72] = 0.0; - out_8300172317114018464[73] = 0.0; - out_8300172317114018464[74] = 0.0; - out_8300172317114018464[75] = 0.0; - out_8300172317114018464[76] = 1.0; - out_8300172317114018464[77] = 0.0; - out_8300172317114018464[78] = 0.0; - out_8300172317114018464[79] = 0.0; - out_8300172317114018464[80] = 0.0; - out_8300172317114018464[81] = 0.0; - out_8300172317114018464[82] = 0.0; - out_8300172317114018464[83] = 0.0; - out_8300172317114018464[84] = 0.0; - out_8300172317114018464[85] = 0.0; - out_8300172317114018464[86] = 0.0; - out_8300172317114018464[87] = 0.0; - out_8300172317114018464[88] = 0.0; - out_8300172317114018464[89] = 0.0; - out_8300172317114018464[90] = 0.0; - out_8300172317114018464[91] = 0.0; - out_8300172317114018464[92] = 0.0; - out_8300172317114018464[93] = 0.0; - out_8300172317114018464[94] = 0.0; - out_8300172317114018464[95] = 1.0; - out_8300172317114018464[96] = 0.0; - out_8300172317114018464[97] = 0.0; - out_8300172317114018464[98] = 0.0; - out_8300172317114018464[99] = 0.0; - out_8300172317114018464[100] = 0.0; - out_8300172317114018464[101] = 0.0; - out_8300172317114018464[102] = 0.0; - out_8300172317114018464[103] = 0.0; - out_8300172317114018464[104] = 0.0; - out_8300172317114018464[105] = 0.0; - out_8300172317114018464[106] = 0.0; - out_8300172317114018464[107] = 0.0; - out_8300172317114018464[108] = 0.0; - out_8300172317114018464[109] = 0.0; - out_8300172317114018464[110] = 0.0; - out_8300172317114018464[111] = 0.0; - out_8300172317114018464[112] = 0.0; - out_8300172317114018464[113] = 0.0; - out_8300172317114018464[114] = 1.0; - out_8300172317114018464[115] = 0.0; - out_8300172317114018464[116] = 0.0; - out_8300172317114018464[117] = 0.0; - out_8300172317114018464[118] = 0.0; - out_8300172317114018464[119] = 0.0; - out_8300172317114018464[120] = 0.0; - out_8300172317114018464[121] = 0.0; - out_8300172317114018464[122] = 0.0; - out_8300172317114018464[123] = 0.0; - out_8300172317114018464[124] = 0.0; - out_8300172317114018464[125] = 0.0; - out_8300172317114018464[126] = 0.0; - out_8300172317114018464[127] = 0.0; - out_8300172317114018464[128] = 0.0; - out_8300172317114018464[129] = 0.0; - out_8300172317114018464[130] = 0.0; - out_8300172317114018464[131] = 0.0; - out_8300172317114018464[132] = 0.0; - out_8300172317114018464[133] = 1.0; - out_8300172317114018464[134] = 0.0; - out_8300172317114018464[135] = 0.0; - out_8300172317114018464[136] = 0.0; - out_8300172317114018464[137] = 0.0; - out_8300172317114018464[138] = 0.0; - out_8300172317114018464[139] = 0.0; - out_8300172317114018464[140] = 0.0; - out_8300172317114018464[141] = 0.0; - out_8300172317114018464[142] = 0.0; - out_8300172317114018464[143] = 0.0; - out_8300172317114018464[144] = 0.0; - out_8300172317114018464[145] = 0.0; - out_8300172317114018464[146] = 0.0; - out_8300172317114018464[147] = 0.0; - out_8300172317114018464[148] = 0.0; - out_8300172317114018464[149] = 0.0; - out_8300172317114018464[150] = 0.0; - out_8300172317114018464[151] = 0.0; - out_8300172317114018464[152] = 1.0; - out_8300172317114018464[153] = 0.0; - out_8300172317114018464[154] = 0.0; - out_8300172317114018464[155] = 0.0; - out_8300172317114018464[156] = 0.0; - out_8300172317114018464[157] = 0.0; - out_8300172317114018464[158] = 0.0; - out_8300172317114018464[159] = 0.0; - out_8300172317114018464[160] = 0.0; - out_8300172317114018464[161] = 0.0; - out_8300172317114018464[162] = 0.0; - out_8300172317114018464[163] = 0.0; - out_8300172317114018464[164] = 0.0; - out_8300172317114018464[165] = 0.0; - out_8300172317114018464[166] = 0.0; - out_8300172317114018464[167] = 0.0; - out_8300172317114018464[168] = 0.0; - out_8300172317114018464[169] = 0.0; - out_8300172317114018464[170] = 0.0; - out_8300172317114018464[171] = 1.0; - out_8300172317114018464[172] = 0.0; - out_8300172317114018464[173] = 0.0; - out_8300172317114018464[174] = 0.0; - out_8300172317114018464[175] = 0.0; - out_8300172317114018464[176] = 0.0; - out_8300172317114018464[177] = 0.0; - out_8300172317114018464[178] = 0.0; - out_8300172317114018464[179] = 0.0; - out_8300172317114018464[180] = 0.0; - out_8300172317114018464[181] = 0.0; - out_8300172317114018464[182] = 0.0; - out_8300172317114018464[183] = 0.0; - out_8300172317114018464[184] = 0.0; - out_8300172317114018464[185] = 0.0; - out_8300172317114018464[186] = 0.0; - out_8300172317114018464[187] = 0.0; - out_8300172317114018464[188] = 0.0; - out_8300172317114018464[189] = 0.0; - out_8300172317114018464[190] = 1.0; - out_8300172317114018464[191] = 0.0; - out_8300172317114018464[192] = 0.0; - out_8300172317114018464[193] = 0.0; - out_8300172317114018464[194] = 0.0; - out_8300172317114018464[195] = 0.0; - out_8300172317114018464[196] = 0.0; - out_8300172317114018464[197] = 0.0; - out_8300172317114018464[198] = 0.0; - out_8300172317114018464[199] = 0.0; - out_8300172317114018464[200] = 0.0; - out_8300172317114018464[201] = 0.0; - out_8300172317114018464[202] = 0.0; - out_8300172317114018464[203] = 0.0; - out_8300172317114018464[204] = 0.0; - out_8300172317114018464[205] = 0.0; - out_8300172317114018464[206] = 0.0; - out_8300172317114018464[207] = 0.0; - out_8300172317114018464[208] = 0.0; - out_8300172317114018464[209] = 1.0; - out_8300172317114018464[210] = 0.0; - out_8300172317114018464[211] = 0.0; - out_8300172317114018464[212] = 0.0; - out_8300172317114018464[213] = 0.0; - out_8300172317114018464[214] = 0.0; - out_8300172317114018464[215] = 0.0; - out_8300172317114018464[216] = 0.0; - out_8300172317114018464[217] = 0.0; - out_8300172317114018464[218] = 0.0; - out_8300172317114018464[219] = 0.0; - out_8300172317114018464[220] = 0.0; - out_8300172317114018464[221] = 0.0; - out_8300172317114018464[222] = 0.0; - out_8300172317114018464[223] = 0.0; - out_8300172317114018464[224] = 0.0; - out_8300172317114018464[225] = 0.0; - out_8300172317114018464[226] = 0.0; - out_8300172317114018464[227] = 0.0; - out_8300172317114018464[228] = 1.0; - out_8300172317114018464[229] = 0.0; - out_8300172317114018464[230] = 0.0; - out_8300172317114018464[231] = 0.0; - out_8300172317114018464[232] = 0.0; - out_8300172317114018464[233] = 0.0; - out_8300172317114018464[234] = 0.0; - out_8300172317114018464[235] = 0.0; - out_8300172317114018464[236] = 0.0; - out_8300172317114018464[237] = 0.0; - out_8300172317114018464[238] = 0.0; - out_8300172317114018464[239] = 0.0; - out_8300172317114018464[240] = 0.0; - out_8300172317114018464[241] = 0.0; - out_8300172317114018464[242] = 0.0; - out_8300172317114018464[243] = 0.0; - out_8300172317114018464[244] = 0.0; - out_8300172317114018464[245] = 0.0; - out_8300172317114018464[246] = 0.0; - out_8300172317114018464[247] = 1.0; - out_8300172317114018464[248] = 0.0; - out_8300172317114018464[249] = 0.0; - out_8300172317114018464[250] = 0.0; - out_8300172317114018464[251] = 0.0; - out_8300172317114018464[252] = 0.0; - out_8300172317114018464[253] = 0.0; - out_8300172317114018464[254] = 0.0; - out_8300172317114018464[255] = 0.0; - out_8300172317114018464[256] = 0.0; - out_8300172317114018464[257] = 0.0; - out_8300172317114018464[258] = 0.0; - out_8300172317114018464[259] = 0.0; - out_8300172317114018464[260] = 0.0; - out_8300172317114018464[261] = 0.0; - out_8300172317114018464[262] = 0.0; - out_8300172317114018464[263] = 0.0; - out_8300172317114018464[264] = 0.0; - out_8300172317114018464[265] = 0.0; - out_8300172317114018464[266] = 1.0; - out_8300172317114018464[267] = 0.0; - out_8300172317114018464[268] = 0.0; - out_8300172317114018464[269] = 0.0; - out_8300172317114018464[270] = 0.0; - out_8300172317114018464[271] = 0.0; - out_8300172317114018464[272] = 0.0; - out_8300172317114018464[273] = 0.0; - out_8300172317114018464[274] = 0.0; - out_8300172317114018464[275] = 0.0; - out_8300172317114018464[276] = 0.0; - out_8300172317114018464[277] = 0.0; - out_8300172317114018464[278] = 0.0; - out_8300172317114018464[279] = 0.0; - out_8300172317114018464[280] = 0.0; - out_8300172317114018464[281] = 0.0; - out_8300172317114018464[282] = 0.0; - out_8300172317114018464[283] = 0.0; - out_8300172317114018464[284] = 0.0; - out_8300172317114018464[285] = 1.0; - out_8300172317114018464[286] = 0.0; - out_8300172317114018464[287] = 0.0; - out_8300172317114018464[288] = 0.0; - out_8300172317114018464[289] = 0.0; - out_8300172317114018464[290] = 0.0; - out_8300172317114018464[291] = 0.0; - out_8300172317114018464[292] = 0.0; - out_8300172317114018464[293] = 0.0; - out_8300172317114018464[294] = 0.0; - out_8300172317114018464[295] = 0.0; - out_8300172317114018464[296] = 0.0; - out_8300172317114018464[297] = 0.0; - out_8300172317114018464[298] = 0.0; - out_8300172317114018464[299] = 0.0; - out_8300172317114018464[300] = 0.0; - out_8300172317114018464[301] = 0.0; - out_8300172317114018464[302] = 0.0; - out_8300172317114018464[303] = 0.0; - out_8300172317114018464[304] = 1.0; - out_8300172317114018464[305] = 0.0; - out_8300172317114018464[306] = 0.0; - out_8300172317114018464[307] = 0.0; - out_8300172317114018464[308] = 0.0; - out_8300172317114018464[309] = 0.0; - out_8300172317114018464[310] = 0.0; - out_8300172317114018464[311] = 0.0; - out_8300172317114018464[312] = 0.0; - out_8300172317114018464[313] = 0.0; - out_8300172317114018464[314] = 0.0; - out_8300172317114018464[315] = 0.0; - out_8300172317114018464[316] = 0.0; - out_8300172317114018464[317] = 0.0; - out_8300172317114018464[318] = 0.0; - out_8300172317114018464[319] = 0.0; - out_8300172317114018464[320] = 0.0; - out_8300172317114018464[321] = 0.0; - out_8300172317114018464[322] = 0.0; - out_8300172317114018464[323] = 1.0; +void H_mod_fun(double *state, double *out_6815214623410224821) { + out_6815214623410224821[0] = 1.0; + out_6815214623410224821[1] = 0.0; + out_6815214623410224821[2] = 0.0; + out_6815214623410224821[3] = 0.0; + out_6815214623410224821[4] = 0.0; + out_6815214623410224821[5] = 0.0; + out_6815214623410224821[6] = 0.0; + out_6815214623410224821[7] = 0.0; + out_6815214623410224821[8] = 0.0; + out_6815214623410224821[9] = 0.0; + out_6815214623410224821[10] = 0.0; + out_6815214623410224821[11] = 0.0; + out_6815214623410224821[12] = 0.0; + out_6815214623410224821[13] = 0.0; + out_6815214623410224821[14] = 0.0; + out_6815214623410224821[15] = 0.0; + out_6815214623410224821[16] = 0.0; + out_6815214623410224821[17] = 0.0; + out_6815214623410224821[18] = 0.0; + out_6815214623410224821[19] = 1.0; + out_6815214623410224821[20] = 0.0; + out_6815214623410224821[21] = 0.0; + out_6815214623410224821[22] = 0.0; + out_6815214623410224821[23] = 0.0; + out_6815214623410224821[24] = 0.0; + out_6815214623410224821[25] = 0.0; + out_6815214623410224821[26] = 0.0; + out_6815214623410224821[27] = 0.0; + out_6815214623410224821[28] = 0.0; + out_6815214623410224821[29] = 0.0; + out_6815214623410224821[30] = 0.0; + out_6815214623410224821[31] = 0.0; + out_6815214623410224821[32] = 0.0; + out_6815214623410224821[33] = 0.0; + out_6815214623410224821[34] = 0.0; + out_6815214623410224821[35] = 0.0; + out_6815214623410224821[36] = 0.0; + out_6815214623410224821[37] = 0.0; + out_6815214623410224821[38] = 1.0; + out_6815214623410224821[39] = 0.0; + out_6815214623410224821[40] = 0.0; + out_6815214623410224821[41] = 0.0; + out_6815214623410224821[42] = 0.0; + out_6815214623410224821[43] = 0.0; + out_6815214623410224821[44] = 0.0; + out_6815214623410224821[45] = 0.0; + out_6815214623410224821[46] = 0.0; + out_6815214623410224821[47] = 0.0; + out_6815214623410224821[48] = 0.0; + out_6815214623410224821[49] = 0.0; + out_6815214623410224821[50] = 0.0; + out_6815214623410224821[51] = 0.0; + out_6815214623410224821[52] = 0.0; + out_6815214623410224821[53] = 0.0; + out_6815214623410224821[54] = 0.0; + out_6815214623410224821[55] = 0.0; + out_6815214623410224821[56] = 0.0; + out_6815214623410224821[57] = 1.0; + out_6815214623410224821[58] = 0.0; + out_6815214623410224821[59] = 0.0; + out_6815214623410224821[60] = 0.0; + out_6815214623410224821[61] = 0.0; + out_6815214623410224821[62] = 0.0; + out_6815214623410224821[63] = 0.0; + out_6815214623410224821[64] = 0.0; + out_6815214623410224821[65] = 0.0; + out_6815214623410224821[66] = 0.0; + out_6815214623410224821[67] = 0.0; + out_6815214623410224821[68] = 0.0; + out_6815214623410224821[69] = 0.0; + out_6815214623410224821[70] = 0.0; + out_6815214623410224821[71] = 0.0; + out_6815214623410224821[72] = 0.0; + out_6815214623410224821[73] = 0.0; + out_6815214623410224821[74] = 0.0; + out_6815214623410224821[75] = 0.0; + out_6815214623410224821[76] = 1.0; + out_6815214623410224821[77] = 0.0; + out_6815214623410224821[78] = 0.0; + out_6815214623410224821[79] = 0.0; + out_6815214623410224821[80] = 0.0; + out_6815214623410224821[81] = 0.0; + out_6815214623410224821[82] = 0.0; + out_6815214623410224821[83] = 0.0; + out_6815214623410224821[84] = 0.0; + out_6815214623410224821[85] = 0.0; + out_6815214623410224821[86] = 0.0; + out_6815214623410224821[87] = 0.0; + out_6815214623410224821[88] = 0.0; + out_6815214623410224821[89] = 0.0; + out_6815214623410224821[90] = 0.0; + out_6815214623410224821[91] = 0.0; + out_6815214623410224821[92] = 0.0; + out_6815214623410224821[93] = 0.0; + out_6815214623410224821[94] = 0.0; + out_6815214623410224821[95] = 1.0; + out_6815214623410224821[96] = 0.0; + out_6815214623410224821[97] = 0.0; + out_6815214623410224821[98] = 0.0; + out_6815214623410224821[99] = 0.0; + out_6815214623410224821[100] = 0.0; + out_6815214623410224821[101] = 0.0; + out_6815214623410224821[102] = 0.0; + out_6815214623410224821[103] = 0.0; + out_6815214623410224821[104] = 0.0; + out_6815214623410224821[105] = 0.0; + out_6815214623410224821[106] = 0.0; + out_6815214623410224821[107] = 0.0; + out_6815214623410224821[108] = 0.0; + out_6815214623410224821[109] = 0.0; + out_6815214623410224821[110] = 0.0; + out_6815214623410224821[111] = 0.0; + out_6815214623410224821[112] = 0.0; + out_6815214623410224821[113] = 0.0; + out_6815214623410224821[114] = 1.0; + out_6815214623410224821[115] = 0.0; + out_6815214623410224821[116] = 0.0; + out_6815214623410224821[117] = 0.0; + out_6815214623410224821[118] = 0.0; + out_6815214623410224821[119] = 0.0; + out_6815214623410224821[120] = 0.0; + out_6815214623410224821[121] = 0.0; + out_6815214623410224821[122] = 0.0; + out_6815214623410224821[123] = 0.0; + out_6815214623410224821[124] = 0.0; + out_6815214623410224821[125] = 0.0; + out_6815214623410224821[126] = 0.0; + out_6815214623410224821[127] = 0.0; + out_6815214623410224821[128] = 0.0; + out_6815214623410224821[129] = 0.0; + out_6815214623410224821[130] = 0.0; + out_6815214623410224821[131] = 0.0; + out_6815214623410224821[132] = 0.0; + out_6815214623410224821[133] = 1.0; + out_6815214623410224821[134] = 0.0; + out_6815214623410224821[135] = 0.0; + out_6815214623410224821[136] = 0.0; + out_6815214623410224821[137] = 0.0; + out_6815214623410224821[138] = 0.0; + out_6815214623410224821[139] = 0.0; + out_6815214623410224821[140] = 0.0; + out_6815214623410224821[141] = 0.0; + out_6815214623410224821[142] = 0.0; + out_6815214623410224821[143] = 0.0; + out_6815214623410224821[144] = 0.0; + out_6815214623410224821[145] = 0.0; + out_6815214623410224821[146] = 0.0; + out_6815214623410224821[147] = 0.0; + out_6815214623410224821[148] = 0.0; + out_6815214623410224821[149] = 0.0; + out_6815214623410224821[150] = 0.0; + out_6815214623410224821[151] = 0.0; + out_6815214623410224821[152] = 1.0; + out_6815214623410224821[153] = 0.0; + out_6815214623410224821[154] = 0.0; + out_6815214623410224821[155] = 0.0; + out_6815214623410224821[156] = 0.0; + out_6815214623410224821[157] = 0.0; + out_6815214623410224821[158] = 0.0; + out_6815214623410224821[159] = 0.0; + out_6815214623410224821[160] = 0.0; + out_6815214623410224821[161] = 0.0; + out_6815214623410224821[162] = 0.0; + out_6815214623410224821[163] = 0.0; + out_6815214623410224821[164] = 0.0; + out_6815214623410224821[165] = 0.0; + out_6815214623410224821[166] = 0.0; + out_6815214623410224821[167] = 0.0; + out_6815214623410224821[168] = 0.0; + out_6815214623410224821[169] = 0.0; + out_6815214623410224821[170] = 0.0; + out_6815214623410224821[171] = 1.0; + out_6815214623410224821[172] = 0.0; + out_6815214623410224821[173] = 0.0; + out_6815214623410224821[174] = 0.0; + out_6815214623410224821[175] = 0.0; + out_6815214623410224821[176] = 0.0; + out_6815214623410224821[177] = 0.0; + out_6815214623410224821[178] = 0.0; + out_6815214623410224821[179] = 0.0; + out_6815214623410224821[180] = 0.0; + out_6815214623410224821[181] = 0.0; + out_6815214623410224821[182] = 0.0; + out_6815214623410224821[183] = 0.0; + out_6815214623410224821[184] = 0.0; + out_6815214623410224821[185] = 0.0; + out_6815214623410224821[186] = 0.0; + out_6815214623410224821[187] = 0.0; + out_6815214623410224821[188] = 0.0; + out_6815214623410224821[189] = 0.0; + out_6815214623410224821[190] = 1.0; + out_6815214623410224821[191] = 0.0; + out_6815214623410224821[192] = 0.0; + out_6815214623410224821[193] = 0.0; + out_6815214623410224821[194] = 0.0; + out_6815214623410224821[195] = 0.0; + out_6815214623410224821[196] = 0.0; + out_6815214623410224821[197] = 0.0; + out_6815214623410224821[198] = 0.0; + out_6815214623410224821[199] = 0.0; + out_6815214623410224821[200] = 0.0; + out_6815214623410224821[201] = 0.0; + out_6815214623410224821[202] = 0.0; + out_6815214623410224821[203] = 0.0; + out_6815214623410224821[204] = 0.0; + out_6815214623410224821[205] = 0.0; + out_6815214623410224821[206] = 0.0; + out_6815214623410224821[207] = 0.0; + out_6815214623410224821[208] = 0.0; + out_6815214623410224821[209] = 1.0; + out_6815214623410224821[210] = 0.0; + out_6815214623410224821[211] = 0.0; + out_6815214623410224821[212] = 0.0; + out_6815214623410224821[213] = 0.0; + out_6815214623410224821[214] = 0.0; + out_6815214623410224821[215] = 0.0; + out_6815214623410224821[216] = 0.0; + out_6815214623410224821[217] = 0.0; + out_6815214623410224821[218] = 0.0; + out_6815214623410224821[219] = 0.0; + out_6815214623410224821[220] = 0.0; + out_6815214623410224821[221] = 0.0; + out_6815214623410224821[222] = 0.0; + out_6815214623410224821[223] = 0.0; + out_6815214623410224821[224] = 0.0; + out_6815214623410224821[225] = 0.0; + out_6815214623410224821[226] = 0.0; + out_6815214623410224821[227] = 0.0; + out_6815214623410224821[228] = 1.0; + out_6815214623410224821[229] = 0.0; + out_6815214623410224821[230] = 0.0; + out_6815214623410224821[231] = 0.0; + out_6815214623410224821[232] = 0.0; + out_6815214623410224821[233] = 0.0; + out_6815214623410224821[234] = 0.0; + out_6815214623410224821[235] = 0.0; + out_6815214623410224821[236] = 0.0; + out_6815214623410224821[237] = 0.0; + out_6815214623410224821[238] = 0.0; + out_6815214623410224821[239] = 0.0; + out_6815214623410224821[240] = 0.0; + out_6815214623410224821[241] = 0.0; + out_6815214623410224821[242] = 0.0; + out_6815214623410224821[243] = 0.0; + out_6815214623410224821[244] = 0.0; + out_6815214623410224821[245] = 0.0; + out_6815214623410224821[246] = 0.0; + out_6815214623410224821[247] = 1.0; + out_6815214623410224821[248] = 0.0; + out_6815214623410224821[249] = 0.0; + out_6815214623410224821[250] = 0.0; + out_6815214623410224821[251] = 0.0; + out_6815214623410224821[252] = 0.0; + out_6815214623410224821[253] = 0.0; + out_6815214623410224821[254] = 0.0; + out_6815214623410224821[255] = 0.0; + out_6815214623410224821[256] = 0.0; + out_6815214623410224821[257] = 0.0; + out_6815214623410224821[258] = 0.0; + out_6815214623410224821[259] = 0.0; + out_6815214623410224821[260] = 0.0; + out_6815214623410224821[261] = 0.0; + out_6815214623410224821[262] = 0.0; + out_6815214623410224821[263] = 0.0; + out_6815214623410224821[264] = 0.0; + out_6815214623410224821[265] = 0.0; + out_6815214623410224821[266] = 1.0; + out_6815214623410224821[267] = 0.0; + out_6815214623410224821[268] = 0.0; + out_6815214623410224821[269] = 0.0; + out_6815214623410224821[270] = 0.0; + out_6815214623410224821[271] = 0.0; + out_6815214623410224821[272] = 0.0; + out_6815214623410224821[273] = 0.0; + out_6815214623410224821[274] = 0.0; + out_6815214623410224821[275] = 0.0; + out_6815214623410224821[276] = 0.0; + out_6815214623410224821[277] = 0.0; + out_6815214623410224821[278] = 0.0; + out_6815214623410224821[279] = 0.0; + out_6815214623410224821[280] = 0.0; + out_6815214623410224821[281] = 0.0; + out_6815214623410224821[282] = 0.0; + out_6815214623410224821[283] = 0.0; + out_6815214623410224821[284] = 0.0; + out_6815214623410224821[285] = 1.0; + out_6815214623410224821[286] = 0.0; + out_6815214623410224821[287] = 0.0; + out_6815214623410224821[288] = 0.0; + out_6815214623410224821[289] = 0.0; + out_6815214623410224821[290] = 0.0; + out_6815214623410224821[291] = 0.0; + out_6815214623410224821[292] = 0.0; + out_6815214623410224821[293] = 0.0; + out_6815214623410224821[294] = 0.0; + out_6815214623410224821[295] = 0.0; + out_6815214623410224821[296] = 0.0; + out_6815214623410224821[297] = 0.0; + out_6815214623410224821[298] = 0.0; + out_6815214623410224821[299] = 0.0; + out_6815214623410224821[300] = 0.0; + out_6815214623410224821[301] = 0.0; + out_6815214623410224821[302] = 0.0; + out_6815214623410224821[303] = 0.0; + out_6815214623410224821[304] = 1.0; + out_6815214623410224821[305] = 0.0; + out_6815214623410224821[306] = 0.0; + out_6815214623410224821[307] = 0.0; + out_6815214623410224821[308] = 0.0; + out_6815214623410224821[309] = 0.0; + out_6815214623410224821[310] = 0.0; + out_6815214623410224821[311] = 0.0; + out_6815214623410224821[312] = 0.0; + out_6815214623410224821[313] = 0.0; + out_6815214623410224821[314] = 0.0; + out_6815214623410224821[315] = 0.0; + out_6815214623410224821[316] = 0.0; + out_6815214623410224821[317] = 0.0; + out_6815214623410224821[318] = 0.0; + out_6815214623410224821[319] = 0.0; + out_6815214623410224821[320] = 0.0; + out_6815214623410224821[321] = 0.0; + out_6815214623410224821[322] = 0.0; + out_6815214623410224821[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4063489601932878054) { - out_4063489601932878054[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_4063489601932878054[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_4063489601932878054[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_4063489601932878054[3] = dt*state[12] + state[3]; - out_4063489601932878054[4] = dt*state[13] + state[4]; - out_4063489601932878054[5] = dt*state[14] + state[5]; - out_4063489601932878054[6] = state[6]; - out_4063489601932878054[7] = state[7]; - out_4063489601932878054[8] = state[8]; - out_4063489601932878054[9] = state[9]; - out_4063489601932878054[10] = state[10]; - out_4063489601932878054[11] = state[11]; - out_4063489601932878054[12] = state[12]; - out_4063489601932878054[13] = state[13]; - out_4063489601932878054[14] = state[14]; - out_4063489601932878054[15] = state[15]; - out_4063489601932878054[16] = state[16]; - out_4063489601932878054[17] = state[17]; +void f_fun(double *state, double dt, double *out_4724171785691844407) { + out_4724171785691844407[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_4724171785691844407[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_4724171785691844407[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_4724171785691844407[3] = dt*state[12] + state[3]; + out_4724171785691844407[4] = dt*state[13] + state[4]; + out_4724171785691844407[5] = dt*state[14] + state[5]; + out_4724171785691844407[6] = state[6]; + out_4724171785691844407[7] = state[7]; + out_4724171785691844407[8] = state[8]; + out_4724171785691844407[9] = state[9]; + out_4724171785691844407[10] = state[10]; + out_4724171785691844407[11] = state[11]; + out_4724171785691844407[12] = state[12]; + out_4724171785691844407[13] = state[13]; + out_4724171785691844407[14] = state[14]; + out_4724171785691844407[15] = state[15]; + out_4724171785691844407[16] = state[16]; + out_4724171785691844407[17] = state[17]; } -void F_fun(double *state, double dt, double *out_493800117352810078) { - out_493800117352810078[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_493800117352810078[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_493800117352810078[2] = 0; - out_493800117352810078[3] = 0; - out_493800117352810078[4] = 0; - out_493800117352810078[5] = 0; - out_493800117352810078[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_493800117352810078[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_493800117352810078[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_493800117352810078[9] = 0; - out_493800117352810078[10] = 0; - out_493800117352810078[11] = 0; - out_493800117352810078[12] = 0; - out_493800117352810078[13] = 0; - out_493800117352810078[14] = 0; - out_493800117352810078[15] = 0; - out_493800117352810078[16] = 0; - out_493800117352810078[17] = 0; - out_493800117352810078[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_493800117352810078[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_493800117352810078[20] = 0; - out_493800117352810078[21] = 0; - out_493800117352810078[22] = 0; - out_493800117352810078[23] = 0; - out_493800117352810078[24] = 0; - out_493800117352810078[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_493800117352810078[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_493800117352810078[27] = 0; - out_493800117352810078[28] = 0; - out_493800117352810078[29] = 0; - out_493800117352810078[30] = 0; - out_493800117352810078[31] = 0; - out_493800117352810078[32] = 0; - out_493800117352810078[33] = 0; - out_493800117352810078[34] = 0; - out_493800117352810078[35] = 0; - out_493800117352810078[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_493800117352810078[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_493800117352810078[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_493800117352810078[39] = 0; - out_493800117352810078[40] = 0; - out_493800117352810078[41] = 0; - out_493800117352810078[42] = 0; - out_493800117352810078[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_493800117352810078[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_493800117352810078[45] = 0; - out_493800117352810078[46] = 0; - out_493800117352810078[47] = 0; - out_493800117352810078[48] = 0; - out_493800117352810078[49] = 0; - out_493800117352810078[50] = 0; - out_493800117352810078[51] = 0; - out_493800117352810078[52] = 0; - out_493800117352810078[53] = 0; - out_493800117352810078[54] = 0; - out_493800117352810078[55] = 0; - out_493800117352810078[56] = 0; - out_493800117352810078[57] = 1; - out_493800117352810078[58] = 0; - out_493800117352810078[59] = 0; - out_493800117352810078[60] = 0; - out_493800117352810078[61] = 0; - out_493800117352810078[62] = 0; - out_493800117352810078[63] = 0; - out_493800117352810078[64] = 0; - out_493800117352810078[65] = 0; - out_493800117352810078[66] = dt; - out_493800117352810078[67] = 0; - out_493800117352810078[68] = 0; - out_493800117352810078[69] = 0; - out_493800117352810078[70] = 0; - out_493800117352810078[71] = 0; - out_493800117352810078[72] = 0; - out_493800117352810078[73] = 0; - out_493800117352810078[74] = 0; - out_493800117352810078[75] = 0; - out_493800117352810078[76] = 1; - out_493800117352810078[77] = 0; - out_493800117352810078[78] = 0; - out_493800117352810078[79] = 0; - out_493800117352810078[80] = 0; - out_493800117352810078[81] = 0; - out_493800117352810078[82] = 0; - out_493800117352810078[83] = 0; - out_493800117352810078[84] = 0; - out_493800117352810078[85] = dt; - out_493800117352810078[86] = 0; - out_493800117352810078[87] = 0; - out_493800117352810078[88] = 0; - out_493800117352810078[89] = 0; - out_493800117352810078[90] = 0; - out_493800117352810078[91] = 0; - out_493800117352810078[92] = 0; - out_493800117352810078[93] = 0; - out_493800117352810078[94] = 0; - out_493800117352810078[95] = 1; - out_493800117352810078[96] = 0; - out_493800117352810078[97] = 0; - out_493800117352810078[98] = 0; - out_493800117352810078[99] = 0; - out_493800117352810078[100] = 0; - out_493800117352810078[101] = 0; - out_493800117352810078[102] = 0; - out_493800117352810078[103] = 0; - out_493800117352810078[104] = dt; - out_493800117352810078[105] = 0; - out_493800117352810078[106] = 0; - out_493800117352810078[107] = 0; - out_493800117352810078[108] = 0; - out_493800117352810078[109] = 0; - out_493800117352810078[110] = 0; - out_493800117352810078[111] = 0; - out_493800117352810078[112] = 0; - out_493800117352810078[113] = 0; - out_493800117352810078[114] = 1; - out_493800117352810078[115] = 0; - out_493800117352810078[116] = 0; - out_493800117352810078[117] = 0; - out_493800117352810078[118] = 0; - out_493800117352810078[119] = 0; - out_493800117352810078[120] = 0; - out_493800117352810078[121] = 0; - out_493800117352810078[122] = 0; - out_493800117352810078[123] = 0; - out_493800117352810078[124] = 0; - out_493800117352810078[125] = 0; - out_493800117352810078[126] = 0; - out_493800117352810078[127] = 0; - out_493800117352810078[128] = 0; - out_493800117352810078[129] = 0; - out_493800117352810078[130] = 0; - out_493800117352810078[131] = 0; - out_493800117352810078[132] = 0; - out_493800117352810078[133] = 1; - out_493800117352810078[134] = 0; - out_493800117352810078[135] = 0; - out_493800117352810078[136] = 0; - out_493800117352810078[137] = 0; - out_493800117352810078[138] = 0; - out_493800117352810078[139] = 0; - out_493800117352810078[140] = 0; - out_493800117352810078[141] = 0; - out_493800117352810078[142] = 0; - out_493800117352810078[143] = 0; - out_493800117352810078[144] = 0; - out_493800117352810078[145] = 0; - out_493800117352810078[146] = 0; - out_493800117352810078[147] = 0; - out_493800117352810078[148] = 0; - out_493800117352810078[149] = 0; - out_493800117352810078[150] = 0; - out_493800117352810078[151] = 0; - out_493800117352810078[152] = 1; - out_493800117352810078[153] = 0; - out_493800117352810078[154] = 0; - out_493800117352810078[155] = 0; - out_493800117352810078[156] = 0; - out_493800117352810078[157] = 0; - out_493800117352810078[158] = 0; - out_493800117352810078[159] = 0; - out_493800117352810078[160] = 0; - out_493800117352810078[161] = 0; - out_493800117352810078[162] = 0; - out_493800117352810078[163] = 0; - out_493800117352810078[164] = 0; - out_493800117352810078[165] = 0; - out_493800117352810078[166] = 0; - out_493800117352810078[167] = 0; - out_493800117352810078[168] = 0; - out_493800117352810078[169] = 0; - out_493800117352810078[170] = 0; - out_493800117352810078[171] = 1; - out_493800117352810078[172] = 0; - out_493800117352810078[173] = 0; - out_493800117352810078[174] = 0; - out_493800117352810078[175] = 0; - out_493800117352810078[176] = 0; - out_493800117352810078[177] = 0; - out_493800117352810078[178] = 0; - out_493800117352810078[179] = 0; - out_493800117352810078[180] = 0; - out_493800117352810078[181] = 0; - out_493800117352810078[182] = 0; - out_493800117352810078[183] = 0; - out_493800117352810078[184] = 0; - out_493800117352810078[185] = 0; - out_493800117352810078[186] = 0; - out_493800117352810078[187] = 0; - out_493800117352810078[188] = 0; - out_493800117352810078[189] = 0; - out_493800117352810078[190] = 1; - out_493800117352810078[191] = 0; - out_493800117352810078[192] = 0; - out_493800117352810078[193] = 0; - out_493800117352810078[194] = 0; - out_493800117352810078[195] = 0; - out_493800117352810078[196] = 0; - out_493800117352810078[197] = 0; - out_493800117352810078[198] = 0; - out_493800117352810078[199] = 0; - out_493800117352810078[200] = 0; - out_493800117352810078[201] = 0; - out_493800117352810078[202] = 0; - out_493800117352810078[203] = 0; - out_493800117352810078[204] = 0; - out_493800117352810078[205] = 0; - out_493800117352810078[206] = 0; - out_493800117352810078[207] = 0; - out_493800117352810078[208] = 0; - out_493800117352810078[209] = 1; - out_493800117352810078[210] = 0; - out_493800117352810078[211] = 0; - out_493800117352810078[212] = 0; - out_493800117352810078[213] = 0; - out_493800117352810078[214] = 0; - out_493800117352810078[215] = 0; - out_493800117352810078[216] = 0; - out_493800117352810078[217] = 0; - out_493800117352810078[218] = 0; - out_493800117352810078[219] = 0; - out_493800117352810078[220] = 0; - out_493800117352810078[221] = 0; - out_493800117352810078[222] = 0; - out_493800117352810078[223] = 0; - out_493800117352810078[224] = 0; - out_493800117352810078[225] = 0; - out_493800117352810078[226] = 0; - out_493800117352810078[227] = 0; - out_493800117352810078[228] = 1; - out_493800117352810078[229] = 0; - out_493800117352810078[230] = 0; - out_493800117352810078[231] = 0; - out_493800117352810078[232] = 0; - out_493800117352810078[233] = 0; - out_493800117352810078[234] = 0; - out_493800117352810078[235] = 0; - out_493800117352810078[236] = 0; - out_493800117352810078[237] = 0; - out_493800117352810078[238] = 0; - out_493800117352810078[239] = 0; - out_493800117352810078[240] = 0; - out_493800117352810078[241] = 0; - out_493800117352810078[242] = 0; - out_493800117352810078[243] = 0; - out_493800117352810078[244] = 0; - out_493800117352810078[245] = 0; - out_493800117352810078[246] = 0; - out_493800117352810078[247] = 1; - out_493800117352810078[248] = 0; - out_493800117352810078[249] = 0; - out_493800117352810078[250] = 0; - out_493800117352810078[251] = 0; - out_493800117352810078[252] = 0; - out_493800117352810078[253] = 0; - out_493800117352810078[254] = 0; - out_493800117352810078[255] = 0; - out_493800117352810078[256] = 0; - out_493800117352810078[257] = 0; - out_493800117352810078[258] = 0; - out_493800117352810078[259] = 0; - out_493800117352810078[260] = 0; - out_493800117352810078[261] = 0; - out_493800117352810078[262] = 0; - out_493800117352810078[263] = 0; - out_493800117352810078[264] = 0; - out_493800117352810078[265] = 0; - out_493800117352810078[266] = 1; - out_493800117352810078[267] = 0; - out_493800117352810078[268] = 0; - out_493800117352810078[269] = 0; - out_493800117352810078[270] = 0; - out_493800117352810078[271] = 0; - out_493800117352810078[272] = 0; - out_493800117352810078[273] = 0; - out_493800117352810078[274] = 0; - out_493800117352810078[275] = 0; - out_493800117352810078[276] = 0; - out_493800117352810078[277] = 0; - out_493800117352810078[278] = 0; - out_493800117352810078[279] = 0; - out_493800117352810078[280] = 0; - out_493800117352810078[281] = 0; - out_493800117352810078[282] = 0; - out_493800117352810078[283] = 0; - out_493800117352810078[284] = 0; - out_493800117352810078[285] = 1; - out_493800117352810078[286] = 0; - out_493800117352810078[287] = 0; - out_493800117352810078[288] = 0; - out_493800117352810078[289] = 0; - out_493800117352810078[290] = 0; - out_493800117352810078[291] = 0; - out_493800117352810078[292] = 0; - out_493800117352810078[293] = 0; - out_493800117352810078[294] = 0; - out_493800117352810078[295] = 0; - out_493800117352810078[296] = 0; - out_493800117352810078[297] = 0; - out_493800117352810078[298] = 0; - out_493800117352810078[299] = 0; - out_493800117352810078[300] = 0; - out_493800117352810078[301] = 0; - out_493800117352810078[302] = 0; - out_493800117352810078[303] = 0; - out_493800117352810078[304] = 1; - out_493800117352810078[305] = 0; - out_493800117352810078[306] = 0; - out_493800117352810078[307] = 0; - out_493800117352810078[308] = 0; - out_493800117352810078[309] = 0; - out_493800117352810078[310] = 0; - out_493800117352810078[311] = 0; - out_493800117352810078[312] = 0; - out_493800117352810078[313] = 0; - out_493800117352810078[314] = 0; - out_493800117352810078[315] = 0; - out_493800117352810078[316] = 0; - out_493800117352810078[317] = 0; - out_493800117352810078[318] = 0; - out_493800117352810078[319] = 0; - out_493800117352810078[320] = 0; - out_493800117352810078[321] = 0; - out_493800117352810078[322] = 0; - out_493800117352810078[323] = 1; +void F_fun(double *state, double dt, double *out_638456415896569458) { + out_638456415896569458[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_638456415896569458[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_638456415896569458[2] = 0; + out_638456415896569458[3] = 0; + out_638456415896569458[4] = 0; + out_638456415896569458[5] = 0; + out_638456415896569458[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_638456415896569458[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_638456415896569458[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_638456415896569458[9] = 0; + out_638456415896569458[10] = 0; + out_638456415896569458[11] = 0; + out_638456415896569458[12] = 0; + out_638456415896569458[13] = 0; + out_638456415896569458[14] = 0; + out_638456415896569458[15] = 0; + out_638456415896569458[16] = 0; + out_638456415896569458[17] = 0; + out_638456415896569458[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_638456415896569458[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_638456415896569458[20] = 0; + out_638456415896569458[21] = 0; + out_638456415896569458[22] = 0; + out_638456415896569458[23] = 0; + out_638456415896569458[24] = 0; + out_638456415896569458[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_638456415896569458[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_638456415896569458[27] = 0; + out_638456415896569458[28] = 0; + out_638456415896569458[29] = 0; + out_638456415896569458[30] = 0; + out_638456415896569458[31] = 0; + out_638456415896569458[32] = 0; + out_638456415896569458[33] = 0; + out_638456415896569458[34] = 0; + out_638456415896569458[35] = 0; + out_638456415896569458[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_638456415896569458[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_638456415896569458[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_638456415896569458[39] = 0; + out_638456415896569458[40] = 0; + out_638456415896569458[41] = 0; + out_638456415896569458[42] = 0; + out_638456415896569458[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_638456415896569458[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_638456415896569458[45] = 0; + out_638456415896569458[46] = 0; + out_638456415896569458[47] = 0; + out_638456415896569458[48] = 0; + out_638456415896569458[49] = 0; + out_638456415896569458[50] = 0; + out_638456415896569458[51] = 0; + out_638456415896569458[52] = 0; + out_638456415896569458[53] = 0; + out_638456415896569458[54] = 0; + out_638456415896569458[55] = 0; + out_638456415896569458[56] = 0; + out_638456415896569458[57] = 1; + out_638456415896569458[58] = 0; + out_638456415896569458[59] = 0; + out_638456415896569458[60] = 0; + out_638456415896569458[61] = 0; + out_638456415896569458[62] = 0; + out_638456415896569458[63] = 0; + out_638456415896569458[64] = 0; + out_638456415896569458[65] = 0; + out_638456415896569458[66] = dt; + out_638456415896569458[67] = 0; + out_638456415896569458[68] = 0; + out_638456415896569458[69] = 0; + out_638456415896569458[70] = 0; + out_638456415896569458[71] = 0; + out_638456415896569458[72] = 0; + out_638456415896569458[73] = 0; + out_638456415896569458[74] = 0; + out_638456415896569458[75] = 0; + out_638456415896569458[76] = 1; + out_638456415896569458[77] = 0; + out_638456415896569458[78] = 0; + out_638456415896569458[79] = 0; + out_638456415896569458[80] = 0; + out_638456415896569458[81] = 0; + out_638456415896569458[82] = 0; + out_638456415896569458[83] = 0; + out_638456415896569458[84] = 0; + out_638456415896569458[85] = dt; + out_638456415896569458[86] = 0; + out_638456415896569458[87] = 0; + out_638456415896569458[88] = 0; + out_638456415896569458[89] = 0; + out_638456415896569458[90] = 0; + out_638456415896569458[91] = 0; + out_638456415896569458[92] = 0; + out_638456415896569458[93] = 0; + out_638456415896569458[94] = 0; + out_638456415896569458[95] = 1; + out_638456415896569458[96] = 0; + out_638456415896569458[97] = 0; + out_638456415896569458[98] = 0; + out_638456415896569458[99] = 0; + out_638456415896569458[100] = 0; + out_638456415896569458[101] = 0; + out_638456415896569458[102] = 0; + out_638456415896569458[103] = 0; + out_638456415896569458[104] = dt; + out_638456415896569458[105] = 0; + out_638456415896569458[106] = 0; + out_638456415896569458[107] = 0; + out_638456415896569458[108] = 0; + out_638456415896569458[109] = 0; + out_638456415896569458[110] = 0; + out_638456415896569458[111] = 0; + out_638456415896569458[112] = 0; + out_638456415896569458[113] = 0; + out_638456415896569458[114] = 1; + out_638456415896569458[115] = 0; + out_638456415896569458[116] = 0; + out_638456415896569458[117] = 0; + out_638456415896569458[118] = 0; + out_638456415896569458[119] = 0; + out_638456415896569458[120] = 0; + out_638456415896569458[121] = 0; + out_638456415896569458[122] = 0; + out_638456415896569458[123] = 0; + out_638456415896569458[124] = 0; + out_638456415896569458[125] = 0; + out_638456415896569458[126] = 0; + out_638456415896569458[127] = 0; + out_638456415896569458[128] = 0; + out_638456415896569458[129] = 0; + out_638456415896569458[130] = 0; + out_638456415896569458[131] = 0; + out_638456415896569458[132] = 0; + out_638456415896569458[133] = 1; + out_638456415896569458[134] = 0; + out_638456415896569458[135] = 0; + out_638456415896569458[136] = 0; + out_638456415896569458[137] = 0; + out_638456415896569458[138] = 0; + out_638456415896569458[139] = 0; + out_638456415896569458[140] = 0; + out_638456415896569458[141] = 0; + out_638456415896569458[142] = 0; + out_638456415896569458[143] = 0; + out_638456415896569458[144] = 0; + out_638456415896569458[145] = 0; + out_638456415896569458[146] = 0; + out_638456415896569458[147] = 0; + out_638456415896569458[148] = 0; + out_638456415896569458[149] = 0; + out_638456415896569458[150] = 0; + out_638456415896569458[151] = 0; + out_638456415896569458[152] = 1; + out_638456415896569458[153] = 0; + out_638456415896569458[154] = 0; + out_638456415896569458[155] = 0; + out_638456415896569458[156] = 0; + out_638456415896569458[157] = 0; + out_638456415896569458[158] = 0; + out_638456415896569458[159] = 0; + out_638456415896569458[160] = 0; + out_638456415896569458[161] = 0; + out_638456415896569458[162] = 0; + out_638456415896569458[163] = 0; + out_638456415896569458[164] = 0; + out_638456415896569458[165] = 0; + out_638456415896569458[166] = 0; + out_638456415896569458[167] = 0; + out_638456415896569458[168] = 0; + out_638456415896569458[169] = 0; + out_638456415896569458[170] = 0; + out_638456415896569458[171] = 1; + out_638456415896569458[172] = 0; + out_638456415896569458[173] = 0; + out_638456415896569458[174] = 0; + out_638456415896569458[175] = 0; + out_638456415896569458[176] = 0; + out_638456415896569458[177] = 0; + out_638456415896569458[178] = 0; + out_638456415896569458[179] = 0; + out_638456415896569458[180] = 0; + out_638456415896569458[181] = 0; + out_638456415896569458[182] = 0; + out_638456415896569458[183] = 0; + out_638456415896569458[184] = 0; + out_638456415896569458[185] = 0; + out_638456415896569458[186] = 0; + out_638456415896569458[187] = 0; + out_638456415896569458[188] = 0; + out_638456415896569458[189] = 0; + out_638456415896569458[190] = 1; + out_638456415896569458[191] = 0; + out_638456415896569458[192] = 0; + out_638456415896569458[193] = 0; + out_638456415896569458[194] = 0; + out_638456415896569458[195] = 0; + out_638456415896569458[196] = 0; + out_638456415896569458[197] = 0; + out_638456415896569458[198] = 0; + out_638456415896569458[199] = 0; + out_638456415896569458[200] = 0; + out_638456415896569458[201] = 0; + out_638456415896569458[202] = 0; + out_638456415896569458[203] = 0; + out_638456415896569458[204] = 0; + out_638456415896569458[205] = 0; + out_638456415896569458[206] = 0; + out_638456415896569458[207] = 0; + out_638456415896569458[208] = 0; + out_638456415896569458[209] = 1; + out_638456415896569458[210] = 0; + out_638456415896569458[211] = 0; + out_638456415896569458[212] = 0; + out_638456415896569458[213] = 0; + out_638456415896569458[214] = 0; + out_638456415896569458[215] = 0; + out_638456415896569458[216] = 0; + out_638456415896569458[217] = 0; + out_638456415896569458[218] = 0; + out_638456415896569458[219] = 0; + out_638456415896569458[220] = 0; + out_638456415896569458[221] = 0; + out_638456415896569458[222] = 0; + out_638456415896569458[223] = 0; + out_638456415896569458[224] = 0; + out_638456415896569458[225] = 0; + out_638456415896569458[226] = 0; + out_638456415896569458[227] = 0; + out_638456415896569458[228] = 1; + out_638456415896569458[229] = 0; + out_638456415896569458[230] = 0; + out_638456415896569458[231] = 0; + out_638456415896569458[232] = 0; + out_638456415896569458[233] = 0; + out_638456415896569458[234] = 0; + out_638456415896569458[235] = 0; + out_638456415896569458[236] = 0; + out_638456415896569458[237] = 0; + out_638456415896569458[238] = 0; + out_638456415896569458[239] = 0; + out_638456415896569458[240] = 0; + out_638456415896569458[241] = 0; + out_638456415896569458[242] = 0; + out_638456415896569458[243] = 0; + out_638456415896569458[244] = 0; + out_638456415896569458[245] = 0; + out_638456415896569458[246] = 0; + out_638456415896569458[247] = 1; + out_638456415896569458[248] = 0; + out_638456415896569458[249] = 0; + out_638456415896569458[250] = 0; + out_638456415896569458[251] = 0; + out_638456415896569458[252] = 0; + out_638456415896569458[253] = 0; + out_638456415896569458[254] = 0; + out_638456415896569458[255] = 0; + out_638456415896569458[256] = 0; + out_638456415896569458[257] = 0; + out_638456415896569458[258] = 0; + out_638456415896569458[259] = 0; + out_638456415896569458[260] = 0; + out_638456415896569458[261] = 0; + out_638456415896569458[262] = 0; + out_638456415896569458[263] = 0; + out_638456415896569458[264] = 0; + out_638456415896569458[265] = 0; + out_638456415896569458[266] = 1; + out_638456415896569458[267] = 0; + out_638456415896569458[268] = 0; + out_638456415896569458[269] = 0; + out_638456415896569458[270] = 0; + out_638456415896569458[271] = 0; + out_638456415896569458[272] = 0; + out_638456415896569458[273] = 0; + out_638456415896569458[274] = 0; + out_638456415896569458[275] = 0; + out_638456415896569458[276] = 0; + out_638456415896569458[277] = 0; + out_638456415896569458[278] = 0; + out_638456415896569458[279] = 0; + out_638456415896569458[280] = 0; + out_638456415896569458[281] = 0; + out_638456415896569458[282] = 0; + out_638456415896569458[283] = 0; + out_638456415896569458[284] = 0; + out_638456415896569458[285] = 1; + out_638456415896569458[286] = 0; + out_638456415896569458[287] = 0; + out_638456415896569458[288] = 0; + out_638456415896569458[289] = 0; + out_638456415896569458[290] = 0; + out_638456415896569458[291] = 0; + out_638456415896569458[292] = 0; + out_638456415896569458[293] = 0; + out_638456415896569458[294] = 0; + out_638456415896569458[295] = 0; + out_638456415896569458[296] = 0; + out_638456415896569458[297] = 0; + out_638456415896569458[298] = 0; + out_638456415896569458[299] = 0; + out_638456415896569458[300] = 0; + out_638456415896569458[301] = 0; + out_638456415896569458[302] = 0; + out_638456415896569458[303] = 0; + out_638456415896569458[304] = 1; + out_638456415896569458[305] = 0; + out_638456415896569458[306] = 0; + out_638456415896569458[307] = 0; + out_638456415896569458[308] = 0; + out_638456415896569458[309] = 0; + out_638456415896569458[310] = 0; + out_638456415896569458[311] = 0; + out_638456415896569458[312] = 0; + out_638456415896569458[313] = 0; + out_638456415896569458[314] = 0; + out_638456415896569458[315] = 0; + out_638456415896569458[316] = 0; + out_638456415896569458[317] = 0; + out_638456415896569458[318] = 0; + out_638456415896569458[319] = 0; + out_638456415896569458[320] = 0; + out_638456415896569458[321] = 0; + out_638456415896569458[322] = 0; + out_638456415896569458[323] = 1; } -void h_4(double *state, double *unused, double *out_6026765849154187968) { - out_6026765849154187968[0] = state[6] + state[9]; - out_6026765849154187968[1] = state[7] + state[10]; - out_6026765849154187968[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_3715383509872023288) { + out_3715383509872023288[0] = state[6] + state[9]; + out_3715383509872023288[1] = state[7] + state[10]; + out_3715383509872023288[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_7102224835231120360) { - out_7102224835231120360[0] = 0; - out_7102224835231120360[1] = 0; - out_7102224835231120360[2] = 0; - out_7102224835231120360[3] = 0; - out_7102224835231120360[4] = 0; - out_7102224835231120360[5] = 0; - out_7102224835231120360[6] = 1; - out_7102224835231120360[7] = 0; - out_7102224835231120360[8] = 0; - out_7102224835231120360[9] = 1; - out_7102224835231120360[10] = 0; - out_7102224835231120360[11] = 0; - out_7102224835231120360[12] = 0; - out_7102224835231120360[13] = 0; - out_7102224835231120360[14] = 0; - out_7102224835231120360[15] = 0; - out_7102224835231120360[16] = 0; - out_7102224835231120360[17] = 0; - out_7102224835231120360[18] = 0; - out_7102224835231120360[19] = 0; - out_7102224835231120360[20] = 0; - out_7102224835231120360[21] = 0; - out_7102224835231120360[22] = 0; - out_7102224835231120360[23] = 0; - out_7102224835231120360[24] = 0; - out_7102224835231120360[25] = 1; - out_7102224835231120360[26] = 0; - out_7102224835231120360[27] = 0; - out_7102224835231120360[28] = 1; - out_7102224835231120360[29] = 0; - out_7102224835231120360[30] = 0; - out_7102224835231120360[31] = 0; - out_7102224835231120360[32] = 0; - out_7102224835231120360[33] = 0; - out_7102224835231120360[34] = 0; - out_7102224835231120360[35] = 0; - out_7102224835231120360[36] = 0; - out_7102224835231120360[37] = 0; - out_7102224835231120360[38] = 0; - out_7102224835231120360[39] = 0; - out_7102224835231120360[40] = 0; - out_7102224835231120360[41] = 0; - out_7102224835231120360[42] = 0; - out_7102224835231120360[43] = 0; - out_7102224835231120360[44] = 1; - out_7102224835231120360[45] = 0; - out_7102224835231120360[46] = 0; - out_7102224835231120360[47] = 1; - out_7102224835231120360[48] = 0; - out_7102224835231120360[49] = 0; - out_7102224835231120360[50] = 0; - out_7102224835231120360[51] = 0; - out_7102224835231120360[52] = 0; - out_7102224835231120360[53] = 0; +void H_4(double *state, double *unused, double *out_7859722240455419223) { + out_7859722240455419223[0] = 0; + out_7859722240455419223[1] = 0; + out_7859722240455419223[2] = 0; + out_7859722240455419223[3] = 0; + out_7859722240455419223[4] = 0; + out_7859722240455419223[5] = 0; + out_7859722240455419223[6] = 1; + out_7859722240455419223[7] = 0; + out_7859722240455419223[8] = 0; + out_7859722240455419223[9] = 1; + out_7859722240455419223[10] = 0; + out_7859722240455419223[11] = 0; + out_7859722240455419223[12] = 0; + out_7859722240455419223[13] = 0; + out_7859722240455419223[14] = 0; + out_7859722240455419223[15] = 0; + out_7859722240455419223[16] = 0; + out_7859722240455419223[17] = 0; + out_7859722240455419223[18] = 0; + out_7859722240455419223[19] = 0; + out_7859722240455419223[20] = 0; + out_7859722240455419223[21] = 0; + out_7859722240455419223[22] = 0; + out_7859722240455419223[23] = 0; + out_7859722240455419223[24] = 0; + out_7859722240455419223[25] = 1; + out_7859722240455419223[26] = 0; + out_7859722240455419223[27] = 0; + out_7859722240455419223[28] = 1; + out_7859722240455419223[29] = 0; + out_7859722240455419223[30] = 0; + out_7859722240455419223[31] = 0; + out_7859722240455419223[32] = 0; + out_7859722240455419223[33] = 0; + out_7859722240455419223[34] = 0; + out_7859722240455419223[35] = 0; + out_7859722240455419223[36] = 0; + out_7859722240455419223[37] = 0; + out_7859722240455419223[38] = 0; + out_7859722240455419223[39] = 0; + out_7859722240455419223[40] = 0; + out_7859722240455419223[41] = 0; + out_7859722240455419223[42] = 0; + out_7859722240455419223[43] = 0; + out_7859722240455419223[44] = 1; + out_7859722240455419223[45] = 0; + out_7859722240455419223[46] = 0; + out_7859722240455419223[47] = 1; + out_7859722240455419223[48] = 0; + out_7859722240455419223[49] = 0; + out_7859722240455419223[50] = 0; + out_7859722240455419223[51] = 0; + out_7859722240455419223[52] = 0; + out_7859722240455419223[53] = 0; } -void h_10(double *state, double *unused, double *out_1866086890220189324) { - out_1866086890220189324[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_1866086890220189324[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_1866086890220189324[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_1493881979826544307) { + out_1493881979826544307[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_1493881979826544307[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_1493881979826544307[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_834802170779042492) { - out_834802170779042492[0] = 0; - out_834802170779042492[1] = 9.8100000000000005*cos(state[1]); - out_834802170779042492[2] = 0; - out_834802170779042492[3] = 0; - out_834802170779042492[4] = -state[8]; - out_834802170779042492[5] = state[7]; - out_834802170779042492[6] = 0; - out_834802170779042492[7] = state[5]; - out_834802170779042492[8] = -state[4]; - out_834802170779042492[9] = 0; - out_834802170779042492[10] = 0; - out_834802170779042492[11] = 0; - out_834802170779042492[12] = 1; - out_834802170779042492[13] = 0; - out_834802170779042492[14] = 0; - out_834802170779042492[15] = 1; - out_834802170779042492[16] = 0; - out_834802170779042492[17] = 0; - out_834802170779042492[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_834802170779042492[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_834802170779042492[20] = 0; - out_834802170779042492[21] = state[8]; - out_834802170779042492[22] = 0; - out_834802170779042492[23] = -state[6]; - out_834802170779042492[24] = -state[5]; - out_834802170779042492[25] = 0; - out_834802170779042492[26] = state[3]; - out_834802170779042492[27] = 0; - out_834802170779042492[28] = 0; - out_834802170779042492[29] = 0; - out_834802170779042492[30] = 0; - out_834802170779042492[31] = 1; - out_834802170779042492[32] = 0; - out_834802170779042492[33] = 0; - out_834802170779042492[34] = 1; - out_834802170779042492[35] = 0; - out_834802170779042492[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_834802170779042492[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_834802170779042492[38] = 0; - out_834802170779042492[39] = -state[7]; - out_834802170779042492[40] = state[6]; - out_834802170779042492[41] = 0; - out_834802170779042492[42] = state[4]; - out_834802170779042492[43] = -state[3]; - out_834802170779042492[44] = 0; - out_834802170779042492[45] = 0; - out_834802170779042492[46] = 0; - out_834802170779042492[47] = 0; - out_834802170779042492[48] = 0; - out_834802170779042492[49] = 0; - out_834802170779042492[50] = 1; - out_834802170779042492[51] = 0; - out_834802170779042492[52] = 0; - out_834802170779042492[53] = 1; +void H_10(double *state, double *unused, double *out_8226181140571901267) { + out_8226181140571901267[0] = 0; + out_8226181140571901267[1] = 9.8100000000000005*cos(state[1]); + out_8226181140571901267[2] = 0; + out_8226181140571901267[3] = 0; + out_8226181140571901267[4] = -state[8]; + out_8226181140571901267[5] = state[7]; + out_8226181140571901267[6] = 0; + out_8226181140571901267[7] = state[5]; + out_8226181140571901267[8] = -state[4]; + out_8226181140571901267[9] = 0; + out_8226181140571901267[10] = 0; + out_8226181140571901267[11] = 0; + out_8226181140571901267[12] = 1; + out_8226181140571901267[13] = 0; + out_8226181140571901267[14] = 0; + out_8226181140571901267[15] = 1; + out_8226181140571901267[16] = 0; + out_8226181140571901267[17] = 0; + out_8226181140571901267[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_8226181140571901267[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_8226181140571901267[20] = 0; + out_8226181140571901267[21] = state[8]; + out_8226181140571901267[22] = 0; + out_8226181140571901267[23] = -state[6]; + out_8226181140571901267[24] = -state[5]; + out_8226181140571901267[25] = 0; + out_8226181140571901267[26] = state[3]; + out_8226181140571901267[27] = 0; + out_8226181140571901267[28] = 0; + out_8226181140571901267[29] = 0; + out_8226181140571901267[30] = 0; + out_8226181140571901267[31] = 1; + out_8226181140571901267[32] = 0; + out_8226181140571901267[33] = 0; + out_8226181140571901267[34] = 1; + out_8226181140571901267[35] = 0; + out_8226181140571901267[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_8226181140571901267[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_8226181140571901267[38] = 0; + out_8226181140571901267[39] = -state[7]; + out_8226181140571901267[40] = state[6]; + out_8226181140571901267[41] = 0; + out_8226181140571901267[42] = state[4]; + out_8226181140571901267[43] = -state[3]; + out_8226181140571901267[44] = 0; + out_8226181140571901267[45] = 0; + out_8226181140571901267[46] = 0; + out_8226181140571901267[47] = 0; + out_8226181140571901267[48] = 0; + out_8226181140571901267[49] = 0; + out_8226181140571901267[50] = 1; + out_8226181140571901267[51] = 0; + out_8226181140571901267[52] = 0; + out_8226181140571901267[53] = 1; } -void h_13(double *state, double *unused, double *out_4520826989648677276) { - out_4520826989648677276[0] = state[3]; - out_4520826989648677276[1] = state[4]; - out_4520826989648677276[2] = state[5]; +void h_13(double *state, double *unused, double *out_820596947016455055) { + out_820596947016455055[0] = state[3]; + out_820596947016455055[1] = state[4]; + out_820596947016455055[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3889951009898787559) { - out_3889951009898787559[0] = 0; - out_3889951009898787559[1] = 0; - out_3889951009898787559[2] = 0; - out_3889951009898787559[3] = 1; - out_3889951009898787559[4] = 0; - out_3889951009898787559[5] = 0; - out_3889951009898787559[6] = 0; - out_3889951009898787559[7] = 0; - out_3889951009898787559[8] = 0; - out_3889951009898787559[9] = 0; - out_3889951009898787559[10] = 0; - out_3889951009898787559[11] = 0; - out_3889951009898787559[12] = 0; - out_3889951009898787559[13] = 0; - out_3889951009898787559[14] = 0; - out_3889951009898787559[15] = 0; - out_3889951009898787559[16] = 0; - out_3889951009898787559[17] = 0; - out_3889951009898787559[18] = 0; - out_3889951009898787559[19] = 0; - out_3889951009898787559[20] = 0; - out_3889951009898787559[21] = 0; - out_3889951009898787559[22] = 1; - out_3889951009898787559[23] = 0; - out_3889951009898787559[24] = 0; - out_3889951009898787559[25] = 0; - out_3889951009898787559[26] = 0; - out_3889951009898787559[27] = 0; - out_3889951009898787559[28] = 0; - out_3889951009898787559[29] = 0; - out_3889951009898787559[30] = 0; - out_3889951009898787559[31] = 0; - out_3889951009898787559[32] = 0; - out_3889951009898787559[33] = 0; - out_3889951009898787559[34] = 0; - out_3889951009898787559[35] = 0; - out_3889951009898787559[36] = 0; - out_3889951009898787559[37] = 0; - out_3889951009898787559[38] = 0; - out_3889951009898787559[39] = 0; - out_3889951009898787559[40] = 0; - out_3889951009898787559[41] = 1; - out_3889951009898787559[42] = 0; - out_3889951009898787559[43] = 0; - out_3889951009898787559[44] = 0; - out_3889951009898787559[45] = 0; - out_3889951009898787559[46] = 0; - out_3889951009898787559[47] = 0; - out_3889951009898787559[48] = 0; - out_3889951009898787559[49] = 0; - out_3889951009898787559[50] = 0; - out_3889951009898787559[51] = 0; - out_3889951009898787559[52] = 0; - out_3889951009898787559[53] = 0; +void H_13(double *state, double *unused, double *out_4647448415123086422) { + out_4647448415123086422[0] = 0; + out_4647448415123086422[1] = 0; + out_4647448415123086422[2] = 0; + out_4647448415123086422[3] = 1; + out_4647448415123086422[4] = 0; + out_4647448415123086422[5] = 0; + out_4647448415123086422[6] = 0; + out_4647448415123086422[7] = 0; + out_4647448415123086422[8] = 0; + out_4647448415123086422[9] = 0; + out_4647448415123086422[10] = 0; + out_4647448415123086422[11] = 0; + out_4647448415123086422[12] = 0; + out_4647448415123086422[13] = 0; + out_4647448415123086422[14] = 0; + out_4647448415123086422[15] = 0; + out_4647448415123086422[16] = 0; + out_4647448415123086422[17] = 0; + out_4647448415123086422[18] = 0; + out_4647448415123086422[19] = 0; + out_4647448415123086422[20] = 0; + out_4647448415123086422[21] = 0; + out_4647448415123086422[22] = 1; + out_4647448415123086422[23] = 0; + out_4647448415123086422[24] = 0; + out_4647448415123086422[25] = 0; + out_4647448415123086422[26] = 0; + out_4647448415123086422[27] = 0; + out_4647448415123086422[28] = 0; + out_4647448415123086422[29] = 0; + out_4647448415123086422[30] = 0; + out_4647448415123086422[31] = 0; + out_4647448415123086422[32] = 0; + out_4647448415123086422[33] = 0; + out_4647448415123086422[34] = 0; + out_4647448415123086422[35] = 0; + out_4647448415123086422[36] = 0; + out_4647448415123086422[37] = 0; + out_4647448415123086422[38] = 0; + out_4647448415123086422[39] = 0; + out_4647448415123086422[40] = 0; + out_4647448415123086422[41] = 1; + out_4647448415123086422[42] = 0; + out_4647448415123086422[43] = 0; + out_4647448415123086422[44] = 0; + out_4647448415123086422[45] = 0; + out_4647448415123086422[46] = 0; + out_4647448415123086422[47] = 0; + out_4647448415123086422[48] = 0; + out_4647448415123086422[49] = 0; + out_4647448415123086422[50] = 0; + out_4647448415123086422[51] = 0; + out_4647448415123086422[52] = 0; + out_4647448415123086422[53] = 0; } -void h_14(double *state, double *unused, double *out_733646167343188736) { - out_733646167343188736[0] = state[6]; - out_733646167343188736[1] = state[7]; - out_733646167343188736[2] = state[8]; +void h_14(double *state, double *unused, double *out_7488710011373447915) { + out_7488710011373447915[0] = state[6]; + out_7488710011373447915[1] = state[7]; + out_7488710011373447915[2] = state[8]; } -void H_14(double *state, double *unused, double *out_8261730806183058960) { - out_8261730806183058960[0] = 0; - out_8261730806183058960[1] = 0; - out_8261730806183058960[2] = 0; - out_8261730806183058960[3] = 0; - out_8261730806183058960[4] = 0; - out_8261730806183058960[5] = 0; - out_8261730806183058960[6] = 1; - out_8261730806183058960[7] = 0; - out_8261730806183058960[8] = 0; - out_8261730806183058960[9] = 0; - out_8261730806183058960[10] = 0; - out_8261730806183058960[11] = 0; - out_8261730806183058960[12] = 0; - out_8261730806183058960[13] = 0; - out_8261730806183058960[14] = 0; - out_8261730806183058960[15] = 0; - out_8261730806183058960[16] = 0; - out_8261730806183058960[17] = 0; - out_8261730806183058960[18] = 0; - out_8261730806183058960[19] = 0; - out_8261730806183058960[20] = 0; - out_8261730806183058960[21] = 0; - out_8261730806183058960[22] = 0; - out_8261730806183058960[23] = 0; - out_8261730806183058960[24] = 0; - out_8261730806183058960[25] = 1; - out_8261730806183058960[26] = 0; - out_8261730806183058960[27] = 0; - out_8261730806183058960[28] = 0; - out_8261730806183058960[29] = 0; - out_8261730806183058960[30] = 0; - out_8261730806183058960[31] = 0; - out_8261730806183058960[32] = 0; - out_8261730806183058960[33] = 0; - out_8261730806183058960[34] = 0; - out_8261730806183058960[35] = 0; - out_8261730806183058960[36] = 0; - out_8261730806183058960[37] = 0; - out_8261730806183058960[38] = 0; - out_8261730806183058960[39] = 0; - out_8261730806183058960[40] = 0; - out_8261730806183058960[41] = 0; - out_8261730806183058960[42] = 0; - out_8261730806183058960[43] = 0; - out_8261730806183058960[44] = 1; - out_8261730806183058960[45] = 0; - out_8261730806183058960[46] = 0; - out_8261730806183058960[47] = 0; - out_8261730806183058960[48] = 0; - out_8261730806183058960[49] = 0; - out_8261730806183058960[50] = 0; - out_8261730806183058960[51] = 0; - out_8261730806183058960[52] = 0; - out_8261730806183058960[53] = 0; +void H_14(double *state, double *unused, double *out_3896481384115934694) { + out_3896481384115934694[0] = 0; + out_3896481384115934694[1] = 0; + out_3896481384115934694[2] = 0; + out_3896481384115934694[3] = 0; + out_3896481384115934694[4] = 0; + out_3896481384115934694[5] = 0; + out_3896481384115934694[6] = 1; + out_3896481384115934694[7] = 0; + out_3896481384115934694[8] = 0; + out_3896481384115934694[9] = 0; + out_3896481384115934694[10] = 0; + out_3896481384115934694[11] = 0; + out_3896481384115934694[12] = 0; + out_3896481384115934694[13] = 0; + out_3896481384115934694[14] = 0; + out_3896481384115934694[15] = 0; + out_3896481384115934694[16] = 0; + out_3896481384115934694[17] = 0; + out_3896481384115934694[18] = 0; + out_3896481384115934694[19] = 0; + out_3896481384115934694[20] = 0; + out_3896481384115934694[21] = 0; + out_3896481384115934694[22] = 0; + out_3896481384115934694[23] = 0; + out_3896481384115934694[24] = 0; + out_3896481384115934694[25] = 1; + out_3896481384115934694[26] = 0; + out_3896481384115934694[27] = 0; + out_3896481384115934694[28] = 0; + out_3896481384115934694[29] = 0; + out_3896481384115934694[30] = 0; + out_3896481384115934694[31] = 0; + out_3896481384115934694[32] = 0; + out_3896481384115934694[33] = 0; + out_3896481384115934694[34] = 0; + out_3896481384115934694[35] = 0; + out_3896481384115934694[36] = 0; + out_3896481384115934694[37] = 0; + out_3896481384115934694[38] = 0; + out_3896481384115934694[39] = 0; + out_3896481384115934694[40] = 0; + out_3896481384115934694[41] = 0; + out_3896481384115934694[42] = 0; + out_3896481384115934694[43] = 0; + out_3896481384115934694[44] = 1; + out_3896481384115934694[45] = 0; + out_3896481384115934694[46] = 0; + out_3896481384115934694[47] = 0; + out_3896481384115934694[48] = 0; + out_3896481384115934694[49] = 0; + out_3896481384115934694[50] = 0; + out_3896481384115934694[51] = 0; + out_3896481384115934694[52] = 0; + out_3896481384115934694[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_3341780452292505997) { - err_fun(nom_x, delta_x, out_3341780452292505997); +void pose_err_fun(double *nom_x, double *delta_x, double *out_3650099251769799718) { + err_fun(nom_x, delta_x, out_3650099251769799718); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5409150910353518766) { - inv_err_fun(nom_x, true_x, out_5409150910353518766); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4549899801136771746) { + inv_err_fun(nom_x, true_x, out_4549899801136771746); } -void pose_H_mod_fun(double *state, double *out_8300172317114018464) { - H_mod_fun(state, out_8300172317114018464); +void pose_H_mod_fun(double *state, double *out_6815214623410224821) { + H_mod_fun(state, out_6815214623410224821); } -void pose_f_fun(double *state, double dt, double *out_4063489601932878054) { - f_fun(state, dt, out_4063489601932878054); +void pose_f_fun(double *state, double dt, double *out_4724171785691844407) { + f_fun(state, dt, out_4724171785691844407); } -void pose_F_fun(double *state, double dt, double *out_493800117352810078) { - F_fun(state, dt, out_493800117352810078); +void pose_F_fun(double *state, double dt, double *out_638456415896569458) { + F_fun(state, dt, out_638456415896569458); } -void pose_h_4(double *state, double *unused, double *out_6026765849154187968) { - h_4(state, unused, out_6026765849154187968); +void pose_h_4(double *state, double *unused, double *out_3715383509872023288) { + h_4(state, unused, out_3715383509872023288); } -void pose_H_4(double *state, double *unused, double *out_7102224835231120360) { - H_4(state, unused, out_7102224835231120360); +void pose_H_4(double *state, double *unused, double *out_7859722240455419223) { + H_4(state, unused, out_7859722240455419223); } -void pose_h_10(double *state, double *unused, double *out_1866086890220189324) { - h_10(state, unused, out_1866086890220189324); +void pose_h_10(double *state, double *unused, double *out_1493881979826544307) { + h_10(state, unused, out_1493881979826544307); } -void pose_H_10(double *state, double *unused, double *out_834802170779042492) { - H_10(state, unused, out_834802170779042492); +void pose_H_10(double *state, double *unused, double *out_8226181140571901267) { + H_10(state, unused, out_8226181140571901267); } -void pose_h_13(double *state, double *unused, double *out_4520826989648677276) { - h_13(state, unused, out_4520826989648677276); +void pose_h_13(double *state, double *unused, double *out_820596947016455055) { + h_13(state, unused, out_820596947016455055); } -void pose_H_13(double *state, double *unused, double *out_3889951009898787559) { - H_13(state, unused, out_3889951009898787559); +void pose_H_13(double *state, double *unused, double *out_4647448415123086422) { + H_13(state, unused, out_4647448415123086422); } -void pose_h_14(double *state, double *unused, double *out_733646167343188736) { - h_14(state, unused, out_733646167343188736); +void pose_h_14(double *state, double *unused, double *out_7488710011373447915) { + h_14(state, unused, out_7488710011373447915); } -void pose_H_14(double *state, double *unused, double *out_8261730806183058960) { - H_14(state, unused, out_8261730806183058960); +void pose_H_14(double *state, double *unused, double *out_3896481384115934694) { + H_14(state, unused, out_3896481384115934694); } 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 68e915733..d782dc726 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_3341780452292505997); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5409150910353518766); -void pose_H_mod_fun(double *state, double *out_8300172317114018464); -void pose_f_fun(double *state, double dt, double *out_4063489601932878054); -void pose_F_fun(double *state, double dt, double *out_493800117352810078); -void pose_h_4(double *state, double *unused, double *out_6026765849154187968); -void pose_H_4(double *state, double *unused, double *out_7102224835231120360); -void pose_h_10(double *state, double *unused, double *out_1866086890220189324); -void pose_H_10(double *state, double *unused, double *out_834802170779042492); -void pose_h_13(double *state, double *unused, double *out_4520826989648677276); -void pose_H_13(double *state, double *unused, double *out_3889951009898787559); -void pose_h_14(double *state, double *unused, double *out_733646167343188736); -void pose_H_14(double *state, double *unused, double *out_8261730806183058960); +void pose_err_fun(double *nom_x, double *delta_x, double *out_3650099251769799718); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4549899801136771746); +void pose_H_mod_fun(double *state, double *out_6815214623410224821); +void pose_f_fun(double *state, double dt, double *out_4724171785691844407); +void pose_F_fun(double *state, double dt, double *out_638456415896569458); +void pose_h_4(double *state, double *unused, double *out_3715383509872023288); +void pose_H_4(double *state, double *unused, double *out_7859722240455419223); +void pose_h_10(double *state, double *unused, double *out_1493881979826544307); +void pose_H_10(double *state, double *unused, double *out_8226181140571901267); +void pose_h_13(double *state, double *unused, double *out_820596947016455055); +void pose_H_13(double *state, double *unused, double *out_4647448415123086422); +void pose_h_14(double *state, double *unused, double *out_7488710011373447915); +void pose_H_14(double *state, double *unused, double *out_3896481384115934694); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 6fd78681f..05d7ccaf2 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index c1ad733d5..b870a7090 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index c735d4582..118f0da00 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 1ad7a3639..8a0b327a3 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 811670dca..7858d22a5 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 883a3232d..a7c7dcb88 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ