diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 036205a06..d85289af4 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 15c5091ae..dac249fa4 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 5808f762b..094adb17b 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 f2cf4ab27..c430a569a 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 1a10bb68d..72d91ad14 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 d77a157c3..8e0b43f67 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 53104d021..ba010518e 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 53104d021..ba010518e 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 009c71817..a31af1cec 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 d77a157c3..8e0b43f67 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 c09bc1190..35dc2892d 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-da88d20b-DEBUG"; +const uint8_t gitversion[19] = "DEV-0cddc630-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index e3b5536d9..7d136668c 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 d668370d2..e09fe805e 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 b2cc0fbc9..ac756286a 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 1283f33ae..77358179d 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 6150ef29f..505daa3a4 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 629840403..7edb24fe5 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 f58872d3c..dce89c9f5 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 bace8bfbb..cb9e35f1e 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 2367d6123..d412eceb7 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 01f4f007e..255b274d8 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 19750b8f0..2b7786e18 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 15f72ffc9..4ec933556 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 6e9b656b0..aafedb3d4 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 750e10dc2..678cfce51 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 c4b2dd7dd..c0de92be6 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 1c4bc90a3..bc3f35e8e 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 24d6adbf8..969e7e338 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 136488b83..d83ea1ee7 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 aeafd115b..8c964b31a 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 1d4c8a6bf..fd11442ea 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 be1a6cd74..f9d5cc7d3 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-da88d20b-DEBUG \ No newline at end of file +DEV-0cddc630-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index e16b8af41..ad3118ce1 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_3568804456057098654) { - out_3568804456057098654[0] = delta_x[0] + nom_x[0]; - out_3568804456057098654[1] = delta_x[1] + nom_x[1]; - out_3568804456057098654[2] = delta_x[2] + nom_x[2]; - out_3568804456057098654[3] = delta_x[3] + nom_x[3]; - out_3568804456057098654[4] = delta_x[4] + nom_x[4]; - out_3568804456057098654[5] = delta_x[5] + nom_x[5]; - out_3568804456057098654[6] = delta_x[6] + nom_x[6]; - out_3568804456057098654[7] = delta_x[7] + nom_x[7]; - out_3568804456057098654[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_8308214496735031777) { + out_8308214496735031777[0] = delta_x[0] + nom_x[0]; + out_8308214496735031777[1] = delta_x[1] + nom_x[1]; + out_8308214496735031777[2] = delta_x[2] + nom_x[2]; + out_8308214496735031777[3] = delta_x[3] + nom_x[3]; + out_8308214496735031777[4] = delta_x[4] + nom_x[4]; + out_8308214496735031777[5] = delta_x[5] + nom_x[5]; + out_8308214496735031777[6] = delta_x[6] + nom_x[6]; + out_8308214496735031777[7] = delta_x[7] + nom_x[7]; + out_8308214496735031777[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1840397183166108031) { - out_1840397183166108031[0] = -nom_x[0] + true_x[0]; - out_1840397183166108031[1] = -nom_x[1] + true_x[1]; - out_1840397183166108031[2] = -nom_x[2] + true_x[2]; - out_1840397183166108031[3] = -nom_x[3] + true_x[3]; - out_1840397183166108031[4] = -nom_x[4] + true_x[4]; - out_1840397183166108031[5] = -nom_x[5] + true_x[5]; - out_1840397183166108031[6] = -nom_x[6] + true_x[6]; - out_1840397183166108031[7] = -nom_x[7] + true_x[7]; - out_1840397183166108031[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8999766425853420329) { + out_8999766425853420329[0] = -nom_x[0] + true_x[0]; + out_8999766425853420329[1] = -nom_x[1] + true_x[1]; + out_8999766425853420329[2] = -nom_x[2] + true_x[2]; + out_8999766425853420329[3] = -nom_x[3] + true_x[3]; + out_8999766425853420329[4] = -nom_x[4] + true_x[4]; + out_8999766425853420329[5] = -nom_x[5] + true_x[5]; + out_8999766425853420329[6] = -nom_x[6] + true_x[6]; + out_8999766425853420329[7] = -nom_x[7] + true_x[7]; + out_8999766425853420329[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_8921258435077713963) { - out_8921258435077713963[0] = 1.0; - out_8921258435077713963[1] = 0.0; - out_8921258435077713963[2] = 0.0; - out_8921258435077713963[3] = 0.0; - out_8921258435077713963[4] = 0.0; - out_8921258435077713963[5] = 0.0; - out_8921258435077713963[6] = 0.0; - out_8921258435077713963[7] = 0.0; - out_8921258435077713963[8] = 0.0; - out_8921258435077713963[9] = 0.0; - out_8921258435077713963[10] = 1.0; - out_8921258435077713963[11] = 0.0; - out_8921258435077713963[12] = 0.0; - out_8921258435077713963[13] = 0.0; - out_8921258435077713963[14] = 0.0; - out_8921258435077713963[15] = 0.0; - out_8921258435077713963[16] = 0.0; - out_8921258435077713963[17] = 0.0; - out_8921258435077713963[18] = 0.0; - out_8921258435077713963[19] = 0.0; - out_8921258435077713963[20] = 1.0; - out_8921258435077713963[21] = 0.0; - out_8921258435077713963[22] = 0.0; - out_8921258435077713963[23] = 0.0; - out_8921258435077713963[24] = 0.0; - out_8921258435077713963[25] = 0.0; - out_8921258435077713963[26] = 0.0; - out_8921258435077713963[27] = 0.0; - out_8921258435077713963[28] = 0.0; - out_8921258435077713963[29] = 0.0; - out_8921258435077713963[30] = 1.0; - out_8921258435077713963[31] = 0.0; - out_8921258435077713963[32] = 0.0; - out_8921258435077713963[33] = 0.0; - out_8921258435077713963[34] = 0.0; - out_8921258435077713963[35] = 0.0; - out_8921258435077713963[36] = 0.0; - out_8921258435077713963[37] = 0.0; - out_8921258435077713963[38] = 0.0; - out_8921258435077713963[39] = 0.0; - out_8921258435077713963[40] = 1.0; - out_8921258435077713963[41] = 0.0; - out_8921258435077713963[42] = 0.0; - out_8921258435077713963[43] = 0.0; - out_8921258435077713963[44] = 0.0; - out_8921258435077713963[45] = 0.0; - out_8921258435077713963[46] = 0.0; - out_8921258435077713963[47] = 0.0; - out_8921258435077713963[48] = 0.0; - out_8921258435077713963[49] = 0.0; - out_8921258435077713963[50] = 1.0; - out_8921258435077713963[51] = 0.0; - out_8921258435077713963[52] = 0.0; - out_8921258435077713963[53] = 0.0; - out_8921258435077713963[54] = 0.0; - out_8921258435077713963[55] = 0.0; - out_8921258435077713963[56] = 0.0; - out_8921258435077713963[57] = 0.0; - out_8921258435077713963[58] = 0.0; - out_8921258435077713963[59] = 0.0; - out_8921258435077713963[60] = 1.0; - out_8921258435077713963[61] = 0.0; - out_8921258435077713963[62] = 0.0; - out_8921258435077713963[63] = 0.0; - out_8921258435077713963[64] = 0.0; - out_8921258435077713963[65] = 0.0; - out_8921258435077713963[66] = 0.0; - out_8921258435077713963[67] = 0.0; - out_8921258435077713963[68] = 0.0; - out_8921258435077713963[69] = 0.0; - out_8921258435077713963[70] = 1.0; - out_8921258435077713963[71] = 0.0; - out_8921258435077713963[72] = 0.0; - out_8921258435077713963[73] = 0.0; - out_8921258435077713963[74] = 0.0; - out_8921258435077713963[75] = 0.0; - out_8921258435077713963[76] = 0.0; - out_8921258435077713963[77] = 0.0; - out_8921258435077713963[78] = 0.0; - out_8921258435077713963[79] = 0.0; - out_8921258435077713963[80] = 1.0; +void H_mod_fun(double *state, double *out_8671923853756600219) { + out_8671923853756600219[0] = 1.0; + out_8671923853756600219[1] = 0.0; + out_8671923853756600219[2] = 0.0; + out_8671923853756600219[3] = 0.0; + out_8671923853756600219[4] = 0.0; + out_8671923853756600219[5] = 0.0; + out_8671923853756600219[6] = 0.0; + out_8671923853756600219[7] = 0.0; + out_8671923853756600219[8] = 0.0; + out_8671923853756600219[9] = 0.0; + out_8671923853756600219[10] = 1.0; + out_8671923853756600219[11] = 0.0; + out_8671923853756600219[12] = 0.0; + out_8671923853756600219[13] = 0.0; + out_8671923853756600219[14] = 0.0; + out_8671923853756600219[15] = 0.0; + out_8671923853756600219[16] = 0.0; + out_8671923853756600219[17] = 0.0; + out_8671923853756600219[18] = 0.0; + out_8671923853756600219[19] = 0.0; + out_8671923853756600219[20] = 1.0; + out_8671923853756600219[21] = 0.0; + out_8671923853756600219[22] = 0.0; + out_8671923853756600219[23] = 0.0; + out_8671923853756600219[24] = 0.0; + out_8671923853756600219[25] = 0.0; + out_8671923853756600219[26] = 0.0; + out_8671923853756600219[27] = 0.0; + out_8671923853756600219[28] = 0.0; + out_8671923853756600219[29] = 0.0; + out_8671923853756600219[30] = 1.0; + out_8671923853756600219[31] = 0.0; + out_8671923853756600219[32] = 0.0; + out_8671923853756600219[33] = 0.0; + out_8671923853756600219[34] = 0.0; + out_8671923853756600219[35] = 0.0; + out_8671923853756600219[36] = 0.0; + out_8671923853756600219[37] = 0.0; + out_8671923853756600219[38] = 0.0; + out_8671923853756600219[39] = 0.0; + out_8671923853756600219[40] = 1.0; + out_8671923853756600219[41] = 0.0; + out_8671923853756600219[42] = 0.0; + out_8671923853756600219[43] = 0.0; + out_8671923853756600219[44] = 0.0; + out_8671923853756600219[45] = 0.0; + out_8671923853756600219[46] = 0.0; + out_8671923853756600219[47] = 0.0; + out_8671923853756600219[48] = 0.0; + out_8671923853756600219[49] = 0.0; + out_8671923853756600219[50] = 1.0; + out_8671923853756600219[51] = 0.0; + out_8671923853756600219[52] = 0.0; + out_8671923853756600219[53] = 0.0; + out_8671923853756600219[54] = 0.0; + out_8671923853756600219[55] = 0.0; + out_8671923853756600219[56] = 0.0; + out_8671923853756600219[57] = 0.0; + out_8671923853756600219[58] = 0.0; + out_8671923853756600219[59] = 0.0; + out_8671923853756600219[60] = 1.0; + out_8671923853756600219[61] = 0.0; + out_8671923853756600219[62] = 0.0; + out_8671923853756600219[63] = 0.0; + out_8671923853756600219[64] = 0.0; + out_8671923853756600219[65] = 0.0; + out_8671923853756600219[66] = 0.0; + out_8671923853756600219[67] = 0.0; + out_8671923853756600219[68] = 0.0; + out_8671923853756600219[69] = 0.0; + out_8671923853756600219[70] = 1.0; + out_8671923853756600219[71] = 0.0; + out_8671923853756600219[72] = 0.0; + out_8671923853756600219[73] = 0.0; + out_8671923853756600219[74] = 0.0; + out_8671923853756600219[75] = 0.0; + out_8671923853756600219[76] = 0.0; + out_8671923853756600219[77] = 0.0; + out_8671923853756600219[78] = 0.0; + out_8671923853756600219[79] = 0.0; + out_8671923853756600219[80] = 1.0; } -void f_fun(double *state, double dt, double *out_3277027219914288776) { - out_3277027219914288776[0] = state[0]; - out_3277027219914288776[1] = state[1]; - out_3277027219914288776[2] = state[2]; - out_3277027219914288776[3] = state[3]; - out_3277027219914288776[4] = state[4]; - out_3277027219914288776[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_3277027219914288776[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_3277027219914288776[7] = state[7]; - out_3277027219914288776[8] = state[8]; +void f_fun(double *state, double dt, double *out_3762356930851513593) { + out_3762356930851513593[0] = state[0]; + out_3762356930851513593[1] = state[1]; + out_3762356930851513593[2] = state[2]; + out_3762356930851513593[3] = state[3]; + out_3762356930851513593[4] = state[4]; + out_3762356930851513593[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_3762356930851513593[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_3762356930851513593[7] = state[7]; + out_3762356930851513593[8] = state[8]; } -void F_fun(double *state, double dt, double *out_370955893725369602) { - out_370955893725369602[0] = 1; - out_370955893725369602[1] = 0; - out_370955893725369602[2] = 0; - out_370955893725369602[3] = 0; - out_370955893725369602[4] = 0; - out_370955893725369602[5] = 0; - out_370955893725369602[6] = 0; - out_370955893725369602[7] = 0; - out_370955893725369602[8] = 0; - out_370955893725369602[9] = 0; - out_370955893725369602[10] = 1; - out_370955893725369602[11] = 0; - out_370955893725369602[12] = 0; - out_370955893725369602[13] = 0; - out_370955893725369602[14] = 0; - out_370955893725369602[15] = 0; - out_370955893725369602[16] = 0; - out_370955893725369602[17] = 0; - out_370955893725369602[18] = 0; - out_370955893725369602[19] = 0; - out_370955893725369602[20] = 1; - out_370955893725369602[21] = 0; - out_370955893725369602[22] = 0; - out_370955893725369602[23] = 0; - out_370955893725369602[24] = 0; - out_370955893725369602[25] = 0; - out_370955893725369602[26] = 0; - out_370955893725369602[27] = 0; - out_370955893725369602[28] = 0; - out_370955893725369602[29] = 0; - out_370955893725369602[30] = 1; - out_370955893725369602[31] = 0; - out_370955893725369602[32] = 0; - out_370955893725369602[33] = 0; - out_370955893725369602[34] = 0; - out_370955893725369602[35] = 0; - out_370955893725369602[36] = 0; - out_370955893725369602[37] = 0; - out_370955893725369602[38] = 0; - out_370955893725369602[39] = 0; - out_370955893725369602[40] = 1; - out_370955893725369602[41] = 0; - out_370955893725369602[42] = 0; - out_370955893725369602[43] = 0; - out_370955893725369602[44] = 0; - out_370955893725369602[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_370955893725369602[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_370955893725369602[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_370955893725369602[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_370955893725369602[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_370955893725369602[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_370955893725369602[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_370955893725369602[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_370955893725369602[53] = -9.8100000000000005*dt; - out_370955893725369602[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_370955893725369602[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_370955893725369602[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_370955893725369602[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_370955893725369602[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_370955893725369602[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_370955893725369602[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_370955893725369602[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_370955893725369602[62] = 0; - out_370955893725369602[63] = 0; - out_370955893725369602[64] = 0; - out_370955893725369602[65] = 0; - out_370955893725369602[66] = 0; - out_370955893725369602[67] = 0; - out_370955893725369602[68] = 0; - out_370955893725369602[69] = 0; - out_370955893725369602[70] = 1; - out_370955893725369602[71] = 0; - out_370955893725369602[72] = 0; - out_370955893725369602[73] = 0; - out_370955893725369602[74] = 0; - out_370955893725369602[75] = 0; - out_370955893725369602[76] = 0; - out_370955893725369602[77] = 0; - out_370955893725369602[78] = 0; - out_370955893725369602[79] = 0; - out_370955893725369602[80] = 1; +void F_fun(double *state, double dt, double *out_110226856445903204) { + out_110226856445903204[0] = 1; + out_110226856445903204[1] = 0; + out_110226856445903204[2] = 0; + out_110226856445903204[3] = 0; + out_110226856445903204[4] = 0; + out_110226856445903204[5] = 0; + out_110226856445903204[6] = 0; + out_110226856445903204[7] = 0; + out_110226856445903204[8] = 0; + out_110226856445903204[9] = 0; + out_110226856445903204[10] = 1; + out_110226856445903204[11] = 0; + out_110226856445903204[12] = 0; + out_110226856445903204[13] = 0; + out_110226856445903204[14] = 0; + out_110226856445903204[15] = 0; + out_110226856445903204[16] = 0; + out_110226856445903204[17] = 0; + out_110226856445903204[18] = 0; + out_110226856445903204[19] = 0; + out_110226856445903204[20] = 1; + out_110226856445903204[21] = 0; + out_110226856445903204[22] = 0; + out_110226856445903204[23] = 0; + out_110226856445903204[24] = 0; + out_110226856445903204[25] = 0; + out_110226856445903204[26] = 0; + out_110226856445903204[27] = 0; + out_110226856445903204[28] = 0; + out_110226856445903204[29] = 0; + out_110226856445903204[30] = 1; + out_110226856445903204[31] = 0; + out_110226856445903204[32] = 0; + out_110226856445903204[33] = 0; + out_110226856445903204[34] = 0; + out_110226856445903204[35] = 0; + out_110226856445903204[36] = 0; + out_110226856445903204[37] = 0; + out_110226856445903204[38] = 0; + out_110226856445903204[39] = 0; + out_110226856445903204[40] = 1; + out_110226856445903204[41] = 0; + out_110226856445903204[42] = 0; + out_110226856445903204[43] = 0; + out_110226856445903204[44] = 0; + out_110226856445903204[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_110226856445903204[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_110226856445903204[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_110226856445903204[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_110226856445903204[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_110226856445903204[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_110226856445903204[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_110226856445903204[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_110226856445903204[53] = -9.8100000000000005*dt; + out_110226856445903204[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_110226856445903204[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_110226856445903204[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_110226856445903204[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_110226856445903204[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_110226856445903204[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_110226856445903204[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_110226856445903204[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_110226856445903204[62] = 0; + out_110226856445903204[63] = 0; + out_110226856445903204[64] = 0; + out_110226856445903204[65] = 0; + out_110226856445903204[66] = 0; + out_110226856445903204[67] = 0; + out_110226856445903204[68] = 0; + out_110226856445903204[69] = 0; + out_110226856445903204[70] = 1; + out_110226856445903204[71] = 0; + out_110226856445903204[72] = 0; + out_110226856445903204[73] = 0; + out_110226856445903204[74] = 0; + out_110226856445903204[75] = 0; + out_110226856445903204[76] = 0; + out_110226856445903204[77] = 0; + out_110226856445903204[78] = 0; + out_110226856445903204[79] = 0; + out_110226856445903204[80] = 1; } -void h_25(double *state, double *unused, double *out_8154090013633331696) { - out_8154090013633331696[0] = state[6]; +void h_25(double *state, double *unused, double *out_5646094358831955306) { + out_5646094358831955306[0] = state[6]; } -void H_25(double *state, double *unused, double *out_8415923992877417251) { - out_8415923992877417251[0] = 0; - out_8415923992877417251[1] = 0; - out_8415923992877417251[2] = 0; - out_8415923992877417251[3] = 0; - out_8415923992877417251[4] = 0; - out_8415923992877417251[5] = 0; - out_8415923992877417251[6] = 1; - out_8415923992877417251[7] = 0; - out_8415923992877417251[8] = 0; +void H_25(double *state, double *unused, double *out_4871128394768286557) { + out_4871128394768286557[0] = 0; + out_4871128394768286557[1] = 0; + out_4871128394768286557[2] = 0; + out_4871128394768286557[3] = 0; + out_4871128394768286557[4] = 0; + out_4871128394768286557[5] = 0; + out_4871128394768286557[6] = 1; + out_4871128394768286557[7] = 0; + out_4871128394768286557[8] = 0; } -void h_24(double *state, double *unused, double *out_7351280482288278166) { - out_7351280482288278166[0] = state[4]; - out_7351280482288278166[1] = state[5]; +void h_24(double *state, double *unused, double *out_7079061790558552168) { + out_7079061790558552168[0] = state[4]; + out_7079061790558552168[1] = state[5]; } -void H_24(double *state, double *unused, double *out_8301956961654820296) { - out_8301956961654820296[0] = 0; - out_8301956961654820296[1] = 0; - out_8301956961654820296[2] = 0; - out_8301956961654820296[3] = 0; - out_8301956961654820296[4] = 1; - out_8301956961654820296[5] = 0; - out_8301956961654820296[6] = 0; - out_8301956961654820296[7] = 0; - out_8301956961654820296[8] = 0; - out_8301956961654820296[9] = 0; - out_8301956961654820296[10] = 0; - out_8301956961654820296[11] = 0; - out_8301956961654820296[12] = 0; - out_8301956961654820296[13] = 0; - out_8301956961654820296[14] = 1; - out_8301956961654820296[15] = 0; - out_8301956961654820296[16] = 0; - out_8301956961654820296[17] = 0; +void H_24(double *state, double *unused, double *out_7374582999090103633) { + out_7374582999090103633[0] = 0; + out_7374582999090103633[1] = 0; + out_7374582999090103633[2] = 0; + out_7374582999090103633[3] = 0; + out_7374582999090103633[4] = 1; + out_7374582999090103633[5] = 0; + out_7374582999090103633[6] = 0; + out_7374582999090103633[7] = 0; + out_7374582999090103633[8] = 0; + out_7374582999090103633[9] = 0; + out_7374582999090103633[10] = 0; + out_7374582999090103633[11] = 0; + out_7374582999090103633[12] = 0; + out_7374582999090103633[13] = 0; + out_7374582999090103633[14] = 1; + out_7374582999090103633[15] = 0; + out_7374582999090103633[16] = 0; + out_7374582999090103633[17] = 0; } -void h_30(double *state, double *unused, double *out_4292406537204830044) { - out_4292406537204830044[0] = state[4]; +void h_30(double *state, double *unused, double *out_2117170897988717135) { + out_2117170897988717135[0] = state[4]; } -void H_30(double *state, double *unused, double *out_1499233651385800496) { - out_1499233651385800496[0] = 0; - out_1499233651385800496[1] = 0; - out_1499233651385800496[2] = 0; - out_1499233651385800496[3] = 0; - out_1499233651385800496[4] = 1; - out_1499233651385800496[5] = 0; - out_1499233651385800496[6] = 0; - out_1499233651385800496[7] = 0; - out_1499233651385800496[8] = 0; +void H_30(double *state, double *unused, double *out_2352795436261037930) { + out_2352795436261037930[0] = 0; + out_2352795436261037930[1] = 0; + out_2352795436261037930[2] = 0; + out_2352795436261037930[3] = 0; + out_2352795436261037930[4] = 1; + out_2352795436261037930[5] = 0; + out_2352795436261037930[6] = 0; + out_2352795436261037930[7] = 0; + out_2352795436261037930[8] = 0; } -void h_26(double *state, double *unused, double *out_7000316085453517212) { - out_7000316085453517212[0] = state[7]; +void h_26(double *state, double *unused, double *out_7183588416707059858) { + out_7183588416707059858[0] = state[7]; } -void H_26(double *state, double *unused, double *out_6289316761958078141) { - out_6289316761958078141[0] = 0; - out_6289316761958078141[1] = 0; - out_6289316761958078141[2] = 0; - out_6289316761958078141[3] = 0; - out_6289316761958078141[4] = 0; - out_6289316761958078141[5] = 0; - out_6289316761958078141[6] = 0; - out_6289316761958078141[7] = 1; - out_6289316761958078141[8] = 0; +void H_26(double *state, double *unused, double *out_8612631713642342781) { + out_8612631713642342781[0] = 0; + out_8612631713642342781[1] = 0; + out_8612631713642342781[2] = 0; + out_8612631713642342781[3] = 0; + out_8612631713642342781[4] = 0; + out_8612631713642342781[5] = 0; + out_8612631713642342781[6] = 0; + out_8612631713642342781[7] = 1; + out_8612631713642342781[8] = 0; } -void h_27(double *state, double *unused, double *out_3964284714812858416) { - out_3964284714812858416[0] = state[3]; +void h_27(double *state, double *unused, double *out_3041689038023257655) { + out_3041689038023257655[0] = state[3]; } -void H_27(double *state, double *unused, double *out_3673996963186225407) { - out_3673996963186225407[0] = 0; - out_3673996963186225407[1] = 0; - out_3673996963186225407[2] = 0; - out_3673996963186225407[3] = 1; - out_3673996963186225407[4] = 0; - out_3673996963186225407[5] = 0; - out_3673996963186225407[6] = 0; - out_3673996963186225407[7] = 0; - out_3673996963186225407[8] = 0; +void H_27(double *state, double *unused, double *out_129201365077094713) { + out_129201365077094713[0] = 0; + out_129201365077094713[1] = 0; + out_129201365077094713[2] = 0; + out_129201365077094713[3] = 1; + out_129201365077094713[4] = 0; + out_129201365077094713[5] = 0; + out_129201365077094713[6] = 0; + out_129201365077094713[7] = 0; + out_129201365077094713[8] = 0; } -void h_29(double *state, double *unused, double *out_3689090652528352527) { - out_3689090652528352527[0] = state[1]; +void h_29(double *state, double *unused, double *out_2884502369672128510) { + out_2884502369672128510[0] = state[1]; } -void H_29(double *state, double *unused, double *out_989002307071408312) { - out_989002307071408312[0] = 0; - out_989002307071408312[1] = 1; - out_989002307071408312[2] = 0; - out_989002307071408312[3] = 0; - out_989002307071408312[4] = 0; - out_989002307071408312[5] = 0; - out_989002307071408312[6] = 0; - out_989002307071408312[7] = 0; - out_989002307071408312[8] = 0; +void H_29(double *state, double *unused, double *out_1842564091946645746) { + out_1842564091946645746[0] = 0; + out_1842564091946645746[1] = 1; + out_1842564091946645746[2] = 0; + out_1842564091946645746[3] = 0; + out_1842564091946645746[4] = 0; + out_1842564091946645746[5] = 0; + out_1842564091946645746[6] = 0; + out_1842564091946645746[7] = 0; + out_1842564091946645746[8] = 0; } -void h_28(double *state, double *unused, double *out_1914000772745856672) { - out_1914000772745856672[0] = state[0]; +void h_28(double *state, double *unused, double *out_4902986578924970) { + out_4902986578924970[0] = state[0]; } -void H_28(double *state, double *unused, double *out_6071401324140938886) { - out_6071401324140938886[0] = 1; - out_6071401324140938886[1] = 0; - out_6071401324140938886[2] = 0; - out_6071401324140938886[3] = 0; - out_6071401324140938886[4] = 0; - out_6071401324140938886[5] = 0; - out_6071401324140938886[6] = 0; - out_6071401324140938886[7] = 0; - out_6071401324140938886[8] = 0; +void H_28(double *state, double *unused, double *out_6924963109016176320) { + out_6924963109016176320[0] = 1; + out_6924963109016176320[1] = 0; + out_6924963109016176320[2] = 0; + out_6924963109016176320[3] = 0; + out_6924963109016176320[4] = 0; + out_6924963109016176320[5] = 0; + out_6924963109016176320[6] = 0; + out_6924963109016176320[7] = 0; + out_6924963109016176320[8] = 0; } -void h_31(double *state, double *unused, double *out_8205050127743358283) { - out_8205050127743358283[0] = state[8]; +void h_31(double *state, double *unused, double *out_8547812156113883153) { + out_8547812156113883153[0] = state[8]; } -void H_31(double *state, double *unused, double *out_8385278031000456823) { - out_8385278031000456823[0] = 0; - out_8385278031000456823[1] = 0; - out_8385278031000456823[2] = 0; - out_8385278031000456823[3] = 0; - out_8385278031000456823[4] = 0; - out_8385278031000456823[5] = 0; - out_8385278031000456823[6] = 0; - out_8385278031000456823[7] = 0; - out_8385278031000456823[8] = 1; +void H_31(double *state, double *unused, double *out_4840482432891326129) { + out_4840482432891326129[0] = 0; + out_4840482432891326129[1] = 0; + out_4840482432891326129[2] = 0; + out_4840482432891326129[3] = 0; + out_4840482432891326129[4] = 0; + out_4840482432891326129[5] = 0; + out_4840482432891326129[6] = 0; + out_4840482432891326129[7] = 0; + out_4840482432891326129[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_3568804456057098654) { - err_fun(nom_x, delta_x, out_3568804456057098654); +void car_err_fun(double *nom_x, double *delta_x, double *out_8308214496735031777) { + err_fun(nom_x, delta_x, out_8308214496735031777); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1840397183166108031) { - inv_err_fun(nom_x, true_x, out_1840397183166108031); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8999766425853420329) { + inv_err_fun(nom_x, true_x, out_8999766425853420329); } -void car_H_mod_fun(double *state, double *out_8921258435077713963) { - H_mod_fun(state, out_8921258435077713963); +void car_H_mod_fun(double *state, double *out_8671923853756600219) { + H_mod_fun(state, out_8671923853756600219); } -void car_f_fun(double *state, double dt, double *out_3277027219914288776) { - f_fun(state, dt, out_3277027219914288776); +void car_f_fun(double *state, double dt, double *out_3762356930851513593) { + f_fun(state, dt, out_3762356930851513593); } -void car_F_fun(double *state, double dt, double *out_370955893725369602) { - F_fun(state, dt, out_370955893725369602); +void car_F_fun(double *state, double dt, double *out_110226856445903204) { + F_fun(state, dt, out_110226856445903204); } -void car_h_25(double *state, double *unused, double *out_8154090013633331696) { - h_25(state, unused, out_8154090013633331696); +void car_h_25(double *state, double *unused, double *out_5646094358831955306) { + h_25(state, unused, out_5646094358831955306); } -void car_H_25(double *state, double *unused, double *out_8415923992877417251) { - H_25(state, unused, out_8415923992877417251); +void car_H_25(double *state, double *unused, double *out_4871128394768286557) { + H_25(state, unused, out_4871128394768286557); } -void car_h_24(double *state, double *unused, double *out_7351280482288278166) { - h_24(state, unused, out_7351280482288278166); +void car_h_24(double *state, double *unused, double *out_7079061790558552168) { + h_24(state, unused, out_7079061790558552168); } -void car_H_24(double *state, double *unused, double *out_8301956961654820296) { - H_24(state, unused, out_8301956961654820296); +void car_H_24(double *state, double *unused, double *out_7374582999090103633) { + H_24(state, unused, out_7374582999090103633); } -void car_h_30(double *state, double *unused, double *out_4292406537204830044) { - h_30(state, unused, out_4292406537204830044); +void car_h_30(double *state, double *unused, double *out_2117170897988717135) { + h_30(state, unused, out_2117170897988717135); } -void car_H_30(double *state, double *unused, double *out_1499233651385800496) { - H_30(state, unused, out_1499233651385800496); +void car_H_30(double *state, double *unused, double *out_2352795436261037930) { + H_30(state, unused, out_2352795436261037930); } -void car_h_26(double *state, double *unused, double *out_7000316085453517212) { - h_26(state, unused, out_7000316085453517212); +void car_h_26(double *state, double *unused, double *out_7183588416707059858) { + h_26(state, unused, out_7183588416707059858); } -void car_H_26(double *state, double *unused, double *out_6289316761958078141) { - H_26(state, unused, out_6289316761958078141); +void car_H_26(double *state, double *unused, double *out_8612631713642342781) { + H_26(state, unused, out_8612631713642342781); } -void car_h_27(double *state, double *unused, double *out_3964284714812858416) { - h_27(state, unused, out_3964284714812858416); +void car_h_27(double *state, double *unused, double *out_3041689038023257655) { + h_27(state, unused, out_3041689038023257655); } -void car_H_27(double *state, double *unused, double *out_3673996963186225407) { - H_27(state, unused, out_3673996963186225407); +void car_H_27(double *state, double *unused, double *out_129201365077094713) { + H_27(state, unused, out_129201365077094713); } -void car_h_29(double *state, double *unused, double *out_3689090652528352527) { - h_29(state, unused, out_3689090652528352527); +void car_h_29(double *state, double *unused, double *out_2884502369672128510) { + h_29(state, unused, out_2884502369672128510); } -void car_H_29(double *state, double *unused, double *out_989002307071408312) { - H_29(state, unused, out_989002307071408312); +void car_H_29(double *state, double *unused, double *out_1842564091946645746) { + H_29(state, unused, out_1842564091946645746); } -void car_h_28(double *state, double *unused, double *out_1914000772745856672) { - h_28(state, unused, out_1914000772745856672); +void car_h_28(double *state, double *unused, double *out_4902986578924970) { + h_28(state, unused, out_4902986578924970); } -void car_H_28(double *state, double *unused, double *out_6071401324140938886) { - H_28(state, unused, out_6071401324140938886); +void car_H_28(double *state, double *unused, double *out_6924963109016176320) { + H_28(state, unused, out_6924963109016176320); } -void car_h_31(double *state, double *unused, double *out_8205050127743358283) { - h_31(state, unused, out_8205050127743358283); +void car_h_31(double *state, double *unused, double *out_8547812156113883153) { + h_31(state, unused, out_8547812156113883153); } -void car_H_31(double *state, double *unused, double *out_8385278031000456823) { - H_31(state, unused, out_8385278031000456823); +void car_H_31(double *state, double *unused, double *out_4840482432891326129) { + H_31(state, unused, out_4840482432891326129); } 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 655b5e1ae..99c6da0d6 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_3568804456057098654); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1840397183166108031); -void car_H_mod_fun(double *state, double *out_8921258435077713963); -void car_f_fun(double *state, double dt, double *out_3277027219914288776); -void car_F_fun(double *state, double dt, double *out_370955893725369602); -void car_h_25(double *state, double *unused, double *out_8154090013633331696); -void car_H_25(double *state, double *unused, double *out_8415923992877417251); -void car_h_24(double *state, double *unused, double *out_7351280482288278166); -void car_H_24(double *state, double *unused, double *out_8301956961654820296); -void car_h_30(double *state, double *unused, double *out_4292406537204830044); -void car_H_30(double *state, double *unused, double *out_1499233651385800496); -void car_h_26(double *state, double *unused, double *out_7000316085453517212); -void car_H_26(double *state, double *unused, double *out_6289316761958078141); -void car_h_27(double *state, double *unused, double *out_3964284714812858416); -void car_H_27(double *state, double *unused, double *out_3673996963186225407); -void car_h_29(double *state, double *unused, double *out_3689090652528352527); -void car_H_29(double *state, double *unused, double *out_989002307071408312); -void car_h_28(double *state, double *unused, double *out_1914000772745856672); -void car_H_28(double *state, double *unused, double *out_6071401324140938886); -void car_h_31(double *state, double *unused, double *out_8205050127743358283); -void car_H_31(double *state, double *unused, double *out_8385278031000456823); +void car_err_fun(double *nom_x, double *delta_x, double *out_8308214496735031777); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8999766425853420329); +void car_H_mod_fun(double *state, double *out_8671923853756600219); +void car_f_fun(double *state, double dt, double *out_3762356930851513593); +void car_F_fun(double *state, double dt, double *out_110226856445903204); +void car_h_25(double *state, double *unused, double *out_5646094358831955306); +void car_H_25(double *state, double *unused, double *out_4871128394768286557); +void car_h_24(double *state, double *unused, double *out_7079061790558552168); +void car_H_24(double *state, double *unused, double *out_7374582999090103633); +void car_h_30(double *state, double *unused, double *out_2117170897988717135); +void car_H_30(double *state, double *unused, double *out_2352795436261037930); +void car_h_26(double *state, double *unused, double *out_7183588416707059858); +void car_H_26(double *state, double *unused, double *out_8612631713642342781); +void car_h_27(double *state, double *unused, double *out_3041689038023257655); +void car_H_27(double *state, double *unused, double *out_129201365077094713); +void car_h_29(double *state, double *unused, double *out_2884502369672128510); +void car_H_29(double *state, double *unused, double *out_1842564091946645746); +void car_h_28(double *state, double *unused, double *out_4902986578924970); +void car_H_28(double *state, double *unused, double *out_6924963109016176320); +void car_h_31(double *state, double *unused, double *out_8547812156113883153); +void car_H_31(double *state, double *unused, double *out_4840482432891326129); 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 c62883fa7..161790e22 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_3667758909157266385) { - out_3667758909157266385[0] = delta_x[0] + nom_x[0]; - out_3667758909157266385[1] = delta_x[1] + nom_x[1]; - out_3667758909157266385[2] = delta_x[2] + nom_x[2]; - out_3667758909157266385[3] = delta_x[3] + nom_x[3]; - out_3667758909157266385[4] = delta_x[4] + nom_x[4]; - out_3667758909157266385[5] = delta_x[5] + nom_x[5]; - out_3667758909157266385[6] = delta_x[6] + nom_x[6]; - out_3667758909157266385[7] = delta_x[7] + nom_x[7]; - out_3667758909157266385[8] = delta_x[8] + nom_x[8]; - out_3667758909157266385[9] = delta_x[9] + nom_x[9]; - out_3667758909157266385[10] = delta_x[10] + nom_x[10]; - out_3667758909157266385[11] = delta_x[11] + nom_x[11]; - out_3667758909157266385[12] = delta_x[12] + nom_x[12]; - out_3667758909157266385[13] = delta_x[13] + nom_x[13]; - out_3667758909157266385[14] = delta_x[14] + nom_x[14]; - out_3667758909157266385[15] = delta_x[15] + nom_x[15]; - out_3667758909157266385[16] = delta_x[16] + nom_x[16]; - out_3667758909157266385[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_8958383251573640) { + out_8958383251573640[0] = delta_x[0] + nom_x[0]; + out_8958383251573640[1] = delta_x[1] + nom_x[1]; + out_8958383251573640[2] = delta_x[2] + nom_x[2]; + out_8958383251573640[3] = delta_x[3] + nom_x[3]; + out_8958383251573640[4] = delta_x[4] + nom_x[4]; + out_8958383251573640[5] = delta_x[5] + nom_x[5]; + out_8958383251573640[6] = delta_x[6] + nom_x[6]; + out_8958383251573640[7] = delta_x[7] + nom_x[7]; + out_8958383251573640[8] = delta_x[8] + nom_x[8]; + out_8958383251573640[9] = delta_x[9] + nom_x[9]; + out_8958383251573640[10] = delta_x[10] + nom_x[10]; + out_8958383251573640[11] = delta_x[11] + nom_x[11]; + out_8958383251573640[12] = delta_x[12] + nom_x[12]; + out_8958383251573640[13] = delta_x[13] + nom_x[13]; + out_8958383251573640[14] = delta_x[14] + nom_x[14]; + out_8958383251573640[15] = delta_x[15] + nom_x[15]; + out_8958383251573640[16] = delta_x[16] + nom_x[16]; + out_8958383251573640[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3255008687637156165) { - out_3255008687637156165[0] = -nom_x[0] + true_x[0]; - out_3255008687637156165[1] = -nom_x[1] + true_x[1]; - out_3255008687637156165[2] = -nom_x[2] + true_x[2]; - out_3255008687637156165[3] = -nom_x[3] + true_x[3]; - out_3255008687637156165[4] = -nom_x[4] + true_x[4]; - out_3255008687637156165[5] = -nom_x[5] + true_x[5]; - out_3255008687637156165[6] = -nom_x[6] + true_x[6]; - out_3255008687637156165[7] = -nom_x[7] + true_x[7]; - out_3255008687637156165[8] = -nom_x[8] + true_x[8]; - out_3255008687637156165[9] = -nom_x[9] + true_x[9]; - out_3255008687637156165[10] = -nom_x[10] + true_x[10]; - out_3255008687637156165[11] = -nom_x[11] + true_x[11]; - out_3255008687637156165[12] = -nom_x[12] + true_x[12]; - out_3255008687637156165[13] = -nom_x[13] + true_x[13]; - out_3255008687637156165[14] = -nom_x[14] + true_x[14]; - out_3255008687637156165[15] = -nom_x[15] + true_x[15]; - out_3255008687637156165[16] = -nom_x[16] + true_x[16]; - out_3255008687637156165[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3510167321874192836) { + out_3510167321874192836[0] = -nom_x[0] + true_x[0]; + out_3510167321874192836[1] = -nom_x[1] + true_x[1]; + out_3510167321874192836[2] = -nom_x[2] + true_x[2]; + out_3510167321874192836[3] = -nom_x[3] + true_x[3]; + out_3510167321874192836[4] = -nom_x[4] + true_x[4]; + out_3510167321874192836[5] = -nom_x[5] + true_x[5]; + out_3510167321874192836[6] = -nom_x[6] + true_x[6]; + out_3510167321874192836[7] = -nom_x[7] + true_x[7]; + out_3510167321874192836[8] = -nom_x[8] + true_x[8]; + out_3510167321874192836[9] = -nom_x[9] + true_x[9]; + out_3510167321874192836[10] = -nom_x[10] + true_x[10]; + out_3510167321874192836[11] = -nom_x[11] + true_x[11]; + out_3510167321874192836[12] = -nom_x[12] + true_x[12]; + out_3510167321874192836[13] = -nom_x[13] + true_x[13]; + out_3510167321874192836[14] = -nom_x[14] + true_x[14]; + out_3510167321874192836[15] = -nom_x[15] + true_x[15]; + out_3510167321874192836[16] = -nom_x[16] + true_x[16]; + out_3510167321874192836[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_1678148223451849404) { - out_1678148223451849404[0] = 1.0; - out_1678148223451849404[1] = 0.0; - out_1678148223451849404[2] = 0.0; - out_1678148223451849404[3] = 0.0; - out_1678148223451849404[4] = 0.0; - out_1678148223451849404[5] = 0.0; - out_1678148223451849404[6] = 0.0; - out_1678148223451849404[7] = 0.0; - out_1678148223451849404[8] = 0.0; - out_1678148223451849404[9] = 0.0; - out_1678148223451849404[10] = 0.0; - out_1678148223451849404[11] = 0.0; - out_1678148223451849404[12] = 0.0; - out_1678148223451849404[13] = 0.0; - out_1678148223451849404[14] = 0.0; - out_1678148223451849404[15] = 0.0; - out_1678148223451849404[16] = 0.0; - out_1678148223451849404[17] = 0.0; - out_1678148223451849404[18] = 0.0; - out_1678148223451849404[19] = 1.0; - out_1678148223451849404[20] = 0.0; - out_1678148223451849404[21] = 0.0; - out_1678148223451849404[22] = 0.0; - out_1678148223451849404[23] = 0.0; - out_1678148223451849404[24] = 0.0; - out_1678148223451849404[25] = 0.0; - out_1678148223451849404[26] = 0.0; - out_1678148223451849404[27] = 0.0; - out_1678148223451849404[28] = 0.0; - out_1678148223451849404[29] = 0.0; - out_1678148223451849404[30] = 0.0; - out_1678148223451849404[31] = 0.0; - out_1678148223451849404[32] = 0.0; - out_1678148223451849404[33] = 0.0; - out_1678148223451849404[34] = 0.0; - out_1678148223451849404[35] = 0.0; - out_1678148223451849404[36] = 0.0; - out_1678148223451849404[37] = 0.0; - out_1678148223451849404[38] = 1.0; - out_1678148223451849404[39] = 0.0; - out_1678148223451849404[40] = 0.0; - out_1678148223451849404[41] = 0.0; - out_1678148223451849404[42] = 0.0; - out_1678148223451849404[43] = 0.0; - out_1678148223451849404[44] = 0.0; - out_1678148223451849404[45] = 0.0; - out_1678148223451849404[46] = 0.0; - out_1678148223451849404[47] = 0.0; - out_1678148223451849404[48] = 0.0; - out_1678148223451849404[49] = 0.0; - out_1678148223451849404[50] = 0.0; - out_1678148223451849404[51] = 0.0; - out_1678148223451849404[52] = 0.0; - out_1678148223451849404[53] = 0.0; - out_1678148223451849404[54] = 0.0; - out_1678148223451849404[55] = 0.0; - out_1678148223451849404[56] = 0.0; - out_1678148223451849404[57] = 1.0; - out_1678148223451849404[58] = 0.0; - out_1678148223451849404[59] = 0.0; - out_1678148223451849404[60] = 0.0; - out_1678148223451849404[61] = 0.0; - out_1678148223451849404[62] = 0.0; - out_1678148223451849404[63] = 0.0; - out_1678148223451849404[64] = 0.0; - out_1678148223451849404[65] = 0.0; - out_1678148223451849404[66] = 0.0; - out_1678148223451849404[67] = 0.0; - out_1678148223451849404[68] = 0.0; - out_1678148223451849404[69] = 0.0; - out_1678148223451849404[70] = 0.0; - out_1678148223451849404[71] = 0.0; - out_1678148223451849404[72] = 0.0; - out_1678148223451849404[73] = 0.0; - out_1678148223451849404[74] = 0.0; - out_1678148223451849404[75] = 0.0; - out_1678148223451849404[76] = 1.0; - out_1678148223451849404[77] = 0.0; - out_1678148223451849404[78] = 0.0; - out_1678148223451849404[79] = 0.0; - out_1678148223451849404[80] = 0.0; - out_1678148223451849404[81] = 0.0; - out_1678148223451849404[82] = 0.0; - out_1678148223451849404[83] = 0.0; - out_1678148223451849404[84] = 0.0; - out_1678148223451849404[85] = 0.0; - out_1678148223451849404[86] = 0.0; - out_1678148223451849404[87] = 0.0; - out_1678148223451849404[88] = 0.0; - out_1678148223451849404[89] = 0.0; - out_1678148223451849404[90] = 0.0; - out_1678148223451849404[91] = 0.0; - out_1678148223451849404[92] = 0.0; - out_1678148223451849404[93] = 0.0; - out_1678148223451849404[94] = 0.0; - out_1678148223451849404[95] = 1.0; - out_1678148223451849404[96] = 0.0; - out_1678148223451849404[97] = 0.0; - out_1678148223451849404[98] = 0.0; - out_1678148223451849404[99] = 0.0; - out_1678148223451849404[100] = 0.0; - out_1678148223451849404[101] = 0.0; - out_1678148223451849404[102] = 0.0; - out_1678148223451849404[103] = 0.0; - out_1678148223451849404[104] = 0.0; - out_1678148223451849404[105] = 0.0; - out_1678148223451849404[106] = 0.0; - out_1678148223451849404[107] = 0.0; - out_1678148223451849404[108] = 0.0; - out_1678148223451849404[109] = 0.0; - out_1678148223451849404[110] = 0.0; - out_1678148223451849404[111] = 0.0; - out_1678148223451849404[112] = 0.0; - out_1678148223451849404[113] = 0.0; - out_1678148223451849404[114] = 1.0; - out_1678148223451849404[115] = 0.0; - out_1678148223451849404[116] = 0.0; - out_1678148223451849404[117] = 0.0; - out_1678148223451849404[118] = 0.0; - out_1678148223451849404[119] = 0.0; - out_1678148223451849404[120] = 0.0; - out_1678148223451849404[121] = 0.0; - out_1678148223451849404[122] = 0.0; - out_1678148223451849404[123] = 0.0; - out_1678148223451849404[124] = 0.0; - out_1678148223451849404[125] = 0.0; - out_1678148223451849404[126] = 0.0; - out_1678148223451849404[127] = 0.0; - out_1678148223451849404[128] = 0.0; - out_1678148223451849404[129] = 0.0; - out_1678148223451849404[130] = 0.0; - out_1678148223451849404[131] = 0.0; - out_1678148223451849404[132] = 0.0; - out_1678148223451849404[133] = 1.0; - out_1678148223451849404[134] = 0.0; - out_1678148223451849404[135] = 0.0; - out_1678148223451849404[136] = 0.0; - out_1678148223451849404[137] = 0.0; - out_1678148223451849404[138] = 0.0; - out_1678148223451849404[139] = 0.0; - out_1678148223451849404[140] = 0.0; - out_1678148223451849404[141] = 0.0; - out_1678148223451849404[142] = 0.0; - out_1678148223451849404[143] = 0.0; - out_1678148223451849404[144] = 0.0; - out_1678148223451849404[145] = 0.0; - out_1678148223451849404[146] = 0.0; - out_1678148223451849404[147] = 0.0; - out_1678148223451849404[148] = 0.0; - out_1678148223451849404[149] = 0.0; - out_1678148223451849404[150] = 0.0; - out_1678148223451849404[151] = 0.0; - out_1678148223451849404[152] = 1.0; - out_1678148223451849404[153] = 0.0; - out_1678148223451849404[154] = 0.0; - out_1678148223451849404[155] = 0.0; - out_1678148223451849404[156] = 0.0; - out_1678148223451849404[157] = 0.0; - out_1678148223451849404[158] = 0.0; - out_1678148223451849404[159] = 0.0; - out_1678148223451849404[160] = 0.0; - out_1678148223451849404[161] = 0.0; - out_1678148223451849404[162] = 0.0; - out_1678148223451849404[163] = 0.0; - out_1678148223451849404[164] = 0.0; - out_1678148223451849404[165] = 0.0; - out_1678148223451849404[166] = 0.0; - out_1678148223451849404[167] = 0.0; - out_1678148223451849404[168] = 0.0; - out_1678148223451849404[169] = 0.0; - out_1678148223451849404[170] = 0.0; - out_1678148223451849404[171] = 1.0; - out_1678148223451849404[172] = 0.0; - out_1678148223451849404[173] = 0.0; - out_1678148223451849404[174] = 0.0; - out_1678148223451849404[175] = 0.0; - out_1678148223451849404[176] = 0.0; - out_1678148223451849404[177] = 0.0; - out_1678148223451849404[178] = 0.0; - out_1678148223451849404[179] = 0.0; - out_1678148223451849404[180] = 0.0; - out_1678148223451849404[181] = 0.0; - out_1678148223451849404[182] = 0.0; - out_1678148223451849404[183] = 0.0; - out_1678148223451849404[184] = 0.0; - out_1678148223451849404[185] = 0.0; - out_1678148223451849404[186] = 0.0; - out_1678148223451849404[187] = 0.0; - out_1678148223451849404[188] = 0.0; - out_1678148223451849404[189] = 0.0; - out_1678148223451849404[190] = 1.0; - out_1678148223451849404[191] = 0.0; - out_1678148223451849404[192] = 0.0; - out_1678148223451849404[193] = 0.0; - out_1678148223451849404[194] = 0.0; - out_1678148223451849404[195] = 0.0; - out_1678148223451849404[196] = 0.0; - out_1678148223451849404[197] = 0.0; - out_1678148223451849404[198] = 0.0; - out_1678148223451849404[199] = 0.0; - out_1678148223451849404[200] = 0.0; - out_1678148223451849404[201] = 0.0; - out_1678148223451849404[202] = 0.0; - out_1678148223451849404[203] = 0.0; - out_1678148223451849404[204] = 0.0; - out_1678148223451849404[205] = 0.0; - out_1678148223451849404[206] = 0.0; - out_1678148223451849404[207] = 0.0; - out_1678148223451849404[208] = 0.0; - out_1678148223451849404[209] = 1.0; - out_1678148223451849404[210] = 0.0; - out_1678148223451849404[211] = 0.0; - out_1678148223451849404[212] = 0.0; - out_1678148223451849404[213] = 0.0; - out_1678148223451849404[214] = 0.0; - out_1678148223451849404[215] = 0.0; - out_1678148223451849404[216] = 0.0; - out_1678148223451849404[217] = 0.0; - out_1678148223451849404[218] = 0.0; - out_1678148223451849404[219] = 0.0; - out_1678148223451849404[220] = 0.0; - out_1678148223451849404[221] = 0.0; - out_1678148223451849404[222] = 0.0; - out_1678148223451849404[223] = 0.0; - out_1678148223451849404[224] = 0.0; - out_1678148223451849404[225] = 0.0; - out_1678148223451849404[226] = 0.0; - out_1678148223451849404[227] = 0.0; - out_1678148223451849404[228] = 1.0; - out_1678148223451849404[229] = 0.0; - out_1678148223451849404[230] = 0.0; - out_1678148223451849404[231] = 0.0; - out_1678148223451849404[232] = 0.0; - out_1678148223451849404[233] = 0.0; - out_1678148223451849404[234] = 0.0; - out_1678148223451849404[235] = 0.0; - out_1678148223451849404[236] = 0.0; - out_1678148223451849404[237] = 0.0; - out_1678148223451849404[238] = 0.0; - out_1678148223451849404[239] = 0.0; - out_1678148223451849404[240] = 0.0; - out_1678148223451849404[241] = 0.0; - out_1678148223451849404[242] = 0.0; - out_1678148223451849404[243] = 0.0; - out_1678148223451849404[244] = 0.0; - out_1678148223451849404[245] = 0.0; - out_1678148223451849404[246] = 0.0; - out_1678148223451849404[247] = 1.0; - out_1678148223451849404[248] = 0.0; - out_1678148223451849404[249] = 0.0; - out_1678148223451849404[250] = 0.0; - out_1678148223451849404[251] = 0.0; - out_1678148223451849404[252] = 0.0; - out_1678148223451849404[253] = 0.0; - out_1678148223451849404[254] = 0.0; - out_1678148223451849404[255] = 0.0; - out_1678148223451849404[256] = 0.0; - out_1678148223451849404[257] = 0.0; - out_1678148223451849404[258] = 0.0; - out_1678148223451849404[259] = 0.0; - out_1678148223451849404[260] = 0.0; - out_1678148223451849404[261] = 0.0; - out_1678148223451849404[262] = 0.0; - out_1678148223451849404[263] = 0.0; - out_1678148223451849404[264] = 0.0; - out_1678148223451849404[265] = 0.0; - out_1678148223451849404[266] = 1.0; - out_1678148223451849404[267] = 0.0; - out_1678148223451849404[268] = 0.0; - out_1678148223451849404[269] = 0.0; - out_1678148223451849404[270] = 0.0; - out_1678148223451849404[271] = 0.0; - out_1678148223451849404[272] = 0.0; - out_1678148223451849404[273] = 0.0; - out_1678148223451849404[274] = 0.0; - out_1678148223451849404[275] = 0.0; - out_1678148223451849404[276] = 0.0; - out_1678148223451849404[277] = 0.0; - out_1678148223451849404[278] = 0.0; - out_1678148223451849404[279] = 0.0; - out_1678148223451849404[280] = 0.0; - out_1678148223451849404[281] = 0.0; - out_1678148223451849404[282] = 0.0; - out_1678148223451849404[283] = 0.0; - out_1678148223451849404[284] = 0.0; - out_1678148223451849404[285] = 1.0; - out_1678148223451849404[286] = 0.0; - out_1678148223451849404[287] = 0.0; - out_1678148223451849404[288] = 0.0; - out_1678148223451849404[289] = 0.0; - out_1678148223451849404[290] = 0.0; - out_1678148223451849404[291] = 0.0; - out_1678148223451849404[292] = 0.0; - out_1678148223451849404[293] = 0.0; - out_1678148223451849404[294] = 0.0; - out_1678148223451849404[295] = 0.0; - out_1678148223451849404[296] = 0.0; - out_1678148223451849404[297] = 0.0; - out_1678148223451849404[298] = 0.0; - out_1678148223451849404[299] = 0.0; - out_1678148223451849404[300] = 0.0; - out_1678148223451849404[301] = 0.0; - out_1678148223451849404[302] = 0.0; - out_1678148223451849404[303] = 0.0; - out_1678148223451849404[304] = 1.0; - out_1678148223451849404[305] = 0.0; - out_1678148223451849404[306] = 0.0; - out_1678148223451849404[307] = 0.0; - out_1678148223451849404[308] = 0.0; - out_1678148223451849404[309] = 0.0; - out_1678148223451849404[310] = 0.0; - out_1678148223451849404[311] = 0.0; - out_1678148223451849404[312] = 0.0; - out_1678148223451849404[313] = 0.0; - out_1678148223451849404[314] = 0.0; - out_1678148223451849404[315] = 0.0; - out_1678148223451849404[316] = 0.0; - out_1678148223451849404[317] = 0.0; - out_1678148223451849404[318] = 0.0; - out_1678148223451849404[319] = 0.0; - out_1678148223451849404[320] = 0.0; - out_1678148223451849404[321] = 0.0; - out_1678148223451849404[322] = 0.0; - out_1678148223451849404[323] = 1.0; +void H_mod_fun(double *state, double *out_10314592692584858) { + out_10314592692584858[0] = 1.0; + out_10314592692584858[1] = 0.0; + out_10314592692584858[2] = 0.0; + out_10314592692584858[3] = 0.0; + out_10314592692584858[4] = 0.0; + out_10314592692584858[5] = 0.0; + out_10314592692584858[6] = 0.0; + out_10314592692584858[7] = 0.0; + out_10314592692584858[8] = 0.0; + out_10314592692584858[9] = 0.0; + out_10314592692584858[10] = 0.0; + out_10314592692584858[11] = 0.0; + out_10314592692584858[12] = 0.0; + out_10314592692584858[13] = 0.0; + out_10314592692584858[14] = 0.0; + out_10314592692584858[15] = 0.0; + out_10314592692584858[16] = 0.0; + out_10314592692584858[17] = 0.0; + out_10314592692584858[18] = 0.0; + out_10314592692584858[19] = 1.0; + out_10314592692584858[20] = 0.0; + out_10314592692584858[21] = 0.0; + out_10314592692584858[22] = 0.0; + out_10314592692584858[23] = 0.0; + out_10314592692584858[24] = 0.0; + out_10314592692584858[25] = 0.0; + out_10314592692584858[26] = 0.0; + out_10314592692584858[27] = 0.0; + out_10314592692584858[28] = 0.0; + out_10314592692584858[29] = 0.0; + out_10314592692584858[30] = 0.0; + out_10314592692584858[31] = 0.0; + out_10314592692584858[32] = 0.0; + out_10314592692584858[33] = 0.0; + out_10314592692584858[34] = 0.0; + out_10314592692584858[35] = 0.0; + out_10314592692584858[36] = 0.0; + out_10314592692584858[37] = 0.0; + out_10314592692584858[38] = 1.0; + out_10314592692584858[39] = 0.0; + out_10314592692584858[40] = 0.0; + out_10314592692584858[41] = 0.0; + out_10314592692584858[42] = 0.0; + out_10314592692584858[43] = 0.0; + out_10314592692584858[44] = 0.0; + out_10314592692584858[45] = 0.0; + out_10314592692584858[46] = 0.0; + out_10314592692584858[47] = 0.0; + out_10314592692584858[48] = 0.0; + out_10314592692584858[49] = 0.0; + out_10314592692584858[50] = 0.0; + out_10314592692584858[51] = 0.0; + out_10314592692584858[52] = 0.0; + out_10314592692584858[53] = 0.0; + out_10314592692584858[54] = 0.0; + out_10314592692584858[55] = 0.0; + out_10314592692584858[56] = 0.0; + out_10314592692584858[57] = 1.0; + out_10314592692584858[58] = 0.0; + out_10314592692584858[59] = 0.0; + out_10314592692584858[60] = 0.0; + out_10314592692584858[61] = 0.0; + out_10314592692584858[62] = 0.0; + out_10314592692584858[63] = 0.0; + out_10314592692584858[64] = 0.0; + out_10314592692584858[65] = 0.0; + out_10314592692584858[66] = 0.0; + out_10314592692584858[67] = 0.0; + out_10314592692584858[68] = 0.0; + out_10314592692584858[69] = 0.0; + out_10314592692584858[70] = 0.0; + out_10314592692584858[71] = 0.0; + out_10314592692584858[72] = 0.0; + out_10314592692584858[73] = 0.0; + out_10314592692584858[74] = 0.0; + out_10314592692584858[75] = 0.0; + out_10314592692584858[76] = 1.0; + out_10314592692584858[77] = 0.0; + out_10314592692584858[78] = 0.0; + out_10314592692584858[79] = 0.0; + out_10314592692584858[80] = 0.0; + out_10314592692584858[81] = 0.0; + out_10314592692584858[82] = 0.0; + out_10314592692584858[83] = 0.0; + out_10314592692584858[84] = 0.0; + out_10314592692584858[85] = 0.0; + out_10314592692584858[86] = 0.0; + out_10314592692584858[87] = 0.0; + out_10314592692584858[88] = 0.0; + out_10314592692584858[89] = 0.0; + out_10314592692584858[90] = 0.0; + out_10314592692584858[91] = 0.0; + out_10314592692584858[92] = 0.0; + out_10314592692584858[93] = 0.0; + out_10314592692584858[94] = 0.0; + out_10314592692584858[95] = 1.0; + out_10314592692584858[96] = 0.0; + out_10314592692584858[97] = 0.0; + out_10314592692584858[98] = 0.0; + out_10314592692584858[99] = 0.0; + out_10314592692584858[100] = 0.0; + out_10314592692584858[101] = 0.0; + out_10314592692584858[102] = 0.0; + out_10314592692584858[103] = 0.0; + out_10314592692584858[104] = 0.0; + out_10314592692584858[105] = 0.0; + out_10314592692584858[106] = 0.0; + out_10314592692584858[107] = 0.0; + out_10314592692584858[108] = 0.0; + out_10314592692584858[109] = 0.0; + out_10314592692584858[110] = 0.0; + out_10314592692584858[111] = 0.0; + out_10314592692584858[112] = 0.0; + out_10314592692584858[113] = 0.0; + out_10314592692584858[114] = 1.0; + out_10314592692584858[115] = 0.0; + out_10314592692584858[116] = 0.0; + out_10314592692584858[117] = 0.0; + out_10314592692584858[118] = 0.0; + out_10314592692584858[119] = 0.0; + out_10314592692584858[120] = 0.0; + out_10314592692584858[121] = 0.0; + out_10314592692584858[122] = 0.0; + out_10314592692584858[123] = 0.0; + out_10314592692584858[124] = 0.0; + out_10314592692584858[125] = 0.0; + out_10314592692584858[126] = 0.0; + out_10314592692584858[127] = 0.0; + out_10314592692584858[128] = 0.0; + out_10314592692584858[129] = 0.0; + out_10314592692584858[130] = 0.0; + out_10314592692584858[131] = 0.0; + out_10314592692584858[132] = 0.0; + out_10314592692584858[133] = 1.0; + out_10314592692584858[134] = 0.0; + out_10314592692584858[135] = 0.0; + out_10314592692584858[136] = 0.0; + out_10314592692584858[137] = 0.0; + out_10314592692584858[138] = 0.0; + out_10314592692584858[139] = 0.0; + out_10314592692584858[140] = 0.0; + out_10314592692584858[141] = 0.0; + out_10314592692584858[142] = 0.0; + out_10314592692584858[143] = 0.0; + out_10314592692584858[144] = 0.0; + out_10314592692584858[145] = 0.0; + out_10314592692584858[146] = 0.0; + out_10314592692584858[147] = 0.0; + out_10314592692584858[148] = 0.0; + out_10314592692584858[149] = 0.0; + out_10314592692584858[150] = 0.0; + out_10314592692584858[151] = 0.0; + out_10314592692584858[152] = 1.0; + out_10314592692584858[153] = 0.0; + out_10314592692584858[154] = 0.0; + out_10314592692584858[155] = 0.0; + out_10314592692584858[156] = 0.0; + out_10314592692584858[157] = 0.0; + out_10314592692584858[158] = 0.0; + out_10314592692584858[159] = 0.0; + out_10314592692584858[160] = 0.0; + out_10314592692584858[161] = 0.0; + out_10314592692584858[162] = 0.0; + out_10314592692584858[163] = 0.0; + out_10314592692584858[164] = 0.0; + out_10314592692584858[165] = 0.0; + out_10314592692584858[166] = 0.0; + out_10314592692584858[167] = 0.0; + out_10314592692584858[168] = 0.0; + out_10314592692584858[169] = 0.0; + out_10314592692584858[170] = 0.0; + out_10314592692584858[171] = 1.0; + out_10314592692584858[172] = 0.0; + out_10314592692584858[173] = 0.0; + out_10314592692584858[174] = 0.0; + out_10314592692584858[175] = 0.0; + out_10314592692584858[176] = 0.0; + out_10314592692584858[177] = 0.0; + out_10314592692584858[178] = 0.0; + out_10314592692584858[179] = 0.0; + out_10314592692584858[180] = 0.0; + out_10314592692584858[181] = 0.0; + out_10314592692584858[182] = 0.0; + out_10314592692584858[183] = 0.0; + out_10314592692584858[184] = 0.0; + out_10314592692584858[185] = 0.0; + out_10314592692584858[186] = 0.0; + out_10314592692584858[187] = 0.0; + out_10314592692584858[188] = 0.0; + out_10314592692584858[189] = 0.0; + out_10314592692584858[190] = 1.0; + out_10314592692584858[191] = 0.0; + out_10314592692584858[192] = 0.0; + out_10314592692584858[193] = 0.0; + out_10314592692584858[194] = 0.0; + out_10314592692584858[195] = 0.0; + out_10314592692584858[196] = 0.0; + out_10314592692584858[197] = 0.0; + out_10314592692584858[198] = 0.0; + out_10314592692584858[199] = 0.0; + out_10314592692584858[200] = 0.0; + out_10314592692584858[201] = 0.0; + out_10314592692584858[202] = 0.0; + out_10314592692584858[203] = 0.0; + out_10314592692584858[204] = 0.0; + out_10314592692584858[205] = 0.0; + out_10314592692584858[206] = 0.0; + out_10314592692584858[207] = 0.0; + out_10314592692584858[208] = 0.0; + out_10314592692584858[209] = 1.0; + out_10314592692584858[210] = 0.0; + out_10314592692584858[211] = 0.0; + out_10314592692584858[212] = 0.0; + out_10314592692584858[213] = 0.0; + out_10314592692584858[214] = 0.0; + out_10314592692584858[215] = 0.0; + out_10314592692584858[216] = 0.0; + out_10314592692584858[217] = 0.0; + out_10314592692584858[218] = 0.0; + out_10314592692584858[219] = 0.0; + out_10314592692584858[220] = 0.0; + out_10314592692584858[221] = 0.0; + out_10314592692584858[222] = 0.0; + out_10314592692584858[223] = 0.0; + out_10314592692584858[224] = 0.0; + out_10314592692584858[225] = 0.0; + out_10314592692584858[226] = 0.0; + out_10314592692584858[227] = 0.0; + out_10314592692584858[228] = 1.0; + out_10314592692584858[229] = 0.0; + out_10314592692584858[230] = 0.0; + out_10314592692584858[231] = 0.0; + out_10314592692584858[232] = 0.0; + out_10314592692584858[233] = 0.0; + out_10314592692584858[234] = 0.0; + out_10314592692584858[235] = 0.0; + out_10314592692584858[236] = 0.0; + out_10314592692584858[237] = 0.0; + out_10314592692584858[238] = 0.0; + out_10314592692584858[239] = 0.0; + out_10314592692584858[240] = 0.0; + out_10314592692584858[241] = 0.0; + out_10314592692584858[242] = 0.0; + out_10314592692584858[243] = 0.0; + out_10314592692584858[244] = 0.0; + out_10314592692584858[245] = 0.0; + out_10314592692584858[246] = 0.0; + out_10314592692584858[247] = 1.0; + out_10314592692584858[248] = 0.0; + out_10314592692584858[249] = 0.0; + out_10314592692584858[250] = 0.0; + out_10314592692584858[251] = 0.0; + out_10314592692584858[252] = 0.0; + out_10314592692584858[253] = 0.0; + out_10314592692584858[254] = 0.0; + out_10314592692584858[255] = 0.0; + out_10314592692584858[256] = 0.0; + out_10314592692584858[257] = 0.0; + out_10314592692584858[258] = 0.0; + out_10314592692584858[259] = 0.0; + out_10314592692584858[260] = 0.0; + out_10314592692584858[261] = 0.0; + out_10314592692584858[262] = 0.0; + out_10314592692584858[263] = 0.0; + out_10314592692584858[264] = 0.0; + out_10314592692584858[265] = 0.0; + out_10314592692584858[266] = 1.0; + out_10314592692584858[267] = 0.0; + out_10314592692584858[268] = 0.0; + out_10314592692584858[269] = 0.0; + out_10314592692584858[270] = 0.0; + out_10314592692584858[271] = 0.0; + out_10314592692584858[272] = 0.0; + out_10314592692584858[273] = 0.0; + out_10314592692584858[274] = 0.0; + out_10314592692584858[275] = 0.0; + out_10314592692584858[276] = 0.0; + out_10314592692584858[277] = 0.0; + out_10314592692584858[278] = 0.0; + out_10314592692584858[279] = 0.0; + out_10314592692584858[280] = 0.0; + out_10314592692584858[281] = 0.0; + out_10314592692584858[282] = 0.0; + out_10314592692584858[283] = 0.0; + out_10314592692584858[284] = 0.0; + out_10314592692584858[285] = 1.0; + out_10314592692584858[286] = 0.0; + out_10314592692584858[287] = 0.0; + out_10314592692584858[288] = 0.0; + out_10314592692584858[289] = 0.0; + out_10314592692584858[290] = 0.0; + out_10314592692584858[291] = 0.0; + out_10314592692584858[292] = 0.0; + out_10314592692584858[293] = 0.0; + out_10314592692584858[294] = 0.0; + out_10314592692584858[295] = 0.0; + out_10314592692584858[296] = 0.0; + out_10314592692584858[297] = 0.0; + out_10314592692584858[298] = 0.0; + out_10314592692584858[299] = 0.0; + out_10314592692584858[300] = 0.0; + out_10314592692584858[301] = 0.0; + out_10314592692584858[302] = 0.0; + out_10314592692584858[303] = 0.0; + out_10314592692584858[304] = 1.0; + out_10314592692584858[305] = 0.0; + out_10314592692584858[306] = 0.0; + out_10314592692584858[307] = 0.0; + out_10314592692584858[308] = 0.0; + out_10314592692584858[309] = 0.0; + out_10314592692584858[310] = 0.0; + out_10314592692584858[311] = 0.0; + out_10314592692584858[312] = 0.0; + out_10314592692584858[313] = 0.0; + out_10314592692584858[314] = 0.0; + out_10314592692584858[315] = 0.0; + out_10314592692584858[316] = 0.0; + out_10314592692584858[317] = 0.0; + out_10314592692584858[318] = 0.0; + out_10314592692584858[319] = 0.0; + out_10314592692584858[320] = 0.0; + out_10314592692584858[321] = 0.0; + out_10314592692584858[322] = 0.0; + out_10314592692584858[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4390690219844123460) { - out_4390690219844123460[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_4390690219844123460[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_4390690219844123460[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_4390690219844123460[3] = dt*state[12] + state[3]; - out_4390690219844123460[4] = dt*state[13] + state[4]; - out_4390690219844123460[5] = dt*state[14] + state[5]; - out_4390690219844123460[6] = state[6]; - out_4390690219844123460[7] = state[7]; - out_4390690219844123460[8] = state[8]; - out_4390690219844123460[9] = state[9]; - out_4390690219844123460[10] = state[10]; - out_4390690219844123460[11] = state[11]; - out_4390690219844123460[12] = state[12]; - out_4390690219844123460[13] = state[13]; - out_4390690219844123460[14] = state[14]; - out_4390690219844123460[15] = state[15]; - out_4390690219844123460[16] = state[16]; - out_4390690219844123460[17] = state[17]; +void f_fun(double *state, double dt, double *out_3469176563342886468) { + out_3469176563342886468[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_3469176563342886468[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_3469176563342886468[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_3469176563342886468[3] = dt*state[12] + state[3]; + out_3469176563342886468[4] = dt*state[13] + state[4]; + out_3469176563342886468[5] = dt*state[14] + state[5]; + out_3469176563342886468[6] = state[6]; + out_3469176563342886468[7] = state[7]; + out_3469176563342886468[8] = state[8]; + out_3469176563342886468[9] = state[9]; + out_3469176563342886468[10] = state[10]; + out_3469176563342886468[11] = state[11]; + out_3469176563342886468[12] = state[12]; + out_3469176563342886468[13] = state[13]; + out_3469176563342886468[14] = state[14]; + out_3469176563342886468[15] = state[15]; + out_3469176563342886468[16] = state[16]; + out_3469176563342886468[17] = state[17]; } -void F_fun(double *state, double dt, double *out_5251920544283288288) { - out_5251920544283288288[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_5251920544283288288[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_5251920544283288288[2] = 0; - out_5251920544283288288[3] = 0; - out_5251920544283288288[4] = 0; - out_5251920544283288288[5] = 0; - out_5251920544283288288[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_5251920544283288288[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_5251920544283288288[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_5251920544283288288[9] = 0; - out_5251920544283288288[10] = 0; - out_5251920544283288288[11] = 0; - out_5251920544283288288[12] = 0; - out_5251920544283288288[13] = 0; - out_5251920544283288288[14] = 0; - out_5251920544283288288[15] = 0; - out_5251920544283288288[16] = 0; - out_5251920544283288288[17] = 0; - out_5251920544283288288[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_5251920544283288288[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_5251920544283288288[20] = 0; - out_5251920544283288288[21] = 0; - out_5251920544283288288[22] = 0; - out_5251920544283288288[23] = 0; - out_5251920544283288288[24] = 0; - out_5251920544283288288[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_5251920544283288288[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_5251920544283288288[27] = 0; - out_5251920544283288288[28] = 0; - out_5251920544283288288[29] = 0; - out_5251920544283288288[30] = 0; - out_5251920544283288288[31] = 0; - out_5251920544283288288[32] = 0; - out_5251920544283288288[33] = 0; - out_5251920544283288288[34] = 0; - out_5251920544283288288[35] = 0; - out_5251920544283288288[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_5251920544283288288[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_5251920544283288288[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_5251920544283288288[39] = 0; - out_5251920544283288288[40] = 0; - out_5251920544283288288[41] = 0; - out_5251920544283288288[42] = 0; - out_5251920544283288288[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_5251920544283288288[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_5251920544283288288[45] = 0; - out_5251920544283288288[46] = 0; - out_5251920544283288288[47] = 0; - out_5251920544283288288[48] = 0; - out_5251920544283288288[49] = 0; - out_5251920544283288288[50] = 0; - out_5251920544283288288[51] = 0; - out_5251920544283288288[52] = 0; - out_5251920544283288288[53] = 0; - out_5251920544283288288[54] = 0; - out_5251920544283288288[55] = 0; - out_5251920544283288288[56] = 0; - out_5251920544283288288[57] = 1; - out_5251920544283288288[58] = 0; - out_5251920544283288288[59] = 0; - out_5251920544283288288[60] = 0; - out_5251920544283288288[61] = 0; - out_5251920544283288288[62] = 0; - out_5251920544283288288[63] = 0; - out_5251920544283288288[64] = 0; - out_5251920544283288288[65] = 0; - out_5251920544283288288[66] = dt; - out_5251920544283288288[67] = 0; - out_5251920544283288288[68] = 0; - out_5251920544283288288[69] = 0; - out_5251920544283288288[70] = 0; - out_5251920544283288288[71] = 0; - out_5251920544283288288[72] = 0; - out_5251920544283288288[73] = 0; - out_5251920544283288288[74] = 0; - out_5251920544283288288[75] = 0; - out_5251920544283288288[76] = 1; - out_5251920544283288288[77] = 0; - out_5251920544283288288[78] = 0; - out_5251920544283288288[79] = 0; - out_5251920544283288288[80] = 0; - out_5251920544283288288[81] = 0; - out_5251920544283288288[82] = 0; - out_5251920544283288288[83] = 0; - out_5251920544283288288[84] = 0; - out_5251920544283288288[85] = dt; - out_5251920544283288288[86] = 0; - out_5251920544283288288[87] = 0; - out_5251920544283288288[88] = 0; - out_5251920544283288288[89] = 0; - out_5251920544283288288[90] = 0; - out_5251920544283288288[91] = 0; - out_5251920544283288288[92] = 0; - out_5251920544283288288[93] = 0; - out_5251920544283288288[94] = 0; - out_5251920544283288288[95] = 1; - out_5251920544283288288[96] = 0; - out_5251920544283288288[97] = 0; - out_5251920544283288288[98] = 0; - out_5251920544283288288[99] = 0; - out_5251920544283288288[100] = 0; - out_5251920544283288288[101] = 0; - out_5251920544283288288[102] = 0; - out_5251920544283288288[103] = 0; - out_5251920544283288288[104] = dt; - out_5251920544283288288[105] = 0; - out_5251920544283288288[106] = 0; - out_5251920544283288288[107] = 0; - out_5251920544283288288[108] = 0; - out_5251920544283288288[109] = 0; - out_5251920544283288288[110] = 0; - out_5251920544283288288[111] = 0; - out_5251920544283288288[112] = 0; - out_5251920544283288288[113] = 0; - out_5251920544283288288[114] = 1; - out_5251920544283288288[115] = 0; - out_5251920544283288288[116] = 0; - out_5251920544283288288[117] = 0; - out_5251920544283288288[118] = 0; - out_5251920544283288288[119] = 0; - out_5251920544283288288[120] = 0; - out_5251920544283288288[121] = 0; - out_5251920544283288288[122] = 0; - out_5251920544283288288[123] = 0; - out_5251920544283288288[124] = 0; - out_5251920544283288288[125] = 0; - out_5251920544283288288[126] = 0; - out_5251920544283288288[127] = 0; - out_5251920544283288288[128] = 0; - out_5251920544283288288[129] = 0; - out_5251920544283288288[130] = 0; - out_5251920544283288288[131] = 0; - out_5251920544283288288[132] = 0; - out_5251920544283288288[133] = 1; - out_5251920544283288288[134] = 0; - out_5251920544283288288[135] = 0; - out_5251920544283288288[136] = 0; - out_5251920544283288288[137] = 0; - out_5251920544283288288[138] = 0; - out_5251920544283288288[139] = 0; - out_5251920544283288288[140] = 0; - out_5251920544283288288[141] = 0; - out_5251920544283288288[142] = 0; - out_5251920544283288288[143] = 0; - out_5251920544283288288[144] = 0; - out_5251920544283288288[145] = 0; - out_5251920544283288288[146] = 0; - out_5251920544283288288[147] = 0; - out_5251920544283288288[148] = 0; - out_5251920544283288288[149] = 0; - out_5251920544283288288[150] = 0; - out_5251920544283288288[151] = 0; - out_5251920544283288288[152] = 1; - out_5251920544283288288[153] = 0; - out_5251920544283288288[154] = 0; - out_5251920544283288288[155] = 0; - out_5251920544283288288[156] = 0; - out_5251920544283288288[157] = 0; - out_5251920544283288288[158] = 0; - out_5251920544283288288[159] = 0; - out_5251920544283288288[160] = 0; - out_5251920544283288288[161] = 0; - out_5251920544283288288[162] = 0; - out_5251920544283288288[163] = 0; - out_5251920544283288288[164] = 0; - out_5251920544283288288[165] = 0; - out_5251920544283288288[166] = 0; - out_5251920544283288288[167] = 0; - out_5251920544283288288[168] = 0; - out_5251920544283288288[169] = 0; - out_5251920544283288288[170] = 0; - out_5251920544283288288[171] = 1; - out_5251920544283288288[172] = 0; - out_5251920544283288288[173] = 0; - out_5251920544283288288[174] = 0; - out_5251920544283288288[175] = 0; - out_5251920544283288288[176] = 0; - out_5251920544283288288[177] = 0; - out_5251920544283288288[178] = 0; - out_5251920544283288288[179] = 0; - out_5251920544283288288[180] = 0; - out_5251920544283288288[181] = 0; - out_5251920544283288288[182] = 0; - out_5251920544283288288[183] = 0; - out_5251920544283288288[184] = 0; - out_5251920544283288288[185] = 0; - out_5251920544283288288[186] = 0; - out_5251920544283288288[187] = 0; - out_5251920544283288288[188] = 0; - out_5251920544283288288[189] = 0; - out_5251920544283288288[190] = 1; - out_5251920544283288288[191] = 0; - out_5251920544283288288[192] = 0; - out_5251920544283288288[193] = 0; - out_5251920544283288288[194] = 0; - out_5251920544283288288[195] = 0; - out_5251920544283288288[196] = 0; - out_5251920544283288288[197] = 0; - out_5251920544283288288[198] = 0; - out_5251920544283288288[199] = 0; - out_5251920544283288288[200] = 0; - out_5251920544283288288[201] = 0; - out_5251920544283288288[202] = 0; - out_5251920544283288288[203] = 0; - out_5251920544283288288[204] = 0; - out_5251920544283288288[205] = 0; - out_5251920544283288288[206] = 0; - out_5251920544283288288[207] = 0; - out_5251920544283288288[208] = 0; - out_5251920544283288288[209] = 1; - out_5251920544283288288[210] = 0; - out_5251920544283288288[211] = 0; - out_5251920544283288288[212] = 0; - out_5251920544283288288[213] = 0; - out_5251920544283288288[214] = 0; - out_5251920544283288288[215] = 0; - out_5251920544283288288[216] = 0; - out_5251920544283288288[217] = 0; - out_5251920544283288288[218] = 0; - out_5251920544283288288[219] = 0; - out_5251920544283288288[220] = 0; - out_5251920544283288288[221] = 0; - out_5251920544283288288[222] = 0; - out_5251920544283288288[223] = 0; - out_5251920544283288288[224] = 0; - out_5251920544283288288[225] = 0; - out_5251920544283288288[226] = 0; - out_5251920544283288288[227] = 0; - out_5251920544283288288[228] = 1; - out_5251920544283288288[229] = 0; - out_5251920544283288288[230] = 0; - out_5251920544283288288[231] = 0; - out_5251920544283288288[232] = 0; - out_5251920544283288288[233] = 0; - out_5251920544283288288[234] = 0; - out_5251920544283288288[235] = 0; - out_5251920544283288288[236] = 0; - out_5251920544283288288[237] = 0; - out_5251920544283288288[238] = 0; - out_5251920544283288288[239] = 0; - out_5251920544283288288[240] = 0; - out_5251920544283288288[241] = 0; - out_5251920544283288288[242] = 0; - out_5251920544283288288[243] = 0; - out_5251920544283288288[244] = 0; - out_5251920544283288288[245] = 0; - out_5251920544283288288[246] = 0; - out_5251920544283288288[247] = 1; - out_5251920544283288288[248] = 0; - out_5251920544283288288[249] = 0; - out_5251920544283288288[250] = 0; - out_5251920544283288288[251] = 0; - out_5251920544283288288[252] = 0; - out_5251920544283288288[253] = 0; - out_5251920544283288288[254] = 0; - out_5251920544283288288[255] = 0; - out_5251920544283288288[256] = 0; - out_5251920544283288288[257] = 0; - out_5251920544283288288[258] = 0; - out_5251920544283288288[259] = 0; - out_5251920544283288288[260] = 0; - out_5251920544283288288[261] = 0; - out_5251920544283288288[262] = 0; - out_5251920544283288288[263] = 0; - out_5251920544283288288[264] = 0; - out_5251920544283288288[265] = 0; - out_5251920544283288288[266] = 1; - out_5251920544283288288[267] = 0; - out_5251920544283288288[268] = 0; - out_5251920544283288288[269] = 0; - out_5251920544283288288[270] = 0; - out_5251920544283288288[271] = 0; - out_5251920544283288288[272] = 0; - out_5251920544283288288[273] = 0; - out_5251920544283288288[274] = 0; - out_5251920544283288288[275] = 0; - out_5251920544283288288[276] = 0; - out_5251920544283288288[277] = 0; - out_5251920544283288288[278] = 0; - out_5251920544283288288[279] = 0; - out_5251920544283288288[280] = 0; - out_5251920544283288288[281] = 0; - out_5251920544283288288[282] = 0; - out_5251920544283288288[283] = 0; - out_5251920544283288288[284] = 0; - out_5251920544283288288[285] = 1; - out_5251920544283288288[286] = 0; - out_5251920544283288288[287] = 0; - out_5251920544283288288[288] = 0; - out_5251920544283288288[289] = 0; - out_5251920544283288288[290] = 0; - out_5251920544283288288[291] = 0; - out_5251920544283288288[292] = 0; - out_5251920544283288288[293] = 0; - out_5251920544283288288[294] = 0; - out_5251920544283288288[295] = 0; - out_5251920544283288288[296] = 0; - out_5251920544283288288[297] = 0; - out_5251920544283288288[298] = 0; - out_5251920544283288288[299] = 0; - out_5251920544283288288[300] = 0; - out_5251920544283288288[301] = 0; - out_5251920544283288288[302] = 0; - out_5251920544283288288[303] = 0; - out_5251920544283288288[304] = 1; - out_5251920544283288288[305] = 0; - out_5251920544283288288[306] = 0; - out_5251920544283288288[307] = 0; - out_5251920544283288288[308] = 0; - out_5251920544283288288[309] = 0; - out_5251920544283288288[310] = 0; - out_5251920544283288288[311] = 0; - out_5251920544283288288[312] = 0; - out_5251920544283288288[313] = 0; - out_5251920544283288288[314] = 0; - out_5251920544283288288[315] = 0; - out_5251920544283288288[316] = 0; - out_5251920544283288288[317] = 0; - out_5251920544283288288[318] = 0; - out_5251920544283288288[319] = 0; - out_5251920544283288288[320] = 0; - out_5251920544283288288[321] = 0; - out_5251920544283288288[322] = 0; - out_5251920544283288288[323] = 1; +void F_fun(double *state, double dt, double *out_6053143964145098388) { + out_6053143964145098388[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_6053143964145098388[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_6053143964145098388[2] = 0; + out_6053143964145098388[3] = 0; + out_6053143964145098388[4] = 0; + out_6053143964145098388[5] = 0; + out_6053143964145098388[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_6053143964145098388[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_6053143964145098388[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_6053143964145098388[9] = 0; + out_6053143964145098388[10] = 0; + out_6053143964145098388[11] = 0; + out_6053143964145098388[12] = 0; + out_6053143964145098388[13] = 0; + out_6053143964145098388[14] = 0; + out_6053143964145098388[15] = 0; + out_6053143964145098388[16] = 0; + out_6053143964145098388[17] = 0; + out_6053143964145098388[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_6053143964145098388[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_6053143964145098388[20] = 0; + out_6053143964145098388[21] = 0; + out_6053143964145098388[22] = 0; + out_6053143964145098388[23] = 0; + out_6053143964145098388[24] = 0; + out_6053143964145098388[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_6053143964145098388[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_6053143964145098388[27] = 0; + out_6053143964145098388[28] = 0; + out_6053143964145098388[29] = 0; + out_6053143964145098388[30] = 0; + out_6053143964145098388[31] = 0; + out_6053143964145098388[32] = 0; + out_6053143964145098388[33] = 0; + out_6053143964145098388[34] = 0; + out_6053143964145098388[35] = 0; + out_6053143964145098388[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_6053143964145098388[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_6053143964145098388[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_6053143964145098388[39] = 0; + out_6053143964145098388[40] = 0; + out_6053143964145098388[41] = 0; + out_6053143964145098388[42] = 0; + out_6053143964145098388[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_6053143964145098388[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_6053143964145098388[45] = 0; + out_6053143964145098388[46] = 0; + out_6053143964145098388[47] = 0; + out_6053143964145098388[48] = 0; + out_6053143964145098388[49] = 0; + out_6053143964145098388[50] = 0; + out_6053143964145098388[51] = 0; + out_6053143964145098388[52] = 0; + out_6053143964145098388[53] = 0; + out_6053143964145098388[54] = 0; + out_6053143964145098388[55] = 0; + out_6053143964145098388[56] = 0; + out_6053143964145098388[57] = 1; + out_6053143964145098388[58] = 0; + out_6053143964145098388[59] = 0; + out_6053143964145098388[60] = 0; + out_6053143964145098388[61] = 0; + out_6053143964145098388[62] = 0; + out_6053143964145098388[63] = 0; + out_6053143964145098388[64] = 0; + out_6053143964145098388[65] = 0; + out_6053143964145098388[66] = dt; + out_6053143964145098388[67] = 0; + out_6053143964145098388[68] = 0; + out_6053143964145098388[69] = 0; + out_6053143964145098388[70] = 0; + out_6053143964145098388[71] = 0; + out_6053143964145098388[72] = 0; + out_6053143964145098388[73] = 0; + out_6053143964145098388[74] = 0; + out_6053143964145098388[75] = 0; + out_6053143964145098388[76] = 1; + out_6053143964145098388[77] = 0; + out_6053143964145098388[78] = 0; + out_6053143964145098388[79] = 0; + out_6053143964145098388[80] = 0; + out_6053143964145098388[81] = 0; + out_6053143964145098388[82] = 0; + out_6053143964145098388[83] = 0; + out_6053143964145098388[84] = 0; + out_6053143964145098388[85] = dt; + out_6053143964145098388[86] = 0; + out_6053143964145098388[87] = 0; + out_6053143964145098388[88] = 0; + out_6053143964145098388[89] = 0; + out_6053143964145098388[90] = 0; + out_6053143964145098388[91] = 0; + out_6053143964145098388[92] = 0; + out_6053143964145098388[93] = 0; + out_6053143964145098388[94] = 0; + out_6053143964145098388[95] = 1; + out_6053143964145098388[96] = 0; + out_6053143964145098388[97] = 0; + out_6053143964145098388[98] = 0; + out_6053143964145098388[99] = 0; + out_6053143964145098388[100] = 0; + out_6053143964145098388[101] = 0; + out_6053143964145098388[102] = 0; + out_6053143964145098388[103] = 0; + out_6053143964145098388[104] = dt; + out_6053143964145098388[105] = 0; + out_6053143964145098388[106] = 0; + out_6053143964145098388[107] = 0; + out_6053143964145098388[108] = 0; + out_6053143964145098388[109] = 0; + out_6053143964145098388[110] = 0; + out_6053143964145098388[111] = 0; + out_6053143964145098388[112] = 0; + out_6053143964145098388[113] = 0; + out_6053143964145098388[114] = 1; + out_6053143964145098388[115] = 0; + out_6053143964145098388[116] = 0; + out_6053143964145098388[117] = 0; + out_6053143964145098388[118] = 0; + out_6053143964145098388[119] = 0; + out_6053143964145098388[120] = 0; + out_6053143964145098388[121] = 0; + out_6053143964145098388[122] = 0; + out_6053143964145098388[123] = 0; + out_6053143964145098388[124] = 0; + out_6053143964145098388[125] = 0; + out_6053143964145098388[126] = 0; + out_6053143964145098388[127] = 0; + out_6053143964145098388[128] = 0; + out_6053143964145098388[129] = 0; + out_6053143964145098388[130] = 0; + out_6053143964145098388[131] = 0; + out_6053143964145098388[132] = 0; + out_6053143964145098388[133] = 1; + out_6053143964145098388[134] = 0; + out_6053143964145098388[135] = 0; + out_6053143964145098388[136] = 0; + out_6053143964145098388[137] = 0; + out_6053143964145098388[138] = 0; + out_6053143964145098388[139] = 0; + out_6053143964145098388[140] = 0; + out_6053143964145098388[141] = 0; + out_6053143964145098388[142] = 0; + out_6053143964145098388[143] = 0; + out_6053143964145098388[144] = 0; + out_6053143964145098388[145] = 0; + out_6053143964145098388[146] = 0; + out_6053143964145098388[147] = 0; + out_6053143964145098388[148] = 0; + out_6053143964145098388[149] = 0; + out_6053143964145098388[150] = 0; + out_6053143964145098388[151] = 0; + out_6053143964145098388[152] = 1; + out_6053143964145098388[153] = 0; + out_6053143964145098388[154] = 0; + out_6053143964145098388[155] = 0; + out_6053143964145098388[156] = 0; + out_6053143964145098388[157] = 0; + out_6053143964145098388[158] = 0; + out_6053143964145098388[159] = 0; + out_6053143964145098388[160] = 0; + out_6053143964145098388[161] = 0; + out_6053143964145098388[162] = 0; + out_6053143964145098388[163] = 0; + out_6053143964145098388[164] = 0; + out_6053143964145098388[165] = 0; + out_6053143964145098388[166] = 0; + out_6053143964145098388[167] = 0; + out_6053143964145098388[168] = 0; + out_6053143964145098388[169] = 0; + out_6053143964145098388[170] = 0; + out_6053143964145098388[171] = 1; + out_6053143964145098388[172] = 0; + out_6053143964145098388[173] = 0; + out_6053143964145098388[174] = 0; + out_6053143964145098388[175] = 0; + out_6053143964145098388[176] = 0; + out_6053143964145098388[177] = 0; + out_6053143964145098388[178] = 0; + out_6053143964145098388[179] = 0; + out_6053143964145098388[180] = 0; + out_6053143964145098388[181] = 0; + out_6053143964145098388[182] = 0; + out_6053143964145098388[183] = 0; + out_6053143964145098388[184] = 0; + out_6053143964145098388[185] = 0; + out_6053143964145098388[186] = 0; + out_6053143964145098388[187] = 0; + out_6053143964145098388[188] = 0; + out_6053143964145098388[189] = 0; + out_6053143964145098388[190] = 1; + out_6053143964145098388[191] = 0; + out_6053143964145098388[192] = 0; + out_6053143964145098388[193] = 0; + out_6053143964145098388[194] = 0; + out_6053143964145098388[195] = 0; + out_6053143964145098388[196] = 0; + out_6053143964145098388[197] = 0; + out_6053143964145098388[198] = 0; + out_6053143964145098388[199] = 0; + out_6053143964145098388[200] = 0; + out_6053143964145098388[201] = 0; + out_6053143964145098388[202] = 0; + out_6053143964145098388[203] = 0; + out_6053143964145098388[204] = 0; + out_6053143964145098388[205] = 0; + out_6053143964145098388[206] = 0; + out_6053143964145098388[207] = 0; + out_6053143964145098388[208] = 0; + out_6053143964145098388[209] = 1; + out_6053143964145098388[210] = 0; + out_6053143964145098388[211] = 0; + out_6053143964145098388[212] = 0; + out_6053143964145098388[213] = 0; + out_6053143964145098388[214] = 0; + out_6053143964145098388[215] = 0; + out_6053143964145098388[216] = 0; + out_6053143964145098388[217] = 0; + out_6053143964145098388[218] = 0; + out_6053143964145098388[219] = 0; + out_6053143964145098388[220] = 0; + out_6053143964145098388[221] = 0; + out_6053143964145098388[222] = 0; + out_6053143964145098388[223] = 0; + out_6053143964145098388[224] = 0; + out_6053143964145098388[225] = 0; + out_6053143964145098388[226] = 0; + out_6053143964145098388[227] = 0; + out_6053143964145098388[228] = 1; + out_6053143964145098388[229] = 0; + out_6053143964145098388[230] = 0; + out_6053143964145098388[231] = 0; + out_6053143964145098388[232] = 0; + out_6053143964145098388[233] = 0; + out_6053143964145098388[234] = 0; + out_6053143964145098388[235] = 0; + out_6053143964145098388[236] = 0; + out_6053143964145098388[237] = 0; + out_6053143964145098388[238] = 0; + out_6053143964145098388[239] = 0; + out_6053143964145098388[240] = 0; + out_6053143964145098388[241] = 0; + out_6053143964145098388[242] = 0; + out_6053143964145098388[243] = 0; + out_6053143964145098388[244] = 0; + out_6053143964145098388[245] = 0; + out_6053143964145098388[246] = 0; + out_6053143964145098388[247] = 1; + out_6053143964145098388[248] = 0; + out_6053143964145098388[249] = 0; + out_6053143964145098388[250] = 0; + out_6053143964145098388[251] = 0; + out_6053143964145098388[252] = 0; + out_6053143964145098388[253] = 0; + out_6053143964145098388[254] = 0; + out_6053143964145098388[255] = 0; + out_6053143964145098388[256] = 0; + out_6053143964145098388[257] = 0; + out_6053143964145098388[258] = 0; + out_6053143964145098388[259] = 0; + out_6053143964145098388[260] = 0; + out_6053143964145098388[261] = 0; + out_6053143964145098388[262] = 0; + out_6053143964145098388[263] = 0; + out_6053143964145098388[264] = 0; + out_6053143964145098388[265] = 0; + out_6053143964145098388[266] = 1; + out_6053143964145098388[267] = 0; + out_6053143964145098388[268] = 0; + out_6053143964145098388[269] = 0; + out_6053143964145098388[270] = 0; + out_6053143964145098388[271] = 0; + out_6053143964145098388[272] = 0; + out_6053143964145098388[273] = 0; + out_6053143964145098388[274] = 0; + out_6053143964145098388[275] = 0; + out_6053143964145098388[276] = 0; + out_6053143964145098388[277] = 0; + out_6053143964145098388[278] = 0; + out_6053143964145098388[279] = 0; + out_6053143964145098388[280] = 0; + out_6053143964145098388[281] = 0; + out_6053143964145098388[282] = 0; + out_6053143964145098388[283] = 0; + out_6053143964145098388[284] = 0; + out_6053143964145098388[285] = 1; + out_6053143964145098388[286] = 0; + out_6053143964145098388[287] = 0; + out_6053143964145098388[288] = 0; + out_6053143964145098388[289] = 0; + out_6053143964145098388[290] = 0; + out_6053143964145098388[291] = 0; + out_6053143964145098388[292] = 0; + out_6053143964145098388[293] = 0; + out_6053143964145098388[294] = 0; + out_6053143964145098388[295] = 0; + out_6053143964145098388[296] = 0; + out_6053143964145098388[297] = 0; + out_6053143964145098388[298] = 0; + out_6053143964145098388[299] = 0; + out_6053143964145098388[300] = 0; + out_6053143964145098388[301] = 0; + out_6053143964145098388[302] = 0; + out_6053143964145098388[303] = 0; + out_6053143964145098388[304] = 1; + out_6053143964145098388[305] = 0; + out_6053143964145098388[306] = 0; + out_6053143964145098388[307] = 0; + out_6053143964145098388[308] = 0; + out_6053143964145098388[309] = 0; + out_6053143964145098388[310] = 0; + out_6053143964145098388[311] = 0; + out_6053143964145098388[312] = 0; + out_6053143964145098388[313] = 0; + out_6053143964145098388[314] = 0; + out_6053143964145098388[315] = 0; + out_6053143964145098388[316] = 0; + out_6053143964145098388[317] = 0; + out_6053143964145098388[318] = 0; + out_6053143964145098388[319] = 0; + out_6053143964145098388[320] = 0; + out_6053143964145098388[321] = 0; + out_6053143964145098388[322] = 0; + out_6053143964145098388[323] = 1; } -void h_4(double *state, double *unused, double *out_2086181486544094617) { - out_2086181486544094617[0] = state[6] + state[9]; - out_2086181486544094617[1] = state[7] + state[10]; - out_2086181486544094617[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_4331217932147478278) { + out_4331217932147478278[0] = state[6] + state[9]; + out_4331217932147478278[1] = state[7] + state[10]; + out_4331217932147478278[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_141550502454447457) { - out_141550502454447457[0] = 0; - out_141550502454447457[1] = 0; - out_141550502454447457[2] = 0; - out_141550502454447457[3] = 0; - out_141550502454447457[4] = 0; - out_141550502454447457[5] = 0; - out_141550502454447457[6] = 1; - out_141550502454447457[7] = 0; - out_141550502454447457[8] = 0; - out_141550502454447457[9] = 1; - out_141550502454447457[10] = 0; - out_141550502454447457[11] = 0; - out_141550502454447457[12] = 0; - out_141550502454447457[13] = 0; - out_141550502454447457[14] = 0; - out_141550502454447457[15] = 0; - out_141550502454447457[16] = 0; - out_141550502454447457[17] = 0; - out_141550502454447457[18] = 0; - out_141550502454447457[19] = 0; - out_141550502454447457[20] = 0; - out_141550502454447457[21] = 0; - out_141550502454447457[22] = 0; - out_141550502454447457[23] = 0; - out_141550502454447457[24] = 0; - out_141550502454447457[25] = 1; - out_141550502454447457[26] = 0; - out_141550502454447457[27] = 0; - out_141550502454447457[28] = 1; - out_141550502454447457[29] = 0; - out_141550502454447457[30] = 0; - out_141550502454447457[31] = 0; - out_141550502454447457[32] = 0; - out_141550502454447457[33] = 0; - out_141550502454447457[34] = 0; - out_141550502454447457[35] = 0; - out_141550502454447457[36] = 0; - out_141550502454447457[37] = 0; - out_141550502454447457[38] = 0; - out_141550502454447457[39] = 0; - out_141550502454447457[40] = 0; - out_141550502454447457[41] = 0; - out_141550502454447457[42] = 0; - out_141550502454447457[43] = 0; - out_141550502454447457[44] = 1; - out_141550502454447457[45] = 0; - out_141550502454447457[46] = 0; - out_141550502454447457[47] = 1; - out_141550502454447457[48] = 0; - out_141550502454447457[49] = 0; - out_141550502454447457[50] = 0; - out_141550502454447457[51] = 0; - out_141550502454447457[52] = 0; - out_141550502454447457[53] = 0; +void H_4(double *state, double *unused, double *out_3761492617151322714) { + out_3761492617151322714[0] = 0; + out_3761492617151322714[1] = 0; + out_3761492617151322714[2] = 0; + out_3761492617151322714[3] = 0; + out_3761492617151322714[4] = 0; + out_3761492617151322714[5] = 0; + out_3761492617151322714[6] = 1; + out_3761492617151322714[7] = 0; + out_3761492617151322714[8] = 0; + out_3761492617151322714[9] = 1; + out_3761492617151322714[10] = 0; + out_3761492617151322714[11] = 0; + out_3761492617151322714[12] = 0; + out_3761492617151322714[13] = 0; + out_3761492617151322714[14] = 0; + out_3761492617151322714[15] = 0; + out_3761492617151322714[16] = 0; + out_3761492617151322714[17] = 0; + out_3761492617151322714[18] = 0; + out_3761492617151322714[19] = 0; + out_3761492617151322714[20] = 0; + out_3761492617151322714[21] = 0; + out_3761492617151322714[22] = 0; + out_3761492617151322714[23] = 0; + out_3761492617151322714[24] = 0; + out_3761492617151322714[25] = 1; + out_3761492617151322714[26] = 0; + out_3761492617151322714[27] = 0; + out_3761492617151322714[28] = 1; + out_3761492617151322714[29] = 0; + out_3761492617151322714[30] = 0; + out_3761492617151322714[31] = 0; + out_3761492617151322714[32] = 0; + out_3761492617151322714[33] = 0; + out_3761492617151322714[34] = 0; + out_3761492617151322714[35] = 0; + out_3761492617151322714[36] = 0; + out_3761492617151322714[37] = 0; + out_3761492617151322714[38] = 0; + out_3761492617151322714[39] = 0; + out_3761492617151322714[40] = 0; + out_3761492617151322714[41] = 0; + out_3761492617151322714[42] = 0; + out_3761492617151322714[43] = 0; + out_3761492617151322714[44] = 1; + out_3761492617151322714[45] = 0; + out_3761492617151322714[46] = 0; + out_3761492617151322714[47] = 1; + out_3761492617151322714[48] = 0; + out_3761492617151322714[49] = 0; + out_3761492617151322714[50] = 0; + out_3761492617151322714[51] = 0; + out_3761492617151322714[52] = 0; + out_3761492617151322714[53] = 0; } -void h_10(double *state, double *unused, double *out_2304974017593414723) { - out_2304974017593414723[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_2304974017593414723[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_2304974017593414723[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_1904703987337421912) { + out_1904703987337421912[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_1904703987337421912[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_1904703987337421912[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_521311265061217175) { - out_521311265061217175[0] = 0; - out_521311265061217175[1] = 9.8100000000000005*cos(state[1]); - out_521311265061217175[2] = 0; - out_521311265061217175[3] = 0; - out_521311265061217175[4] = -state[8]; - out_521311265061217175[5] = state[7]; - out_521311265061217175[6] = 0; - out_521311265061217175[7] = state[5]; - out_521311265061217175[8] = -state[4]; - out_521311265061217175[9] = 0; - out_521311265061217175[10] = 0; - out_521311265061217175[11] = 0; - out_521311265061217175[12] = 1; - out_521311265061217175[13] = 0; - out_521311265061217175[14] = 0; - out_521311265061217175[15] = 1; - out_521311265061217175[16] = 0; - out_521311265061217175[17] = 0; - out_521311265061217175[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_521311265061217175[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_521311265061217175[20] = 0; - out_521311265061217175[21] = state[8]; - out_521311265061217175[22] = 0; - out_521311265061217175[23] = -state[6]; - out_521311265061217175[24] = -state[5]; - out_521311265061217175[25] = 0; - out_521311265061217175[26] = state[3]; - out_521311265061217175[27] = 0; - out_521311265061217175[28] = 0; - out_521311265061217175[29] = 0; - out_521311265061217175[30] = 0; - out_521311265061217175[31] = 1; - out_521311265061217175[32] = 0; - out_521311265061217175[33] = 0; - out_521311265061217175[34] = 1; - out_521311265061217175[35] = 0; - out_521311265061217175[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_521311265061217175[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_521311265061217175[38] = 0; - out_521311265061217175[39] = -state[7]; - out_521311265061217175[40] = state[6]; - out_521311265061217175[41] = 0; - out_521311265061217175[42] = state[4]; - out_521311265061217175[43] = -state[3]; - out_521311265061217175[44] = 0; - out_521311265061217175[45] = 0; - out_521311265061217175[46] = 0; - out_521311265061217175[47] = 0; - out_521311265061217175[48] = 0; - out_521311265061217175[49] = 0; - out_521311265061217175[50] = 1; - out_521311265061217175[51] = 0; - out_521311265061217175[52] = 0; - out_521311265061217175[53] = 1; +void H_10(double *state, double *unused, double *out_4366147958481579120) { + out_4366147958481579120[0] = 0; + out_4366147958481579120[1] = 9.8100000000000005*cos(state[1]); + out_4366147958481579120[2] = 0; + out_4366147958481579120[3] = 0; + out_4366147958481579120[4] = -state[8]; + out_4366147958481579120[5] = state[7]; + out_4366147958481579120[6] = 0; + out_4366147958481579120[7] = state[5]; + out_4366147958481579120[8] = -state[4]; + out_4366147958481579120[9] = 0; + out_4366147958481579120[10] = 0; + out_4366147958481579120[11] = 0; + out_4366147958481579120[12] = 1; + out_4366147958481579120[13] = 0; + out_4366147958481579120[14] = 0; + out_4366147958481579120[15] = 1; + out_4366147958481579120[16] = 0; + out_4366147958481579120[17] = 0; + out_4366147958481579120[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_4366147958481579120[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_4366147958481579120[20] = 0; + out_4366147958481579120[21] = state[8]; + out_4366147958481579120[22] = 0; + out_4366147958481579120[23] = -state[6]; + out_4366147958481579120[24] = -state[5]; + out_4366147958481579120[25] = 0; + out_4366147958481579120[26] = state[3]; + out_4366147958481579120[27] = 0; + out_4366147958481579120[28] = 0; + out_4366147958481579120[29] = 0; + out_4366147958481579120[30] = 0; + out_4366147958481579120[31] = 1; + out_4366147958481579120[32] = 0; + out_4366147958481579120[33] = 0; + out_4366147958481579120[34] = 1; + out_4366147958481579120[35] = 0; + out_4366147958481579120[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_4366147958481579120[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_4366147958481579120[38] = 0; + out_4366147958481579120[39] = -state[7]; + out_4366147958481579120[40] = state[6]; + out_4366147958481579120[41] = 0; + out_4366147958481579120[42] = state[4]; + out_4366147958481579120[43] = -state[3]; + out_4366147958481579120[44] = 0; + out_4366147958481579120[45] = 0; + out_4366147958481579120[46] = 0; + out_4366147958481579120[47] = 0; + out_4366147958481579120[48] = 0; + out_4366147958481579120[49] = 0; + out_4366147958481579120[50] = 1; + out_4366147958481579120[51] = 0; + out_4366147958481579120[52] = 0; + out_4366147958481579120[53] = 1; } -void h_13(double *state, double *unused, double *out_6563098082746675966) { - out_6563098082746675966[0] = state[3]; - out_6563098082746675966[1] = state[4]; - out_6563098082746675966[2] = state[5]; +void h_13(double *state, double *unused, double *out_9149454562438237975) { + out_9149454562438237975[0] = state[3]; + out_9149454562438237975[1] = state[4]; + out_9149454562438237975[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3070723322877885344) { - out_3070723322877885344[0] = 0; - out_3070723322877885344[1] = 0; - out_3070723322877885344[2] = 0; - out_3070723322877885344[3] = 1; - out_3070723322877885344[4] = 0; - out_3070723322877885344[5] = 0; - out_3070723322877885344[6] = 0; - out_3070723322877885344[7] = 0; - out_3070723322877885344[8] = 0; - out_3070723322877885344[9] = 0; - out_3070723322877885344[10] = 0; - out_3070723322877885344[11] = 0; - out_3070723322877885344[12] = 0; - out_3070723322877885344[13] = 0; - out_3070723322877885344[14] = 0; - out_3070723322877885344[15] = 0; - out_3070723322877885344[16] = 0; - out_3070723322877885344[17] = 0; - out_3070723322877885344[18] = 0; - out_3070723322877885344[19] = 0; - out_3070723322877885344[20] = 0; - out_3070723322877885344[21] = 0; - out_3070723322877885344[22] = 1; - out_3070723322877885344[23] = 0; - out_3070723322877885344[24] = 0; - out_3070723322877885344[25] = 0; - out_3070723322877885344[26] = 0; - out_3070723322877885344[27] = 0; - out_3070723322877885344[28] = 0; - out_3070723322877885344[29] = 0; - out_3070723322877885344[30] = 0; - out_3070723322877885344[31] = 0; - out_3070723322877885344[32] = 0; - out_3070723322877885344[33] = 0; - out_3070723322877885344[34] = 0; - out_3070723322877885344[35] = 0; - out_3070723322877885344[36] = 0; - out_3070723322877885344[37] = 0; - out_3070723322877885344[38] = 0; - out_3070723322877885344[39] = 0; - out_3070723322877885344[40] = 0; - out_3070723322877885344[41] = 1; - out_3070723322877885344[42] = 0; - out_3070723322877885344[43] = 0; - out_3070723322877885344[44] = 0; - out_3070723322877885344[45] = 0; - out_3070723322877885344[46] = 0; - out_3070723322877885344[47] = 0; - out_3070723322877885344[48] = 0; - out_3070723322877885344[49] = 0; - out_3070723322877885344[50] = 0; - out_3070723322877885344[51] = 0; - out_3070723322877885344[52] = 0; - out_3070723322877885344[53] = 0; +void H_13(double *state, double *unused, double *out_6973766442483655515) { + out_6973766442483655515[0] = 0; + out_6973766442483655515[1] = 0; + out_6973766442483655515[2] = 0; + out_6973766442483655515[3] = 1; + out_6973766442483655515[4] = 0; + out_6973766442483655515[5] = 0; + out_6973766442483655515[6] = 0; + out_6973766442483655515[7] = 0; + out_6973766442483655515[8] = 0; + out_6973766442483655515[9] = 0; + out_6973766442483655515[10] = 0; + out_6973766442483655515[11] = 0; + out_6973766442483655515[12] = 0; + out_6973766442483655515[13] = 0; + out_6973766442483655515[14] = 0; + out_6973766442483655515[15] = 0; + out_6973766442483655515[16] = 0; + out_6973766442483655515[17] = 0; + out_6973766442483655515[18] = 0; + out_6973766442483655515[19] = 0; + out_6973766442483655515[20] = 0; + out_6973766442483655515[21] = 0; + out_6973766442483655515[22] = 1; + out_6973766442483655515[23] = 0; + out_6973766442483655515[24] = 0; + out_6973766442483655515[25] = 0; + out_6973766442483655515[26] = 0; + out_6973766442483655515[27] = 0; + out_6973766442483655515[28] = 0; + out_6973766442483655515[29] = 0; + out_6973766442483655515[30] = 0; + out_6973766442483655515[31] = 0; + out_6973766442483655515[32] = 0; + out_6973766442483655515[33] = 0; + out_6973766442483655515[34] = 0; + out_6973766442483655515[35] = 0; + out_6973766442483655515[36] = 0; + out_6973766442483655515[37] = 0; + out_6973766442483655515[38] = 0; + out_6973766442483655515[39] = 0; + out_6973766442483655515[40] = 0; + out_6973766442483655515[41] = 1; + out_6973766442483655515[42] = 0; + out_6973766442483655515[43] = 0; + out_6973766442483655515[44] = 0; + out_6973766442483655515[45] = 0; + out_6973766442483655515[46] = 0; + out_6973766442483655515[47] = 0; + out_6973766442483655515[48] = 0; + out_6973766442483655515[49] = 0; + out_6973766442483655515[50] = 0; + out_6973766442483655515[51] = 0; + out_6973766442483655515[52] = 0; + out_6973766442483655515[53] = 0; } -void h_14(double *state, double *unused, double *out_4832039927491863335) { - out_4832039927491863335[0] = state[6]; - out_4832039927491863335[1] = state[7]; - out_4832039927491863335[2] = state[8]; +void h_14(double *state, double *unused, double *out_1095581425659281568) { + out_1095581425659281568[0] = state[6]; + out_1095581425659281568[1] = state[7]; + out_1095581425659281568[2] = state[8]; } -void H_14(double *state, double *unused, double *out_3821690353885037072) { - out_3821690353885037072[0] = 0; - out_3821690353885037072[1] = 0; - out_3821690353885037072[2] = 0; - out_3821690353885037072[3] = 0; - out_3821690353885037072[4] = 0; - out_3821690353885037072[5] = 0; - out_3821690353885037072[6] = 1; - out_3821690353885037072[7] = 0; - out_3821690353885037072[8] = 0; - out_3821690353885037072[9] = 0; - out_3821690353885037072[10] = 0; - out_3821690353885037072[11] = 0; - out_3821690353885037072[12] = 0; - out_3821690353885037072[13] = 0; - out_3821690353885037072[14] = 0; - out_3821690353885037072[15] = 0; - out_3821690353885037072[16] = 0; - out_3821690353885037072[17] = 0; - out_3821690353885037072[18] = 0; - out_3821690353885037072[19] = 0; - out_3821690353885037072[20] = 0; - out_3821690353885037072[21] = 0; - out_3821690353885037072[22] = 0; - out_3821690353885037072[23] = 0; - out_3821690353885037072[24] = 0; - out_3821690353885037072[25] = 1; - out_3821690353885037072[26] = 0; - out_3821690353885037072[27] = 0; - out_3821690353885037072[28] = 0; - out_3821690353885037072[29] = 0; - out_3821690353885037072[30] = 0; - out_3821690353885037072[31] = 0; - out_3821690353885037072[32] = 0; - out_3821690353885037072[33] = 0; - out_3821690353885037072[34] = 0; - out_3821690353885037072[35] = 0; - out_3821690353885037072[36] = 0; - out_3821690353885037072[37] = 0; - out_3821690353885037072[38] = 0; - out_3821690353885037072[39] = 0; - out_3821690353885037072[40] = 0; - out_3821690353885037072[41] = 0; - out_3821690353885037072[42] = 0; - out_3821690353885037072[43] = 0; - out_3821690353885037072[44] = 1; - out_3821690353885037072[45] = 0; - out_3821690353885037072[46] = 0; - out_3821690353885037072[47] = 0; - out_3821690353885037072[48] = 0; - out_3821690353885037072[49] = 0; - out_3821690353885037072[50] = 0; - out_3821690353885037072[51] = 0; - out_3821690353885037072[52] = 0; - out_3821690353885037072[53] = 0; +void H_14(double *state, double *unused, double *out_678704184855950418) { + out_678704184855950418[0] = 0; + out_678704184855950418[1] = 0; + out_678704184855950418[2] = 0; + out_678704184855950418[3] = 0; + out_678704184855950418[4] = 0; + out_678704184855950418[5] = 0; + out_678704184855950418[6] = 1; + out_678704184855950418[7] = 0; + out_678704184855950418[8] = 0; + out_678704184855950418[9] = 0; + out_678704184855950418[10] = 0; + out_678704184855950418[11] = 0; + out_678704184855950418[12] = 0; + out_678704184855950418[13] = 0; + out_678704184855950418[14] = 0; + out_678704184855950418[15] = 0; + out_678704184855950418[16] = 0; + out_678704184855950418[17] = 0; + out_678704184855950418[18] = 0; + out_678704184855950418[19] = 0; + out_678704184855950418[20] = 0; + out_678704184855950418[21] = 0; + out_678704184855950418[22] = 0; + out_678704184855950418[23] = 0; + out_678704184855950418[24] = 0; + out_678704184855950418[25] = 1; + out_678704184855950418[26] = 0; + out_678704184855950418[27] = 0; + out_678704184855950418[28] = 0; + out_678704184855950418[29] = 0; + out_678704184855950418[30] = 0; + out_678704184855950418[31] = 0; + out_678704184855950418[32] = 0; + out_678704184855950418[33] = 0; + out_678704184855950418[34] = 0; + out_678704184855950418[35] = 0; + out_678704184855950418[36] = 0; + out_678704184855950418[37] = 0; + out_678704184855950418[38] = 0; + out_678704184855950418[39] = 0; + out_678704184855950418[40] = 0; + out_678704184855950418[41] = 0; + out_678704184855950418[42] = 0; + out_678704184855950418[43] = 0; + out_678704184855950418[44] = 1; + out_678704184855950418[45] = 0; + out_678704184855950418[46] = 0; + out_678704184855950418[47] = 0; + out_678704184855950418[48] = 0; + out_678704184855950418[49] = 0; + out_678704184855950418[50] = 0; + out_678704184855950418[51] = 0; + out_678704184855950418[52] = 0; + out_678704184855950418[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_3667758909157266385) { - err_fun(nom_x, delta_x, out_3667758909157266385); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8958383251573640) { + err_fun(nom_x, delta_x, out_8958383251573640); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3255008687637156165) { - inv_err_fun(nom_x, true_x, out_3255008687637156165); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3510167321874192836) { + inv_err_fun(nom_x, true_x, out_3510167321874192836); } -void pose_H_mod_fun(double *state, double *out_1678148223451849404) { - H_mod_fun(state, out_1678148223451849404); +void pose_H_mod_fun(double *state, double *out_10314592692584858) { + H_mod_fun(state, out_10314592692584858); } -void pose_f_fun(double *state, double dt, double *out_4390690219844123460) { - f_fun(state, dt, out_4390690219844123460); +void pose_f_fun(double *state, double dt, double *out_3469176563342886468) { + f_fun(state, dt, out_3469176563342886468); } -void pose_F_fun(double *state, double dt, double *out_5251920544283288288) { - F_fun(state, dt, out_5251920544283288288); +void pose_F_fun(double *state, double dt, double *out_6053143964145098388) { + F_fun(state, dt, out_6053143964145098388); } -void pose_h_4(double *state, double *unused, double *out_2086181486544094617) { - h_4(state, unused, out_2086181486544094617); +void pose_h_4(double *state, double *unused, double *out_4331217932147478278) { + h_4(state, unused, out_4331217932147478278); } -void pose_H_4(double *state, double *unused, double *out_141550502454447457) { - H_4(state, unused, out_141550502454447457); +void pose_H_4(double *state, double *unused, double *out_3761492617151322714) { + H_4(state, unused, out_3761492617151322714); } -void pose_h_10(double *state, double *unused, double *out_2304974017593414723) { - h_10(state, unused, out_2304974017593414723); +void pose_h_10(double *state, double *unused, double *out_1904703987337421912) { + h_10(state, unused, out_1904703987337421912); } -void pose_H_10(double *state, double *unused, double *out_521311265061217175) { - H_10(state, unused, out_521311265061217175); +void pose_H_10(double *state, double *unused, double *out_4366147958481579120) { + H_10(state, unused, out_4366147958481579120); } -void pose_h_13(double *state, double *unused, double *out_6563098082746675966) { - h_13(state, unused, out_6563098082746675966); +void pose_h_13(double *state, double *unused, double *out_9149454562438237975) { + h_13(state, unused, out_9149454562438237975); } -void pose_H_13(double *state, double *unused, double *out_3070723322877885344) { - H_13(state, unused, out_3070723322877885344); +void pose_H_13(double *state, double *unused, double *out_6973766442483655515) { + H_13(state, unused, out_6973766442483655515); } -void pose_h_14(double *state, double *unused, double *out_4832039927491863335) { - h_14(state, unused, out_4832039927491863335); +void pose_h_14(double *state, double *unused, double *out_1095581425659281568) { + h_14(state, unused, out_1095581425659281568); } -void pose_H_14(double *state, double *unused, double *out_3821690353885037072) { - H_14(state, unused, out_3821690353885037072); +void pose_H_14(double *state, double *unused, double *out_678704184855950418) { + H_14(state, unused, out_678704184855950418); } 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 d6c8c6159..520bd59f8 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_3667758909157266385); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3255008687637156165); -void pose_H_mod_fun(double *state, double *out_1678148223451849404); -void pose_f_fun(double *state, double dt, double *out_4390690219844123460); -void pose_F_fun(double *state, double dt, double *out_5251920544283288288); -void pose_h_4(double *state, double *unused, double *out_2086181486544094617); -void pose_H_4(double *state, double *unused, double *out_141550502454447457); -void pose_h_10(double *state, double *unused, double *out_2304974017593414723); -void pose_H_10(double *state, double *unused, double *out_521311265061217175); -void pose_h_13(double *state, double *unused, double *out_6563098082746675966); -void pose_H_13(double *state, double *unused, double *out_3070723322877885344); -void pose_h_14(double *state, double *unused, double *out_4832039927491863335); -void pose_H_14(double *state, double *unused, double *out_3821690353885037072); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8958383251573640); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3510167321874192836); +void pose_H_mod_fun(double *state, double *out_10314592692584858); +void pose_f_fun(double *state, double dt, double *out_3469176563342886468); +void pose_F_fun(double *state, double dt, double *out_6053143964145098388); +void pose_h_4(double *state, double *unused, double *out_4331217932147478278); +void pose_H_4(double *state, double *unused, double *out_3761492617151322714); +void pose_h_10(double *state, double *unused, double *out_1904703987337421912); +void pose_H_10(double *state, double *unused, double *out_4366147958481579120); +void pose_h_13(double *state, double *unused, double *out_9149454562438237975); +void pose_H_13(double *state, double *unused, double *out_6973766442483655515); +void pose_h_14(double *state, double *unused, double *out_1095581425659281568); +void pose_H_14(double *state, double *unused, double *out_678704184855950418); 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 a730076d4..3bc0b5ed1 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 268fca898..1d63584c1 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ