diff --git a/common/libcommon.a b/common/libcommon.a index 06e26b99b..3a2e9e39d 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 23fbd225d..d757271b0 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index dacc62d6a..9cde239c7 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 a73402cf8..c8f096ffb 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 7c0782403..97d6ad756 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 76cb8aa0d..c57c38c4f 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 bbf36b7fa..e1e7c2834 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 f099ead16..a88d40113 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 bd0992e8e..6e825729a 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 bd0992e8e..6e825729a 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 2d4aa7d49..6ae5b95b3 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 f099ead16..a88d40113 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 84ade634b..52f641c16 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-5312ce30-DEBUG"; +const uint8_t gitversion[19] = "DEV-016e657e-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 7a9dba447..a32d2faf3 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 fa68f6bf7..762664319 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 9f7b0f0e9..0244a3c79 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 4bf137d38..b73b72ef9 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 94f79a12a..8e8b17aac 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 f53e3ccda..d207870de 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 debeb0b54..7235fde67 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 237ab3a54..88448b6f3 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 e539d72a6..b47e583b6 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 d8d582430..8b6b0a7bf 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 4f492ac35..63c3e1a92 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 fc0bd0b8b..940ef5277 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 31de76a33..c4af42a57 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 6aad6d5bb..ec7f0e7b8 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 c85453a0c..1a5301020 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 31a56dc1d..9d0900d63 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 85bb2491e..442673f78 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 4f67db0fe..044a6301b 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 d3a24caed..25ac49000 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 fce025783..54a35741c 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 4a55a7f95..86dc3ddf3 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-5312ce30-DEBUG \ No newline at end of file +DEV-016e657e-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 95603dbf6..f4ef56929 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_8994217345304474551) { - out_8994217345304474551[0] = delta_x[0] + nom_x[0]; - out_8994217345304474551[1] = delta_x[1] + nom_x[1]; - out_8994217345304474551[2] = delta_x[2] + nom_x[2]; - out_8994217345304474551[3] = delta_x[3] + nom_x[3]; - out_8994217345304474551[4] = delta_x[4] + nom_x[4]; - out_8994217345304474551[5] = delta_x[5] + nom_x[5]; - out_8994217345304474551[6] = delta_x[6] + nom_x[6]; - out_8994217345304474551[7] = delta_x[7] + nom_x[7]; - out_8994217345304474551[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_3321760492551468366) { + out_3321760492551468366[0] = delta_x[0] + nom_x[0]; + out_3321760492551468366[1] = delta_x[1] + nom_x[1]; + out_3321760492551468366[2] = delta_x[2] + nom_x[2]; + out_3321760492551468366[3] = delta_x[3] + nom_x[3]; + out_3321760492551468366[4] = delta_x[4] + nom_x[4]; + out_3321760492551468366[5] = delta_x[5] + nom_x[5]; + out_3321760492551468366[6] = delta_x[6] + nom_x[6]; + out_3321760492551468366[7] = delta_x[7] + nom_x[7]; + out_3321760492551468366[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8635883422281677209) { - out_8635883422281677209[0] = -nom_x[0] + true_x[0]; - out_8635883422281677209[1] = -nom_x[1] + true_x[1]; - out_8635883422281677209[2] = -nom_x[2] + true_x[2]; - out_8635883422281677209[3] = -nom_x[3] + true_x[3]; - out_8635883422281677209[4] = -nom_x[4] + true_x[4]; - out_8635883422281677209[5] = -nom_x[5] + true_x[5]; - out_8635883422281677209[6] = -nom_x[6] + true_x[6]; - out_8635883422281677209[7] = -nom_x[7] + true_x[7]; - out_8635883422281677209[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3697485090417495632) { + out_3697485090417495632[0] = -nom_x[0] + true_x[0]; + out_3697485090417495632[1] = -nom_x[1] + true_x[1]; + out_3697485090417495632[2] = -nom_x[2] + true_x[2]; + out_3697485090417495632[3] = -nom_x[3] + true_x[3]; + out_3697485090417495632[4] = -nom_x[4] + true_x[4]; + out_3697485090417495632[5] = -nom_x[5] + true_x[5]; + out_3697485090417495632[6] = -nom_x[6] + true_x[6]; + out_3697485090417495632[7] = -nom_x[7] + true_x[7]; + out_3697485090417495632[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_5835492163818982048) { - out_5835492163818982048[0] = 1.0; - out_5835492163818982048[1] = 0.0; - out_5835492163818982048[2] = 0.0; - out_5835492163818982048[3] = 0.0; - out_5835492163818982048[4] = 0.0; - out_5835492163818982048[5] = 0.0; - out_5835492163818982048[6] = 0.0; - out_5835492163818982048[7] = 0.0; - out_5835492163818982048[8] = 0.0; - out_5835492163818982048[9] = 0.0; - out_5835492163818982048[10] = 1.0; - out_5835492163818982048[11] = 0.0; - out_5835492163818982048[12] = 0.0; - out_5835492163818982048[13] = 0.0; - out_5835492163818982048[14] = 0.0; - out_5835492163818982048[15] = 0.0; - out_5835492163818982048[16] = 0.0; - out_5835492163818982048[17] = 0.0; - out_5835492163818982048[18] = 0.0; - out_5835492163818982048[19] = 0.0; - out_5835492163818982048[20] = 1.0; - out_5835492163818982048[21] = 0.0; - out_5835492163818982048[22] = 0.0; - out_5835492163818982048[23] = 0.0; - out_5835492163818982048[24] = 0.0; - out_5835492163818982048[25] = 0.0; - out_5835492163818982048[26] = 0.0; - out_5835492163818982048[27] = 0.0; - out_5835492163818982048[28] = 0.0; - out_5835492163818982048[29] = 0.0; - out_5835492163818982048[30] = 1.0; - out_5835492163818982048[31] = 0.0; - out_5835492163818982048[32] = 0.0; - out_5835492163818982048[33] = 0.0; - out_5835492163818982048[34] = 0.0; - out_5835492163818982048[35] = 0.0; - out_5835492163818982048[36] = 0.0; - out_5835492163818982048[37] = 0.0; - out_5835492163818982048[38] = 0.0; - out_5835492163818982048[39] = 0.0; - out_5835492163818982048[40] = 1.0; - out_5835492163818982048[41] = 0.0; - out_5835492163818982048[42] = 0.0; - out_5835492163818982048[43] = 0.0; - out_5835492163818982048[44] = 0.0; - out_5835492163818982048[45] = 0.0; - out_5835492163818982048[46] = 0.0; - out_5835492163818982048[47] = 0.0; - out_5835492163818982048[48] = 0.0; - out_5835492163818982048[49] = 0.0; - out_5835492163818982048[50] = 1.0; - out_5835492163818982048[51] = 0.0; - out_5835492163818982048[52] = 0.0; - out_5835492163818982048[53] = 0.0; - out_5835492163818982048[54] = 0.0; - out_5835492163818982048[55] = 0.0; - out_5835492163818982048[56] = 0.0; - out_5835492163818982048[57] = 0.0; - out_5835492163818982048[58] = 0.0; - out_5835492163818982048[59] = 0.0; - out_5835492163818982048[60] = 1.0; - out_5835492163818982048[61] = 0.0; - out_5835492163818982048[62] = 0.0; - out_5835492163818982048[63] = 0.0; - out_5835492163818982048[64] = 0.0; - out_5835492163818982048[65] = 0.0; - out_5835492163818982048[66] = 0.0; - out_5835492163818982048[67] = 0.0; - out_5835492163818982048[68] = 0.0; - out_5835492163818982048[69] = 0.0; - out_5835492163818982048[70] = 1.0; - out_5835492163818982048[71] = 0.0; - out_5835492163818982048[72] = 0.0; - out_5835492163818982048[73] = 0.0; - out_5835492163818982048[74] = 0.0; - out_5835492163818982048[75] = 0.0; - out_5835492163818982048[76] = 0.0; - out_5835492163818982048[77] = 0.0; - out_5835492163818982048[78] = 0.0; - out_5835492163818982048[79] = 0.0; - out_5835492163818982048[80] = 1.0; +void H_mod_fun(double *state, double *out_4190227621087458402) { + out_4190227621087458402[0] = 1.0; + out_4190227621087458402[1] = 0.0; + out_4190227621087458402[2] = 0.0; + out_4190227621087458402[3] = 0.0; + out_4190227621087458402[4] = 0.0; + out_4190227621087458402[5] = 0.0; + out_4190227621087458402[6] = 0.0; + out_4190227621087458402[7] = 0.0; + out_4190227621087458402[8] = 0.0; + out_4190227621087458402[9] = 0.0; + out_4190227621087458402[10] = 1.0; + out_4190227621087458402[11] = 0.0; + out_4190227621087458402[12] = 0.0; + out_4190227621087458402[13] = 0.0; + out_4190227621087458402[14] = 0.0; + out_4190227621087458402[15] = 0.0; + out_4190227621087458402[16] = 0.0; + out_4190227621087458402[17] = 0.0; + out_4190227621087458402[18] = 0.0; + out_4190227621087458402[19] = 0.0; + out_4190227621087458402[20] = 1.0; + out_4190227621087458402[21] = 0.0; + out_4190227621087458402[22] = 0.0; + out_4190227621087458402[23] = 0.0; + out_4190227621087458402[24] = 0.0; + out_4190227621087458402[25] = 0.0; + out_4190227621087458402[26] = 0.0; + out_4190227621087458402[27] = 0.0; + out_4190227621087458402[28] = 0.0; + out_4190227621087458402[29] = 0.0; + out_4190227621087458402[30] = 1.0; + out_4190227621087458402[31] = 0.0; + out_4190227621087458402[32] = 0.0; + out_4190227621087458402[33] = 0.0; + out_4190227621087458402[34] = 0.0; + out_4190227621087458402[35] = 0.0; + out_4190227621087458402[36] = 0.0; + out_4190227621087458402[37] = 0.0; + out_4190227621087458402[38] = 0.0; + out_4190227621087458402[39] = 0.0; + out_4190227621087458402[40] = 1.0; + out_4190227621087458402[41] = 0.0; + out_4190227621087458402[42] = 0.0; + out_4190227621087458402[43] = 0.0; + out_4190227621087458402[44] = 0.0; + out_4190227621087458402[45] = 0.0; + out_4190227621087458402[46] = 0.0; + out_4190227621087458402[47] = 0.0; + out_4190227621087458402[48] = 0.0; + out_4190227621087458402[49] = 0.0; + out_4190227621087458402[50] = 1.0; + out_4190227621087458402[51] = 0.0; + out_4190227621087458402[52] = 0.0; + out_4190227621087458402[53] = 0.0; + out_4190227621087458402[54] = 0.0; + out_4190227621087458402[55] = 0.0; + out_4190227621087458402[56] = 0.0; + out_4190227621087458402[57] = 0.0; + out_4190227621087458402[58] = 0.0; + out_4190227621087458402[59] = 0.0; + out_4190227621087458402[60] = 1.0; + out_4190227621087458402[61] = 0.0; + out_4190227621087458402[62] = 0.0; + out_4190227621087458402[63] = 0.0; + out_4190227621087458402[64] = 0.0; + out_4190227621087458402[65] = 0.0; + out_4190227621087458402[66] = 0.0; + out_4190227621087458402[67] = 0.0; + out_4190227621087458402[68] = 0.0; + out_4190227621087458402[69] = 0.0; + out_4190227621087458402[70] = 1.0; + out_4190227621087458402[71] = 0.0; + out_4190227621087458402[72] = 0.0; + out_4190227621087458402[73] = 0.0; + out_4190227621087458402[74] = 0.0; + out_4190227621087458402[75] = 0.0; + out_4190227621087458402[76] = 0.0; + out_4190227621087458402[77] = 0.0; + out_4190227621087458402[78] = 0.0; + out_4190227621087458402[79] = 0.0; + out_4190227621087458402[80] = 1.0; } -void f_fun(double *state, double dt, double *out_9046400166044584491) { - out_9046400166044584491[0] = state[0]; - out_9046400166044584491[1] = state[1]; - out_9046400166044584491[2] = state[2]; - out_9046400166044584491[3] = state[3]; - out_9046400166044584491[4] = state[4]; - out_9046400166044584491[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_9046400166044584491[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_9046400166044584491[7] = state[7]; - out_9046400166044584491[8] = state[8]; +void f_fun(double *state, double dt, double *out_6456348943274406543) { + out_6456348943274406543[0] = state[0]; + out_6456348943274406543[1] = state[1]; + out_6456348943274406543[2] = state[2]; + out_6456348943274406543[3] = state[3]; + out_6456348943274406543[4] = state[4]; + out_6456348943274406543[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_6456348943274406543[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_6456348943274406543[7] = state[7]; + out_6456348943274406543[8] = state[8]; } -void F_fun(double *state, double dt, double *out_8585883916966185499) { - out_8585883916966185499[0] = 1; - out_8585883916966185499[1] = 0; - out_8585883916966185499[2] = 0; - out_8585883916966185499[3] = 0; - out_8585883916966185499[4] = 0; - out_8585883916966185499[5] = 0; - out_8585883916966185499[6] = 0; - out_8585883916966185499[7] = 0; - out_8585883916966185499[8] = 0; - out_8585883916966185499[9] = 0; - out_8585883916966185499[10] = 1; - out_8585883916966185499[11] = 0; - out_8585883916966185499[12] = 0; - out_8585883916966185499[13] = 0; - out_8585883916966185499[14] = 0; - out_8585883916966185499[15] = 0; - out_8585883916966185499[16] = 0; - out_8585883916966185499[17] = 0; - out_8585883916966185499[18] = 0; - out_8585883916966185499[19] = 0; - out_8585883916966185499[20] = 1; - out_8585883916966185499[21] = 0; - out_8585883916966185499[22] = 0; - out_8585883916966185499[23] = 0; - out_8585883916966185499[24] = 0; - out_8585883916966185499[25] = 0; - out_8585883916966185499[26] = 0; - out_8585883916966185499[27] = 0; - out_8585883916966185499[28] = 0; - out_8585883916966185499[29] = 0; - out_8585883916966185499[30] = 1; - out_8585883916966185499[31] = 0; - out_8585883916966185499[32] = 0; - out_8585883916966185499[33] = 0; - out_8585883916966185499[34] = 0; - out_8585883916966185499[35] = 0; - out_8585883916966185499[36] = 0; - out_8585883916966185499[37] = 0; - out_8585883916966185499[38] = 0; - out_8585883916966185499[39] = 0; - out_8585883916966185499[40] = 1; - out_8585883916966185499[41] = 0; - out_8585883916966185499[42] = 0; - out_8585883916966185499[43] = 0; - out_8585883916966185499[44] = 0; - out_8585883916966185499[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_8585883916966185499[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_8585883916966185499[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8585883916966185499[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8585883916966185499[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_8585883916966185499[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_8585883916966185499[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_8585883916966185499[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_8585883916966185499[53] = -9.8100000000000005*dt; - out_8585883916966185499[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_8585883916966185499[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_8585883916966185499[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8585883916966185499[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8585883916966185499[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_8585883916966185499[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_8585883916966185499[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_8585883916966185499[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8585883916966185499[62] = 0; - out_8585883916966185499[63] = 0; - out_8585883916966185499[64] = 0; - out_8585883916966185499[65] = 0; - out_8585883916966185499[66] = 0; - out_8585883916966185499[67] = 0; - out_8585883916966185499[68] = 0; - out_8585883916966185499[69] = 0; - out_8585883916966185499[70] = 1; - out_8585883916966185499[71] = 0; - out_8585883916966185499[72] = 0; - out_8585883916966185499[73] = 0; - out_8585883916966185499[74] = 0; - out_8585883916966185499[75] = 0; - out_8585883916966185499[76] = 0; - out_8585883916966185499[77] = 0; - out_8585883916966185499[78] = 0; - out_8585883916966185499[79] = 0; - out_8585883916966185499[80] = 1; +void F_fun(double *state, double dt, double *out_4201765287475637381) { + out_4201765287475637381[0] = 1; + out_4201765287475637381[1] = 0; + out_4201765287475637381[2] = 0; + out_4201765287475637381[3] = 0; + out_4201765287475637381[4] = 0; + out_4201765287475637381[5] = 0; + out_4201765287475637381[6] = 0; + out_4201765287475637381[7] = 0; + out_4201765287475637381[8] = 0; + out_4201765287475637381[9] = 0; + out_4201765287475637381[10] = 1; + out_4201765287475637381[11] = 0; + out_4201765287475637381[12] = 0; + out_4201765287475637381[13] = 0; + out_4201765287475637381[14] = 0; + out_4201765287475637381[15] = 0; + out_4201765287475637381[16] = 0; + out_4201765287475637381[17] = 0; + out_4201765287475637381[18] = 0; + out_4201765287475637381[19] = 0; + out_4201765287475637381[20] = 1; + out_4201765287475637381[21] = 0; + out_4201765287475637381[22] = 0; + out_4201765287475637381[23] = 0; + out_4201765287475637381[24] = 0; + out_4201765287475637381[25] = 0; + out_4201765287475637381[26] = 0; + out_4201765287475637381[27] = 0; + out_4201765287475637381[28] = 0; + out_4201765287475637381[29] = 0; + out_4201765287475637381[30] = 1; + out_4201765287475637381[31] = 0; + out_4201765287475637381[32] = 0; + out_4201765287475637381[33] = 0; + out_4201765287475637381[34] = 0; + out_4201765287475637381[35] = 0; + out_4201765287475637381[36] = 0; + out_4201765287475637381[37] = 0; + out_4201765287475637381[38] = 0; + out_4201765287475637381[39] = 0; + out_4201765287475637381[40] = 1; + out_4201765287475637381[41] = 0; + out_4201765287475637381[42] = 0; + out_4201765287475637381[43] = 0; + out_4201765287475637381[44] = 0; + out_4201765287475637381[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_4201765287475637381[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_4201765287475637381[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4201765287475637381[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4201765287475637381[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_4201765287475637381[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_4201765287475637381[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_4201765287475637381[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_4201765287475637381[53] = -9.8100000000000005*dt; + out_4201765287475637381[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_4201765287475637381[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_4201765287475637381[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4201765287475637381[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4201765287475637381[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_4201765287475637381[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_4201765287475637381[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_4201765287475637381[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4201765287475637381[62] = 0; + out_4201765287475637381[63] = 0; + out_4201765287475637381[64] = 0; + out_4201765287475637381[65] = 0; + out_4201765287475637381[66] = 0; + out_4201765287475637381[67] = 0; + out_4201765287475637381[68] = 0; + out_4201765287475637381[69] = 0; + out_4201765287475637381[70] = 1; + out_4201765287475637381[71] = 0; + out_4201765287475637381[72] = 0; + out_4201765287475637381[73] = 0; + out_4201765287475637381[74] = 0; + out_4201765287475637381[75] = 0; + out_4201765287475637381[76] = 0; + out_4201765287475637381[77] = 0; + out_4201765287475637381[78] = 0; + out_4201765287475637381[79] = 0; + out_4201765287475637381[80] = 1; } -void h_25(double *state, double *unused, double *out_1895365982627284096) { - out_1895365982627284096[0] = state[6]; +void h_25(double *state, double *unused, double *out_9136101134851125551) { + out_9136101134851125551[0] = state[6]; } -void H_25(double *state, double *unused, double *out_3357696062093255189) { - out_3357696062093255189[0] = 0; - out_3357696062093255189[1] = 0; - out_3357696062093255189[2] = 0; - out_3357696062093255189[3] = 0; - out_3357696062093255189[4] = 0; - out_3357696062093255189[5] = 0; - out_3357696062093255189[6] = 1; - out_3357696062093255189[7] = 0; - out_3357696062093255189[8] = 0; +void H_25(double *state, double *unused, double *out_7713208271076950079) { + out_7713208271076950079[0] = 0; + out_7713208271076950079[1] = 0; + out_7713208271076950079[2] = 0; + out_7713208271076950079[3] = 0; + out_7713208271076950079[4] = 0; + out_7713208271076950079[5] = 0; + out_7713208271076950079[6] = 1; + out_7713208271076950079[7] = 0; + out_7713208271076950079[8] = 0; } -void h_24(double *state, double *unused, double *out_5360338391580302842) { - out_5360338391580302842[0] = state[4]; - out_5360338391580302842[1] = state[5]; +void h_24(double *state, double *unused, double *out_8665340836537612490) { + out_8665340836537612490[0] = state[4]; + out_8665340836537612490[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5530345661098754755) { - out_5530345661098754755[0] = 0; - out_5530345661098754755[1] = 0; - out_5530345661098754755[2] = 0; - out_5530345661098754755[3] = 0; - out_5530345661098754755[4] = 1; - out_5530345661098754755[5] = 0; - out_5530345661098754755[6] = 0; - out_5530345661098754755[7] = 0; - out_5530345661098754755[8] = 0; - out_5530345661098754755[9] = 0; - out_5530345661098754755[10] = 0; - out_5530345661098754755[11] = 0; - out_5530345661098754755[12] = 0; - out_5530345661098754755[13] = 0; - out_5530345661098754755[14] = 1; - out_5530345661098754755[15] = 0; - out_5530345661098754755[16] = 0; - out_5530345661098754755[17] = 0; +void H_24(double *state, double *unused, double *out_7175610203830278401) { + out_7175610203830278401[0] = 0; + out_7175610203830278401[1] = 0; + out_7175610203830278401[2] = 0; + out_7175610203830278401[3] = 0; + out_7175610203830278401[4] = 1; + out_7175610203830278401[5] = 0; + out_7175610203830278401[6] = 0; + out_7175610203830278401[7] = 0; + out_7175610203830278401[8] = 0; + out_7175610203830278401[9] = 0; + out_7175610203830278401[10] = 0; + out_7175610203830278401[11] = 0; + out_7175610203830278401[12] = 0; + out_7175610203830278401[13] = 0; + out_7175610203830278401[14] = 1; + out_7175610203830278401[15] = 0; + out_7175610203830278401[16] = 0; + out_7175610203830278401[17] = 0; } -void h_30(double *state, double *unused, double *out_2052552650978413241) { - out_2052552650978413241[0] = state[4]; +void h_30(double *state, double *unused, double *out_4641549406917500638) { + out_4641549406917500638[0] = state[4]; } -void H_30(double *state, double *unused, double *out_839363103586006562) { - out_839363103586006562[0] = 0; - out_839363103586006562[1] = 0; - out_839363103586006562[2] = 0; - out_839363103586006562[3] = 0; - out_839363103586006562[4] = 1; - out_839363103586006562[5] = 0; - out_839363103586006562[6] = 0; - out_839363103586006562[7] = 0; - out_839363103586006562[8] = 0; +void H_30(double *state, double *unused, double *out_3185511940949341881) { + out_3185511940949341881[0] = 0; + out_3185511940949341881[1] = 0; + out_3185511940949341881[2] = 0; + out_3185511940949341881[3] = 0; + out_3185511940949341881[4] = 1; + out_3185511940949341881[5] = 0; + out_3185511940949341881[6] = 0; + out_3185511940949341881[7] = 0; + out_3185511940949341881[8] = 0; } -void h_26(double *state, double *unused, double *out_2655868535654890758) { - out_2655868535654890758[0] = state[7]; +void h_26(double *state, double *unused, double *out_2793594027511200175) { + out_2793594027511200175[0] = state[7]; } -void H_26(double *state, double *unused, double *out_53170092332454588) { - out_53170092332454588[0] = 0; - out_53170092332454588[1] = 0; - out_53170092332454588[2] = 0; - out_53170092332454588[3] = 0; - out_53170092332454588[4] = 0; - out_53170092332454588[5] = 0; - out_53170092332454588[6] = 0; - out_53170092332454588[7] = 1; - out_53170092332454588[8] = 0; +void H_26(double *state, double *unused, double *out_3971704952202893855) { + out_3971704952202893855[0] = 0; + out_3971704952202893855[1] = 0; + out_3971704952202893855[2] = 0; + out_3971704952202893855[3] = 0; + out_3971704952202893855[4] = 0; + out_3971704952202893855[5] = 0; + out_3971704952202893855[6] = 0; + out_3971704952202893855[7] = 1; + out_3971704952202893855[8] = 0; } -void h_27(double *state, double *unused, double *out_3774772722470275260) { - out_3774772722470275260[0] = state[3]; +void h_27(double *state, double *unused, double *out_5860253322103668962) { + out_5860253322103668962[0] = state[3]; } -void H_27(double *state, double *unused, double *out_1384230967597936655) { - out_1384230967597936655[0] = 0; - out_1384230967597936655[1] = 0; - out_1384230967597936655[2] = 0; - out_1384230967597936655[3] = 1; - out_1384230967597936655[4] = 0; - out_1384230967597936655[5] = 0; - out_1384230967597936655[6] = 0; - out_1384230967597936655[7] = 0; - out_1384230967597936655[8] = 0; +void H_27(double *state, double *unused, double *out_5409106012133285098) { + out_5409106012133285098[0] = 0; + out_5409106012133285098[1] = 0; + out_5409106012133285098[2] = 0; + out_5409106012133285098[3] = 1; + out_5409106012133285098[4] = 0; + out_5409106012133285098[5] = 0; + out_5409106012133285098[6] = 0; + out_5409106012133285098[7] = 0; + out_5409106012133285098[8] = 0; } -void h_29(double *state, double *unused, double *out_4656957971787110892) { - out_4656957971787110892[0] = state[1]; +void h_29(double *state, double *unused, double *out_5585059259819163073) { + out_5585059259819163073[0] = state[1]; } -void H_29(double *state, double *unused, double *out_329131759271614378) { - out_329131759271614378[0] = 0; - out_329131759271614378[1] = 1; - out_329131759271614378[2] = 0; - out_329131759271614378[3] = 0; - out_329131759271614378[4] = 0; - out_329131759271614378[5] = 0; - out_329131759271614378[6] = 0; - out_329131759271614378[7] = 0; - out_329131759271614378[8] = 0; +void H_29(double *state, double *unused, double *out_3695743285263734065) { + out_3695743285263734065[0] = 0; + out_3695743285263734065[1] = 1; + out_3695743285263734065[2] = 0; + out_3695743285263734065[3] = 0; + out_3695743285263734065[4] = 0; + out_3695743285263734065[5] = 0; + out_3695743285263734065[6] = 0; + out_3695743285263734065[7] = 0; + out_3695743285263734065[8] = 0; } -void h_28(double *state, double *unused, double *out_1170367401661577609) { - out_1170367401661577609[0] = state[0]; +void h_28(double *state, double *unused, double *out_6187449234192033508) { + out_6187449234192033508[0] = state[0]; } -void H_28(double *state, double *unused, double *out_5411530776341144952) { - out_5411530776341144952[0] = 1; - out_5411530776341144952[1] = 0; - out_5411530776341144952[2] = 0; - out_5411530776341144952[3] = 0; - out_5411530776341144952[4] = 0; - out_5411530776341144952[5] = 0; - out_5411530776341144952[6] = 0; - out_5411530776341144952[7] = 0; - out_5411530776341144952[8] = 0; +void H_28(double *state, double *unused, double *out_1386655731805796509) { + out_1386655731805796509[0] = 1; + out_1386655731805796509[1] = 0; + out_1386655731805796509[2] = 0; + out_1386655731805796509[3] = 0; + out_1386655731805796509[4] = 0; + out_1386655731805796509[5] = 0; + out_1386655731805796509[6] = 0; + out_1386655731805796509[7] = 0; + out_1386655731805796509[8] = 0; } -void h_31(double *state, double *unused, double *out_1437164620468722434) { - out_1437164620468722434[0] = state[8]; +void h_31(double *state, double *unused, double *out_2365265908500774615) { + out_2365265908500774615[0] = state[8]; } -void H_31(double *state, double *unused, double *out_7725407483200662889) { - out_7725407483200662889[0] = 0; - out_7725407483200662889[1] = 0; - out_7725407483200662889[2] = 0; - out_7725407483200662889[3] = 0; - out_7725407483200662889[4] = 0; - out_7725407483200662889[5] = 0; - out_7725407483200662889[6] = 0; - out_7725407483200662889[7] = 0; - out_7725407483200662889[8] = 1; +void H_31(double *state, double *unused, double *out_7743854232953910507) { + out_7743854232953910507[0] = 0; + out_7743854232953910507[1] = 0; + out_7743854232953910507[2] = 0; + out_7743854232953910507[3] = 0; + out_7743854232953910507[4] = 0; + out_7743854232953910507[5] = 0; + out_7743854232953910507[6] = 0; + out_7743854232953910507[7] = 0; + out_7743854232953910507[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_8994217345304474551) { - err_fun(nom_x, delta_x, out_8994217345304474551); +void car_err_fun(double *nom_x, double *delta_x, double *out_3321760492551468366) { + err_fun(nom_x, delta_x, out_3321760492551468366); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8635883422281677209) { - inv_err_fun(nom_x, true_x, out_8635883422281677209); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3697485090417495632) { + inv_err_fun(nom_x, true_x, out_3697485090417495632); } -void car_H_mod_fun(double *state, double *out_5835492163818982048) { - H_mod_fun(state, out_5835492163818982048); +void car_H_mod_fun(double *state, double *out_4190227621087458402) { + H_mod_fun(state, out_4190227621087458402); } -void car_f_fun(double *state, double dt, double *out_9046400166044584491) { - f_fun(state, dt, out_9046400166044584491); +void car_f_fun(double *state, double dt, double *out_6456348943274406543) { + f_fun(state, dt, out_6456348943274406543); } -void car_F_fun(double *state, double dt, double *out_8585883916966185499) { - F_fun(state, dt, out_8585883916966185499); +void car_F_fun(double *state, double dt, double *out_4201765287475637381) { + F_fun(state, dt, out_4201765287475637381); } -void car_h_25(double *state, double *unused, double *out_1895365982627284096) { - h_25(state, unused, out_1895365982627284096); +void car_h_25(double *state, double *unused, double *out_9136101134851125551) { + h_25(state, unused, out_9136101134851125551); } -void car_H_25(double *state, double *unused, double *out_3357696062093255189) { - H_25(state, unused, out_3357696062093255189); +void car_H_25(double *state, double *unused, double *out_7713208271076950079) { + H_25(state, unused, out_7713208271076950079); } -void car_h_24(double *state, double *unused, double *out_5360338391580302842) { - h_24(state, unused, out_5360338391580302842); +void car_h_24(double *state, double *unused, double *out_8665340836537612490) { + h_24(state, unused, out_8665340836537612490); } -void car_H_24(double *state, double *unused, double *out_5530345661098754755) { - H_24(state, unused, out_5530345661098754755); +void car_H_24(double *state, double *unused, double *out_7175610203830278401) { + H_24(state, unused, out_7175610203830278401); } -void car_h_30(double *state, double *unused, double *out_2052552650978413241) { - h_30(state, unused, out_2052552650978413241); +void car_h_30(double *state, double *unused, double *out_4641549406917500638) { + h_30(state, unused, out_4641549406917500638); } -void car_H_30(double *state, double *unused, double *out_839363103586006562) { - H_30(state, unused, out_839363103586006562); +void car_H_30(double *state, double *unused, double *out_3185511940949341881) { + H_30(state, unused, out_3185511940949341881); } -void car_h_26(double *state, double *unused, double *out_2655868535654890758) { - h_26(state, unused, out_2655868535654890758); +void car_h_26(double *state, double *unused, double *out_2793594027511200175) { + h_26(state, unused, out_2793594027511200175); } -void car_H_26(double *state, double *unused, double *out_53170092332454588) { - H_26(state, unused, out_53170092332454588); +void car_H_26(double *state, double *unused, double *out_3971704952202893855) { + H_26(state, unused, out_3971704952202893855); } -void car_h_27(double *state, double *unused, double *out_3774772722470275260) { - h_27(state, unused, out_3774772722470275260); +void car_h_27(double *state, double *unused, double *out_5860253322103668962) { + h_27(state, unused, out_5860253322103668962); } -void car_H_27(double *state, double *unused, double *out_1384230967597936655) { - H_27(state, unused, out_1384230967597936655); +void car_H_27(double *state, double *unused, double *out_5409106012133285098) { + H_27(state, unused, out_5409106012133285098); } -void car_h_29(double *state, double *unused, double *out_4656957971787110892) { - h_29(state, unused, out_4656957971787110892); +void car_h_29(double *state, double *unused, double *out_5585059259819163073) { + h_29(state, unused, out_5585059259819163073); } -void car_H_29(double *state, double *unused, double *out_329131759271614378) { - H_29(state, unused, out_329131759271614378); +void car_H_29(double *state, double *unused, double *out_3695743285263734065) { + H_29(state, unused, out_3695743285263734065); } -void car_h_28(double *state, double *unused, double *out_1170367401661577609) { - h_28(state, unused, out_1170367401661577609); +void car_h_28(double *state, double *unused, double *out_6187449234192033508) { + h_28(state, unused, out_6187449234192033508); } -void car_H_28(double *state, double *unused, double *out_5411530776341144952) { - H_28(state, unused, out_5411530776341144952); +void car_H_28(double *state, double *unused, double *out_1386655731805796509) { + H_28(state, unused, out_1386655731805796509); } -void car_h_31(double *state, double *unused, double *out_1437164620468722434) { - h_31(state, unused, out_1437164620468722434); +void car_h_31(double *state, double *unused, double *out_2365265908500774615) { + h_31(state, unused, out_2365265908500774615); } -void car_H_31(double *state, double *unused, double *out_7725407483200662889) { - H_31(state, unused, out_7725407483200662889); +void car_H_31(double *state, double *unused, double *out_7743854232953910507) { + H_31(state, unused, out_7743854232953910507); } 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 a4aa6ece9..8e602ba77 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_8994217345304474551); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8635883422281677209); -void car_H_mod_fun(double *state, double *out_5835492163818982048); -void car_f_fun(double *state, double dt, double *out_9046400166044584491); -void car_F_fun(double *state, double dt, double *out_8585883916966185499); -void car_h_25(double *state, double *unused, double *out_1895365982627284096); -void car_H_25(double *state, double *unused, double *out_3357696062093255189); -void car_h_24(double *state, double *unused, double *out_5360338391580302842); -void car_H_24(double *state, double *unused, double *out_5530345661098754755); -void car_h_30(double *state, double *unused, double *out_2052552650978413241); -void car_H_30(double *state, double *unused, double *out_839363103586006562); -void car_h_26(double *state, double *unused, double *out_2655868535654890758); -void car_H_26(double *state, double *unused, double *out_53170092332454588); -void car_h_27(double *state, double *unused, double *out_3774772722470275260); -void car_H_27(double *state, double *unused, double *out_1384230967597936655); -void car_h_29(double *state, double *unused, double *out_4656957971787110892); -void car_H_29(double *state, double *unused, double *out_329131759271614378); -void car_h_28(double *state, double *unused, double *out_1170367401661577609); -void car_H_28(double *state, double *unused, double *out_5411530776341144952); -void car_h_31(double *state, double *unused, double *out_1437164620468722434); -void car_H_31(double *state, double *unused, double *out_7725407483200662889); +void car_err_fun(double *nom_x, double *delta_x, double *out_3321760492551468366); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3697485090417495632); +void car_H_mod_fun(double *state, double *out_4190227621087458402); +void car_f_fun(double *state, double dt, double *out_6456348943274406543); +void car_F_fun(double *state, double dt, double *out_4201765287475637381); +void car_h_25(double *state, double *unused, double *out_9136101134851125551); +void car_H_25(double *state, double *unused, double *out_7713208271076950079); +void car_h_24(double *state, double *unused, double *out_8665340836537612490); +void car_H_24(double *state, double *unused, double *out_7175610203830278401); +void car_h_30(double *state, double *unused, double *out_4641549406917500638); +void car_H_30(double *state, double *unused, double *out_3185511940949341881); +void car_h_26(double *state, double *unused, double *out_2793594027511200175); +void car_H_26(double *state, double *unused, double *out_3971704952202893855); +void car_h_27(double *state, double *unused, double *out_5860253322103668962); +void car_H_27(double *state, double *unused, double *out_5409106012133285098); +void car_h_29(double *state, double *unused, double *out_5585059259819163073); +void car_H_29(double *state, double *unused, double *out_3695743285263734065); +void car_h_28(double *state, double *unused, double *out_6187449234192033508); +void car_H_28(double *state, double *unused, double *out_1386655731805796509); +void car_h_31(double *state, double *unused, double *out_2365265908500774615); +void car_H_31(double *state, double *unused, double *out_7743854232953910507); 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 088874416..5a086344a 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_5188089899736494345) { - out_5188089899736494345[0] = delta_x[0] + nom_x[0]; - out_5188089899736494345[1] = delta_x[1] + nom_x[1]; - out_5188089899736494345[2] = delta_x[2] + nom_x[2]; - out_5188089899736494345[3] = delta_x[3] + nom_x[3]; - out_5188089899736494345[4] = delta_x[4] + nom_x[4]; - out_5188089899736494345[5] = delta_x[5] + nom_x[5]; - out_5188089899736494345[6] = delta_x[6] + nom_x[6]; - out_5188089899736494345[7] = delta_x[7] + nom_x[7]; - out_5188089899736494345[8] = delta_x[8] + nom_x[8]; - out_5188089899736494345[9] = delta_x[9] + nom_x[9]; - out_5188089899736494345[10] = delta_x[10] + nom_x[10]; - out_5188089899736494345[11] = delta_x[11] + nom_x[11]; - out_5188089899736494345[12] = delta_x[12] + nom_x[12]; - out_5188089899736494345[13] = delta_x[13] + nom_x[13]; - out_5188089899736494345[14] = delta_x[14] + nom_x[14]; - out_5188089899736494345[15] = delta_x[15] + nom_x[15]; - out_5188089899736494345[16] = delta_x[16] + nom_x[16]; - out_5188089899736494345[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_6247163002975687744) { + out_6247163002975687744[0] = delta_x[0] + nom_x[0]; + out_6247163002975687744[1] = delta_x[1] + nom_x[1]; + out_6247163002975687744[2] = delta_x[2] + nom_x[2]; + out_6247163002975687744[3] = delta_x[3] + nom_x[3]; + out_6247163002975687744[4] = delta_x[4] + nom_x[4]; + out_6247163002975687744[5] = delta_x[5] + nom_x[5]; + out_6247163002975687744[6] = delta_x[6] + nom_x[6]; + out_6247163002975687744[7] = delta_x[7] + nom_x[7]; + out_6247163002975687744[8] = delta_x[8] + nom_x[8]; + out_6247163002975687744[9] = delta_x[9] + nom_x[9]; + out_6247163002975687744[10] = delta_x[10] + nom_x[10]; + out_6247163002975687744[11] = delta_x[11] + nom_x[11]; + out_6247163002975687744[12] = delta_x[12] + nom_x[12]; + out_6247163002975687744[13] = delta_x[13] + nom_x[13]; + out_6247163002975687744[14] = delta_x[14] + nom_x[14]; + out_6247163002975687744[15] = delta_x[15] + nom_x[15]; + out_6247163002975687744[16] = delta_x[16] + nom_x[16]; + out_6247163002975687744[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_516319331577825853) { - out_516319331577825853[0] = -nom_x[0] + true_x[0]; - out_516319331577825853[1] = -nom_x[1] + true_x[1]; - out_516319331577825853[2] = -nom_x[2] + true_x[2]; - out_516319331577825853[3] = -nom_x[3] + true_x[3]; - out_516319331577825853[4] = -nom_x[4] + true_x[4]; - out_516319331577825853[5] = -nom_x[5] + true_x[5]; - out_516319331577825853[6] = -nom_x[6] + true_x[6]; - out_516319331577825853[7] = -nom_x[7] + true_x[7]; - out_516319331577825853[8] = -nom_x[8] + true_x[8]; - out_516319331577825853[9] = -nom_x[9] + true_x[9]; - out_516319331577825853[10] = -nom_x[10] + true_x[10]; - out_516319331577825853[11] = -nom_x[11] + true_x[11]; - out_516319331577825853[12] = -nom_x[12] + true_x[12]; - out_516319331577825853[13] = -nom_x[13] + true_x[13]; - out_516319331577825853[14] = -nom_x[14] + true_x[14]; - out_516319331577825853[15] = -nom_x[15] + true_x[15]; - out_516319331577825853[16] = -nom_x[16] + true_x[16]; - out_516319331577825853[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2784495282432373016) { + out_2784495282432373016[0] = -nom_x[0] + true_x[0]; + out_2784495282432373016[1] = -nom_x[1] + true_x[1]; + out_2784495282432373016[2] = -nom_x[2] + true_x[2]; + out_2784495282432373016[3] = -nom_x[3] + true_x[3]; + out_2784495282432373016[4] = -nom_x[4] + true_x[4]; + out_2784495282432373016[5] = -nom_x[5] + true_x[5]; + out_2784495282432373016[6] = -nom_x[6] + true_x[6]; + out_2784495282432373016[7] = -nom_x[7] + true_x[7]; + out_2784495282432373016[8] = -nom_x[8] + true_x[8]; + out_2784495282432373016[9] = -nom_x[9] + true_x[9]; + out_2784495282432373016[10] = -nom_x[10] + true_x[10]; + out_2784495282432373016[11] = -nom_x[11] + true_x[11]; + out_2784495282432373016[12] = -nom_x[12] + true_x[12]; + out_2784495282432373016[13] = -nom_x[13] + true_x[13]; + out_2784495282432373016[14] = -nom_x[14] + true_x[14]; + out_2784495282432373016[15] = -nom_x[15] + true_x[15]; + out_2784495282432373016[16] = -nom_x[16] + true_x[16]; + out_2784495282432373016[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_4951567132479851062) { - out_4951567132479851062[0] = 1.0; - out_4951567132479851062[1] = 0.0; - out_4951567132479851062[2] = 0.0; - out_4951567132479851062[3] = 0.0; - out_4951567132479851062[4] = 0.0; - out_4951567132479851062[5] = 0.0; - out_4951567132479851062[6] = 0.0; - out_4951567132479851062[7] = 0.0; - out_4951567132479851062[8] = 0.0; - out_4951567132479851062[9] = 0.0; - out_4951567132479851062[10] = 0.0; - out_4951567132479851062[11] = 0.0; - out_4951567132479851062[12] = 0.0; - out_4951567132479851062[13] = 0.0; - out_4951567132479851062[14] = 0.0; - out_4951567132479851062[15] = 0.0; - out_4951567132479851062[16] = 0.0; - out_4951567132479851062[17] = 0.0; - out_4951567132479851062[18] = 0.0; - out_4951567132479851062[19] = 1.0; - out_4951567132479851062[20] = 0.0; - out_4951567132479851062[21] = 0.0; - out_4951567132479851062[22] = 0.0; - out_4951567132479851062[23] = 0.0; - out_4951567132479851062[24] = 0.0; - out_4951567132479851062[25] = 0.0; - out_4951567132479851062[26] = 0.0; - out_4951567132479851062[27] = 0.0; - out_4951567132479851062[28] = 0.0; - out_4951567132479851062[29] = 0.0; - out_4951567132479851062[30] = 0.0; - out_4951567132479851062[31] = 0.0; - out_4951567132479851062[32] = 0.0; - out_4951567132479851062[33] = 0.0; - out_4951567132479851062[34] = 0.0; - out_4951567132479851062[35] = 0.0; - out_4951567132479851062[36] = 0.0; - out_4951567132479851062[37] = 0.0; - out_4951567132479851062[38] = 1.0; - out_4951567132479851062[39] = 0.0; - out_4951567132479851062[40] = 0.0; - out_4951567132479851062[41] = 0.0; - out_4951567132479851062[42] = 0.0; - out_4951567132479851062[43] = 0.0; - out_4951567132479851062[44] = 0.0; - out_4951567132479851062[45] = 0.0; - out_4951567132479851062[46] = 0.0; - out_4951567132479851062[47] = 0.0; - out_4951567132479851062[48] = 0.0; - out_4951567132479851062[49] = 0.0; - out_4951567132479851062[50] = 0.0; - out_4951567132479851062[51] = 0.0; - out_4951567132479851062[52] = 0.0; - out_4951567132479851062[53] = 0.0; - out_4951567132479851062[54] = 0.0; - out_4951567132479851062[55] = 0.0; - out_4951567132479851062[56] = 0.0; - out_4951567132479851062[57] = 1.0; - out_4951567132479851062[58] = 0.0; - out_4951567132479851062[59] = 0.0; - out_4951567132479851062[60] = 0.0; - out_4951567132479851062[61] = 0.0; - out_4951567132479851062[62] = 0.0; - out_4951567132479851062[63] = 0.0; - out_4951567132479851062[64] = 0.0; - out_4951567132479851062[65] = 0.0; - out_4951567132479851062[66] = 0.0; - out_4951567132479851062[67] = 0.0; - out_4951567132479851062[68] = 0.0; - out_4951567132479851062[69] = 0.0; - out_4951567132479851062[70] = 0.0; - out_4951567132479851062[71] = 0.0; - out_4951567132479851062[72] = 0.0; - out_4951567132479851062[73] = 0.0; - out_4951567132479851062[74] = 0.0; - out_4951567132479851062[75] = 0.0; - out_4951567132479851062[76] = 1.0; - out_4951567132479851062[77] = 0.0; - out_4951567132479851062[78] = 0.0; - out_4951567132479851062[79] = 0.0; - out_4951567132479851062[80] = 0.0; - out_4951567132479851062[81] = 0.0; - out_4951567132479851062[82] = 0.0; - out_4951567132479851062[83] = 0.0; - out_4951567132479851062[84] = 0.0; - out_4951567132479851062[85] = 0.0; - out_4951567132479851062[86] = 0.0; - out_4951567132479851062[87] = 0.0; - out_4951567132479851062[88] = 0.0; - out_4951567132479851062[89] = 0.0; - out_4951567132479851062[90] = 0.0; - out_4951567132479851062[91] = 0.0; - out_4951567132479851062[92] = 0.0; - out_4951567132479851062[93] = 0.0; - out_4951567132479851062[94] = 0.0; - out_4951567132479851062[95] = 1.0; - out_4951567132479851062[96] = 0.0; - out_4951567132479851062[97] = 0.0; - out_4951567132479851062[98] = 0.0; - out_4951567132479851062[99] = 0.0; - out_4951567132479851062[100] = 0.0; - out_4951567132479851062[101] = 0.0; - out_4951567132479851062[102] = 0.0; - out_4951567132479851062[103] = 0.0; - out_4951567132479851062[104] = 0.0; - out_4951567132479851062[105] = 0.0; - out_4951567132479851062[106] = 0.0; - out_4951567132479851062[107] = 0.0; - out_4951567132479851062[108] = 0.0; - out_4951567132479851062[109] = 0.0; - out_4951567132479851062[110] = 0.0; - out_4951567132479851062[111] = 0.0; - out_4951567132479851062[112] = 0.0; - out_4951567132479851062[113] = 0.0; - out_4951567132479851062[114] = 1.0; - out_4951567132479851062[115] = 0.0; - out_4951567132479851062[116] = 0.0; - out_4951567132479851062[117] = 0.0; - out_4951567132479851062[118] = 0.0; - out_4951567132479851062[119] = 0.0; - out_4951567132479851062[120] = 0.0; - out_4951567132479851062[121] = 0.0; - out_4951567132479851062[122] = 0.0; - out_4951567132479851062[123] = 0.0; - out_4951567132479851062[124] = 0.0; - out_4951567132479851062[125] = 0.0; - out_4951567132479851062[126] = 0.0; - out_4951567132479851062[127] = 0.0; - out_4951567132479851062[128] = 0.0; - out_4951567132479851062[129] = 0.0; - out_4951567132479851062[130] = 0.0; - out_4951567132479851062[131] = 0.0; - out_4951567132479851062[132] = 0.0; - out_4951567132479851062[133] = 1.0; - out_4951567132479851062[134] = 0.0; - out_4951567132479851062[135] = 0.0; - out_4951567132479851062[136] = 0.0; - out_4951567132479851062[137] = 0.0; - out_4951567132479851062[138] = 0.0; - out_4951567132479851062[139] = 0.0; - out_4951567132479851062[140] = 0.0; - out_4951567132479851062[141] = 0.0; - out_4951567132479851062[142] = 0.0; - out_4951567132479851062[143] = 0.0; - out_4951567132479851062[144] = 0.0; - out_4951567132479851062[145] = 0.0; - out_4951567132479851062[146] = 0.0; - out_4951567132479851062[147] = 0.0; - out_4951567132479851062[148] = 0.0; - out_4951567132479851062[149] = 0.0; - out_4951567132479851062[150] = 0.0; - out_4951567132479851062[151] = 0.0; - out_4951567132479851062[152] = 1.0; - out_4951567132479851062[153] = 0.0; - out_4951567132479851062[154] = 0.0; - out_4951567132479851062[155] = 0.0; - out_4951567132479851062[156] = 0.0; - out_4951567132479851062[157] = 0.0; - out_4951567132479851062[158] = 0.0; - out_4951567132479851062[159] = 0.0; - out_4951567132479851062[160] = 0.0; - out_4951567132479851062[161] = 0.0; - out_4951567132479851062[162] = 0.0; - out_4951567132479851062[163] = 0.0; - out_4951567132479851062[164] = 0.0; - out_4951567132479851062[165] = 0.0; - out_4951567132479851062[166] = 0.0; - out_4951567132479851062[167] = 0.0; - out_4951567132479851062[168] = 0.0; - out_4951567132479851062[169] = 0.0; - out_4951567132479851062[170] = 0.0; - out_4951567132479851062[171] = 1.0; - out_4951567132479851062[172] = 0.0; - out_4951567132479851062[173] = 0.0; - out_4951567132479851062[174] = 0.0; - out_4951567132479851062[175] = 0.0; - out_4951567132479851062[176] = 0.0; - out_4951567132479851062[177] = 0.0; - out_4951567132479851062[178] = 0.0; - out_4951567132479851062[179] = 0.0; - out_4951567132479851062[180] = 0.0; - out_4951567132479851062[181] = 0.0; - out_4951567132479851062[182] = 0.0; - out_4951567132479851062[183] = 0.0; - out_4951567132479851062[184] = 0.0; - out_4951567132479851062[185] = 0.0; - out_4951567132479851062[186] = 0.0; - out_4951567132479851062[187] = 0.0; - out_4951567132479851062[188] = 0.0; - out_4951567132479851062[189] = 0.0; - out_4951567132479851062[190] = 1.0; - out_4951567132479851062[191] = 0.0; - out_4951567132479851062[192] = 0.0; - out_4951567132479851062[193] = 0.0; - out_4951567132479851062[194] = 0.0; - out_4951567132479851062[195] = 0.0; - out_4951567132479851062[196] = 0.0; - out_4951567132479851062[197] = 0.0; - out_4951567132479851062[198] = 0.0; - out_4951567132479851062[199] = 0.0; - out_4951567132479851062[200] = 0.0; - out_4951567132479851062[201] = 0.0; - out_4951567132479851062[202] = 0.0; - out_4951567132479851062[203] = 0.0; - out_4951567132479851062[204] = 0.0; - out_4951567132479851062[205] = 0.0; - out_4951567132479851062[206] = 0.0; - out_4951567132479851062[207] = 0.0; - out_4951567132479851062[208] = 0.0; - out_4951567132479851062[209] = 1.0; - out_4951567132479851062[210] = 0.0; - out_4951567132479851062[211] = 0.0; - out_4951567132479851062[212] = 0.0; - out_4951567132479851062[213] = 0.0; - out_4951567132479851062[214] = 0.0; - out_4951567132479851062[215] = 0.0; - out_4951567132479851062[216] = 0.0; - out_4951567132479851062[217] = 0.0; - out_4951567132479851062[218] = 0.0; - out_4951567132479851062[219] = 0.0; - out_4951567132479851062[220] = 0.0; - out_4951567132479851062[221] = 0.0; - out_4951567132479851062[222] = 0.0; - out_4951567132479851062[223] = 0.0; - out_4951567132479851062[224] = 0.0; - out_4951567132479851062[225] = 0.0; - out_4951567132479851062[226] = 0.0; - out_4951567132479851062[227] = 0.0; - out_4951567132479851062[228] = 1.0; - out_4951567132479851062[229] = 0.0; - out_4951567132479851062[230] = 0.0; - out_4951567132479851062[231] = 0.0; - out_4951567132479851062[232] = 0.0; - out_4951567132479851062[233] = 0.0; - out_4951567132479851062[234] = 0.0; - out_4951567132479851062[235] = 0.0; - out_4951567132479851062[236] = 0.0; - out_4951567132479851062[237] = 0.0; - out_4951567132479851062[238] = 0.0; - out_4951567132479851062[239] = 0.0; - out_4951567132479851062[240] = 0.0; - out_4951567132479851062[241] = 0.0; - out_4951567132479851062[242] = 0.0; - out_4951567132479851062[243] = 0.0; - out_4951567132479851062[244] = 0.0; - out_4951567132479851062[245] = 0.0; - out_4951567132479851062[246] = 0.0; - out_4951567132479851062[247] = 1.0; - out_4951567132479851062[248] = 0.0; - out_4951567132479851062[249] = 0.0; - out_4951567132479851062[250] = 0.0; - out_4951567132479851062[251] = 0.0; - out_4951567132479851062[252] = 0.0; - out_4951567132479851062[253] = 0.0; - out_4951567132479851062[254] = 0.0; - out_4951567132479851062[255] = 0.0; - out_4951567132479851062[256] = 0.0; - out_4951567132479851062[257] = 0.0; - out_4951567132479851062[258] = 0.0; - out_4951567132479851062[259] = 0.0; - out_4951567132479851062[260] = 0.0; - out_4951567132479851062[261] = 0.0; - out_4951567132479851062[262] = 0.0; - out_4951567132479851062[263] = 0.0; - out_4951567132479851062[264] = 0.0; - out_4951567132479851062[265] = 0.0; - out_4951567132479851062[266] = 1.0; - out_4951567132479851062[267] = 0.0; - out_4951567132479851062[268] = 0.0; - out_4951567132479851062[269] = 0.0; - out_4951567132479851062[270] = 0.0; - out_4951567132479851062[271] = 0.0; - out_4951567132479851062[272] = 0.0; - out_4951567132479851062[273] = 0.0; - out_4951567132479851062[274] = 0.0; - out_4951567132479851062[275] = 0.0; - out_4951567132479851062[276] = 0.0; - out_4951567132479851062[277] = 0.0; - out_4951567132479851062[278] = 0.0; - out_4951567132479851062[279] = 0.0; - out_4951567132479851062[280] = 0.0; - out_4951567132479851062[281] = 0.0; - out_4951567132479851062[282] = 0.0; - out_4951567132479851062[283] = 0.0; - out_4951567132479851062[284] = 0.0; - out_4951567132479851062[285] = 1.0; - out_4951567132479851062[286] = 0.0; - out_4951567132479851062[287] = 0.0; - out_4951567132479851062[288] = 0.0; - out_4951567132479851062[289] = 0.0; - out_4951567132479851062[290] = 0.0; - out_4951567132479851062[291] = 0.0; - out_4951567132479851062[292] = 0.0; - out_4951567132479851062[293] = 0.0; - out_4951567132479851062[294] = 0.0; - out_4951567132479851062[295] = 0.0; - out_4951567132479851062[296] = 0.0; - out_4951567132479851062[297] = 0.0; - out_4951567132479851062[298] = 0.0; - out_4951567132479851062[299] = 0.0; - out_4951567132479851062[300] = 0.0; - out_4951567132479851062[301] = 0.0; - out_4951567132479851062[302] = 0.0; - out_4951567132479851062[303] = 0.0; - out_4951567132479851062[304] = 1.0; - out_4951567132479851062[305] = 0.0; - out_4951567132479851062[306] = 0.0; - out_4951567132479851062[307] = 0.0; - out_4951567132479851062[308] = 0.0; - out_4951567132479851062[309] = 0.0; - out_4951567132479851062[310] = 0.0; - out_4951567132479851062[311] = 0.0; - out_4951567132479851062[312] = 0.0; - out_4951567132479851062[313] = 0.0; - out_4951567132479851062[314] = 0.0; - out_4951567132479851062[315] = 0.0; - out_4951567132479851062[316] = 0.0; - out_4951567132479851062[317] = 0.0; - out_4951567132479851062[318] = 0.0; - out_4951567132479851062[319] = 0.0; - out_4951567132479851062[320] = 0.0; - out_4951567132479851062[321] = 0.0; - out_4951567132479851062[322] = 0.0; - out_4951567132479851062[323] = 1.0; +void H_mod_fun(double *state, double *out_662979392671356586) { + out_662979392671356586[0] = 1.0; + out_662979392671356586[1] = 0.0; + out_662979392671356586[2] = 0.0; + out_662979392671356586[3] = 0.0; + out_662979392671356586[4] = 0.0; + out_662979392671356586[5] = 0.0; + out_662979392671356586[6] = 0.0; + out_662979392671356586[7] = 0.0; + out_662979392671356586[8] = 0.0; + out_662979392671356586[9] = 0.0; + out_662979392671356586[10] = 0.0; + out_662979392671356586[11] = 0.0; + out_662979392671356586[12] = 0.0; + out_662979392671356586[13] = 0.0; + out_662979392671356586[14] = 0.0; + out_662979392671356586[15] = 0.0; + out_662979392671356586[16] = 0.0; + out_662979392671356586[17] = 0.0; + out_662979392671356586[18] = 0.0; + out_662979392671356586[19] = 1.0; + out_662979392671356586[20] = 0.0; + out_662979392671356586[21] = 0.0; + out_662979392671356586[22] = 0.0; + out_662979392671356586[23] = 0.0; + out_662979392671356586[24] = 0.0; + out_662979392671356586[25] = 0.0; + out_662979392671356586[26] = 0.0; + out_662979392671356586[27] = 0.0; + out_662979392671356586[28] = 0.0; + out_662979392671356586[29] = 0.0; + out_662979392671356586[30] = 0.0; + out_662979392671356586[31] = 0.0; + out_662979392671356586[32] = 0.0; + out_662979392671356586[33] = 0.0; + out_662979392671356586[34] = 0.0; + out_662979392671356586[35] = 0.0; + out_662979392671356586[36] = 0.0; + out_662979392671356586[37] = 0.0; + out_662979392671356586[38] = 1.0; + out_662979392671356586[39] = 0.0; + out_662979392671356586[40] = 0.0; + out_662979392671356586[41] = 0.0; + out_662979392671356586[42] = 0.0; + out_662979392671356586[43] = 0.0; + out_662979392671356586[44] = 0.0; + out_662979392671356586[45] = 0.0; + out_662979392671356586[46] = 0.0; + out_662979392671356586[47] = 0.0; + out_662979392671356586[48] = 0.0; + out_662979392671356586[49] = 0.0; + out_662979392671356586[50] = 0.0; + out_662979392671356586[51] = 0.0; + out_662979392671356586[52] = 0.0; + out_662979392671356586[53] = 0.0; + out_662979392671356586[54] = 0.0; + out_662979392671356586[55] = 0.0; + out_662979392671356586[56] = 0.0; + out_662979392671356586[57] = 1.0; + out_662979392671356586[58] = 0.0; + out_662979392671356586[59] = 0.0; + out_662979392671356586[60] = 0.0; + out_662979392671356586[61] = 0.0; + out_662979392671356586[62] = 0.0; + out_662979392671356586[63] = 0.0; + out_662979392671356586[64] = 0.0; + out_662979392671356586[65] = 0.0; + out_662979392671356586[66] = 0.0; + out_662979392671356586[67] = 0.0; + out_662979392671356586[68] = 0.0; + out_662979392671356586[69] = 0.0; + out_662979392671356586[70] = 0.0; + out_662979392671356586[71] = 0.0; + out_662979392671356586[72] = 0.0; + out_662979392671356586[73] = 0.0; + out_662979392671356586[74] = 0.0; + out_662979392671356586[75] = 0.0; + out_662979392671356586[76] = 1.0; + out_662979392671356586[77] = 0.0; + out_662979392671356586[78] = 0.0; + out_662979392671356586[79] = 0.0; + out_662979392671356586[80] = 0.0; + out_662979392671356586[81] = 0.0; + out_662979392671356586[82] = 0.0; + out_662979392671356586[83] = 0.0; + out_662979392671356586[84] = 0.0; + out_662979392671356586[85] = 0.0; + out_662979392671356586[86] = 0.0; + out_662979392671356586[87] = 0.0; + out_662979392671356586[88] = 0.0; + out_662979392671356586[89] = 0.0; + out_662979392671356586[90] = 0.0; + out_662979392671356586[91] = 0.0; + out_662979392671356586[92] = 0.0; + out_662979392671356586[93] = 0.0; + out_662979392671356586[94] = 0.0; + out_662979392671356586[95] = 1.0; + out_662979392671356586[96] = 0.0; + out_662979392671356586[97] = 0.0; + out_662979392671356586[98] = 0.0; + out_662979392671356586[99] = 0.0; + out_662979392671356586[100] = 0.0; + out_662979392671356586[101] = 0.0; + out_662979392671356586[102] = 0.0; + out_662979392671356586[103] = 0.0; + out_662979392671356586[104] = 0.0; + out_662979392671356586[105] = 0.0; + out_662979392671356586[106] = 0.0; + out_662979392671356586[107] = 0.0; + out_662979392671356586[108] = 0.0; + out_662979392671356586[109] = 0.0; + out_662979392671356586[110] = 0.0; + out_662979392671356586[111] = 0.0; + out_662979392671356586[112] = 0.0; + out_662979392671356586[113] = 0.0; + out_662979392671356586[114] = 1.0; + out_662979392671356586[115] = 0.0; + out_662979392671356586[116] = 0.0; + out_662979392671356586[117] = 0.0; + out_662979392671356586[118] = 0.0; + out_662979392671356586[119] = 0.0; + out_662979392671356586[120] = 0.0; + out_662979392671356586[121] = 0.0; + out_662979392671356586[122] = 0.0; + out_662979392671356586[123] = 0.0; + out_662979392671356586[124] = 0.0; + out_662979392671356586[125] = 0.0; + out_662979392671356586[126] = 0.0; + out_662979392671356586[127] = 0.0; + out_662979392671356586[128] = 0.0; + out_662979392671356586[129] = 0.0; + out_662979392671356586[130] = 0.0; + out_662979392671356586[131] = 0.0; + out_662979392671356586[132] = 0.0; + out_662979392671356586[133] = 1.0; + out_662979392671356586[134] = 0.0; + out_662979392671356586[135] = 0.0; + out_662979392671356586[136] = 0.0; + out_662979392671356586[137] = 0.0; + out_662979392671356586[138] = 0.0; + out_662979392671356586[139] = 0.0; + out_662979392671356586[140] = 0.0; + out_662979392671356586[141] = 0.0; + out_662979392671356586[142] = 0.0; + out_662979392671356586[143] = 0.0; + out_662979392671356586[144] = 0.0; + out_662979392671356586[145] = 0.0; + out_662979392671356586[146] = 0.0; + out_662979392671356586[147] = 0.0; + out_662979392671356586[148] = 0.0; + out_662979392671356586[149] = 0.0; + out_662979392671356586[150] = 0.0; + out_662979392671356586[151] = 0.0; + out_662979392671356586[152] = 1.0; + out_662979392671356586[153] = 0.0; + out_662979392671356586[154] = 0.0; + out_662979392671356586[155] = 0.0; + out_662979392671356586[156] = 0.0; + out_662979392671356586[157] = 0.0; + out_662979392671356586[158] = 0.0; + out_662979392671356586[159] = 0.0; + out_662979392671356586[160] = 0.0; + out_662979392671356586[161] = 0.0; + out_662979392671356586[162] = 0.0; + out_662979392671356586[163] = 0.0; + out_662979392671356586[164] = 0.0; + out_662979392671356586[165] = 0.0; + out_662979392671356586[166] = 0.0; + out_662979392671356586[167] = 0.0; + out_662979392671356586[168] = 0.0; + out_662979392671356586[169] = 0.0; + out_662979392671356586[170] = 0.0; + out_662979392671356586[171] = 1.0; + out_662979392671356586[172] = 0.0; + out_662979392671356586[173] = 0.0; + out_662979392671356586[174] = 0.0; + out_662979392671356586[175] = 0.0; + out_662979392671356586[176] = 0.0; + out_662979392671356586[177] = 0.0; + out_662979392671356586[178] = 0.0; + out_662979392671356586[179] = 0.0; + out_662979392671356586[180] = 0.0; + out_662979392671356586[181] = 0.0; + out_662979392671356586[182] = 0.0; + out_662979392671356586[183] = 0.0; + out_662979392671356586[184] = 0.0; + out_662979392671356586[185] = 0.0; + out_662979392671356586[186] = 0.0; + out_662979392671356586[187] = 0.0; + out_662979392671356586[188] = 0.0; + out_662979392671356586[189] = 0.0; + out_662979392671356586[190] = 1.0; + out_662979392671356586[191] = 0.0; + out_662979392671356586[192] = 0.0; + out_662979392671356586[193] = 0.0; + out_662979392671356586[194] = 0.0; + out_662979392671356586[195] = 0.0; + out_662979392671356586[196] = 0.0; + out_662979392671356586[197] = 0.0; + out_662979392671356586[198] = 0.0; + out_662979392671356586[199] = 0.0; + out_662979392671356586[200] = 0.0; + out_662979392671356586[201] = 0.0; + out_662979392671356586[202] = 0.0; + out_662979392671356586[203] = 0.0; + out_662979392671356586[204] = 0.0; + out_662979392671356586[205] = 0.0; + out_662979392671356586[206] = 0.0; + out_662979392671356586[207] = 0.0; + out_662979392671356586[208] = 0.0; + out_662979392671356586[209] = 1.0; + out_662979392671356586[210] = 0.0; + out_662979392671356586[211] = 0.0; + out_662979392671356586[212] = 0.0; + out_662979392671356586[213] = 0.0; + out_662979392671356586[214] = 0.0; + out_662979392671356586[215] = 0.0; + out_662979392671356586[216] = 0.0; + out_662979392671356586[217] = 0.0; + out_662979392671356586[218] = 0.0; + out_662979392671356586[219] = 0.0; + out_662979392671356586[220] = 0.0; + out_662979392671356586[221] = 0.0; + out_662979392671356586[222] = 0.0; + out_662979392671356586[223] = 0.0; + out_662979392671356586[224] = 0.0; + out_662979392671356586[225] = 0.0; + out_662979392671356586[226] = 0.0; + out_662979392671356586[227] = 0.0; + out_662979392671356586[228] = 1.0; + out_662979392671356586[229] = 0.0; + out_662979392671356586[230] = 0.0; + out_662979392671356586[231] = 0.0; + out_662979392671356586[232] = 0.0; + out_662979392671356586[233] = 0.0; + out_662979392671356586[234] = 0.0; + out_662979392671356586[235] = 0.0; + out_662979392671356586[236] = 0.0; + out_662979392671356586[237] = 0.0; + out_662979392671356586[238] = 0.0; + out_662979392671356586[239] = 0.0; + out_662979392671356586[240] = 0.0; + out_662979392671356586[241] = 0.0; + out_662979392671356586[242] = 0.0; + out_662979392671356586[243] = 0.0; + out_662979392671356586[244] = 0.0; + out_662979392671356586[245] = 0.0; + out_662979392671356586[246] = 0.0; + out_662979392671356586[247] = 1.0; + out_662979392671356586[248] = 0.0; + out_662979392671356586[249] = 0.0; + out_662979392671356586[250] = 0.0; + out_662979392671356586[251] = 0.0; + out_662979392671356586[252] = 0.0; + out_662979392671356586[253] = 0.0; + out_662979392671356586[254] = 0.0; + out_662979392671356586[255] = 0.0; + out_662979392671356586[256] = 0.0; + out_662979392671356586[257] = 0.0; + out_662979392671356586[258] = 0.0; + out_662979392671356586[259] = 0.0; + out_662979392671356586[260] = 0.0; + out_662979392671356586[261] = 0.0; + out_662979392671356586[262] = 0.0; + out_662979392671356586[263] = 0.0; + out_662979392671356586[264] = 0.0; + out_662979392671356586[265] = 0.0; + out_662979392671356586[266] = 1.0; + out_662979392671356586[267] = 0.0; + out_662979392671356586[268] = 0.0; + out_662979392671356586[269] = 0.0; + out_662979392671356586[270] = 0.0; + out_662979392671356586[271] = 0.0; + out_662979392671356586[272] = 0.0; + out_662979392671356586[273] = 0.0; + out_662979392671356586[274] = 0.0; + out_662979392671356586[275] = 0.0; + out_662979392671356586[276] = 0.0; + out_662979392671356586[277] = 0.0; + out_662979392671356586[278] = 0.0; + out_662979392671356586[279] = 0.0; + out_662979392671356586[280] = 0.0; + out_662979392671356586[281] = 0.0; + out_662979392671356586[282] = 0.0; + out_662979392671356586[283] = 0.0; + out_662979392671356586[284] = 0.0; + out_662979392671356586[285] = 1.0; + out_662979392671356586[286] = 0.0; + out_662979392671356586[287] = 0.0; + out_662979392671356586[288] = 0.0; + out_662979392671356586[289] = 0.0; + out_662979392671356586[290] = 0.0; + out_662979392671356586[291] = 0.0; + out_662979392671356586[292] = 0.0; + out_662979392671356586[293] = 0.0; + out_662979392671356586[294] = 0.0; + out_662979392671356586[295] = 0.0; + out_662979392671356586[296] = 0.0; + out_662979392671356586[297] = 0.0; + out_662979392671356586[298] = 0.0; + out_662979392671356586[299] = 0.0; + out_662979392671356586[300] = 0.0; + out_662979392671356586[301] = 0.0; + out_662979392671356586[302] = 0.0; + out_662979392671356586[303] = 0.0; + out_662979392671356586[304] = 1.0; + out_662979392671356586[305] = 0.0; + out_662979392671356586[306] = 0.0; + out_662979392671356586[307] = 0.0; + out_662979392671356586[308] = 0.0; + out_662979392671356586[309] = 0.0; + out_662979392671356586[310] = 0.0; + out_662979392671356586[311] = 0.0; + out_662979392671356586[312] = 0.0; + out_662979392671356586[313] = 0.0; + out_662979392671356586[314] = 0.0; + out_662979392671356586[315] = 0.0; + out_662979392671356586[316] = 0.0; + out_662979392671356586[317] = 0.0; + out_662979392671356586[318] = 0.0; + out_662979392671356586[319] = 0.0; + out_662979392671356586[320] = 0.0; + out_662979392671356586[321] = 0.0; + out_662979392671356586[322] = 0.0; + out_662979392671356586[323] = 1.0; } -void f_fun(double *state, double dt, double *out_898454825503606476) { - out_898454825503606476[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_898454825503606476[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_898454825503606476[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_898454825503606476[3] = dt*state[12] + state[3]; - out_898454825503606476[4] = dt*state[13] + state[4]; - out_898454825503606476[5] = dt*state[14] + state[5]; - out_898454825503606476[6] = state[6]; - out_898454825503606476[7] = state[7]; - out_898454825503606476[8] = state[8]; - out_898454825503606476[9] = state[9]; - out_898454825503606476[10] = state[10]; - out_898454825503606476[11] = state[11]; - out_898454825503606476[12] = state[12]; - out_898454825503606476[13] = state[13]; - out_898454825503606476[14] = state[14]; - out_898454825503606476[15] = state[15]; - out_898454825503606476[16] = state[16]; - out_898454825503606476[17] = state[17]; +void f_fun(double *state, double dt, double *out_5358842043988680887) { + out_5358842043988680887[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_5358842043988680887[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_5358842043988680887[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_5358842043988680887[3] = dt*state[12] + state[3]; + out_5358842043988680887[4] = dt*state[13] + state[4]; + out_5358842043988680887[5] = dt*state[14] + state[5]; + out_5358842043988680887[6] = state[6]; + out_5358842043988680887[7] = state[7]; + out_5358842043988680887[8] = state[8]; + out_5358842043988680887[9] = state[9]; + out_5358842043988680887[10] = state[10]; + out_5358842043988680887[11] = state[11]; + out_5358842043988680887[12] = state[12]; + out_5358842043988680887[13] = state[13]; + out_5358842043988680887[14] = state[14]; + out_5358842043988680887[15] = state[15]; + out_5358842043988680887[16] = state[16]; + out_5358842043988680887[17] = state[17]; } -void F_fun(double *state, double dt, double *out_906698228237069812) { - out_906698228237069812[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_906698228237069812[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_906698228237069812[2] = 0; - out_906698228237069812[3] = 0; - out_906698228237069812[4] = 0; - out_906698228237069812[5] = 0; - out_906698228237069812[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_906698228237069812[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_906698228237069812[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_906698228237069812[9] = 0; - out_906698228237069812[10] = 0; - out_906698228237069812[11] = 0; - out_906698228237069812[12] = 0; - out_906698228237069812[13] = 0; - out_906698228237069812[14] = 0; - out_906698228237069812[15] = 0; - out_906698228237069812[16] = 0; - out_906698228237069812[17] = 0; - out_906698228237069812[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_906698228237069812[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_906698228237069812[20] = 0; - out_906698228237069812[21] = 0; - out_906698228237069812[22] = 0; - out_906698228237069812[23] = 0; - out_906698228237069812[24] = 0; - out_906698228237069812[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_906698228237069812[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_906698228237069812[27] = 0; - out_906698228237069812[28] = 0; - out_906698228237069812[29] = 0; - out_906698228237069812[30] = 0; - out_906698228237069812[31] = 0; - out_906698228237069812[32] = 0; - out_906698228237069812[33] = 0; - out_906698228237069812[34] = 0; - out_906698228237069812[35] = 0; - out_906698228237069812[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_906698228237069812[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_906698228237069812[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_906698228237069812[39] = 0; - out_906698228237069812[40] = 0; - out_906698228237069812[41] = 0; - out_906698228237069812[42] = 0; - out_906698228237069812[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_906698228237069812[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_906698228237069812[45] = 0; - out_906698228237069812[46] = 0; - out_906698228237069812[47] = 0; - out_906698228237069812[48] = 0; - out_906698228237069812[49] = 0; - out_906698228237069812[50] = 0; - out_906698228237069812[51] = 0; - out_906698228237069812[52] = 0; - out_906698228237069812[53] = 0; - out_906698228237069812[54] = 0; - out_906698228237069812[55] = 0; - out_906698228237069812[56] = 0; - out_906698228237069812[57] = 1; - out_906698228237069812[58] = 0; - out_906698228237069812[59] = 0; - out_906698228237069812[60] = 0; - out_906698228237069812[61] = 0; - out_906698228237069812[62] = 0; - out_906698228237069812[63] = 0; - out_906698228237069812[64] = 0; - out_906698228237069812[65] = 0; - out_906698228237069812[66] = dt; - out_906698228237069812[67] = 0; - out_906698228237069812[68] = 0; - out_906698228237069812[69] = 0; - out_906698228237069812[70] = 0; - out_906698228237069812[71] = 0; - out_906698228237069812[72] = 0; - out_906698228237069812[73] = 0; - out_906698228237069812[74] = 0; - out_906698228237069812[75] = 0; - out_906698228237069812[76] = 1; - out_906698228237069812[77] = 0; - out_906698228237069812[78] = 0; - out_906698228237069812[79] = 0; - out_906698228237069812[80] = 0; - out_906698228237069812[81] = 0; - out_906698228237069812[82] = 0; - out_906698228237069812[83] = 0; - out_906698228237069812[84] = 0; - out_906698228237069812[85] = dt; - out_906698228237069812[86] = 0; - out_906698228237069812[87] = 0; - out_906698228237069812[88] = 0; - out_906698228237069812[89] = 0; - out_906698228237069812[90] = 0; - out_906698228237069812[91] = 0; - out_906698228237069812[92] = 0; - out_906698228237069812[93] = 0; - out_906698228237069812[94] = 0; - out_906698228237069812[95] = 1; - out_906698228237069812[96] = 0; - out_906698228237069812[97] = 0; - out_906698228237069812[98] = 0; - out_906698228237069812[99] = 0; - out_906698228237069812[100] = 0; - out_906698228237069812[101] = 0; - out_906698228237069812[102] = 0; - out_906698228237069812[103] = 0; - out_906698228237069812[104] = dt; - out_906698228237069812[105] = 0; - out_906698228237069812[106] = 0; - out_906698228237069812[107] = 0; - out_906698228237069812[108] = 0; - out_906698228237069812[109] = 0; - out_906698228237069812[110] = 0; - out_906698228237069812[111] = 0; - out_906698228237069812[112] = 0; - out_906698228237069812[113] = 0; - out_906698228237069812[114] = 1; - out_906698228237069812[115] = 0; - out_906698228237069812[116] = 0; - out_906698228237069812[117] = 0; - out_906698228237069812[118] = 0; - out_906698228237069812[119] = 0; - out_906698228237069812[120] = 0; - out_906698228237069812[121] = 0; - out_906698228237069812[122] = 0; - out_906698228237069812[123] = 0; - out_906698228237069812[124] = 0; - out_906698228237069812[125] = 0; - out_906698228237069812[126] = 0; - out_906698228237069812[127] = 0; - out_906698228237069812[128] = 0; - out_906698228237069812[129] = 0; - out_906698228237069812[130] = 0; - out_906698228237069812[131] = 0; - out_906698228237069812[132] = 0; - out_906698228237069812[133] = 1; - out_906698228237069812[134] = 0; - out_906698228237069812[135] = 0; - out_906698228237069812[136] = 0; - out_906698228237069812[137] = 0; - out_906698228237069812[138] = 0; - out_906698228237069812[139] = 0; - out_906698228237069812[140] = 0; - out_906698228237069812[141] = 0; - out_906698228237069812[142] = 0; - out_906698228237069812[143] = 0; - out_906698228237069812[144] = 0; - out_906698228237069812[145] = 0; - out_906698228237069812[146] = 0; - out_906698228237069812[147] = 0; - out_906698228237069812[148] = 0; - out_906698228237069812[149] = 0; - out_906698228237069812[150] = 0; - out_906698228237069812[151] = 0; - out_906698228237069812[152] = 1; - out_906698228237069812[153] = 0; - out_906698228237069812[154] = 0; - out_906698228237069812[155] = 0; - out_906698228237069812[156] = 0; - out_906698228237069812[157] = 0; - out_906698228237069812[158] = 0; - out_906698228237069812[159] = 0; - out_906698228237069812[160] = 0; - out_906698228237069812[161] = 0; - out_906698228237069812[162] = 0; - out_906698228237069812[163] = 0; - out_906698228237069812[164] = 0; - out_906698228237069812[165] = 0; - out_906698228237069812[166] = 0; - out_906698228237069812[167] = 0; - out_906698228237069812[168] = 0; - out_906698228237069812[169] = 0; - out_906698228237069812[170] = 0; - out_906698228237069812[171] = 1; - out_906698228237069812[172] = 0; - out_906698228237069812[173] = 0; - out_906698228237069812[174] = 0; - out_906698228237069812[175] = 0; - out_906698228237069812[176] = 0; - out_906698228237069812[177] = 0; - out_906698228237069812[178] = 0; - out_906698228237069812[179] = 0; - out_906698228237069812[180] = 0; - out_906698228237069812[181] = 0; - out_906698228237069812[182] = 0; - out_906698228237069812[183] = 0; - out_906698228237069812[184] = 0; - out_906698228237069812[185] = 0; - out_906698228237069812[186] = 0; - out_906698228237069812[187] = 0; - out_906698228237069812[188] = 0; - out_906698228237069812[189] = 0; - out_906698228237069812[190] = 1; - out_906698228237069812[191] = 0; - out_906698228237069812[192] = 0; - out_906698228237069812[193] = 0; - out_906698228237069812[194] = 0; - out_906698228237069812[195] = 0; - out_906698228237069812[196] = 0; - out_906698228237069812[197] = 0; - out_906698228237069812[198] = 0; - out_906698228237069812[199] = 0; - out_906698228237069812[200] = 0; - out_906698228237069812[201] = 0; - out_906698228237069812[202] = 0; - out_906698228237069812[203] = 0; - out_906698228237069812[204] = 0; - out_906698228237069812[205] = 0; - out_906698228237069812[206] = 0; - out_906698228237069812[207] = 0; - out_906698228237069812[208] = 0; - out_906698228237069812[209] = 1; - out_906698228237069812[210] = 0; - out_906698228237069812[211] = 0; - out_906698228237069812[212] = 0; - out_906698228237069812[213] = 0; - out_906698228237069812[214] = 0; - out_906698228237069812[215] = 0; - out_906698228237069812[216] = 0; - out_906698228237069812[217] = 0; - out_906698228237069812[218] = 0; - out_906698228237069812[219] = 0; - out_906698228237069812[220] = 0; - out_906698228237069812[221] = 0; - out_906698228237069812[222] = 0; - out_906698228237069812[223] = 0; - out_906698228237069812[224] = 0; - out_906698228237069812[225] = 0; - out_906698228237069812[226] = 0; - out_906698228237069812[227] = 0; - out_906698228237069812[228] = 1; - out_906698228237069812[229] = 0; - out_906698228237069812[230] = 0; - out_906698228237069812[231] = 0; - out_906698228237069812[232] = 0; - out_906698228237069812[233] = 0; - out_906698228237069812[234] = 0; - out_906698228237069812[235] = 0; - out_906698228237069812[236] = 0; - out_906698228237069812[237] = 0; - out_906698228237069812[238] = 0; - out_906698228237069812[239] = 0; - out_906698228237069812[240] = 0; - out_906698228237069812[241] = 0; - out_906698228237069812[242] = 0; - out_906698228237069812[243] = 0; - out_906698228237069812[244] = 0; - out_906698228237069812[245] = 0; - out_906698228237069812[246] = 0; - out_906698228237069812[247] = 1; - out_906698228237069812[248] = 0; - out_906698228237069812[249] = 0; - out_906698228237069812[250] = 0; - out_906698228237069812[251] = 0; - out_906698228237069812[252] = 0; - out_906698228237069812[253] = 0; - out_906698228237069812[254] = 0; - out_906698228237069812[255] = 0; - out_906698228237069812[256] = 0; - out_906698228237069812[257] = 0; - out_906698228237069812[258] = 0; - out_906698228237069812[259] = 0; - out_906698228237069812[260] = 0; - out_906698228237069812[261] = 0; - out_906698228237069812[262] = 0; - out_906698228237069812[263] = 0; - out_906698228237069812[264] = 0; - out_906698228237069812[265] = 0; - out_906698228237069812[266] = 1; - out_906698228237069812[267] = 0; - out_906698228237069812[268] = 0; - out_906698228237069812[269] = 0; - out_906698228237069812[270] = 0; - out_906698228237069812[271] = 0; - out_906698228237069812[272] = 0; - out_906698228237069812[273] = 0; - out_906698228237069812[274] = 0; - out_906698228237069812[275] = 0; - out_906698228237069812[276] = 0; - out_906698228237069812[277] = 0; - out_906698228237069812[278] = 0; - out_906698228237069812[279] = 0; - out_906698228237069812[280] = 0; - out_906698228237069812[281] = 0; - out_906698228237069812[282] = 0; - out_906698228237069812[283] = 0; - out_906698228237069812[284] = 0; - out_906698228237069812[285] = 1; - out_906698228237069812[286] = 0; - out_906698228237069812[287] = 0; - out_906698228237069812[288] = 0; - out_906698228237069812[289] = 0; - out_906698228237069812[290] = 0; - out_906698228237069812[291] = 0; - out_906698228237069812[292] = 0; - out_906698228237069812[293] = 0; - out_906698228237069812[294] = 0; - out_906698228237069812[295] = 0; - out_906698228237069812[296] = 0; - out_906698228237069812[297] = 0; - out_906698228237069812[298] = 0; - out_906698228237069812[299] = 0; - out_906698228237069812[300] = 0; - out_906698228237069812[301] = 0; - out_906698228237069812[302] = 0; - out_906698228237069812[303] = 0; - out_906698228237069812[304] = 1; - out_906698228237069812[305] = 0; - out_906698228237069812[306] = 0; - out_906698228237069812[307] = 0; - out_906698228237069812[308] = 0; - out_906698228237069812[309] = 0; - out_906698228237069812[310] = 0; - out_906698228237069812[311] = 0; - out_906698228237069812[312] = 0; - out_906698228237069812[313] = 0; - out_906698228237069812[314] = 0; - out_906698228237069812[315] = 0; - out_906698228237069812[316] = 0; - out_906698228237069812[317] = 0; - out_906698228237069812[318] = 0; - out_906698228237069812[319] = 0; - out_906698228237069812[320] = 0; - out_906698228237069812[321] = 0; - out_906698228237069812[322] = 0; - out_906698228237069812[323] = 1; +void F_fun(double *state, double dt, double *out_6970610951587922797) { + out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[2] = 0; + out_6970610951587922797[3] = 0; + out_6970610951587922797[4] = 0; + out_6970610951587922797[5] = 0; + out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[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_6970610951587922797[9] = 0; + out_6970610951587922797[10] = 0; + out_6970610951587922797[11] = 0; + out_6970610951587922797[12] = 0; + out_6970610951587922797[13] = 0; + out_6970610951587922797[14] = 0; + out_6970610951587922797[15] = 0; + out_6970610951587922797[16] = 0; + out_6970610951587922797[17] = 0; + out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[20] = 0; + out_6970610951587922797[21] = 0; + out_6970610951587922797[22] = 0; + out_6970610951587922797[23] = 0; + out_6970610951587922797[24] = 0; + out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[27] = 0; + out_6970610951587922797[28] = 0; + out_6970610951587922797[29] = 0; + out_6970610951587922797[30] = 0; + out_6970610951587922797[31] = 0; + out_6970610951587922797[32] = 0; + out_6970610951587922797[33] = 0; + out_6970610951587922797[34] = 0; + out_6970610951587922797[35] = 0; + out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[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_6970610951587922797[39] = 0; + out_6970610951587922797[40] = 0; + out_6970610951587922797[41] = 0; + out_6970610951587922797[42] = 0; + out_6970610951587922797[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_6970610951587922797[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_6970610951587922797[45] = 0; + out_6970610951587922797[46] = 0; + out_6970610951587922797[47] = 0; + out_6970610951587922797[48] = 0; + out_6970610951587922797[49] = 0; + out_6970610951587922797[50] = 0; + out_6970610951587922797[51] = 0; + out_6970610951587922797[52] = 0; + out_6970610951587922797[53] = 0; + out_6970610951587922797[54] = 0; + out_6970610951587922797[55] = 0; + out_6970610951587922797[56] = 0; + out_6970610951587922797[57] = 1; + out_6970610951587922797[58] = 0; + out_6970610951587922797[59] = 0; + out_6970610951587922797[60] = 0; + out_6970610951587922797[61] = 0; + out_6970610951587922797[62] = 0; + out_6970610951587922797[63] = 0; + out_6970610951587922797[64] = 0; + out_6970610951587922797[65] = 0; + out_6970610951587922797[66] = dt; + out_6970610951587922797[67] = 0; + out_6970610951587922797[68] = 0; + out_6970610951587922797[69] = 0; + out_6970610951587922797[70] = 0; + out_6970610951587922797[71] = 0; + out_6970610951587922797[72] = 0; + out_6970610951587922797[73] = 0; + out_6970610951587922797[74] = 0; + out_6970610951587922797[75] = 0; + out_6970610951587922797[76] = 1; + out_6970610951587922797[77] = 0; + out_6970610951587922797[78] = 0; + out_6970610951587922797[79] = 0; + out_6970610951587922797[80] = 0; + out_6970610951587922797[81] = 0; + out_6970610951587922797[82] = 0; + out_6970610951587922797[83] = 0; + out_6970610951587922797[84] = 0; + out_6970610951587922797[85] = dt; + out_6970610951587922797[86] = 0; + out_6970610951587922797[87] = 0; + out_6970610951587922797[88] = 0; + out_6970610951587922797[89] = 0; + out_6970610951587922797[90] = 0; + out_6970610951587922797[91] = 0; + out_6970610951587922797[92] = 0; + out_6970610951587922797[93] = 0; + out_6970610951587922797[94] = 0; + out_6970610951587922797[95] = 1; + out_6970610951587922797[96] = 0; + out_6970610951587922797[97] = 0; + out_6970610951587922797[98] = 0; + out_6970610951587922797[99] = 0; + out_6970610951587922797[100] = 0; + out_6970610951587922797[101] = 0; + out_6970610951587922797[102] = 0; + out_6970610951587922797[103] = 0; + out_6970610951587922797[104] = dt; + out_6970610951587922797[105] = 0; + out_6970610951587922797[106] = 0; + out_6970610951587922797[107] = 0; + out_6970610951587922797[108] = 0; + out_6970610951587922797[109] = 0; + out_6970610951587922797[110] = 0; + out_6970610951587922797[111] = 0; + out_6970610951587922797[112] = 0; + out_6970610951587922797[113] = 0; + out_6970610951587922797[114] = 1; + out_6970610951587922797[115] = 0; + out_6970610951587922797[116] = 0; + out_6970610951587922797[117] = 0; + out_6970610951587922797[118] = 0; + out_6970610951587922797[119] = 0; + out_6970610951587922797[120] = 0; + out_6970610951587922797[121] = 0; + out_6970610951587922797[122] = 0; + out_6970610951587922797[123] = 0; + out_6970610951587922797[124] = 0; + out_6970610951587922797[125] = 0; + out_6970610951587922797[126] = 0; + out_6970610951587922797[127] = 0; + out_6970610951587922797[128] = 0; + out_6970610951587922797[129] = 0; + out_6970610951587922797[130] = 0; + out_6970610951587922797[131] = 0; + out_6970610951587922797[132] = 0; + out_6970610951587922797[133] = 1; + out_6970610951587922797[134] = 0; + out_6970610951587922797[135] = 0; + out_6970610951587922797[136] = 0; + out_6970610951587922797[137] = 0; + out_6970610951587922797[138] = 0; + out_6970610951587922797[139] = 0; + out_6970610951587922797[140] = 0; + out_6970610951587922797[141] = 0; + out_6970610951587922797[142] = 0; + out_6970610951587922797[143] = 0; + out_6970610951587922797[144] = 0; + out_6970610951587922797[145] = 0; + out_6970610951587922797[146] = 0; + out_6970610951587922797[147] = 0; + out_6970610951587922797[148] = 0; + out_6970610951587922797[149] = 0; + out_6970610951587922797[150] = 0; + out_6970610951587922797[151] = 0; + out_6970610951587922797[152] = 1; + out_6970610951587922797[153] = 0; + out_6970610951587922797[154] = 0; + out_6970610951587922797[155] = 0; + out_6970610951587922797[156] = 0; + out_6970610951587922797[157] = 0; + out_6970610951587922797[158] = 0; + out_6970610951587922797[159] = 0; + out_6970610951587922797[160] = 0; + out_6970610951587922797[161] = 0; + out_6970610951587922797[162] = 0; + out_6970610951587922797[163] = 0; + out_6970610951587922797[164] = 0; + out_6970610951587922797[165] = 0; + out_6970610951587922797[166] = 0; + out_6970610951587922797[167] = 0; + out_6970610951587922797[168] = 0; + out_6970610951587922797[169] = 0; + out_6970610951587922797[170] = 0; + out_6970610951587922797[171] = 1; + out_6970610951587922797[172] = 0; + out_6970610951587922797[173] = 0; + out_6970610951587922797[174] = 0; + out_6970610951587922797[175] = 0; + out_6970610951587922797[176] = 0; + out_6970610951587922797[177] = 0; + out_6970610951587922797[178] = 0; + out_6970610951587922797[179] = 0; + out_6970610951587922797[180] = 0; + out_6970610951587922797[181] = 0; + out_6970610951587922797[182] = 0; + out_6970610951587922797[183] = 0; + out_6970610951587922797[184] = 0; + out_6970610951587922797[185] = 0; + out_6970610951587922797[186] = 0; + out_6970610951587922797[187] = 0; + out_6970610951587922797[188] = 0; + out_6970610951587922797[189] = 0; + out_6970610951587922797[190] = 1; + out_6970610951587922797[191] = 0; + out_6970610951587922797[192] = 0; + out_6970610951587922797[193] = 0; + out_6970610951587922797[194] = 0; + out_6970610951587922797[195] = 0; + out_6970610951587922797[196] = 0; + out_6970610951587922797[197] = 0; + out_6970610951587922797[198] = 0; + out_6970610951587922797[199] = 0; + out_6970610951587922797[200] = 0; + out_6970610951587922797[201] = 0; + out_6970610951587922797[202] = 0; + out_6970610951587922797[203] = 0; + out_6970610951587922797[204] = 0; + out_6970610951587922797[205] = 0; + out_6970610951587922797[206] = 0; + out_6970610951587922797[207] = 0; + out_6970610951587922797[208] = 0; + out_6970610951587922797[209] = 1; + out_6970610951587922797[210] = 0; + out_6970610951587922797[211] = 0; + out_6970610951587922797[212] = 0; + out_6970610951587922797[213] = 0; + out_6970610951587922797[214] = 0; + out_6970610951587922797[215] = 0; + out_6970610951587922797[216] = 0; + out_6970610951587922797[217] = 0; + out_6970610951587922797[218] = 0; + out_6970610951587922797[219] = 0; + out_6970610951587922797[220] = 0; + out_6970610951587922797[221] = 0; + out_6970610951587922797[222] = 0; + out_6970610951587922797[223] = 0; + out_6970610951587922797[224] = 0; + out_6970610951587922797[225] = 0; + out_6970610951587922797[226] = 0; + out_6970610951587922797[227] = 0; + out_6970610951587922797[228] = 1; + out_6970610951587922797[229] = 0; + out_6970610951587922797[230] = 0; + out_6970610951587922797[231] = 0; + out_6970610951587922797[232] = 0; + out_6970610951587922797[233] = 0; + out_6970610951587922797[234] = 0; + out_6970610951587922797[235] = 0; + out_6970610951587922797[236] = 0; + out_6970610951587922797[237] = 0; + out_6970610951587922797[238] = 0; + out_6970610951587922797[239] = 0; + out_6970610951587922797[240] = 0; + out_6970610951587922797[241] = 0; + out_6970610951587922797[242] = 0; + out_6970610951587922797[243] = 0; + out_6970610951587922797[244] = 0; + out_6970610951587922797[245] = 0; + out_6970610951587922797[246] = 0; + out_6970610951587922797[247] = 1; + out_6970610951587922797[248] = 0; + out_6970610951587922797[249] = 0; + out_6970610951587922797[250] = 0; + out_6970610951587922797[251] = 0; + out_6970610951587922797[252] = 0; + out_6970610951587922797[253] = 0; + out_6970610951587922797[254] = 0; + out_6970610951587922797[255] = 0; + out_6970610951587922797[256] = 0; + out_6970610951587922797[257] = 0; + out_6970610951587922797[258] = 0; + out_6970610951587922797[259] = 0; + out_6970610951587922797[260] = 0; + out_6970610951587922797[261] = 0; + out_6970610951587922797[262] = 0; + out_6970610951587922797[263] = 0; + out_6970610951587922797[264] = 0; + out_6970610951587922797[265] = 0; + out_6970610951587922797[266] = 1; + out_6970610951587922797[267] = 0; + out_6970610951587922797[268] = 0; + out_6970610951587922797[269] = 0; + out_6970610951587922797[270] = 0; + out_6970610951587922797[271] = 0; + out_6970610951587922797[272] = 0; + out_6970610951587922797[273] = 0; + out_6970610951587922797[274] = 0; + out_6970610951587922797[275] = 0; + out_6970610951587922797[276] = 0; + out_6970610951587922797[277] = 0; + out_6970610951587922797[278] = 0; + out_6970610951587922797[279] = 0; + out_6970610951587922797[280] = 0; + out_6970610951587922797[281] = 0; + out_6970610951587922797[282] = 0; + out_6970610951587922797[283] = 0; + out_6970610951587922797[284] = 0; + out_6970610951587922797[285] = 1; + out_6970610951587922797[286] = 0; + out_6970610951587922797[287] = 0; + out_6970610951587922797[288] = 0; + out_6970610951587922797[289] = 0; + out_6970610951587922797[290] = 0; + out_6970610951587922797[291] = 0; + out_6970610951587922797[292] = 0; + out_6970610951587922797[293] = 0; + out_6970610951587922797[294] = 0; + out_6970610951587922797[295] = 0; + out_6970610951587922797[296] = 0; + out_6970610951587922797[297] = 0; + out_6970610951587922797[298] = 0; + out_6970610951587922797[299] = 0; + out_6970610951587922797[300] = 0; + out_6970610951587922797[301] = 0; + out_6970610951587922797[302] = 0; + out_6970610951587922797[303] = 0; + out_6970610951587922797[304] = 1; + out_6970610951587922797[305] = 0; + out_6970610951587922797[306] = 0; + out_6970610951587922797[307] = 0; + out_6970610951587922797[308] = 0; + out_6970610951587922797[309] = 0; + out_6970610951587922797[310] = 0; + out_6970610951587922797[311] = 0; + out_6970610951587922797[312] = 0; + out_6970610951587922797[313] = 0; + out_6970610951587922797[314] = 0; + out_6970610951587922797[315] = 0; + out_6970610951587922797[316] = 0; + out_6970610951587922797[317] = 0; + out_6970610951587922797[318] = 0; + out_6970610951587922797[319] = 0; + out_6970610951587922797[320] = 0; + out_6970610951587922797[321] = 0; + out_6970610951587922797[322] = 0; + out_6970610951587922797[323] = 1; } -void h_4(double *state, double *unused, double *out_6577940567349030778) { - out_6577940567349030778[0] = state[6] + state[9]; - out_6577940567349030778[1] = state[7] + state[10]; - out_6577940567349030778[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_3613363558024225199) { + out_3613363558024225199[0] = state[6] + state[9]; + out_3613363558024225199[1] = state[7] + state[10]; + out_3613363558024225199[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_913158878120160673) { - out_913158878120160673[0] = 0; - out_913158878120160673[1] = 0; - out_913158878120160673[2] = 0; - out_913158878120160673[3] = 0; - out_913158878120160673[4] = 0; - out_913158878120160673[5] = 0; - out_913158878120160673[6] = 1; - out_913158878120160673[7] = 0; - out_913158878120160673[8] = 0; - out_913158878120160673[9] = 1; - out_913158878120160673[10] = 0; - out_913158878120160673[11] = 0; - out_913158878120160673[12] = 0; - out_913158878120160673[13] = 0; - out_913158878120160673[14] = 0; - out_913158878120160673[15] = 0; - out_913158878120160673[16] = 0; - out_913158878120160673[17] = 0; - out_913158878120160673[18] = 0; - out_913158878120160673[19] = 0; - out_913158878120160673[20] = 0; - out_913158878120160673[21] = 0; - out_913158878120160673[22] = 0; - out_913158878120160673[23] = 0; - out_913158878120160673[24] = 0; - out_913158878120160673[25] = 1; - out_913158878120160673[26] = 0; - out_913158878120160673[27] = 0; - out_913158878120160673[28] = 1; - out_913158878120160673[29] = 0; - out_913158878120160673[30] = 0; - out_913158878120160673[31] = 0; - out_913158878120160673[32] = 0; - out_913158878120160673[33] = 0; - out_913158878120160673[34] = 0; - out_913158878120160673[35] = 0; - out_913158878120160673[36] = 0; - out_913158878120160673[37] = 0; - out_913158878120160673[38] = 0; - out_913158878120160673[39] = 0; - out_913158878120160673[40] = 0; - out_913158878120160673[41] = 0; - out_913158878120160673[42] = 0; - out_913158878120160673[43] = 0; - out_913158878120160673[44] = 1; - out_913158878120160673[45] = 0; - out_913158878120160673[46] = 0; - out_913158878120160673[47] = 1; - out_913158878120160673[48] = 0; - out_913158878120160673[49] = 0; - out_913158878120160673[50] = 0; - out_913158878120160673[51] = 0; - out_913158878120160673[52] = 0; - out_913158878120160673[53] = 0; +void H_4(double *state, double *unused, double *out_8770780261124510538) { + out_8770780261124510538[0] = 0; + out_8770780261124510538[1] = 0; + out_8770780261124510538[2] = 0; + out_8770780261124510538[3] = 0; + out_8770780261124510538[4] = 0; + out_8770780261124510538[5] = 0; + out_8770780261124510538[6] = 1; + out_8770780261124510538[7] = 0; + out_8770780261124510538[8] = 0; + out_8770780261124510538[9] = 1; + out_8770780261124510538[10] = 0; + out_8770780261124510538[11] = 0; + out_8770780261124510538[12] = 0; + out_8770780261124510538[13] = 0; + out_8770780261124510538[14] = 0; + out_8770780261124510538[15] = 0; + out_8770780261124510538[16] = 0; + out_8770780261124510538[17] = 0; + out_8770780261124510538[18] = 0; + out_8770780261124510538[19] = 0; + out_8770780261124510538[20] = 0; + out_8770780261124510538[21] = 0; + out_8770780261124510538[22] = 0; + out_8770780261124510538[23] = 0; + out_8770780261124510538[24] = 0; + out_8770780261124510538[25] = 1; + out_8770780261124510538[26] = 0; + out_8770780261124510538[27] = 0; + out_8770780261124510538[28] = 1; + out_8770780261124510538[29] = 0; + out_8770780261124510538[30] = 0; + out_8770780261124510538[31] = 0; + out_8770780261124510538[32] = 0; + out_8770780261124510538[33] = 0; + out_8770780261124510538[34] = 0; + out_8770780261124510538[35] = 0; + out_8770780261124510538[36] = 0; + out_8770780261124510538[37] = 0; + out_8770780261124510538[38] = 0; + out_8770780261124510538[39] = 0; + out_8770780261124510538[40] = 0; + out_8770780261124510538[41] = 0; + out_8770780261124510538[42] = 0; + out_8770780261124510538[43] = 0; + out_8770780261124510538[44] = 1; + out_8770780261124510538[45] = 0; + out_8770780261124510538[46] = 0; + out_8770780261124510538[47] = 1; + out_8770780261124510538[48] = 0; + out_8770780261124510538[49] = 0; + out_8770780261124510538[50] = 0; + out_8770780261124510538[51] = 0; + out_8770780261124510538[52] = 0; + out_8770780261124510538[53] = 0; } -void h_10(double *state, double *unused, double *out_3373051515203641771) { - out_3373051515203641771[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_3373051515203641771[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_3373051515203641771[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_7325246586980236980) { + out_7325246586980236980[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_7325246586980236980[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_7325246586980236980[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_7288760212554836769) { - out_7288760212554836769[0] = 0; - out_7288760212554836769[1] = 9.8100000000000005*cos(state[1]); - out_7288760212554836769[2] = 0; - out_7288760212554836769[3] = 0; - out_7288760212554836769[4] = -state[8]; - out_7288760212554836769[5] = state[7]; - out_7288760212554836769[6] = 0; - out_7288760212554836769[7] = state[5]; - out_7288760212554836769[8] = -state[4]; - out_7288760212554836769[9] = 0; - out_7288760212554836769[10] = 0; - out_7288760212554836769[11] = 0; - out_7288760212554836769[12] = 1; - out_7288760212554836769[13] = 0; - out_7288760212554836769[14] = 0; - out_7288760212554836769[15] = 1; - out_7288760212554836769[16] = 0; - out_7288760212554836769[17] = 0; - out_7288760212554836769[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_7288760212554836769[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_7288760212554836769[20] = 0; - out_7288760212554836769[21] = state[8]; - out_7288760212554836769[22] = 0; - out_7288760212554836769[23] = -state[6]; - out_7288760212554836769[24] = -state[5]; - out_7288760212554836769[25] = 0; - out_7288760212554836769[26] = state[3]; - out_7288760212554836769[27] = 0; - out_7288760212554836769[28] = 0; - out_7288760212554836769[29] = 0; - out_7288760212554836769[30] = 0; - out_7288760212554836769[31] = 1; - out_7288760212554836769[32] = 0; - out_7288760212554836769[33] = 0; - out_7288760212554836769[34] = 1; - out_7288760212554836769[35] = 0; - out_7288760212554836769[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_7288760212554836769[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_7288760212554836769[38] = 0; - out_7288760212554836769[39] = -state[7]; - out_7288760212554836769[40] = state[6]; - out_7288760212554836769[41] = 0; - out_7288760212554836769[42] = state[4]; - out_7288760212554836769[43] = -state[3]; - out_7288760212554836769[44] = 0; - out_7288760212554836769[45] = 0; - out_7288760212554836769[46] = 0; - out_7288760212554836769[47] = 0; - out_7288760212554836769[48] = 0; - out_7288760212554836769[49] = 0; - out_7288760212554836769[50] = 1; - out_7288760212554836769[51] = 0; - out_7288760212554836769[52] = 0; - out_7288760212554836769[53] = 1; +void H_10(double *state, double *unused, double *out_8143750738918534214) { + out_8143750738918534214[0] = 0; + out_8143750738918534214[1] = 9.8100000000000005*cos(state[1]); + out_8143750738918534214[2] = 0; + out_8143750738918534214[3] = 0; + out_8143750738918534214[4] = -state[8]; + out_8143750738918534214[5] = state[7]; + out_8143750738918534214[6] = 0; + out_8143750738918534214[7] = state[5]; + out_8143750738918534214[8] = -state[4]; + out_8143750738918534214[9] = 0; + out_8143750738918534214[10] = 0; + out_8143750738918534214[11] = 0; + out_8143750738918534214[12] = 1; + out_8143750738918534214[13] = 0; + out_8143750738918534214[14] = 0; + out_8143750738918534214[15] = 1; + out_8143750738918534214[16] = 0; + out_8143750738918534214[17] = 0; + out_8143750738918534214[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_8143750738918534214[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_8143750738918534214[20] = 0; + out_8143750738918534214[21] = state[8]; + out_8143750738918534214[22] = 0; + out_8143750738918534214[23] = -state[6]; + out_8143750738918534214[24] = -state[5]; + out_8143750738918534214[25] = 0; + out_8143750738918534214[26] = state[3]; + out_8143750738918534214[27] = 0; + out_8143750738918534214[28] = 0; + out_8143750738918534214[29] = 0; + out_8143750738918534214[30] = 0; + out_8143750738918534214[31] = 1; + out_8143750738918534214[32] = 0; + out_8143750738918534214[33] = 0; + out_8143750738918534214[34] = 1; + out_8143750738918534214[35] = 0; + out_8143750738918534214[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_8143750738918534214[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_8143750738918534214[38] = 0; + out_8143750738918534214[39] = -state[7]; + out_8143750738918534214[40] = state[6]; + out_8143750738918534214[41] = 0; + out_8143750738918534214[42] = state[4]; + out_8143750738918534214[43] = -state[3]; + out_8143750738918534214[44] = 0; + out_8143750738918534214[45] = 0; + out_8143750738918534214[46] = 0; + out_8143750738918534214[47] = 0; + out_8143750738918534214[48] = 0; + out_8143750738918534214[49] = 0; + out_8143750738918534214[50] = 1; + out_8143750738918534214[51] = 0; + out_8143750738918534214[52] = 0; + out_8143750738918534214[53] = 1; } -void h_13(double *state, double *unused, double *out_413669578140119338) { - out_413669578140119338[0] = state[3]; - out_413669578140119338[1] = state[4]; - out_413669578140119338[2] = state[5]; +void h_13(double *state, double *unused, double *out_4440237870440636920) { + out_4440237870440636920[0] = state[3]; + out_4440237870440636920[1] = state[4]; + out_4440237870440636920[2] = state[5]; } -void H_13(double *state, double *unused, double *out_8523790086436861602) { - out_8523790086436861602[0] = 0; - out_8523790086436861602[1] = 0; - out_8523790086436861602[2] = 0; - out_8523790086436861602[3] = 1; - out_8523790086436861602[4] = 0; - out_8523790086436861602[5] = 0; - out_8523790086436861602[6] = 0; - out_8523790086436861602[7] = 0; - out_8523790086436861602[8] = 0; - out_8523790086436861602[9] = 0; - out_8523790086436861602[10] = 0; - out_8523790086436861602[11] = 0; - out_8523790086436861602[12] = 0; - out_8523790086436861602[13] = 0; - out_8523790086436861602[14] = 0; - out_8523790086436861602[15] = 0; - out_8523790086436861602[16] = 0; - out_8523790086436861602[17] = 0; - out_8523790086436861602[18] = 0; - out_8523790086436861602[19] = 0; - out_8523790086436861602[20] = 0; - out_8523790086436861602[21] = 0; - out_8523790086436861602[22] = 1; - out_8523790086436861602[23] = 0; - out_8523790086436861602[24] = 0; - out_8523790086436861602[25] = 0; - out_8523790086436861602[26] = 0; - out_8523790086436861602[27] = 0; - out_8523790086436861602[28] = 0; - out_8523790086436861602[29] = 0; - out_8523790086436861602[30] = 0; - out_8523790086436861602[31] = 0; - out_8523790086436861602[32] = 0; - out_8523790086436861602[33] = 0; - out_8523790086436861602[34] = 0; - out_8523790086436861602[35] = 0; - out_8523790086436861602[36] = 0; - out_8523790086436861602[37] = 0; - out_8523790086436861602[38] = 0; - out_8523790086436861602[39] = 0; - out_8523790086436861602[40] = 0; - out_8523790086436861602[41] = 1; - out_8523790086436861602[42] = 0; - out_8523790086436861602[43] = 0; - out_8523790086436861602[44] = 0; - out_8523790086436861602[45] = 0; - out_8523790086436861602[46] = 0; - out_8523790086436861602[47] = 0; - out_8523790086436861602[48] = 0; - out_8523790086436861602[49] = 0; - out_8523790086436861602[50] = 0; - out_8523790086436861602[51] = 0; - out_8523790086436861602[52] = 0; - out_8523790086436861602[53] = 0; +void H_13(double *state, double *unused, double *out_5558506435792177737) { + out_5558506435792177737[0] = 0; + out_5558506435792177737[1] = 0; + out_5558506435792177737[2] = 0; + out_5558506435792177737[3] = 1; + out_5558506435792177737[4] = 0; + out_5558506435792177737[5] = 0; + out_5558506435792177737[6] = 0; + out_5558506435792177737[7] = 0; + out_5558506435792177737[8] = 0; + out_5558506435792177737[9] = 0; + out_5558506435792177737[10] = 0; + out_5558506435792177737[11] = 0; + out_5558506435792177737[12] = 0; + out_5558506435792177737[13] = 0; + out_5558506435792177737[14] = 0; + out_5558506435792177737[15] = 0; + out_5558506435792177737[16] = 0; + out_5558506435792177737[17] = 0; + out_5558506435792177737[18] = 0; + out_5558506435792177737[19] = 0; + out_5558506435792177737[20] = 0; + out_5558506435792177737[21] = 0; + out_5558506435792177737[22] = 1; + out_5558506435792177737[23] = 0; + out_5558506435792177737[24] = 0; + out_5558506435792177737[25] = 0; + out_5558506435792177737[26] = 0; + out_5558506435792177737[27] = 0; + out_5558506435792177737[28] = 0; + out_5558506435792177737[29] = 0; + out_5558506435792177737[30] = 0; + out_5558506435792177737[31] = 0; + out_5558506435792177737[32] = 0; + out_5558506435792177737[33] = 0; + out_5558506435792177737[34] = 0; + out_5558506435792177737[35] = 0; + out_5558506435792177737[36] = 0; + out_5558506435792177737[37] = 0; + out_5558506435792177737[38] = 0; + out_5558506435792177737[39] = 0; + out_5558506435792177737[40] = 0; + out_5558506435792177737[41] = 1; + out_5558506435792177737[42] = 0; + out_5558506435792177737[43] = 0; + out_5558506435792177737[44] = 0; + out_5558506435792177737[45] = 0; + out_5558506435792177737[46] = 0; + out_5558506435792177737[47] = 0; + out_5558506435792177737[48] = 0; + out_5558506435792177737[49] = 0; + out_5558506435792177737[50] = 0; + out_5558506435792177737[51] = 0; + out_5558506435792177737[52] = 0; + out_5558506435792177737[53] = 0; } -void h_14(double *state, double *unused, double *out_8770101800505378255) { - out_8770101800505378255[0] = state[6]; - out_8770101800505378255[1] = state[7]; - out_8770101800505378255[2] = state[8]; +void h_14(double *state, double *unused, double *out_854572314087465107) { + out_854572314087465107[0] = state[6]; + out_854572314087465107[1] = state[7]; + out_854572314087465107[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4876399734459645202) { - out_4876399734459645202[0] = 0; - out_4876399734459645202[1] = 0; - out_4876399734459645202[2] = 0; - out_4876399734459645202[3] = 0; - out_4876399734459645202[4] = 0; - out_4876399734459645202[5] = 0; - out_4876399734459645202[6] = 1; - out_4876399734459645202[7] = 0; - out_4876399734459645202[8] = 0; - out_4876399734459645202[9] = 0; - out_4876399734459645202[10] = 0; - out_4876399734459645202[11] = 0; - out_4876399734459645202[12] = 0; - out_4876399734459645202[13] = 0; - out_4876399734459645202[14] = 0; - out_4876399734459645202[15] = 0; - out_4876399734459645202[16] = 0; - out_4876399734459645202[17] = 0; - out_4876399734459645202[18] = 0; - out_4876399734459645202[19] = 0; - out_4876399734459645202[20] = 0; - out_4876399734459645202[21] = 0; - out_4876399734459645202[22] = 0; - out_4876399734459645202[23] = 0; - out_4876399734459645202[24] = 0; - out_4876399734459645202[25] = 1; - out_4876399734459645202[26] = 0; - out_4876399734459645202[27] = 0; - out_4876399734459645202[28] = 0; - out_4876399734459645202[29] = 0; - out_4876399734459645202[30] = 0; - out_4876399734459645202[31] = 0; - out_4876399734459645202[32] = 0; - out_4876399734459645202[33] = 0; - out_4876399734459645202[34] = 0; - out_4876399734459645202[35] = 0; - out_4876399734459645202[36] = 0; - out_4876399734459645202[37] = 0; - out_4876399734459645202[38] = 0; - out_4876399734459645202[39] = 0; - out_4876399734459645202[40] = 0; - out_4876399734459645202[41] = 0; - out_4876399734459645202[42] = 0; - out_4876399734459645202[43] = 0; - out_4876399734459645202[44] = 1; - out_4876399734459645202[45] = 0; - out_4876399734459645202[46] = 0; - out_4876399734459645202[47] = 0; - out_4876399734459645202[48] = 0; - out_4876399734459645202[49] = 0; - out_4876399734459645202[50] = 0; - out_4876399734459645202[51] = 0; - out_4876399734459645202[52] = 0; - out_4876399734459645202[53] = 0; +void H_14(double *state, double *unused, double *out_4807539404785026009) { + out_4807539404785026009[0] = 0; + out_4807539404785026009[1] = 0; + out_4807539404785026009[2] = 0; + out_4807539404785026009[3] = 0; + out_4807539404785026009[4] = 0; + out_4807539404785026009[5] = 0; + out_4807539404785026009[6] = 1; + out_4807539404785026009[7] = 0; + out_4807539404785026009[8] = 0; + out_4807539404785026009[9] = 0; + out_4807539404785026009[10] = 0; + out_4807539404785026009[11] = 0; + out_4807539404785026009[12] = 0; + out_4807539404785026009[13] = 0; + out_4807539404785026009[14] = 0; + out_4807539404785026009[15] = 0; + out_4807539404785026009[16] = 0; + out_4807539404785026009[17] = 0; + out_4807539404785026009[18] = 0; + out_4807539404785026009[19] = 0; + out_4807539404785026009[20] = 0; + out_4807539404785026009[21] = 0; + out_4807539404785026009[22] = 0; + out_4807539404785026009[23] = 0; + out_4807539404785026009[24] = 0; + out_4807539404785026009[25] = 1; + out_4807539404785026009[26] = 0; + out_4807539404785026009[27] = 0; + out_4807539404785026009[28] = 0; + out_4807539404785026009[29] = 0; + out_4807539404785026009[30] = 0; + out_4807539404785026009[31] = 0; + out_4807539404785026009[32] = 0; + out_4807539404785026009[33] = 0; + out_4807539404785026009[34] = 0; + out_4807539404785026009[35] = 0; + out_4807539404785026009[36] = 0; + out_4807539404785026009[37] = 0; + out_4807539404785026009[38] = 0; + out_4807539404785026009[39] = 0; + out_4807539404785026009[40] = 0; + out_4807539404785026009[41] = 0; + out_4807539404785026009[42] = 0; + out_4807539404785026009[43] = 0; + out_4807539404785026009[44] = 1; + out_4807539404785026009[45] = 0; + out_4807539404785026009[46] = 0; + out_4807539404785026009[47] = 0; + out_4807539404785026009[48] = 0; + out_4807539404785026009[49] = 0; + out_4807539404785026009[50] = 0; + out_4807539404785026009[51] = 0; + out_4807539404785026009[52] = 0; + out_4807539404785026009[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_5188089899736494345) { - err_fun(nom_x, delta_x, out_5188089899736494345); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6247163002975687744) { + err_fun(nom_x, delta_x, out_6247163002975687744); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_516319331577825853) { - inv_err_fun(nom_x, true_x, out_516319331577825853); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2784495282432373016) { + inv_err_fun(nom_x, true_x, out_2784495282432373016); } -void pose_H_mod_fun(double *state, double *out_4951567132479851062) { - H_mod_fun(state, out_4951567132479851062); +void pose_H_mod_fun(double *state, double *out_662979392671356586) { + H_mod_fun(state, out_662979392671356586); } -void pose_f_fun(double *state, double dt, double *out_898454825503606476) { - f_fun(state, dt, out_898454825503606476); +void pose_f_fun(double *state, double dt, double *out_5358842043988680887) { + f_fun(state, dt, out_5358842043988680887); } -void pose_F_fun(double *state, double dt, double *out_906698228237069812) { - F_fun(state, dt, out_906698228237069812); +void pose_F_fun(double *state, double dt, double *out_6970610951587922797) { + F_fun(state, dt, out_6970610951587922797); } -void pose_h_4(double *state, double *unused, double *out_6577940567349030778) { - h_4(state, unused, out_6577940567349030778); +void pose_h_4(double *state, double *unused, double *out_3613363558024225199) { + h_4(state, unused, out_3613363558024225199); } -void pose_H_4(double *state, double *unused, double *out_913158878120160673) { - H_4(state, unused, out_913158878120160673); +void pose_H_4(double *state, double *unused, double *out_8770780261124510538) { + H_4(state, unused, out_8770780261124510538); } -void pose_h_10(double *state, double *unused, double *out_3373051515203641771) { - h_10(state, unused, out_3373051515203641771); +void pose_h_10(double *state, double *unused, double *out_7325246586980236980) { + h_10(state, unused, out_7325246586980236980); } -void pose_H_10(double *state, double *unused, double *out_7288760212554836769) { - H_10(state, unused, out_7288760212554836769); +void pose_H_10(double *state, double *unused, double *out_8143750738918534214) { + H_10(state, unused, out_8143750738918534214); } -void pose_h_13(double *state, double *unused, double *out_413669578140119338) { - h_13(state, unused, out_413669578140119338); +void pose_h_13(double *state, double *unused, double *out_4440237870440636920) { + h_13(state, unused, out_4440237870440636920); } -void pose_H_13(double *state, double *unused, double *out_8523790086436861602) { - H_13(state, unused, out_8523790086436861602); +void pose_H_13(double *state, double *unused, double *out_5558506435792177737) { + H_13(state, unused, out_5558506435792177737); } -void pose_h_14(double *state, double *unused, double *out_8770101800505378255) { - h_14(state, unused, out_8770101800505378255); +void pose_h_14(double *state, double *unused, double *out_854572314087465107) { + h_14(state, unused, out_854572314087465107); } -void pose_H_14(double *state, double *unused, double *out_4876399734459645202) { - H_14(state, unused, out_4876399734459645202); +void pose_H_14(double *state, double *unused, double *out_4807539404785026009) { + H_14(state, unused, out_4807539404785026009); } 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 4a1d716f9..29cbd8120 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_5188089899736494345); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_516319331577825853); -void pose_H_mod_fun(double *state, double *out_4951567132479851062); -void pose_f_fun(double *state, double dt, double *out_898454825503606476); -void pose_F_fun(double *state, double dt, double *out_906698228237069812); -void pose_h_4(double *state, double *unused, double *out_6577940567349030778); -void pose_H_4(double *state, double *unused, double *out_913158878120160673); -void pose_h_10(double *state, double *unused, double *out_3373051515203641771); -void pose_H_10(double *state, double *unused, double *out_7288760212554836769); -void pose_h_13(double *state, double *unused, double *out_413669578140119338); -void pose_H_13(double *state, double *unused, double *out_8523790086436861602); -void pose_h_14(double *state, double *unused, double *out_8770101800505378255); -void pose_H_14(double *state, double *unused, double *out_4876399734459645202); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6247163002975687744); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2784495282432373016); +void pose_H_mod_fun(double *state, double *out_662979392671356586); +void pose_f_fun(double *state, double dt, double *out_5358842043988680887); +void pose_F_fun(double *state, double dt, double *out_6970610951587922797); +void pose_h_4(double *state, double *unused, double *out_3613363558024225199); +void pose_H_4(double *state, double *unused, double *out_8770780261124510538); +void pose_h_10(double *state, double *unused, double *out_7325246586980236980); +void pose_H_10(double *state, double *unused, double *out_8143750738918534214); +void pose_h_13(double *state, double *unused, double *out_4440237870440636920); +void pose_H_13(double *state, double *unused, double *out_5558506435792177737); +void pose_h_14(double *state, double *unused, double *out_854572314087465107); +void pose_H_14(double *state, double *unused, double *out_4807539404785026009); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 7c17b3d68..690189863 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index a2e997d1a..7bb6d0122 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index aa0173fba..88f1368d5 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 8c7366acf..9121ea97f 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index cf9361e06..610d5bb34 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 403b84224..d65bc3371 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ