diff --git a/common/libcommon.a b/common/libcommon.a index 5e5513f06..6b7769ea8 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 0970e269f..f09e14802 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 960270786..c468f3f98 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 5ebab7251..94ad01f7b 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 e59d718dc..24d6ddbd9 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 e69f3eca3..17fc477f3 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 16cf3ba47..4950c4a4e 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 695f0bba3..b50a4b4f1 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 fe3c6bc06..7b6dc5cc1 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 fe3c6bc06..7b6dc5cc1 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 321e9b2c7..ff682d434 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 695f0bba3..b50a4b4f1 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 4414046ee..cb28c39ee 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-ffd8f640-DEBUG"; +const uint8_t gitversion[19] = "DEV-e969a96c-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 71dfe8199..ad26da870 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 cbcf9bfdb..c5ed73914 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 3b5af0d83..adc3084a9 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 1cab9eb50..dc461751f 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 4e6c8a2b7..f8008d480 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 f7cf629a5..dff1b1ef0 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 7a2cfae58..529ae9fb1 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 a6b5cc16b..e6893fdc9 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 5f6df47b1..d5e102871 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 bca14c65f..2520baa03 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 928f28327..f2524feef 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 9f8689b3b..da44041ff 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 6029cb7ce..f581d2016 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 523bea62c..54aae4f77 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 063945e87..8e14c2455 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 5ad65dd77..5e9d86e13 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 6df9ba815..a904ea396 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 49f386584..39030b6dc 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 67b3c600a..45d8a540f 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 195c891bc..728892884 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 d7b657815..7ac1d753f 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-ffd8f640-DEBUG \ No newline at end of file +DEV-e969a96c-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 2cfb506cc..da7392775 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_5399948258775111327) { - out_5399948258775111327[0] = delta_x[0] + nom_x[0]; - out_5399948258775111327[1] = delta_x[1] + nom_x[1]; - out_5399948258775111327[2] = delta_x[2] + nom_x[2]; - out_5399948258775111327[3] = delta_x[3] + nom_x[3]; - out_5399948258775111327[4] = delta_x[4] + nom_x[4]; - out_5399948258775111327[5] = delta_x[5] + nom_x[5]; - out_5399948258775111327[6] = delta_x[6] + nom_x[6]; - out_5399948258775111327[7] = delta_x[7] + nom_x[7]; - out_5399948258775111327[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_2567017903067559466) { + out_2567017903067559466[0] = delta_x[0] + nom_x[0]; + out_2567017903067559466[1] = delta_x[1] + nom_x[1]; + out_2567017903067559466[2] = delta_x[2] + nom_x[2]; + out_2567017903067559466[3] = delta_x[3] + nom_x[3]; + out_2567017903067559466[4] = delta_x[4] + nom_x[4]; + out_2567017903067559466[5] = delta_x[5] + nom_x[5]; + out_2567017903067559466[6] = delta_x[6] + nom_x[6]; + out_2567017903067559466[7] = delta_x[7] + nom_x[7]; + out_2567017903067559466[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_538771626761939485) { - out_538771626761939485[0] = -nom_x[0] + true_x[0]; - out_538771626761939485[1] = -nom_x[1] + true_x[1]; - out_538771626761939485[2] = -nom_x[2] + true_x[2]; - out_538771626761939485[3] = -nom_x[3] + true_x[3]; - out_538771626761939485[4] = -nom_x[4] + true_x[4]; - out_538771626761939485[5] = -nom_x[5] + true_x[5]; - out_538771626761939485[6] = -nom_x[6] + true_x[6]; - out_538771626761939485[7] = -nom_x[7] + true_x[7]; - out_538771626761939485[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1749285410370024184) { + out_1749285410370024184[0] = -nom_x[0] + true_x[0]; + out_1749285410370024184[1] = -nom_x[1] + true_x[1]; + out_1749285410370024184[2] = -nom_x[2] + true_x[2]; + out_1749285410370024184[3] = -nom_x[3] + true_x[3]; + out_1749285410370024184[4] = -nom_x[4] + true_x[4]; + out_1749285410370024184[5] = -nom_x[5] + true_x[5]; + out_1749285410370024184[6] = -nom_x[6] + true_x[6]; + out_1749285410370024184[7] = -nom_x[7] + true_x[7]; + out_1749285410370024184[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_6102609910614786292) { - out_6102609910614786292[0] = 1.0; - out_6102609910614786292[1] = 0.0; - out_6102609910614786292[2] = 0.0; - out_6102609910614786292[3] = 0.0; - out_6102609910614786292[4] = 0.0; - out_6102609910614786292[5] = 0.0; - out_6102609910614786292[6] = 0.0; - out_6102609910614786292[7] = 0.0; - out_6102609910614786292[8] = 0.0; - out_6102609910614786292[9] = 0.0; - out_6102609910614786292[10] = 1.0; - out_6102609910614786292[11] = 0.0; - out_6102609910614786292[12] = 0.0; - out_6102609910614786292[13] = 0.0; - out_6102609910614786292[14] = 0.0; - out_6102609910614786292[15] = 0.0; - out_6102609910614786292[16] = 0.0; - out_6102609910614786292[17] = 0.0; - out_6102609910614786292[18] = 0.0; - out_6102609910614786292[19] = 0.0; - out_6102609910614786292[20] = 1.0; - out_6102609910614786292[21] = 0.0; - out_6102609910614786292[22] = 0.0; - out_6102609910614786292[23] = 0.0; - out_6102609910614786292[24] = 0.0; - out_6102609910614786292[25] = 0.0; - out_6102609910614786292[26] = 0.0; - out_6102609910614786292[27] = 0.0; - out_6102609910614786292[28] = 0.0; - out_6102609910614786292[29] = 0.0; - out_6102609910614786292[30] = 1.0; - out_6102609910614786292[31] = 0.0; - out_6102609910614786292[32] = 0.0; - out_6102609910614786292[33] = 0.0; - out_6102609910614786292[34] = 0.0; - out_6102609910614786292[35] = 0.0; - out_6102609910614786292[36] = 0.0; - out_6102609910614786292[37] = 0.0; - out_6102609910614786292[38] = 0.0; - out_6102609910614786292[39] = 0.0; - out_6102609910614786292[40] = 1.0; - out_6102609910614786292[41] = 0.0; - out_6102609910614786292[42] = 0.0; - out_6102609910614786292[43] = 0.0; - out_6102609910614786292[44] = 0.0; - out_6102609910614786292[45] = 0.0; - out_6102609910614786292[46] = 0.0; - out_6102609910614786292[47] = 0.0; - out_6102609910614786292[48] = 0.0; - out_6102609910614786292[49] = 0.0; - out_6102609910614786292[50] = 1.0; - out_6102609910614786292[51] = 0.0; - out_6102609910614786292[52] = 0.0; - out_6102609910614786292[53] = 0.0; - out_6102609910614786292[54] = 0.0; - out_6102609910614786292[55] = 0.0; - out_6102609910614786292[56] = 0.0; - out_6102609910614786292[57] = 0.0; - out_6102609910614786292[58] = 0.0; - out_6102609910614786292[59] = 0.0; - out_6102609910614786292[60] = 1.0; - out_6102609910614786292[61] = 0.0; - out_6102609910614786292[62] = 0.0; - out_6102609910614786292[63] = 0.0; - out_6102609910614786292[64] = 0.0; - out_6102609910614786292[65] = 0.0; - out_6102609910614786292[66] = 0.0; - out_6102609910614786292[67] = 0.0; - out_6102609910614786292[68] = 0.0; - out_6102609910614786292[69] = 0.0; - out_6102609910614786292[70] = 1.0; - out_6102609910614786292[71] = 0.0; - out_6102609910614786292[72] = 0.0; - out_6102609910614786292[73] = 0.0; - out_6102609910614786292[74] = 0.0; - out_6102609910614786292[75] = 0.0; - out_6102609910614786292[76] = 0.0; - out_6102609910614786292[77] = 0.0; - out_6102609910614786292[78] = 0.0; - out_6102609910614786292[79] = 0.0; - out_6102609910614786292[80] = 1.0; +void H_mod_fun(double *state, double *out_8979551951706364182) { + out_8979551951706364182[0] = 1.0; + out_8979551951706364182[1] = 0.0; + out_8979551951706364182[2] = 0.0; + out_8979551951706364182[3] = 0.0; + out_8979551951706364182[4] = 0.0; + out_8979551951706364182[5] = 0.0; + out_8979551951706364182[6] = 0.0; + out_8979551951706364182[7] = 0.0; + out_8979551951706364182[8] = 0.0; + out_8979551951706364182[9] = 0.0; + out_8979551951706364182[10] = 1.0; + out_8979551951706364182[11] = 0.0; + out_8979551951706364182[12] = 0.0; + out_8979551951706364182[13] = 0.0; + out_8979551951706364182[14] = 0.0; + out_8979551951706364182[15] = 0.0; + out_8979551951706364182[16] = 0.0; + out_8979551951706364182[17] = 0.0; + out_8979551951706364182[18] = 0.0; + out_8979551951706364182[19] = 0.0; + out_8979551951706364182[20] = 1.0; + out_8979551951706364182[21] = 0.0; + out_8979551951706364182[22] = 0.0; + out_8979551951706364182[23] = 0.0; + out_8979551951706364182[24] = 0.0; + out_8979551951706364182[25] = 0.0; + out_8979551951706364182[26] = 0.0; + out_8979551951706364182[27] = 0.0; + out_8979551951706364182[28] = 0.0; + out_8979551951706364182[29] = 0.0; + out_8979551951706364182[30] = 1.0; + out_8979551951706364182[31] = 0.0; + out_8979551951706364182[32] = 0.0; + out_8979551951706364182[33] = 0.0; + out_8979551951706364182[34] = 0.0; + out_8979551951706364182[35] = 0.0; + out_8979551951706364182[36] = 0.0; + out_8979551951706364182[37] = 0.0; + out_8979551951706364182[38] = 0.0; + out_8979551951706364182[39] = 0.0; + out_8979551951706364182[40] = 1.0; + out_8979551951706364182[41] = 0.0; + out_8979551951706364182[42] = 0.0; + out_8979551951706364182[43] = 0.0; + out_8979551951706364182[44] = 0.0; + out_8979551951706364182[45] = 0.0; + out_8979551951706364182[46] = 0.0; + out_8979551951706364182[47] = 0.0; + out_8979551951706364182[48] = 0.0; + out_8979551951706364182[49] = 0.0; + out_8979551951706364182[50] = 1.0; + out_8979551951706364182[51] = 0.0; + out_8979551951706364182[52] = 0.0; + out_8979551951706364182[53] = 0.0; + out_8979551951706364182[54] = 0.0; + out_8979551951706364182[55] = 0.0; + out_8979551951706364182[56] = 0.0; + out_8979551951706364182[57] = 0.0; + out_8979551951706364182[58] = 0.0; + out_8979551951706364182[59] = 0.0; + out_8979551951706364182[60] = 1.0; + out_8979551951706364182[61] = 0.0; + out_8979551951706364182[62] = 0.0; + out_8979551951706364182[63] = 0.0; + out_8979551951706364182[64] = 0.0; + out_8979551951706364182[65] = 0.0; + out_8979551951706364182[66] = 0.0; + out_8979551951706364182[67] = 0.0; + out_8979551951706364182[68] = 0.0; + out_8979551951706364182[69] = 0.0; + out_8979551951706364182[70] = 1.0; + out_8979551951706364182[71] = 0.0; + out_8979551951706364182[72] = 0.0; + out_8979551951706364182[73] = 0.0; + out_8979551951706364182[74] = 0.0; + out_8979551951706364182[75] = 0.0; + out_8979551951706364182[76] = 0.0; + out_8979551951706364182[77] = 0.0; + out_8979551951706364182[78] = 0.0; + out_8979551951706364182[79] = 0.0; + out_8979551951706364182[80] = 1.0; } -void f_fun(double *state, double dt, double *out_667706195184437986) { - out_667706195184437986[0] = state[0]; - out_667706195184437986[1] = state[1]; - out_667706195184437986[2] = state[2]; - out_667706195184437986[3] = state[3]; - out_667706195184437986[4] = state[4]; - out_667706195184437986[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_667706195184437986[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_667706195184437986[7] = state[7]; - out_667706195184437986[8] = state[8]; +void f_fun(double *state, double dt, double *out_3674179640817441392) { + out_3674179640817441392[0] = state[0]; + out_3674179640817441392[1] = state[1]; + out_3674179640817441392[2] = state[2]; + out_3674179640817441392[3] = state[3]; + out_3674179640817441392[4] = state[4]; + out_3674179640817441392[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_3674179640817441392[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_3674179640817441392[7] = state[7]; + out_3674179640817441392[8] = state[8]; } -void F_fun(double *state, double dt, double *out_157487213156187863) { - out_157487213156187863[0] = 1; - out_157487213156187863[1] = 0; - out_157487213156187863[2] = 0; - out_157487213156187863[3] = 0; - out_157487213156187863[4] = 0; - out_157487213156187863[5] = 0; - out_157487213156187863[6] = 0; - out_157487213156187863[7] = 0; - out_157487213156187863[8] = 0; - out_157487213156187863[9] = 0; - out_157487213156187863[10] = 1; - out_157487213156187863[11] = 0; - out_157487213156187863[12] = 0; - out_157487213156187863[13] = 0; - out_157487213156187863[14] = 0; - out_157487213156187863[15] = 0; - out_157487213156187863[16] = 0; - out_157487213156187863[17] = 0; - out_157487213156187863[18] = 0; - out_157487213156187863[19] = 0; - out_157487213156187863[20] = 1; - out_157487213156187863[21] = 0; - out_157487213156187863[22] = 0; - out_157487213156187863[23] = 0; - out_157487213156187863[24] = 0; - out_157487213156187863[25] = 0; - out_157487213156187863[26] = 0; - out_157487213156187863[27] = 0; - out_157487213156187863[28] = 0; - out_157487213156187863[29] = 0; - out_157487213156187863[30] = 1; - out_157487213156187863[31] = 0; - out_157487213156187863[32] = 0; - out_157487213156187863[33] = 0; - out_157487213156187863[34] = 0; - out_157487213156187863[35] = 0; - out_157487213156187863[36] = 0; - out_157487213156187863[37] = 0; - out_157487213156187863[38] = 0; - out_157487213156187863[39] = 0; - out_157487213156187863[40] = 1; - out_157487213156187863[41] = 0; - out_157487213156187863[42] = 0; - out_157487213156187863[43] = 0; - out_157487213156187863[44] = 0; - out_157487213156187863[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_157487213156187863[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_157487213156187863[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_157487213156187863[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_157487213156187863[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_157487213156187863[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_157487213156187863[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_157487213156187863[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_157487213156187863[53] = -9.8100000000000005*dt; - out_157487213156187863[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_157487213156187863[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_157487213156187863[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_157487213156187863[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_157487213156187863[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_157487213156187863[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_157487213156187863[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_157487213156187863[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_157487213156187863[62] = 0; - out_157487213156187863[63] = 0; - out_157487213156187863[64] = 0; - out_157487213156187863[65] = 0; - out_157487213156187863[66] = 0; - out_157487213156187863[67] = 0; - out_157487213156187863[68] = 0; - out_157487213156187863[69] = 0; - out_157487213156187863[70] = 1; - out_157487213156187863[71] = 0; - out_157487213156187863[72] = 0; - out_157487213156187863[73] = 0; - out_157487213156187863[74] = 0; - out_157487213156187863[75] = 0; - out_157487213156187863[76] = 0; - out_157487213156187863[77] = 0; - out_157487213156187863[78] = 0; - out_157487213156187863[79] = 0; - out_157487213156187863[80] = 1; +void F_fun(double *state, double dt, double *out_258925546890284745) { + out_258925546890284745[0] = 1; + out_258925546890284745[1] = 0; + out_258925546890284745[2] = 0; + out_258925546890284745[3] = 0; + out_258925546890284745[4] = 0; + out_258925546890284745[5] = 0; + out_258925546890284745[6] = 0; + out_258925546890284745[7] = 0; + out_258925546890284745[8] = 0; + out_258925546890284745[9] = 0; + out_258925546890284745[10] = 1; + out_258925546890284745[11] = 0; + out_258925546890284745[12] = 0; + out_258925546890284745[13] = 0; + out_258925546890284745[14] = 0; + out_258925546890284745[15] = 0; + out_258925546890284745[16] = 0; + out_258925546890284745[17] = 0; + out_258925546890284745[18] = 0; + out_258925546890284745[19] = 0; + out_258925546890284745[20] = 1; + out_258925546890284745[21] = 0; + out_258925546890284745[22] = 0; + out_258925546890284745[23] = 0; + out_258925546890284745[24] = 0; + out_258925546890284745[25] = 0; + out_258925546890284745[26] = 0; + out_258925546890284745[27] = 0; + out_258925546890284745[28] = 0; + out_258925546890284745[29] = 0; + out_258925546890284745[30] = 1; + out_258925546890284745[31] = 0; + out_258925546890284745[32] = 0; + out_258925546890284745[33] = 0; + out_258925546890284745[34] = 0; + out_258925546890284745[35] = 0; + out_258925546890284745[36] = 0; + out_258925546890284745[37] = 0; + out_258925546890284745[38] = 0; + out_258925546890284745[39] = 0; + out_258925546890284745[40] = 1; + out_258925546890284745[41] = 0; + out_258925546890284745[42] = 0; + out_258925546890284745[43] = 0; + out_258925546890284745[44] = 0; + out_258925546890284745[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_258925546890284745[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_258925546890284745[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_258925546890284745[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_258925546890284745[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_258925546890284745[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_258925546890284745[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_258925546890284745[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_258925546890284745[53] = -9.8100000000000005*dt; + out_258925546890284745[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_258925546890284745[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_258925546890284745[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_258925546890284745[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_258925546890284745[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_258925546890284745[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_258925546890284745[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_258925546890284745[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_258925546890284745[62] = 0; + out_258925546890284745[63] = 0; + out_258925546890284745[64] = 0; + out_258925546890284745[65] = 0; + out_258925546890284745[66] = 0; + out_258925546890284745[67] = 0; + out_258925546890284745[68] = 0; + out_258925546890284745[69] = 0; + out_258925546890284745[70] = 1; + out_258925546890284745[71] = 0; + out_258925546890284745[72] = 0; + out_258925546890284745[73] = 0; + out_258925546890284745[74] = 0; + out_258925546890284745[75] = 0; + out_258925546890284745[76] = 0; + out_258925546890284745[77] = 0; + out_258925546890284745[78] = 0; + out_258925546890284745[79] = 0; + out_258925546890284745[80] = 1; } -void h_25(double *state, double *unused, double *out_7616837909277052709) { - out_7616837909277052709[0] = state[6]; +void h_25(double *state, double *unused, double *out_5262304077966076166) { + out_5262304077966076166[0] = state[6]; } -void H_25(double *state, double *unused, double *out_8821153513105273647) { - out_8821153513105273647[0] = 0; - out_8821153513105273647[1] = 0; - out_8821153513105273647[2] = 0; - out_8821153513105273647[3] = 0; - out_8821153513105273647[4] = 0; - out_8821153513105273647[5] = 0; - out_8821153513105273647[6] = 1; - out_8821153513105273647[7] = 0; - out_8821153513105273647[8] = 0; +void H_25(double *state, double *unused, double *out_4563500296818522594) { + out_4563500296818522594[0] = 0; + out_4563500296818522594[1] = 0; + out_4563500296818522594[2] = 0; + out_4563500296818522594[3] = 0; + out_4563500296818522594[4] = 0; + out_4563500296818522594[5] = 0; + out_4563500296818522594[6] = 1; + out_4563500296818522594[7] = 0; + out_4563500296818522594[8] = 0; } -void h_24(double *state, double *unused, double *out_3643618505328510339) { - out_3643618505328510339[0] = state[4]; - out_3643618505328510339[1] = state[5]; +void h_24(double *state, double *unused, double *out_2894074188837958464) { + out_2894074188837958464[0] = state[4]; + out_2894074188837958464[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5263227914302950511) { - out_5263227914302950511[0] = 0; - out_5263227914302950511[1] = 0; - out_5263227914302950511[2] = 0; - out_5263227914302950511[3] = 0; - out_5263227914302950511[4] = 1; - out_5263227914302950511[5] = 0; - out_5263227914302950511[6] = 0; - out_5263227914302950511[7] = 0; - out_5263227914302950511[8] = 0; - out_5263227914302950511[9] = 0; - out_5263227914302950511[10] = 0; - out_5263227914302950511[11] = 0; - out_5263227914302950511[12] = 0; - out_5263227914302950511[13] = 0; - out_5263227914302950511[14] = 1; - out_5263227914302950511[15] = 0; - out_5263227914302950511[16] = 0; - out_5263227914302950511[17] = 0; +void H_24(double *state, double *unused, double *out_2012071509772995507) { + out_2012071509772995507[0] = 0; + out_2012071509772995507[1] = 0; + out_2012071509772995507[2] = 0; + out_2012071509772995507[3] = 0; + out_2012071509772995507[4] = 1; + out_2012071509772995507[5] = 0; + out_2012071509772995507[6] = 0; + out_2012071509772995507[7] = 0; + out_2012071509772995507[8] = 0; + out_2012071509772995507[9] = 0; + out_2012071509772995507[10] = 0; + out_2012071509772995507[11] = 0; + out_2012071509772995507[12] = 0; + out_2012071509772995507[13] = 0; + out_2012071509772995507[14] = 1; + out_2012071509772995507[15] = 0; + out_2012071509772995507[16] = 0; + out_2012071509772995507[17] = 0; } -void h_30(double *state, double *unused, double *out_6577068605668785750) { - out_6577068605668785750[0] = state[4]; +void h_30(double *state, double *unused, double *out_9005980160461201074) { + out_9005980160461201074[0] = state[4]; } -void H_30(double *state, double *unused, double *out_8950492460248513717) { - out_8950492460248513717[0] = 0; - out_8950492460248513717[1] = 0; - out_8950492460248513717[2] = 0; - out_8950492460248513717[3] = 0; - out_8950492460248513717[4] = 1; - out_8950492460248513717[5] = 0; - out_8950492460248513717[6] = 0; - out_8950492460248513717[7] = 0; - out_8950492460248513717[8] = 0; +void H_30(double *state, double *unused, double *out_2045167338311273967) { + out_2045167338311273967[0] = 0; + out_2045167338311273967[1] = 0; + out_2045167338311273967[2] = 0; + out_2045167338311273967[3] = 0; + out_2045167338311273967[4] = 1; + out_2045167338311273967[5] = 0; + out_2045167338311273967[6] = 0; + out_2045167338311273967[7] = 0; + out_2045167338311273967[8] = 0; } -void h_26(double *state, double *unused, double *out_5973752720992308233) { - out_5973752720992308233[0] = state[7]; +void h_26(double *state, double *unused, double *out_8837448028571873025) { + out_8837448028571873025[0] = state[7]; } -void H_26(double *state, double *unused, double *out_5884087241730221745) { - out_5884087241730221745[0] = 0; - out_5884087241730221745[1] = 0; - out_5884087241730221745[2] = 0; - out_5884087241730221745[3] = 0; - out_5884087241730221745[4] = 0; - out_5884087241730221745[5] = 0; - out_5884087241730221745[6] = 0; - out_5884087241730221745[7] = 1; - out_5884087241730221745[8] = 0; +void H_26(double *state, double *unused, double *out_1258974327057721993) { + out_1258974327057721993[0] = 0; + out_1258974327057721993[1] = 0; + out_1258974327057721993[2] = 0; + out_1258974327057721993[3] = 0; + out_1258974327057721993[4] = 0; + out_1258974327057721993[5] = 0; + out_1258974327057721993[6] = 0; + out_1258974327057721993[7] = 1; + out_1258974327057721993[8] = 0; } -void h_27(double *state, double *unused, double *out_8560347762178715144) { - out_8560347762178715144[0] = state[3]; +void h_27(double *state, double *unused, double *out_6785398478329626304) { + out_6785398478329626304[0] = state[3]; } -void H_27(double *state, double *unused, double *out_7321488301660612988) { - out_7321488301660612988[0] = 0; - out_7321488301660612988[1] = 0; - out_7321488301660612988[2] = 0; - out_7321488301660612988[3] = 1; - out_7321488301660612988[4] = 0; - out_7321488301660612988[5] = 0; - out_7321488301660612988[6] = 0; - out_7321488301660612988[7] = 0; - out_7321488301660612988[8] = 0; +void H_27(double *state, double *unused, double *out_178426732872669250) { + out_178426732872669250[0] = 0; + out_178426732872669250[1] = 0; + out_178426732872669250[2] = 0; + out_178426732872669250[3] = 1; + out_178426732872669250[4] = 0; + out_178426732872669250[5] = 0; + out_178426732872669250[6] = 0; + out_178426732872669250[7] = 0; + out_178426732872669250[8] = 0; } -void h_29(double *state, double *unused, double *out_1246518834812294588) { - out_1246518834812294588[0] = state[1]; +void h_29(double *state, double *unused, double *out_8023896067125902962) { + out_8023896067125902962[0] = state[1]; } -void H_29(double *state, double *unused, double *out_5608125574791061955) { - out_5608125574791061955[0] = 0; - out_5608125574791061955[1] = 1; - out_5608125574791061955[2] = 0; - out_5608125574791061955[3] = 0; - out_5608125574791061955[4] = 0; - out_5608125574791061955[5] = 0; - out_5608125574791061955[6] = 0; - out_5608125574791061955[7] = 0; - out_5608125574791061955[8] = 0; +void H_29(double *state, double *unused, double *out_1534935993996881783) { + out_1534935993996881783[0] = 0; + out_1534935993996881783[1] = 1; + out_1534935993996881783[2] = 0; + out_1534935993996881783[3] = 0; + out_1534935993996881783[4] = 0; + out_1534935993996881783[5] = 0; + out_1534935993996881783[6] = 0; + out_1534935993996881783[7] = 0; + out_1534935993996881783[8] = 0; } -void h_28(double *state, double *unused, double *out_7957031877502237627) { - out_7957031877502237627[0] = state[0]; +void h_28(double *state, double *unused, double *out_5617654677253484567) { + out_5617654677253484567[0] = state[0]; } -void H_28(double *state, double *unused, double *out_7571755846356388206) { - out_7571755846356388206[0] = 1; - out_7571755846356388206[1] = 0; - out_7571755846356388206[2] = 0; - out_7571755846356388206[3] = 0; - out_7571755846356388206[4] = 0; - out_7571755846356388206[5] = 0; - out_7571755846356388206[6] = 0; - out_7571755846356388206[7] = 0; - out_7571755846356388206[8] = 0; +void H_28(double *state, double *unused, double *out_6617335011066412357) { + out_6617335011066412357[0] = 1; + out_6617335011066412357[1] = 0; + out_6617335011066412357[2] = 0; + out_6617335011066412357[3] = 0; + out_6617335011066412357[4] = 0; + out_6617335011066412357[5] = 0; + out_6617335011066412357[6] = 0; + out_6617335011066412357[7] = 0; + out_6617335011066412357[8] = 0; } -void h_31(double *state, double *unused, double *out_7192456636178476557) { - out_7192456636178476557[0] = state[8]; +void h_31(double *state, double *unused, double *out_9123987554394577818) { + out_9123987554394577818[0] = state[8]; } -void H_31(double *state, double *unused, double *out_8790507551228313219) { - out_8790507551228313219[0] = 0; - out_8790507551228313219[1] = 0; - out_8790507551228313219[2] = 0; - out_8790507551228313219[3] = 0; - out_8790507551228313219[4] = 0; - out_8790507551228313219[5] = 0; - out_8790507551228313219[6] = 0; - out_8790507551228313219[7] = 0; - out_8790507551228313219[8] = 1; +void H_31(double *state, double *unused, double *out_4532854334941562166) { + out_4532854334941562166[0] = 0; + out_4532854334941562166[1] = 0; + out_4532854334941562166[2] = 0; + out_4532854334941562166[3] = 0; + out_4532854334941562166[4] = 0; + out_4532854334941562166[5] = 0; + out_4532854334941562166[6] = 0; + out_4532854334941562166[7] = 0; + out_4532854334941562166[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_5399948258775111327) { - err_fun(nom_x, delta_x, out_5399948258775111327); +void car_err_fun(double *nom_x, double *delta_x, double *out_2567017903067559466) { + err_fun(nom_x, delta_x, out_2567017903067559466); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_538771626761939485) { - inv_err_fun(nom_x, true_x, out_538771626761939485); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1749285410370024184) { + inv_err_fun(nom_x, true_x, out_1749285410370024184); } -void car_H_mod_fun(double *state, double *out_6102609910614786292) { - H_mod_fun(state, out_6102609910614786292); +void car_H_mod_fun(double *state, double *out_8979551951706364182) { + H_mod_fun(state, out_8979551951706364182); } -void car_f_fun(double *state, double dt, double *out_667706195184437986) { - f_fun(state, dt, out_667706195184437986); +void car_f_fun(double *state, double dt, double *out_3674179640817441392) { + f_fun(state, dt, out_3674179640817441392); } -void car_F_fun(double *state, double dt, double *out_157487213156187863) { - F_fun(state, dt, out_157487213156187863); +void car_F_fun(double *state, double dt, double *out_258925546890284745) { + F_fun(state, dt, out_258925546890284745); } -void car_h_25(double *state, double *unused, double *out_7616837909277052709) { - h_25(state, unused, out_7616837909277052709); +void car_h_25(double *state, double *unused, double *out_5262304077966076166) { + h_25(state, unused, out_5262304077966076166); } -void car_H_25(double *state, double *unused, double *out_8821153513105273647) { - H_25(state, unused, out_8821153513105273647); +void car_H_25(double *state, double *unused, double *out_4563500296818522594) { + H_25(state, unused, out_4563500296818522594); } -void car_h_24(double *state, double *unused, double *out_3643618505328510339) { - h_24(state, unused, out_3643618505328510339); +void car_h_24(double *state, double *unused, double *out_2894074188837958464) { + h_24(state, unused, out_2894074188837958464); } -void car_H_24(double *state, double *unused, double *out_5263227914302950511) { - H_24(state, unused, out_5263227914302950511); +void car_H_24(double *state, double *unused, double *out_2012071509772995507) { + H_24(state, unused, out_2012071509772995507); } -void car_h_30(double *state, double *unused, double *out_6577068605668785750) { - h_30(state, unused, out_6577068605668785750); +void car_h_30(double *state, double *unused, double *out_9005980160461201074) { + h_30(state, unused, out_9005980160461201074); } -void car_H_30(double *state, double *unused, double *out_8950492460248513717) { - H_30(state, unused, out_8950492460248513717); +void car_H_30(double *state, double *unused, double *out_2045167338311273967) { + H_30(state, unused, out_2045167338311273967); } -void car_h_26(double *state, double *unused, double *out_5973752720992308233) { - h_26(state, unused, out_5973752720992308233); +void car_h_26(double *state, double *unused, double *out_8837448028571873025) { + h_26(state, unused, out_8837448028571873025); } -void car_H_26(double *state, double *unused, double *out_5884087241730221745) { - H_26(state, unused, out_5884087241730221745); +void car_H_26(double *state, double *unused, double *out_1258974327057721993) { + H_26(state, unused, out_1258974327057721993); } -void car_h_27(double *state, double *unused, double *out_8560347762178715144) { - h_27(state, unused, out_8560347762178715144); +void car_h_27(double *state, double *unused, double *out_6785398478329626304) { + h_27(state, unused, out_6785398478329626304); } -void car_H_27(double *state, double *unused, double *out_7321488301660612988) { - H_27(state, unused, out_7321488301660612988); +void car_H_27(double *state, double *unused, double *out_178426732872669250) { + H_27(state, unused, out_178426732872669250); } -void car_h_29(double *state, double *unused, double *out_1246518834812294588) { - h_29(state, unused, out_1246518834812294588); +void car_h_29(double *state, double *unused, double *out_8023896067125902962) { + h_29(state, unused, out_8023896067125902962); } -void car_H_29(double *state, double *unused, double *out_5608125574791061955) { - H_29(state, unused, out_5608125574791061955); +void car_H_29(double *state, double *unused, double *out_1534935993996881783) { + H_29(state, unused, out_1534935993996881783); } -void car_h_28(double *state, double *unused, double *out_7957031877502237627) { - h_28(state, unused, out_7957031877502237627); +void car_h_28(double *state, double *unused, double *out_5617654677253484567) { + h_28(state, unused, out_5617654677253484567); } -void car_H_28(double *state, double *unused, double *out_7571755846356388206) { - H_28(state, unused, out_7571755846356388206); +void car_H_28(double *state, double *unused, double *out_6617335011066412357) { + H_28(state, unused, out_6617335011066412357); } -void car_h_31(double *state, double *unused, double *out_7192456636178476557) { - h_31(state, unused, out_7192456636178476557); +void car_h_31(double *state, double *unused, double *out_9123987554394577818) { + h_31(state, unused, out_9123987554394577818); } -void car_H_31(double *state, double *unused, double *out_8790507551228313219) { - H_31(state, unused, out_8790507551228313219); +void car_H_31(double *state, double *unused, double *out_4532854334941562166) { + H_31(state, unused, out_4532854334941562166); } 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 29d14ec99..f3a9348bc 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_5399948258775111327); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_538771626761939485); -void car_H_mod_fun(double *state, double *out_6102609910614786292); -void car_f_fun(double *state, double dt, double *out_667706195184437986); -void car_F_fun(double *state, double dt, double *out_157487213156187863); -void car_h_25(double *state, double *unused, double *out_7616837909277052709); -void car_H_25(double *state, double *unused, double *out_8821153513105273647); -void car_h_24(double *state, double *unused, double *out_3643618505328510339); -void car_H_24(double *state, double *unused, double *out_5263227914302950511); -void car_h_30(double *state, double *unused, double *out_6577068605668785750); -void car_H_30(double *state, double *unused, double *out_8950492460248513717); -void car_h_26(double *state, double *unused, double *out_5973752720992308233); -void car_H_26(double *state, double *unused, double *out_5884087241730221745); -void car_h_27(double *state, double *unused, double *out_8560347762178715144); -void car_H_27(double *state, double *unused, double *out_7321488301660612988); -void car_h_29(double *state, double *unused, double *out_1246518834812294588); -void car_H_29(double *state, double *unused, double *out_5608125574791061955); -void car_h_28(double *state, double *unused, double *out_7957031877502237627); -void car_H_28(double *state, double *unused, double *out_7571755846356388206); -void car_h_31(double *state, double *unused, double *out_7192456636178476557); -void car_H_31(double *state, double *unused, double *out_8790507551228313219); +void car_err_fun(double *nom_x, double *delta_x, double *out_2567017903067559466); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1749285410370024184); +void car_H_mod_fun(double *state, double *out_8979551951706364182); +void car_f_fun(double *state, double dt, double *out_3674179640817441392); +void car_F_fun(double *state, double dt, double *out_258925546890284745); +void car_h_25(double *state, double *unused, double *out_5262304077966076166); +void car_H_25(double *state, double *unused, double *out_4563500296818522594); +void car_h_24(double *state, double *unused, double *out_2894074188837958464); +void car_H_24(double *state, double *unused, double *out_2012071509772995507); +void car_h_30(double *state, double *unused, double *out_9005980160461201074); +void car_H_30(double *state, double *unused, double *out_2045167338311273967); +void car_h_26(double *state, double *unused, double *out_8837448028571873025); +void car_H_26(double *state, double *unused, double *out_1258974327057721993); +void car_h_27(double *state, double *unused, double *out_6785398478329626304); +void car_H_27(double *state, double *unused, double *out_178426732872669250); +void car_h_29(double *state, double *unused, double *out_8023896067125902962); +void car_H_29(double *state, double *unused, double *out_1534935993996881783); +void car_h_28(double *state, double *unused, double *out_5617654677253484567); +void car_H_28(double *state, double *unused, double *out_6617335011066412357); +void car_h_31(double *state, double *unused, double *out_9123987554394577818); +void car_H_31(double *state, double *unused, double *out_4532854334941562166); 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 4d383c195..415e6382d 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_9170572944906816852) { - out_9170572944906816852[0] = delta_x[0] + nom_x[0]; - out_9170572944906816852[1] = delta_x[1] + nom_x[1]; - out_9170572944906816852[2] = delta_x[2] + nom_x[2]; - out_9170572944906816852[3] = delta_x[3] + nom_x[3]; - out_9170572944906816852[4] = delta_x[4] + nom_x[4]; - out_9170572944906816852[5] = delta_x[5] + nom_x[5]; - out_9170572944906816852[6] = delta_x[6] + nom_x[6]; - out_9170572944906816852[7] = delta_x[7] + nom_x[7]; - out_9170572944906816852[8] = delta_x[8] + nom_x[8]; - out_9170572944906816852[9] = delta_x[9] + nom_x[9]; - out_9170572944906816852[10] = delta_x[10] + nom_x[10]; - out_9170572944906816852[11] = delta_x[11] + nom_x[11]; - out_9170572944906816852[12] = delta_x[12] + nom_x[12]; - out_9170572944906816852[13] = delta_x[13] + nom_x[13]; - out_9170572944906816852[14] = delta_x[14] + nom_x[14]; - out_9170572944906816852[15] = delta_x[15] + nom_x[15]; - out_9170572944906816852[16] = delta_x[16] + nom_x[16]; - out_9170572944906816852[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_2526150130430510074) { + out_2526150130430510074[0] = delta_x[0] + nom_x[0]; + out_2526150130430510074[1] = delta_x[1] + nom_x[1]; + out_2526150130430510074[2] = delta_x[2] + nom_x[2]; + out_2526150130430510074[3] = delta_x[3] + nom_x[3]; + out_2526150130430510074[4] = delta_x[4] + nom_x[4]; + out_2526150130430510074[5] = delta_x[5] + nom_x[5]; + out_2526150130430510074[6] = delta_x[6] + nom_x[6]; + out_2526150130430510074[7] = delta_x[7] + nom_x[7]; + out_2526150130430510074[8] = delta_x[8] + nom_x[8]; + out_2526150130430510074[9] = delta_x[9] + nom_x[9]; + out_2526150130430510074[10] = delta_x[10] + nom_x[10]; + out_2526150130430510074[11] = delta_x[11] + nom_x[11]; + out_2526150130430510074[12] = delta_x[12] + nom_x[12]; + out_2526150130430510074[13] = delta_x[13] + nom_x[13]; + out_2526150130430510074[14] = delta_x[14] + nom_x[14]; + out_2526150130430510074[15] = delta_x[15] + nom_x[15]; + out_2526150130430510074[16] = delta_x[16] + nom_x[16]; + out_2526150130430510074[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4867215415263963768) { - out_4867215415263963768[0] = -nom_x[0] + true_x[0]; - out_4867215415263963768[1] = -nom_x[1] + true_x[1]; - out_4867215415263963768[2] = -nom_x[2] + true_x[2]; - out_4867215415263963768[3] = -nom_x[3] + true_x[3]; - out_4867215415263963768[4] = -nom_x[4] + true_x[4]; - out_4867215415263963768[5] = -nom_x[5] + true_x[5]; - out_4867215415263963768[6] = -nom_x[6] + true_x[6]; - out_4867215415263963768[7] = -nom_x[7] + true_x[7]; - out_4867215415263963768[8] = -nom_x[8] + true_x[8]; - out_4867215415263963768[9] = -nom_x[9] + true_x[9]; - out_4867215415263963768[10] = -nom_x[10] + true_x[10]; - out_4867215415263963768[11] = -nom_x[11] + true_x[11]; - out_4867215415263963768[12] = -nom_x[12] + true_x[12]; - out_4867215415263963768[13] = -nom_x[13] + true_x[13]; - out_4867215415263963768[14] = -nom_x[14] + true_x[14]; - out_4867215415263963768[15] = -nom_x[15] + true_x[15]; - out_4867215415263963768[16] = -nom_x[16] + true_x[16]; - out_4867215415263963768[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3515561648914426109) { + out_3515561648914426109[0] = -nom_x[0] + true_x[0]; + out_3515561648914426109[1] = -nom_x[1] + true_x[1]; + out_3515561648914426109[2] = -nom_x[2] + true_x[2]; + out_3515561648914426109[3] = -nom_x[3] + true_x[3]; + out_3515561648914426109[4] = -nom_x[4] + true_x[4]; + out_3515561648914426109[5] = -nom_x[5] + true_x[5]; + out_3515561648914426109[6] = -nom_x[6] + true_x[6]; + out_3515561648914426109[7] = -nom_x[7] + true_x[7]; + out_3515561648914426109[8] = -nom_x[8] + true_x[8]; + out_3515561648914426109[9] = -nom_x[9] + true_x[9]; + out_3515561648914426109[10] = -nom_x[10] + true_x[10]; + out_3515561648914426109[11] = -nom_x[11] + true_x[11]; + out_3515561648914426109[12] = -nom_x[12] + true_x[12]; + out_3515561648914426109[13] = -nom_x[13] + true_x[13]; + out_3515561648914426109[14] = -nom_x[14] + true_x[14]; + out_3515561648914426109[15] = -nom_x[15] + true_x[15]; + out_3515561648914426109[16] = -nom_x[16] + true_x[16]; + out_3515561648914426109[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_1396735664605592698) { - out_1396735664605592698[0] = 1.0; - out_1396735664605592698[1] = 0.0; - out_1396735664605592698[2] = 0.0; - out_1396735664605592698[3] = 0.0; - out_1396735664605592698[4] = 0.0; - out_1396735664605592698[5] = 0.0; - out_1396735664605592698[6] = 0.0; - out_1396735664605592698[7] = 0.0; - out_1396735664605592698[8] = 0.0; - out_1396735664605592698[9] = 0.0; - out_1396735664605592698[10] = 0.0; - out_1396735664605592698[11] = 0.0; - out_1396735664605592698[12] = 0.0; - out_1396735664605592698[13] = 0.0; - out_1396735664605592698[14] = 0.0; - out_1396735664605592698[15] = 0.0; - out_1396735664605592698[16] = 0.0; - out_1396735664605592698[17] = 0.0; - out_1396735664605592698[18] = 0.0; - out_1396735664605592698[19] = 1.0; - out_1396735664605592698[20] = 0.0; - out_1396735664605592698[21] = 0.0; - out_1396735664605592698[22] = 0.0; - out_1396735664605592698[23] = 0.0; - out_1396735664605592698[24] = 0.0; - out_1396735664605592698[25] = 0.0; - out_1396735664605592698[26] = 0.0; - out_1396735664605592698[27] = 0.0; - out_1396735664605592698[28] = 0.0; - out_1396735664605592698[29] = 0.0; - out_1396735664605592698[30] = 0.0; - out_1396735664605592698[31] = 0.0; - out_1396735664605592698[32] = 0.0; - out_1396735664605592698[33] = 0.0; - out_1396735664605592698[34] = 0.0; - out_1396735664605592698[35] = 0.0; - out_1396735664605592698[36] = 0.0; - out_1396735664605592698[37] = 0.0; - out_1396735664605592698[38] = 1.0; - out_1396735664605592698[39] = 0.0; - out_1396735664605592698[40] = 0.0; - out_1396735664605592698[41] = 0.0; - out_1396735664605592698[42] = 0.0; - out_1396735664605592698[43] = 0.0; - out_1396735664605592698[44] = 0.0; - out_1396735664605592698[45] = 0.0; - out_1396735664605592698[46] = 0.0; - out_1396735664605592698[47] = 0.0; - out_1396735664605592698[48] = 0.0; - out_1396735664605592698[49] = 0.0; - out_1396735664605592698[50] = 0.0; - out_1396735664605592698[51] = 0.0; - out_1396735664605592698[52] = 0.0; - out_1396735664605592698[53] = 0.0; - out_1396735664605592698[54] = 0.0; - out_1396735664605592698[55] = 0.0; - out_1396735664605592698[56] = 0.0; - out_1396735664605592698[57] = 1.0; - out_1396735664605592698[58] = 0.0; - out_1396735664605592698[59] = 0.0; - out_1396735664605592698[60] = 0.0; - out_1396735664605592698[61] = 0.0; - out_1396735664605592698[62] = 0.0; - out_1396735664605592698[63] = 0.0; - out_1396735664605592698[64] = 0.0; - out_1396735664605592698[65] = 0.0; - out_1396735664605592698[66] = 0.0; - out_1396735664605592698[67] = 0.0; - out_1396735664605592698[68] = 0.0; - out_1396735664605592698[69] = 0.0; - out_1396735664605592698[70] = 0.0; - out_1396735664605592698[71] = 0.0; - out_1396735664605592698[72] = 0.0; - out_1396735664605592698[73] = 0.0; - out_1396735664605592698[74] = 0.0; - out_1396735664605592698[75] = 0.0; - out_1396735664605592698[76] = 1.0; - out_1396735664605592698[77] = 0.0; - out_1396735664605592698[78] = 0.0; - out_1396735664605592698[79] = 0.0; - out_1396735664605592698[80] = 0.0; - out_1396735664605592698[81] = 0.0; - out_1396735664605592698[82] = 0.0; - out_1396735664605592698[83] = 0.0; - out_1396735664605592698[84] = 0.0; - out_1396735664605592698[85] = 0.0; - out_1396735664605592698[86] = 0.0; - out_1396735664605592698[87] = 0.0; - out_1396735664605592698[88] = 0.0; - out_1396735664605592698[89] = 0.0; - out_1396735664605592698[90] = 0.0; - out_1396735664605592698[91] = 0.0; - out_1396735664605592698[92] = 0.0; - out_1396735664605592698[93] = 0.0; - out_1396735664605592698[94] = 0.0; - out_1396735664605592698[95] = 1.0; - out_1396735664605592698[96] = 0.0; - out_1396735664605592698[97] = 0.0; - out_1396735664605592698[98] = 0.0; - out_1396735664605592698[99] = 0.0; - out_1396735664605592698[100] = 0.0; - out_1396735664605592698[101] = 0.0; - out_1396735664605592698[102] = 0.0; - out_1396735664605592698[103] = 0.0; - out_1396735664605592698[104] = 0.0; - out_1396735664605592698[105] = 0.0; - out_1396735664605592698[106] = 0.0; - out_1396735664605592698[107] = 0.0; - out_1396735664605592698[108] = 0.0; - out_1396735664605592698[109] = 0.0; - out_1396735664605592698[110] = 0.0; - out_1396735664605592698[111] = 0.0; - out_1396735664605592698[112] = 0.0; - out_1396735664605592698[113] = 0.0; - out_1396735664605592698[114] = 1.0; - out_1396735664605592698[115] = 0.0; - out_1396735664605592698[116] = 0.0; - out_1396735664605592698[117] = 0.0; - out_1396735664605592698[118] = 0.0; - out_1396735664605592698[119] = 0.0; - out_1396735664605592698[120] = 0.0; - out_1396735664605592698[121] = 0.0; - out_1396735664605592698[122] = 0.0; - out_1396735664605592698[123] = 0.0; - out_1396735664605592698[124] = 0.0; - out_1396735664605592698[125] = 0.0; - out_1396735664605592698[126] = 0.0; - out_1396735664605592698[127] = 0.0; - out_1396735664605592698[128] = 0.0; - out_1396735664605592698[129] = 0.0; - out_1396735664605592698[130] = 0.0; - out_1396735664605592698[131] = 0.0; - out_1396735664605592698[132] = 0.0; - out_1396735664605592698[133] = 1.0; - out_1396735664605592698[134] = 0.0; - out_1396735664605592698[135] = 0.0; - out_1396735664605592698[136] = 0.0; - out_1396735664605592698[137] = 0.0; - out_1396735664605592698[138] = 0.0; - out_1396735664605592698[139] = 0.0; - out_1396735664605592698[140] = 0.0; - out_1396735664605592698[141] = 0.0; - out_1396735664605592698[142] = 0.0; - out_1396735664605592698[143] = 0.0; - out_1396735664605592698[144] = 0.0; - out_1396735664605592698[145] = 0.0; - out_1396735664605592698[146] = 0.0; - out_1396735664605592698[147] = 0.0; - out_1396735664605592698[148] = 0.0; - out_1396735664605592698[149] = 0.0; - out_1396735664605592698[150] = 0.0; - out_1396735664605592698[151] = 0.0; - out_1396735664605592698[152] = 1.0; - out_1396735664605592698[153] = 0.0; - out_1396735664605592698[154] = 0.0; - out_1396735664605592698[155] = 0.0; - out_1396735664605592698[156] = 0.0; - out_1396735664605592698[157] = 0.0; - out_1396735664605592698[158] = 0.0; - out_1396735664605592698[159] = 0.0; - out_1396735664605592698[160] = 0.0; - out_1396735664605592698[161] = 0.0; - out_1396735664605592698[162] = 0.0; - out_1396735664605592698[163] = 0.0; - out_1396735664605592698[164] = 0.0; - out_1396735664605592698[165] = 0.0; - out_1396735664605592698[166] = 0.0; - out_1396735664605592698[167] = 0.0; - out_1396735664605592698[168] = 0.0; - out_1396735664605592698[169] = 0.0; - out_1396735664605592698[170] = 0.0; - out_1396735664605592698[171] = 1.0; - out_1396735664605592698[172] = 0.0; - out_1396735664605592698[173] = 0.0; - out_1396735664605592698[174] = 0.0; - out_1396735664605592698[175] = 0.0; - out_1396735664605592698[176] = 0.0; - out_1396735664605592698[177] = 0.0; - out_1396735664605592698[178] = 0.0; - out_1396735664605592698[179] = 0.0; - out_1396735664605592698[180] = 0.0; - out_1396735664605592698[181] = 0.0; - out_1396735664605592698[182] = 0.0; - out_1396735664605592698[183] = 0.0; - out_1396735664605592698[184] = 0.0; - out_1396735664605592698[185] = 0.0; - out_1396735664605592698[186] = 0.0; - out_1396735664605592698[187] = 0.0; - out_1396735664605592698[188] = 0.0; - out_1396735664605592698[189] = 0.0; - out_1396735664605592698[190] = 1.0; - out_1396735664605592698[191] = 0.0; - out_1396735664605592698[192] = 0.0; - out_1396735664605592698[193] = 0.0; - out_1396735664605592698[194] = 0.0; - out_1396735664605592698[195] = 0.0; - out_1396735664605592698[196] = 0.0; - out_1396735664605592698[197] = 0.0; - out_1396735664605592698[198] = 0.0; - out_1396735664605592698[199] = 0.0; - out_1396735664605592698[200] = 0.0; - out_1396735664605592698[201] = 0.0; - out_1396735664605592698[202] = 0.0; - out_1396735664605592698[203] = 0.0; - out_1396735664605592698[204] = 0.0; - out_1396735664605592698[205] = 0.0; - out_1396735664605592698[206] = 0.0; - out_1396735664605592698[207] = 0.0; - out_1396735664605592698[208] = 0.0; - out_1396735664605592698[209] = 1.0; - out_1396735664605592698[210] = 0.0; - out_1396735664605592698[211] = 0.0; - out_1396735664605592698[212] = 0.0; - out_1396735664605592698[213] = 0.0; - out_1396735664605592698[214] = 0.0; - out_1396735664605592698[215] = 0.0; - out_1396735664605592698[216] = 0.0; - out_1396735664605592698[217] = 0.0; - out_1396735664605592698[218] = 0.0; - out_1396735664605592698[219] = 0.0; - out_1396735664605592698[220] = 0.0; - out_1396735664605592698[221] = 0.0; - out_1396735664605592698[222] = 0.0; - out_1396735664605592698[223] = 0.0; - out_1396735664605592698[224] = 0.0; - out_1396735664605592698[225] = 0.0; - out_1396735664605592698[226] = 0.0; - out_1396735664605592698[227] = 0.0; - out_1396735664605592698[228] = 1.0; - out_1396735664605592698[229] = 0.0; - out_1396735664605592698[230] = 0.0; - out_1396735664605592698[231] = 0.0; - out_1396735664605592698[232] = 0.0; - out_1396735664605592698[233] = 0.0; - out_1396735664605592698[234] = 0.0; - out_1396735664605592698[235] = 0.0; - out_1396735664605592698[236] = 0.0; - out_1396735664605592698[237] = 0.0; - out_1396735664605592698[238] = 0.0; - out_1396735664605592698[239] = 0.0; - out_1396735664605592698[240] = 0.0; - out_1396735664605592698[241] = 0.0; - out_1396735664605592698[242] = 0.0; - out_1396735664605592698[243] = 0.0; - out_1396735664605592698[244] = 0.0; - out_1396735664605592698[245] = 0.0; - out_1396735664605592698[246] = 0.0; - out_1396735664605592698[247] = 1.0; - out_1396735664605592698[248] = 0.0; - out_1396735664605592698[249] = 0.0; - out_1396735664605592698[250] = 0.0; - out_1396735664605592698[251] = 0.0; - out_1396735664605592698[252] = 0.0; - out_1396735664605592698[253] = 0.0; - out_1396735664605592698[254] = 0.0; - out_1396735664605592698[255] = 0.0; - out_1396735664605592698[256] = 0.0; - out_1396735664605592698[257] = 0.0; - out_1396735664605592698[258] = 0.0; - out_1396735664605592698[259] = 0.0; - out_1396735664605592698[260] = 0.0; - out_1396735664605592698[261] = 0.0; - out_1396735664605592698[262] = 0.0; - out_1396735664605592698[263] = 0.0; - out_1396735664605592698[264] = 0.0; - out_1396735664605592698[265] = 0.0; - out_1396735664605592698[266] = 1.0; - out_1396735664605592698[267] = 0.0; - out_1396735664605592698[268] = 0.0; - out_1396735664605592698[269] = 0.0; - out_1396735664605592698[270] = 0.0; - out_1396735664605592698[271] = 0.0; - out_1396735664605592698[272] = 0.0; - out_1396735664605592698[273] = 0.0; - out_1396735664605592698[274] = 0.0; - out_1396735664605592698[275] = 0.0; - out_1396735664605592698[276] = 0.0; - out_1396735664605592698[277] = 0.0; - out_1396735664605592698[278] = 0.0; - out_1396735664605592698[279] = 0.0; - out_1396735664605592698[280] = 0.0; - out_1396735664605592698[281] = 0.0; - out_1396735664605592698[282] = 0.0; - out_1396735664605592698[283] = 0.0; - out_1396735664605592698[284] = 0.0; - out_1396735664605592698[285] = 1.0; - out_1396735664605592698[286] = 0.0; - out_1396735664605592698[287] = 0.0; - out_1396735664605592698[288] = 0.0; - out_1396735664605592698[289] = 0.0; - out_1396735664605592698[290] = 0.0; - out_1396735664605592698[291] = 0.0; - out_1396735664605592698[292] = 0.0; - out_1396735664605592698[293] = 0.0; - out_1396735664605592698[294] = 0.0; - out_1396735664605592698[295] = 0.0; - out_1396735664605592698[296] = 0.0; - out_1396735664605592698[297] = 0.0; - out_1396735664605592698[298] = 0.0; - out_1396735664605592698[299] = 0.0; - out_1396735664605592698[300] = 0.0; - out_1396735664605592698[301] = 0.0; - out_1396735664605592698[302] = 0.0; - out_1396735664605592698[303] = 0.0; - out_1396735664605592698[304] = 1.0; - out_1396735664605592698[305] = 0.0; - out_1396735664605592698[306] = 0.0; - out_1396735664605592698[307] = 0.0; - out_1396735664605592698[308] = 0.0; - out_1396735664605592698[309] = 0.0; - out_1396735664605592698[310] = 0.0; - out_1396735664605592698[311] = 0.0; - out_1396735664605592698[312] = 0.0; - out_1396735664605592698[313] = 0.0; - out_1396735664605592698[314] = 0.0; - out_1396735664605592698[315] = 0.0; - out_1396735664605592698[316] = 0.0; - out_1396735664605592698[317] = 0.0; - out_1396735664605592698[318] = 0.0; - out_1396735664605592698[319] = 0.0; - out_1396735664605592698[320] = 0.0; - out_1396735664605592698[321] = 0.0; - out_1396735664605592698[322] = 0.0; - out_1396735664605592698[323] = 1.0; +void H_mod_fun(double *state, double *out_6056162720073724257) { + out_6056162720073724257[0] = 1.0; + out_6056162720073724257[1] = 0.0; + out_6056162720073724257[2] = 0.0; + out_6056162720073724257[3] = 0.0; + out_6056162720073724257[4] = 0.0; + out_6056162720073724257[5] = 0.0; + out_6056162720073724257[6] = 0.0; + out_6056162720073724257[7] = 0.0; + out_6056162720073724257[8] = 0.0; + out_6056162720073724257[9] = 0.0; + out_6056162720073724257[10] = 0.0; + out_6056162720073724257[11] = 0.0; + out_6056162720073724257[12] = 0.0; + out_6056162720073724257[13] = 0.0; + out_6056162720073724257[14] = 0.0; + out_6056162720073724257[15] = 0.0; + out_6056162720073724257[16] = 0.0; + out_6056162720073724257[17] = 0.0; + out_6056162720073724257[18] = 0.0; + out_6056162720073724257[19] = 1.0; + out_6056162720073724257[20] = 0.0; + out_6056162720073724257[21] = 0.0; + out_6056162720073724257[22] = 0.0; + out_6056162720073724257[23] = 0.0; + out_6056162720073724257[24] = 0.0; + out_6056162720073724257[25] = 0.0; + out_6056162720073724257[26] = 0.0; + out_6056162720073724257[27] = 0.0; + out_6056162720073724257[28] = 0.0; + out_6056162720073724257[29] = 0.0; + out_6056162720073724257[30] = 0.0; + out_6056162720073724257[31] = 0.0; + out_6056162720073724257[32] = 0.0; + out_6056162720073724257[33] = 0.0; + out_6056162720073724257[34] = 0.0; + out_6056162720073724257[35] = 0.0; + out_6056162720073724257[36] = 0.0; + out_6056162720073724257[37] = 0.0; + out_6056162720073724257[38] = 1.0; + out_6056162720073724257[39] = 0.0; + out_6056162720073724257[40] = 0.0; + out_6056162720073724257[41] = 0.0; + out_6056162720073724257[42] = 0.0; + out_6056162720073724257[43] = 0.0; + out_6056162720073724257[44] = 0.0; + out_6056162720073724257[45] = 0.0; + out_6056162720073724257[46] = 0.0; + out_6056162720073724257[47] = 0.0; + out_6056162720073724257[48] = 0.0; + out_6056162720073724257[49] = 0.0; + out_6056162720073724257[50] = 0.0; + out_6056162720073724257[51] = 0.0; + out_6056162720073724257[52] = 0.0; + out_6056162720073724257[53] = 0.0; + out_6056162720073724257[54] = 0.0; + out_6056162720073724257[55] = 0.0; + out_6056162720073724257[56] = 0.0; + out_6056162720073724257[57] = 1.0; + out_6056162720073724257[58] = 0.0; + out_6056162720073724257[59] = 0.0; + out_6056162720073724257[60] = 0.0; + out_6056162720073724257[61] = 0.0; + out_6056162720073724257[62] = 0.0; + out_6056162720073724257[63] = 0.0; + out_6056162720073724257[64] = 0.0; + out_6056162720073724257[65] = 0.0; + out_6056162720073724257[66] = 0.0; + out_6056162720073724257[67] = 0.0; + out_6056162720073724257[68] = 0.0; + out_6056162720073724257[69] = 0.0; + out_6056162720073724257[70] = 0.0; + out_6056162720073724257[71] = 0.0; + out_6056162720073724257[72] = 0.0; + out_6056162720073724257[73] = 0.0; + out_6056162720073724257[74] = 0.0; + out_6056162720073724257[75] = 0.0; + out_6056162720073724257[76] = 1.0; + out_6056162720073724257[77] = 0.0; + out_6056162720073724257[78] = 0.0; + out_6056162720073724257[79] = 0.0; + out_6056162720073724257[80] = 0.0; + out_6056162720073724257[81] = 0.0; + out_6056162720073724257[82] = 0.0; + out_6056162720073724257[83] = 0.0; + out_6056162720073724257[84] = 0.0; + out_6056162720073724257[85] = 0.0; + out_6056162720073724257[86] = 0.0; + out_6056162720073724257[87] = 0.0; + out_6056162720073724257[88] = 0.0; + out_6056162720073724257[89] = 0.0; + out_6056162720073724257[90] = 0.0; + out_6056162720073724257[91] = 0.0; + out_6056162720073724257[92] = 0.0; + out_6056162720073724257[93] = 0.0; + out_6056162720073724257[94] = 0.0; + out_6056162720073724257[95] = 1.0; + out_6056162720073724257[96] = 0.0; + out_6056162720073724257[97] = 0.0; + out_6056162720073724257[98] = 0.0; + out_6056162720073724257[99] = 0.0; + out_6056162720073724257[100] = 0.0; + out_6056162720073724257[101] = 0.0; + out_6056162720073724257[102] = 0.0; + out_6056162720073724257[103] = 0.0; + out_6056162720073724257[104] = 0.0; + out_6056162720073724257[105] = 0.0; + out_6056162720073724257[106] = 0.0; + out_6056162720073724257[107] = 0.0; + out_6056162720073724257[108] = 0.0; + out_6056162720073724257[109] = 0.0; + out_6056162720073724257[110] = 0.0; + out_6056162720073724257[111] = 0.0; + out_6056162720073724257[112] = 0.0; + out_6056162720073724257[113] = 0.0; + out_6056162720073724257[114] = 1.0; + out_6056162720073724257[115] = 0.0; + out_6056162720073724257[116] = 0.0; + out_6056162720073724257[117] = 0.0; + out_6056162720073724257[118] = 0.0; + out_6056162720073724257[119] = 0.0; + out_6056162720073724257[120] = 0.0; + out_6056162720073724257[121] = 0.0; + out_6056162720073724257[122] = 0.0; + out_6056162720073724257[123] = 0.0; + out_6056162720073724257[124] = 0.0; + out_6056162720073724257[125] = 0.0; + out_6056162720073724257[126] = 0.0; + out_6056162720073724257[127] = 0.0; + out_6056162720073724257[128] = 0.0; + out_6056162720073724257[129] = 0.0; + out_6056162720073724257[130] = 0.0; + out_6056162720073724257[131] = 0.0; + out_6056162720073724257[132] = 0.0; + out_6056162720073724257[133] = 1.0; + out_6056162720073724257[134] = 0.0; + out_6056162720073724257[135] = 0.0; + out_6056162720073724257[136] = 0.0; + out_6056162720073724257[137] = 0.0; + out_6056162720073724257[138] = 0.0; + out_6056162720073724257[139] = 0.0; + out_6056162720073724257[140] = 0.0; + out_6056162720073724257[141] = 0.0; + out_6056162720073724257[142] = 0.0; + out_6056162720073724257[143] = 0.0; + out_6056162720073724257[144] = 0.0; + out_6056162720073724257[145] = 0.0; + out_6056162720073724257[146] = 0.0; + out_6056162720073724257[147] = 0.0; + out_6056162720073724257[148] = 0.0; + out_6056162720073724257[149] = 0.0; + out_6056162720073724257[150] = 0.0; + out_6056162720073724257[151] = 0.0; + out_6056162720073724257[152] = 1.0; + out_6056162720073724257[153] = 0.0; + out_6056162720073724257[154] = 0.0; + out_6056162720073724257[155] = 0.0; + out_6056162720073724257[156] = 0.0; + out_6056162720073724257[157] = 0.0; + out_6056162720073724257[158] = 0.0; + out_6056162720073724257[159] = 0.0; + out_6056162720073724257[160] = 0.0; + out_6056162720073724257[161] = 0.0; + out_6056162720073724257[162] = 0.0; + out_6056162720073724257[163] = 0.0; + out_6056162720073724257[164] = 0.0; + out_6056162720073724257[165] = 0.0; + out_6056162720073724257[166] = 0.0; + out_6056162720073724257[167] = 0.0; + out_6056162720073724257[168] = 0.0; + out_6056162720073724257[169] = 0.0; + out_6056162720073724257[170] = 0.0; + out_6056162720073724257[171] = 1.0; + out_6056162720073724257[172] = 0.0; + out_6056162720073724257[173] = 0.0; + out_6056162720073724257[174] = 0.0; + out_6056162720073724257[175] = 0.0; + out_6056162720073724257[176] = 0.0; + out_6056162720073724257[177] = 0.0; + out_6056162720073724257[178] = 0.0; + out_6056162720073724257[179] = 0.0; + out_6056162720073724257[180] = 0.0; + out_6056162720073724257[181] = 0.0; + out_6056162720073724257[182] = 0.0; + out_6056162720073724257[183] = 0.0; + out_6056162720073724257[184] = 0.0; + out_6056162720073724257[185] = 0.0; + out_6056162720073724257[186] = 0.0; + out_6056162720073724257[187] = 0.0; + out_6056162720073724257[188] = 0.0; + out_6056162720073724257[189] = 0.0; + out_6056162720073724257[190] = 1.0; + out_6056162720073724257[191] = 0.0; + out_6056162720073724257[192] = 0.0; + out_6056162720073724257[193] = 0.0; + out_6056162720073724257[194] = 0.0; + out_6056162720073724257[195] = 0.0; + out_6056162720073724257[196] = 0.0; + out_6056162720073724257[197] = 0.0; + out_6056162720073724257[198] = 0.0; + out_6056162720073724257[199] = 0.0; + out_6056162720073724257[200] = 0.0; + out_6056162720073724257[201] = 0.0; + out_6056162720073724257[202] = 0.0; + out_6056162720073724257[203] = 0.0; + out_6056162720073724257[204] = 0.0; + out_6056162720073724257[205] = 0.0; + out_6056162720073724257[206] = 0.0; + out_6056162720073724257[207] = 0.0; + out_6056162720073724257[208] = 0.0; + out_6056162720073724257[209] = 1.0; + out_6056162720073724257[210] = 0.0; + out_6056162720073724257[211] = 0.0; + out_6056162720073724257[212] = 0.0; + out_6056162720073724257[213] = 0.0; + out_6056162720073724257[214] = 0.0; + out_6056162720073724257[215] = 0.0; + out_6056162720073724257[216] = 0.0; + out_6056162720073724257[217] = 0.0; + out_6056162720073724257[218] = 0.0; + out_6056162720073724257[219] = 0.0; + out_6056162720073724257[220] = 0.0; + out_6056162720073724257[221] = 0.0; + out_6056162720073724257[222] = 0.0; + out_6056162720073724257[223] = 0.0; + out_6056162720073724257[224] = 0.0; + out_6056162720073724257[225] = 0.0; + out_6056162720073724257[226] = 0.0; + out_6056162720073724257[227] = 0.0; + out_6056162720073724257[228] = 1.0; + out_6056162720073724257[229] = 0.0; + out_6056162720073724257[230] = 0.0; + out_6056162720073724257[231] = 0.0; + out_6056162720073724257[232] = 0.0; + out_6056162720073724257[233] = 0.0; + out_6056162720073724257[234] = 0.0; + out_6056162720073724257[235] = 0.0; + out_6056162720073724257[236] = 0.0; + out_6056162720073724257[237] = 0.0; + out_6056162720073724257[238] = 0.0; + out_6056162720073724257[239] = 0.0; + out_6056162720073724257[240] = 0.0; + out_6056162720073724257[241] = 0.0; + out_6056162720073724257[242] = 0.0; + out_6056162720073724257[243] = 0.0; + out_6056162720073724257[244] = 0.0; + out_6056162720073724257[245] = 0.0; + out_6056162720073724257[246] = 0.0; + out_6056162720073724257[247] = 1.0; + out_6056162720073724257[248] = 0.0; + out_6056162720073724257[249] = 0.0; + out_6056162720073724257[250] = 0.0; + out_6056162720073724257[251] = 0.0; + out_6056162720073724257[252] = 0.0; + out_6056162720073724257[253] = 0.0; + out_6056162720073724257[254] = 0.0; + out_6056162720073724257[255] = 0.0; + out_6056162720073724257[256] = 0.0; + out_6056162720073724257[257] = 0.0; + out_6056162720073724257[258] = 0.0; + out_6056162720073724257[259] = 0.0; + out_6056162720073724257[260] = 0.0; + out_6056162720073724257[261] = 0.0; + out_6056162720073724257[262] = 0.0; + out_6056162720073724257[263] = 0.0; + out_6056162720073724257[264] = 0.0; + out_6056162720073724257[265] = 0.0; + out_6056162720073724257[266] = 1.0; + out_6056162720073724257[267] = 0.0; + out_6056162720073724257[268] = 0.0; + out_6056162720073724257[269] = 0.0; + out_6056162720073724257[270] = 0.0; + out_6056162720073724257[271] = 0.0; + out_6056162720073724257[272] = 0.0; + out_6056162720073724257[273] = 0.0; + out_6056162720073724257[274] = 0.0; + out_6056162720073724257[275] = 0.0; + out_6056162720073724257[276] = 0.0; + out_6056162720073724257[277] = 0.0; + out_6056162720073724257[278] = 0.0; + out_6056162720073724257[279] = 0.0; + out_6056162720073724257[280] = 0.0; + out_6056162720073724257[281] = 0.0; + out_6056162720073724257[282] = 0.0; + out_6056162720073724257[283] = 0.0; + out_6056162720073724257[284] = 0.0; + out_6056162720073724257[285] = 1.0; + out_6056162720073724257[286] = 0.0; + out_6056162720073724257[287] = 0.0; + out_6056162720073724257[288] = 0.0; + out_6056162720073724257[289] = 0.0; + out_6056162720073724257[290] = 0.0; + out_6056162720073724257[291] = 0.0; + out_6056162720073724257[292] = 0.0; + out_6056162720073724257[293] = 0.0; + out_6056162720073724257[294] = 0.0; + out_6056162720073724257[295] = 0.0; + out_6056162720073724257[296] = 0.0; + out_6056162720073724257[297] = 0.0; + out_6056162720073724257[298] = 0.0; + out_6056162720073724257[299] = 0.0; + out_6056162720073724257[300] = 0.0; + out_6056162720073724257[301] = 0.0; + out_6056162720073724257[302] = 0.0; + out_6056162720073724257[303] = 0.0; + out_6056162720073724257[304] = 1.0; + out_6056162720073724257[305] = 0.0; + out_6056162720073724257[306] = 0.0; + out_6056162720073724257[307] = 0.0; + out_6056162720073724257[308] = 0.0; + out_6056162720073724257[309] = 0.0; + out_6056162720073724257[310] = 0.0; + out_6056162720073724257[311] = 0.0; + out_6056162720073724257[312] = 0.0; + out_6056162720073724257[313] = 0.0; + out_6056162720073724257[314] = 0.0; + out_6056162720073724257[315] = 0.0; + out_6056162720073724257[316] = 0.0; + out_6056162720073724257[317] = 0.0; + out_6056162720073724257[318] = 0.0; + out_6056162720073724257[319] = 0.0; + out_6056162720073724257[320] = 0.0; + out_6056162720073724257[321] = 0.0; + out_6056162720073724257[322] = 0.0; + out_6056162720073724257[323] = 1.0; } -void f_fun(double *state, double dt, double *out_916482441408583797) { - out_916482441408583797[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_916482441408583797[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_916482441408583797[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_916482441408583797[3] = dt*state[12] + state[3]; - out_916482441408583797[4] = dt*state[13] + state[4]; - out_916482441408583797[5] = dt*state[14] + state[5]; - out_916482441408583797[6] = state[6]; - out_916482441408583797[7] = state[7]; - out_916482441408583797[8] = state[8]; - out_916482441408583797[9] = state[9]; - out_916482441408583797[10] = state[10]; - out_916482441408583797[11] = state[11]; - out_916482441408583797[12] = state[12]; - out_916482441408583797[13] = state[13]; - out_916482441408583797[14] = state[14]; - out_916482441408583797[15] = state[15]; - out_916482441408583797[16] = state[16]; - out_916482441408583797[17] = state[17]; +void f_fun(double *state, double dt, double *out_8780284750928315681) { + out_8780284750928315681[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_8780284750928315681[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_8780284750928315681[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_8780284750928315681[3] = dt*state[12] + state[3]; + out_8780284750928315681[4] = dt*state[13] + state[4]; + out_8780284750928315681[5] = dt*state[14] + state[5]; + out_8780284750928315681[6] = state[6]; + out_8780284750928315681[7] = state[7]; + out_8780284750928315681[8] = state[8]; + out_8780284750928315681[9] = state[9]; + out_8780284750928315681[10] = state[10]; + out_8780284750928315681[11] = state[11]; + out_8780284750928315681[12] = state[12]; + out_8780284750928315681[13] = state[13]; + out_8780284750928315681[14] = state[14]; + out_8780284750928315681[15] = state[15]; + out_8780284750928315681[16] = state[16]; + out_8780284750928315681[17] = state[17]; } -void F_fun(double *state, double dt, double *out_5639772936950525263) { - out_5639772936950525263[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_5639772936950525263[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_5639772936950525263[2] = 0; - out_5639772936950525263[3] = 0; - out_5639772936950525263[4] = 0; - out_5639772936950525263[5] = 0; - out_5639772936950525263[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_5639772936950525263[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_5639772936950525263[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_5639772936950525263[9] = 0; - out_5639772936950525263[10] = 0; - out_5639772936950525263[11] = 0; - out_5639772936950525263[12] = 0; - out_5639772936950525263[13] = 0; - out_5639772936950525263[14] = 0; - out_5639772936950525263[15] = 0; - out_5639772936950525263[16] = 0; - out_5639772936950525263[17] = 0; - out_5639772936950525263[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_5639772936950525263[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_5639772936950525263[20] = 0; - out_5639772936950525263[21] = 0; - out_5639772936950525263[22] = 0; - out_5639772936950525263[23] = 0; - out_5639772936950525263[24] = 0; - out_5639772936950525263[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_5639772936950525263[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_5639772936950525263[27] = 0; - out_5639772936950525263[28] = 0; - out_5639772936950525263[29] = 0; - out_5639772936950525263[30] = 0; - out_5639772936950525263[31] = 0; - out_5639772936950525263[32] = 0; - out_5639772936950525263[33] = 0; - out_5639772936950525263[34] = 0; - out_5639772936950525263[35] = 0; - out_5639772936950525263[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_5639772936950525263[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_5639772936950525263[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_5639772936950525263[39] = 0; - out_5639772936950525263[40] = 0; - out_5639772936950525263[41] = 0; - out_5639772936950525263[42] = 0; - out_5639772936950525263[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_5639772936950525263[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_5639772936950525263[45] = 0; - out_5639772936950525263[46] = 0; - out_5639772936950525263[47] = 0; - out_5639772936950525263[48] = 0; - out_5639772936950525263[49] = 0; - out_5639772936950525263[50] = 0; - out_5639772936950525263[51] = 0; - out_5639772936950525263[52] = 0; - out_5639772936950525263[53] = 0; - out_5639772936950525263[54] = 0; - out_5639772936950525263[55] = 0; - out_5639772936950525263[56] = 0; - out_5639772936950525263[57] = 1; - out_5639772936950525263[58] = 0; - out_5639772936950525263[59] = 0; - out_5639772936950525263[60] = 0; - out_5639772936950525263[61] = 0; - out_5639772936950525263[62] = 0; - out_5639772936950525263[63] = 0; - out_5639772936950525263[64] = 0; - out_5639772936950525263[65] = 0; - out_5639772936950525263[66] = dt; - out_5639772936950525263[67] = 0; - out_5639772936950525263[68] = 0; - out_5639772936950525263[69] = 0; - out_5639772936950525263[70] = 0; - out_5639772936950525263[71] = 0; - out_5639772936950525263[72] = 0; - out_5639772936950525263[73] = 0; - out_5639772936950525263[74] = 0; - out_5639772936950525263[75] = 0; - out_5639772936950525263[76] = 1; - out_5639772936950525263[77] = 0; - out_5639772936950525263[78] = 0; - out_5639772936950525263[79] = 0; - out_5639772936950525263[80] = 0; - out_5639772936950525263[81] = 0; - out_5639772936950525263[82] = 0; - out_5639772936950525263[83] = 0; - out_5639772936950525263[84] = 0; - out_5639772936950525263[85] = dt; - out_5639772936950525263[86] = 0; - out_5639772936950525263[87] = 0; - out_5639772936950525263[88] = 0; - out_5639772936950525263[89] = 0; - out_5639772936950525263[90] = 0; - out_5639772936950525263[91] = 0; - out_5639772936950525263[92] = 0; - out_5639772936950525263[93] = 0; - out_5639772936950525263[94] = 0; - out_5639772936950525263[95] = 1; - out_5639772936950525263[96] = 0; - out_5639772936950525263[97] = 0; - out_5639772936950525263[98] = 0; - out_5639772936950525263[99] = 0; - out_5639772936950525263[100] = 0; - out_5639772936950525263[101] = 0; - out_5639772936950525263[102] = 0; - out_5639772936950525263[103] = 0; - out_5639772936950525263[104] = dt; - out_5639772936950525263[105] = 0; - out_5639772936950525263[106] = 0; - out_5639772936950525263[107] = 0; - out_5639772936950525263[108] = 0; - out_5639772936950525263[109] = 0; - out_5639772936950525263[110] = 0; - out_5639772936950525263[111] = 0; - out_5639772936950525263[112] = 0; - out_5639772936950525263[113] = 0; - out_5639772936950525263[114] = 1; - out_5639772936950525263[115] = 0; - out_5639772936950525263[116] = 0; - out_5639772936950525263[117] = 0; - out_5639772936950525263[118] = 0; - out_5639772936950525263[119] = 0; - out_5639772936950525263[120] = 0; - out_5639772936950525263[121] = 0; - out_5639772936950525263[122] = 0; - out_5639772936950525263[123] = 0; - out_5639772936950525263[124] = 0; - out_5639772936950525263[125] = 0; - out_5639772936950525263[126] = 0; - out_5639772936950525263[127] = 0; - out_5639772936950525263[128] = 0; - out_5639772936950525263[129] = 0; - out_5639772936950525263[130] = 0; - out_5639772936950525263[131] = 0; - out_5639772936950525263[132] = 0; - out_5639772936950525263[133] = 1; - out_5639772936950525263[134] = 0; - out_5639772936950525263[135] = 0; - out_5639772936950525263[136] = 0; - out_5639772936950525263[137] = 0; - out_5639772936950525263[138] = 0; - out_5639772936950525263[139] = 0; - out_5639772936950525263[140] = 0; - out_5639772936950525263[141] = 0; - out_5639772936950525263[142] = 0; - out_5639772936950525263[143] = 0; - out_5639772936950525263[144] = 0; - out_5639772936950525263[145] = 0; - out_5639772936950525263[146] = 0; - out_5639772936950525263[147] = 0; - out_5639772936950525263[148] = 0; - out_5639772936950525263[149] = 0; - out_5639772936950525263[150] = 0; - out_5639772936950525263[151] = 0; - out_5639772936950525263[152] = 1; - out_5639772936950525263[153] = 0; - out_5639772936950525263[154] = 0; - out_5639772936950525263[155] = 0; - out_5639772936950525263[156] = 0; - out_5639772936950525263[157] = 0; - out_5639772936950525263[158] = 0; - out_5639772936950525263[159] = 0; - out_5639772936950525263[160] = 0; - out_5639772936950525263[161] = 0; - out_5639772936950525263[162] = 0; - out_5639772936950525263[163] = 0; - out_5639772936950525263[164] = 0; - out_5639772936950525263[165] = 0; - out_5639772936950525263[166] = 0; - out_5639772936950525263[167] = 0; - out_5639772936950525263[168] = 0; - out_5639772936950525263[169] = 0; - out_5639772936950525263[170] = 0; - out_5639772936950525263[171] = 1; - out_5639772936950525263[172] = 0; - out_5639772936950525263[173] = 0; - out_5639772936950525263[174] = 0; - out_5639772936950525263[175] = 0; - out_5639772936950525263[176] = 0; - out_5639772936950525263[177] = 0; - out_5639772936950525263[178] = 0; - out_5639772936950525263[179] = 0; - out_5639772936950525263[180] = 0; - out_5639772936950525263[181] = 0; - out_5639772936950525263[182] = 0; - out_5639772936950525263[183] = 0; - out_5639772936950525263[184] = 0; - out_5639772936950525263[185] = 0; - out_5639772936950525263[186] = 0; - out_5639772936950525263[187] = 0; - out_5639772936950525263[188] = 0; - out_5639772936950525263[189] = 0; - out_5639772936950525263[190] = 1; - out_5639772936950525263[191] = 0; - out_5639772936950525263[192] = 0; - out_5639772936950525263[193] = 0; - out_5639772936950525263[194] = 0; - out_5639772936950525263[195] = 0; - out_5639772936950525263[196] = 0; - out_5639772936950525263[197] = 0; - out_5639772936950525263[198] = 0; - out_5639772936950525263[199] = 0; - out_5639772936950525263[200] = 0; - out_5639772936950525263[201] = 0; - out_5639772936950525263[202] = 0; - out_5639772936950525263[203] = 0; - out_5639772936950525263[204] = 0; - out_5639772936950525263[205] = 0; - out_5639772936950525263[206] = 0; - out_5639772936950525263[207] = 0; - out_5639772936950525263[208] = 0; - out_5639772936950525263[209] = 1; - out_5639772936950525263[210] = 0; - out_5639772936950525263[211] = 0; - out_5639772936950525263[212] = 0; - out_5639772936950525263[213] = 0; - out_5639772936950525263[214] = 0; - out_5639772936950525263[215] = 0; - out_5639772936950525263[216] = 0; - out_5639772936950525263[217] = 0; - out_5639772936950525263[218] = 0; - out_5639772936950525263[219] = 0; - out_5639772936950525263[220] = 0; - out_5639772936950525263[221] = 0; - out_5639772936950525263[222] = 0; - out_5639772936950525263[223] = 0; - out_5639772936950525263[224] = 0; - out_5639772936950525263[225] = 0; - out_5639772936950525263[226] = 0; - out_5639772936950525263[227] = 0; - out_5639772936950525263[228] = 1; - out_5639772936950525263[229] = 0; - out_5639772936950525263[230] = 0; - out_5639772936950525263[231] = 0; - out_5639772936950525263[232] = 0; - out_5639772936950525263[233] = 0; - out_5639772936950525263[234] = 0; - out_5639772936950525263[235] = 0; - out_5639772936950525263[236] = 0; - out_5639772936950525263[237] = 0; - out_5639772936950525263[238] = 0; - out_5639772936950525263[239] = 0; - out_5639772936950525263[240] = 0; - out_5639772936950525263[241] = 0; - out_5639772936950525263[242] = 0; - out_5639772936950525263[243] = 0; - out_5639772936950525263[244] = 0; - out_5639772936950525263[245] = 0; - out_5639772936950525263[246] = 0; - out_5639772936950525263[247] = 1; - out_5639772936950525263[248] = 0; - out_5639772936950525263[249] = 0; - out_5639772936950525263[250] = 0; - out_5639772936950525263[251] = 0; - out_5639772936950525263[252] = 0; - out_5639772936950525263[253] = 0; - out_5639772936950525263[254] = 0; - out_5639772936950525263[255] = 0; - out_5639772936950525263[256] = 0; - out_5639772936950525263[257] = 0; - out_5639772936950525263[258] = 0; - out_5639772936950525263[259] = 0; - out_5639772936950525263[260] = 0; - out_5639772936950525263[261] = 0; - out_5639772936950525263[262] = 0; - out_5639772936950525263[263] = 0; - out_5639772936950525263[264] = 0; - out_5639772936950525263[265] = 0; - out_5639772936950525263[266] = 1; - out_5639772936950525263[267] = 0; - out_5639772936950525263[268] = 0; - out_5639772936950525263[269] = 0; - out_5639772936950525263[270] = 0; - out_5639772936950525263[271] = 0; - out_5639772936950525263[272] = 0; - out_5639772936950525263[273] = 0; - out_5639772936950525263[274] = 0; - out_5639772936950525263[275] = 0; - out_5639772936950525263[276] = 0; - out_5639772936950525263[277] = 0; - out_5639772936950525263[278] = 0; - out_5639772936950525263[279] = 0; - out_5639772936950525263[280] = 0; - out_5639772936950525263[281] = 0; - out_5639772936950525263[282] = 0; - out_5639772936950525263[283] = 0; - out_5639772936950525263[284] = 0; - out_5639772936950525263[285] = 1; - out_5639772936950525263[286] = 0; - out_5639772936950525263[287] = 0; - out_5639772936950525263[288] = 0; - out_5639772936950525263[289] = 0; - out_5639772936950525263[290] = 0; - out_5639772936950525263[291] = 0; - out_5639772936950525263[292] = 0; - out_5639772936950525263[293] = 0; - out_5639772936950525263[294] = 0; - out_5639772936950525263[295] = 0; - out_5639772936950525263[296] = 0; - out_5639772936950525263[297] = 0; - out_5639772936950525263[298] = 0; - out_5639772936950525263[299] = 0; - out_5639772936950525263[300] = 0; - out_5639772936950525263[301] = 0; - out_5639772936950525263[302] = 0; - out_5639772936950525263[303] = 0; - out_5639772936950525263[304] = 1; - out_5639772936950525263[305] = 0; - out_5639772936950525263[306] = 0; - out_5639772936950525263[307] = 0; - out_5639772936950525263[308] = 0; - out_5639772936950525263[309] = 0; - out_5639772936950525263[310] = 0; - out_5639772936950525263[311] = 0; - out_5639772936950525263[312] = 0; - out_5639772936950525263[313] = 0; - out_5639772936950525263[314] = 0; - out_5639772936950525263[315] = 0; - out_5639772936950525263[316] = 0; - out_5639772936950525263[317] = 0; - out_5639772936950525263[318] = 0; - out_5639772936950525263[319] = 0; - out_5639772936950525263[320] = 0; - out_5639772936950525263[321] = 0; - out_5639772936950525263[322] = 0; - out_5639772936950525263[323] = 1; +void F_fun(double *state, double dt, double *out_1516035527731464330) { + out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[2] = 0; + out_1516035527731464330[3] = 0; + out_1516035527731464330[4] = 0; + out_1516035527731464330[5] = 0; + out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[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_1516035527731464330[9] = 0; + out_1516035527731464330[10] = 0; + out_1516035527731464330[11] = 0; + out_1516035527731464330[12] = 0; + out_1516035527731464330[13] = 0; + out_1516035527731464330[14] = 0; + out_1516035527731464330[15] = 0; + out_1516035527731464330[16] = 0; + out_1516035527731464330[17] = 0; + out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[20] = 0; + out_1516035527731464330[21] = 0; + out_1516035527731464330[22] = 0; + out_1516035527731464330[23] = 0; + out_1516035527731464330[24] = 0; + out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[27] = 0; + out_1516035527731464330[28] = 0; + out_1516035527731464330[29] = 0; + out_1516035527731464330[30] = 0; + out_1516035527731464330[31] = 0; + out_1516035527731464330[32] = 0; + out_1516035527731464330[33] = 0; + out_1516035527731464330[34] = 0; + out_1516035527731464330[35] = 0; + out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[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_1516035527731464330[39] = 0; + out_1516035527731464330[40] = 0; + out_1516035527731464330[41] = 0; + out_1516035527731464330[42] = 0; + out_1516035527731464330[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_1516035527731464330[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_1516035527731464330[45] = 0; + out_1516035527731464330[46] = 0; + out_1516035527731464330[47] = 0; + out_1516035527731464330[48] = 0; + out_1516035527731464330[49] = 0; + out_1516035527731464330[50] = 0; + out_1516035527731464330[51] = 0; + out_1516035527731464330[52] = 0; + out_1516035527731464330[53] = 0; + out_1516035527731464330[54] = 0; + out_1516035527731464330[55] = 0; + out_1516035527731464330[56] = 0; + out_1516035527731464330[57] = 1; + out_1516035527731464330[58] = 0; + out_1516035527731464330[59] = 0; + out_1516035527731464330[60] = 0; + out_1516035527731464330[61] = 0; + out_1516035527731464330[62] = 0; + out_1516035527731464330[63] = 0; + out_1516035527731464330[64] = 0; + out_1516035527731464330[65] = 0; + out_1516035527731464330[66] = dt; + out_1516035527731464330[67] = 0; + out_1516035527731464330[68] = 0; + out_1516035527731464330[69] = 0; + out_1516035527731464330[70] = 0; + out_1516035527731464330[71] = 0; + out_1516035527731464330[72] = 0; + out_1516035527731464330[73] = 0; + out_1516035527731464330[74] = 0; + out_1516035527731464330[75] = 0; + out_1516035527731464330[76] = 1; + out_1516035527731464330[77] = 0; + out_1516035527731464330[78] = 0; + out_1516035527731464330[79] = 0; + out_1516035527731464330[80] = 0; + out_1516035527731464330[81] = 0; + out_1516035527731464330[82] = 0; + out_1516035527731464330[83] = 0; + out_1516035527731464330[84] = 0; + out_1516035527731464330[85] = dt; + out_1516035527731464330[86] = 0; + out_1516035527731464330[87] = 0; + out_1516035527731464330[88] = 0; + out_1516035527731464330[89] = 0; + out_1516035527731464330[90] = 0; + out_1516035527731464330[91] = 0; + out_1516035527731464330[92] = 0; + out_1516035527731464330[93] = 0; + out_1516035527731464330[94] = 0; + out_1516035527731464330[95] = 1; + out_1516035527731464330[96] = 0; + out_1516035527731464330[97] = 0; + out_1516035527731464330[98] = 0; + out_1516035527731464330[99] = 0; + out_1516035527731464330[100] = 0; + out_1516035527731464330[101] = 0; + out_1516035527731464330[102] = 0; + out_1516035527731464330[103] = 0; + out_1516035527731464330[104] = dt; + out_1516035527731464330[105] = 0; + out_1516035527731464330[106] = 0; + out_1516035527731464330[107] = 0; + out_1516035527731464330[108] = 0; + out_1516035527731464330[109] = 0; + out_1516035527731464330[110] = 0; + out_1516035527731464330[111] = 0; + out_1516035527731464330[112] = 0; + out_1516035527731464330[113] = 0; + out_1516035527731464330[114] = 1; + out_1516035527731464330[115] = 0; + out_1516035527731464330[116] = 0; + out_1516035527731464330[117] = 0; + out_1516035527731464330[118] = 0; + out_1516035527731464330[119] = 0; + out_1516035527731464330[120] = 0; + out_1516035527731464330[121] = 0; + out_1516035527731464330[122] = 0; + out_1516035527731464330[123] = 0; + out_1516035527731464330[124] = 0; + out_1516035527731464330[125] = 0; + out_1516035527731464330[126] = 0; + out_1516035527731464330[127] = 0; + out_1516035527731464330[128] = 0; + out_1516035527731464330[129] = 0; + out_1516035527731464330[130] = 0; + out_1516035527731464330[131] = 0; + out_1516035527731464330[132] = 0; + out_1516035527731464330[133] = 1; + out_1516035527731464330[134] = 0; + out_1516035527731464330[135] = 0; + out_1516035527731464330[136] = 0; + out_1516035527731464330[137] = 0; + out_1516035527731464330[138] = 0; + out_1516035527731464330[139] = 0; + out_1516035527731464330[140] = 0; + out_1516035527731464330[141] = 0; + out_1516035527731464330[142] = 0; + out_1516035527731464330[143] = 0; + out_1516035527731464330[144] = 0; + out_1516035527731464330[145] = 0; + out_1516035527731464330[146] = 0; + out_1516035527731464330[147] = 0; + out_1516035527731464330[148] = 0; + out_1516035527731464330[149] = 0; + out_1516035527731464330[150] = 0; + out_1516035527731464330[151] = 0; + out_1516035527731464330[152] = 1; + out_1516035527731464330[153] = 0; + out_1516035527731464330[154] = 0; + out_1516035527731464330[155] = 0; + out_1516035527731464330[156] = 0; + out_1516035527731464330[157] = 0; + out_1516035527731464330[158] = 0; + out_1516035527731464330[159] = 0; + out_1516035527731464330[160] = 0; + out_1516035527731464330[161] = 0; + out_1516035527731464330[162] = 0; + out_1516035527731464330[163] = 0; + out_1516035527731464330[164] = 0; + out_1516035527731464330[165] = 0; + out_1516035527731464330[166] = 0; + out_1516035527731464330[167] = 0; + out_1516035527731464330[168] = 0; + out_1516035527731464330[169] = 0; + out_1516035527731464330[170] = 0; + out_1516035527731464330[171] = 1; + out_1516035527731464330[172] = 0; + out_1516035527731464330[173] = 0; + out_1516035527731464330[174] = 0; + out_1516035527731464330[175] = 0; + out_1516035527731464330[176] = 0; + out_1516035527731464330[177] = 0; + out_1516035527731464330[178] = 0; + out_1516035527731464330[179] = 0; + out_1516035527731464330[180] = 0; + out_1516035527731464330[181] = 0; + out_1516035527731464330[182] = 0; + out_1516035527731464330[183] = 0; + out_1516035527731464330[184] = 0; + out_1516035527731464330[185] = 0; + out_1516035527731464330[186] = 0; + out_1516035527731464330[187] = 0; + out_1516035527731464330[188] = 0; + out_1516035527731464330[189] = 0; + out_1516035527731464330[190] = 1; + out_1516035527731464330[191] = 0; + out_1516035527731464330[192] = 0; + out_1516035527731464330[193] = 0; + out_1516035527731464330[194] = 0; + out_1516035527731464330[195] = 0; + out_1516035527731464330[196] = 0; + out_1516035527731464330[197] = 0; + out_1516035527731464330[198] = 0; + out_1516035527731464330[199] = 0; + out_1516035527731464330[200] = 0; + out_1516035527731464330[201] = 0; + out_1516035527731464330[202] = 0; + out_1516035527731464330[203] = 0; + out_1516035527731464330[204] = 0; + out_1516035527731464330[205] = 0; + out_1516035527731464330[206] = 0; + out_1516035527731464330[207] = 0; + out_1516035527731464330[208] = 0; + out_1516035527731464330[209] = 1; + out_1516035527731464330[210] = 0; + out_1516035527731464330[211] = 0; + out_1516035527731464330[212] = 0; + out_1516035527731464330[213] = 0; + out_1516035527731464330[214] = 0; + out_1516035527731464330[215] = 0; + out_1516035527731464330[216] = 0; + out_1516035527731464330[217] = 0; + out_1516035527731464330[218] = 0; + out_1516035527731464330[219] = 0; + out_1516035527731464330[220] = 0; + out_1516035527731464330[221] = 0; + out_1516035527731464330[222] = 0; + out_1516035527731464330[223] = 0; + out_1516035527731464330[224] = 0; + out_1516035527731464330[225] = 0; + out_1516035527731464330[226] = 0; + out_1516035527731464330[227] = 0; + out_1516035527731464330[228] = 1; + out_1516035527731464330[229] = 0; + out_1516035527731464330[230] = 0; + out_1516035527731464330[231] = 0; + out_1516035527731464330[232] = 0; + out_1516035527731464330[233] = 0; + out_1516035527731464330[234] = 0; + out_1516035527731464330[235] = 0; + out_1516035527731464330[236] = 0; + out_1516035527731464330[237] = 0; + out_1516035527731464330[238] = 0; + out_1516035527731464330[239] = 0; + out_1516035527731464330[240] = 0; + out_1516035527731464330[241] = 0; + out_1516035527731464330[242] = 0; + out_1516035527731464330[243] = 0; + out_1516035527731464330[244] = 0; + out_1516035527731464330[245] = 0; + out_1516035527731464330[246] = 0; + out_1516035527731464330[247] = 1; + out_1516035527731464330[248] = 0; + out_1516035527731464330[249] = 0; + out_1516035527731464330[250] = 0; + out_1516035527731464330[251] = 0; + out_1516035527731464330[252] = 0; + out_1516035527731464330[253] = 0; + out_1516035527731464330[254] = 0; + out_1516035527731464330[255] = 0; + out_1516035527731464330[256] = 0; + out_1516035527731464330[257] = 0; + out_1516035527731464330[258] = 0; + out_1516035527731464330[259] = 0; + out_1516035527731464330[260] = 0; + out_1516035527731464330[261] = 0; + out_1516035527731464330[262] = 0; + out_1516035527731464330[263] = 0; + out_1516035527731464330[264] = 0; + out_1516035527731464330[265] = 0; + out_1516035527731464330[266] = 1; + out_1516035527731464330[267] = 0; + out_1516035527731464330[268] = 0; + out_1516035527731464330[269] = 0; + out_1516035527731464330[270] = 0; + out_1516035527731464330[271] = 0; + out_1516035527731464330[272] = 0; + out_1516035527731464330[273] = 0; + out_1516035527731464330[274] = 0; + out_1516035527731464330[275] = 0; + out_1516035527731464330[276] = 0; + out_1516035527731464330[277] = 0; + out_1516035527731464330[278] = 0; + out_1516035527731464330[279] = 0; + out_1516035527731464330[280] = 0; + out_1516035527731464330[281] = 0; + out_1516035527731464330[282] = 0; + out_1516035527731464330[283] = 0; + out_1516035527731464330[284] = 0; + out_1516035527731464330[285] = 1; + out_1516035527731464330[286] = 0; + out_1516035527731464330[287] = 0; + out_1516035527731464330[288] = 0; + out_1516035527731464330[289] = 0; + out_1516035527731464330[290] = 0; + out_1516035527731464330[291] = 0; + out_1516035527731464330[292] = 0; + out_1516035527731464330[293] = 0; + out_1516035527731464330[294] = 0; + out_1516035527731464330[295] = 0; + out_1516035527731464330[296] = 0; + out_1516035527731464330[297] = 0; + out_1516035527731464330[298] = 0; + out_1516035527731464330[299] = 0; + out_1516035527731464330[300] = 0; + out_1516035527731464330[301] = 0; + out_1516035527731464330[302] = 0; + out_1516035527731464330[303] = 0; + out_1516035527731464330[304] = 1; + out_1516035527731464330[305] = 0; + out_1516035527731464330[306] = 0; + out_1516035527731464330[307] = 0; + out_1516035527731464330[308] = 0; + out_1516035527731464330[309] = 0; + out_1516035527731464330[310] = 0; + out_1516035527731464330[311] = 0; + out_1516035527731464330[312] = 0; + out_1516035527731464330[313] = 0; + out_1516035527731464330[314] = 0; + out_1516035527731464330[315] = 0; + out_1516035527731464330[316] = 0; + out_1516035527731464330[317] = 0; + out_1516035527731464330[318] = 0; + out_1516035527731464330[319] = 0; + out_1516035527731464330[320] = 0; + out_1516035527731464330[321] = 0; + out_1516035527731464330[322] = 0; + out_1516035527731464330[323] = 1; } -void h_4(double *state, double *unused, double *out_5848855439961291946) { - out_5848855439961291946[0] = state[6] + state[9]; - out_5848855439961291946[1] = state[7] + state[10]; - out_5848855439961291946[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_4492721189395603550) { + out_4492721189395603550[0] = state[6] + state[9]; + out_4492721189395603550[1] = state[7] + state[10]; + out_4492721189395603550[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_3290246568201368788) { - out_3290246568201368788[0] = 0; - out_3290246568201368788[1] = 0; - out_3290246568201368788[2] = 0; - out_3290246568201368788[3] = 0; - out_3290246568201368788[4] = 0; - out_3290246568201368788[5] = 0; - out_3290246568201368788[6] = 1; - out_3290246568201368788[7] = 0; - out_3290246568201368788[8] = 0; - out_3290246568201368788[9] = 1; - out_3290246568201368788[10] = 0; - out_3290246568201368788[11] = 0; - out_3290246568201368788[12] = 0; - out_3290246568201368788[13] = 0; - out_3290246568201368788[14] = 0; - out_3290246568201368788[15] = 0; - out_3290246568201368788[16] = 0; - out_3290246568201368788[17] = 0; - out_3290246568201368788[18] = 0; - out_3290246568201368788[19] = 0; - out_3290246568201368788[20] = 0; - out_3290246568201368788[21] = 0; - out_3290246568201368788[22] = 0; - out_3290246568201368788[23] = 0; - out_3290246568201368788[24] = 0; - out_3290246568201368788[25] = 1; - out_3290246568201368788[26] = 0; - out_3290246568201368788[27] = 0; - out_3290246568201368788[28] = 1; - out_3290246568201368788[29] = 0; - out_3290246568201368788[30] = 0; - out_3290246568201368788[31] = 0; - out_3290246568201368788[32] = 0; - out_3290246568201368788[33] = 0; - out_3290246568201368788[34] = 0; - out_3290246568201368788[35] = 0; - out_3290246568201368788[36] = 0; - out_3290246568201368788[37] = 0; - out_3290246568201368788[38] = 0; - out_3290246568201368788[39] = 0; - out_3290246568201368788[40] = 0; - out_3290246568201368788[41] = 0; - out_3290246568201368788[42] = 0; - out_3290246568201368788[43] = 0; - out_3290246568201368788[44] = 1; - out_3290246568201368788[45] = 0; - out_3290246568201368788[46] = 0; - out_3290246568201368788[47] = 1; - out_3290246568201368788[48] = 0; - out_3290246568201368788[49] = 0; - out_3290246568201368788[50] = 0; - out_3290246568201368788[51] = 0; - out_3290246568201368788[52] = 0; - out_3290246568201368788[53] = 0; +void H_4(double *state, double *unused, double *out_4858215238190826153) { + out_4858215238190826153[0] = 0; + out_4858215238190826153[1] = 0; + out_4858215238190826153[2] = 0; + out_4858215238190826153[3] = 0; + out_4858215238190826153[4] = 0; + out_4858215238190826153[5] = 0; + out_4858215238190826153[6] = 1; + out_4858215238190826153[7] = 0; + out_4858215238190826153[8] = 0; + out_4858215238190826153[9] = 1; + out_4858215238190826153[10] = 0; + out_4858215238190826153[11] = 0; + out_4858215238190826153[12] = 0; + out_4858215238190826153[13] = 0; + out_4858215238190826153[14] = 0; + out_4858215238190826153[15] = 0; + out_4858215238190826153[16] = 0; + out_4858215238190826153[17] = 0; + out_4858215238190826153[18] = 0; + out_4858215238190826153[19] = 0; + out_4858215238190826153[20] = 0; + out_4858215238190826153[21] = 0; + out_4858215238190826153[22] = 0; + out_4858215238190826153[23] = 0; + out_4858215238190826153[24] = 0; + out_4858215238190826153[25] = 1; + out_4858215238190826153[26] = 0; + out_4858215238190826153[27] = 0; + out_4858215238190826153[28] = 1; + out_4858215238190826153[29] = 0; + out_4858215238190826153[30] = 0; + out_4858215238190826153[31] = 0; + out_4858215238190826153[32] = 0; + out_4858215238190826153[33] = 0; + out_4858215238190826153[34] = 0; + out_4858215238190826153[35] = 0; + out_4858215238190826153[36] = 0; + out_4858215238190826153[37] = 0; + out_4858215238190826153[38] = 0; + out_4858215238190826153[39] = 0; + out_4858215238190826153[40] = 0; + out_4858215238190826153[41] = 0; + out_4858215238190826153[42] = 0; + out_4858215238190826153[43] = 0; + out_4858215238190826153[44] = 1; + out_4858215238190826153[45] = 0; + out_4858215238190826153[46] = 0; + out_4858215238190826153[47] = 1; + out_4858215238190826153[48] = 0; + out_4858215238190826153[49] = 0; + out_4858215238190826153[50] = 0; + out_4858215238190826153[51] = 0; + out_4858215238190826153[52] = 0; + out_4858215238190826153[53] = 0; } -void h_10(double *state, double *unused, double *out_3954090448202517665) { - out_3954090448202517665[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_3954090448202517665[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_3954090448202517665[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_558240635521768637) { + out_558240635521768637[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_558240635521768637[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_558240635521768637[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_1376781969837987839) { - out_1376781969837987839[0] = 0; - out_1376781969837987839[1] = 9.8100000000000005*cos(state[1]); - out_1376781969837987839[2] = 0; - out_1376781969837987839[3] = 0; - out_1376781969837987839[4] = -state[8]; - out_1376781969837987839[5] = state[7]; - out_1376781969837987839[6] = 0; - out_1376781969837987839[7] = state[5]; - out_1376781969837987839[8] = -state[4]; - out_1376781969837987839[9] = 0; - out_1376781969837987839[10] = 0; - out_1376781969837987839[11] = 0; - out_1376781969837987839[12] = 1; - out_1376781969837987839[13] = 0; - out_1376781969837987839[14] = 0; - out_1376781969837987839[15] = 1; - out_1376781969837987839[16] = 0; - out_1376781969837987839[17] = 0; - out_1376781969837987839[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_1376781969837987839[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_1376781969837987839[20] = 0; - out_1376781969837987839[21] = state[8]; - out_1376781969837987839[22] = 0; - out_1376781969837987839[23] = -state[6]; - out_1376781969837987839[24] = -state[5]; - out_1376781969837987839[25] = 0; - out_1376781969837987839[26] = state[3]; - out_1376781969837987839[27] = 0; - out_1376781969837987839[28] = 0; - out_1376781969837987839[29] = 0; - out_1376781969837987839[30] = 0; - out_1376781969837987839[31] = 1; - out_1376781969837987839[32] = 0; - out_1376781969837987839[33] = 0; - out_1376781969837987839[34] = 1; - out_1376781969837987839[35] = 0; - out_1376781969837987839[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_1376781969837987839[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_1376781969837987839[38] = 0; - out_1376781969837987839[39] = -state[7]; - out_1376781969837987839[40] = state[6]; - out_1376781969837987839[41] = 0; - out_1376781969837987839[42] = state[4]; - out_1376781969837987839[43] = -state[3]; - out_1376781969837987839[44] = 0; - out_1376781969837987839[45] = 0; - out_1376781969837987839[46] = 0; - out_1376781969837987839[47] = 0; - out_1376781969837987839[48] = 0; - out_1376781969837987839[49] = 0; - out_1376781969837987839[50] = 1; - out_1376781969837987839[51] = 0; - out_1376781969837987839[52] = 0; - out_1376781969837987839[53] = 1; +void H_10(double *state, double *unused, double *out_7377176961528777036) { + out_7377176961528777036[0] = 0; + out_7377176961528777036[1] = 9.8100000000000005*cos(state[1]); + out_7377176961528777036[2] = 0; + out_7377176961528777036[3] = 0; + out_7377176961528777036[4] = -state[8]; + out_7377176961528777036[5] = state[7]; + out_7377176961528777036[6] = 0; + out_7377176961528777036[7] = state[5]; + out_7377176961528777036[8] = -state[4]; + out_7377176961528777036[9] = 0; + out_7377176961528777036[10] = 0; + out_7377176961528777036[11] = 0; + out_7377176961528777036[12] = 1; + out_7377176961528777036[13] = 0; + out_7377176961528777036[14] = 0; + out_7377176961528777036[15] = 1; + out_7377176961528777036[16] = 0; + out_7377176961528777036[17] = 0; + out_7377176961528777036[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_7377176961528777036[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_7377176961528777036[20] = 0; + out_7377176961528777036[21] = state[8]; + out_7377176961528777036[22] = 0; + out_7377176961528777036[23] = -state[6]; + out_7377176961528777036[24] = -state[5]; + out_7377176961528777036[25] = 0; + out_7377176961528777036[26] = state[3]; + out_7377176961528777036[27] = 0; + out_7377176961528777036[28] = 0; + out_7377176961528777036[29] = 0; + out_7377176961528777036[30] = 0; + out_7377176961528777036[31] = 1; + out_7377176961528777036[32] = 0; + out_7377176961528777036[33] = 0; + out_7377176961528777036[34] = 1; + out_7377176961528777036[35] = 0; + out_7377176961528777036[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_7377176961528777036[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_7377176961528777036[38] = 0; + out_7377176961528777036[39] = -state[7]; + out_7377176961528777036[40] = state[6]; + out_7377176961528777036[41] = 0; + out_7377176961528777036[42] = state[4]; + out_7377176961528777036[43] = -state[3]; + out_7377176961528777036[44] = 0; + out_7377176961528777036[45] = 0; + out_7377176961528777036[46] = 0; + out_7377176961528777036[47] = 0; + out_7377176961528777036[48] = 0; + out_7377176961528777036[49] = 0; + out_7377176961528777036[50] = 1; + out_7377176961528777036[51] = 0; + out_7377176961528777036[52] = 0; + out_7377176961528777036[53] = 1; } -void h_13(double *state, double *unused, double *out_8116870387044428464) { - out_8116870387044428464[0] = state[3]; - out_8116870387044428464[1] = state[4]; - out_8116870387044428464[2] = state[5]; +void h_13(double *state, double *unused, double *out_3864223786954470726) { + out_3864223786954470726[0] = state[3]; + out_3864223786954470726[1] = state[4]; + out_3864223786954470726[2] = state[5]; } -void H_13(double *state, double *unused, double *out_77972742869035987) { - out_77972742869035987[0] = 0; - out_77972742869035987[1] = 0; - out_77972742869035987[2] = 0; - out_77972742869035987[3] = 1; - out_77972742869035987[4] = 0; - out_77972742869035987[5] = 0; - out_77972742869035987[6] = 0; - out_77972742869035987[7] = 0; - out_77972742869035987[8] = 0; - out_77972742869035987[9] = 0; - out_77972742869035987[10] = 0; - out_77972742869035987[11] = 0; - out_77972742869035987[12] = 0; - out_77972742869035987[13] = 0; - out_77972742869035987[14] = 0; - out_77972742869035987[15] = 0; - out_77972742869035987[16] = 0; - out_77972742869035987[17] = 0; - out_77972742869035987[18] = 0; - out_77972742869035987[19] = 0; - out_77972742869035987[20] = 0; - out_77972742869035987[21] = 0; - out_77972742869035987[22] = 1; - out_77972742869035987[23] = 0; - out_77972742869035987[24] = 0; - out_77972742869035987[25] = 0; - out_77972742869035987[26] = 0; - out_77972742869035987[27] = 0; - out_77972742869035987[28] = 0; - out_77972742869035987[29] = 0; - out_77972742869035987[30] = 0; - out_77972742869035987[31] = 0; - out_77972742869035987[32] = 0; - out_77972742869035987[33] = 0; - out_77972742869035987[34] = 0; - out_77972742869035987[35] = 0; - out_77972742869035987[36] = 0; - out_77972742869035987[37] = 0; - out_77972742869035987[38] = 0; - out_77972742869035987[39] = 0; - out_77972742869035987[40] = 0; - out_77972742869035987[41] = 1; - out_77972742869035987[42] = 0; - out_77972742869035987[43] = 0; - out_77972742869035987[44] = 0; - out_77972742869035987[45] = 0; - out_77972742869035987[46] = 0; - out_77972742869035987[47] = 0; - out_77972742869035987[48] = 0; - out_77972742869035987[49] = 0; - out_77972742869035987[50] = 0; - out_77972742869035987[51] = 0; - out_77972742869035987[52] = 0; - out_77972742869035987[53] = 0; +void H_13(double *state, double *unused, double *out_1645941412858493352) { + out_1645941412858493352[0] = 0; + out_1645941412858493352[1] = 0; + out_1645941412858493352[2] = 0; + out_1645941412858493352[3] = 1; + out_1645941412858493352[4] = 0; + out_1645941412858493352[5] = 0; + out_1645941412858493352[6] = 0; + out_1645941412858493352[7] = 0; + out_1645941412858493352[8] = 0; + out_1645941412858493352[9] = 0; + out_1645941412858493352[10] = 0; + out_1645941412858493352[11] = 0; + out_1645941412858493352[12] = 0; + out_1645941412858493352[13] = 0; + out_1645941412858493352[14] = 0; + out_1645941412858493352[15] = 0; + out_1645941412858493352[16] = 0; + out_1645941412858493352[17] = 0; + out_1645941412858493352[18] = 0; + out_1645941412858493352[19] = 0; + out_1645941412858493352[20] = 0; + out_1645941412858493352[21] = 0; + out_1645941412858493352[22] = 1; + out_1645941412858493352[23] = 0; + out_1645941412858493352[24] = 0; + out_1645941412858493352[25] = 0; + out_1645941412858493352[26] = 0; + out_1645941412858493352[27] = 0; + out_1645941412858493352[28] = 0; + out_1645941412858493352[29] = 0; + out_1645941412858493352[30] = 0; + out_1645941412858493352[31] = 0; + out_1645941412858493352[32] = 0; + out_1645941412858493352[33] = 0; + out_1645941412858493352[34] = 0; + out_1645941412858493352[35] = 0; + out_1645941412858493352[36] = 0; + out_1645941412858493352[37] = 0; + out_1645941412858493352[38] = 0; + out_1645941412858493352[39] = 0; + out_1645941412858493352[40] = 0; + out_1645941412858493352[41] = 1; + out_1645941412858493352[42] = 0; + out_1645941412858493352[43] = 0; + out_1645941412858493352[44] = 0; + out_1645941412858493352[45] = 0; + out_1645941412858493352[46] = 0; + out_1645941412858493352[47] = 0; + out_1645941412858493352[48] = 0; + out_1645941412858493352[49] = 0; + out_1645941412858493352[50] = 0; + out_1645941412858493352[51] = 0; + out_1645941412858493352[52] = 0; + out_1645941412858493352[53] = 0; } -void h_14(double *state, double *unused, double *out_2879136579664093334) { - out_2879136579664093334[0] = state[6]; - out_2879136579664093334[1] = state[7]; - out_2879136579664093334[2] = state[8]; +void h_14(double *state, double *unused, double *out_2242454366414003666) { + out_2242454366414003666[0] = state[6]; + out_2242454366414003666[1] = state[7]; + out_2242454366414003666[2] = state[8]; } -void H_14(double *state, double *unused, double *out_672994288138115741) { - out_672994288138115741[0] = 0; - out_672994288138115741[1] = 0; - out_672994288138115741[2] = 0; - out_672994288138115741[3] = 0; - out_672994288138115741[4] = 0; - out_672994288138115741[5] = 0; - out_672994288138115741[6] = 1; - out_672994288138115741[7] = 0; - out_672994288138115741[8] = 0; - out_672994288138115741[9] = 0; - out_672994288138115741[10] = 0; - out_672994288138115741[11] = 0; - out_672994288138115741[12] = 0; - out_672994288138115741[13] = 0; - out_672994288138115741[14] = 0; - out_672994288138115741[15] = 0; - out_672994288138115741[16] = 0; - out_672994288138115741[17] = 0; - out_672994288138115741[18] = 0; - out_672994288138115741[19] = 0; - out_672994288138115741[20] = 0; - out_672994288138115741[21] = 0; - out_672994288138115741[22] = 0; - out_672994288138115741[23] = 0; - out_672994288138115741[24] = 0; - out_672994288138115741[25] = 1; - out_672994288138115741[26] = 0; - out_672994288138115741[27] = 0; - out_672994288138115741[28] = 0; - out_672994288138115741[29] = 0; - out_672994288138115741[30] = 0; - out_672994288138115741[31] = 0; - out_672994288138115741[32] = 0; - out_672994288138115741[33] = 0; - out_672994288138115741[34] = 0; - out_672994288138115741[35] = 0; - out_672994288138115741[36] = 0; - out_672994288138115741[37] = 0; - out_672994288138115741[38] = 0; - out_672994288138115741[39] = 0; - out_672994288138115741[40] = 0; - out_672994288138115741[41] = 0; - out_672994288138115741[42] = 0; - out_672994288138115741[43] = 0; - out_672994288138115741[44] = 1; - out_672994288138115741[45] = 0; - out_672994288138115741[46] = 0; - out_672994288138115741[47] = 0; - out_672994288138115741[48] = 0; - out_672994288138115741[49] = 0; - out_672994288138115741[50] = 0; - out_672994288138115741[51] = 0; - out_672994288138115741[52] = 0; - out_672994288138115741[53] = 0; +void H_14(double *state, double *unused, double *out_7941003670486198449) { + out_7941003670486198449[0] = 0; + out_7941003670486198449[1] = 0; + out_7941003670486198449[2] = 0; + out_7941003670486198449[3] = 0; + out_7941003670486198449[4] = 0; + out_7941003670486198449[5] = 0; + out_7941003670486198449[6] = 1; + out_7941003670486198449[7] = 0; + out_7941003670486198449[8] = 0; + out_7941003670486198449[9] = 0; + out_7941003670486198449[10] = 0; + out_7941003670486198449[11] = 0; + out_7941003670486198449[12] = 0; + out_7941003670486198449[13] = 0; + out_7941003670486198449[14] = 0; + out_7941003670486198449[15] = 0; + out_7941003670486198449[16] = 0; + out_7941003670486198449[17] = 0; + out_7941003670486198449[18] = 0; + out_7941003670486198449[19] = 0; + out_7941003670486198449[20] = 0; + out_7941003670486198449[21] = 0; + out_7941003670486198449[22] = 0; + out_7941003670486198449[23] = 0; + out_7941003670486198449[24] = 0; + out_7941003670486198449[25] = 1; + out_7941003670486198449[26] = 0; + out_7941003670486198449[27] = 0; + out_7941003670486198449[28] = 0; + out_7941003670486198449[29] = 0; + out_7941003670486198449[30] = 0; + out_7941003670486198449[31] = 0; + out_7941003670486198449[32] = 0; + out_7941003670486198449[33] = 0; + out_7941003670486198449[34] = 0; + out_7941003670486198449[35] = 0; + out_7941003670486198449[36] = 0; + out_7941003670486198449[37] = 0; + out_7941003670486198449[38] = 0; + out_7941003670486198449[39] = 0; + out_7941003670486198449[40] = 0; + out_7941003670486198449[41] = 0; + out_7941003670486198449[42] = 0; + out_7941003670486198449[43] = 0; + out_7941003670486198449[44] = 1; + out_7941003670486198449[45] = 0; + out_7941003670486198449[46] = 0; + out_7941003670486198449[47] = 0; + out_7941003670486198449[48] = 0; + out_7941003670486198449[49] = 0; + out_7941003670486198449[50] = 0; + out_7941003670486198449[51] = 0; + out_7941003670486198449[52] = 0; + out_7941003670486198449[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_9170572944906816852) { - err_fun(nom_x, delta_x, out_9170572944906816852); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2526150130430510074) { + err_fun(nom_x, delta_x, out_2526150130430510074); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4867215415263963768) { - inv_err_fun(nom_x, true_x, out_4867215415263963768); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3515561648914426109) { + inv_err_fun(nom_x, true_x, out_3515561648914426109); } -void pose_H_mod_fun(double *state, double *out_1396735664605592698) { - H_mod_fun(state, out_1396735664605592698); +void pose_H_mod_fun(double *state, double *out_6056162720073724257) { + H_mod_fun(state, out_6056162720073724257); } -void pose_f_fun(double *state, double dt, double *out_916482441408583797) { - f_fun(state, dt, out_916482441408583797); +void pose_f_fun(double *state, double dt, double *out_8780284750928315681) { + f_fun(state, dt, out_8780284750928315681); } -void pose_F_fun(double *state, double dt, double *out_5639772936950525263) { - F_fun(state, dt, out_5639772936950525263); +void pose_F_fun(double *state, double dt, double *out_1516035527731464330) { + F_fun(state, dt, out_1516035527731464330); } -void pose_h_4(double *state, double *unused, double *out_5848855439961291946) { - h_4(state, unused, out_5848855439961291946); +void pose_h_4(double *state, double *unused, double *out_4492721189395603550) { + h_4(state, unused, out_4492721189395603550); } -void pose_H_4(double *state, double *unused, double *out_3290246568201368788) { - H_4(state, unused, out_3290246568201368788); +void pose_H_4(double *state, double *unused, double *out_4858215238190826153) { + H_4(state, unused, out_4858215238190826153); } -void pose_h_10(double *state, double *unused, double *out_3954090448202517665) { - h_10(state, unused, out_3954090448202517665); +void pose_h_10(double *state, double *unused, double *out_558240635521768637) { + h_10(state, unused, out_558240635521768637); } -void pose_H_10(double *state, double *unused, double *out_1376781969837987839) { - H_10(state, unused, out_1376781969837987839); +void pose_H_10(double *state, double *unused, double *out_7377176961528777036) { + H_10(state, unused, out_7377176961528777036); } -void pose_h_13(double *state, double *unused, double *out_8116870387044428464) { - h_13(state, unused, out_8116870387044428464); +void pose_h_13(double *state, double *unused, double *out_3864223786954470726) { + h_13(state, unused, out_3864223786954470726); } -void pose_H_13(double *state, double *unused, double *out_77972742869035987) { - H_13(state, unused, out_77972742869035987); +void pose_H_13(double *state, double *unused, double *out_1645941412858493352) { + H_13(state, unused, out_1645941412858493352); } -void pose_h_14(double *state, double *unused, double *out_2879136579664093334) { - h_14(state, unused, out_2879136579664093334); +void pose_h_14(double *state, double *unused, double *out_2242454366414003666) { + h_14(state, unused, out_2242454366414003666); } -void pose_H_14(double *state, double *unused, double *out_672994288138115741) { - H_14(state, unused, out_672994288138115741); +void pose_H_14(double *state, double *unused, double *out_7941003670486198449) { + H_14(state, unused, out_7941003670486198449); } 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 8e75e5380..e7ae94715 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_9170572944906816852); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4867215415263963768); -void pose_H_mod_fun(double *state, double *out_1396735664605592698); -void pose_f_fun(double *state, double dt, double *out_916482441408583797); -void pose_F_fun(double *state, double dt, double *out_5639772936950525263); -void pose_h_4(double *state, double *unused, double *out_5848855439961291946); -void pose_H_4(double *state, double *unused, double *out_3290246568201368788); -void pose_h_10(double *state, double *unused, double *out_3954090448202517665); -void pose_H_10(double *state, double *unused, double *out_1376781969837987839); -void pose_h_13(double *state, double *unused, double *out_8116870387044428464); -void pose_H_13(double *state, double *unused, double *out_77972742869035987); -void pose_h_14(double *state, double *unused, double *out_2879136579664093334); -void pose_H_14(double *state, double *unused, double *out_672994288138115741); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2526150130430510074); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3515561648914426109); +void pose_H_mod_fun(double *state, double *out_6056162720073724257); +void pose_f_fun(double *state, double dt, double *out_8780284750928315681); +void pose_F_fun(double *state, double dt, double *out_1516035527731464330); +void pose_h_4(double *state, double *unused, double *out_4492721189395603550); +void pose_H_4(double *state, double *unused, double *out_4858215238190826153); +void pose_h_10(double *state, double *unused, double *out_558240635521768637); +void pose_H_10(double *state, double *unused, double *out_7377176961528777036); +void pose_h_13(double *state, double *unused, double *out_3864223786954470726); +void pose_H_13(double *state, double *unused, double *out_1645941412858493352); +void pose_h_14(double *state, double *unused, double *out_2242454366414003666); +void pose_H_14(double *state, double *unused, double *out_7941003670486198449); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index bcbe70be3..6a50ba1c4 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 3d07c9f12..288efafed 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 41db831bb..ef6a99b7d 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 34cbc1f15..97fc92789 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index a6069a2de..4e4029f4f 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index c6627a465..a72e60002 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ