diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 036205a06f..6f4ed01754 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 15c5091aef..9be223cffc 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 5808f762ba..b98e5e0b78 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 f2cf4ab27c..63852ad131 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 1a10bb68d3..62296bf04d 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 d77a157c3a..9e646c5761 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 53104d0217..db60fa4d18 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 53104d0217..db60fa4d18 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 009c71817e..921790a1c1 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 d77a157c3a..9e646c5761 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 c09bc11908..556bf8976e 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-76dbf941-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index e3b5536d98..7ffa630946 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 d668370d20..c8796a3e16 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 b2cc0fbc94..4e12bb8322 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 1283f33ae3..aa8d538775 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 6150ef29f1..2cd445efa4 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 6298404039..5b553e7599 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 f58872d3c8..d9d44b250d 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 bace8bfbb0..80544392e9 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 2367d61239..bf059e9196 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 01f4f007eb..ec642fdc3b 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 19750b8f02..d09be3bf3d 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 15f72ffc95..ee75a90a78 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 6e9b656b06..4628a1bed3 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 750e10dc29..2566299c2a 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 c4b2dd7dd3..1612bfe22c 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 1c4bc90a36..6b4200b377 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 24d6adbf8b..8c5b03b98e 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 136488b83b..d365c7fac8 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 aeafd115b6..60ff8859d4 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 1d4c8a6bfb..74793b91f9 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 be1a6cd740..991b1c2418 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-da88d20b-DEBUG \ No newline at end of file +DEV-76dbf941-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index e16b8af410..ab0dc36789 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_8544853402428642963) { + out_8544853402428642963[0] = delta_x[0] + nom_x[0]; + out_8544853402428642963[1] = delta_x[1] + nom_x[1]; + out_8544853402428642963[2] = delta_x[2] + nom_x[2]; + out_8544853402428642963[3] = delta_x[3] + nom_x[3]; + out_8544853402428642963[4] = delta_x[4] + nom_x[4]; + out_8544853402428642963[5] = delta_x[5] + nom_x[5]; + out_8544853402428642963[6] = delta_x[6] + nom_x[6]; + out_8544853402428642963[7] = delta_x[7] + nom_x[7]; + out_8544853402428642963[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_4966147196229178357) { + out_4966147196229178357[0] = -nom_x[0] + true_x[0]; + out_4966147196229178357[1] = -nom_x[1] + true_x[1]; + out_4966147196229178357[2] = -nom_x[2] + true_x[2]; + out_4966147196229178357[3] = -nom_x[3] + true_x[3]; + out_4966147196229178357[4] = -nom_x[4] + true_x[4]; + out_4966147196229178357[5] = -nom_x[5] + true_x[5]; + out_4966147196229178357[6] = -nom_x[6] + true_x[6]; + out_4966147196229178357[7] = -nom_x[7] + true_x[7]; + out_4966147196229178357[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_5259284650049385741) { + out_5259284650049385741[0] = 1.0; + out_5259284650049385741[1] = 0.0; + out_5259284650049385741[2] = 0.0; + out_5259284650049385741[3] = 0.0; + out_5259284650049385741[4] = 0.0; + out_5259284650049385741[5] = 0.0; + out_5259284650049385741[6] = 0.0; + out_5259284650049385741[7] = 0.0; + out_5259284650049385741[8] = 0.0; + out_5259284650049385741[9] = 0.0; + out_5259284650049385741[10] = 1.0; + out_5259284650049385741[11] = 0.0; + out_5259284650049385741[12] = 0.0; + out_5259284650049385741[13] = 0.0; + out_5259284650049385741[14] = 0.0; + out_5259284650049385741[15] = 0.0; + out_5259284650049385741[16] = 0.0; + out_5259284650049385741[17] = 0.0; + out_5259284650049385741[18] = 0.0; + out_5259284650049385741[19] = 0.0; + out_5259284650049385741[20] = 1.0; + out_5259284650049385741[21] = 0.0; + out_5259284650049385741[22] = 0.0; + out_5259284650049385741[23] = 0.0; + out_5259284650049385741[24] = 0.0; + out_5259284650049385741[25] = 0.0; + out_5259284650049385741[26] = 0.0; + out_5259284650049385741[27] = 0.0; + out_5259284650049385741[28] = 0.0; + out_5259284650049385741[29] = 0.0; + out_5259284650049385741[30] = 1.0; + out_5259284650049385741[31] = 0.0; + out_5259284650049385741[32] = 0.0; + out_5259284650049385741[33] = 0.0; + out_5259284650049385741[34] = 0.0; + out_5259284650049385741[35] = 0.0; + out_5259284650049385741[36] = 0.0; + out_5259284650049385741[37] = 0.0; + out_5259284650049385741[38] = 0.0; + out_5259284650049385741[39] = 0.0; + out_5259284650049385741[40] = 1.0; + out_5259284650049385741[41] = 0.0; + out_5259284650049385741[42] = 0.0; + out_5259284650049385741[43] = 0.0; + out_5259284650049385741[44] = 0.0; + out_5259284650049385741[45] = 0.0; + out_5259284650049385741[46] = 0.0; + out_5259284650049385741[47] = 0.0; + out_5259284650049385741[48] = 0.0; + out_5259284650049385741[49] = 0.0; + out_5259284650049385741[50] = 1.0; + out_5259284650049385741[51] = 0.0; + out_5259284650049385741[52] = 0.0; + out_5259284650049385741[53] = 0.0; + out_5259284650049385741[54] = 0.0; + out_5259284650049385741[55] = 0.0; + out_5259284650049385741[56] = 0.0; + out_5259284650049385741[57] = 0.0; + out_5259284650049385741[58] = 0.0; + out_5259284650049385741[59] = 0.0; + out_5259284650049385741[60] = 1.0; + out_5259284650049385741[61] = 0.0; + out_5259284650049385741[62] = 0.0; + out_5259284650049385741[63] = 0.0; + out_5259284650049385741[64] = 0.0; + out_5259284650049385741[65] = 0.0; + out_5259284650049385741[66] = 0.0; + out_5259284650049385741[67] = 0.0; + out_5259284650049385741[68] = 0.0; + out_5259284650049385741[69] = 0.0; + out_5259284650049385741[70] = 1.0; + out_5259284650049385741[71] = 0.0; + out_5259284650049385741[72] = 0.0; + out_5259284650049385741[73] = 0.0; + out_5259284650049385741[74] = 0.0; + out_5259284650049385741[75] = 0.0; + out_5259284650049385741[76] = 0.0; + out_5259284650049385741[77] = 0.0; + out_5259284650049385741[78] = 0.0; + out_5259284650049385741[79] = 0.0; + out_5259284650049385741[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_5571654789601065205) { + out_5571654789601065205[0] = state[0]; + out_5571654789601065205[1] = state[1]; + out_5571654789601065205[2] = state[2]; + out_5571654789601065205[3] = state[3]; + out_5571654789601065205[4] = state[4]; + out_5571654789601065205[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_5571654789601065205[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_5571654789601065205[7] = state[7]; + out_5571654789601065205[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_7466465670683569774) { + out_7466465670683569774[0] = 1; + out_7466465670683569774[1] = 0; + out_7466465670683569774[2] = 0; + out_7466465670683569774[3] = 0; + out_7466465670683569774[4] = 0; + out_7466465670683569774[5] = 0; + out_7466465670683569774[6] = 0; + out_7466465670683569774[7] = 0; + out_7466465670683569774[8] = 0; + out_7466465670683569774[9] = 0; + out_7466465670683569774[10] = 1; + out_7466465670683569774[11] = 0; + out_7466465670683569774[12] = 0; + out_7466465670683569774[13] = 0; + out_7466465670683569774[14] = 0; + out_7466465670683569774[15] = 0; + out_7466465670683569774[16] = 0; + out_7466465670683569774[17] = 0; + out_7466465670683569774[18] = 0; + out_7466465670683569774[19] = 0; + out_7466465670683569774[20] = 1; + out_7466465670683569774[21] = 0; + out_7466465670683569774[22] = 0; + out_7466465670683569774[23] = 0; + out_7466465670683569774[24] = 0; + out_7466465670683569774[25] = 0; + out_7466465670683569774[26] = 0; + out_7466465670683569774[27] = 0; + out_7466465670683569774[28] = 0; + out_7466465670683569774[29] = 0; + out_7466465670683569774[30] = 1; + out_7466465670683569774[31] = 0; + out_7466465670683569774[32] = 0; + out_7466465670683569774[33] = 0; + out_7466465670683569774[34] = 0; + out_7466465670683569774[35] = 0; + out_7466465670683569774[36] = 0; + out_7466465670683569774[37] = 0; + out_7466465670683569774[38] = 0; + out_7466465670683569774[39] = 0; + out_7466465670683569774[40] = 1; + out_7466465670683569774[41] = 0; + out_7466465670683569774[42] = 0; + out_7466465670683569774[43] = 0; + out_7466465670683569774[44] = 0; + out_7466465670683569774[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_7466465670683569774[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_7466465670683569774[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7466465670683569774[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7466465670683569774[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_7466465670683569774[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_7466465670683569774[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_7466465670683569774[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_7466465670683569774[53] = -9.8100000000000005*dt; + out_7466465670683569774[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_7466465670683569774[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_7466465670683569774[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7466465670683569774[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7466465670683569774[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_7466465670683569774[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_7466465670683569774[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_7466465670683569774[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7466465670683569774[62] = 0; + out_7466465670683569774[63] = 0; + out_7466465670683569774[64] = 0; + out_7466465670683569774[65] = 0; + out_7466465670683569774[66] = 0; + out_7466465670683569774[67] = 0; + out_7466465670683569774[68] = 0; + out_7466465670683569774[69] = 0; + out_7466465670683569774[70] = 1; + out_7466465670683569774[71] = 0; + out_7466465670683569774[72] = 0; + out_7466465670683569774[73] = 0; + out_7466465670683569774[74] = 0; + out_7466465670683569774[75] = 0; + out_7466465670683569774[76] = 0; + out_7466465670683569774[77] = 0; + out_7466465670683569774[78] = 0; + out_7466465670683569774[79] = 0; + out_7466465670683569774[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_7657999268865908143) { + out_7657999268865908143[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_8283767598475501035) { + out_8283767598475501035[0] = 0; + out_8283767598475501035[1] = 0; + out_8283767598475501035[2] = 0; + out_8283767598475501035[3] = 0; + out_8283767598475501035[4] = 0; + out_8283767598475501035[5] = 0; + out_8283767598475501035[6] = 1; + out_8283767598475501035[7] = 0; + out_8283767598475501035[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_8959567811302299328) { + out_8959567811302299328[0] = state[4]; + out_8959567811302299328[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_1708195791883982934) { + out_1708195791883982934[0] = 0; + out_1708195791883982934[1] = 0; + out_1708195791883982934[2] = 0; + out_1708195791883982934[3] = 0; + out_1708195791883982934[4] = 1; + out_1708195791883982934[5] = 0; + out_1708195791883982934[6] = 0; + out_1708195791883982934[7] = 0; + out_1708195791883982934[8] = 0; + out_1708195791883982934[9] = 0; + out_1708195791883982934[10] = 0; + out_1708195791883982934[11] = 0; + out_1708195791883982934[12] = 0; + out_1708195791883982934[13] = 0; + out_1708195791883982934[14] = 1; + out_1708195791883982934[15] = 0; + out_1708195791883982934[16] = 0; + out_1708195791883982934[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_4661606344031996983) { + out_4661606344031996983[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_5765434639968252408) { + out_5765434639968252408[0] = 0; + out_5765434639968252408[1] = 0; + out_5765434639968252408[2] = 0; + out_5765434639968252408[3] = 0; + out_5765434639968252408[4] = 1; + out_5765434639968252408[5] = 0; + out_5765434639968252408[6] = 0; + out_5765434639968252408[7] = 0; + out_5765434639968252408[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_668943426824494179) { + out_668943426824494179[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_6421473156359994357) { + out_6421473156359994357[0] = 0; + out_6421473156359994357[1] = 0; + out_6421473156359994357[2] = 0; + out_6421473156359994357[3] = 0; + out_6421473156359994357[4] = 0; + out_6421473156359994357[5] = 0; + out_6421473156359994357[6] = 0; + out_6421473156359994357[7] = 1; + out_6421473156359994357[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_4581587017363022418) { + out_4581587017363022418[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_3541840568784309191) { + out_3541840568784309191[0] = 0; + out_3541840568784309191[1] = 0; + out_3541840568784309191[2] = 0; + out_3541840568784309191[3] = 1; + out_3541840568784309191[4] = 0; + out_3541840568784309191[5] = 0; + out_3541840568784309191[6] = 0; + out_3541840568784309191[7] = 0; + out_3541840568784309191[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_3311352477521500941) { + out_3311352477521500941[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_5255203295653860224) { + out_5255203295653860224[0] = 0; + out_5255203295653860224[1] = 1; + out_5255203295653860224[2] = 0; + out_5255203295653860224[3] = 0; + out_5255203295653860224[4] = 0; + out_5255203295653860224[5] = 0; + out_5255203295653860224[6] = 0; + out_5255203295653860224[7] = 0; + out_5255203295653860224[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_1769034914093169035) { + out_1769034914093169035[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_8109141760986160818) { + out_8109141760986160818[0] = 1; + out_8109141760986160818[1] = 0; + out_8109141760986160818[2] = 0; + out_8109141760986160818[3] = 0; + out_8109141760986160818[4] = 0; + out_8109141760986160818[5] = 0; + out_8109141760986160818[6] = 0; + out_8109141760986160818[7] = 0; + out_8109141760986160818[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_3703077070402039012) { + out_3703077070402039012[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_8253121636598540607) { + out_8253121636598540607[0] = 0; + out_8253121636598540607[1] = 0; + out_8253121636598540607[2] = 0; + out_8253121636598540607[3] = 0; + out_8253121636598540607[4] = 0; + out_8253121636598540607[5] = 0; + out_8253121636598540607[6] = 0; + out_8253121636598540607[7] = 0; + out_8253121636598540607[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_8544853402428642963) { + err_fun(nom_x, delta_x, out_8544853402428642963); } -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_4966147196229178357) { + inv_err_fun(nom_x, true_x, out_4966147196229178357); } -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_5259284650049385741) { + H_mod_fun(state, out_5259284650049385741); } -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_5571654789601065205) { + f_fun(state, dt, out_5571654789601065205); } -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_7466465670683569774) { + F_fun(state, dt, out_7466465670683569774); } -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_7657999268865908143) { + h_25(state, unused, out_7657999268865908143); } -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_8283767598475501035) { + H_25(state, unused, out_8283767598475501035); } -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_8959567811302299328) { + h_24(state, unused, out_8959567811302299328); } -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_1708195791883982934) { + H_24(state, unused, out_1708195791883982934); } -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_4661606344031996983) { + h_30(state, unused, out_4661606344031996983); } -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_5765434639968252408) { + H_30(state, unused, out_5765434639968252408); } -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_668943426824494179) { + h_26(state, unused, out_668943426824494179); } -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_6421473156359994357) { + H_26(state, unused, out_6421473156359994357); } -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_4581587017363022418) { + h_27(state, unused, out_4581587017363022418); } -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_3541840568784309191) { + H_27(state, unused, out_3541840568784309191); } -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_3311352477521500941) { + h_29(state, unused, out_3311352477521500941); } -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_5255203295653860224) { + H_29(state, unused, out_5255203295653860224); } -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_1769034914093169035) { + h_28(state, unused, out_1769034914093169035); } -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_8109141760986160818) { + H_28(state, unused, out_8109141760986160818); } -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_3703077070402039012) { + h_31(state, unused, out_3703077070402039012); } -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_8253121636598540607) { + H_31(state, unused, out_8253121636598540607); } 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 655b5e1aea..085fc72278 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_8544853402428642963); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_4966147196229178357); +void car_H_mod_fun(double *state, double *out_5259284650049385741); +void car_f_fun(double *state, double dt, double *out_5571654789601065205); +void car_F_fun(double *state, double dt, double *out_7466465670683569774); +void car_h_25(double *state, double *unused, double *out_7657999268865908143); +void car_H_25(double *state, double *unused, double *out_8283767598475501035); +void car_h_24(double *state, double *unused, double *out_8959567811302299328); +void car_H_24(double *state, double *unused, double *out_1708195791883982934); +void car_h_30(double *state, double *unused, double *out_4661606344031996983); +void car_H_30(double *state, double *unused, double *out_5765434639968252408); +void car_h_26(double *state, double *unused, double *out_668943426824494179); +void car_H_26(double *state, double *unused, double *out_6421473156359994357); +void car_h_27(double *state, double *unused, double *out_4581587017363022418); +void car_H_27(double *state, double *unused, double *out_3541840568784309191); +void car_h_29(double *state, double *unused, double *out_3311352477521500941); +void car_H_29(double *state, double *unused, double *out_5255203295653860224); +void car_h_28(double *state, double *unused, double *out_1769034914093169035); +void car_H_28(double *state, double *unused, double *out_8109141760986160818); +void car_h_31(double *state, double *unused, double *out_3703077070402039012); +void car_H_31(double *state, double *unused, double *out_8253121636598540607); 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 c62883fa7b..78b98cc4a9 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_3328634486354213070) { + out_3328634486354213070[0] = delta_x[0] + nom_x[0]; + out_3328634486354213070[1] = delta_x[1] + nom_x[1]; + out_3328634486354213070[2] = delta_x[2] + nom_x[2]; + out_3328634486354213070[3] = delta_x[3] + nom_x[3]; + out_3328634486354213070[4] = delta_x[4] + nom_x[4]; + out_3328634486354213070[5] = delta_x[5] + nom_x[5]; + out_3328634486354213070[6] = delta_x[6] + nom_x[6]; + out_3328634486354213070[7] = delta_x[7] + nom_x[7]; + out_3328634486354213070[8] = delta_x[8] + nom_x[8]; + out_3328634486354213070[9] = delta_x[9] + nom_x[9]; + out_3328634486354213070[10] = delta_x[10] + nom_x[10]; + out_3328634486354213070[11] = delta_x[11] + nom_x[11]; + out_3328634486354213070[12] = delta_x[12] + nom_x[12]; + out_3328634486354213070[13] = delta_x[13] + nom_x[13]; + out_3328634486354213070[14] = delta_x[14] + nom_x[14]; + out_3328634486354213070[15] = delta_x[15] + nom_x[15]; + out_3328634486354213070[16] = delta_x[16] + nom_x[16]; + out_3328634486354213070[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_2068367058924519839) { + out_2068367058924519839[0] = -nom_x[0] + true_x[0]; + out_2068367058924519839[1] = -nom_x[1] + true_x[1]; + out_2068367058924519839[2] = -nom_x[2] + true_x[2]; + out_2068367058924519839[3] = -nom_x[3] + true_x[3]; + out_2068367058924519839[4] = -nom_x[4] + true_x[4]; + out_2068367058924519839[5] = -nom_x[5] + true_x[5]; + out_2068367058924519839[6] = -nom_x[6] + true_x[6]; + out_2068367058924519839[7] = -nom_x[7] + true_x[7]; + out_2068367058924519839[8] = -nom_x[8] + true_x[8]; + out_2068367058924519839[9] = -nom_x[9] + true_x[9]; + out_2068367058924519839[10] = -nom_x[10] + true_x[10]; + out_2068367058924519839[11] = -nom_x[11] + true_x[11]; + out_2068367058924519839[12] = -nom_x[12] + true_x[12]; + out_2068367058924519839[13] = -nom_x[13] + true_x[13]; + out_2068367058924519839[14] = -nom_x[14] + true_x[14]; + out_2068367058924519839[15] = -nom_x[15] + true_x[15]; + out_2068367058924519839[16] = -nom_x[16] + true_x[16]; + out_2068367058924519839[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_5273281981643743211) { + out_5273281981643743211[0] = 1.0; + out_5273281981643743211[1] = 0.0; + out_5273281981643743211[2] = 0.0; + out_5273281981643743211[3] = 0.0; + out_5273281981643743211[4] = 0.0; + out_5273281981643743211[5] = 0.0; + out_5273281981643743211[6] = 0.0; + out_5273281981643743211[7] = 0.0; + out_5273281981643743211[8] = 0.0; + out_5273281981643743211[9] = 0.0; + out_5273281981643743211[10] = 0.0; + out_5273281981643743211[11] = 0.0; + out_5273281981643743211[12] = 0.0; + out_5273281981643743211[13] = 0.0; + out_5273281981643743211[14] = 0.0; + out_5273281981643743211[15] = 0.0; + out_5273281981643743211[16] = 0.0; + out_5273281981643743211[17] = 0.0; + out_5273281981643743211[18] = 0.0; + out_5273281981643743211[19] = 1.0; + out_5273281981643743211[20] = 0.0; + out_5273281981643743211[21] = 0.0; + out_5273281981643743211[22] = 0.0; + out_5273281981643743211[23] = 0.0; + out_5273281981643743211[24] = 0.0; + out_5273281981643743211[25] = 0.0; + out_5273281981643743211[26] = 0.0; + out_5273281981643743211[27] = 0.0; + out_5273281981643743211[28] = 0.0; + out_5273281981643743211[29] = 0.0; + out_5273281981643743211[30] = 0.0; + out_5273281981643743211[31] = 0.0; + out_5273281981643743211[32] = 0.0; + out_5273281981643743211[33] = 0.0; + out_5273281981643743211[34] = 0.0; + out_5273281981643743211[35] = 0.0; + out_5273281981643743211[36] = 0.0; + out_5273281981643743211[37] = 0.0; + out_5273281981643743211[38] = 1.0; + out_5273281981643743211[39] = 0.0; + out_5273281981643743211[40] = 0.0; + out_5273281981643743211[41] = 0.0; + out_5273281981643743211[42] = 0.0; + out_5273281981643743211[43] = 0.0; + out_5273281981643743211[44] = 0.0; + out_5273281981643743211[45] = 0.0; + out_5273281981643743211[46] = 0.0; + out_5273281981643743211[47] = 0.0; + out_5273281981643743211[48] = 0.0; + out_5273281981643743211[49] = 0.0; + out_5273281981643743211[50] = 0.0; + out_5273281981643743211[51] = 0.0; + out_5273281981643743211[52] = 0.0; + out_5273281981643743211[53] = 0.0; + out_5273281981643743211[54] = 0.0; + out_5273281981643743211[55] = 0.0; + out_5273281981643743211[56] = 0.0; + out_5273281981643743211[57] = 1.0; + out_5273281981643743211[58] = 0.0; + out_5273281981643743211[59] = 0.0; + out_5273281981643743211[60] = 0.0; + out_5273281981643743211[61] = 0.0; + out_5273281981643743211[62] = 0.0; + out_5273281981643743211[63] = 0.0; + out_5273281981643743211[64] = 0.0; + out_5273281981643743211[65] = 0.0; + out_5273281981643743211[66] = 0.0; + out_5273281981643743211[67] = 0.0; + out_5273281981643743211[68] = 0.0; + out_5273281981643743211[69] = 0.0; + out_5273281981643743211[70] = 0.0; + out_5273281981643743211[71] = 0.0; + out_5273281981643743211[72] = 0.0; + out_5273281981643743211[73] = 0.0; + out_5273281981643743211[74] = 0.0; + out_5273281981643743211[75] = 0.0; + out_5273281981643743211[76] = 1.0; + out_5273281981643743211[77] = 0.0; + out_5273281981643743211[78] = 0.0; + out_5273281981643743211[79] = 0.0; + out_5273281981643743211[80] = 0.0; + out_5273281981643743211[81] = 0.0; + out_5273281981643743211[82] = 0.0; + out_5273281981643743211[83] = 0.0; + out_5273281981643743211[84] = 0.0; + out_5273281981643743211[85] = 0.0; + out_5273281981643743211[86] = 0.0; + out_5273281981643743211[87] = 0.0; + out_5273281981643743211[88] = 0.0; + out_5273281981643743211[89] = 0.0; + out_5273281981643743211[90] = 0.0; + out_5273281981643743211[91] = 0.0; + out_5273281981643743211[92] = 0.0; + out_5273281981643743211[93] = 0.0; + out_5273281981643743211[94] = 0.0; + out_5273281981643743211[95] = 1.0; + out_5273281981643743211[96] = 0.0; + out_5273281981643743211[97] = 0.0; + out_5273281981643743211[98] = 0.0; + out_5273281981643743211[99] = 0.0; + out_5273281981643743211[100] = 0.0; + out_5273281981643743211[101] = 0.0; + out_5273281981643743211[102] = 0.0; + out_5273281981643743211[103] = 0.0; + out_5273281981643743211[104] = 0.0; + out_5273281981643743211[105] = 0.0; + out_5273281981643743211[106] = 0.0; + out_5273281981643743211[107] = 0.0; + out_5273281981643743211[108] = 0.0; + out_5273281981643743211[109] = 0.0; + out_5273281981643743211[110] = 0.0; + out_5273281981643743211[111] = 0.0; + out_5273281981643743211[112] = 0.0; + out_5273281981643743211[113] = 0.0; + out_5273281981643743211[114] = 1.0; + out_5273281981643743211[115] = 0.0; + out_5273281981643743211[116] = 0.0; + out_5273281981643743211[117] = 0.0; + out_5273281981643743211[118] = 0.0; + out_5273281981643743211[119] = 0.0; + out_5273281981643743211[120] = 0.0; + out_5273281981643743211[121] = 0.0; + out_5273281981643743211[122] = 0.0; + out_5273281981643743211[123] = 0.0; + out_5273281981643743211[124] = 0.0; + out_5273281981643743211[125] = 0.0; + out_5273281981643743211[126] = 0.0; + out_5273281981643743211[127] = 0.0; + out_5273281981643743211[128] = 0.0; + out_5273281981643743211[129] = 0.0; + out_5273281981643743211[130] = 0.0; + out_5273281981643743211[131] = 0.0; + out_5273281981643743211[132] = 0.0; + out_5273281981643743211[133] = 1.0; + out_5273281981643743211[134] = 0.0; + out_5273281981643743211[135] = 0.0; + out_5273281981643743211[136] = 0.0; + out_5273281981643743211[137] = 0.0; + out_5273281981643743211[138] = 0.0; + out_5273281981643743211[139] = 0.0; + out_5273281981643743211[140] = 0.0; + out_5273281981643743211[141] = 0.0; + out_5273281981643743211[142] = 0.0; + out_5273281981643743211[143] = 0.0; + out_5273281981643743211[144] = 0.0; + out_5273281981643743211[145] = 0.0; + out_5273281981643743211[146] = 0.0; + out_5273281981643743211[147] = 0.0; + out_5273281981643743211[148] = 0.0; + out_5273281981643743211[149] = 0.0; + out_5273281981643743211[150] = 0.0; + out_5273281981643743211[151] = 0.0; + out_5273281981643743211[152] = 1.0; + out_5273281981643743211[153] = 0.0; + out_5273281981643743211[154] = 0.0; + out_5273281981643743211[155] = 0.0; + out_5273281981643743211[156] = 0.0; + out_5273281981643743211[157] = 0.0; + out_5273281981643743211[158] = 0.0; + out_5273281981643743211[159] = 0.0; + out_5273281981643743211[160] = 0.0; + out_5273281981643743211[161] = 0.0; + out_5273281981643743211[162] = 0.0; + out_5273281981643743211[163] = 0.0; + out_5273281981643743211[164] = 0.0; + out_5273281981643743211[165] = 0.0; + out_5273281981643743211[166] = 0.0; + out_5273281981643743211[167] = 0.0; + out_5273281981643743211[168] = 0.0; + out_5273281981643743211[169] = 0.0; + out_5273281981643743211[170] = 0.0; + out_5273281981643743211[171] = 1.0; + out_5273281981643743211[172] = 0.0; + out_5273281981643743211[173] = 0.0; + out_5273281981643743211[174] = 0.0; + out_5273281981643743211[175] = 0.0; + out_5273281981643743211[176] = 0.0; + out_5273281981643743211[177] = 0.0; + out_5273281981643743211[178] = 0.0; + out_5273281981643743211[179] = 0.0; + out_5273281981643743211[180] = 0.0; + out_5273281981643743211[181] = 0.0; + out_5273281981643743211[182] = 0.0; + out_5273281981643743211[183] = 0.0; + out_5273281981643743211[184] = 0.0; + out_5273281981643743211[185] = 0.0; + out_5273281981643743211[186] = 0.0; + out_5273281981643743211[187] = 0.0; + out_5273281981643743211[188] = 0.0; + out_5273281981643743211[189] = 0.0; + out_5273281981643743211[190] = 1.0; + out_5273281981643743211[191] = 0.0; + out_5273281981643743211[192] = 0.0; + out_5273281981643743211[193] = 0.0; + out_5273281981643743211[194] = 0.0; + out_5273281981643743211[195] = 0.0; + out_5273281981643743211[196] = 0.0; + out_5273281981643743211[197] = 0.0; + out_5273281981643743211[198] = 0.0; + out_5273281981643743211[199] = 0.0; + out_5273281981643743211[200] = 0.0; + out_5273281981643743211[201] = 0.0; + out_5273281981643743211[202] = 0.0; + out_5273281981643743211[203] = 0.0; + out_5273281981643743211[204] = 0.0; + out_5273281981643743211[205] = 0.0; + out_5273281981643743211[206] = 0.0; + out_5273281981643743211[207] = 0.0; + out_5273281981643743211[208] = 0.0; + out_5273281981643743211[209] = 1.0; + out_5273281981643743211[210] = 0.0; + out_5273281981643743211[211] = 0.0; + out_5273281981643743211[212] = 0.0; + out_5273281981643743211[213] = 0.0; + out_5273281981643743211[214] = 0.0; + out_5273281981643743211[215] = 0.0; + out_5273281981643743211[216] = 0.0; + out_5273281981643743211[217] = 0.0; + out_5273281981643743211[218] = 0.0; + out_5273281981643743211[219] = 0.0; + out_5273281981643743211[220] = 0.0; + out_5273281981643743211[221] = 0.0; + out_5273281981643743211[222] = 0.0; + out_5273281981643743211[223] = 0.0; + out_5273281981643743211[224] = 0.0; + out_5273281981643743211[225] = 0.0; + out_5273281981643743211[226] = 0.0; + out_5273281981643743211[227] = 0.0; + out_5273281981643743211[228] = 1.0; + out_5273281981643743211[229] = 0.0; + out_5273281981643743211[230] = 0.0; + out_5273281981643743211[231] = 0.0; + out_5273281981643743211[232] = 0.0; + out_5273281981643743211[233] = 0.0; + out_5273281981643743211[234] = 0.0; + out_5273281981643743211[235] = 0.0; + out_5273281981643743211[236] = 0.0; + out_5273281981643743211[237] = 0.0; + out_5273281981643743211[238] = 0.0; + out_5273281981643743211[239] = 0.0; + out_5273281981643743211[240] = 0.0; + out_5273281981643743211[241] = 0.0; + out_5273281981643743211[242] = 0.0; + out_5273281981643743211[243] = 0.0; + out_5273281981643743211[244] = 0.0; + out_5273281981643743211[245] = 0.0; + out_5273281981643743211[246] = 0.0; + out_5273281981643743211[247] = 1.0; + out_5273281981643743211[248] = 0.0; + out_5273281981643743211[249] = 0.0; + out_5273281981643743211[250] = 0.0; + out_5273281981643743211[251] = 0.0; + out_5273281981643743211[252] = 0.0; + out_5273281981643743211[253] = 0.0; + out_5273281981643743211[254] = 0.0; + out_5273281981643743211[255] = 0.0; + out_5273281981643743211[256] = 0.0; + out_5273281981643743211[257] = 0.0; + out_5273281981643743211[258] = 0.0; + out_5273281981643743211[259] = 0.0; + out_5273281981643743211[260] = 0.0; + out_5273281981643743211[261] = 0.0; + out_5273281981643743211[262] = 0.0; + out_5273281981643743211[263] = 0.0; + out_5273281981643743211[264] = 0.0; + out_5273281981643743211[265] = 0.0; + out_5273281981643743211[266] = 1.0; + out_5273281981643743211[267] = 0.0; + out_5273281981643743211[268] = 0.0; + out_5273281981643743211[269] = 0.0; + out_5273281981643743211[270] = 0.0; + out_5273281981643743211[271] = 0.0; + out_5273281981643743211[272] = 0.0; + out_5273281981643743211[273] = 0.0; + out_5273281981643743211[274] = 0.0; + out_5273281981643743211[275] = 0.0; + out_5273281981643743211[276] = 0.0; + out_5273281981643743211[277] = 0.0; + out_5273281981643743211[278] = 0.0; + out_5273281981643743211[279] = 0.0; + out_5273281981643743211[280] = 0.0; + out_5273281981643743211[281] = 0.0; + out_5273281981643743211[282] = 0.0; + out_5273281981643743211[283] = 0.0; + out_5273281981643743211[284] = 0.0; + out_5273281981643743211[285] = 1.0; + out_5273281981643743211[286] = 0.0; + out_5273281981643743211[287] = 0.0; + out_5273281981643743211[288] = 0.0; + out_5273281981643743211[289] = 0.0; + out_5273281981643743211[290] = 0.0; + out_5273281981643743211[291] = 0.0; + out_5273281981643743211[292] = 0.0; + out_5273281981643743211[293] = 0.0; + out_5273281981643743211[294] = 0.0; + out_5273281981643743211[295] = 0.0; + out_5273281981643743211[296] = 0.0; + out_5273281981643743211[297] = 0.0; + out_5273281981643743211[298] = 0.0; + out_5273281981643743211[299] = 0.0; + out_5273281981643743211[300] = 0.0; + out_5273281981643743211[301] = 0.0; + out_5273281981643743211[302] = 0.0; + out_5273281981643743211[303] = 0.0; + out_5273281981643743211[304] = 1.0; + out_5273281981643743211[305] = 0.0; + out_5273281981643743211[306] = 0.0; + out_5273281981643743211[307] = 0.0; + out_5273281981643743211[308] = 0.0; + out_5273281981643743211[309] = 0.0; + out_5273281981643743211[310] = 0.0; + out_5273281981643743211[311] = 0.0; + out_5273281981643743211[312] = 0.0; + out_5273281981643743211[313] = 0.0; + out_5273281981643743211[314] = 0.0; + out_5273281981643743211[315] = 0.0; + out_5273281981643743211[316] = 0.0; + out_5273281981643743211[317] = 0.0; + out_5273281981643743211[318] = 0.0; + out_5273281981643743211[319] = 0.0; + out_5273281981643743211[320] = 0.0; + out_5273281981643743211[321] = 0.0; + out_5273281981643743211[322] = 0.0; + out_5273281981643743211[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_3798976948542527989) { + out_3798976948542527989[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_3798976948542527989[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_3798976948542527989[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_3798976948542527989[3] = dt*state[12] + state[3]; + out_3798976948542527989[4] = dt*state[13] + state[4]; + out_3798976948542527989[5] = dt*state[14] + state[5]; + out_3798976948542527989[6] = state[6]; + out_3798976948542527989[7] = state[7]; + out_3798976948542527989[8] = state[8]; + out_3798976948542527989[9] = state[9]; + out_3798976948542527989[10] = state[10]; + out_3798976948542527989[11] = state[11]; + out_3798976948542527989[12] = state[12]; + out_3798976948542527989[13] = state[13]; + out_3798976948542527989[14] = state[14]; + out_3798976948542527989[15] = state[15]; + out_3798976948542527989[16] = state[16]; + out_3798976948542527989[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_5376087696404799721) { + out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[2] = 0; + out_5376087696404799721[3] = 0; + out_5376087696404799721[4] = 0; + out_5376087696404799721[5] = 0; + out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[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_5376087696404799721[9] = 0; + out_5376087696404799721[10] = 0; + out_5376087696404799721[11] = 0; + out_5376087696404799721[12] = 0; + out_5376087696404799721[13] = 0; + out_5376087696404799721[14] = 0; + out_5376087696404799721[15] = 0; + out_5376087696404799721[16] = 0; + out_5376087696404799721[17] = 0; + out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[20] = 0; + out_5376087696404799721[21] = 0; + out_5376087696404799721[22] = 0; + out_5376087696404799721[23] = 0; + out_5376087696404799721[24] = 0; + out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[27] = 0; + out_5376087696404799721[28] = 0; + out_5376087696404799721[29] = 0; + out_5376087696404799721[30] = 0; + out_5376087696404799721[31] = 0; + out_5376087696404799721[32] = 0; + out_5376087696404799721[33] = 0; + out_5376087696404799721[34] = 0; + out_5376087696404799721[35] = 0; + out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[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_5376087696404799721[39] = 0; + out_5376087696404799721[40] = 0; + out_5376087696404799721[41] = 0; + out_5376087696404799721[42] = 0; + out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[45] = 0; + out_5376087696404799721[46] = 0; + out_5376087696404799721[47] = 0; + out_5376087696404799721[48] = 0; + out_5376087696404799721[49] = 0; + out_5376087696404799721[50] = 0; + out_5376087696404799721[51] = 0; + out_5376087696404799721[52] = 0; + out_5376087696404799721[53] = 0; + out_5376087696404799721[54] = 0; + out_5376087696404799721[55] = 0; + out_5376087696404799721[56] = 0; + out_5376087696404799721[57] = 1; + out_5376087696404799721[58] = 0; + out_5376087696404799721[59] = 0; + out_5376087696404799721[60] = 0; + out_5376087696404799721[61] = 0; + out_5376087696404799721[62] = 0; + out_5376087696404799721[63] = 0; + out_5376087696404799721[64] = 0; + out_5376087696404799721[65] = 0; + out_5376087696404799721[66] = dt; + out_5376087696404799721[67] = 0; + out_5376087696404799721[68] = 0; + out_5376087696404799721[69] = 0; + out_5376087696404799721[70] = 0; + out_5376087696404799721[71] = 0; + out_5376087696404799721[72] = 0; + out_5376087696404799721[73] = 0; + out_5376087696404799721[74] = 0; + out_5376087696404799721[75] = 0; + out_5376087696404799721[76] = 1; + out_5376087696404799721[77] = 0; + out_5376087696404799721[78] = 0; + out_5376087696404799721[79] = 0; + out_5376087696404799721[80] = 0; + out_5376087696404799721[81] = 0; + out_5376087696404799721[82] = 0; + out_5376087696404799721[83] = 0; + out_5376087696404799721[84] = 0; + out_5376087696404799721[85] = dt; + out_5376087696404799721[86] = 0; + out_5376087696404799721[87] = 0; + out_5376087696404799721[88] = 0; + out_5376087696404799721[89] = 0; + out_5376087696404799721[90] = 0; + out_5376087696404799721[91] = 0; + out_5376087696404799721[92] = 0; + out_5376087696404799721[93] = 0; + out_5376087696404799721[94] = 0; + out_5376087696404799721[95] = 1; + out_5376087696404799721[96] = 0; + out_5376087696404799721[97] = 0; + out_5376087696404799721[98] = 0; + out_5376087696404799721[99] = 0; + out_5376087696404799721[100] = 0; + out_5376087696404799721[101] = 0; + out_5376087696404799721[102] = 0; + out_5376087696404799721[103] = 0; + out_5376087696404799721[104] = dt; + out_5376087696404799721[105] = 0; + out_5376087696404799721[106] = 0; + out_5376087696404799721[107] = 0; + out_5376087696404799721[108] = 0; + out_5376087696404799721[109] = 0; + out_5376087696404799721[110] = 0; + out_5376087696404799721[111] = 0; + out_5376087696404799721[112] = 0; + out_5376087696404799721[113] = 0; + out_5376087696404799721[114] = 1; + out_5376087696404799721[115] = 0; + out_5376087696404799721[116] = 0; + out_5376087696404799721[117] = 0; + out_5376087696404799721[118] = 0; + out_5376087696404799721[119] = 0; + out_5376087696404799721[120] = 0; + out_5376087696404799721[121] = 0; + out_5376087696404799721[122] = 0; + out_5376087696404799721[123] = 0; + out_5376087696404799721[124] = 0; + out_5376087696404799721[125] = 0; + out_5376087696404799721[126] = 0; + out_5376087696404799721[127] = 0; + out_5376087696404799721[128] = 0; + out_5376087696404799721[129] = 0; + out_5376087696404799721[130] = 0; + out_5376087696404799721[131] = 0; + out_5376087696404799721[132] = 0; + out_5376087696404799721[133] = 1; + out_5376087696404799721[134] = 0; + out_5376087696404799721[135] = 0; + out_5376087696404799721[136] = 0; + out_5376087696404799721[137] = 0; + out_5376087696404799721[138] = 0; + out_5376087696404799721[139] = 0; + out_5376087696404799721[140] = 0; + out_5376087696404799721[141] = 0; + out_5376087696404799721[142] = 0; + out_5376087696404799721[143] = 0; + out_5376087696404799721[144] = 0; + out_5376087696404799721[145] = 0; + out_5376087696404799721[146] = 0; + out_5376087696404799721[147] = 0; + out_5376087696404799721[148] = 0; + out_5376087696404799721[149] = 0; + out_5376087696404799721[150] = 0; + out_5376087696404799721[151] = 0; + out_5376087696404799721[152] = 1; + out_5376087696404799721[153] = 0; + out_5376087696404799721[154] = 0; + out_5376087696404799721[155] = 0; + out_5376087696404799721[156] = 0; + out_5376087696404799721[157] = 0; + out_5376087696404799721[158] = 0; + out_5376087696404799721[159] = 0; + out_5376087696404799721[160] = 0; + out_5376087696404799721[161] = 0; + out_5376087696404799721[162] = 0; + out_5376087696404799721[163] = 0; + out_5376087696404799721[164] = 0; + out_5376087696404799721[165] = 0; + out_5376087696404799721[166] = 0; + out_5376087696404799721[167] = 0; + out_5376087696404799721[168] = 0; + out_5376087696404799721[169] = 0; + out_5376087696404799721[170] = 0; + out_5376087696404799721[171] = 1; + out_5376087696404799721[172] = 0; + out_5376087696404799721[173] = 0; + out_5376087696404799721[174] = 0; + out_5376087696404799721[175] = 0; + out_5376087696404799721[176] = 0; + out_5376087696404799721[177] = 0; + out_5376087696404799721[178] = 0; + out_5376087696404799721[179] = 0; + out_5376087696404799721[180] = 0; + out_5376087696404799721[181] = 0; + out_5376087696404799721[182] = 0; + out_5376087696404799721[183] = 0; + out_5376087696404799721[184] = 0; + out_5376087696404799721[185] = 0; + out_5376087696404799721[186] = 0; + out_5376087696404799721[187] = 0; + out_5376087696404799721[188] = 0; + out_5376087696404799721[189] = 0; + out_5376087696404799721[190] = 1; + out_5376087696404799721[191] = 0; + out_5376087696404799721[192] = 0; + out_5376087696404799721[193] = 0; + out_5376087696404799721[194] = 0; + out_5376087696404799721[195] = 0; + out_5376087696404799721[196] = 0; + out_5376087696404799721[197] = 0; + out_5376087696404799721[198] = 0; + out_5376087696404799721[199] = 0; + out_5376087696404799721[200] = 0; + out_5376087696404799721[201] = 0; + out_5376087696404799721[202] = 0; + out_5376087696404799721[203] = 0; + out_5376087696404799721[204] = 0; + out_5376087696404799721[205] = 0; + out_5376087696404799721[206] = 0; + out_5376087696404799721[207] = 0; + out_5376087696404799721[208] = 0; + out_5376087696404799721[209] = 1; + out_5376087696404799721[210] = 0; + out_5376087696404799721[211] = 0; + out_5376087696404799721[212] = 0; + out_5376087696404799721[213] = 0; + out_5376087696404799721[214] = 0; + out_5376087696404799721[215] = 0; + out_5376087696404799721[216] = 0; + out_5376087696404799721[217] = 0; + out_5376087696404799721[218] = 0; + out_5376087696404799721[219] = 0; + out_5376087696404799721[220] = 0; + out_5376087696404799721[221] = 0; + out_5376087696404799721[222] = 0; + out_5376087696404799721[223] = 0; + out_5376087696404799721[224] = 0; + out_5376087696404799721[225] = 0; + out_5376087696404799721[226] = 0; + out_5376087696404799721[227] = 0; + out_5376087696404799721[228] = 1; + out_5376087696404799721[229] = 0; + out_5376087696404799721[230] = 0; + out_5376087696404799721[231] = 0; + out_5376087696404799721[232] = 0; + out_5376087696404799721[233] = 0; + out_5376087696404799721[234] = 0; + out_5376087696404799721[235] = 0; + out_5376087696404799721[236] = 0; + out_5376087696404799721[237] = 0; + out_5376087696404799721[238] = 0; + out_5376087696404799721[239] = 0; + out_5376087696404799721[240] = 0; + out_5376087696404799721[241] = 0; + out_5376087696404799721[242] = 0; + out_5376087696404799721[243] = 0; + out_5376087696404799721[244] = 0; + out_5376087696404799721[245] = 0; + out_5376087696404799721[246] = 0; + out_5376087696404799721[247] = 1; + out_5376087696404799721[248] = 0; + out_5376087696404799721[249] = 0; + out_5376087696404799721[250] = 0; + out_5376087696404799721[251] = 0; + out_5376087696404799721[252] = 0; + out_5376087696404799721[253] = 0; + out_5376087696404799721[254] = 0; + out_5376087696404799721[255] = 0; + out_5376087696404799721[256] = 0; + out_5376087696404799721[257] = 0; + out_5376087696404799721[258] = 0; + out_5376087696404799721[259] = 0; + out_5376087696404799721[260] = 0; + out_5376087696404799721[261] = 0; + out_5376087696404799721[262] = 0; + out_5376087696404799721[263] = 0; + out_5376087696404799721[264] = 0; + out_5376087696404799721[265] = 0; + out_5376087696404799721[266] = 1; + out_5376087696404799721[267] = 0; + out_5376087696404799721[268] = 0; + out_5376087696404799721[269] = 0; + out_5376087696404799721[270] = 0; + out_5376087696404799721[271] = 0; + out_5376087696404799721[272] = 0; + out_5376087696404799721[273] = 0; + out_5376087696404799721[274] = 0; + out_5376087696404799721[275] = 0; + out_5376087696404799721[276] = 0; + out_5376087696404799721[277] = 0; + out_5376087696404799721[278] = 0; + out_5376087696404799721[279] = 0; + out_5376087696404799721[280] = 0; + out_5376087696404799721[281] = 0; + out_5376087696404799721[282] = 0; + out_5376087696404799721[283] = 0; + out_5376087696404799721[284] = 0; + out_5376087696404799721[285] = 1; + out_5376087696404799721[286] = 0; + out_5376087696404799721[287] = 0; + out_5376087696404799721[288] = 0; + out_5376087696404799721[289] = 0; + out_5376087696404799721[290] = 0; + out_5376087696404799721[291] = 0; + out_5376087696404799721[292] = 0; + out_5376087696404799721[293] = 0; + out_5376087696404799721[294] = 0; + out_5376087696404799721[295] = 0; + out_5376087696404799721[296] = 0; + out_5376087696404799721[297] = 0; + out_5376087696404799721[298] = 0; + out_5376087696404799721[299] = 0; + out_5376087696404799721[300] = 0; + out_5376087696404799721[301] = 0; + out_5376087696404799721[302] = 0; + out_5376087696404799721[303] = 0; + out_5376087696404799721[304] = 1; + out_5376087696404799721[305] = 0; + out_5376087696404799721[306] = 0; + out_5376087696404799721[307] = 0; + out_5376087696404799721[308] = 0; + out_5376087696404799721[309] = 0; + out_5376087696404799721[310] = 0; + out_5376087696404799721[311] = 0; + out_5376087696404799721[312] = 0; + out_5376087696404799721[313] = 0; + out_5376087696404799721[314] = 0; + out_5376087696404799721[315] = 0; + out_5376087696404799721[316] = 0; + out_5376087696404799721[317] = 0; + out_5376087696404799721[318] = 0; + out_5376087696404799721[319] = 0; + out_5376087696404799721[320] = 0; + out_5376087696404799721[321] = 0; + out_5376087696404799721[322] = 0; + out_5376087696404799721[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_6100547545538860384) { + out_6100547545538860384[0] = state[6] + state[9]; + out_6100547545538860384[1] = state[7] + state[10]; + out_6100547545538860384[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_9045089191487650783) { + out_9045089191487650783[0] = 0; + out_9045089191487650783[1] = 0; + out_9045089191487650783[2] = 0; + out_9045089191487650783[3] = 0; + out_9045089191487650783[4] = 0; + out_9045089191487650783[5] = 0; + out_9045089191487650783[6] = 1; + out_9045089191487650783[7] = 0; + out_9045089191487650783[8] = 0; + out_9045089191487650783[9] = 1; + out_9045089191487650783[10] = 0; + out_9045089191487650783[11] = 0; + out_9045089191487650783[12] = 0; + out_9045089191487650783[13] = 0; + out_9045089191487650783[14] = 0; + out_9045089191487650783[15] = 0; + out_9045089191487650783[16] = 0; + out_9045089191487650783[17] = 0; + out_9045089191487650783[18] = 0; + out_9045089191487650783[19] = 0; + out_9045089191487650783[20] = 0; + out_9045089191487650783[21] = 0; + out_9045089191487650783[22] = 0; + out_9045089191487650783[23] = 0; + out_9045089191487650783[24] = 0; + out_9045089191487650783[25] = 1; + out_9045089191487650783[26] = 0; + out_9045089191487650783[27] = 0; + out_9045089191487650783[28] = 1; + out_9045089191487650783[29] = 0; + out_9045089191487650783[30] = 0; + out_9045089191487650783[31] = 0; + out_9045089191487650783[32] = 0; + out_9045089191487650783[33] = 0; + out_9045089191487650783[34] = 0; + out_9045089191487650783[35] = 0; + out_9045089191487650783[36] = 0; + out_9045089191487650783[37] = 0; + out_9045089191487650783[38] = 0; + out_9045089191487650783[39] = 0; + out_9045089191487650783[40] = 0; + out_9045089191487650783[41] = 0; + out_9045089191487650783[42] = 0; + out_9045089191487650783[43] = 0; + out_9045089191487650783[44] = 1; + out_9045089191487650783[45] = 0; + out_9045089191487650783[46] = 0; + out_9045089191487650783[47] = 1; + out_9045089191487650783[48] = 0; + out_9045089191487650783[49] = 0; + out_9045089191487650783[50] = 0; + out_9045089191487650783[51] = 0; + out_9045089191487650783[52] = 0; + out_9045089191487650783[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_7661539883733460450) { + out_7661539883733460450[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_7661539883733460450[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_7661539883733460450[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_2752907549792722588) { + out_2752907549792722588[0] = 0; + out_2752907549792722588[1] = 9.8100000000000005*cos(state[1]); + out_2752907549792722588[2] = 0; + out_2752907549792722588[3] = 0; + out_2752907549792722588[4] = -state[8]; + out_2752907549792722588[5] = state[7]; + out_2752907549792722588[6] = 0; + out_2752907549792722588[7] = state[5]; + out_2752907549792722588[8] = -state[4]; + out_2752907549792722588[9] = 0; + out_2752907549792722588[10] = 0; + out_2752907549792722588[11] = 0; + out_2752907549792722588[12] = 1; + out_2752907549792722588[13] = 0; + out_2752907549792722588[14] = 0; + out_2752907549792722588[15] = 1; + out_2752907549792722588[16] = 0; + out_2752907549792722588[17] = 0; + out_2752907549792722588[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_2752907549792722588[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_2752907549792722588[20] = 0; + out_2752907549792722588[21] = state[8]; + out_2752907549792722588[22] = 0; + out_2752907549792722588[23] = -state[6]; + out_2752907549792722588[24] = -state[5]; + out_2752907549792722588[25] = 0; + out_2752907549792722588[26] = state[3]; + out_2752907549792722588[27] = 0; + out_2752907549792722588[28] = 0; + out_2752907549792722588[29] = 0; + out_2752907549792722588[30] = 0; + out_2752907549792722588[31] = 1; + out_2752907549792722588[32] = 0; + out_2752907549792722588[33] = 0; + out_2752907549792722588[34] = 1; + out_2752907549792722588[35] = 0; + out_2752907549792722588[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_2752907549792722588[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_2752907549792722588[38] = 0; + out_2752907549792722588[39] = -state[7]; + out_2752907549792722588[40] = state[6]; + out_2752907549792722588[41] = 0; + out_2752907549792722588[42] = state[4]; + out_2752907549792722588[43] = -state[3]; + out_2752907549792722588[44] = 0; + out_2752907549792722588[45] = 0; + out_2752907549792722588[46] = 0; + out_2752907549792722588[47] = 0; + out_2752907549792722588[48] = 0; + out_2752907549792722588[49] = 0; + out_2752907549792722588[50] = 1; + out_2752907549792722588[51] = 0; + out_2752907549792722588[52] = 0; + out_2752907549792722588[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_4247381704481051963) { + out_4247381704481051963[0] = state[3]; + out_4247381704481051963[1] = state[4]; + out_4247381704481051963[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_1791023673905199904) { + out_1791023673905199904[0] = 0; + out_1791023673905199904[1] = 0; + out_1791023673905199904[2] = 0; + out_1791023673905199904[3] = 1; + out_1791023673905199904[4] = 0; + out_1791023673905199904[5] = 0; + out_1791023673905199904[6] = 0; + out_1791023673905199904[7] = 0; + out_1791023673905199904[8] = 0; + out_1791023673905199904[9] = 0; + out_1791023673905199904[10] = 0; + out_1791023673905199904[11] = 0; + out_1791023673905199904[12] = 0; + out_1791023673905199904[13] = 0; + out_1791023673905199904[14] = 0; + out_1791023673905199904[15] = 0; + out_1791023673905199904[16] = 0; + out_1791023673905199904[17] = 0; + out_1791023673905199904[18] = 0; + out_1791023673905199904[19] = 0; + out_1791023673905199904[20] = 0; + out_1791023673905199904[21] = 0; + out_1791023673905199904[22] = 1; + out_1791023673905199904[23] = 0; + out_1791023673905199904[24] = 0; + out_1791023673905199904[25] = 0; + out_1791023673905199904[26] = 0; + out_1791023673905199904[27] = 0; + out_1791023673905199904[28] = 0; + out_1791023673905199904[29] = 0; + out_1791023673905199904[30] = 0; + out_1791023673905199904[31] = 0; + out_1791023673905199904[32] = 0; + out_1791023673905199904[33] = 0; + out_1791023673905199904[34] = 0; + out_1791023673905199904[35] = 0; + out_1791023673905199904[36] = 0; + out_1791023673905199904[37] = 0; + out_1791023673905199904[38] = 0; + out_1791023673905199904[39] = 0; + out_1791023673905199904[40] = 0; + out_1791023673905199904[41] = 1; + out_1791023673905199904[42] = 0; + out_1791023673905199904[43] = 0; + out_1791023673905199904[44] = 0; + out_1791023673905199904[45] = 0; + out_1791023673905199904[46] = 0; + out_1791023673905199904[47] = 0; + out_1791023673905199904[48] = 0; + out_1791023673905199904[49] = 0; + out_1791023673905199904[50] = 0; + out_1791023673905199904[51] = 0; + out_1791023673905199904[52] = 0; + out_1791023673905199904[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_7786063953392294783) { + out_7786063953392294783[0] = state[6]; + out_7786063953392294783[1] = state[7]; + out_7786063953392294783[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_5438414025882416304) { + out_5438414025882416304[0] = 0; + out_5438414025882416304[1] = 0; + out_5438414025882416304[2] = 0; + out_5438414025882416304[3] = 0; + out_5438414025882416304[4] = 0; + out_5438414025882416304[5] = 0; + out_5438414025882416304[6] = 1; + out_5438414025882416304[7] = 0; + out_5438414025882416304[8] = 0; + out_5438414025882416304[9] = 0; + out_5438414025882416304[10] = 0; + out_5438414025882416304[11] = 0; + out_5438414025882416304[12] = 0; + out_5438414025882416304[13] = 0; + out_5438414025882416304[14] = 0; + out_5438414025882416304[15] = 0; + out_5438414025882416304[16] = 0; + out_5438414025882416304[17] = 0; + out_5438414025882416304[18] = 0; + out_5438414025882416304[19] = 0; + out_5438414025882416304[20] = 0; + out_5438414025882416304[21] = 0; + out_5438414025882416304[22] = 0; + out_5438414025882416304[23] = 0; + out_5438414025882416304[24] = 0; + out_5438414025882416304[25] = 1; + out_5438414025882416304[26] = 0; + out_5438414025882416304[27] = 0; + out_5438414025882416304[28] = 0; + out_5438414025882416304[29] = 0; + out_5438414025882416304[30] = 0; + out_5438414025882416304[31] = 0; + out_5438414025882416304[32] = 0; + out_5438414025882416304[33] = 0; + out_5438414025882416304[34] = 0; + out_5438414025882416304[35] = 0; + out_5438414025882416304[36] = 0; + out_5438414025882416304[37] = 0; + out_5438414025882416304[38] = 0; + out_5438414025882416304[39] = 0; + out_5438414025882416304[40] = 0; + out_5438414025882416304[41] = 0; + out_5438414025882416304[42] = 0; + out_5438414025882416304[43] = 0; + out_5438414025882416304[44] = 1; + out_5438414025882416304[45] = 0; + out_5438414025882416304[46] = 0; + out_5438414025882416304[47] = 0; + out_5438414025882416304[48] = 0; + out_5438414025882416304[49] = 0; + out_5438414025882416304[50] = 0; + out_5438414025882416304[51] = 0; + out_5438414025882416304[52] = 0; + out_5438414025882416304[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_3328634486354213070) { + err_fun(nom_x, delta_x, out_3328634486354213070); } -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_2068367058924519839) { + inv_err_fun(nom_x, true_x, out_2068367058924519839); } -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_5273281981643743211) { + H_mod_fun(state, out_5273281981643743211); } -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_3798976948542527989) { + f_fun(state, dt, out_3798976948542527989); } -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_5376087696404799721) { + F_fun(state, dt, out_5376087696404799721); } -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_6100547545538860384) { + h_4(state, unused, out_6100547545538860384); } -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_9045089191487650783) { + H_4(state, unused, out_9045089191487650783); } -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_7661539883733460450) { + h_10(state, unused, out_7661539883733460450); } -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_2752907549792722588) { + H_10(state, unused, out_2752907549792722588); } -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_4247381704481051963) { + h_13(state, unused, out_4247381704481051963); } -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_1791023673905199904) { + H_13(state, unused, out_1791023673905199904); } -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_7786063953392294783) { + h_14(state, unused, out_7786063953392294783); } -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_5438414025882416304) { + H_14(state, unused, out_5438414025882416304); } 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 d6c8c61599..071312037d 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_3328634486354213070); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2068367058924519839); +void pose_H_mod_fun(double *state, double *out_5273281981643743211); +void pose_f_fun(double *state, double dt, double *out_3798976948542527989); +void pose_F_fun(double *state, double dt, double *out_5376087696404799721); +void pose_h_4(double *state, double *unused, double *out_6100547545538860384); +void pose_H_4(double *state, double *unused, double *out_9045089191487650783); +void pose_h_10(double *state, double *unused, double *out_7661539883733460450); +void pose_H_10(double *state, double *unused, double *out_2752907549792722588); +void pose_h_13(double *state, double *unused, double *out_4247381704481051963); +void pose_H_13(double *state, double *unused, double *out_1791023673905199904); +void pose_h_14(double *state, double *unused, double *out_7786063953392294783); +void pose_H_14(double *state, double *unused, double *out_5438414025882416304); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 268fca8985..d4416fe657 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index a14fc1e58e..0194af73fc 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ