diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index e46cd9a3b..b493c1eca 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 2762a67b7..16c1868c0 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 b43d84885..bc4f0d41a 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 4715f915d..cb732aa90 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 3bb9d92ac..ccbe38b5a 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 14939783f..799bc7b9e 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 be61312e2..a4f9ba342 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 be61312e2..a4f9ba342 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 dff953b9b..3f45109c8 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 14939783f..799bc7b9e 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 e186e7736..0f1f53d8e 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-a9771a0e-DEBUG"; +const uint8_t gitversion[19] = "DEV-e6b12d61-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 880543179..c24490517 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 43632a104..39ebe78ac 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 490cb2045..613803b58 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 c858d9d97..4f776d239 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 4ee8c1796..07ba2df56 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 575854986..fa10e66f9 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 13d6d2faf..528ae3b04 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 e1c247ae4..650f88fe2 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 b29519284..a2cf85c6f 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 f9bfe7429..2b92a9c91 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 c2fe89189..264c03f5a 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 f3c19b60e..86eea1295 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 26df0b853..04d94d08e 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 e289febe0..4fe13ab3f 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 1bd5de38d..79479a4ad 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 524135ced..1ecd44010 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 a0101bf95..3d39c0201 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 176806680..7888a1027 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 bd2c8f062..6ecc37606 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 411147735..d8b9e22db 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 0b1094dd8..67cb0e676 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-a9771a0e-DEBUG \ No newline at end of file +DEV-e6b12d61-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 97f590038..31838b60f 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_7709214462285076644) { - out_7709214462285076644[0] = delta_x[0] + nom_x[0]; - out_7709214462285076644[1] = delta_x[1] + nom_x[1]; - out_7709214462285076644[2] = delta_x[2] + nom_x[2]; - out_7709214462285076644[3] = delta_x[3] + nom_x[3]; - out_7709214462285076644[4] = delta_x[4] + nom_x[4]; - out_7709214462285076644[5] = delta_x[5] + nom_x[5]; - out_7709214462285076644[6] = delta_x[6] + nom_x[6]; - out_7709214462285076644[7] = delta_x[7] + nom_x[7]; - out_7709214462285076644[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_2605865889076671127) { + out_2605865889076671127[0] = delta_x[0] + nom_x[0]; + out_2605865889076671127[1] = delta_x[1] + nom_x[1]; + out_2605865889076671127[2] = delta_x[2] + nom_x[2]; + out_2605865889076671127[3] = delta_x[3] + nom_x[3]; + out_2605865889076671127[4] = delta_x[4] + nom_x[4]; + out_2605865889076671127[5] = delta_x[5] + nom_x[5]; + out_2605865889076671127[6] = delta_x[6] + nom_x[6]; + out_2605865889076671127[7] = delta_x[7] + nom_x[7]; + out_2605865889076671127[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_181432404984807710) { - out_181432404984807710[0] = -nom_x[0] + true_x[0]; - out_181432404984807710[1] = -nom_x[1] + true_x[1]; - out_181432404984807710[2] = -nom_x[2] + true_x[2]; - out_181432404984807710[3] = -nom_x[3] + true_x[3]; - out_181432404984807710[4] = -nom_x[4] + true_x[4]; - out_181432404984807710[5] = -nom_x[5] + true_x[5]; - out_181432404984807710[6] = -nom_x[6] + true_x[6]; - out_181432404984807710[7] = -nom_x[7] + true_x[7]; - out_181432404984807710[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6092587495462394066) { + out_6092587495462394066[0] = -nom_x[0] + true_x[0]; + out_6092587495462394066[1] = -nom_x[1] + true_x[1]; + out_6092587495462394066[2] = -nom_x[2] + true_x[2]; + out_6092587495462394066[3] = -nom_x[3] + true_x[3]; + out_6092587495462394066[4] = -nom_x[4] + true_x[4]; + out_6092587495462394066[5] = -nom_x[5] + true_x[5]; + out_6092587495462394066[6] = -nom_x[6] + true_x[6]; + out_6092587495462394066[7] = -nom_x[7] + true_x[7]; + out_6092587495462394066[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_8282876149494425233) { - out_8282876149494425233[0] = 1.0; - out_8282876149494425233[1] = 0.0; - out_8282876149494425233[2] = 0.0; - out_8282876149494425233[3] = 0.0; - out_8282876149494425233[4] = 0.0; - out_8282876149494425233[5] = 0.0; - out_8282876149494425233[6] = 0.0; - out_8282876149494425233[7] = 0.0; - out_8282876149494425233[8] = 0.0; - out_8282876149494425233[9] = 0.0; - out_8282876149494425233[10] = 1.0; - out_8282876149494425233[11] = 0.0; - out_8282876149494425233[12] = 0.0; - out_8282876149494425233[13] = 0.0; - out_8282876149494425233[14] = 0.0; - out_8282876149494425233[15] = 0.0; - out_8282876149494425233[16] = 0.0; - out_8282876149494425233[17] = 0.0; - out_8282876149494425233[18] = 0.0; - out_8282876149494425233[19] = 0.0; - out_8282876149494425233[20] = 1.0; - out_8282876149494425233[21] = 0.0; - out_8282876149494425233[22] = 0.0; - out_8282876149494425233[23] = 0.0; - out_8282876149494425233[24] = 0.0; - out_8282876149494425233[25] = 0.0; - out_8282876149494425233[26] = 0.0; - out_8282876149494425233[27] = 0.0; - out_8282876149494425233[28] = 0.0; - out_8282876149494425233[29] = 0.0; - out_8282876149494425233[30] = 1.0; - out_8282876149494425233[31] = 0.0; - out_8282876149494425233[32] = 0.0; - out_8282876149494425233[33] = 0.0; - out_8282876149494425233[34] = 0.0; - out_8282876149494425233[35] = 0.0; - out_8282876149494425233[36] = 0.0; - out_8282876149494425233[37] = 0.0; - out_8282876149494425233[38] = 0.0; - out_8282876149494425233[39] = 0.0; - out_8282876149494425233[40] = 1.0; - out_8282876149494425233[41] = 0.0; - out_8282876149494425233[42] = 0.0; - out_8282876149494425233[43] = 0.0; - out_8282876149494425233[44] = 0.0; - out_8282876149494425233[45] = 0.0; - out_8282876149494425233[46] = 0.0; - out_8282876149494425233[47] = 0.0; - out_8282876149494425233[48] = 0.0; - out_8282876149494425233[49] = 0.0; - out_8282876149494425233[50] = 1.0; - out_8282876149494425233[51] = 0.0; - out_8282876149494425233[52] = 0.0; - out_8282876149494425233[53] = 0.0; - out_8282876149494425233[54] = 0.0; - out_8282876149494425233[55] = 0.0; - out_8282876149494425233[56] = 0.0; - out_8282876149494425233[57] = 0.0; - out_8282876149494425233[58] = 0.0; - out_8282876149494425233[59] = 0.0; - out_8282876149494425233[60] = 1.0; - out_8282876149494425233[61] = 0.0; - out_8282876149494425233[62] = 0.0; - out_8282876149494425233[63] = 0.0; - out_8282876149494425233[64] = 0.0; - out_8282876149494425233[65] = 0.0; - out_8282876149494425233[66] = 0.0; - out_8282876149494425233[67] = 0.0; - out_8282876149494425233[68] = 0.0; - out_8282876149494425233[69] = 0.0; - out_8282876149494425233[70] = 1.0; - out_8282876149494425233[71] = 0.0; - out_8282876149494425233[72] = 0.0; - out_8282876149494425233[73] = 0.0; - out_8282876149494425233[74] = 0.0; - out_8282876149494425233[75] = 0.0; - out_8282876149494425233[76] = 0.0; - out_8282876149494425233[77] = 0.0; - out_8282876149494425233[78] = 0.0; - out_8282876149494425233[79] = 0.0; - out_8282876149494425233[80] = 1.0; +void H_mod_fun(double *state, double *out_5416567318777544442) { + out_5416567318777544442[0] = 1.0; + out_5416567318777544442[1] = 0.0; + out_5416567318777544442[2] = 0.0; + out_5416567318777544442[3] = 0.0; + out_5416567318777544442[4] = 0.0; + out_5416567318777544442[5] = 0.0; + out_5416567318777544442[6] = 0.0; + out_5416567318777544442[7] = 0.0; + out_5416567318777544442[8] = 0.0; + out_5416567318777544442[9] = 0.0; + out_5416567318777544442[10] = 1.0; + out_5416567318777544442[11] = 0.0; + out_5416567318777544442[12] = 0.0; + out_5416567318777544442[13] = 0.0; + out_5416567318777544442[14] = 0.0; + out_5416567318777544442[15] = 0.0; + out_5416567318777544442[16] = 0.0; + out_5416567318777544442[17] = 0.0; + out_5416567318777544442[18] = 0.0; + out_5416567318777544442[19] = 0.0; + out_5416567318777544442[20] = 1.0; + out_5416567318777544442[21] = 0.0; + out_5416567318777544442[22] = 0.0; + out_5416567318777544442[23] = 0.0; + out_5416567318777544442[24] = 0.0; + out_5416567318777544442[25] = 0.0; + out_5416567318777544442[26] = 0.0; + out_5416567318777544442[27] = 0.0; + out_5416567318777544442[28] = 0.0; + out_5416567318777544442[29] = 0.0; + out_5416567318777544442[30] = 1.0; + out_5416567318777544442[31] = 0.0; + out_5416567318777544442[32] = 0.0; + out_5416567318777544442[33] = 0.0; + out_5416567318777544442[34] = 0.0; + out_5416567318777544442[35] = 0.0; + out_5416567318777544442[36] = 0.0; + out_5416567318777544442[37] = 0.0; + out_5416567318777544442[38] = 0.0; + out_5416567318777544442[39] = 0.0; + out_5416567318777544442[40] = 1.0; + out_5416567318777544442[41] = 0.0; + out_5416567318777544442[42] = 0.0; + out_5416567318777544442[43] = 0.0; + out_5416567318777544442[44] = 0.0; + out_5416567318777544442[45] = 0.0; + out_5416567318777544442[46] = 0.0; + out_5416567318777544442[47] = 0.0; + out_5416567318777544442[48] = 0.0; + out_5416567318777544442[49] = 0.0; + out_5416567318777544442[50] = 1.0; + out_5416567318777544442[51] = 0.0; + out_5416567318777544442[52] = 0.0; + out_5416567318777544442[53] = 0.0; + out_5416567318777544442[54] = 0.0; + out_5416567318777544442[55] = 0.0; + out_5416567318777544442[56] = 0.0; + out_5416567318777544442[57] = 0.0; + out_5416567318777544442[58] = 0.0; + out_5416567318777544442[59] = 0.0; + out_5416567318777544442[60] = 1.0; + out_5416567318777544442[61] = 0.0; + out_5416567318777544442[62] = 0.0; + out_5416567318777544442[63] = 0.0; + out_5416567318777544442[64] = 0.0; + out_5416567318777544442[65] = 0.0; + out_5416567318777544442[66] = 0.0; + out_5416567318777544442[67] = 0.0; + out_5416567318777544442[68] = 0.0; + out_5416567318777544442[69] = 0.0; + out_5416567318777544442[70] = 1.0; + out_5416567318777544442[71] = 0.0; + out_5416567318777544442[72] = 0.0; + out_5416567318777544442[73] = 0.0; + out_5416567318777544442[74] = 0.0; + out_5416567318777544442[75] = 0.0; + out_5416567318777544442[76] = 0.0; + out_5416567318777544442[77] = 0.0; + out_5416567318777544442[78] = 0.0; + out_5416567318777544442[79] = 0.0; + out_5416567318777544442[80] = 1.0; } -void f_fun(double *state, double dt, double *out_1683945396559752472) { - out_1683945396559752472[0] = state[0]; - out_1683945396559752472[1] = state[1]; - out_1683945396559752472[2] = state[2]; - out_1683945396559752472[3] = state[3]; - out_1683945396559752472[4] = state[4]; - out_1683945396559752472[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_1683945396559752472[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_1683945396559752472[7] = state[7]; - out_1683945396559752472[8] = state[8]; +void f_fun(double *state, double dt, double *out_8892385645104681966) { + out_8892385645104681966[0] = state[0]; + out_8892385645104681966[1] = state[1]; + out_8892385645104681966[2] = state[2]; + out_8892385645104681966[3] = state[3]; + out_8892385645104681966[4] = state[4]; + out_8892385645104681966[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_8892385645104681966[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_8892385645104681966[7] = state[7]; + out_8892385645104681966[8] = state[8]; } -void F_fun(double *state, double dt, double *out_239932902801011581) { - out_239932902801011581[0] = 1; - out_239932902801011581[1] = 0; - out_239932902801011581[2] = 0; - out_239932902801011581[3] = 0; - out_239932902801011581[4] = 0; - out_239932902801011581[5] = 0; - out_239932902801011581[6] = 0; - out_239932902801011581[7] = 0; - out_239932902801011581[8] = 0; - out_239932902801011581[9] = 0; - out_239932902801011581[10] = 1; - out_239932902801011581[11] = 0; - out_239932902801011581[12] = 0; - out_239932902801011581[13] = 0; - out_239932902801011581[14] = 0; - out_239932902801011581[15] = 0; - out_239932902801011581[16] = 0; - out_239932902801011581[17] = 0; - out_239932902801011581[18] = 0; - out_239932902801011581[19] = 0; - out_239932902801011581[20] = 1; - out_239932902801011581[21] = 0; - out_239932902801011581[22] = 0; - out_239932902801011581[23] = 0; - out_239932902801011581[24] = 0; - out_239932902801011581[25] = 0; - out_239932902801011581[26] = 0; - out_239932902801011581[27] = 0; - out_239932902801011581[28] = 0; - out_239932902801011581[29] = 0; - out_239932902801011581[30] = 1; - out_239932902801011581[31] = 0; - out_239932902801011581[32] = 0; - out_239932902801011581[33] = 0; - out_239932902801011581[34] = 0; - out_239932902801011581[35] = 0; - out_239932902801011581[36] = 0; - out_239932902801011581[37] = 0; - out_239932902801011581[38] = 0; - out_239932902801011581[39] = 0; - out_239932902801011581[40] = 1; - out_239932902801011581[41] = 0; - out_239932902801011581[42] = 0; - out_239932902801011581[43] = 0; - out_239932902801011581[44] = 0; - out_239932902801011581[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_239932902801011581[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_239932902801011581[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_239932902801011581[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_239932902801011581[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_239932902801011581[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_239932902801011581[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_239932902801011581[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_239932902801011581[53] = -9.8100000000000005*dt; - out_239932902801011581[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_239932902801011581[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_239932902801011581[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_239932902801011581[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_239932902801011581[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_239932902801011581[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_239932902801011581[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_239932902801011581[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_239932902801011581[62] = 0; - out_239932902801011581[63] = 0; - out_239932902801011581[64] = 0; - out_239932902801011581[65] = 0; - out_239932902801011581[66] = 0; - out_239932902801011581[67] = 0; - out_239932902801011581[68] = 0; - out_239932902801011581[69] = 0; - out_239932902801011581[70] = 1; - out_239932902801011581[71] = 0; - out_239932902801011581[72] = 0; - out_239932902801011581[73] = 0; - out_239932902801011581[74] = 0; - out_239932902801011581[75] = 0; - out_239932902801011581[76] = 0; - out_239932902801011581[77] = 0; - out_239932902801011581[78] = 0; - out_239932902801011581[79] = 0; - out_239932902801011581[80] = 1; +void F_fun(double *state, double dt, double *out_8140915848168938492) { + out_8140915848168938492[0] = 1; + out_8140915848168938492[1] = 0; + out_8140915848168938492[2] = 0; + out_8140915848168938492[3] = 0; + out_8140915848168938492[4] = 0; + out_8140915848168938492[5] = 0; + out_8140915848168938492[6] = 0; + out_8140915848168938492[7] = 0; + out_8140915848168938492[8] = 0; + out_8140915848168938492[9] = 0; + out_8140915848168938492[10] = 1; + out_8140915848168938492[11] = 0; + out_8140915848168938492[12] = 0; + out_8140915848168938492[13] = 0; + out_8140915848168938492[14] = 0; + out_8140915848168938492[15] = 0; + out_8140915848168938492[16] = 0; + out_8140915848168938492[17] = 0; + out_8140915848168938492[18] = 0; + out_8140915848168938492[19] = 0; + out_8140915848168938492[20] = 1; + out_8140915848168938492[21] = 0; + out_8140915848168938492[22] = 0; + out_8140915848168938492[23] = 0; + out_8140915848168938492[24] = 0; + out_8140915848168938492[25] = 0; + out_8140915848168938492[26] = 0; + out_8140915848168938492[27] = 0; + out_8140915848168938492[28] = 0; + out_8140915848168938492[29] = 0; + out_8140915848168938492[30] = 1; + out_8140915848168938492[31] = 0; + out_8140915848168938492[32] = 0; + out_8140915848168938492[33] = 0; + out_8140915848168938492[34] = 0; + out_8140915848168938492[35] = 0; + out_8140915848168938492[36] = 0; + out_8140915848168938492[37] = 0; + out_8140915848168938492[38] = 0; + out_8140915848168938492[39] = 0; + out_8140915848168938492[40] = 1; + out_8140915848168938492[41] = 0; + out_8140915848168938492[42] = 0; + out_8140915848168938492[43] = 0; + out_8140915848168938492[44] = 0; + out_8140915848168938492[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_8140915848168938492[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_8140915848168938492[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8140915848168938492[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8140915848168938492[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_8140915848168938492[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_8140915848168938492[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_8140915848168938492[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_8140915848168938492[53] = -9.8100000000000005*dt; + out_8140915848168938492[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_8140915848168938492[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_8140915848168938492[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8140915848168938492[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8140915848168938492[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_8140915848168938492[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_8140915848168938492[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_8140915848168938492[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8140915848168938492[62] = 0; + out_8140915848168938492[63] = 0; + out_8140915848168938492[64] = 0; + out_8140915848168938492[65] = 0; + out_8140915848168938492[66] = 0; + out_8140915848168938492[67] = 0; + out_8140915848168938492[68] = 0; + out_8140915848168938492[69] = 0; + out_8140915848168938492[70] = 1; + out_8140915848168938492[71] = 0; + out_8140915848168938492[72] = 0; + out_8140915848168938492[73] = 0; + out_8140915848168938492[74] = 0; + out_8140915848168938492[75] = 0; + out_8140915848168938492[76] = 0; + out_8140915848168938492[77] = 0; + out_8140915848168938492[78] = 0; + out_8140915848168938492[79] = 0; + out_8140915848168938492[80] = 1; } -void h_25(double *state, double *unused, double *out_2505292091765311779) { - out_2505292091765311779[0] = state[6]; +void h_25(double *state, double *unused, double *out_7505552333880797139) { + out_7505552333880797139[0] = state[6]; } -void H_25(double *state, double *unused, double *out_910312076417812004) { - out_910312076417812004[0] = 0; - out_910312076417812004[1] = 0; - out_910312076417812004[2] = 0; - out_910312076417812004[3] = 0; - out_910312076417812004[4] = 0; - out_910312076417812004[5] = 0; - out_910312076417812004[6] = 1; - out_910312076417812004[7] = 0; - out_910312076417812004[8] = 0; +void H_25(double *state, double *unused, double *out_1893586668788052765) { + out_1893586668788052765[0] = 0; + out_1893586668788052765[1] = 0; + out_1893586668788052765[2] = 0; + out_1893586668788052765[3] = 0; + out_1893586668788052765[4] = 0; + out_1893586668788052765[5] = 0; + out_1893586668788052765[6] = 1; + out_1893586668788052765[7] = 0; + out_1893586668788052765[8] = 0; } -void h_24(double *state, double *unused, double *out_484124202729088643) { - out_484124202729088643[0] = state[4]; - out_484124202729088643[1] = state[5]; +void h_24(double *state, double *unused, double *out_6517755359246205097) { + out_6517755359246205097[0] = state[4]; + out_6517755359246205097[1] = state[5]; } -void H_24(double *state, double *unused, double *out_1315395707561056558) { - out_1315395707561056558[0] = 0; - out_1315395707561056558[1] = 0; - out_1315395707561056558[2] = 0; - out_1315395707561056558[3] = 0; - out_1315395707561056558[4] = 1; - out_1315395707561056558[5] = 0; - out_1315395707561056558[6] = 0; - out_1315395707561056558[7] = 0; - out_1315395707561056558[8] = 0; - out_1315395707561056558[9] = 0; - out_1315395707561056558[10] = 0; - out_1315395707561056558[11] = 0; - out_1315395707561056558[12] = 0; - out_1315395707561056558[13] = 0; - out_1315395707561056558[14] = 1; - out_1315395707561056558[15] = 0; - out_1315395707561056558[16] = 0; - out_1315395707561056558[17] = 0; +void H_24(double *state, double *unused, double *out_1664338930014270371) { + out_1664338930014270371[0] = 0; + out_1664338930014270371[1] = 0; + out_1664338930014270371[2] = 0; + out_1664338930014270371[3] = 0; + out_1664338930014270371[4] = 1; + out_1664338930014270371[5] = 0; + out_1664338930014270371[6] = 0; + out_1664338930014270371[7] = 0; + out_1664338930014270371[8] = 0; + out_1664338930014270371[9] = 0; + out_1664338930014270371[10] = 0; + out_1664338930014270371[11] = 0; + out_1664338930014270371[12] = 0; + out_1664338930014270371[13] = 0; + out_1664338930014270371[14] = 1; + out_1664338930014270371[15] = 0; + out_1664338930014270371[16] = 0; + out_1664338930014270371[17] = 0; } -void h_30(double *state, double *unused, double *out_2556252205875338366) { - out_2556252205875338366[0] = state[4]; +void h_30(double *state, double *unused, double *out_5415134303277731364) { + out_5415134303277731364[0] = state[4]; } -void H_30(double *state, double *unused, double *out_6006378265073804751) { - out_6006378265073804751[0] = 0; - out_6006378265073804751[1] = 0; - out_6006378265073804751[2] = 0; - out_6006378265073804751[3] = 0; - out_6006378265073804751[4] = 1; - out_6006378265073804751[5] = 0; - out_6006378265073804751[6] = 0; - out_6006378265073804751[7] = 0; - out_6006378265073804751[8] = 0; +void H_30(double *state, double *unused, double *out_2022925615931292835) { + out_2022925615931292835[0] = 0; + out_2022925615931292835[1] = 0; + out_2022925615931292835[2] = 0; + out_2022925615931292835[3] = 0; + out_2022925615931292835[4] = 1; + out_2022925615931292835[5] = 0; + out_2022925615931292835[6] = 0; + out_2022925615931292835[7] = 0; + out_2022925615931292835[8] = 0; } -void h_26(double *state, double *unused, double *out_1841699875406290646) { - out_1841699875406290646[0] = state[7]; +void h_26(double *state, double *unused, double *out_6358644156179393799) { + out_6358644156179393799[0] = state[7]; } -void H_26(double *state, double *unused, double *out_4651815395291868228) { - out_4651815395291868228[0] = 0; - out_4651815395291868228[1] = 0; - out_4651815395291868228[2] = 0; - out_4651815395291868228[3] = 0; - out_4651815395291868228[4] = 0; - out_4651815395291868228[5] = 0; - out_4651815395291868228[6] = 0; - out_4651815395291868228[7] = 1; - out_4651815395291868228[8] = 0; +void H_26(double *state, double *unused, double *out_5635089987662108989) { + out_5635089987662108989[0] = 0; + out_5635089987662108989[1] = 0; + out_5635089987662108989[2] = 0; + out_5635089987662108989[3] = 0; + out_5635089987662108989[4] = 0; + out_5635089987662108989[5] = 0; + out_5635089987662108989[6] = 0; + out_5635089987662108989[7] = 1; + out_5635089987662108989[8] = 0; } -void h_27(double *state, double *unused, double *out_3893749425648537367) { - out_3893749425648537367[0] = state[3]; +void h_27(double *state, double *unused, double *out_2535534920184527824) { + out_2535534920184527824[0] = state[3]; } -void H_27(double *state, double *unused, double *out_3831614953273379840) { - out_3831614953273379840[0] = 0; - out_3831614953273379840[1] = 0; - out_3831614953273379840[2] = 0; - out_3831614953273379840[3] = 1; - out_3831614953273379840[4] = 0; - out_3831614953273379840[5] = 0; - out_3831614953273379840[6] = 0; - out_3831614953273379840[7] = 0; - out_3831614953273379840[8] = 0; +void H_27(double *state, double *unused, double *out_4197688927731717746) { + out_4197688927731717746[0] = 0; + out_4197688927731717746[1] = 0; + out_4197688927731717746[2] = 0; + out_4197688927731717746[3] = 1; + out_4197688927731717746[4] = 0; + out_4197688927731717746[5] = 0; + out_4197688927731717746[6] = 0; + out_4197688927731717746[7] = 0; + out_4197688927731717746[8] = 0; } -void h_29(double *state, double *unused, double *out_7782159421710663313) { - out_7782159421710663313[0] = state[1]; +void h_29(double *state, double *unused, double *out_443846924135533467) { + out_443846924135533467[0] = state[1]; } -void H_29(double *state, double *unused, double *out_6516609609388196935) { - out_6516609609388196935[0] = 0; - out_6516609609388196935[1] = 1; - out_6516609609388196935[2] = 0; - out_6516609609388196935[3] = 0; - out_6516609609388196935[4] = 0; - out_6516609609388196935[5] = 0; - out_6516609609388196935[6] = 0; - out_6516609609388196935[7] = 0; - out_6516609609388196935[8] = 0; +void H_29(double *state, double *unused, double *out_1512694271616900651) { + out_1512694271616900651[0] = 0; + out_1512694271616900651[1] = 1; + out_1512694271616900651[2] = 0; + out_1512694271616900651[3] = 0; + out_1512694271616900651[4] = 0; + out_1512694271616900651[5] = 0; + out_1512694271616900651[6] = 0; + out_1512694271616900651[7] = 0; + out_1512694271616900651[8] = 0; } -void h_28(double *state, double *unused, double *out_5061493226724679104) { - out_5061493226724679104[0] = state[0]; +void h_28(double *state, double *unused, double *out_7537208187845373469) { + out_7537208187845373469[0] = state[0]; } -void H_28(double *state, double *unused, double *out_1434210592318666361) { - out_1434210592318666361[0] = 1; - out_1434210592318666361[1] = 0; - out_1434210592318666361[2] = 0; - out_1434210592318666361[3] = 0; - out_1434210592318666361[4] = 0; - out_1434210592318666361[5] = 0; - out_1434210592318666361[6] = 0; - out_1434210592318666361[7] = 0; - out_1434210592318666361[8] = 0; +void H_28(double *state, double *unused, double *out_6595093288686431225) { + out_6595093288686431225[0] = 1; + out_6595093288686431225[1] = 0; + out_6595093288686431225[2] = 0; + out_6595093288686431225[3] = 0; + out_6595093288686431225[4] = 0; + out_6595093288686431225[5] = 0; + out_6595093288686431225[6] = 0; + out_6595093288686431225[7] = 0; + out_6595093288686431225[8] = 0; } -void h_31(double *state, double *unused, double *out_7444791300680499845) { - out_7444791300680499845[0] = state[8]; +void h_31(double *state, double *unused, double *out_6103587875730340171) { + out_6103587875730340171[0] = state[8]; } -void H_31(double *state, double *unused, double *out_879666114540851576) { - out_879666114540851576[0] = 0; - out_879666114540851576[1] = 0; - out_879666114540851576[2] = 0; - out_879666114540851576[3] = 0; - out_879666114540851576[4] = 0; - out_879666114540851576[5] = 0; - out_879666114540851576[6] = 0; - out_879666114540851576[7] = 0; - out_879666114540851576[8] = 1; +void H_31(double *state, double *unused, double *out_1862940706911092337) { + out_1862940706911092337[0] = 0; + out_1862940706911092337[1] = 0; + out_1862940706911092337[2] = 0; + out_1862940706911092337[3] = 0; + out_1862940706911092337[4] = 0; + out_1862940706911092337[5] = 0; + out_1862940706911092337[6] = 0; + out_1862940706911092337[7] = 0; + out_1862940706911092337[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_7709214462285076644) { - err_fun(nom_x, delta_x, out_7709214462285076644); +void car_err_fun(double *nom_x, double *delta_x, double *out_2605865889076671127) { + err_fun(nom_x, delta_x, out_2605865889076671127); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_181432404984807710) { - inv_err_fun(nom_x, true_x, out_181432404984807710); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_6092587495462394066) { + inv_err_fun(nom_x, true_x, out_6092587495462394066); } -void car_H_mod_fun(double *state, double *out_8282876149494425233) { - H_mod_fun(state, out_8282876149494425233); +void car_H_mod_fun(double *state, double *out_5416567318777544442) { + H_mod_fun(state, out_5416567318777544442); } -void car_f_fun(double *state, double dt, double *out_1683945396559752472) { - f_fun(state, dt, out_1683945396559752472); +void car_f_fun(double *state, double dt, double *out_8892385645104681966) { + f_fun(state, dt, out_8892385645104681966); } -void car_F_fun(double *state, double dt, double *out_239932902801011581) { - F_fun(state, dt, out_239932902801011581); +void car_F_fun(double *state, double dt, double *out_8140915848168938492) { + F_fun(state, dt, out_8140915848168938492); } -void car_h_25(double *state, double *unused, double *out_2505292091765311779) { - h_25(state, unused, out_2505292091765311779); +void car_h_25(double *state, double *unused, double *out_7505552333880797139) { + h_25(state, unused, out_7505552333880797139); } -void car_H_25(double *state, double *unused, double *out_910312076417812004) { - H_25(state, unused, out_910312076417812004); +void car_H_25(double *state, double *unused, double *out_1893586668788052765) { + H_25(state, unused, out_1893586668788052765); } -void car_h_24(double *state, double *unused, double *out_484124202729088643) { - h_24(state, unused, out_484124202729088643); +void car_h_24(double *state, double *unused, double *out_6517755359246205097) { + h_24(state, unused, out_6517755359246205097); } -void car_H_24(double *state, double *unused, double *out_1315395707561056558) { - H_24(state, unused, out_1315395707561056558); +void car_H_24(double *state, double *unused, double *out_1664338930014270371) { + H_24(state, unused, out_1664338930014270371); } -void car_h_30(double *state, double *unused, double *out_2556252205875338366) { - h_30(state, unused, out_2556252205875338366); +void car_h_30(double *state, double *unused, double *out_5415134303277731364) { + h_30(state, unused, out_5415134303277731364); } -void car_H_30(double *state, double *unused, double *out_6006378265073804751) { - H_30(state, unused, out_6006378265073804751); +void car_H_30(double *state, double *unused, double *out_2022925615931292835) { + H_30(state, unused, out_2022925615931292835); } -void car_h_26(double *state, double *unused, double *out_1841699875406290646) { - h_26(state, unused, out_1841699875406290646); +void car_h_26(double *state, double *unused, double *out_6358644156179393799) { + h_26(state, unused, out_6358644156179393799); } -void car_H_26(double *state, double *unused, double *out_4651815395291868228) { - H_26(state, unused, out_4651815395291868228); +void car_H_26(double *state, double *unused, double *out_5635089987662108989) { + H_26(state, unused, out_5635089987662108989); } -void car_h_27(double *state, double *unused, double *out_3893749425648537367) { - h_27(state, unused, out_3893749425648537367); +void car_h_27(double *state, double *unused, double *out_2535534920184527824) { + h_27(state, unused, out_2535534920184527824); } -void car_H_27(double *state, double *unused, double *out_3831614953273379840) { - H_27(state, unused, out_3831614953273379840); +void car_H_27(double *state, double *unused, double *out_4197688927731717746) { + H_27(state, unused, out_4197688927731717746); } -void car_h_29(double *state, double *unused, double *out_7782159421710663313) { - h_29(state, unused, out_7782159421710663313); +void car_h_29(double *state, double *unused, double *out_443846924135533467) { + h_29(state, unused, out_443846924135533467); } -void car_H_29(double *state, double *unused, double *out_6516609609388196935) { - H_29(state, unused, out_6516609609388196935); +void car_H_29(double *state, double *unused, double *out_1512694271616900651) { + H_29(state, unused, out_1512694271616900651); } -void car_h_28(double *state, double *unused, double *out_5061493226724679104) { - h_28(state, unused, out_5061493226724679104); +void car_h_28(double *state, double *unused, double *out_7537208187845373469) { + h_28(state, unused, out_7537208187845373469); } -void car_H_28(double *state, double *unused, double *out_1434210592318666361) { - H_28(state, unused, out_1434210592318666361); +void car_H_28(double *state, double *unused, double *out_6595093288686431225) { + H_28(state, unused, out_6595093288686431225); } -void car_h_31(double *state, double *unused, double *out_7444791300680499845) { - h_31(state, unused, out_7444791300680499845); +void car_h_31(double *state, double *unused, double *out_6103587875730340171) { + h_31(state, unused, out_6103587875730340171); } -void car_H_31(double *state, double *unused, double *out_879666114540851576) { - H_31(state, unused, out_879666114540851576); +void car_H_31(double *state, double *unused, double *out_1862940706911092337) { + H_31(state, unused, out_1862940706911092337); } 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 9bcce81b5..8e9cc3c0e 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_7709214462285076644); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_181432404984807710); -void car_H_mod_fun(double *state, double *out_8282876149494425233); -void car_f_fun(double *state, double dt, double *out_1683945396559752472); -void car_F_fun(double *state, double dt, double *out_239932902801011581); -void car_h_25(double *state, double *unused, double *out_2505292091765311779); -void car_H_25(double *state, double *unused, double *out_910312076417812004); -void car_h_24(double *state, double *unused, double *out_484124202729088643); -void car_H_24(double *state, double *unused, double *out_1315395707561056558); -void car_h_30(double *state, double *unused, double *out_2556252205875338366); -void car_H_30(double *state, double *unused, double *out_6006378265073804751); -void car_h_26(double *state, double *unused, double *out_1841699875406290646); -void car_H_26(double *state, double *unused, double *out_4651815395291868228); -void car_h_27(double *state, double *unused, double *out_3893749425648537367); -void car_H_27(double *state, double *unused, double *out_3831614953273379840); -void car_h_29(double *state, double *unused, double *out_7782159421710663313); -void car_H_29(double *state, double *unused, double *out_6516609609388196935); -void car_h_28(double *state, double *unused, double *out_5061493226724679104); -void car_H_28(double *state, double *unused, double *out_1434210592318666361); -void car_h_31(double *state, double *unused, double *out_7444791300680499845); -void car_H_31(double *state, double *unused, double *out_879666114540851576); +void car_err_fun(double *nom_x, double *delta_x, double *out_2605865889076671127); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_6092587495462394066); +void car_H_mod_fun(double *state, double *out_5416567318777544442); +void car_f_fun(double *state, double dt, double *out_8892385645104681966); +void car_F_fun(double *state, double dt, double *out_8140915848168938492); +void car_h_25(double *state, double *unused, double *out_7505552333880797139); +void car_H_25(double *state, double *unused, double *out_1893586668788052765); +void car_h_24(double *state, double *unused, double *out_6517755359246205097); +void car_H_24(double *state, double *unused, double *out_1664338930014270371); +void car_h_30(double *state, double *unused, double *out_5415134303277731364); +void car_H_30(double *state, double *unused, double *out_2022925615931292835); +void car_h_26(double *state, double *unused, double *out_6358644156179393799); +void car_H_26(double *state, double *unused, double *out_5635089987662108989); +void car_h_27(double *state, double *unused, double *out_2535534920184527824); +void car_H_27(double *state, double *unused, double *out_4197688927731717746); +void car_h_29(double *state, double *unused, double *out_443846924135533467); +void car_H_29(double *state, double *unused, double *out_1512694271616900651); +void car_h_28(double *state, double *unused, double *out_7537208187845373469); +void car_H_28(double *state, double *unused, double *out_6595093288686431225); +void car_h_31(double *state, double *unused, double *out_6103587875730340171); +void car_H_31(double *state, double *unused, double *out_1862940706911092337); 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 e8467cc78..d423b5e27 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_6392571160168570842) { - out_6392571160168570842[0] = delta_x[0] + nom_x[0]; - out_6392571160168570842[1] = delta_x[1] + nom_x[1]; - out_6392571160168570842[2] = delta_x[2] + nom_x[2]; - out_6392571160168570842[3] = delta_x[3] + nom_x[3]; - out_6392571160168570842[4] = delta_x[4] + nom_x[4]; - out_6392571160168570842[5] = delta_x[5] + nom_x[5]; - out_6392571160168570842[6] = delta_x[6] + nom_x[6]; - out_6392571160168570842[7] = delta_x[7] + nom_x[7]; - out_6392571160168570842[8] = delta_x[8] + nom_x[8]; - out_6392571160168570842[9] = delta_x[9] + nom_x[9]; - out_6392571160168570842[10] = delta_x[10] + nom_x[10]; - out_6392571160168570842[11] = delta_x[11] + nom_x[11]; - out_6392571160168570842[12] = delta_x[12] + nom_x[12]; - out_6392571160168570842[13] = delta_x[13] + nom_x[13]; - out_6392571160168570842[14] = delta_x[14] + nom_x[14]; - out_6392571160168570842[15] = delta_x[15] + nom_x[15]; - out_6392571160168570842[16] = delta_x[16] + nom_x[16]; - out_6392571160168570842[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_5135933030950096940) { + out_5135933030950096940[0] = delta_x[0] + nom_x[0]; + out_5135933030950096940[1] = delta_x[1] + nom_x[1]; + out_5135933030950096940[2] = delta_x[2] + nom_x[2]; + out_5135933030950096940[3] = delta_x[3] + nom_x[3]; + out_5135933030950096940[4] = delta_x[4] + nom_x[4]; + out_5135933030950096940[5] = delta_x[5] + nom_x[5]; + out_5135933030950096940[6] = delta_x[6] + nom_x[6]; + out_5135933030950096940[7] = delta_x[7] + nom_x[7]; + out_5135933030950096940[8] = delta_x[8] + nom_x[8]; + out_5135933030950096940[9] = delta_x[9] + nom_x[9]; + out_5135933030950096940[10] = delta_x[10] + nom_x[10]; + out_5135933030950096940[11] = delta_x[11] + nom_x[11]; + out_5135933030950096940[12] = delta_x[12] + nom_x[12]; + out_5135933030950096940[13] = delta_x[13] + nom_x[13]; + out_5135933030950096940[14] = delta_x[14] + nom_x[14]; + out_5135933030950096940[15] = delta_x[15] + nom_x[15]; + out_5135933030950096940[16] = delta_x[16] + nom_x[16]; + out_5135933030950096940[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_70108598084165787) { - out_70108598084165787[0] = -nom_x[0] + true_x[0]; - out_70108598084165787[1] = -nom_x[1] + true_x[1]; - out_70108598084165787[2] = -nom_x[2] + true_x[2]; - out_70108598084165787[3] = -nom_x[3] + true_x[3]; - out_70108598084165787[4] = -nom_x[4] + true_x[4]; - out_70108598084165787[5] = -nom_x[5] + true_x[5]; - out_70108598084165787[6] = -nom_x[6] + true_x[6]; - out_70108598084165787[7] = -nom_x[7] + true_x[7]; - out_70108598084165787[8] = -nom_x[8] + true_x[8]; - out_70108598084165787[9] = -nom_x[9] + true_x[9]; - out_70108598084165787[10] = -nom_x[10] + true_x[10]; - out_70108598084165787[11] = -nom_x[11] + true_x[11]; - out_70108598084165787[12] = -nom_x[12] + true_x[12]; - out_70108598084165787[13] = -nom_x[13] + true_x[13]; - out_70108598084165787[14] = -nom_x[14] + true_x[14]; - out_70108598084165787[15] = -nom_x[15] + true_x[15]; - out_70108598084165787[16] = -nom_x[16] + true_x[16]; - out_70108598084165787[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_4473434694497561565) { + out_4473434694497561565[0] = -nom_x[0] + true_x[0]; + out_4473434694497561565[1] = -nom_x[1] + true_x[1]; + out_4473434694497561565[2] = -nom_x[2] + true_x[2]; + out_4473434694497561565[3] = -nom_x[3] + true_x[3]; + out_4473434694497561565[4] = -nom_x[4] + true_x[4]; + out_4473434694497561565[5] = -nom_x[5] + true_x[5]; + out_4473434694497561565[6] = -nom_x[6] + true_x[6]; + out_4473434694497561565[7] = -nom_x[7] + true_x[7]; + out_4473434694497561565[8] = -nom_x[8] + true_x[8]; + out_4473434694497561565[9] = -nom_x[9] + true_x[9]; + out_4473434694497561565[10] = -nom_x[10] + true_x[10]; + out_4473434694497561565[11] = -nom_x[11] + true_x[11]; + out_4473434694497561565[12] = -nom_x[12] + true_x[12]; + out_4473434694497561565[13] = -nom_x[13] + true_x[13]; + out_4473434694497561565[14] = -nom_x[14] + true_x[14]; + out_4473434694497561565[15] = -nom_x[15] + true_x[15]; + out_4473434694497561565[16] = -nom_x[16] + true_x[16]; + out_4473434694497561565[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_7031029695439970094) { - out_7031029695439970094[0] = 1.0; - out_7031029695439970094[1] = 0.0; - out_7031029695439970094[2] = 0.0; - out_7031029695439970094[3] = 0.0; - out_7031029695439970094[4] = 0.0; - out_7031029695439970094[5] = 0.0; - out_7031029695439970094[6] = 0.0; - out_7031029695439970094[7] = 0.0; - out_7031029695439970094[8] = 0.0; - out_7031029695439970094[9] = 0.0; - out_7031029695439970094[10] = 0.0; - out_7031029695439970094[11] = 0.0; - out_7031029695439970094[12] = 0.0; - out_7031029695439970094[13] = 0.0; - out_7031029695439970094[14] = 0.0; - out_7031029695439970094[15] = 0.0; - out_7031029695439970094[16] = 0.0; - out_7031029695439970094[17] = 0.0; - out_7031029695439970094[18] = 0.0; - out_7031029695439970094[19] = 1.0; - out_7031029695439970094[20] = 0.0; - out_7031029695439970094[21] = 0.0; - out_7031029695439970094[22] = 0.0; - out_7031029695439970094[23] = 0.0; - out_7031029695439970094[24] = 0.0; - out_7031029695439970094[25] = 0.0; - out_7031029695439970094[26] = 0.0; - out_7031029695439970094[27] = 0.0; - out_7031029695439970094[28] = 0.0; - out_7031029695439970094[29] = 0.0; - out_7031029695439970094[30] = 0.0; - out_7031029695439970094[31] = 0.0; - out_7031029695439970094[32] = 0.0; - out_7031029695439970094[33] = 0.0; - out_7031029695439970094[34] = 0.0; - out_7031029695439970094[35] = 0.0; - out_7031029695439970094[36] = 0.0; - out_7031029695439970094[37] = 0.0; - out_7031029695439970094[38] = 1.0; - out_7031029695439970094[39] = 0.0; - out_7031029695439970094[40] = 0.0; - out_7031029695439970094[41] = 0.0; - out_7031029695439970094[42] = 0.0; - out_7031029695439970094[43] = 0.0; - out_7031029695439970094[44] = 0.0; - out_7031029695439970094[45] = 0.0; - out_7031029695439970094[46] = 0.0; - out_7031029695439970094[47] = 0.0; - out_7031029695439970094[48] = 0.0; - out_7031029695439970094[49] = 0.0; - out_7031029695439970094[50] = 0.0; - out_7031029695439970094[51] = 0.0; - out_7031029695439970094[52] = 0.0; - out_7031029695439970094[53] = 0.0; - out_7031029695439970094[54] = 0.0; - out_7031029695439970094[55] = 0.0; - out_7031029695439970094[56] = 0.0; - out_7031029695439970094[57] = 1.0; - out_7031029695439970094[58] = 0.0; - out_7031029695439970094[59] = 0.0; - out_7031029695439970094[60] = 0.0; - out_7031029695439970094[61] = 0.0; - out_7031029695439970094[62] = 0.0; - out_7031029695439970094[63] = 0.0; - out_7031029695439970094[64] = 0.0; - out_7031029695439970094[65] = 0.0; - out_7031029695439970094[66] = 0.0; - out_7031029695439970094[67] = 0.0; - out_7031029695439970094[68] = 0.0; - out_7031029695439970094[69] = 0.0; - out_7031029695439970094[70] = 0.0; - out_7031029695439970094[71] = 0.0; - out_7031029695439970094[72] = 0.0; - out_7031029695439970094[73] = 0.0; - out_7031029695439970094[74] = 0.0; - out_7031029695439970094[75] = 0.0; - out_7031029695439970094[76] = 1.0; - out_7031029695439970094[77] = 0.0; - out_7031029695439970094[78] = 0.0; - out_7031029695439970094[79] = 0.0; - out_7031029695439970094[80] = 0.0; - out_7031029695439970094[81] = 0.0; - out_7031029695439970094[82] = 0.0; - out_7031029695439970094[83] = 0.0; - out_7031029695439970094[84] = 0.0; - out_7031029695439970094[85] = 0.0; - out_7031029695439970094[86] = 0.0; - out_7031029695439970094[87] = 0.0; - out_7031029695439970094[88] = 0.0; - out_7031029695439970094[89] = 0.0; - out_7031029695439970094[90] = 0.0; - out_7031029695439970094[91] = 0.0; - out_7031029695439970094[92] = 0.0; - out_7031029695439970094[93] = 0.0; - out_7031029695439970094[94] = 0.0; - out_7031029695439970094[95] = 1.0; - out_7031029695439970094[96] = 0.0; - out_7031029695439970094[97] = 0.0; - out_7031029695439970094[98] = 0.0; - out_7031029695439970094[99] = 0.0; - out_7031029695439970094[100] = 0.0; - out_7031029695439970094[101] = 0.0; - out_7031029695439970094[102] = 0.0; - out_7031029695439970094[103] = 0.0; - out_7031029695439970094[104] = 0.0; - out_7031029695439970094[105] = 0.0; - out_7031029695439970094[106] = 0.0; - out_7031029695439970094[107] = 0.0; - out_7031029695439970094[108] = 0.0; - out_7031029695439970094[109] = 0.0; - out_7031029695439970094[110] = 0.0; - out_7031029695439970094[111] = 0.0; - out_7031029695439970094[112] = 0.0; - out_7031029695439970094[113] = 0.0; - out_7031029695439970094[114] = 1.0; - out_7031029695439970094[115] = 0.0; - out_7031029695439970094[116] = 0.0; - out_7031029695439970094[117] = 0.0; - out_7031029695439970094[118] = 0.0; - out_7031029695439970094[119] = 0.0; - out_7031029695439970094[120] = 0.0; - out_7031029695439970094[121] = 0.0; - out_7031029695439970094[122] = 0.0; - out_7031029695439970094[123] = 0.0; - out_7031029695439970094[124] = 0.0; - out_7031029695439970094[125] = 0.0; - out_7031029695439970094[126] = 0.0; - out_7031029695439970094[127] = 0.0; - out_7031029695439970094[128] = 0.0; - out_7031029695439970094[129] = 0.0; - out_7031029695439970094[130] = 0.0; - out_7031029695439970094[131] = 0.0; - out_7031029695439970094[132] = 0.0; - out_7031029695439970094[133] = 1.0; - out_7031029695439970094[134] = 0.0; - out_7031029695439970094[135] = 0.0; - out_7031029695439970094[136] = 0.0; - out_7031029695439970094[137] = 0.0; - out_7031029695439970094[138] = 0.0; - out_7031029695439970094[139] = 0.0; - out_7031029695439970094[140] = 0.0; - out_7031029695439970094[141] = 0.0; - out_7031029695439970094[142] = 0.0; - out_7031029695439970094[143] = 0.0; - out_7031029695439970094[144] = 0.0; - out_7031029695439970094[145] = 0.0; - out_7031029695439970094[146] = 0.0; - out_7031029695439970094[147] = 0.0; - out_7031029695439970094[148] = 0.0; - out_7031029695439970094[149] = 0.0; - out_7031029695439970094[150] = 0.0; - out_7031029695439970094[151] = 0.0; - out_7031029695439970094[152] = 1.0; - out_7031029695439970094[153] = 0.0; - out_7031029695439970094[154] = 0.0; - out_7031029695439970094[155] = 0.0; - out_7031029695439970094[156] = 0.0; - out_7031029695439970094[157] = 0.0; - out_7031029695439970094[158] = 0.0; - out_7031029695439970094[159] = 0.0; - out_7031029695439970094[160] = 0.0; - out_7031029695439970094[161] = 0.0; - out_7031029695439970094[162] = 0.0; - out_7031029695439970094[163] = 0.0; - out_7031029695439970094[164] = 0.0; - out_7031029695439970094[165] = 0.0; - out_7031029695439970094[166] = 0.0; - out_7031029695439970094[167] = 0.0; - out_7031029695439970094[168] = 0.0; - out_7031029695439970094[169] = 0.0; - out_7031029695439970094[170] = 0.0; - out_7031029695439970094[171] = 1.0; - out_7031029695439970094[172] = 0.0; - out_7031029695439970094[173] = 0.0; - out_7031029695439970094[174] = 0.0; - out_7031029695439970094[175] = 0.0; - out_7031029695439970094[176] = 0.0; - out_7031029695439970094[177] = 0.0; - out_7031029695439970094[178] = 0.0; - out_7031029695439970094[179] = 0.0; - out_7031029695439970094[180] = 0.0; - out_7031029695439970094[181] = 0.0; - out_7031029695439970094[182] = 0.0; - out_7031029695439970094[183] = 0.0; - out_7031029695439970094[184] = 0.0; - out_7031029695439970094[185] = 0.0; - out_7031029695439970094[186] = 0.0; - out_7031029695439970094[187] = 0.0; - out_7031029695439970094[188] = 0.0; - out_7031029695439970094[189] = 0.0; - out_7031029695439970094[190] = 1.0; - out_7031029695439970094[191] = 0.0; - out_7031029695439970094[192] = 0.0; - out_7031029695439970094[193] = 0.0; - out_7031029695439970094[194] = 0.0; - out_7031029695439970094[195] = 0.0; - out_7031029695439970094[196] = 0.0; - out_7031029695439970094[197] = 0.0; - out_7031029695439970094[198] = 0.0; - out_7031029695439970094[199] = 0.0; - out_7031029695439970094[200] = 0.0; - out_7031029695439970094[201] = 0.0; - out_7031029695439970094[202] = 0.0; - out_7031029695439970094[203] = 0.0; - out_7031029695439970094[204] = 0.0; - out_7031029695439970094[205] = 0.0; - out_7031029695439970094[206] = 0.0; - out_7031029695439970094[207] = 0.0; - out_7031029695439970094[208] = 0.0; - out_7031029695439970094[209] = 1.0; - out_7031029695439970094[210] = 0.0; - out_7031029695439970094[211] = 0.0; - out_7031029695439970094[212] = 0.0; - out_7031029695439970094[213] = 0.0; - out_7031029695439970094[214] = 0.0; - out_7031029695439970094[215] = 0.0; - out_7031029695439970094[216] = 0.0; - out_7031029695439970094[217] = 0.0; - out_7031029695439970094[218] = 0.0; - out_7031029695439970094[219] = 0.0; - out_7031029695439970094[220] = 0.0; - out_7031029695439970094[221] = 0.0; - out_7031029695439970094[222] = 0.0; - out_7031029695439970094[223] = 0.0; - out_7031029695439970094[224] = 0.0; - out_7031029695439970094[225] = 0.0; - out_7031029695439970094[226] = 0.0; - out_7031029695439970094[227] = 0.0; - out_7031029695439970094[228] = 1.0; - out_7031029695439970094[229] = 0.0; - out_7031029695439970094[230] = 0.0; - out_7031029695439970094[231] = 0.0; - out_7031029695439970094[232] = 0.0; - out_7031029695439970094[233] = 0.0; - out_7031029695439970094[234] = 0.0; - out_7031029695439970094[235] = 0.0; - out_7031029695439970094[236] = 0.0; - out_7031029695439970094[237] = 0.0; - out_7031029695439970094[238] = 0.0; - out_7031029695439970094[239] = 0.0; - out_7031029695439970094[240] = 0.0; - out_7031029695439970094[241] = 0.0; - out_7031029695439970094[242] = 0.0; - out_7031029695439970094[243] = 0.0; - out_7031029695439970094[244] = 0.0; - out_7031029695439970094[245] = 0.0; - out_7031029695439970094[246] = 0.0; - out_7031029695439970094[247] = 1.0; - out_7031029695439970094[248] = 0.0; - out_7031029695439970094[249] = 0.0; - out_7031029695439970094[250] = 0.0; - out_7031029695439970094[251] = 0.0; - out_7031029695439970094[252] = 0.0; - out_7031029695439970094[253] = 0.0; - out_7031029695439970094[254] = 0.0; - out_7031029695439970094[255] = 0.0; - out_7031029695439970094[256] = 0.0; - out_7031029695439970094[257] = 0.0; - out_7031029695439970094[258] = 0.0; - out_7031029695439970094[259] = 0.0; - out_7031029695439970094[260] = 0.0; - out_7031029695439970094[261] = 0.0; - out_7031029695439970094[262] = 0.0; - out_7031029695439970094[263] = 0.0; - out_7031029695439970094[264] = 0.0; - out_7031029695439970094[265] = 0.0; - out_7031029695439970094[266] = 1.0; - out_7031029695439970094[267] = 0.0; - out_7031029695439970094[268] = 0.0; - out_7031029695439970094[269] = 0.0; - out_7031029695439970094[270] = 0.0; - out_7031029695439970094[271] = 0.0; - out_7031029695439970094[272] = 0.0; - out_7031029695439970094[273] = 0.0; - out_7031029695439970094[274] = 0.0; - out_7031029695439970094[275] = 0.0; - out_7031029695439970094[276] = 0.0; - out_7031029695439970094[277] = 0.0; - out_7031029695439970094[278] = 0.0; - out_7031029695439970094[279] = 0.0; - out_7031029695439970094[280] = 0.0; - out_7031029695439970094[281] = 0.0; - out_7031029695439970094[282] = 0.0; - out_7031029695439970094[283] = 0.0; - out_7031029695439970094[284] = 0.0; - out_7031029695439970094[285] = 1.0; - out_7031029695439970094[286] = 0.0; - out_7031029695439970094[287] = 0.0; - out_7031029695439970094[288] = 0.0; - out_7031029695439970094[289] = 0.0; - out_7031029695439970094[290] = 0.0; - out_7031029695439970094[291] = 0.0; - out_7031029695439970094[292] = 0.0; - out_7031029695439970094[293] = 0.0; - out_7031029695439970094[294] = 0.0; - out_7031029695439970094[295] = 0.0; - out_7031029695439970094[296] = 0.0; - out_7031029695439970094[297] = 0.0; - out_7031029695439970094[298] = 0.0; - out_7031029695439970094[299] = 0.0; - out_7031029695439970094[300] = 0.0; - out_7031029695439970094[301] = 0.0; - out_7031029695439970094[302] = 0.0; - out_7031029695439970094[303] = 0.0; - out_7031029695439970094[304] = 1.0; - out_7031029695439970094[305] = 0.0; - out_7031029695439970094[306] = 0.0; - out_7031029695439970094[307] = 0.0; - out_7031029695439970094[308] = 0.0; - out_7031029695439970094[309] = 0.0; - out_7031029695439970094[310] = 0.0; - out_7031029695439970094[311] = 0.0; - out_7031029695439970094[312] = 0.0; - out_7031029695439970094[313] = 0.0; - out_7031029695439970094[314] = 0.0; - out_7031029695439970094[315] = 0.0; - out_7031029695439970094[316] = 0.0; - out_7031029695439970094[317] = 0.0; - out_7031029695439970094[318] = 0.0; - out_7031029695439970094[319] = 0.0; - out_7031029695439970094[320] = 0.0; - out_7031029695439970094[321] = 0.0; - out_7031029695439970094[322] = 0.0; - out_7031029695439970094[323] = 1.0; +void H_mod_fun(double *state, double *out_4955206330411314418) { + out_4955206330411314418[0] = 1.0; + out_4955206330411314418[1] = 0.0; + out_4955206330411314418[2] = 0.0; + out_4955206330411314418[3] = 0.0; + out_4955206330411314418[4] = 0.0; + out_4955206330411314418[5] = 0.0; + out_4955206330411314418[6] = 0.0; + out_4955206330411314418[7] = 0.0; + out_4955206330411314418[8] = 0.0; + out_4955206330411314418[9] = 0.0; + out_4955206330411314418[10] = 0.0; + out_4955206330411314418[11] = 0.0; + out_4955206330411314418[12] = 0.0; + out_4955206330411314418[13] = 0.0; + out_4955206330411314418[14] = 0.0; + out_4955206330411314418[15] = 0.0; + out_4955206330411314418[16] = 0.0; + out_4955206330411314418[17] = 0.0; + out_4955206330411314418[18] = 0.0; + out_4955206330411314418[19] = 1.0; + out_4955206330411314418[20] = 0.0; + out_4955206330411314418[21] = 0.0; + out_4955206330411314418[22] = 0.0; + out_4955206330411314418[23] = 0.0; + out_4955206330411314418[24] = 0.0; + out_4955206330411314418[25] = 0.0; + out_4955206330411314418[26] = 0.0; + out_4955206330411314418[27] = 0.0; + out_4955206330411314418[28] = 0.0; + out_4955206330411314418[29] = 0.0; + out_4955206330411314418[30] = 0.0; + out_4955206330411314418[31] = 0.0; + out_4955206330411314418[32] = 0.0; + out_4955206330411314418[33] = 0.0; + out_4955206330411314418[34] = 0.0; + out_4955206330411314418[35] = 0.0; + out_4955206330411314418[36] = 0.0; + out_4955206330411314418[37] = 0.0; + out_4955206330411314418[38] = 1.0; + out_4955206330411314418[39] = 0.0; + out_4955206330411314418[40] = 0.0; + out_4955206330411314418[41] = 0.0; + out_4955206330411314418[42] = 0.0; + out_4955206330411314418[43] = 0.0; + out_4955206330411314418[44] = 0.0; + out_4955206330411314418[45] = 0.0; + out_4955206330411314418[46] = 0.0; + out_4955206330411314418[47] = 0.0; + out_4955206330411314418[48] = 0.0; + out_4955206330411314418[49] = 0.0; + out_4955206330411314418[50] = 0.0; + out_4955206330411314418[51] = 0.0; + out_4955206330411314418[52] = 0.0; + out_4955206330411314418[53] = 0.0; + out_4955206330411314418[54] = 0.0; + out_4955206330411314418[55] = 0.0; + out_4955206330411314418[56] = 0.0; + out_4955206330411314418[57] = 1.0; + out_4955206330411314418[58] = 0.0; + out_4955206330411314418[59] = 0.0; + out_4955206330411314418[60] = 0.0; + out_4955206330411314418[61] = 0.0; + out_4955206330411314418[62] = 0.0; + out_4955206330411314418[63] = 0.0; + out_4955206330411314418[64] = 0.0; + out_4955206330411314418[65] = 0.0; + out_4955206330411314418[66] = 0.0; + out_4955206330411314418[67] = 0.0; + out_4955206330411314418[68] = 0.0; + out_4955206330411314418[69] = 0.0; + out_4955206330411314418[70] = 0.0; + out_4955206330411314418[71] = 0.0; + out_4955206330411314418[72] = 0.0; + out_4955206330411314418[73] = 0.0; + out_4955206330411314418[74] = 0.0; + out_4955206330411314418[75] = 0.0; + out_4955206330411314418[76] = 1.0; + out_4955206330411314418[77] = 0.0; + out_4955206330411314418[78] = 0.0; + out_4955206330411314418[79] = 0.0; + out_4955206330411314418[80] = 0.0; + out_4955206330411314418[81] = 0.0; + out_4955206330411314418[82] = 0.0; + out_4955206330411314418[83] = 0.0; + out_4955206330411314418[84] = 0.0; + out_4955206330411314418[85] = 0.0; + out_4955206330411314418[86] = 0.0; + out_4955206330411314418[87] = 0.0; + out_4955206330411314418[88] = 0.0; + out_4955206330411314418[89] = 0.0; + out_4955206330411314418[90] = 0.0; + out_4955206330411314418[91] = 0.0; + out_4955206330411314418[92] = 0.0; + out_4955206330411314418[93] = 0.0; + out_4955206330411314418[94] = 0.0; + out_4955206330411314418[95] = 1.0; + out_4955206330411314418[96] = 0.0; + out_4955206330411314418[97] = 0.0; + out_4955206330411314418[98] = 0.0; + out_4955206330411314418[99] = 0.0; + out_4955206330411314418[100] = 0.0; + out_4955206330411314418[101] = 0.0; + out_4955206330411314418[102] = 0.0; + out_4955206330411314418[103] = 0.0; + out_4955206330411314418[104] = 0.0; + out_4955206330411314418[105] = 0.0; + out_4955206330411314418[106] = 0.0; + out_4955206330411314418[107] = 0.0; + out_4955206330411314418[108] = 0.0; + out_4955206330411314418[109] = 0.0; + out_4955206330411314418[110] = 0.0; + out_4955206330411314418[111] = 0.0; + out_4955206330411314418[112] = 0.0; + out_4955206330411314418[113] = 0.0; + out_4955206330411314418[114] = 1.0; + out_4955206330411314418[115] = 0.0; + out_4955206330411314418[116] = 0.0; + out_4955206330411314418[117] = 0.0; + out_4955206330411314418[118] = 0.0; + out_4955206330411314418[119] = 0.0; + out_4955206330411314418[120] = 0.0; + out_4955206330411314418[121] = 0.0; + out_4955206330411314418[122] = 0.0; + out_4955206330411314418[123] = 0.0; + out_4955206330411314418[124] = 0.0; + out_4955206330411314418[125] = 0.0; + out_4955206330411314418[126] = 0.0; + out_4955206330411314418[127] = 0.0; + out_4955206330411314418[128] = 0.0; + out_4955206330411314418[129] = 0.0; + out_4955206330411314418[130] = 0.0; + out_4955206330411314418[131] = 0.0; + out_4955206330411314418[132] = 0.0; + out_4955206330411314418[133] = 1.0; + out_4955206330411314418[134] = 0.0; + out_4955206330411314418[135] = 0.0; + out_4955206330411314418[136] = 0.0; + out_4955206330411314418[137] = 0.0; + out_4955206330411314418[138] = 0.0; + out_4955206330411314418[139] = 0.0; + out_4955206330411314418[140] = 0.0; + out_4955206330411314418[141] = 0.0; + out_4955206330411314418[142] = 0.0; + out_4955206330411314418[143] = 0.0; + out_4955206330411314418[144] = 0.0; + out_4955206330411314418[145] = 0.0; + out_4955206330411314418[146] = 0.0; + out_4955206330411314418[147] = 0.0; + out_4955206330411314418[148] = 0.0; + out_4955206330411314418[149] = 0.0; + out_4955206330411314418[150] = 0.0; + out_4955206330411314418[151] = 0.0; + out_4955206330411314418[152] = 1.0; + out_4955206330411314418[153] = 0.0; + out_4955206330411314418[154] = 0.0; + out_4955206330411314418[155] = 0.0; + out_4955206330411314418[156] = 0.0; + out_4955206330411314418[157] = 0.0; + out_4955206330411314418[158] = 0.0; + out_4955206330411314418[159] = 0.0; + out_4955206330411314418[160] = 0.0; + out_4955206330411314418[161] = 0.0; + out_4955206330411314418[162] = 0.0; + out_4955206330411314418[163] = 0.0; + out_4955206330411314418[164] = 0.0; + out_4955206330411314418[165] = 0.0; + out_4955206330411314418[166] = 0.0; + out_4955206330411314418[167] = 0.0; + out_4955206330411314418[168] = 0.0; + out_4955206330411314418[169] = 0.0; + out_4955206330411314418[170] = 0.0; + out_4955206330411314418[171] = 1.0; + out_4955206330411314418[172] = 0.0; + out_4955206330411314418[173] = 0.0; + out_4955206330411314418[174] = 0.0; + out_4955206330411314418[175] = 0.0; + out_4955206330411314418[176] = 0.0; + out_4955206330411314418[177] = 0.0; + out_4955206330411314418[178] = 0.0; + out_4955206330411314418[179] = 0.0; + out_4955206330411314418[180] = 0.0; + out_4955206330411314418[181] = 0.0; + out_4955206330411314418[182] = 0.0; + out_4955206330411314418[183] = 0.0; + out_4955206330411314418[184] = 0.0; + out_4955206330411314418[185] = 0.0; + out_4955206330411314418[186] = 0.0; + out_4955206330411314418[187] = 0.0; + out_4955206330411314418[188] = 0.0; + out_4955206330411314418[189] = 0.0; + out_4955206330411314418[190] = 1.0; + out_4955206330411314418[191] = 0.0; + out_4955206330411314418[192] = 0.0; + out_4955206330411314418[193] = 0.0; + out_4955206330411314418[194] = 0.0; + out_4955206330411314418[195] = 0.0; + out_4955206330411314418[196] = 0.0; + out_4955206330411314418[197] = 0.0; + out_4955206330411314418[198] = 0.0; + out_4955206330411314418[199] = 0.0; + out_4955206330411314418[200] = 0.0; + out_4955206330411314418[201] = 0.0; + out_4955206330411314418[202] = 0.0; + out_4955206330411314418[203] = 0.0; + out_4955206330411314418[204] = 0.0; + out_4955206330411314418[205] = 0.0; + out_4955206330411314418[206] = 0.0; + out_4955206330411314418[207] = 0.0; + out_4955206330411314418[208] = 0.0; + out_4955206330411314418[209] = 1.0; + out_4955206330411314418[210] = 0.0; + out_4955206330411314418[211] = 0.0; + out_4955206330411314418[212] = 0.0; + out_4955206330411314418[213] = 0.0; + out_4955206330411314418[214] = 0.0; + out_4955206330411314418[215] = 0.0; + out_4955206330411314418[216] = 0.0; + out_4955206330411314418[217] = 0.0; + out_4955206330411314418[218] = 0.0; + out_4955206330411314418[219] = 0.0; + out_4955206330411314418[220] = 0.0; + out_4955206330411314418[221] = 0.0; + out_4955206330411314418[222] = 0.0; + out_4955206330411314418[223] = 0.0; + out_4955206330411314418[224] = 0.0; + out_4955206330411314418[225] = 0.0; + out_4955206330411314418[226] = 0.0; + out_4955206330411314418[227] = 0.0; + out_4955206330411314418[228] = 1.0; + out_4955206330411314418[229] = 0.0; + out_4955206330411314418[230] = 0.0; + out_4955206330411314418[231] = 0.0; + out_4955206330411314418[232] = 0.0; + out_4955206330411314418[233] = 0.0; + out_4955206330411314418[234] = 0.0; + out_4955206330411314418[235] = 0.0; + out_4955206330411314418[236] = 0.0; + out_4955206330411314418[237] = 0.0; + out_4955206330411314418[238] = 0.0; + out_4955206330411314418[239] = 0.0; + out_4955206330411314418[240] = 0.0; + out_4955206330411314418[241] = 0.0; + out_4955206330411314418[242] = 0.0; + out_4955206330411314418[243] = 0.0; + out_4955206330411314418[244] = 0.0; + out_4955206330411314418[245] = 0.0; + out_4955206330411314418[246] = 0.0; + out_4955206330411314418[247] = 1.0; + out_4955206330411314418[248] = 0.0; + out_4955206330411314418[249] = 0.0; + out_4955206330411314418[250] = 0.0; + out_4955206330411314418[251] = 0.0; + out_4955206330411314418[252] = 0.0; + out_4955206330411314418[253] = 0.0; + out_4955206330411314418[254] = 0.0; + out_4955206330411314418[255] = 0.0; + out_4955206330411314418[256] = 0.0; + out_4955206330411314418[257] = 0.0; + out_4955206330411314418[258] = 0.0; + out_4955206330411314418[259] = 0.0; + out_4955206330411314418[260] = 0.0; + out_4955206330411314418[261] = 0.0; + out_4955206330411314418[262] = 0.0; + out_4955206330411314418[263] = 0.0; + out_4955206330411314418[264] = 0.0; + out_4955206330411314418[265] = 0.0; + out_4955206330411314418[266] = 1.0; + out_4955206330411314418[267] = 0.0; + out_4955206330411314418[268] = 0.0; + out_4955206330411314418[269] = 0.0; + out_4955206330411314418[270] = 0.0; + out_4955206330411314418[271] = 0.0; + out_4955206330411314418[272] = 0.0; + out_4955206330411314418[273] = 0.0; + out_4955206330411314418[274] = 0.0; + out_4955206330411314418[275] = 0.0; + out_4955206330411314418[276] = 0.0; + out_4955206330411314418[277] = 0.0; + out_4955206330411314418[278] = 0.0; + out_4955206330411314418[279] = 0.0; + out_4955206330411314418[280] = 0.0; + out_4955206330411314418[281] = 0.0; + out_4955206330411314418[282] = 0.0; + out_4955206330411314418[283] = 0.0; + out_4955206330411314418[284] = 0.0; + out_4955206330411314418[285] = 1.0; + out_4955206330411314418[286] = 0.0; + out_4955206330411314418[287] = 0.0; + out_4955206330411314418[288] = 0.0; + out_4955206330411314418[289] = 0.0; + out_4955206330411314418[290] = 0.0; + out_4955206330411314418[291] = 0.0; + out_4955206330411314418[292] = 0.0; + out_4955206330411314418[293] = 0.0; + out_4955206330411314418[294] = 0.0; + out_4955206330411314418[295] = 0.0; + out_4955206330411314418[296] = 0.0; + out_4955206330411314418[297] = 0.0; + out_4955206330411314418[298] = 0.0; + out_4955206330411314418[299] = 0.0; + out_4955206330411314418[300] = 0.0; + out_4955206330411314418[301] = 0.0; + out_4955206330411314418[302] = 0.0; + out_4955206330411314418[303] = 0.0; + out_4955206330411314418[304] = 1.0; + out_4955206330411314418[305] = 0.0; + out_4955206330411314418[306] = 0.0; + out_4955206330411314418[307] = 0.0; + out_4955206330411314418[308] = 0.0; + out_4955206330411314418[309] = 0.0; + out_4955206330411314418[310] = 0.0; + out_4955206330411314418[311] = 0.0; + out_4955206330411314418[312] = 0.0; + out_4955206330411314418[313] = 0.0; + out_4955206330411314418[314] = 0.0; + out_4955206330411314418[315] = 0.0; + out_4955206330411314418[316] = 0.0; + out_4955206330411314418[317] = 0.0; + out_4955206330411314418[318] = 0.0; + out_4955206330411314418[319] = 0.0; + out_4955206330411314418[320] = 0.0; + out_4955206330411314418[321] = 0.0; + out_4955206330411314418[322] = 0.0; + out_4955206330411314418[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4622611615613940133) { - out_4622611615613940133[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_4622611615613940133[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_4622611615613940133[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_4622611615613940133[3] = dt*state[12] + state[3]; - out_4622611615613940133[4] = dt*state[13] + state[4]; - out_4622611615613940133[5] = dt*state[14] + state[5]; - out_4622611615613940133[6] = state[6]; - out_4622611615613940133[7] = state[7]; - out_4622611615613940133[8] = state[8]; - out_4622611615613940133[9] = state[9]; - out_4622611615613940133[10] = state[10]; - out_4622611615613940133[11] = state[11]; - out_4622611615613940133[12] = state[12]; - out_4622611615613940133[13] = state[13]; - out_4622611615613940133[14] = state[14]; - out_4622611615613940133[15] = state[15]; - out_4622611615613940133[16] = state[16]; - out_4622611615613940133[17] = state[17]; +void f_fun(double *state, double dt, double *out_1052805527796842003) { + out_1052805527796842003[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_1052805527796842003[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_1052805527796842003[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_1052805527796842003[3] = dt*state[12] + state[3]; + out_1052805527796842003[4] = dt*state[13] + state[4]; + out_1052805527796842003[5] = dt*state[14] + state[5]; + out_1052805527796842003[6] = state[6]; + out_1052805527796842003[7] = state[7]; + out_1052805527796842003[8] = state[8]; + out_1052805527796842003[9] = state[9]; + out_1052805527796842003[10] = state[10]; + out_1052805527796842003[11] = state[11]; + out_1052805527796842003[12] = state[12]; + out_1052805527796842003[13] = state[13]; + out_1052805527796842003[14] = state[14]; + out_1052805527796842003[15] = state[15]; + out_1052805527796842003[16] = state[16]; + out_1052805527796842003[17] = state[17]; } -void F_fun(double *state, double dt, double *out_542246045313485722) { - out_542246045313485722[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_542246045313485722[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_542246045313485722[2] = 0; - out_542246045313485722[3] = 0; - out_542246045313485722[4] = 0; - out_542246045313485722[5] = 0; - out_542246045313485722[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_542246045313485722[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_542246045313485722[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_542246045313485722[9] = 0; - out_542246045313485722[10] = 0; - out_542246045313485722[11] = 0; - out_542246045313485722[12] = 0; - out_542246045313485722[13] = 0; - out_542246045313485722[14] = 0; - out_542246045313485722[15] = 0; - out_542246045313485722[16] = 0; - out_542246045313485722[17] = 0; - out_542246045313485722[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_542246045313485722[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_542246045313485722[20] = 0; - out_542246045313485722[21] = 0; - out_542246045313485722[22] = 0; - out_542246045313485722[23] = 0; - out_542246045313485722[24] = 0; - out_542246045313485722[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_542246045313485722[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_542246045313485722[27] = 0; - out_542246045313485722[28] = 0; - out_542246045313485722[29] = 0; - out_542246045313485722[30] = 0; - out_542246045313485722[31] = 0; - out_542246045313485722[32] = 0; - out_542246045313485722[33] = 0; - out_542246045313485722[34] = 0; - out_542246045313485722[35] = 0; - out_542246045313485722[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_542246045313485722[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_542246045313485722[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_542246045313485722[39] = 0; - out_542246045313485722[40] = 0; - out_542246045313485722[41] = 0; - out_542246045313485722[42] = 0; - out_542246045313485722[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_542246045313485722[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_542246045313485722[45] = 0; - out_542246045313485722[46] = 0; - out_542246045313485722[47] = 0; - out_542246045313485722[48] = 0; - out_542246045313485722[49] = 0; - out_542246045313485722[50] = 0; - out_542246045313485722[51] = 0; - out_542246045313485722[52] = 0; - out_542246045313485722[53] = 0; - out_542246045313485722[54] = 0; - out_542246045313485722[55] = 0; - out_542246045313485722[56] = 0; - out_542246045313485722[57] = 1; - out_542246045313485722[58] = 0; - out_542246045313485722[59] = 0; - out_542246045313485722[60] = 0; - out_542246045313485722[61] = 0; - out_542246045313485722[62] = 0; - out_542246045313485722[63] = 0; - out_542246045313485722[64] = 0; - out_542246045313485722[65] = 0; - out_542246045313485722[66] = dt; - out_542246045313485722[67] = 0; - out_542246045313485722[68] = 0; - out_542246045313485722[69] = 0; - out_542246045313485722[70] = 0; - out_542246045313485722[71] = 0; - out_542246045313485722[72] = 0; - out_542246045313485722[73] = 0; - out_542246045313485722[74] = 0; - out_542246045313485722[75] = 0; - out_542246045313485722[76] = 1; - out_542246045313485722[77] = 0; - out_542246045313485722[78] = 0; - out_542246045313485722[79] = 0; - out_542246045313485722[80] = 0; - out_542246045313485722[81] = 0; - out_542246045313485722[82] = 0; - out_542246045313485722[83] = 0; - out_542246045313485722[84] = 0; - out_542246045313485722[85] = dt; - out_542246045313485722[86] = 0; - out_542246045313485722[87] = 0; - out_542246045313485722[88] = 0; - out_542246045313485722[89] = 0; - out_542246045313485722[90] = 0; - out_542246045313485722[91] = 0; - out_542246045313485722[92] = 0; - out_542246045313485722[93] = 0; - out_542246045313485722[94] = 0; - out_542246045313485722[95] = 1; - out_542246045313485722[96] = 0; - out_542246045313485722[97] = 0; - out_542246045313485722[98] = 0; - out_542246045313485722[99] = 0; - out_542246045313485722[100] = 0; - out_542246045313485722[101] = 0; - out_542246045313485722[102] = 0; - out_542246045313485722[103] = 0; - out_542246045313485722[104] = dt; - out_542246045313485722[105] = 0; - out_542246045313485722[106] = 0; - out_542246045313485722[107] = 0; - out_542246045313485722[108] = 0; - out_542246045313485722[109] = 0; - out_542246045313485722[110] = 0; - out_542246045313485722[111] = 0; - out_542246045313485722[112] = 0; - out_542246045313485722[113] = 0; - out_542246045313485722[114] = 1; - out_542246045313485722[115] = 0; - out_542246045313485722[116] = 0; - out_542246045313485722[117] = 0; - out_542246045313485722[118] = 0; - out_542246045313485722[119] = 0; - out_542246045313485722[120] = 0; - out_542246045313485722[121] = 0; - out_542246045313485722[122] = 0; - out_542246045313485722[123] = 0; - out_542246045313485722[124] = 0; - out_542246045313485722[125] = 0; - out_542246045313485722[126] = 0; - out_542246045313485722[127] = 0; - out_542246045313485722[128] = 0; - out_542246045313485722[129] = 0; - out_542246045313485722[130] = 0; - out_542246045313485722[131] = 0; - out_542246045313485722[132] = 0; - out_542246045313485722[133] = 1; - out_542246045313485722[134] = 0; - out_542246045313485722[135] = 0; - out_542246045313485722[136] = 0; - out_542246045313485722[137] = 0; - out_542246045313485722[138] = 0; - out_542246045313485722[139] = 0; - out_542246045313485722[140] = 0; - out_542246045313485722[141] = 0; - out_542246045313485722[142] = 0; - out_542246045313485722[143] = 0; - out_542246045313485722[144] = 0; - out_542246045313485722[145] = 0; - out_542246045313485722[146] = 0; - out_542246045313485722[147] = 0; - out_542246045313485722[148] = 0; - out_542246045313485722[149] = 0; - out_542246045313485722[150] = 0; - out_542246045313485722[151] = 0; - out_542246045313485722[152] = 1; - out_542246045313485722[153] = 0; - out_542246045313485722[154] = 0; - out_542246045313485722[155] = 0; - out_542246045313485722[156] = 0; - out_542246045313485722[157] = 0; - out_542246045313485722[158] = 0; - out_542246045313485722[159] = 0; - out_542246045313485722[160] = 0; - out_542246045313485722[161] = 0; - out_542246045313485722[162] = 0; - out_542246045313485722[163] = 0; - out_542246045313485722[164] = 0; - out_542246045313485722[165] = 0; - out_542246045313485722[166] = 0; - out_542246045313485722[167] = 0; - out_542246045313485722[168] = 0; - out_542246045313485722[169] = 0; - out_542246045313485722[170] = 0; - out_542246045313485722[171] = 1; - out_542246045313485722[172] = 0; - out_542246045313485722[173] = 0; - out_542246045313485722[174] = 0; - out_542246045313485722[175] = 0; - out_542246045313485722[176] = 0; - out_542246045313485722[177] = 0; - out_542246045313485722[178] = 0; - out_542246045313485722[179] = 0; - out_542246045313485722[180] = 0; - out_542246045313485722[181] = 0; - out_542246045313485722[182] = 0; - out_542246045313485722[183] = 0; - out_542246045313485722[184] = 0; - out_542246045313485722[185] = 0; - out_542246045313485722[186] = 0; - out_542246045313485722[187] = 0; - out_542246045313485722[188] = 0; - out_542246045313485722[189] = 0; - out_542246045313485722[190] = 1; - out_542246045313485722[191] = 0; - out_542246045313485722[192] = 0; - out_542246045313485722[193] = 0; - out_542246045313485722[194] = 0; - out_542246045313485722[195] = 0; - out_542246045313485722[196] = 0; - out_542246045313485722[197] = 0; - out_542246045313485722[198] = 0; - out_542246045313485722[199] = 0; - out_542246045313485722[200] = 0; - out_542246045313485722[201] = 0; - out_542246045313485722[202] = 0; - out_542246045313485722[203] = 0; - out_542246045313485722[204] = 0; - out_542246045313485722[205] = 0; - out_542246045313485722[206] = 0; - out_542246045313485722[207] = 0; - out_542246045313485722[208] = 0; - out_542246045313485722[209] = 1; - out_542246045313485722[210] = 0; - out_542246045313485722[211] = 0; - out_542246045313485722[212] = 0; - out_542246045313485722[213] = 0; - out_542246045313485722[214] = 0; - out_542246045313485722[215] = 0; - out_542246045313485722[216] = 0; - out_542246045313485722[217] = 0; - out_542246045313485722[218] = 0; - out_542246045313485722[219] = 0; - out_542246045313485722[220] = 0; - out_542246045313485722[221] = 0; - out_542246045313485722[222] = 0; - out_542246045313485722[223] = 0; - out_542246045313485722[224] = 0; - out_542246045313485722[225] = 0; - out_542246045313485722[226] = 0; - out_542246045313485722[227] = 0; - out_542246045313485722[228] = 1; - out_542246045313485722[229] = 0; - out_542246045313485722[230] = 0; - out_542246045313485722[231] = 0; - out_542246045313485722[232] = 0; - out_542246045313485722[233] = 0; - out_542246045313485722[234] = 0; - out_542246045313485722[235] = 0; - out_542246045313485722[236] = 0; - out_542246045313485722[237] = 0; - out_542246045313485722[238] = 0; - out_542246045313485722[239] = 0; - out_542246045313485722[240] = 0; - out_542246045313485722[241] = 0; - out_542246045313485722[242] = 0; - out_542246045313485722[243] = 0; - out_542246045313485722[244] = 0; - out_542246045313485722[245] = 0; - out_542246045313485722[246] = 0; - out_542246045313485722[247] = 1; - out_542246045313485722[248] = 0; - out_542246045313485722[249] = 0; - out_542246045313485722[250] = 0; - out_542246045313485722[251] = 0; - out_542246045313485722[252] = 0; - out_542246045313485722[253] = 0; - out_542246045313485722[254] = 0; - out_542246045313485722[255] = 0; - out_542246045313485722[256] = 0; - out_542246045313485722[257] = 0; - out_542246045313485722[258] = 0; - out_542246045313485722[259] = 0; - out_542246045313485722[260] = 0; - out_542246045313485722[261] = 0; - out_542246045313485722[262] = 0; - out_542246045313485722[263] = 0; - out_542246045313485722[264] = 0; - out_542246045313485722[265] = 0; - out_542246045313485722[266] = 1; - out_542246045313485722[267] = 0; - out_542246045313485722[268] = 0; - out_542246045313485722[269] = 0; - out_542246045313485722[270] = 0; - out_542246045313485722[271] = 0; - out_542246045313485722[272] = 0; - out_542246045313485722[273] = 0; - out_542246045313485722[274] = 0; - out_542246045313485722[275] = 0; - out_542246045313485722[276] = 0; - out_542246045313485722[277] = 0; - out_542246045313485722[278] = 0; - out_542246045313485722[279] = 0; - out_542246045313485722[280] = 0; - out_542246045313485722[281] = 0; - out_542246045313485722[282] = 0; - out_542246045313485722[283] = 0; - out_542246045313485722[284] = 0; - out_542246045313485722[285] = 1; - out_542246045313485722[286] = 0; - out_542246045313485722[287] = 0; - out_542246045313485722[288] = 0; - out_542246045313485722[289] = 0; - out_542246045313485722[290] = 0; - out_542246045313485722[291] = 0; - out_542246045313485722[292] = 0; - out_542246045313485722[293] = 0; - out_542246045313485722[294] = 0; - out_542246045313485722[295] = 0; - out_542246045313485722[296] = 0; - out_542246045313485722[297] = 0; - out_542246045313485722[298] = 0; - out_542246045313485722[299] = 0; - out_542246045313485722[300] = 0; - out_542246045313485722[301] = 0; - out_542246045313485722[302] = 0; - out_542246045313485722[303] = 0; - out_542246045313485722[304] = 1; - out_542246045313485722[305] = 0; - out_542246045313485722[306] = 0; - out_542246045313485722[307] = 0; - out_542246045313485722[308] = 0; - out_542246045313485722[309] = 0; - out_542246045313485722[310] = 0; - out_542246045313485722[311] = 0; - out_542246045313485722[312] = 0; - out_542246045313485722[313] = 0; - out_542246045313485722[314] = 0; - out_542246045313485722[315] = 0; - out_542246045313485722[316] = 0; - out_542246045313485722[317] = 0; - out_542246045313485722[318] = 0; - out_542246045313485722[319] = 0; - out_542246045313485722[320] = 0; - out_542246045313485722[321] = 0; - out_542246045313485722[322] = 0; - out_542246045313485722[323] = 1; +void F_fun(double *state, double dt, double *out_5871869036335399812) { + out_5871869036335399812[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_5871869036335399812[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_5871869036335399812[2] = 0; + out_5871869036335399812[3] = 0; + out_5871869036335399812[4] = 0; + out_5871869036335399812[5] = 0; + out_5871869036335399812[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_5871869036335399812[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_5871869036335399812[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_5871869036335399812[9] = 0; + out_5871869036335399812[10] = 0; + out_5871869036335399812[11] = 0; + out_5871869036335399812[12] = 0; + out_5871869036335399812[13] = 0; + out_5871869036335399812[14] = 0; + out_5871869036335399812[15] = 0; + out_5871869036335399812[16] = 0; + out_5871869036335399812[17] = 0; + out_5871869036335399812[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_5871869036335399812[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_5871869036335399812[20] = 0; + out_5871869036335399812[21] = 0; + out_5871869036335399812[22] = 0; + out_5871869036335399812[23] = 0; + out_5871869036335399812[24] = 0; + out_5871869036335399812[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_5871869036335399812[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_5871869036335399812[27] = 0; + out_5871869036335399812[28] = 0; + out_5871869036335399812[29] = 0; + out_5871869036335399812[30] = 0; + out_5871869036335399812[31] = 0; + out_5871869036335399812[32] = 0; + out_5871869036335399812[33] = 0; + out_5871869036335399812[34] = 0; + out_5871869036335399812[35] = 0; + out_5871869036335399812[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_5871869036335399812[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_5871869036335399812[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_5871869036335399812[39] = 0; + out_5871869036335399812[40] = 0; + out_5871869036335399812[41] = 0; + out_5871869036335399812[42] = 0; + out_5871869036335399812[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_5871869036335399812[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_5871869036335399812[45] = 0; + out_5871869036335399812[46] = 0; + out_5871869036335399812[47] = 0; + out_5871869036335399812[48] = 0; + out_5871869036335399812[49] = 0; + out_5871869036335399812[50] = 0; + out_5871869036335399812[51] = 0; + out_5871869036335399812[52] = 0; + out_5871869036335399812[53] = 0; + out_5871869036335399812[54] = 0; + out_5871869036335399812[55] = 0; + out_5871869036335399812[56] = 0; + out_5871869036335399812[57] = 1; + out_5871869036335399812[58] = 0; + out_5871869036335399812[59] = 0; + out_5871869036335399812[60] = 0; + out_5871869036335399812[61] = 0; + out_5871869036335399812[62] = 0; + out_5871869036335399812[63] = 0; + out_5871869036335399812[64] = 0; + out_5871869036335399812[65] = 0; + out_5871869036335399812[66] = dt; + out_5871869036335399812[67] = 0; + out_5871869036335399812[68] = 0; + out_5871869036335399812[69] = 0; + out_5871869036335399812[70] = 0; + out_5871869036335399812[71] = 0; + out_5871869036335399812[72] = 0; + out_5871869036335399812[73] = 0; + out_5871869036335399812[74] = 0; + out_5871869036335399812[75] = 0; + out_5871869036335399812[76] = 1; + out_5871869036335399812[77] = 0; + out_5871869036335399812[78] = 0; + out_5871869036335399812[79] = 0; + out_5871869036335399812[80] = 0; + out_5871869036335399812[81] = 0; + out_5871869036335399812[82] = 0; + out_5871869036335399812[83] = 0; + out_5871869036335399812[84] = 0; + out_5871869036335399812[85] = dt; + out_5871869036335399812[86] = 0; + out_5871869036335399812[87] = 0; + out_5871869036335399812[88] = 0; + out_5871869036335399812[89] = 0; + out_5871869036335399812[90] = 0; + out_5871869036335399812[91] = 0; + out_5871869036335399812[92] = 0; + out_5871869036335399812[93] = 0; + out_5871869036335399812[94] = 0; + out_5871869036335399812[95] = 1; + out_5871869036335399812[96] = 0; + out_5871869036335399812[97] = 0; + out_5871869036335399812[98] = 0; + out_5871869036335399812[99] = 0; + out_5871869036335399812[100] = 0; + out_5871869036335399812[101] = 0; + out_5871869036335399812[102] = 0; + out_5871869036335399812[103] = 0; + out_5871869036335399812[104] = dt; + out_5871869036335399812[105] = 0; + out_5871869036335399812[106] = 0; + out_5871869036335399812[107] = 0; + out_5871869036335399812[108] = 0; + out_5871869036335399812[109] = 0; + out_5871869036335399812[110] = 0; + out_5871869036335399812[111] = 0; + out_5871869036335399812[112] = 0; + out_5871869036335399812[113] = 0; + out_5871869036335399812[114] = 1; + out_5871869036335399812[115] = 0; + out_5871869036335399812[116] = 0; + out_5871869036335399812[117] = 0; + out_5871869036335399812[118] = 0; + out_5871869036335399812[119] = 0; + out_5871869036335399812[120] = 0; + out_5871869036335399812[121] = 0; + out_5871869036335399812[122] = 0; + out_5871869036335399812[123] = 0; + out_5871869036335399812[124] = 0; + out_5871869036335399812[125] = 0; + out_5871869036335399812[126] = 0; + out_5871869036335399812[127] = 0; + out_5871869036335399812[128] = 0; + out_5871869036335399812[129] = 0; + out_5871869036335399812[130] = 0; + out_5871869036335399812[131] = 0; + out_5871869036335399812[132] = 0; + out_5871869036335399812[133] = 1; + out_5871869036335399812[134] = 0; + out_5871869036335399812[135] = 0; + out_5871869036335399812[136] = 0; + out_5871869036335399812[137] = 0; + out_5871869036335399812[138] = 0; + out_5871869036335399812[139] = 0; + out_5871869036335399812[140] = 0; + out_5871869036335399812[141] = 0; + out_5871869036335399812[142] = 0; + out_5871869036335399812[143] = 0; + out_5871869036335399812[144] = 0; + out_5871869036335399812[145] = 0; + out_5871869036335399812[146] = 0; + out_5871869036335399812[147] = 0; + out_5871869036335399812[148] = 0; + out_5871869036335399812[149] = 0; + out_5871869036335399812[150] = 0; + out_5871869036335399812[151] = 0; + out_5871869036335399812[152] = 1; + out_5871869036335399812[153] = 0; + out_5871869036335399812[154] = 0; + out_5871869036335399812[155] = 0; + out_5871869036335399812[156] = 0; + out_5871869036335399812[157] = 0; + out_5871869036335399812[158] = 0; + out_5871869036335399812[159] = 0; + out_5871869036335399812[160] = 0; + out_5871869036335399812[161] = 0; + out_5871869036335399812[162] = 0; + out_5871869036335399812[163] = 0; + out_5871869036335399812[164] = 0; + out_5871869036335399812[165] = 0; + out_5871869036335399812[166] = 0; + out_5871869036335399812[167] = 0; + out_5871869036335399812[168] = 0; + out_5871869036335399812[169] = 0; + out_5871869036335399812[170] = 0; + out_5871869036335399812[171] = 1; + out_5871869036335399812[172] = 0; + out_5871869036335399812[173] = 0; + out_5871869036335399812[174] = 0; + out_5871869036335399812[175] = 0; + out_5871869036335399812[176] = 0; + out_5871869036335399812[177] = 0; + out_5871869036335399812[178] = 0; + out_5871869036335399812[179] = 0; + out_5871869036335399812[180] = 0; + out_5871869036335399812[181] = 0; + out_5871869036335399812[182] = 0; + out_5871869036335399812[183] = 0; + out_5871869036335399812[184] = 0; + out_5871869036335399812[185] = 0; + out_5871869036335399812[186] = 0; + out_5871869036335399812[187] = 0; + out_5871869036335399812[188] = 0; + out_5871869036335399812[189] = 0; + out_5871869036335399812[190] = 1; + out_5871869036335399812[191] = 0; + out_5871869036335399812[192] = 0; + out_5871869036335399812[193] = 0; + out_5871869036335399812[194] = 0; + out_5871869036335399812[195] = 0; + out_5871869036335399812[196] = 0; + out_5871869036335399812[197] = 0; + out_5871869036335399812[198] = 0; + out_5871869036335399812[199] = 0; + out_5871869036335399812[200] = 0; + out_5871869036335399812[201] = 0; + out_5871869036335399812[202] = 0; + out_5871869036335399812[203] = 0; + out_5871869036335399812[204] = 0; + out_5871869036335399812[205] = 0; + out_5871869036335399812[206] = 0; + out_5871869036335399812[207] = 0; + out_5871869036335399812[208] = 0; + out_5871869036335399812[209] = 1; + out_5871869036335399812[210] = 0; + out_5871869036335399812[211] = 0; + out_5871869036335399812[212] = 0; + out_5871869036335399812[213] = 0; + out_5871869036335399812[214] = 0; + out_5871869036335399812[215] = 0; + out_5871869036335399812[216] = 0; + out_5871869036335399812[217] = 0; + out_5871869036335399812[218] = 0; + out_5871869036335399812[219] = 0; + out_5871869036335399812[220] = 0; + out_5871869036335399812[221] = 0; + out_5871869036335399812[222] = 0; + out_5871869036335399812[223] = 0; + out_5871869036335399812[224] = 0; + out_5871869036335399812[225] = 0; + out_5871869036335399812[226] = 0; + out_5871869036335399812[227] = 0; + out_5871869036335399812[228] = 1; + out_5871869036335399812[229] = 0; + out_5871869036335399812[230] = 0; + out_5871869036335399812[231] = 0; + out_5871869036335399812[232] = 0; + out_5871869036335399812[233] = 0; + out_5871869036335399812[234] = 0; + out_5871869036335399812[235] = 0; + out_5871869036335399812[236] = 0; + out_5871869036335399812[237] = 0; + out_5871869036335399812[238] = 0; + out_5871869036335399812[239] = 0; + out_5871869036335399812[240] = 0; + out_5871869036335399812[241] = 0; + out_5871869036335399812[242] = 0; + out_5871869036335399812[243] = 0; + out_5871869036335399812[244] = 0; + out_5871869036335399812[245] = 0; + out_5871869036335399812[246] = 0; + out_5871869036335399812[247] = 1; + out_5871869036335399812[248] = 0; + out_5871869036335399812[249] = 0; + out_5871869036335399812[250] = 0; + out_5871869036335399812[251] = 0; + out_5871869036335399812[252] = 0; + out_5871869036335399812[253] = 0; + out_5871869036335399812[254] = 0; + out_5871869036335399812[255] = 0; + out_5871869036335399812[256] = 0; + out_5871869036335399812[257] = 0; + out_5871869036335399812[258] = 0; + out_5871869036335399812[259] = 0; + out_5871869036335399812[260] = 0; + out_5871869036335399812[261] = 0; + out_5871869036335399812[262] = 0; + out_5871869036335399812[263] = 0; + out_5871869036335399812[264] = 0; + out_5871869036335399812[265] = 0; + out_5871869036335399812[266] = 1; + out_5871869036335399812[267] = 0; + out_5871869036335399812[268] = 0; + out_5871869036335399812[269] = 0; + out_5871869036335399812[270] = 0; + out_5871869036335399812[271] = 0; + out_5871869036335399812[272] = 0; + out_5871869036335399812[273] = 0; + out_5871869036335399812[274] = 0; + out_5871869036335399812[275] = 0; + out_5871869036335399812[276] = 0; + out_5871869036335399812[277] = 0; + out_5871869036335399812[278] = 0; + out_5871869036335399812[279] = 0; + out_5871869036335399812[280] = 0; + out_5871869036335399812[281] = 0; + out_5871869036335399812[282] = 0; + out_5871869036335399812[283] = 0; + out_5871869036335399812[284] = 0; + out_5871869036335399812[285] = 1; + out_5871869036335399812[286] = 0; + out_5871869036335399812[287] = 0; + out_5871869036335399812[288] = 0; + out_5871869036335399812[289] = 0; + out_5871869036335399812[290] = 0; + out_5871869036335399812[291] = 0; + out_5871869036335399812[292] = 0; + out_5871869036335399812[293] = 0; + out_5871869036335399812[294] = 0; + out_5871869036335399812[295] = 0; + out_5871869036335399812[296] = 0; + out_5871869036335399812[297] = 0; + out_5871869036335399812[298] = 0; + out_5871869036335399812[299] = 0; + out_5871869036335399812[300] = 0; + out_5871869036335399812[301] = 0; + out_5871869036335399812[302] = 0; + out_5871869036335399812[303] = 0; + out_5871869036335399812[304] = 1; + out_5871869036335399812[305] = 0; + out_5871869036335399812[306] = 0; + out_5871869036335399812[307] = 0; + out_5871869036335399812[308] = 0; + out_5871869036335399812[309] = 0; + out_5871869036335399812[310] = 0; + out_5871869036335399812[311] = 0; + out_5871869036335399812[312] = 0; + out_5871869036335399812[313] = 0; + out_5871869036335399812[314] = 0; + out_5871869036335399812[315] = 0; + out_5871869036335399812[316] = 0; + out_5871869036335399812[317] = 0; + out_5871869036335399812[318] = 0; + out_5871869036335399812[319] = 0; + out_5871869036335399812[320] = 0; + out_5871869036335399812[321] = 0; + out_5871869036335399812[322] = 0; + out_5871869036335399812[323] = 1; } -void h_4(double *state, double *unused, double *out_6293306196609579881) { - out_6293306196609579881[0] = state[6] + state[9]; - out_6293306196609579881[1] = state[7] + state[10]; - out_6293306196609579881[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_921118668651624411) { + out_921118668651624411[0] = state[6] + state[9]; + out_921118668651624411[1] = state[7] + state[10]; + out_921118668651624411[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_1878511310400889359) { - out_1878511310400889359[0] = 0; - out_1878511310400889359[1] = 0; - out_1878511310400889359[2] = 0; - out_1878511310400889359[3] = 0; - out_1878511310400889359[4] = 0; - out_1878511310400889359[5] = 0; - out_1878511310400889359[6] = 1; - out_1878511310400889359[7] = 0; - out_1878511310400889359[8] = 0; - out_1878511310400889359[9] = 1; - out_1878511310400889359[10] = 0; - out_1878511310400889359[11] = 0; - out_1878511310400889359[12] = 0; - out_1878511310400889359[13] = 0; - out_1878511310400889359[14] = 0; - out_1878511310400889359[15] = 0; - out_1878511310400889359[16] = 0; - out_1878511310400889359[17] = 0; - out_1878511310400889359[18] = 0; - out_1878511310400889359[19] = 0; - out_1878511310400889359[20] = 0; - out_1878511310400889359[21] = 0; - out_1878511310400889359[22] = 0; - out_1878511310400889359[23] = 0; - out_1878511310400889359[24] = 0; - out_1878511310400889359[25] = 1; - out_1878511310400889359[26] = 0; - out_1878511310400889359[27] = 0; - out_1878511310400889359[28] = 1; - out_1878511310400889359[29] = 0; - out_1878511310400889359[30] = 0; - out_1878511310400889359[31] = 0; - out_1878511310400889359[32] = 0; - out_1878511310400889359[33] = 0; - out_1878511310400889359[34] = 0; - out_1878511310400889359[35] = 0; - out_1878511310400889359[36] = 0; - out_1878511310400889359[37] = 0; - out_1878511310400889359[38] = 0; - out_1878511310400889359[39] = 0; - out_1878511310400889359[40] = 0; - out_1878511310400889359[41] = 0; - out_1878511310400889359[42] = 0; - out_1878511310400889359[43] = 0; - out_1878511310400889359[44] = 1; - out_1878511310400889359[45] = 0; - out_1878511310400889359[46] = 0; - out_1878511310400889359[47] = 1; - out_1878511310400889359[48] = 0; - out_1878511310400889359[49] = 0; - out_1878511310400889359[50] = 0; - out_1878511310400889359[51] = 0; - out_1878511310400889359[52] = 0; - out_1878511310400889359[53] = 0; +void H_4(double *state, double *unused, double *out_3989155335646506924) { + out_3989155335646506924[0] = 0; + out_3989155335646506924[1] = 0; + out_3989155335646506924[2] = 0; + out_3989155335646506924[3] = 0; + out_3989155335646506924[4] = 0; + out_3989155335646506924[5] = 0; + out_3989155335646506924[6] = 1; + out_3989155335646506924[7] = 0; + out_3989155335646506924[8] = 0; + out_3989155335646506924[9] = 1; + out_3989155335646506924[10] = 0; + out_3989155335646506924[11] = 0; + out_3989155335646506924[12] = 0; + out_3989155335646506924[13] = 0; + out_3989155335646506924[14] = 0; + out_3989155335646506924[15] = 0; + out_3989155335646506924[16] = 0; + out_3989155335646506924[17] = 0; + out_3989155335646506924[18] = 0; + out_3989155335646506924[19] = 0; + out_3989155335646506924[20] = 0; + out_3989155335646506924[21] = 0; + out_3989155335646506924[22] = 0; + out_3989155335646506924[23] = 0; + out_3989155335646506924[24] = 0; + out_3989155335646506924[25] = 1; + out_3989155335646506924[26] = 0; + out_3989155335646506924[27] = 0; + out_3989155335646506924[28] = 1; + out_3989155335646506924[29] = 0; + out_3989155335646506924[30] = 0; + out_3989155335646506924[31] = 0; + out_3989155335646506924[32] = 0; + out_3989155335646506924[33] = 0; + out_3989155335646506924[34] = 0; + out_3989155335646506924[35] = 0; + out_3989155335646506924[36] = 0; + out_3989155335646506924[37] = 0; + out_3989155335646506924[38] = 0; + out_3989155335646506924[39] = 0; + out_3989155335646506924[40] = 0; + out_3989155335646506924[41] = 0; + out_3989155335646506924[42] = 0; + out_3989155335646506924[43] = 0; + out_3989155335646506924[44] = 1; + out_3989155335646506924[45] = 0; + out_3989155335646506924[46] = 0; + out_3989155335646506924[47] = 1; + out_3989155335646506924[48] = 0; + out_3989155335646506924[49] = 0; + out_3989155335646506924[50] = 0; + out_3989155335646506924[51] = 0; + out_3989155335646506924[52] = 0; + out_3989155335646506924[53] = 0; } -void h_10(double *state, double *unused, double *out_8801939057301033560) { - out_8801939057301033560[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_8801939057301033560[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_8801939057301033560[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_8738369372345178390) { + out_8738369372345178390[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_8738369372345178390[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_8738369372345178390[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_2335330326985309171) { - out_2335330326985309171[0] = 0; - out_2335330326985309171[1] = 9.8100000000000005*cos(state[1]); - out_2335330326985309171[2] = 0; - out_2335330326985309171[3] = 0; - out_2335330326985309171[4] = -state[8]; - out_2335330326985309171[5] = state[7]; - out_2335330326985309171[6] = 0; - out_2335330326985309171[7] = state[5]; - out_2335330326985309171[8] = -state[4]; - out_2335330326985309171[9] = 0; - out_2335330326985309171[10] = 0; - out_2335330326985309171[11] = 0; - out_2335330326985309171[12] = 1; - out_2335330326985309171[13] = 0; - out_2335330326985309171[14] = 0; - out_2335330326985309171[15] = 1; - out_2335330326985309171[16] = 0; - out_2335330326985309171[17] = 0; - out_2335330326985309171[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_2335330326985309171[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_2335330326985309171[20] = 0; - out_2335330326985309171[21] = state[8]; - out_2335330326985309171[22] = 0; - out_2335330326985309171[23] = -state[6]; - out_2335330326985309171[24] = -state[5]; - out_2335330326985309171[25] = 0; - out_2335330326985309171[26] = state[3]; - out_2335330326985309171[27] = 0; - out_2335330326985309171[28] = 0; - out_2335330326985309171[29] = 0; - out_2335330326985309171[30] = 0; - out_2335330326985309171[31] = 1; - out_2335330326985309171[32] = 0; - out_2335330326985309171[33] = 0; - out_2335330326985309171[34] = 1; - out_2335330326985309171[35] = 0; - out_2335330326985309171[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_2335330326985309171[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_2335330326985309171[38] = 0; - out_2335330326985309171[39] = -state[7]; - out_2335330326985309171[40] = state[6]; - out_2335330326985309171[41] = 0; - out_2335330326985309171[42] = state[4]; - out_2335330326985309171[43] = -state[3]; - out_2335330326985309171[44] = 0; - out_2335330326985309171[45] = 0; - out_2335330326985309171[46] = 0; - out_2335330326985309171[47] = 0; - out_2335330326985309171[48] = 0; - out_2335330326985309171[49] = 0; - out_2335330326985309171[50] = 1; - out_2335330326985309171[51] = 0; - out_2335330326985309171[52] = 0; - out_2335330326985309171[53] = 1; +void H_10(double *state, double *unused, double *out_2875729590105375124) { + out_2875729590105375124[0] = 0; + out_2875729590105375124[1] = 9.8100000000000005*cos(state[1]); + out_2875729590105375124[2] = 0; + out_2875729590105375124[3] = 0; + out_2875729590105375124[4] = -state[8]; + out_2875729590105375124[5] = state[7]; + out_2875729590105375124[6] = 0; + out_2875729590105375124[7] = state[5]; + out_2875729590105375124[8] = -state[4]; + out_2875729590105375124[9] = 0; + out_2875729590105375124[10] = 0; + out_2875729590105375124[11] = 0; + out_2875729590105375124[12] = 1; + out_2875729590105375124[13] = 0; + out_2875729590105375124[14] = 0; + out_2875729590105375124[15] = 1; + out_2875729590105375124[16] = 0; + out_2875729590105375124[17] = 0; + out_2875729590105375124[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_2875729590105375124[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_2875729590105375124[20] = 0; + out_2875729590105375124[21] = state[8]; + out_2875729590105375124[22] = 0; + out_2875729590105375124[23] = -state[6]; + out_2875729590105375124[24] = -state[5]; + out_2875729590105375124[25] = 0; + out_2875729590105375124[26] = state[3]; + out_2875729590105375124[27] = 0; + out_2875729590105375124[28] = 0; + out_2875729590105375124[29] = 0; + out_2875729590105375124[30] = 0; + out_2875729590105375124[31] = 1; + out_2875729590105375124[32] = 0; + out_2875729590105375124[33] = 0; + out_2875729590105375124[34] = 1; + out_2875729590105375124[35] = 0; + out_2875729590105375124[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_2875729590105375124[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_2875729590105375124[38] = 0; + out_2875729590105375124[39] = -state[7]; + out_2875729590105375124[40] = state[6]; + out_2875729590105375124[41] = 0; + out_2875729590105375124[42] = state[4]; + out_2875729590105375124[43] = -state[3]; + out_2875729590105375124[44] = 0; + out_2875729590105375124[45] = 0; + out_2875729590105375124[46] = 0; + out_2875729590105375124[47] = 0; + out_2875729590105375124[48] = 0; + out_2875729590105375124[49] = 0; + out_2875729590105375124[50] = 1; + out_2875729590105375124[51] = 0; + out_2875729590105375124[52] = 0; + out_2875729590105375124[53] = 1; } -void h_13(double *state, double *unused, double *out_442641261201264592) { - out_442641261201264592[0] = state[3]; - out_442641261201264592[1] = state[4]; - out_442641261201264592[2] = state[5]; +void h_13(double *state, double *unused, double *out_1766623406694503914) { + out_1766623406694503914[0] = state[3]; + out_1766623406694503914[1] = state[4]; + out_1766623406694503914[2] = state[5]; } -void H_13(double *state, double *unused, double *out_5712266773703413383) { - out_5712266773703413383[0] = 0; - out_5712266773703413383[1] = 0; - out_5712266773703413383[2] = 0; - out_5712266773703413383[3] = 1; - out_5712266773703413383[4] = 0; - out_5712266773703413383[5] = 0; - out_5712266773703413383[6] = 0; - out_5712266773703413383[7] = 0; - out_5712266773703413383[8] = 0; - out_5712266773703413383[9] = 0; - out_5712266773703413383[10] = 0; - out_5712266773703413383[11] = 0; - out_5712266773703413383[12] = 0; - out_5712266773703413383[13] = 0; - out_5712266773703413383[14] = 0; - out_5712266773703413383[15] = 0; - out_5712266773703413383[16] = 0; - out_5712266773703413383[17] = 0; - out_5712266773703413383[18] = 0; - out_5712266773703413383[19] = 0; - out_5712266773703413383[20] = 0; - out_5712266773703413383[21] = 0; - out_5712266773703413383[22] = 1; - out_5712266773703413383[23] = 0; - out_5712266773703413383[24] = 0; - out_5712266773703413383[25] = 0; - out_5712266773703413383[26] = 0; - out_5712266773703413383[27] = 0; - out_5712266773703413383[28] = 0; - out_5712266773703413383[29] = 0; - out_5712266773703413383[30] = 0; - out_5712266773703413383[31] = 0; - out_5712266773703413383[32] = 0; - out_5712266773703413383[33] = 0; - out_5712266773703413383[34] = 0; - out_5712266773703413383[35] = 0; - out_5712266773703413383[36] = 0; - out_5712266773703413383[37] = 0; - out_5712266773703413383[38] = 0; - out_5712266773703413383[39] = 0; - out_5712266773703413383[40] = 0; - out_5712266773703413383[41] = 1; - out_5712266773703413383[42] = 0; - out_5712266773703413383[43] = 0; - out_5712266773703413383[44] = 0; - out_5712266773703413383[45] = 0; - out_5712266773703413383[46] = 0; - out_5712266773703413383[47] = 0; - out_5712266773703413383[48] = 0; - out_5712266773703413383[49] = 0; - out_5712266773703413383[50] = 0; - out_5712266773703413383[51] = 0; - out_5712266773703413383[52] = 0; - out_5712266773703413383[53] = 0; +void H_13(double *state, double *unused, double *out_776881510314174123) { + out_776881510314174123[0] = 0; + out_776881510314174123[1] = 0; + out_776881510314174123[2] = 0; + out_776881510314174123[3] = 1; + out_776881510314174123[4] = 0; + out_776881510314174123[5] = 0; + out_776881510314174123[6] = 0; + out_776881510314174123[7] = 0; + out_776881510314174123[8] = 0; + out_776881510314174123[9] = 0; + out_776881510314174123[10] = 0; + out_776881510314174123[11] = 0; + out_776881510314174123[12] = 0; + out_776881510314174123[13] = 0; + out_776881510314174123[14] = 0; + out_776881510314174123[15] = 0; + out_776881510314174123[16] = 0; + out_776881510314174123[17] = 0; + out_776881510314174123[18] = 0; + out_776881510314174123[19] = 0; + out_776881510314174123[20] = 0; + out_776881510314174123[21] = 0; + out_776881510314174123[22] = 1; + out_776881510314174123[23] = 0; + out_776881510314174123[24] = 0; + out_776881510314174123[25] = 0; + out_776881510314174123[26] = 0; + out_776881510314174123[27] = 0; + out_776881510314174123[28] = 0; + out_776881510314174123[29] = 0; + out_776881510314174123[30] = 0; + out_776881510314174123[31] = 0; + out_776881510314174123[32] = 0; + out_776881510314174123[33] = 0; + out_776881510314174123[34] = 0; + out_776881510314174123[35] = 0; + out_776881510314174123[36] = 0; + out_776881510314174123[37] = 0; + out_776881510314174123[38] = 0; + out_776881510314174123[39] = 0; + out_776881510314174123[40] = 0; + out_776881510314174123[41] = 1; + out_776881510314174123[42] = 0; + out_776881510314174123[43] = 0; + out_776881510314174123[44] = 0; + out_776881510314174123[45] = 0; + out_776881510314174123[46] = 0; + out_776881510314174123[47] = 0; + out_776881510314174123[48] = 0; + out_776881510314174123[49] = 0; + out_776881510314174123[50] = 0; + out_776881510314174123[51] = 0; + out_776881510314174123[52] = 0; + out_776881510314174123[53] = 0; } -void h_14(double *state, double *unused, double *out_6788275849684674744) { - out_6788275849684674744[0] = state[6]; - out_6788275849684674744[1] = state[7]; - out_6788275849684674744[2] = state[8]; +void h_14(double *state, double *unused, double *out_4485852814658279598) { + out_4485852814658279598[0] = state[6]; + out_4485852814658279598[1] = state[7]; + out_4485852814658279598[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4961299742696261655) { - out_4961299742696261655[0] = 0; - out_4961299742696261655[1] = 0; - out_4961299742696261655[2] = 0; - out_4961299742696261655[3] = 0; - out_4961299742696261655[4] = 0; - out_4961299742696261655[5] = 0; - out_4961299742696261655[6] = 1; - out_4961299742696261655[7] = 0; - out_4961299742696261655[8] = 0; - out_4961299742696261655[9] = 0; - out_4961299742696261655[10] = 0; - out_4961299742696261655[11] = 0; - out_4961299742696261655[12] = 0; - out_4961299742696261655[13] = 0; - out_4961299742696261655[14] = 0; - out_4961299742696261655[15] = 0; - out_4961299742696261655[16] = 0; - out_4961299742696261655[17] = 0; - out_4961299742696261655[18] = 0; - out_4961299742696261655[19] = 0; - out_4961299742696261655[20] = 0; - out_4961299742696261655[21] = 0; - out_4961299742696261655[22] = 0; - out_4961299742696261655[23] = 0; - out_4961299742696261655[24] = 0; - out_4961299742696261655[25] = 1; - out_4961299742696261655[26] = 0; - out_4961299742696261655[27] = 0; - out_4961299742696261655[28] = 0; - out_4961299742696261655[29] = 0; - out_4961299742696261655[30] = 0; - out_4961299742696261655[31] = 0; - out_4961299742696261655[32] = 0; - out_4961299742696261655[33] = 0; - out_4961299742696261655[34] = 0; - out_4961299742696261655[35] = 0; - out_4961299742696261655[36] = 0; - out_4961299742696261655[37] = 0; - out_4961299742696261655[38] = 0; - out_4961299742696261655[39] = 0; - out_4961299742696261655[40] = 0; - out_4961299742696261655[41] = 0; - out_4961299742696261655[42] = 0; - out_4961299742696261655[43] = 0; - out_4961299742696261655[44] = 1; - out_4961299742696261655[45] = 0; - out_4961299742696261655[46] = 0; - out_4961299742696261655[47] = 0; - out_4961299742696261655[48] = 0; - out_4961299742696261655[49] = 0; - out_4961299742696261655[50] = 0; - out_4961299742696261655[51] = 0; - out_4961299742696261655[52] = 0; - out_4961299742696261655[53] = 0; +void H_14(double *state, double *unused, double *out_4424271862291390523) { + out_4424271862291390523[0] = 0; + out_4424271862291390523[1] = 0; + out_4424271862291390523[2] = 0; + out_4424271862291390523[3] = 0; + out_4424271862291390523[4] = 0; + out_4424271862291390523[5] = 0; + out_4424271862291390523[6] = 1; + out_4424271862291390523[7] = 0; + out_4424271862291390523[8] = 0; + out_4424271862291390523[9] = 0; + out_4424271862291390523[10] = 0; + out_4424271862291390523[11] = 0; + out_4424271862291390523[12] = 0; + out_4424271862291390523[13] = 0; + out_4424271862291390523[14] = 0; + out_4424271862291390523[15] = 0; + out_4424271862291390523[16] = 0; + out_4424271862291390523[17] = 0; + out_4424271862291390523[18] = 0; + out_4424271862291390523[19] = 0; + out_4424271862291390523[20] = 0; + out_4424271862291390523[21] = 0; + out_4424271862291390523[22] = 0; + out_4424271862291390523[23] = 0; + out_4424271862291390523[24] = 0; + out_4424271862291390523[25] = 1; + out_4424271862291390523[26] = 0; + out_4424271862291390523[27] = 0; + out_4424271862291390523[28] = 0; + out_4424271862291390523[29] = 0; + out_4424271862291390523[30] = 0; + out_4424271862291390523[31] = 0; + out_4424271862291390523[32] = 0; + out_4424271862291390523[33] = 0; + out_4424271862291390523[34] = 0; + out_4424271862291390523[35] = 0; + out_4424271862291390523[36] = 0; + out_4424271862291390523[37] = 0; + out_4424271862291390523[38] = 0; + out_4424271862291390523[39] = 0; + out_4424271862291390523[40] = 0; + out_4424271862291390523[41] = 0; + out_4424271862291390523[42] = 0; + out_4424271862291390523[43] = 0; + out_4424271862291390523[44] = 1; + out_4424271862291390523[45] = 0; + out_4424271862291390523[46] = 0; + out_4424271862291390523[47] = 0; + out_4424271862291390523[48] = 0; + out_4424271862291390523[49] = 0; + out_4424271862291390523[50] = 0; + out_4424271862291390523[51] = 0; + out_4424271862291390523[52] = 0; + out_4424271862291390523[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_6392571160168570842) { - err_fun(nom_x, delta_x, out_6392571160168570842); +void pose_err_fun(double *nom_x, double *delta_x, double *out_5135933030950096940) { + err_fun(nom_x, delta_x, out_5135933030950096940); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_70108598084165787) { - inv_err_fun(nom_x, true_x, out_70108598084165787); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4473434694497561565) { + inv_err_fun(nom_x, true_x, out_4473434694497561565); } -void pose_H_mod_fun(double *state, double *out_7031029695439970094) { - H_mod_fun(state, out_7031029695439970094); +void pose_H_mod_fun(double *state, double *out_4955206330411314418) { + H_mod_fun(state, out_4955206330411314418); } -void pose_f_fun(double *state, double dt, double *out_4622611615613940133) { - f_fun(state, dt, out_4622611615613940133); +void pose_f_fun(double *state, double dt, double *out_1052805527796842003) { + f_fun(state, dt, out_1052805527796842003); } -void pose_F_fun(double *state, double dt, double *out_542246045313485722) { - F_fun(state, dt, out_542246045313485722); +void pose_F_fun(double *state, double dt, double *out_5871869036335399812) { + F_fun(state, dt, out_5871869036335399812); } -void pose_h_4(double *state, double *unused, double *out_6293306196609579881) { - h_4(state, unused, out_6293306196609579881); +void pose_h_4(double *state, double *unused, double *out_921118668651624411) { + h_4(state, unused, out_921118668651624411); } -void pose_H_4(double *state, double *unused, double *out_1878511310400889359) { - H_4(state, unused, out_1878511310400889359); +void pose_H_4(double *state, double *unused, double *out_3989155335646506924) { + H_4(state, unused, out_3989155335646506924); } -void pose_h_10(double *state, double *unused, double *out_8801939057301033560) { - h_10(state, unused, out_8801939057301033560); +void pose_h_10(double *state, double *unused, double *out_8738369372345178390) { + h_10(state, unused, out_8738369372345178390); } -void pose_H_10(double *state, double *unused, double *out_2335330326985309171) { - H_10(state, unused, out_2335330326985309171); +void pose_H_10(double *state, double *unused, double *out_2875729590105375124) { + H_10(state, unused, out_2875729590105375124); } -void pose_h_13(double *state, double *unused, double *out_442641261201264592) { - h_13(state, unused, out_442641261201264592); +void pose_h_13(double *state, double *unused, double *out_1766623406694503914) { + h_13(state, unused, out_1766623406694503914); } -void pose_H_13(double *state, double *unused, double *out_5712266773703413383) { - H_13(state, unused, out_5712266773703413383); +void pose_H_13(double *state, double *unused, double *out_776881510314174123) { + H_13(state, unused, out_776881510314174123); } -void pose_h_14(double *state, double *unused, double *out_6788275849684674744) { - h_14(state, unused, out_6788275849684674744); +void pose_h_14(double *state, double *unused, double *out_4485852814658279598) { + h_14(state, unused, out_4485852814658279598); } -void pose_H_14(double *state, double *unused, double *out_4961299742696261655) { - H_14(state, unused, out_4961299742696261655); +void pose_H_14(double *state, double *unused, double *out_4424271862291390523) { + H_14(state, unused, out_4424271862291390523); } 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 27acfaf13..7b49879f4 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_6392571160168570842); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_70108598084165787); -void pose_H_mod_fun(double *state, double *out_7031029695439970094); -void pose_f_fun(double *state, double dt, double *out_4622611615613940133); -void pose_F_fun(double *state, double dt, double *out_542246045313485722); -void pose_h_4(double *state, double *unused, double *out_6293306196609579881); -void pose_H_4(double *state, double *unused, double *out_1878511310400889359); -void pose_h_10(double *state, double *unused, double *out_8801939057301033560); -void pose_H_10(double *state, double *unused, double *out_2335330326985309171); -void pose_h_13(double *state, double *unused, double *out_442641261201264592); -void pose_H_13(double *state, double *unused, double *out_5712266773703413383); -void pose_h_14(double *state, double *unused, double *out_6788275849684674744); -void pose_H_14(double *state, double *unused, double *out_4961299742696261655); +void pose_err_fun(double *nom_x, double *delta_x, double *out_5135933030950096940); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4473434694497561565); +void pose_H_mod_fun(double *state, double *out_4955206330411314418); +void pose_f_fun(double *state, double dt, double *out_1052805527796842003); +void pose_F_fun(double *state, double dt, double *out_5871869036335399812); +void pose_h_4(double *state, double *unused, double *out_921118668651624411); +void pose_H_4(double *state, double *unused, double *out_3989155335646506924); +void pose_h_10(double *state, double *unused, double *out_8738369372345178390); +void pose_H_10(double *state, double *unused, double *out_2875729590105375124); +void pose_h_13(double *state, double *unused, double *out_1766623406694503914); +void pose_H_13(double *state, double *unused, double *out_776881510314174123); +void pose_h_14(double *state, double *unused, double *out_4485852814658279598); +void pose_H_14(double *state, double *unused, double *out_4424271862291390523); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index b0cc6aa43..6f720310d 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ