diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index a042644d3..b41969208 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 9c7d325d9..5f67ed83c 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 c4ec50596..77f625eed 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 443e6db8f..a5f9ed364 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 995aaccd7..8337269e0 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 7c7441932..75bfc2e02 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 2506bd33d..a46debfc2 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 2506bd33d..a46debfc2 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 2d3722cb4..2b2a2d6af 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 7c7441932..75bfc2e02 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 8a16f3bb0..c7c757f4f 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-5f51e670-DEBUG"; +const uint8_t gitversion[19] = "DEV-2f74f933-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 955be3d77..efb7c83a2 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 ae8acaae4..a5f8e487f 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 85a45661e..fdaa8e923 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 acbffabf6..d6ab660ed 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 cbefa3246..3f68eaeee 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 cb568d1af..59bce629b 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 50148a87b..6b1e28d7a 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 c673d7097..00ff9e13b 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 e0a2071bc..e040c29c1 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 67a515a94..a61c202ff 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 ab1b81b65..06713114a 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 09d662eeb..30ec994e8 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 8f48664d7..307378ab2 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 37dc42f38..fef7a82ce 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 cbcf0ea45..c18289d33 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 feeb30bf8..fec21519a 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 dd02978b3..9ba519aeb 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 5e9a25650..034a6cd5c 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 1545a03ff..72c690825 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 ac3dbb0d1..0c26008fa 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 9d2779368..f0b84b966 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-5f51e670-DEBUG \ No newline at end of file +DEV-2f74f933-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 54bac82a1..9c78f10ef 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_2687587277675065552) { - out_2687587277675065552[0] = delta_x[0] + nom_x[0]; - out_2687587277675065552[1] = delta_x[1] + nom_x[1]; - out_2687587277675065552[2] = delta_x[2] + nom_x[2]; - out_2687587277675065552[3] = delta_x[3] + nom_x[3]; - out_2687587277675065552[4] = delta_x[4] + nom_x[4]; - out_2687587277675065552[5] = delta_x[5] + nom_x[5]; - out_2687587277675065552[6] = delta_x[6] + nom_x[6]; - out_2687587277675065552[7] = delta_x[7] + nom_x[7]; - out_2687587277675065552[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1461273597009064329) { + out_1461273597009064329[0] = delta_x[0] + nom_x[0]; + out_1461273597009064329[1] = delta_x[1] + nom_x[1]; + out_1461273597009064329[2] = delta_x[2] + nom_x[2]; + out_1461273597009064329[3] = delta_x[3] + nom_x[3]; + out_1461273597009064329[4] = delta_x[4] + nom_x[4]; + out_1461273597009064329[5] = delta_x[5] + nom_x[5]; + out_1461273597009064329[6] = delta_x[6] + nom_x[6]; + out_1461273597009064329[7] = delta_x[7] + nom_x[7]; + out_1461273597009064329[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_6222700140799525089) { - out_6222700140799525089[0] = -nom_x[0] + true_x[0]; - out_6222700140799525089[1] = -nom_x[1] + true_x[1]; - out_6222700140799525089[2] = -nom_x[2] + true_x[2]; - out_6222700140799525089[3] = -nom_x[3] + true_x[3]; - out_6222700140799525089[4] = -nom_x[4] + true_x[4]; - out_6222700140799525089[5] = -nom_x[5] + true_x[5]; - out_6222700140799525089[6] = -nom_x[6] + true_x[6]; - out_6222700140799525089[7] = -nom_x[7] + true_x[7]; - out_6222700140799525089[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_7289765730580897738) { + out_7289765730580897738[0] = -nom_x[0] + true_x[0]; + out_7289765730580897738[1] = -nom_x[1] + true_x[1]; + out_7289765730580897738[2] = -nom_x[2] + true_x[2]; + out_7289765730580897738[3] = -nom_x[3] + true_x[3]; + out_7289765730580897738[4] = -nom_x[4] + true_x[4]; + out_7289765730580897738[5] = -nom_x[5] + true_x[5]; + out_7289765730580897738[6] = -nom_x[6] + true_x[6]; + out_7289765730580897738[7] = -nom_x[7] + true_x[7]; + out_7289765730580897738[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_5736311190330935470) { - out_5736311190330935470[0] = 1.0; - out_5736311190330935470[1] = 0.0; - out_5736311190330935470[2] = 0.0; - out_5736311190330935470[3] = 0.0; - out_5736311190330935470[4] = 0.0; - out_5736311190330935470[5] = 0.0; - out_5736311190330935470[6] = 0.0; - out_5736311190330935470[7] = 0.0; - out_5736311190330935470[8] = 0.0; - out_5736311190330935470[9] = 0.0; - out_5736311190330935470[10] = 1.0; - out_5736311190330935470[11] = 0.0; - out_5736311190330935470[12] = 0.0; - out_5736311190330935470[13] = 0.0; - out_5736311190330935470[14] = 0.0; - out_5736311190330935470[15] = 0.0; - out_5736311190330935470[16] = 0.0; - out_5736311190330935470[17] = 0.0; - out_5736311190330935470[18] = 0.0; - out_5736311190330935470[19] = 0.0; - out_5736311190330935470[20] = 1.0; - out_5736311190330935470[21] = 0.0; - out_5736311190330935470[22] = 0.0; - out_5736311190330935470[23] = 0.0; - out_5736311190330935470[24] = 0.0; - out_5736311190330935470[25] = 0.0; - out_5736311190330935470[26] = 0.0; - out_5736311190330935470[27] = 0.0; - out_5736311190330935470[28] = 0.0; - out_5736311190330935470[29] = 0.0; - out_5736311190330935470[30] = 1.0; - out_5736311190330935470[31] = 0.0; - out_5736311190330935470[32] = 0.0; - out_5736311190330935470[33] = 0.0; - out_5736311190330935470[34] = 0.0; - out_5736311190330935470[35] = 0.0; - out_5736311190330935470[36] = 0.0; - out_5736311190330935470[37] = 0.0; - out_5736311190330935470[38] = 0.0; - out_5736311190330935470[39] = 0.0; - out_5736311190330935470[40] = 1.0; - out_5736311190330935470[41] = 0.0; - out_5736311190330935470[42] = 0.0; - out_5736311190330935470[43] = 0.0; - out_5736311190330935470[44] = 0.0; - out_5736311190330935470[45] = 0.0; - out_5736311190330935470[46] = 0.0; - out_5736311190330935470[47] = 0.0; - out_5736311190330935470[48] = 0.0; - out_5736311190330935470[49] = 0.0; - out_5736311190330935470[50] = 1.0; - out_5736311190330935470[51] = 0.0; - out_5736311190330935470[52] = 0.0; - out_5736311190330935470[53] = 0.0; - out_5736311190330935470[54] = 0.0; - out_5736311190330935470[55] = 0.0; - out_5736311190330935470[56] = 0.0; - out_5736311190330935470[57] = 0.0; - out_5736311190330935470[58] = 0.0; - out_5736311190330935470[59] = 0.0; - out_5736311190330935470[60] = 1.0; - out_5736311190330935470[61] = 0.0; - out_5736311190330935470[62] = 0.0; - out_5736311190330935470[63] = 0.0; - out_5736311190330935470[64] = 0.0; - out_5736311190330935470[65] = 0.0; - out_5736311190330935470[66] = 0.0; - out_5736311190330935470[67] = 0.0; - out_5736311190330935470[68] = 0.0; - out_5736311190330935470[69] = 0.0; - out_5736311190330935470[70] = 1.0; - out_5736311190330935470[71] = 0.0; - out_5736311190330935470[72] = 0.0; - out_5736311190330935470[73] = 0.0; - out_5736311190330935470[74] = 0.0; - out_5736311190330935470[75] = 0.0; - out_5736311190330935470[76] = 0.0; - out_5736311190330935470[77] = 0.0; - out_5736311190330935470[78] = 0.0; - out_5736311190330935470[79] = 0.0; - out_5736311190330935470[80] = 1.0; +void H_mod_fun(double *state, double *out_8304527178040913025) { + out_8304527178040913025[0] = 1.0; + out_8304527178040913025[1] = 0.0; + out_8304527178040913025[2] = 0.0; + out_8304527178040913025[3] = 0.0; + out_8304527178040913025[4] = 0.0; + out_8304527178040913025[5] = 0.0; + out_8304527178040913025[6] = 0.0; + out_8304527178040913025[7] = 0.0; + out_8304527178040913025[8] = 0.0; + out_8304527178040913025[9] = 0.0; + out_8304527178040913025[10] = 1.0; + out_8304527178040913025[11] = 0.0; + out_8304527178040913025[12] = 0.0; + out_8304527178040913025[13] = 0.0; + out_8304527178040913025[14] = 0.0; + out_8304527178040913025[15] = 0.0; + out_8304527178040913025[16] = 0.0; + out_8304527178040913025[17] = 0.0; + out_8304527178040913025[18] = 0.0; + out_8304527178040913025[19] = 0.0; + out_8304527178040913025[20] = 1.0; + out_8304527178040913025[21] = 0.0; + out_8304527178040913025[22] = 0.0; + out_8304527178040913025[23] = 0.0; + out_8304527178040913025[24] = 0.0; + out_8304527178040913025[25] = 0.0; + out_8304527178040913025[26] = 0.0; + out_8304527178040913025[27] = 0.0; + out_8304527178040913025[28] = 0.0; + out_8304527178040913025[29] = 0.0; + out_8304527178040913025[30] = 1.0; + out_8304527178040913025[31] = 0.0; + out_8304527178040913025[32] = 0.0; + out_8304527178040913025[33] = 0.0; + out_8304527178040913025[34] = 0.0; + out_8304527178040913025[35] = 0.0; + out_8304527178040913025[36] = 0.0; + out_8304527178040913025[37] = 0.0; + out_8304527178040913025[38] = 0.0; + out_8304527178040913025[39] = 0.0; + out_8304527178040913025[40] = 1.0; + out_8304527178040913025[41] = 0.0; + out_8304527178040913025[42] = 0.0; + out_8304527178040913025[43] = 0.0; + out_8304527178040913025[44] = 0.0; + out_8304527178040913025[45] = 0.0; + out_8304527178040913025[46] = 0.0; + out_8304527178040913025[47] = 0.0; + out_8304527178040913025[48] = 0.0; + out_8304527178040913025[49] = 0.0; + out_8304527178040913025[50] = 1.0; + out_8304527178040913025[51] = 0.0; + out_8304527178040913025[52] = 0.0; + out_8304527178040913025[53] = 0.0; + out_8304527178040913025[54] = 0.0; + out_8304527178040913025[55] = 0.0; + out_8304527178040913025[56] = 0.0; + out_8304527178040913025[57] = 0.0; + out_8304527178040913025[58] = 0.0; + out_8304527178040913025[59] = 0.0; + out_8304527178040913025[60] = 1.0; + out_8304527178040913025[61] = 0.0; + out_8304527178040913025[62] = 0.0; + out_8304527178040913025[63] = 0.0; + out_8304527178040913025[64] = 0.0; + out_8304527178040913025[65] = 0.0; + out_8304527178040913025[66] = 0.0; + out_8304527178040913025[67] = 0.0; + out_8304527178040913025[68] = 0.0; + out_8304527178040913025[69] = 0.0; + out_8304527178040913025[70] = 1.0; + out_8304527178040913025[71] = 0.0; + out_8304527178040913025[72] = 0.0; + out_8304527178040913025[73] = 0.0; + out_8304527178040913025[74] = 0.0; + out_8304527178040913025[75] = 0.0; + out_8304527178040913025[76] = 0.0; + out_8304527178040913025[77] = 0.0; + out_8304527178040913025[78] = 0.0; + out_8304527178040913025[79] = 0.0; + out_8304527178040913025[80] = 1.0; } -void f_fun(double *state, double dt, double *out_8059955085909907268) { - out_8059955085909907268[0] = state[0]; - out_8059955085909907268[1] = state[1]; - out_8059955085909907268[2] = state[2]; - out_8059955085909907268[3] = state[3]; - out_8059955085909907268[4] = state[4]; - out_8059955085909907268[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_8059955085909907268[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_8059955085909907268[7] = state[7]; - out_8059955085909907268[8] = state[8]; +void f_fun(double *state, double dt, double *out_1355173870151422192) { + out_1355173870151422192[0] = state[0]; + out_1355173870151422192[1] = state[1]; + out_1355173870151422192[2] = state[2]; + out_1355173870151422192[3] = state[3]; + out_1355173870151422192[4] = state[4]; + out_1355173870151422192[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_1355173870151422192[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_1355173870151422192[7] = state[7]; + out_1355173870151422192[8] = state[8]; } -void F_fun(double *state, double dt, double *out_535217138807217635) { - out_535217138807217635[0] = 1; - out_535217138807217635[1] = 0; - out_535217138807217635[2] = 0; - out_535217138807217635[3] = 0; - out_535217138807217635[4] = 0; - out_535217138807217635[5] = 0; - out_535217138807217635[6] = 0; - out_535217138807217635[7] = 0; - out_535217138807217635[8] = 0; - out_535217138807217635[9] = 0; - out_535217138807217635[10] = 1; - out_535217138807217635[11] = 0; - out_535217138807217635[12] = 0; - out_535217138807217635[13] = 0; - out_535217138807217635[14] = 0; - out_535217138807217635[15] = 0; - out_535217138807217635[16] = 0; - out_535217138807217635[17] = 0; - out_535217138807217635[18] = 0; - out_535217138807217635[19] = 0; - out_535217138807217635[20] = 1; - out_535217138807217635[21] = 0; - out_535217138807217635[22] = 0; - out_535217138807217635[23] = 0; - out_535217138807217635[24] = 0; - out_535217138807217635[25] = 0; - out_535217138807217635[26] = 0; - out_535217138807217635[27] = 0; - out_535217138807217635[28] = 0; - out_535217138807217635[29] = 0; - out_535217138807217635[30] = 1; - out_535217138807217635[31] = 0; - out_535217138807217635[32] = 0; - out_535217138807217635[33] = 0; - out_535217138807217635[34] = 0; - out_535217138807217635[35] = 0; - out_535217138807217635[36] = 0; - out_535217138807217635[37] = 0; - out_535217138807217635[38] = 0; - out_535217138807217635[39] = 0; - out_535217138807217635[40] = 1; - out_535217138807217635[41] = 0; - out_535217138807217635[42] = 0; - out_535217138807217635[43] = 0; - out_535217138807217635[44] = 0; - out_535217138807217635[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_535217138807217635[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_535217138807217635[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_535217138807217635[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_535217138807217635[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_535217138807217635[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_535217138807217635[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_535217138807217635[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_535217138807217635[53] = -9.8100000000000005*dt; - out_535217138807217635[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_535217138807217635[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_535217138807217635[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_535217138807217635[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_535217138807217635[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_535217138807217635[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_535217138807217635[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_535217138807217635[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_535217138807217635[62] = 0; - out_535217138807217635[63] = 0; - out_535217138807217635[64] = 0; - out_535217138807217635[65] = 0; - out_535217138807217635[66] = 0; - out_535217138807217635[67] = 0; - out_535217138807217635[68] = 0; - out_535217138807217635[69] = 0; - out_535217138807217635[70] = 1; - out_535217138807217635[71] = 0; - out_535217138807217635[72] = 0; - out_535217138807217635[73] = 0; - out_535217138807217635[74] = 0; - out_535217138807217635[75] = 0; - out_535217138807217635[76] = 0; - out_535217138807217635[77] = 0; - out_535217138807217635[78] = 0; - out_535217138807217635[79] = 0; - out_535217138807217635[80] = 1; +void F_fun(double *state, double dt, double *out_3219998669912464347) { + out_3219998669912464347[0] = 1; + out_3219998669912464347[1] = 0; + out_3219998669912464347[2] = 0; + out_3219998669912464347[3] = 0; + out_3219998669912464347[4] = 0; + out_3219998669912464347[5] = 0; + out_3219998669912464347[6] = 0; + out_3219998669912464347[7] = 0; + out_3219998669912464347[8] = 0; + out_3219998669912464347[9] = 0; + out_3219998669912464347[10] = 1; + out_3219998669912464347[11] = 0; + out_3219998669912464347[12] = 0; + out_3219998669912464347[13] = 0; + out_3219998669912464347[14] = 0; + out_3219998669912464347[15] = 0; + out_3219998669912464347[16] = 0; + out_3219998669912464347[17] = 0; + out_3219998669912464347[18] = 0; + out_3219998669912464347[19] = 0; + out_3219998669912464347[20] = 1; + out_3219998669912464347[21] = 0; + out_3219998669912464347[22] = 0; + out_3219998669912464347[23] = 0; + out_3219998669912464347[24] = 0; + out_3219998669912464347[25] = 0; + out_3219998669912464347[26] = 0; + out_3219998669912464347[27] = 0; + out_3219998669912464347[28] = 0; + out_3219998669912464347[29] = 0; + out_3219998669912464347[30] = 1; + out_3219998669912464347[31] = 0; + out_3219998669912464347[32] = 0; + out_3219998669912464347[33] = 0; + out_3219998669912464347[34] = 0; + out_3219998669912464347[35] = 0; + out_3219998669912464347[36] = 0; + out_3219998669912464347[37] = 0; + out_3219998669912464347[38] = 0; + out_3219998669912464347[39] = 0; + out_3219998669912464347[40] = 1; + out_3219998669912464347[41] = 0; + out_3219998669912464347[42] = 0; + out_3219998669912464347[43] = 0; + out_3219998669912464347[44] = 0; + out_3219998669912464347[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_3219998669912464347[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_3219998669912464347[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3219998669912464347[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3219998669912464347[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_3219998669912464347[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_3219998669912464347[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_3219998669912464347[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_3219998669912464347[53] = -9.8100000000000005*dt; + out_3219998669912464347[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_3219998669912464347[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_3219998669912464347[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3219998669912464347[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3219998669912464347[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_3219998669912464347[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_3219998669912464347[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_3219998669912464347[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3219998669912464347[62] = 0; + out_3219998669912464347[63] = 0; + out_3219998669912464347[64] = 0; + out_3219998669912464347[65] = 0; + out_3219998669912464347[66] = 0; + out_3219998669912464347[67] = 0; + out_3219998669912464347[68] = 0; + out_3219998669912464347[69] = 0; + out_3219998669912464347[70] = 1; + out_3219998669912464347[71] = 0; + out_3219998669912464347[72] = 0; + out_3219998669912464347[73] = 0; + out_3219998669912464347[74] = 0; + out_3219998669912464347[75] = 0; + out_3219998669912464347[76] = 0; + out_3219998669912464347[77] = 0; + out_3219998669912464347[78] = 0; + out_3219998669912464347[79] = 0; + out_3219998669912464347[80] = 1; } -void h_25(double *state, double *unused, double *out_7527307434081215851) { - out_7527307434081215851[0] = state[6]; +void h_25(double *state, double *unused, double *out_7686598041594365016) { + out_7686598041594365016[0] = state[6]; } -void H_25(double *state, double *unused, double *out_9187452233389124469) { - out_9187452233389124469[0] = 0; - out_9187452233389124469[1] = 0; - out_9187452233389124469[2] = 0; - out_9187452233389124469[3] = 0; - out_9187452233389124469[4] = 0; - out_9187452233389124469[5] = 0; - out_9187452233389124469[6] = 1; - out_9187452233389124469[7] = 0; - out_9187452233389124469[8] = 0; +void H_25(double *state, double *unused, double *out_2220878862694778786) { + out_2220878862694778786[0] = 0; + out_2220878862694778786[1] = 0; + out_2220878862694778786[2] = 0; + out_2220878862694778786[3] = 0; + out_2220878862694778786[4] = 0; + out_2220878862694778786[5] = 0; + out_2220878862694778786[6] = 1; + out_2220878862694778786[7] = 0; + out_2220878862694778786[8] = 0; } -void h_24(double *state, double *unused, double *out_7476062049775574580) { - out_7476062049775574580[0] = state[4]; - out_7476062049775574580[1] = state[5]; +void h_24(double *state, double *unused, double *out_3813839643785118531) { + out_3813839643785118531[0] = state[4]; + out_3813839643785118531[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5629526634586801333) { - out_5629526634586801333[0] = 0; - out_5629526634586801333[1] = 0; - out_5629526634586801333[2] = 0; - out_5629526634586801333[3] = 0; - out_5629526634586801333[4] = 1; - out_5629526634586801333[5] = 0; - out_5629526634586801333[6] = 0; - out_5629526634586801333[7] = 0; - out_5629526634586801333[8] = 0; - out_5629526634586801333[9] = 0; - out_5629526634586801333[10] = 0; - out_5629526634586801333[11] = 0; - out_5629526634586801333[12] = 0; - out_5629526634586801333[13] = 0; - out_5629526634586801333[14] = 1; - out_5629526634586801333[15] = 0; - out_5629526634586801333[16] = 0; - out_5629526634586801333[17] = 0; +void H_24(double *state, double *unused, double *out_1337046736107544350) { + out_1337046736107544350[0] = 0; + out_1337046736107544350[1] = 0; + out_1337046736107544350[2] = 0; + out_1337046736107544350[3] = 0; + out_1337046736107544350[4] = 1; + out_1337046736107544350[5] = 0; + out_1337046736107544350[6] = 0; + out_1337046736107544350[7] = 0; + out_1337046736107544350[8] = 0; + out_1337046736107544350[9] = 0; + out_1337046736107544350[10] = 0; + out_1337046736107544350[11] = 0; + out_1337046736107544350[12] = 0; + out_1337046736107544350[13] = 0; + out_1337046736107544350[14] = 1; + out_1337046736107544350[15] = 0; + out_1337046736107544350[16] = 0; + out_1337046736107544350[17] = 0; } -void h_30(double *state, double *unused, double *out_1371860238240555722) { - out_1371860238240555722[0] = state[4]; +void h_30(double *state, double *unused, double *out_2120268555396644716) { + out_2120268555396644716[0] = state[4]; } -void H_30(double *state, double *unused, double *out_2270761891897507714) { - out_2270761891897507714[0] = 0; - out_2270761891897507714[1] = 0; - out_2270761891897507714[2] = 0; - out_2270761891897507714[3] = 0; - out_2270761891897507714[4] = 1; - out_2270761891897507714[5] = 0; - out_2270761891897507714[6] = 0; - out_2270761891897507714[7] = 0; - out_2270761891897507714[8] = 0; +void H_30(double *state, double *unused, double *out_297454095812469841) { + out_297454095812469841[0] = 0; + out_297454095812469841[1] = 0; + out_297454095812469841[2] = 0; + out_297454095812469841[3] = 0; + out_297454095812469841[4] = 1; + out_297454095812469841[5] = 0; + out_297454095812469841[6] = 0; + out_297454095812469841[7] = 0; + out_297454095812469841[8] = 0; } -void h_26(double *state, double *unused, double *out_6063283196188145091) { - out_6063283196188145091[0] = state[7]; +void h_26(double *state, double *unused, double *out_2881404712264200929) { + out_2881404712264200929[0] = state[7]; } -void H_26(double *state, double *unused, double *out_5517788521446370923) { - out_5517788521446370923[0] = 0; - out_5517788521446370923[1] = 0; - out_5517788521446370923[2] = 0; - out_5517788521446370923[3] = 0; - out_5517788521446370923[4] = 0; - out_5517788521446370923[5] = 0; - out_5517788521446370923[6] = 0; - out_5517788521446370923[7] = 1; - out_5517788521446370923[8] = 0; +void H_26(double *state, double *unused, double *out_5962382181568835010) { + out_5962382181568835010[0] = 0; + out_5962382181568835010[1] = 0; + out_5962382181568835010[2] = 0; + out_5962382181568835010[3] = 0; + out_5962382181568835010[4] = 0; + out_5962382181568835010[5] = 0; + out_5962382181568835010[6] = 0; + out_5962382181568835010[7] = 1; + out_5962382181568835010[8] = 0; } -void h_27(double *state, double *unused, double *out_8470817286982878286) { - out_8470817286982878286[0] = state[3]; +void h_27(double *state, double *unused, double *out_901564640210476392) { + out_901564640210476392[0] = state[3]; } -void H_27(double *state, double *unused, double *out_4445525203697932625) { - out_4445525203697932625[0] = 0; - out_4445525203697932625[1] = 0; - out_4445525203697932625[2] = 0; - out_4445525203697932625[3] = 1; - out_4445525203697932625[4] = 0; - out_4445525203697932625[5] = 0; - out_4445525203697932625[6] = 0; - out_4445525203697932625[7] = 0; - out_4445525203697932625[8] = 0; +void H_27(double *state, double *unused, double *out_2521048166996413058) { + out_2521048166996413058[0] = 0; + out_2521048166996413058[1] = 0; + out_2521048166996413058[2] = 0; + out_2521048166996413058[3] = 1; + out_2521048166996413058[4] = 0; + out_2521048166996413058[5] = 0; + out_2521048166996413058[6] = 0; + out_2521048166996413058[7] = 0; + out_2521048166996413058[8] = 0; } -void h_29(double *state, double *unused, double *out_7424449226025441418) { - out_7424449226025441418[0] = state[1]; +void h_29(double *state, double *unused, double *out_952524754320502979) { + out_952524754320502979[0] = state[1]; } -void H_29(double *state, double *unused, double *out_1760530547583115530) { - out_1760530547583115530[0] = 0; - out_1760530547583115530[1] = 1; - out_1760530547583115530[2] = 0; - out_1760530547583115530[3] = 0; - out_1760530547583115530[4] = 0; - out_1760530547583115530[5] = 0; - out_1760530547583115530[6] = 0; - out_1760530547583115530[7] = 0; - out_1760530547583115530[8] = 0; +void H_29(double *state, double *unused, double *out_807685440126862025) { + out_807685440126862025[0] = 0; + out_807685440126862025[1] = 1; + out_807685440126862025[2] = 0; + out_807685440126862025[3] = 0; + out_807685440126862025[4] = 0; + out_807685440126862025[5] = 0; + out_807685440126862025[6] = 0; + out_807685440126862025[7] = 0; + out_807685440126862025[8] = 0; } -void h_28(double *state, double *unused, double *out_2540783352297972517) { - out_2540783352297972517[0] = state[0]; +void h_28(double *state, double *unused, double *out_8550909813521810734) { + out_8550909813521810734[0] = state[0]; } -void H_28(double *state, double *unused, double *out_6842929564652646104) { - out_6842929564652646104[0] = 1; - out_6842929564652646104[1] = 0; - out_6842929564652646104[2] = 0; - out_6842929564652646104[3] = 0; - out_6842929564652646104[4] = 0; - out_6842929564652646104[5] = 0; - out_6842929564652646104[6] = 0; - out_6842929564652646104[7] = 0; - out_6842929564652646104[8] = 0; +void H_28(double *state, double *unused, double *out_4274713576942668549) { + out_4274713576942668549[0] = 1; + out_4274713576942668549[1] = 0; + out_4274713576942668549[2] = 0; + out_4274713576942668549[3] = 0; + out_4274713576942668549[4] = 0; + out_4274713576942668549[5] = 0; + out_4274713576942668549[6] = 0; + out_4274713576942668549[7] = 0; + out_4274713576942668549[8] = 0; } -void h_31(double *state, double *unused, double *out_7802501496365721740) { - out_7802501496365721740[0] = state[8]; +void h_31(double *state, double *unused, double *out_7961792103878870905) { + out_7961792103878870905[0] = state[8]; } -void H_31(double *state, double *unused, double *out_9156806271512164041) { - out_9156806271512164041[0] = 0; - out_9156806271512164041[1] = 0; - out_9156806271512164041[2] = 0; - out_9156806271512164041[3] = 0; - out_9156806271512164041[4] = 0; - out_9156806271512164041[5] = 0; - out_9156806271512164041[6] = 0; - out_9156806271512164041[7] = 0; - out_9156806271512164041[8] = 1; +void H_31(double *state, double *unused, double *out_6588590283802186486) { + out_6588590283802186486[0] = 0; + out_6588590283802186486[1] = 0; + out_6588590283802186486[2] = 0; + out_6588590283802186486[3] = 0; + out_6588590283802186486[4] = 0; + out_6588590283802186486[5] = 0; + out_6588590283802186486[6] = 0; + out_6588590283802186486[7] = 0; + out_6588590283802186486[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_2687587277675065552) { - err_fun(nom_x, delta_x, out_2687587277675065552); +void car_err_fun(double *nom_x, double *delta_x, double *out_1461273597009064329) { + err_fun(nom_x, delta_x, out_1461273597009064329); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_6222700140799525089) { - inv_err_fun(nom_x, true_x, out_6222700140799525089); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_7289765730580897738) { + inv_err_fun(nom_x, true_x, out_7289765730580897738); } -void car_H_mod_fun(double *state, double *out_5736311190330935470) { - H_mod_fun(state, out_5736311190330935470); +void car_H_mod_fun(double *state, double *out_8304527178040913025) { + H_mod_fun(state, out_8304527178040913025); } -void car_f_fun(double *state, double dt, double *out_8059955085909907268) { - f_fun(state, dt, out_8059955085909907268); +void car_f_fun(double *state, double dt, double *out_1355173870151422192) { + f_fun(state, dt, out_1355173870151422192); } -void car_F_fun(double *state, double dt, double *out_535217138807217635) { - F_fun(state, dt, out_535217138807217635); +void car_F_fun(double *state, double dt, double *out_3219998669912464347) { + F_fun(state, dt, out_3219998669912464347); } -void car_h_25(double *state, double *unused, double *out_7527307434081215851) { - h_25(state, unused, out_7527307434081215851); +void car_h_25(double *state, double *unused, double *out_7686598041594365016) { + h_25(state, unused, out_7686598041594365016); } -void car_H_25(double *state, double *unused, double *out_9187452233389124469) { - H_25(state, unused, out_9187452233389124469); +void car_H_25(double *state, double *unused, double *out_2220878862694778786) { + H_25(state, unused, out_2220878862694778786); } -void car_h_24(double *state, double *unused, double *out_7476062049775574580) { - h_24(state, unused, out_7476062049775574580); +void car_h_24(double *state, double *unused, double *out_3813839643785118531) { + h_24(state, unused, out_3813839643785118531); } -void car_H_24(double *state, double *unused, double *out_5629526634586801333) { - H_24(state, unused, out_5629526634586801333); +void car_H_24(double *state, double *unused, double *out_1337046736107544350) { + H_24(state, unused, out_1337046736107544350); } -void car_h_30(double *state, double *unused, double *out_1371860238240555722) { - h_30(state, unused, out_1371860238240555722); +void car_h_30(double *state, double *unused, double *out_2120268555396644716) { + h_30(state, unused, out_2120268555396644716); } -void car_H_30(double *state, double *unused, double *out_2270761891897507714) { - H_30(state, unused, out_2270761891897507714); +void car_H_30(double *state, double *unused, double *out_297454095812469841) { + H_30(state, unused, out_297454095812469841); } -void car_h_26(double *state, double *unused, double *out_6063283196188145091) { - h_26(state, unused, out_6063283196188145091); +void car_h_26(double *state, double *unused, double *out_2881404712264200929) { + h_26(state, unused, out_2881404712264200929); } -void car_H_26(double *state, double *unused, double *out_5517788521446370923) { - H_26(state, unused, out_5517788521446370923); +void car_H_26(double *state, double *unused, double *out_5962382181568835010) { + H_26(state, unused, out_5962382181568835010); } -void car_h_27(double *state, double *unused, double *out_8470817286982878286) { - h_27(state, unused, out_8470817286982878286); +void car_h_27(double *state, double *unused, double *out_901564640210476392) { + h_27(state, unused, out_901564640210476392); } -void car_H_27(double *state, double *unused, double *out_4445525203697932625) { - H_27(state, unused, out_4445525203697932625); +void car_H_27(double *state, double *unused, double *out_2521048166996413058) { + H_27(state, unused, out_2521048166996413058); } -void car_h_29(double *state, double *unused, double *out_7424449226025441418) { - h_29(state, unused, out_7424449226025441418); +void car_h_29(double *state, double *unused, double *out_952524754320502979) { + h_29(state, unused, out_952524754320502979); } -void car_H_29(double *state, double *unused, double *out_1760530547583115530) { - H_29(state, unused, out_1760530547583115530); +void car_H_29(double *state, double *unused, double *out_807685440126862025) { + H_29(state, unused, out_807685440126862025); } -void car_h_28(double *state, double *unused, double *out_2540783352297972517) { - h_28(state, unused, out_2540783352297972517); +void car_h_28(double *state, double *unused, double *out_8550909813521810734) { + h_28(state, unused, out_8550909813521810734); } -void car_H_28(double *state, double *unused, double *out_6842929564652646104) { - H_28(state, unused, out_6842929564652646104); +void car_H_28(double *state, double *unused, double *out_4274713576942668549) { + H_28(state, unused, out_4274713576942668549); } -void car_h_31(double *state, double *unused, double *out_7802501496365721740) { - h_31(state, unused, out_7802501496365721740); +void car_h_31(double *state, double *unused, double *out_7961792103878870905) { + h_31(state, unused, out_7961792103878870905); } -void car_H_31(double *state, double *unused, double *out_9156806271512164041) { - H_31(state, unused, out_9156806271512164041); +void car_H_31(double *state, double *unused, double *out_6588590283802186486) { + H_31(state, unused, out_6588590283802186486); } 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 01b4b1286..9b5b1538a 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_2687587277675065552); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_6222700140799525089); -void car_H_mod_fun(double *state, double *out_5736311190330935470); -void car_f_fun(double *state, double dt, double *out_8059955085909907268); -void car_F_fun(double *state, double dt, double *out_535217138807217635); -void car_h_25(double *state, double *unused, double *out_7527307434081215851); -void car_H_25(double *state, double *unused, double *out_9187452233389124469); -void car_h_24(double *state, double *unused, double *out_7476062049775574580); -void car_H_24(double *state, double *unused, double *out_5629526634586801333); -void car_h_30(double *state, double *unused, double *out_1371860238240555722); -void car_H_30(double *state, double *unused, double *out_2270761891897507714); -void car_h_26(double *state, double *unused, double *out_6063283196188145091); -void car_H_26(double *state, double *unused, double *out_5517788521446370923); -void car_h_27(double *state, double *unused, double *out_8470817286982878286); -void car_H_27(double *state, double *unused, double *out_4445525203697932625); -void car_h_29(double *state, double *unused, double *out_7424449226025441418); -void car_H_29(double *state, double *unused, double *out_1760530547583115530); -void car_h_28(double *state, double *unused, double *out_2540783352297972517); -void car_H_28(double *state, double *unused, double *out_6842929564652646104); -void car_h_31(double *state, double *unused, double *out_7802501496365721740); -void car_H_31(double *state, double *unused, double *out_9156806271512164041); +void car_err_fun(double *nom_x, double *delta_x, double *out_1461273597009064329); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_7289765730580897738); +void car_H_mod_fun(double *state, double *out_8304527178040913025); +void car_f_fun(double *state, double dt, double *out_1355173870151422192); +void car_F_fun(double *state, double dt, double *out_3219998669912464347); +void car_h_25(double *state, double *unused, double *out_7686598041594365016); +void car_H_25(double *state, double *unused, double *out_2220878862694778786); +void car_h_24(double *state, double *unused, double *out_3813839643785118531); +void car_H_24(double *state, double *unused, double *out_1337046736107544350); +void car_h_30(double *state, double *unused, double *out_2120268555396644716); +void car_H_30(double *state, double *unused, double *out_297454095812469841); +void car_h_26(double *state, double *unused, double *out_2881404712264200929); +void car_H_26(double *state, double *unused, double *out_5962382181568835010); +void car_h_27(double *state, double *unused, double *out_901564640210476392); +void car_H_27(double *state, double *unused, double *out_2521048166996413058); +void car_h_29(double *state, double *unused, double *out_952524754320502979); +void car_H_29(double *state, double *unused, double *out_807685440126862025); +void car_h_28(double *state, double *unused, double *out_8550909813521810734); +void car_H_28(double *state, double *unused, double *out_4274713576942668549); +void car_h_31(double *state, double *unused, double *out_7961792103878870905); +void car_H_31(double *state, double *unused, double *out_6588590283802186486); 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 b1ee67f33..d8864232f 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_7389011504566069386) { - out_7389011504566069386[0] = delta_x[0] + nom_x[0]; - out_7389011504566069386[1] = delta_x[1] + nom_x[1]; - out_7389011504566069386[2] = delta_x[2] + nom_x[2]; - out_7389011504566069386[3] = delta_x[3] + nom_x[3]; - out_7389011504566069386[4] = delta_x[4] + nom_x[4]; - out_7389011504566069386[5] = delta_x[5] + nom_x[5]; - out_7389011504566069386[6] = delta_x[6] + nom_x[6]; - out_7389011504566069386[7] = delta_x[7] + nom_x[7]; - out_7389011504566069386[8] = delta_x[8] + nom_x[8]; - out_7389011504566069386[9] = delta_x[9] + nom_x[9]; - out_7389011504566069386[10] = delta_x[10] + nom_x[10]; - out_7389011504566069386[11] = delta_x[11] + nom_x[11]; - out_7389011504566069386[12] = delta_x[12] + nom_x[12]; - out_7389011504566069386[13] = delta_x[13] + nom_x[13]; - out_7389011504566069386[14] = delta_x[14] + nom_x[14]; - out_7389011504566069386[15] = delta_x[15] + nom_x[15]; - out_7389011504566069386[16] = delta_x[16] + nom_x[16]; - out_7389011504566069386[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7352766374165151156) { + out_7352766374165151156[0] = delta_x[0] + nom_x[0]; + out_7352766374165151156[1] = delta_x[1] + nom_x[1]; + out_7352766374165151156[2] = delta_x[2] + nom_x[2]; + out_7352766374165151156[3] = delta_x[3] + nom_x[3]; + out_7352766374165151156[4] = delta_x[4] + nom_x[4]; + out_7352766374165151156[5] = delta_x[5] + nom_x[5]; + out_7352766374165151156[6] = delta_x[6] + nom_x[6]; + out_7352766374165151156[7] = delta_x[7] + nom_x[7]; + out_7352766374165151156[8] = delta_x[8] + nom_x[8]; + out_7352766374165151156[9] = delta_x[9] + nom_x[9]; + out_7352766374165151156[10] = delta_x[10] + nom_x[10]; + out_7352766374165151156[11] = delta_x[11] + nom_x[11]; + out_7352766374165151156[12] = delta_x[12] + nom_x[12]; + out_7352766374165151156[13] = delta_x[13] + nom_x[13]; + out_7352766374165151156[14] = delta_x[14] + nom_x[14]; + out_7352766374165151156[15] = delta_x[15] + nom_x[15]; + out_7352766374165151156[16] = delta_x[16] + nom_x[16]; + out_7352766374165151156[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_6795243368226050386) { - out_6795243368226050386[0] = -nom_x[0] + true_x[0]; - out_6795243368226050386[1] = -nom_x[1] + true_x[1]; - out_6795243368226050386[2] = -nom_x[2] + true_x[2]; - out_6795243368226050386[3] = -nom_x[3] + true_x[3]; - out_6795243368226050386[4] = -nom_x[4] + true_x[4]; - out_6795243368226050386[5] = -nom_x[5] + true_x[5]; - out_6795243368226050386[6] = -nom_x[6] + true_x[6]; - out_6795243368226050386[7] = -nom_x[7] + true_x[7]; - out_6795243368226050386[8] = -nom_x[8] + true_x[8]; - out_6795243368226050386[9] = -nom_x[9] + true_x[9]; - out_6795243368226050386[10] = -nom_x[10] + true_x[10]; - out_6795243368226050386[11] = -nom_x[11] + true_x[11]; - out_6795243368226050386[12] = -nom_x[12] + true_x[12]; - out_6795243368226050386[13] = -nom_x[13] + true_x[13]; - out_6795243368226050386[14] = -nom_x[14] + true_x[14]; - out_6795243368226050386[15] = -nom_x[15] + true_x[15]; - out_6795243368226050386[16] = -nom_x[16] + true_x[16]; - out_6795243368226050386[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5943722919136625654) { + out_5943722919136625654[0] = -nom_x[0] + true_x[0]; + out_5943722919136625654[1] = -nom_x[1] + true_x[1]; + out_5943722919136625654[2] = -nom_x[2] + true_x[2]; + out_5943722919136625654[3] = -nom_x[3] + true_x[3]; + out_5943722919136625654[4] = -nom_x[4] + true_x[4]; + out_5943722919136625654[5] = -nom_x[5] + true_x[5]; + out_5943722919136625654[6] = -nom_x[6] + true_x[6]; + out_5943722919136625654[7] = -nom_x[7] + true_x[7]; + out_5943722919136625654[8] = -nom_x[8] + true_x[8]; + out_5943722919136625654[9] = -nom_x[9] + true_x[9]; + out_5943722919136625654[10] = -nom_x[10] + true_x[10]; + out_5943722919136625654[11] = -nom_x[11] + true_x[11]; + out_5943722919136625654[12] = -nom_x[12] + true_x[12]; + out_5943722919136625654[13] = -nom_x[13] + true_x[13]; + out_5943722919136625654[14] = -nom_x[14] + true_x[14]; + out_5943722919136625654[15] = -nom_x[15] + true_x[15]; + out_5943722919136625654[16] = -nom_x[16] + true_x[16]; + out_5943722919136625654[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_6387810172757071512) { - out_6387810172757071512[0] = 1.0; - out_6387810172757071512[1] = 0.0; - out_6387810172757071512[2] = 0.0; - out_6387810172757071512[3] = 0.0; - out_6387810172757071512[4] = 0.0; - out_6387810172757071512[5] = 0.0; - out_6387810172757071512[6] = 0.0; - out_6387810172757071512[7] = 0.0; - out_6387810172757071512[8] = 0.0; - out_6387810172757071512[9] = 0.0; - out_6387810172757071512[10] = 0.0; - out_6387810172757071512[11] = 0.0; - out_6387810172757071512[12] = 0.0; - out_6387810172757071512[13] = 0.0; - out_6387810172757071512[14] = 0.0; - out_6387810172757071512[15] = 0.0; - out_6387810172757071512[16] = 0.0; - out_6387810172757071512[17] = 0.0; - out_6387810172757071512[18] = 0.0; - out_6387810172757071512[19] = 1.0; - out_6387810172757071512[20] = 0.0; - out_6387810172757071512[21] = 0.0; - out_6387810172757071512[22] = 0.0; - out_6387810172757071512[23] = 0.0; - out_6387810172757071512[24] = 0.0; - out_6387810172757071512[25] = 0.0; - out_6387810172757071512[26] = 0.0; - out_6387810172757071512[27] = 0.0; - out_6387810172757071512[28] = 0.0; - out_6387810172757071512[29] = 0.0; - out_6387810172757071512[30] = 0.0; - out_6387810172757071512[31] = 0.0; - out_6387810172757071512[32] = 0.0; - out_6387810172757071512[33] = 0.0; - out_6387810172757071512[34] = 0.0; - out_6387810172757071512[35] = 0.0; - out_6387810172757071512[36] = 0.0; - out_6387810172757071512[37] = 0.0; - out_6387810172757071512[38] = 1.0; - out_6387810172757071512[39] = 0.0; - out_6387810172757071512[40] = 0.0; - out_6387810172757071512[41] = 0.0; - out_6387810172757071512[42] = 0.0; - out_6387810172757071512[43] = 0.0; - out_6387810172757071512[44] = 0.0; - out_6387810172757071512[45] = 0.0; - out_6387810172757071512[46] = 0.0; - out_6387810172757071512[47] = 0.0; - out_6387810172757071512[48] = 0.0; - out_6387810172757071512[49] = 0.0; - out_6387810172757071512[50] = 0.0; - out_6387810172757071512[51] = 0.0; - out_6387810172757071512[52] = 0.0; - out_6387810172757071512[53] = 0.0; - out_6387810172757071512[54] = 0.0; - out_6387810172757071512[55] = 0.0; - out_6387810172757071512[56] = 0.0; - out_6387810172757071512[57] = 1.0; - out_6387810172757071512[58] = 0.0; - out_6387810172757071512[59] = 0.0; - out_6387810172757071512[60] = 0.0; - out_6387810172757071512[61] = 0.0; - out_6387810172757071512[62] = 0.0; - out_6387810172757071512[63] = 0.0; - out_6387810172757071512[64] = 0.0; - out_6387810172757071512[65] = 0.0; - out_6387810172757071512[66] = 0.0; - out_6387810172757071512[67] = 0.0; - out_6387810172757071512[68] = 0.0; - out_6387810172757071512[69] = 0.0; - out_6387810172757071512[70] = 0.0; - out_6387810172757071512[71] = 0.0; - out_6387810172757071512[72] = 0.0; - out_6387810172757071512[73] = 0.0; - out_6387810172757071512[74] = 0.0; - out_6387810172757071512[75] = 0.0; - out_6387810172757071512[76] = 1.0; - out_6387810172757071512[77] = 0.0; - out_6387810172757071512[78] = 0.0; - out_6387810172757071512[79] = 0.0; - out_6387810172757071512[80] = 0.0; - out_6387810172757071512[81] = 0.0; - out_6387810172757071512[82] = 0.0; - out_6387810172757071512[83] = 0.0; - out_6387810172757071512[84] = 0.0; - out_6387810172757071512[85] = 0.0; - out_6387810172757071512[86] = 0.0; - out_6387810172757071512[87] = 0.0; - out_6387810172757071512[88] = 0.0; - out_6387810172757071512[89] = 0.0; - out_6387810172757071512[90] = 0.0; - out_6387810172757071512[91] = 0.0; - out_6387810172757071512[92] = 0.0; - out_6387810172757071512[93] = 0.0; - out_6387810172757071512[94] = 0.0; - out_6387810172757071512[95] = 1.0; - out_6387810172757071512[96] = 0.0; - out_6387810172757071512[97] = 0.0; - out_6387810172757071512[98] = 0.0; - out_6387810172757071512[99] = 0.0; - out_6387810172757071512[100] = 0.0; - out_6387810172757071512[101] = 0.0; - out_6387810172757071512[102] = 0.0; - out_6387810172757071512[103] = 0.0; - out_6387810172757071512[104] = 0.0; - out_6387810172757071512[105] = 0.0; - out_6387810172757071512[106] = 0.0; - out_6387810172757071512[107] = 0.0; - out_6387810172757071512[108] = 0.0; - out_6387810172757071512[109] = 0.0; - out_6387810172757071512[110] = 0.0; - out_6387810172757071512[111] = 0.0; - out_6387810172757071512[112] = 0.0; - out_6387810172757071512[113] = 0.0; - out_6387810172757071512[114] = 1.0; - out_6387810172757071512[115] = 0.0; - out_6387810172757071512[116] = 0.0; - out_6387810172757071512[117] = 0.0; - out_6387810172757071512[118] = 0.0; - out_6387810172757071512[119] = 0.0; - out_6387810172757071512[120] = 0.0; - out_6387810172757071512[121] = 0.0; - out_6387810172757071512[122] = 0.0; - out_6387810172757071512[123] = 0.0; - out_6387810172757071512[124] = 0.0; - out_6387810172757071512[125] = 0.0; - out_6387810172757071512[126] = 0.0; - out_6387810172757071512[127] = 0.0; - out_6387810172757071512[128] = 0.0; - out_6387810172757071512[129] = 0.0; - out_6387810172757071512[130] = 0.0; - out_6387810172757071512[131] = 0.0; - out_6387810172757071512[132] = 0.0; - out_6387810172757071512[133] = 1.0; - out_6387810172757071512[134] = 0.0; - out_6387810172757071512[135] = 0.0; - out_6387810172757071512[136] = 0.0; - out_6387810172757071512[137] = 0.0; - out_6387810172757071512[138] = 0.0; - out_6387810172757071512[139] = 0.0; - out_6387810172757071512[140] = 0.0; - out_6387810172757071512[141] = 0.0; - out_6387810172757071512[142] = 0.0; - out_6387810172757071512[143] = 0.0; - out_6387810172757071512[144] = 0.0; - out_6387810172757071512[145] = 0.0; - out_6387810172757071512[146] = 0.0; - out_6387810172757071512[147] = 0.0; - out_6387810172757071512[148] = 0.0; - out_6387810172757071512[149] = 0.0; - out_6387810172757071512[150] = 0.0; - out_6387810172757071512[151] = 0.0; - out_6387810172757071512[152] = 1.0; - out_6387810172757071512[153] = 0.0; - out_6387810172757071512[154] = 0.0; - out_6387810172757071512[155] = 0.0; - out_6387810172757071512[156] = 0.0; - out_6387810172757071512[157] = 0.0; - out_6387810172757071512[158] = 0.0; - out_6387810172757071512[159] = 0.0; - out_6387810172757071512[160] = 0.0; - out_6387810172757071512[161] = 0.0; - out_6387810172757071512[162] = 0.0; - out_6387810172757071512[163] = 0.0; - out_6387810172757071512[164] = 0.0; - out_6387810172757071512[165] = 0.0; - out_6387810172757071512[166] = 0.0; - out_6387810172757071512[167] = 0.0; - out_6387810172757071512[168] = 0.0; - out_6387810172757071512[169] = 0.0; - out_6387810172757071512[170] = 0.0; - out_6387810172757071512[171] = 1.0; - out_6387810172757071512[172] = 0.0; - out_6387810172757071512[173] = 0.0; - out_6387810172757071512[174] = 0.0; - out_6387810172757071512[175] = 0.0; - out_6387810172757071512[176] = 0.0; - out_6387810172757071512[177] = 0.0; - out_6387810172757071512[178] = 0.0; - out_6387810172757071512[179] = 0.0; - out_6387810172757071512[180] = 0.0; - out_6387810172757071512[181] = 0.0; - out_6387810172757071512[182] = 0.0; - out_6387810172757071512[183] = 0.0; - out_6387810172757071512[184] = 0.0; - out_6387810172757071512[185] = 0.0; - out_6387810172757071512[186] = 0.0; - out_6387810172757071512[187] = 0.0; - out_6387810172757071512[188] = 0.0; - out_6387810172757071512[189] = 0.0; - out_6387810172757071512[190] = 1.0; - out_6387810172757071512[191] = 0.0; - out_6387810172757071512[192] = 0.0; - out_6387810172757071512[193] = 0.0; - out_6387810172757071512[194] = 0.0; - out_6387810172757071512[195] = 0.0; - out_6387810172757071512[196] = 0.0; - out_6387810172757071512[197] = 0.0; - out_6387810172757071512[198] = 0.0; - out_6387810172757071512[199] = 0.0; - out_6387810172757071512[200] = 0.0; - out_6387810172757071512[201] = 0.0; - out_6387810172757071512[202] = 0.0; - out_6387810172757071512[203] = 0.0; - out_6387810172757071512[204] = 0.0; - out_6387810172757071512[205] = 0.0; - out_6387810172757071512[206] = 0.0; - out_6387810172757071512[207] = 0.0; - out_6387810172757071512[208] = 0.0; - out_6387810172757071512[209] = 1.0; - out_6387810172757071512[210] = 0.0; - out_6387810172757071512[211] = 0.0; - out_6387810172757071512[212] = 0.0; - out_6387810172757071512[213] = 0.0; - out_6387810172757071512[214] = 0.0; - out_6387810172757071512[215] = 0.0; - out_6387810172757071512[216] = 0.0; - out_6387810172757071512[217] = 0.0; - out_6387810172757071512[218] = 0.0; - out_6387810172757071512[219] = 0.0; - out_6387810172757071512[220] = 0.0; - out_6387810172757071512[221] = 0.0; - out_6387810172757071512[222] = 0.0; - out_6387810172757071512[223] = 0.0; - out_6387810172757071512[224] = 0.0; - out_6387810172757071512[225] = 0.0; - out_6387810172757071512[226] = 0.0; - out_6387810172757071512[227] = 0.0; - out_6387810172757071512[228] = 1.0; - out_6387810172757071512[229] = 0.0; - out_6387810172757071512[230] = 0.0; - out_6387810172757071512[231] = 0.0; - out_6387810172757071512[232] = 0.0; - out_6387810172757071512[233] = 0.0; - out_6387810172757071512[234] = 0.0; - out_6387810172757071512[235] = 0.0; - out_6387810172757071512[236] = 0.0; - out_6387810172757071512[237] = 0.0; - out_6387810172757071512[238] = 0.0; - out_6387810172757071512[239] = 0.0; - out_6387810172757071512[240] = 0.0; - out_6387810172757071512[241] = 0.0; - out_6387810172757071512[242] = 0.0; - out_6387810172757071512[243] = 0.0; - out_6387810172757071512[244] = 0.0; - out_6387810172757071512[245] = 0.0; - out_6387810172757071512[246] = 0.0; - out_6387810172757071512[247] = 1.0; - out_6387810172757071512[248] = 0.0; - out_6387810172757071512[249] = 0.0; - out_6387810172757071512[250] = 0.0; - out_6387810172757071512[251] = 0.0; - out_6387810172757071512[252] = 0.0; - out_6387810172757071512[253] = 0.0; - out_6387810172757071512[254] = 0.0; - out_6387810172757071512[255] = 0.0; - out_6387810172757071512[256] = 0.0; - out_6387810172757071512[257] = 0.0; - out_6387810172757071512[258] = 0.0; - out_6387810172757071512[259] = 0.0; - out_6387810172757071512[260] = 0.0; - out_6387810172757071512[261] = 0.0; - out_6387810172757071512[262] = 0.0; - out_6387810172757071512[263] = 0.0; - out_6387810172757071512[264] = 0.0; - out_6387810172757071512[265] = 0.0; - out_6387810172757071512[266] = 1.0; - out_6387810172757071512[267] = 0.0; - out_6387810172757071512[268] = 0.0; - out_6387810172757071512[269] = 0.0; - out_6387810172757071512[270] = 0.0; - out_6387810172757071512[271] = 0.0; - out_6387810172757071512[272] = 0.0; - out_6387810172757071512[273] = 0.0; - out_6387810172757071512[274] = 0.0; - out_6387810172757071512[275] = 0.0; - out_6387810172757071512[276] = 0.0; - out_6387810172757071512[277] = 0.0; - out_6387810172757071512[278] = 0.0; - out_6387810172757071512[279] = 0.0; - out_6387810172757071512[280] = 0.0; - out_6387810172757071512[281] = 0.0; - out_6387810172757071512[282] = 0.0; - out_6387810172757071512[283] = 0.0; - out_6387810172757071512[284] = 0.0; - out_6387810172757071512[285] = 1.0; - out_6387810172757071512[286] = 0.0; - out_6387810172757071512[287] = 0.0; - out_6387810172757071512[288] = 0.0; - out_6387810172757071512[289] = 0.0; - out_6387810172757071512[290] = 0.0; - out_6387810172757071512[291] = 0.0; - out_6387810172757071512[292] = 0.0; - out_6387810172757071512[293] = 0.0; - out_6387810172757071512[294] = 0.0; - out_6387810172757071512[295] = 0.0; - out_6387810172757071512[296] = 0.0; - out_6387810172757071512[297] = 0.0; - out_6387810172757071512[298] = 0.0; - out_6387810172757071512[299] = 0.0; - out_6387810172757071512[300] = 0.0; - out_6387810172757071512[301] = 0.0; - out_6387810172757071512[302] = 0.0; - out_6387810172757071512[303] = 0.0; - out_6387810172757071512[304] = 1.0; - out_6387810172757071512[305] = 0.0; - out_6387810172757071512[306] = 0.0; - out_6387810172757071512[307] = 0.0; - out_6387810172757071512[308] = 0.0; - out_6387810172757071512[309] = 0.0; - out_6387810172757071512[310] = 0.0; - out_6387810172757071512[311] = 0.0; - out_6387810172757071512[312] = 0.0; - out_6387810172757071512[313] = 0.0; - out_6387810172757071512[314] = 0.0; - out_6387810172757071512[315] = 0.0; - out_6387810172757071512[316] = 0.0; - out_6387810172757071512[317] = 0.0; - out_6387810172757071512[318] = 0.0; - out_6387810172757071512[319] = 0.0; - out_6387810172757071512[320] = 0.0; - out_6387810172757071512[321] = 0.0; - out_6387810172757071512[322] = 0.0; - out_6387810172757071512[323] = 1.0; +void H_mod_fun(double *state, double *out_2863715751586478773) { + out_2863715751586478773[0] = 1.0; + out_2863715751586478773[1] = 0.0; + out_2863715751586478773[2] = 0.0; + out_2863715751586478773[3] = 0.0; + out_2863715751586478773[4] = 0.0; + out_2863715751586478773[5] = 0.0; + out_2863715751586478773[6] = 0.0; + out_2863715751586478773[7] = 0.0; + out_2863715751586478773[8] = 0.0; + out_2863715751586478773[9] = 0.0; + out_2863715751586478773[10] = 0.0; + out_2863715751586478773[11] = 0.0; + out_2863715751586478773[12] = 0.0; + out_2863715751586478773[13] = 0.0; + out_2863715751586478773[14] = 0.0; + out_2863715751586478773[15] = 0.0; + out_2863715751586478773[16] = 0.0; + out_2863715751586478773[17] = 0.0; + out_2863715751586478773[18] = 0.0; + out_2863715751586478773[19] = 1.0; + out_2863715751586478773[20] = 0.0; + out_2863715751586478773[21] = 0.0; + out_2863715751586478773[22] = 0.0; + out_2863715751586478773[23] = 0.0; + out_2863715751586478773[24] = 0.0; + out_2863715751586478773[25] = 0.0; + out_2863715751586478773[26] = 0.0; + out_2863715751586478773[27] = 0.0; + out_2863715751586478773[28] = 0.0; + out_2863715751586478773[29] = 0.0; + out_2863715751586478773[30] = 0.0; + out_2863715751586478773[31] = 0.0; + out_2863715751586478773[32] = 0.0; + out_2863715751586478773[33] = 0.0; + out_2863715751586478773[34] = 0.0; + out_2863715751586478773[35] = 0.0; + out_2863715751586478773[36] = 0.0; + out_2863715751586478773[37] = 0.0; + out_2863715751586478773[38] = 1.0; + out_2863715751586478773[39] = 0.0; + out_2863715751586478773[40] = 0.0; + out_2863715751586478773[41] = 0.0; + out_2863715751586478773[42] = 0.0; + out_2863715751586478773[43] = 0.0; + out_2863715751586478773[44] = 0.0; + out_2863715751586478773[45] = 0.0; + out_2863715751586478773[46] = 0.0; + out_2863715751586478773[47] = 0.0; + out_2863715751586478773[48] = 0.0; + out_2863715751586478773[49] = 0.0; + out_2863715751586478773[50] = 0.0; + out_2863715751586478773[51] = 0.0; + out_2863715751586478773[52] = 0.0; + out_2863715751586478773[53] = 0.0; + out_2863715751586478773[54] = 0.0; + out_2863715751586478773[55] = 0.0; + out_2863715751586478773[56] = 0.0; + out_2863715751586478773[57] = 1.0; + out_2863715751586478773[58] = 0.0; + out_2863715751586478773[59] = 0.0; + out_2863715751586478773[60] = 0.0; + out_2863715751586478773[61] = 0.0; + out_2863715751586478773[62] = 0.0; + out_2863715751586478773[63] = 0.0; + out_2863715751586478773[64] = 0.0; + out_2863715751586478773[65] = 0.0; + out_2863715751586478773[66] = 0.0; + out_2863715751586478773[67] = 0.0; + out_2863715751586478773[68] = 0.0; + out_2863715751586478773[69] = 0.0; + out_2863715751586478773[70] = 0.0; + out_2863715751586478773[71] = 0.0; + out_2863715751586478773[72] = 0.0; + out_2863715751586478773[73] = 0.0; + out_2863715751586478773[74] = 0.0; + out_2863715751586478773[75] = 0.0; + out_2863715751586478773[76] = 1.0; + out_2863715751586478773[77] = 0.0; + out_2863715751586478773[78] = 0.0; + out_2863715751586478773[79] = 0.0; + out_2863715751586478773[80] = 0.0; + out_2863715751586478773[81] = 0.0; + out_2863715751586478773[82] = 0.0; + out_2863715751586478773[83] = 0.0; + out_2863715751586478773[84] = 0.0; + out_2863715751586478773[85] = 0.0; + out_2863715751586478773[86] = 0.0; + out_2863715751586478773[87] = 0.0; + out_2863715751586478773[88] = 0.0; + out_2863715751586478773[89] = 0.0; + out_2863715751586478773[90] = 0.0; + out_2863715751586478773[91] = 0.0; + out_2863715751586478773[92] = 0.0; + out_2863715751586478773[93] = 0.0; + out_2863715751586478773[94] = 0.0; + out_2863715751586478773[95] = 1.0; + out_2863715751586478773[96] = 0.0; + out_2863715751586478773[97] = 0.0; + out_2863715751586478773[98] = 0.0; + out_2863715751586478773[99] = 0.0; + out_2863715751586478773[100] = 0.0; + out_2863715751586478773[101] = 0.0; + out_2863715751586478773[102] = 0.0; + out_2863715751586478773[103] = 0.0; + out_2863715751586478773[104] = 0.0; + out_2863715751586478773[105] = 0.0; + out_2863715751586478773[106] = 0.0; + out_2863715751586478773[107] = 0.0; + out_2863715751586478773[108] = 0.0; + out_2863715751586478773[109] = 0.0; + out_2863715751586478773[110] = 0.0; + out_2863715751586478773[111] = 0.0; + out_2863715751586478773[112] = 0.0; + out_2863715751586478773[113] = 0.0; + out_2863715751586478773[114] = 1.0; + out_2863715751586478773[115] = 0.0; + out_2863715751586478773[116] = 0.0; + out_2863715751586478773[117] = 0.0; + out_2863715751586478773[118] = 0.0; + out_2863715751586478773[119] = 0.0; + out_2863715751586478773[120] = 0.0; + out_2863715751586478773[121] = 0.0; + out_2863715751586478773[122] = 0.0; + out_2863715751586478773[123] = 0.0; + out_2863715751586478773[124] = 0.0; + out_2863715751586478773[125] = 0.0; + out_2863715751586478773[126] = 0.0; + out_2863715751586478773[127] = 0.0; + out_2863715751586478773[128] = 0.0; + out_2863715751586478773[129] = 0.0; + out_2863715751586478773[130] = 0.0; + out_2863715751586478773[131] = 0.0; + out_2863715751586478773[132] = 0.0; + out_2863715751586478773[133] = 1.0; + out_2863715751586478773[134] = 0.0; + out_2863715751586478773[135] = 0.0; + out_2863715751586478773[136] = 0.0; + out_2863715751586478773[137] = 0.0; + out_2863715751586478773[138] = 0.0; + out_2863715751586478773[139] = 0.0; + out_2863715751586478773[140] = 0.0; + out_2863715751586478773[141] = 0.0; + out_2863715751586478773[142] = 0.0; + out_2863715751586478773[143] = 0.0; + out_2863715751586478773[144] = 0.0; + out_2863715751586478773[145] = 0.0; + out_2863715751586478773[146] = 0.0; + out_2863715751586478773[147] = 0.0; + out_2863715751586478773[148] = 0.0; + out_2863715751586478773[149] = 0.0; + out_2863715751586478773[150] = 0.0; + out_2863715751586478773[151] = 0.0; + out_2863715751586478773[152] = 1.0; + out_2863715751586478773[153] = 0.0; + out_2863715751586478773[154] = 0.0; + out_2863715751586478773[155] = 0.0; + out_2863715751586478773[156] = 0.0; + out_2863715751586478773[157] = 0.0; + out_2863715751586478773[158] = 0.0; + out_2863715751586478773[159] = 0.0; + out_2863715751586478773[160] = 0.0; + out_2863715751586478773[161] = 0.0; + out_2863715751586478773[162] = 0.0; + out_2863715751586478773[163] = 0.0; + out_2863715751586478773[164] = 0.0; + out_2863715751586478773[165] = 0.0; + out_2863715751586478773[166] = 0.0; + out_2863715751586478773[167] = 0.0; + out_2863715751586478773[168] = 0.0; + out_2863715751586478773[169] = 0.0; + out_2863715751586478773[170] = 0.0; + out_2863715751586478773[171] = 1.0; + out_2863715751586478773[172] = 0.0; + out_2863715751586478773[173] = 0.0; + out_2863715751586478773[174] = 0.0; + out_2863715751586478773[175] = 0.0; + out_2863715751586478773[176] = 0.0; + out_2863715751586478773[177] = 0.0; + out_2863715751586478773[178] = 0.0; + out_2863715751586478773[179] = 0.0; + out_2863715751586478773[180] = 0.0; + out_2863715751586478773[181] = 0.0; + out_2863715751586478773[182] = 0.0; + out_2863715751586478773[183] = 0.0; + out_2863715751586478773[184] = 0.0; + out_2863715751586478773[185] = 0.0; + out_2863715751586478773[186] = 0.0; + out_2863715751586478773[187] = 0.0; + out_2863715751586478773[188] = 0.0; + out_2863715751586478773[189] = 0.0; + out_2863715751586478773[190] = 1.0; + out_2863715751586478773[191] = 0.0; + out_2863715751586478773[192] = 0.0; + out_2863715751586478773[193] = 0.0; + out_2863715751586478773[194] = 0.0; + out_2863715751586478773[195] = 0.0; + out_2863715751586478773[196] = 0.0; + out_2863715751586478773[197] = 0.0; + out_2863715751586478773[198] = 0.0; + out_2863715751586478773[199] = 0.0; + out_2863715751586478773[200] = 0.0; + out_2863715751586478773[201] = 0.0; + out_2863715751586478773[202] = 0.0; + out_2863715751586478773[203] = 0.0; + out_2863715751586478773[204] = 0.0; + out_2863715751586478773[205] = 0.0; + out_2863715751586478773[206] = 0.0; + out_2863715751586478773[207] = 0.0; + out_2863715751586478773[208] = 0.0; + out_2863715751586478773[209] = 1.0; + out_2863715751586478773[210] = 0.0; + out_2863715751586478773[211] = 0.0; + out_2863715751586478773[212] = 0.0; + out_2863715751586478773[213] = 0.0; + out_2863715751586478773[214] = 0.0; + out_2863715751586478773[215] = 0.0; + out_2863715751586478773[216] = 0.0; + out_2863715751586478773[217] = 0.0; + out_2863715751586478773[218] = 0.0; + out_2863715751586478773[219] = 0.0; + out_2863715751586478773[220] = 0.0; + out_2863715751586478773[221] = 0.0; + out_2863715751586478773[222] = 0.0; + out_2863715751586478773[223] = 0.0; + out_2863715751586478773[224] = 0.0; + out_2863715751586478773[225] = 0.0; + out_2863715751586478773[226] = 0.0; + out_2863715751586478773[227] = 0.0; + out_2863715751586478773[228] = 1.0; + out_2863715751586478773[229] = 0.0; + out_2863715751586478773[230] = 0.0; + out_2863715751586478773[231] = 0.0; + out_2863715751586478773[232] = 0.0; + out_2863715751586478773[233] = 0.0; + out_2863715751586478773[234] = 0.0; + out_2863715751586478773[235] = 0.0; + out_2863715751586478773[236] = 0.0; + out_2863715751586478773[237] = 0.0; + out_2863715751586478773[238] = 0.0; + out_2863715751586478773[239] = 0.0; + out_2863715751586478773[240] = 0.0; + out_2863715751586478773[241] = 0.0; + out_2863715751586478773[242] = 0.0; + out_2863715751586478773[243] = 0.0; + out_2863715751586478773[244] = 0.0; + out_2863715751586478773[245] = 0.0; + out_2863715751586478773[246] = 0.0; + out_2863715751586478773[247] = 1.0; + out_2863715751586478773[248] = 0.0; + out_2863715751586478773[249] = 0.0; + out_2863715751586478773[250] = 0.0; + out_2863715751586478773[251] = 0.0; + out_2863715751586478773[252] = 0.0; + out_2863715751586478773[253] = 0.0; + out_2863715751586478773[254] = 0.0; + out_2863715751586478773[255] = 0.0; + out_2863715751586478773[256] = 0.0; + out_2863715751586478773[257] = 0.0; + out_2863715751586478773[258] = 0.0; + out_2863715751586478773[259] = 0.0; + out_2863715751586478773[260] = 0.0; + out_2863715751586478773[261] = 0.0; + out_2863715751586478773[262] = 0.0; + out_2863715751586478773[263] = 0.0; + out_2863715751586478773[264] = 0.0; + out_2863715751586478773[265] = 0.0; + out_2863715751586478773[266] = 1.0; + out_2863715751586478773[267] = 0.0; + out_2863715751586478773[268] = 0.0; + out_2863715751586478773[269] = 0.0; + out_2863715751586478773[270] = 0.0; + out_2863715751586478773[271] = 0.0; + out_2863715751586478773[272] = 0.0; + out_2863715751586478773[273] = 0.0; + out_2863715751586478773[274] = 0.0; + out_2863715751586478773[275] = 0.0; + out_2863715751586478773[276] = 0.0; + out_2863715751586478773[277] = 0.0; + out_2863715751586478773[278] = 0.0; + out_2863715751586478773[279] = 0.0; + out_2863715751586478773[280] = 0.0; + out_2863715751586478773[281] = 0.0; + out_2863715751586478773[282] = 0.0; + out_2863715751586478773[283] = 0.0; + out_2863715751586478773[284] = 0.0; + out_2863715751586478773[285] = 1.0; + out_2863715751586478773[286] = 0.0; + out_2863715751586478773[287] = 0.0; + out_2863715751586478773[288] = 0.0; + out_2863715751586478773[289] = 0.0; + out_2863715751586478773[290] = 0.0; + out_2863715751586478773[291] = 0.0; + out_2863715751586478773[292] = 0.0; + out_2863715751586478773[293] = 0.0; + out_2863715751586478773[294] = 0.0; + out_2863715751586478773[295] = 0.0; + out_2863715751586478773[296] = 0.0; + out_2863715751586478773[297] = 0.0; + out_2863715751586478773[298] = 0.0; + out_2863715751586478773[299] = 0.0; + out_2863715751586478773[300] = 0.0; + out_2863715751586478773[301] = 0.0; + out_2863715751586478773[302] = 0.0; + out_2863715751586478773[303] = 0.0; + out_2863715751586478773[304] = 1.0; + out_2863715751586478773[305] = 0.0; + out_2863715751586478773[306] = 0.0; + out_2863715751586478773[307] = 0.0; + out_2863715751586478773[308] = 0.0; + out_2863715751586478773[309] = 0.0; + out_2863715751586478773[310] = 0.0; + out_2863715751586478773[311] = 0.0; + out_2863715751586478773[312] = 0.0; + out_2863715751586478773[313] = 0.0; + out_2863715751586478773[314] = 0.0; + out_2863715751586478773[315] = 0.0; + out_2863715751586478773[316] = 0.0; + out_2863715751586478773[317] = 0.0; + out_2863715751586478773[318] = 0.0; + out_2863715751586478773[319] = 0.0; + out_2863715751586478773[320] = 0.0; + out_2863715751586478773[321] = 0.0; + out_2863715751586478773[322] = 0.0; + out_2863715751586478773[323] = 1.0; } -void f_fun(double *state, double dt, double *out_5190191951972352761) { - out_5190191951972352761[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_5190191951972352761[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_5190191951972352761[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_5190191951972352761[3] = dt*state[12] + state[3]; - out_5190191951972352761[4] = dt*state[13] + state[4]; - out_5190191951972352761[5] = dt*state[14] + state[5]; - out_5190191951972352761[6] = state[6]; - out_5190191951972352761[7] = state[7]; - out_5190191951972352761[8] = state[8]; - out_5190191951972352761[9] = state[9]; - out_5190191951972352761[10] = state[10]; - out_5190191951972352761[11] = state[11]; - out_5190191951972352761[12] = state[12]; - out_5190191951972352761[13] = state[13]; - out_5190191951972352761[14] = state[14]; - out_5190191951972352761[15] = state[15]; - out_5190191951972352761[16] = state[16]; - out_5190191951972352761[17] = state[17]; +void f_fun(double *state, double dt, double *out_4163803070138990822) { + out_4163803070138990822[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_4163803070138990822[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_4163803070138990822[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_4163803070138990822[3] = dt*state[12] + state[3]; + out_4163803070138990822[4] = dt*state[13] + state[4]; + out_4163803070138990822[5] = dt*state[14] + state[5]; + out_4163803070138990822[6] = state[6]; + out_4163803070138990822[7] = state[7]; + out_4163803070138990822[8] = state[8]; + out_4163803070138990822[9] = state[9]; + out_4163803070138990822[10] = state[10]; + out_4163803070138990822[11] = state[11]; + out_4163803070138990822[12] = state[12]; + out_4163803070138990822[13] = state[13]; + out_4163803070138990822[14] = state[14]; + out_4163803070138990822[15] = state[15]; + out_4163803070138990822[16] = state[16]; + out_4163803070138990822[17] = state[17]; } -void F_fun(double *state, double dt, double *out_3425534515038953406) { - out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[2] = 0; - out_3425534515038953406[3] = 0; - out_3425534515038953406[4] = 0; - out_3425534515038953406[5] = 0; - out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[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_3425534515038953406[9] = 0; - out_3425534515038953406[10] = 0; - out_3425534515038953406[11] = 0; - out_3425534515038953406[12] = 0; - out_3425534515038953406[13] = 0; - out_3425534515038953406[14] = 0; - out_3425534515038953406[15] = 0; - out_3425534515038953406[16] = 0; - out_3425534515038953406[17] = 0; - out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[20] = 0; - out_3425534515038953406[21] = 0; - out_3425534515038953406[22] = 0; - out_3425534515038953406[23] = 0; - out_3425534515038953406[24] = 0; - out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[27] = 0; - out_3425534515038953406[28] = 0; - out_3425534515038953406[29] = 0; - out_3425534515038953406[30] = 0; - out_3425534515038953406[31] = 0; - out_3425534515038953406[32] = 0; - out_3425534515038953406[33] = 0; - out_3425534515038953406[34] = 0; - out_3425534515038953406[35] = 0; - out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[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_3425534515038953406[39] = 0; - out_3425534515038953406[40] = 0; - out_3425534515038953406[41] = 0; - out_3425534515038953406[42] = 0; - out_3425534515038953406[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_3425534515038953406[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_3425534515038953406[45] = 0; - out_3425534515038953406[46] = 0; - out_3425534515038953406[47] = 0; - out_3425534515038953406[48] = 0; - out_3425534515038953406[49] = 0; - out_3425534515038953406[50] = 0; - out_3425534515038953406[51] = 0; - out_3425534515038953406[52] = 0; - out_3425534515038953406[53] = 0; - out_3425534515038953406[54] = 0; - out_3425534515038953406[55] = 0; - out_3425534515038953406[56] = 0; - out_3425534515038953406[57] = 1; - out_3425534515038953406[58] = 0; - out_3425534515038953406[59] = 0; - out_3425534515038953406[60] = 0; - out_3425534515038953406[61] = 0; - out_3425534515038953406[62] = 0; - out_3425534515038953406[63] = 0; - out_3425534515038953406[64] = 0; - out_3425534515038953406[65] = 0; - out_3425534515038953406[66] = dt; - out_3425534515038953406[67] = 0; - out_3425534515038953406[68] = 0; - out_3425534515038953406[69] = 0; - out_3425534515038953406[70] = 0; - out_3425534515038953406[71] = 0; - out_3425534515038953406[72] = 0; - out_3425534515038953406[73] = 0; - out_3425534515038953406[74] = 0; - out_3425534515038953406[75] = 0; - out_3425534515038953406[76] = 1; - out_3425534515038953406[77] = 0; - out_3425534515038953406[78] = 0; - out_3425534515038953406[79] = 0; - out_3425534515038953406[80] = 0; - out_3425534515038953406[81] = 0; - out_3425534515038953406[82] = 0; - out_3425534515038953406[83] = 0; - out_3425534515038953406[84] = 0; - out_3425534515038953406[85] = dt; - out_3425534515038953406[86] = 0; - out_3425534515038953406[87] = 0; - out_3425534515038953406[88] = 0; - out_3425534515038953406[89] = 0; - out_3425534515038953406[90] = 0; - out_3425534515038953406[91] = 0; - out_3425534515038953406[92] = 0; - out_3425534515038953406[93] = 0; - out_3425534515038953406[94] = 0; - out_3425534515038953406[95] = 1; - out_3425534515038953406[96] = 0; - out_3425534515038953406[97] = 0; - out_3425534515038953406[98] = 0; - out_3425534515038953406[99] = 0; - out_3425534515038953406[100] = 0; - out_3425534515038953406[101] = 0; - out_3425534515038953406[102] = 0; - out_3425534515038953406[103] = 0; - out_3425534515038953406[104] = dt; - out_3425534515038953406[105] = 0; - out_3425534515038953406[106] = 0; - out_3425534515038953406[107] = 0; - out_3425534515038953406[108] = 0; - out_3425534515038953406[109] = 0; - out_3425534515038953406[110] = 0; - out_3425534515038953406[111] = 0; - out_3425534515038953406[112] = 0; - out_3425534515038953406[113] = 0; - out_3425534515038953406[114] = 1; - out_3425534515038953406[115] = 0; - out_3425534515038953406[116] = 0; - out_3425534515038953406[117] = 0; - out_3425534515038953406[118] = 0; - out_3425534515038953406[119] = 0; - out_3425534515038953406[120] = 0; - out_3425534515038953406[121] = 0; - out_3425534515038953406[122] = 0; - out_3425534515038953406[123] = 0; - out_3425534515038953406[124] = 0; - out_3425534515038953406[125] = 0; - out_3425534515038953406[126] = 0; - out_3425534515038953406[127] = 0; - out_3425534515038953406[128] = 0; - out_3425534515038953406[129] = 0; - out_3425534515038953406[130] = 0; - out_3425534515038953406[131] = 0; - out_3425534515038953406[132] = 0; - out_3425534515038953406[133] = 1; - out_3425534515038953406[134] = 0; - out_3425534515038953406[135] = 0; - out_3425534515038953406[136] = 0; - out_3425534515038953406[137] = 0; - out_3425534515038953406[138] = 0; - out_3425534515038953406[139] = 0; - out_3425534515038953406[140] = 0; - out_3425534515038953406[141] = 0; - out_3425534515038953406[142] = 0; - out_3425534515038953406[143] = 0; - out_3425534515038953406[144] = 0; - out_3425534515038953406[145] = 0; - out_3425534515038953406[146] = 0; - out_3425534515038953406[147] = 0; - out_3425534515038953406[148] = 0; - out_3425534515038953406[149] = 0; - out_3425534515038953406[150] = 0; - out_3425534515038953406[151] = 0; - out_3425534515038953406[152] = 1; - out_3425534515038953406[153] = 0; - out_3425534515038953406[154] = 0; - out_3425534515038953406[155] = 0; - out_3425534515038953406[156] = 0; - out_3425534515038953406[157] = 0; - out_3425534515038953406[158] = 0; - out_3425534515038953406[159] = 0; - out_3425534515038953406[160] = 0; - out_3425534515038953406[161] = 0; - out_3425534515038953406[162] = 0; - out_3425534515038953406[163] = 0; - out_3425534515038953406[164] = 0; - out_3425534515038953406[165] = 0; - out_3425534515038953406[166] = 0; - out_3425534515038953406[167] = 0; - out_3425534515038953406[168] = 0; - out_3425534515038953406[169] = 0; - out_3425534515038953406[170] = 0; - out_3425534515038953406[171] = 1; - out_3425534515038953406[172] = 0; - out_3425534515038953406[173] = 0; - out_3425534515038953406[174] = 0; - out_3425534515038953406[175] = 0; - out_3425534515038953406[176] = 0; - out_3425534515038953406[177] = 0; - out_3425534515038953406[178] = 0; - out_3425534515038953406[179] = 0; - out_3425534515038953406[180] = 0; - out_3425534515038953406[181] = 0; - out_3425534515038953406[182] = 0; - out_3425534515038953406[183] = 0; - out_3425534515038953406[184] = 0; - out_3425534515038953406[185] = 0; - out_3425534515038953406[186] = 0; - out_3425534515038953406[187] = 0; - out_3425534515038953406[188] = 0; - out_3425534515038953406[189] = 0; - out_3425534515038953406[190] = 1; - out_3425534515038953406[191] = 0; - out_3425534515038953406[192] = 0; - out_3425534515038953406[193] = 0; - out_3425534515038953406[194] = 0; - out_3425534515038953406[195] = 0; - out_3425534515038953406[196] = 0; - out_3425534515038953406[197] = 0; - out_3425534515038953406[198] = 0; - out_3425534515038953406[199] = 0; - out_3425534515038953406[200] = 0; - out_3425534515038953406[201] = 0; - out_3425534515038953406[202] = 0; - out_3425534515038953406[203] = 0; - out_3425534515038953406[204] = 0; - out_3425534515038953406[205] = 0; - out_3425534515038953406[206] = 0; - out_3425534515038953406[207] = 0; - out_3425534515038953406[208] = 0; - out_3425534515038953406[209] = 1; - out_3425534515038953406[210] = 0; - out_3425534515038953406[211] = 0; - out_3425534515038953406[212] = 0; - out_3425534515038953406[213] = 0; - out_3425534515038953406[214] = 0; - out_3425534515038953406[215] = 0; - out_3425534515038953406[216] = 0; - out_3425534515038953406[217] = 0; - out_3425534515038953406[218] = 0; - out_3425534515038953406[219] = 0; - out_3425534515038953406[220] = 0; - out_3425534515038953406[221] = 0; - out_3425534515038953406[222] = 0; - out_3425534515038953406[223] = 0; - out_3425534515038953406[224] = 0; - out_3425534515038953406[225] = 0; - out_3425534515038953406[226] = 0; - out_3425534515038953406[227] = 0; - out_3425534515038953406[228] = 1; - out_3425534515038953406[229] = 0; - out_3425534515038953406[230] = 0; - out_3425534515038953406[231] = 0; - out_3425534515038953406[232] = 0; - out_3425534515038953406[233] = 0; - out_3425534515038953406[234] = 0; - out_3425534515038953406[235] = 0; - out_3425534515038953406[236] = 0; - out_3425534515038953406[237] = 0; - out_3425534515038953406[238] = 0; - out_3425534515038953406[239] = 0; - out_3425534515038953406[240] = 0; - out_3425534515038953406[241] = 0; - out_3425534515038953406[242] = 0; - out_3425534515038953406[243] = 0; - out_3425534515038953406[244] = 0; - out_3425534515038953406[245] = 0; - out_3425534515038953406[246] = 0; - out_3425534515038953406[247] = 1; - out_3425534515038953406[248] = 0; - out_3425534515038953406[249] = 0; - out_3425534515038953406[250] = 0; - out_3425534515038953406[251] = 0; - out_3425534515038953406[252] = 0; - out_3425534515038953406[253] = 0; - out_3425534515038953406[254] = 0; - out_3425534515038953406[255] = 0; - out_3425534515038953406[256] = 0; - out_3425534515038953406[257] = 0; - out_3425534515038953406[258] = 0; - out_3425534515038953406[259] = 0; - out_3425534515038953406[260] = 0; - out_3425534515038953406[261] = 0; - out_3425534515038953406[262] = 0; - out_3425534515038953406[263] = 0; - out_3425534515038953406[264] = 0; - out_3425534515038953406[265] = 0; - out_3425534515038953406[266] = 1; - out_3425534515038953406[267] = 0; - out_3425534515038953406[268] = 0; - out_3425534515038953406[269] = 0; - out_3425534515038953406[270] = 0; - out_3425534515038953406[271] = 0; - out_3425534515038953406[272] = 0; - out_3425534515038953406[273] = 0; - out_3425534515038953406[274] = 0; - out_3425534515038953406[275] = 0; - out_3425534515038953406[276] = 0; - out_3425534515038953406[277] = 0; - out_3425534515038953406[278] = 0; - out_3425534515038953406[279] = 0; - out_3425534515038953406[280] = 0; - out_3425534515038953406[281] = 0; - out_3425534515038953406[282] = 0; - out_3425534515038953406[283] = 0; - out_3425534515038953406[284] = 0; - out_3425534515038953406[285] = 1; - out_3425534515038953406[286] = 0; - out_3425534515038953406[287] = 0; - out_3425534515038953406[288] = 0; - out_3425534515038953406[289] = 0; - out_3425534515038953406[290] = 0; - out_3425534515038953406[291] = 0; - out_3425534515038953406[292] = 0; - out_3425534515038953406[293] = 0; - out_3425534515038953406[294] = 0; - out_3425534515038953406[295] = 0; - out_3425534515038953406[296] = 0; - out_3425534515038953406[297] = 0; - out_3425534515038953406[298] = 0; - out_3425534515038953406[299] = 0; - out_3425534515038953406[300] = 0; - out_3425534515038953406[301] = 0; - out_3425534515038953406[302] = 0; - out_3425534515038953406[303] = 0; - out_3425534515038953406[304] = 1; - out_3425534515038953406[305] = 0; - out_3425534515038953406[306] = 0; - out_3425534515038953406[307] = 0; - out_3425534515038953406[308] = 0; - out_3425534515038953406[309] = 0; - out_3425534515038953406[310] = 0; - out_3425534515038953406[311] = 0; - out_3425534515038953406[312] = 0; - out_3425534515038953406[313] = 0; - out_3425534515038953406[314] = 0; - out_3425534515038953406[315] = 0; - out_3425534515038953406[316] = 0; - out_3425534515038953406[317] = 0; - out_3425534515038953406[318] = 0; - out_3425534515038953406[319] = 0; - out_3425534515038953406[320] = 0; - out_3425534515038953406[321] = 0; - out_3425534515038953406[322] = 0; - out_3425534515038953406[323] = 1; +void F_fun(double *state, double dt, double *out_3325085581577154625) { + out_3325085581577154625[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_3325085581577154625[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_3325085581577154625[2] = 0; + out_3325085581577154625[3] = 0; + out_3325085581577154625[4] = 0; + out_3325085581577154625[5] = 0; + out_3325085581577154625[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_3325085581577154625[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_3325085581577154625[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_3325085581577154625[9] = 0; + out_3325085581577154625[10] = 0; + out_3325085581577154625[11] = 0; + out_3325085581577154625[12] = 0; + out_3325085581577154625[13] = 0; + out_3325085581577154625[14] = 0; + out_3325085581577154625[15] = 0; + out_3325085581577154625[16] = 0; + out_3325085581577154625[17] = 0; + out_3325085581577154625[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_3325085581577154625[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_3325085581577154625[20] = 0; + out_3325085581577154625[21] = 0; + out_3325085581577154625[22] = 0; + out_3325085581577154625[23] = 0; + out_3325085581577154625[24] = 0; + out_3325085581577154625[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_3325085581577154625[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_3325085581577154625[27] = 0; + out_3325085581577154625[28] = 0; + out_3325085581577154625[29] = 0; + out_3325085581577154625[30] = 0; + out_3325085581577154625[31] = 0; + out_3325085581577154625[32] = 0; + out_3325085581577154625[33] = 0; + out_3325085581577154625[34] = 0; + out_3325085581577154625[35] = 0; + out_3325085581577154625[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_3325085581577154625[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_3325085581577154625[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_3325085581577154625[39] = 0; + out_3325085581577154625[40] = 0; + out_3325085581577154625[41] = 0; + out_3325085581577154625[42] = 0; + out_3325085581577154625[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_3325085581577154625[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_3325085581577154625[45] = 0; + out_3325085581577154625[46] = 0; + out_3325085581577154625[47] = 0; + out_3325085581577154625[48] = 0; + out_3325085581577154625[49] = 0; + out_3325085581577154625[50] = 0; + out_3325085581577154625[51] = 0; + out_3325085581577154625[52] = 0; + out_3325085581577154625[53] = 0; + out_3325085581577154625[54] = 0; + out_3325085581577154625[55] = 0; + out_3325085581577154625[56] = 0; + out_3325085581577154625[57] = 1; + out_3325085581577154625[58] = 0; + out_3325085581577154625[59] = 0; + out_3325085581577154625[60] = 0; + out_3325085581577154625[61] = 0; + out_3325085581577154625[62] = 0; + out_3325085581577154625[63] = 0; + out_3325085581577154625[64] = 0; + out_3325085581577154625[65] = 0; + out_3325085581577154625[66] = dt; + out_3325085581577154625[67] = 0; + out_3325085581577154625[68] = 0; + out_3325085581577154625[69] = 0; + out_3325085581577154625[70] = 0; + out_3325085581577154625[71] = 0; + out_3325085581577154625[72] = 0; + out_3325085581577154625[73] = 0; + out_3325085581577154625[74] = 0; + out_3325085581577154625[75] = 0; + out_3325085581577154625[76] = 1; + out_3325085581577154625[77] = 0; + out_3325085581577154625[78] = 0; + out_3325085581577154625[79] = 0; + out_3325085581577154625[80] = 0; + out_3325085581577154625[81] = 0; + out_3325085581577154625[82] = 0; + out_3325085581577154625[83] = 0; + out_3325085581577154625[84] = 0; + out_3325085581577154625[85] = dt; + out_3325085581577154625[86] = 0; + out_3325085581577154625[87] = 0; + out_3325085581577154625[88] = 0; + out_3325085581577154625[89] = 0; + out_3325085581577154625[90] = 0; + out_3325085581577154625[91] = 0; + out_3325085581577154625[92] = 0; + out_3325085581577154625[93] = 0; + out_3325085581577154625[94] = 0; + out_3325085581577154625[95] = 1; + out_3325085581577154625[96] = 0; + out_3325085581577154625[97] = 0; + out_3325085581577154625[98] = 0; + out_3325085581577154625[99] = 0; + out_3325085581577154625[100] = 0; + out_3325085581577154625[101] = 0; + out_3325085581577154625[102] = 0; + out_3325085581577154625[103] = 0; + out_3325085581577154625[104] = dt; + out_3325085581577154625[105] = 0; + out_3325085581577154625[106] = 0; + out_3325085581577154625[107] = 0; + out_3325085581577154625[108] = 0; + out_3325085581577154625[109] = 0; + out_3325085581577154625[110] = 0; + out_3325085581577154625[111] = 0; + out_3325085581577154625[112] = 0; + out_3325085581577154625[113] = 0; + out_3325085581577154625[114] = 1; + out_3325085581577154625[115] = 0; + out_3325085581577154625[116] = 0; + out_3325085581577154625[117] = 0; + out_3325085581577154625[118] = 0; + out_3325085581577154625[119] = 0; + out_3325085581577154625[120] = 0; + out_3325085581577154625[121] = 0; + out_3325085581577154625[122] = 0; + out_3325085581577154625[123] = 0; + out_3325085581577154625[124] = 0; + out_3325085581577154625[125] = 0; + out_3325085581577154625[126] = 0; + out_3325085581577154625[127] = 0; + out_3325085581577154625[128] = 0; + out_3325085581577154625[129] = 0; + out_3325085581577154625[130] = 0; + out_3325085581577154625[131] = 0; + out_3325085581577154625[132] = 0; + out_3325085581577154625[133] = 1; + out_3325085581577154625[134] = 0; + out_3325085581577154625[135] = 0; + out_3325085581577154625[136] = 0; + out_3325085581577154625[137] = 0; + out_3325085581577154625[138] = 0; + out_3325085581577154625[139] = 0; + out_3325085581577154625[140] = 0; + out_3325085581577154625[141] = 0; + out_3325085581577154625[142] = 0; + out_3325085581577154625[143] = 0; + out_3325085581577154625[144] = 0; + out_3325085581577154625[145] = 0; + out_3325085581577154625[146] = 0; + out_3325085581577154625[147] = 0; + out_3325085581577154625[148] = 0; + out_3325085581577154625[149] = 0; + out_3325085581577154625[150] = 0; + out_3325085581577154625[151] = 0; + out_3325085581577154625[152] = 1; + out_3325085581577154625[153] = 0; + out_3325085581577154625[154] = 0; + out_3325085581577154625[155] = 0; + out_3325085581577154625[156] = 0; + out_3325085581577154625[157] = 0; + out_3325085581577154625[158] = 0; + out_3325085581577154625[159] = 0; + out_3325085581577154625[160] = 0; + out_3325085581577154625[161] = 0; + out_3325085581577154625[162] = 0; + out_3325085581577154625[163] = 0; + out_3325085581577154625[164] = 0; + out_3325085581577154625[165] = 0; + out_3325085581577154625[166] = 0; + out_3325085581577154625[167] = 0; + out_3325085581577154625[168] = 0; + out_3325085581577154625[169] = 0; + out_3325085581577154625[170] = 0; + out_3325085581577154625[171] = 1; + out_3325085581577154625[172] = 0; + out_3325085581577154625[173] = 0; + out_3325085581577154625[174] = 0; + out_3325085581577154625[175] = 0; + out_3325085581577154625[176] = 0; + out_3325085581577154625[177] = 0; + out_3325085581577154625[178] = 0; + out_3325085581577154625[179] = 0; + out_3325085581577154625[180] = 0; + out_3325085581577154625[181] = 0; + out_3325085581577154625[182] = 0; + out_3325085581577154625[183] = 0; + out_3325085581577154625[184] = 0; + out_3325085581577154625[185] = 0; + out_3325085581577154625[186] = 0; + out_3325085581577154625[187] = 0; + out_3325085581577154625[188] = 0; + out_3325085581577154625[189] = 0; + out_3325085581577154625[190] = 1; + out_3325085581577154625[191] = 0; + out_3325085581577154625[192] = 0; + out_3325085581577154625[193] = 0; + out_3325085581577154625[194] = 0; + out_3325085581577154625[195] = 0; + out_3325085581577154625[196] = 0; + out_3325085581577154625[197] = 0; + out_3325085581577154625[198] = 0; + out_3325085581577154625[199] = 0; + out_3325085581577154625[200] = 0; + out_3325085581577154625[201] = 0; + out_3325085581577154625[202] = 0; + out_3325085581577154625[203] = 0; + out_3325085581577154625[204] = 0; + out_3325085581577154625[205] = 0; + out_3325085581577154625[206] = 0; + out_3325085581577154625[207] = 0; + out_3325085581577154625[208] = 0; + out_3325085581577154625[209] = 1; + out_3325085581577154625[210] = 0; + out_3325085581577154625[211] = 0; + out_3325085581577154625[212] = 0; + out_3325085581577154625[213] = 0; + out_3325085581577154625[214] = 0; + out_3325085581577154625[215] = 0; + out_3325085581577154625[216] = 0; + out_3325085581577154625[217] = 0; + out_3325085581577154625[218] = 0; + out_3325085581577154625[219] = 0; + out_3325085581577154625[220] = 0; + out_3325085581577154625[221] = 0; + out_3325085581577154625[222] = 0; + out_3325085581577154625[223] = 0; + out_3325085581577154625[224] = 0; + out_3325085581577154625[225] = 0; + out_3325085581577154625[226] = 0; + out_3325085581577154625[227] = 0; + out_3325085581577154625[228] = 1; + out_3325085581577154625[229] = 0; + out_3325085581577154625[230] = 0; + out_3325085581577154625[231] = 0; + out_3325085581577154625[232] = 0; + out_3325085581577154625[233] = 0; + out_3325085581577154625[234] = 0; + out_3325085581577154625[235] = 0; + out_3325085581577154625[236] = 0; + out_3325085581577154625[237] = 0; + out_3325085581577154625[238] = 0; + out_3325085581577154625[239] = 0; + out_3325085581577154625[240] = 0; + out_3325085581577154625[241] = 0; + out_3325085581577154625[242] = 0; + out_3325085581577154625[243] = 0; + out_3325085581577154625[244] = 0; + out_3325085581577154625[245] = 0; + out_3325085581577154625[246] = 0; + out_3325085581577154625[247] = 1; + out_3325085581577154625[248] = 0; + out_3325085581577154625[249] = 0; + out_3325085581577154625[250] = 0; + out_3325085581577154625[251] = 0; + out_3325085581577154625[252] = 0; + out_3325085581577154625[253] = 0; + out_3325085581577154625[254] = 0; + out_3325085581577154625[255] = 0; + out_3325085581577154625[256] = 0; + out_3325085581577154625[257] = 0; + out_3325085581577154625[258] = 0; + out_3325085581577154625[259] = 0; + out_3325085581577154625[260] = 0; + out_3325085581577154625[261] = 0; + out_3325085581577154625[262] = 0; + out_3325085581577154625[263] = 0; + out_3325085581577154625[264] = 0; + out_3325085581577154625[265] = 0; + out_3325085581577154625[266] = 1; + out_3325085581577154625[267] = 0; + out_3325085581577154625[268] = 0; + out_3325085581577154625[269] = 0; + out_3325085581577154625[270] = 0; + out_3325085581577154625[271] = 0; + out_3325085581577154625[272] = 0; + out_3325085581577154625[273] = 0; + out_3325085581577154625[274] = 0; + out_3325085581577154625[275] = 0; + out_3325085581577154625[276] = 0; + out_3325085581577154625[277] = 0; + out_3325085581577154625[278] = 0; + out_3325085581577154625[279] = 0; + out_3325085581577154625[280] = 0; + out_3325085581577154625[281] = 0; + out_3325085581577154625[282] = 0; + out_3325085581577154625[283] = 0; + out_3325085581577154625[284] = 0; + out_3325085581577154625[285] = 1; + out_3325085581577154625[286] = 0; + out_3325085581577154625[287] = 0; + out_3325085581577154625[288] = 0; + out_3325085581577154625[289] = 0; + out_3325085581577154625[290] = 0; + out_3325085581577154625[291] = 0; + out_3325085581577154625[292] = 0; + out_3325085581577154625[293] = 0; + out_3325085581577154625[294] = 0; + out_3325085581577154625[295] = 0; + out_3325085581577154625[296] = 0; + out_3325085581577154625[297] = 0; + out_3325085581577154625[298] = 0; + out_3325085581577154625[299] = 0; + out_3325085581577154625[300] = 0; + out_3325085581577154625[301] = 0; + out_3325085581577154625[302] = 0; + out_3325085581577154625[303] = 0; + out_3325085581577154625[304] = 1; + out_3325085581577154625[305] = 0; + out_3325085581577154625[306] = 0; + out_3325085581577154625[307] = 0; + out_3325085581577154625[308] = 0; + out_3325085581577154625[309] = 0; + out_3325085581577154625[310] = 0; + out_3325085581577154625[311] = 0; + out_3325085581577154625[312] = 0; + out_3325085581577154625[313] = 0; + out_3325085581577154625[314] = 0; + out_3325085581577154625[315] = 0; + out_3325085581577154625[316] = 0; + out_3325085581577154625[317] = 0; + out_3325085581577154625[318] = 0; + out_3325085581577154625[319] = 0; + out_3325085581577154625[320] = 0; + out_3325085581577154625[321] = 0; + out_3325085581577154625[322] = 0; + out_3325085581577154625[323] = 1; } -void h_4(double *state, double *unused, double *out_7576152197490450413) { - out_7576152197490450413[0] = state[6] + state[9]; - out_7576152197490450413[1] = state[7] + state[10]; - out_7576152197490450413[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_1824245269356126422) { + out_1824245269356126422[0] = state[6] + state[9]; + out_1824245269356126422[1] = state[7] + state[10]; + out_1824245269356126422[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_3320737668869490966) { - out_3320737668869490966[0] = 0; - out_3320737668869490966[1] = 0; - out_3320737668869490966[2] = 0; - out_3320737668869490966[3] = 0; - out_3320737668869490966[4] = 0; - out_3320737668869490966[5] = 0; - out_3320737668869490966[6] = 1; - out_3320737668869490966[7] = 0; - out_3320737668869490966[8] = 0; - out_3320737668869490966[9] = 1; - out_3320737668869490966[10] = 0; - out_3320737668869490966[11] = 0; - out_3320737668869490966[12] = 0; - out_3320737668869490966[13] = 0; - out_3320737668869490966[14] = 0; - out_3320737668869490966[15] = 0; - out_3320737668869490966[16] = 0; - out_3320737668869490966[17] = 0; - out_3320737668869490966[18] = 0; - out_3320737668869490966[19] = 0; - out_3320737668869490966[20] = 0; - out_3320737668869490966[21] = 0; - out_3320737668869490966[22] = 0; - out_3320737668869490966[23] = 0; - out_3320737668869490966[24] = 0; - out_3320737668869490966[25] = 1; - out_3320737668869490966[26] = 0; - out_3320737668869490966[27] = 0; - out_3320737668869490966[28] = 1; - out_3320737668869490966[29] = 0; - out_3320737668869490966[30] = 0; - out_3320737668869490966[31] = 0; - out_3320737668869490966[32] = 0; - out_3320737668869490966[33] = 0; - out_3320737668869490966[34] = 0; - out_3320737668869490966[35] = 0; - out_3320737668869490966[36] = 0; - out_3320737668869490966[37] = 0; - out_3320737668869490966[38] = 0; - out_3320737668869490966[39] = 0; - out_3320737668869490966[40] = 0; - out_3320737668869490966[41] = 0; - out_3320737668869490966[42] = 0; - out_3320737668869490966[43] = 0; - out_3320737668869490966[44] = 1; - out_3320737668869490966[45] = 0; - out_3320737668869490966[46] = 0; - out_3320737668869490966[47] = 1; - out_3320737668869490966[48] = 0; - out_3320737668869490966[49] = 0; - out_3320737668869490966[50] = 0; - out_3320737668869490966[51] = 0; - out_3320737668869490966[52] = 0; - out_3320737668869490966[53] = 0; +void H_4(double *state, double *unused, double *out_908091458257428799) { + out_908091458257428799[0] = 0; + out_908091458257428799[1] = 0; + out_908091458257428799[2] = 0; + out_908091458257428799[3] = 0; + out_908091458257428799[4] = 0; + out_908091458257428799[5] = 0; + out_908091458257428799[6] = 1; + out_908091458257428799[7] = 0; + out_908091458257428799[8] = 0; + out_908091458257428799[9] = 1; + out_908091458257428799[10] = 0; + out_908091458257428799[11] = 0; + out_908091458257428799[12] = 0; + out_908091458257428799[13] = 0; + out_908091458257428799[14] = 0; + out_908091458257428799[15] = 0; + out_908091458257428799[16] = 0; + out_908091458257428799[17] = 0; + out_908091458257428799[18] = 0; + out_908091458257428799[19] = 0; + out_908091458257428799[20] = 0; + out_908091458257428799[21] = 0; + out_908091458257428799[22] = 0; + out_908091458257428799[23] = 0; + out_908091458257428799[24] = 0; + out_908091458257428799[25] = 1; + out_908091458257428799[26] = 0; + out_908091458257428799[27] = 0; + out_908091458257428799[28] = 1; + out_908091458257428799[29] = 0; + out_908091458257428799[30] = 0; + out_908091458257428799[31] = 0; + out_908091458257428799[32] = 0; + out_908091458257428799[33] = 0; + out_908091458257428799[34] = 0; + out_908091458257428799[35] = 0; + out_908091458257428799[36] = 0; + out_908091458257428799[37] = 0; + out_908091458257428799[38] = 0; + out_908091458257428799[39] = 0; + out_908091458257428799[40] = 0; + out_908091458257428799[41] = 0; + out_908091458257428799[42] = 0; + out_908091458257428799[43] = 0; + out_908091458257428799[44] = 1; + out_908091458257428799[45] = 0; + out_908091458257428799[46] = 0; + out_908091458257428799[47] = 1; + out_908091458257428799[48] = 0; + out_908091458257428799[49] = 0; + out_908091458257428799[50] = 0; + out_908091458257428799[51] = 0; + out_908091458257428799[52] = 0; + out_908091458257428799[53] = 0; } -void h_10(double *state, double *unused, double *out_3130452913144701320) { - out_3130452913144701320[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_3130452913144701320[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_3130452913144701320[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_5992945801677381357) { + out_5992945801677381357[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_5992945801677381357[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_5992945801677381357[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_2096220204218508864) { - out_2096220204218508864[0] = 0; - out_2096220204218508864[1] = 9.8100000000000005*cos(state[1]); - out_2096220204218508864[2] = 0; - out_2096220204218508864[3] = 0; - out_2096220204218508864[4] = -state[8]; - out_2096220204218508864[5] = state[7]; - out_2096220204218508864[6] = 0; - out_2096220204218508864[7] = state[5]; - out_2096220204218508864[8] = -state[4]; - out_2096220204218508864[9] = 0; - out_2096220204218508864[10] = 0; - out_2096220204218508864[11] = 0; - out_2096220204218508864[12] = 1; - out_2096220204218508864[13] = 0; - out_2096220204218508864[14] = 0; - out_2096220204218508864[15] = 1; - out_2096220204218508864[16] = 0; - out_2096220204218508864[17] = 0; - out_2096220204218508864[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_2096220204218508864[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_2096220204218508864[20] = 0; - out_2096220204218508864[21] = state[8]; - out_2096220204218508864[22] = 0; - out_2096220204218508864[23] = -state[6]; - out_2096220204218508864[24] = -state[5]; - out_2096220204218508864[25] = 0; - out_2096220204218508864[26] = state[3]; - out_2096220204218508864[27] = 0; - out_2096220204218508864[28] = 0; - out_2096220204218508864[29] = 0; - out_2096220204218508864[30] = 0; - out_2096220204218508864[31] = 1; - out_2096220204218508864[32] = 0; - out_2096220204218508864[33] = 0; - out_2096220204218508864[34] = 1; - out_2096220204218508864[35] = 0; - out_2096220204218508864[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_2096220204218508864[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_2096220204218508864[38] = 0; - out_2096220204218508864[39] = -state[7]; - out_2096220204218508864[40] = state[6]; - out_2096220204218508864[41] = 0; - out_2096220204218508864[42] = state[4]; - out_2096220204218508864[43] = -state[3]; - out_2096220204218508864[44] = 0; - out_2096220204218508864[45] = 0; - out_2096220204218508864[46] = 0; - out_2096220204218508864[47] = 0; - out_2096220204218508864[48] = 0; - out_2096220204218508864[49] = 0; - out_2096220204218508864[50] = 1; - out_2096220204218508864[51] = 0; - out_2096220204218508864[52] = 0; - out_2096220204218508864[53] = 1; +void H_10(double *state, double *unused, double *out_1551783764236526040) { + out_1551783764236526040[0] = 0; + out_1551783764236526040[1] = 9.8100000000000005*cos(state[1]); + out_1551783764236526040[2] = 0; + out_1551783764236526040[3] = 0; + out_1551783764236526040[4] = -state[8]; + out_1551783764236526040[5] = state[7]; + out_1551783764236526040[6] = 0; + out_1551783764236526040[7] = state[5]; + out_1551783764236526040[8] = -state[4]; + out_1551783764236526040[9] = 0; + out_1551783764236526040[10] = 0; + out_1551783764236526040[11] = 0; + out_1551783764236526040[12] = 1; + out_1551783764236526040[13] = 0; + out_1551783764236526040[14] = 0; + out_1551783764236526040[15] = 1; + out_1551783764236526040[16] = 0; + out_1551783764236526040[17] = 0; + out_1551783764236526040[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_1551783764236526040[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_1551783764236526040[20] = 0; + out_1551783764236526040[21] = state[8]; + out_1551783764236526040[22] = 0; + out_1551783764236526040[23] = -state[6]; + out_1551783764236526040[24] = -state[5]; + out_1551783764236526040[25] = 0; + out_1551783764236526040[26] = state[3]; + out_1551783764236526040[27] = 0; + out_1551783764236526040[28] = 0; + out_1551783764236526040[29] = 0; + out_1551783764236526040[30] = 0; + out_1551783764236526040[31] = 1; + out_1551783764236526040[32] = 0; + out_1551783764236526040[33] = 0; + out_1551783764236526040[34] = 1; + out_1551783764236526040[35] = 0; + out_1551783764236526040[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_1551783764236526040[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_1551783764236526040[38] = 0; + out_1551783764236526040[39] = -state[7]; + out_1551783764236526040[40] = state[6]; + out_1551783764236526040[41] = 0; + out_1551783764236526040[42] = state[4]; + out_1551783764236526040[43] = -state[3]; + out_1551783764236526040[44] = 0; + out_1551783764236526040[45] = 0; + out_1551783764236526040[46] = 0; + out_1551783764236526040[47] = 0; + out_1551783764236526040[48] = 0; + out_1551783764236526040[49] = 0; + out_1551783764236526040[50] = 1; + out_1551783764236526040[51] = 0; + out_1551783764236526040[52] = 0; + out_1551783764236526040[53] = 1; } -void h_13(double *state, double *unused, double *out_8767951916852656852) { - out_8767951916852656852[0] = state[3]; - out_8767951916852656852[1] = state[4]; - out_8767951916852656852[2] = state[5]; +void h_13(double *state, double *unused, double *out_4650572533653388734) { + out_4650572533653388734[0] = state[3]; + out_4650572533653388734[1] = state[4]; + out_4650572533653388734[2] = state[5]; } -void H_13(double *state, double *unused, double *out_108463843537158165) { - out_108463843537158165[0] = 0; - out_108463843537158165[1] = 0; - out_108463843537158165[2] = 0; - out_108463843537158165[3] = 1; - out_108463843537158165[4] = 0; - out_108463843537158165[5] = 0; - out_108463843537158165[6] = 0; - out_108463843537158165[7] = 0; - out_108463843537158165[8] = 0; - out_108463843537158165[9] = 0; - out_108463843537158165[10] = 0; - out_108463843537158165[11] = 0; - out_108463843537158165[12] = 0; - out_108463843537158165[13] = 0; - out_108463843537158165[14] = 0; - out_108463843537158165[15] = 0; - out_108463843537158165[16] = 0; - out_108463843537158165[17] = 0; - out_108463843537158165[18] = 0; - out_108463843537158165[19] = 0; - out_108463843537158165[20] = 0; - out_108463843537158165[21] = 0; - out_108463843537158165[22] = 1; - out_108463843537158165[23] = 0; - out_108463843537158165[24] = 0; - out_108463843537158165[25] = 0; - out_108463843537158165[26] = 0; - out_108463843537158165[27] = 0; - out_108463843537158165[28] = 0; - out_108463843537158165[29] = 0; - out_108463843537158165[30] = 0; - out_108463843537158165[31] = 0; - out_108463843537158165[32] = 0; - out_108463843537158165[33] = 0; - out_108463843537158165[34] = 0; - out_108463843537158165[35] = 0; - out_108463843537158165[36] = 0; - out_108463843537158165[37] = 0; - out_108463843537158165[38] = 0; - out_108463843537158165[39] = 0; - out_108463843537158165[40] = 0; - out_108463843537158165[41] = 1; - out_108463843537158165[42] = 0; - out_108463843537158165[43] = 0; - out_108463843537158165[44] = 0; - out_108463843537158165[45] = 0; - out_108463843537158165[46] = 0; - out_108463843537158165[47] = 0; - out_108463843537158165[48] = 0; - out_108463843537158165[49] = 0; - out_108463843537158165[50] = 0; - out_108463843537158165[51] = 0; - out_108463843537158165[52] = 0; - out_108463843537158165[53] = 0; +void H_13(double *state, double *unused, double *out_1472693377939272903) { + out_1472693377939272903[0] = 0; + out_1472693377939272903[1] = 0; + out_1472693377939272903[2] = 0; + out_1472693377939272903[3] = 1; + out_1472693377939272903[4] = 0; + out_1472693377939272903[5] = 0; + out_1472693377939272903[6] = 0; + out_1472693377939272903[7] = 0; + out_1472693377939272903[8] = 0; + out_1472693377939272903[9] = 0; + out_1472693377939272903[10] = 0; + out_1472693377939272903[11] = 0; + out_1472693377939272903[12] = 0; + out_1472693377939272903[13] = 0; + out_1472693377939272903[14] = 0; + out_1472693377939272903[15] = 0; + out_1472693377939272903[16] = 0; + out_1472693377939272903[17] = 0; + out_1472693377939272903[18] = 0; + out_1472693377939272903[19] = 0; + out_1472693377939272903[20] = 0; + out_1472693377939272903[21] = 0; + out_1472693377939272903[22] = 1; + out_1472693377939272903[23] = 0; + out_1472693377939272903[24] = 0; + out_1472693377939272903[25] = 0; + out_1472693377939272903[26] = 0; + out_1472693377939272903[27] = 0; + out_1472693377939272903[28] = 0; + out_1472693377939272903[29] = 0; + out_1472693377939272903[30] = 0; + out_1472693377939272903[31] = 0; + out_1472693377939272903[32] = 0; + out_1472693377939272903[33] = 0; + out_1472693377939272903[34] = 0; + out_1472693377939272903[35] = 0; + out_1472693377939272903[36] = 0; + out_1472693377939272903[37] = 0; + out_1472693377939272903[38] = 0; + out_1472693377939272903[39] = 0; + out_1472693377939272903[40] = 0; + out_1472693377939272903[41] = 1; + out_1472693377939272903[42] = 0; + out_1472693377939272903[43] = 0; + out_1472693377939272903[44] = 0; + out_1472693377939272903[45] = 0; + out_1472693377939272903[46] = 0; + out_1472693377939272903[47] = 0; + out_1472693377939272903[48] = 0; + out_1472693377939272903[49] = 0; + out_1472693377939272903[50] = 0; + out_1472693377939272903[51] = 0; + out_1472693377939272903[52] = 0; + out_1472693377939272903[53] = 0; } -void h_14(double *state, double *unused, double *out_7181242506613011838) { - out_7181242506613011838[0] = state[6]; - out_7181242506613011838[1] = state[7]; - out_7181242506613011838[2] = state[8]; +void h_14(double *state, double *unused, double *out_9091688560146400700) { + out_9091688560146400700[0] = state[6]; + out_9091688560146400700[1] = state[7]; + out_9091688560146400700[2] = state[8]; } -void H_14(double *state, double *unused, double *out_6403526101164863262) { - out_6403526101164863262[0] = 0; - out_6403526101164863262[1] = 0; - out_6403526101164863262[2] = 0; - out_6403526101164863262[3] = 0; - out_6403526101164863262[4] = 0; - out_6403526101164863262[5] = 0; - out_6403526101164863262[6] = 1; - out_6403526101164863262[7] = 0; - out_6403526101164863262[8] = 0; - out_6403526101164863262[9] = 0; - out_6403526101164863262[10] = 0; - out_6403526101164863262[11] = 0; - out_6403526101164863262[12] = 0; - out_6403526101164863262[13] = 0; - out_6403526101164863262[14] = 0; - out_6403526101164863262[15] = 0; - out_6403526101164863262[16] = 0; - out_6403526101164863262[17] = 0; - out_6403526101164863262[18] = 0; - out_6403526101164863262[19] = 0; - out_6403526101164863262[20] = 0; - out_6403526101164863262[21] = 0; - out_6403526101164863262[22] = 0; - out_6403526101164863262[23] = 0; - out_6403526101164863262[24] = 0; - out_6403526101164863262[25] = 1; - out_6403526101164863262[26] = 0; - out_6403526101164863262[27] = 0; - out_6403526101164863262[28] = 0; - out_6403526101164863262[29] = 0; - out_6403526101164863262[30] = 0; - out_6403526101164863262[31] = 0; - out_6403526101164863262[32] = 0; - out_6403526101164863262[33] = 0; - out_6403526101164863262[34] = 0; - out_6403526101164863262[35] = 0; - out_6403526101164863262[36] = 0; - out_6403526101164863262[37] = 0; - out_6403526101164863262[38] = 0; - out_6403526101164863262[39] = 0; - out_6403526101164863262[40] = 0; - out_6403526101164863262[41] = 0; - out_6403526101164863262[42] = 0; - out_6403526101164863262[43] = 0; - out_6403526101164863262[44] = 1; - out_6403526101164863262[45] = 0; - out_6403526101164863262[46] = 0; - out_6403526101164863262[47] = 0; - out_6403526101164863262[48] = 0; - out_6403526101164863262[49] = 0; - out_6403526101164863262[50] = 0; - out_6403526101164863262[51] = 0; - out_6403526101164863262[52] = 0; - out_6403526101164863262[53] = 0; +void H_14(double *state, double *unused, double *out_2174696974037943497) { + out_2174696974037943497[0] = 0; + out_2174696974037943497[1] = 0; + out_2174696974037943497[2] = 0; + out_2174696974037943497[3] = 0; + out_2174696974037943497[4] = 0; + out_2174696974037943497[5] = 0; + out_2174696974037943497[6] = 1; + out_2174696974037943497[7] = 0; + out_2174696974037943497[8] = 0; + out_2174696974037943497[9] = 0; + out_2174696974037943497[10] = 0; + out_2174696974037943497[11] = 0; + out_2174696974037943497[12] = 0; + out_2174696974037943497[13] = 0; + out_2174696974037943497[14] = 0; + out_2174696974037943497[15] = 0; + out_2174696974037943497[16] = 0; + out_2174696974037943497[17] = 0; + out_2174696974037943497[18] = 0; + out_2174696974037943497[19] = 0; + out_2174696974037943497[20] = 0; + out_2174696974037943497[21] = 0; + out_2174696974037943497[22] = 0; + out_2174696974037943497[23] = 0; + out_2174696974037943497[24] = 0; + out_2174696974037943497[25] = 1; + out_2174696974037943497[26] = 0; + out_2174696974037943497[27] = 0; + out_2174696974037943497[28] = 0; + out_2174696974037943497[29] = 0; + out_2174696974037943497[30] = 0; + out_2174696974037943497[31] = 0; + out_2174696974037943497[32] = 0; + out_2174696974037943497[33] = 0; + out_2174696974037943497[34] = 0; + out_2174696974037943497[35] = 0; + out_2174696974037943497[36] = 0; + out_2174696974037943497[37] = 0; + out_2174696974037943497[38] = 0; + out_2174696974037943497[39] = 0; + out_2174696974037943497[40] = 0; + out_2174696974037943497[41] = 0; + out_2174696974037943497[42] = 0; + out_2174696974037943497[43] = 0; + out_2174696974037943497[44] = 1; + out_2174696974037943497[45] = 0; + out_2174696974037943497[46] = 0; + out_2174696974037943497[47] = 0; + out_2174696974037943497[48] = 0; + out_2174696974037943497[49] = 0; + out_2174696974037943497[50] = 0; + out_2174696974037943497[51] = 0; + out_2174696974037943497[52] = 0; + out_2174696974037943497[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_7389011504566069386) { - err_fun(nom_x, delta_x, out_7389011504566069386); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7352766374165151156) { + err_fun(nom_x, delta_x, out_7352766374165151156); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6795243368226050386) { - inv_err_fun(nom_x, true_x, out_6795243368226050386); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5943722919136625654) { + inv_err_fun(nom_x, true_x, out_5943722919136625654); } -void pose_H_mod_fun(double *state, double *out_6387810172757071512) { - H_mod_fun(state, out_6387810172757071512); +void pose_H_mod_fun(double *state, double *out_2863715751586478773) { + H_mod_fun(state, out_2863715751586478773); } -void pose_f_fun(double *state, double dt, double *out_5190191951972352761) { - f_fun(state, dt, out_5190191951972352761); +void pose_f_fun(double *state, double dt, double *out_4163803070138990822) { + f_fun(state, dt, out_4163803070138990822); } -void pose_F_fun(double *state, double dt, double *out_3425534515038953406) { - F_fun(state, dt, out_3425534515038953406); +void pose_F_fun(double *state, double dt, double *out_3325085581577154625) { + F_fun(state, dt, out_3325085581577154625); } -void pose_h_4(double *state, double *unused, double *out_7576152197490450413) { - h_4(state, unused, out_7576152197490450413); +void pose_h_4(double *state, double *unused, double *out_1824245269356126422) { + h_4(state, unused, out_1824245269356126422); } -void pose_H_4(double *state, double *unused, double *out_3320737668869490966) { - H_4(state, unused, out_3320737668869490966); +void pose_H_4(double *state, double *unused, double *out_908091458257428799) { + H_4(state, unused, out_908091458257428799); } -void pose_h_10(double *state, double *unused, double *out_3130452913144701320) { - h_10(state, unused, out_3130452913144701320); +void pose_h_10(double *state, double *unused, double *out_5992945801677381357) { + h_10(state, unused, out_5992945801677381357); } -void pose_H_10(double *state, double *unused, double *out_2096220204218508864) { - H_10(state, unused, out_2096220204218508864); +void pose_H_10(double *state, double *unused, double *out_1551783764236526040) { + H_10(state, unused, out_1551783764236526040); } -void pose_h_13(double *state, double *unused, double *out_8767951916852656852) { - h_13(state, unused, out_8767951916852656852); +void pose_h_13(double *state, double *unused, double *out_4650572533653388734) { + h_13(state, unused, out_4650572533653388734); } -void pose_H_13(double *state, double *unused, double *out_108463843537158165) { - H_13(state, unused, out_108463843537158165); +void pose_H_13(double *state, double *unused, double *out_1472693377939272903) { + H_13(state, unused, out_1472693377939272903); } -void pose_h_14(double *state, double *unused, double *out_7181242506613011838) { - h_14(state, unused, out_7181242506613011838); +void pose_h_14(double *state, double *unused, double *out_9091688560146400700) { + h_14(state, unused, out_9091688560146400700); } -void pose_H_14(double *state, double *unused, double *out_6403526101164863262) { - H_14(state, unused, out_6403526101164863262); +void pose_H_14(double *state, double *unused, double *out_2174696974037943497) { + H_14(state, unused, out_2174696974037943497); } 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 0e4053ac7..4c7cf1aa9 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_7389011504566069386); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6795243368226050386); -void pose_H_mod_fun(double *state, double *out_6387810172757071512); -void pose_f_fun(double *state, double dt, double *out_5190191951972352761); -void pose_F_fun(double *state, double dt, double *out_3425534515038953406); -void pose_h_4(double *state, double *unused, double *out_7576152197490450413); -void pose_H_4(double *state, double *unused, double *out_3320737668869490966); -void pose_h_10(double *state, double *unused, double *out_3130452913144701320); -void pose_H_10(double *state, double *unused, double *out_2096220204218508864); -void pose_h_13(double *state, double *unused, double *out_8767951916852656852); -void pose_H_13(double *state, double *unused, double *out_108463843537158165); -void pose_h_14(double *state, double *unused, double *out_7181242506613011838); -void pose_H_14(double *state, double *unused, double *out_6403526101164863262); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7352766374165151156); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5943722919136625654); +void pose_H_mod_fun(double *state, double *out_2863715751586478773); +void pose_f_fun(double *state, double dt, double *out_4163803070138990822); +void pose_F_fun(double *state, double dt, double *out_3325085581577154625); +void pose_h_4(double *state, double *unused, double *out_1824245269356126422); +void pose_H_4(double *state, double *unused, double *out_908091458257428799); +void pose_h_10(double *state, double *unused, double *out_5992945801677381357); +void pose_H_10(double *state, double *unused, double *out_1551783764236526040); +void pose_h_13(double *state, double *unused, double *out_4650572533653388734); +void pose_H_13(double *state, double *unused, double *out_1472693377939272903); +void pose_h_14(double *state, double *unused, double *out_9091688560146400700); +void pose_H_14(double *state, double *unused, double *out_2174696974037943497); 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 e21ce7d02..aceec4e86 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ