diff --git a/common/libcommon.a b/common/libcommon.a index 803afc0eb..8f7fd325e 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 a1577173a..5ba34cbc9 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 e408c63a2..d6cdc1062 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 7298ebe42..a6207c31a 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 a05d0d269..4cb2f807a 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 85f62d518..46b9d1d44 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 c0914b629..7a5c7ac36 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 ac6930cfd..76e3729ea 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 70f8a3161..695957834 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 70f8a3161..695957834 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 a8b44ff27..ed5bbe3a5 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 ac6930cfd..76e3729ea 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 9aeb3e487..41c2bde33 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-ce98b31c-DEBUG"; +const uint8_t gitversion[19] = "DEV-6ee00d3f-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 8a8bc0e0a..b3bd217cf 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 846674d8e..461407aca 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 c2f53d3c0..5321b4c6f 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 237486485..145b6c2ff 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 76578b651..b0a488b9a 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 aae777fea..4b988f68b 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 4178c5fc6..4cfc88ca8 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 1df3629ac..cee69bb26 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 d73895f23..882889cd2 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 4c4efc10a..fea89a823 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 d17b23b37..a7a30f5cc 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 57ed15daa..d441e8bc9 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 66379d45a..b6034f158 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 a13a0fce7..a60faaec9 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 031bce653..8173cc10f 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 4fbc58863..f44fd5993 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 91c9c3be7..13c350da7 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 c0fc7c144..31a6380fe 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 2d681e4f4..7cedbf9a1 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 a402c7b44..91a7a2ea8 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 20f9954a9..da62c943d 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-ce98b31c-DEBUG \ No newline at end of file +DEV-6ee00d3f-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 26ccefdc4..187abd1e4 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_4621547673265240374) { - out_4621547673265240374[0] = delta_x[0] + nom_x[0]; - out_4621547673265240374[1] = delta_x[1] + nom_x[1]; - out_4621547673265240374[2] = delta_x[2] + nom_x[2]; - out_4621547673265240374[3] = delta_x[3] + nom_x[3]; - out_4621547673265240374[4] = delta_x[4] + nom_x[4]; - out_4621547673265240374[5] = delta_x[5] + nom_x[5]; - out_4621547673265240374[6] = delta_x[6] + nom_x[6]; - out_4621547673265240374[7] = delta_x[7] + nom_x[7]; - out_4621547673265240374[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1099503832950344155) { + out_1099503832950344155[0] = delta_x[0] + nom_x[0]; + out_1099503832950344155[1] = delta_x[1] + nom_x[1]; + out_1099503832950344155[2] = delta_x[2] + nom_x[2]; + out_1099503832950344155[3] = delta_x[3] + nom_x[3]; + out_1099503832950344155[4] = delta_x[4] + nom_x[4]; + out_1099503832950344155[5] = delta_x[5] + nom_x[5]; + out_1099503832950344155[6] = delta_x[6] + nom_x[6]; + out_1099503832950344155[7] = delta_x[7] + nom_x[7]; + out_1099503832950344155[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4527069186343691966) { - out_4527069186343691966[0] = -nom_x[0] + true_x[0]; - out_4527069186343691966[1] = -nom_x[1] + true_x[1]; - out_4527069186343691966[2] = -nom_x[2] + true_x[2]; - out_4527069186343691966[3] = -nom_x[3] + true_x[3]; - out_4527069186343691966[4] = -nom_x[4] + true_x[4]; - out_4527069186343691966[5] = -nom_x[5] + true_x[5]; - out_4527069186343691966[6] = -nom_x[6] + true_x[6]; - out_4527069186343691966[7] = -nom_x[7] + true_x[7]; - out_4527069186343691966[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1857055699068035744) { + out_1857055699068035744[0] = -nom_x[0] + true_x[0]; + out_1857055699068035744[1] = -nom_x[1] + true_x[1]; + out_1857055699068035744[2] = -nom_x[2] + true_x[2]; + out_1857055699068035744[3] = -nom_x[3] + true_x[3]; + out_1857055699068035744[4] = -nom_x[4] + true_x[4]; + out_1857055699068035744[5] = -nom_x[5] + true_x[5]; + out_1857055699068035744[6] = -nom_x[6] + true_x[6]; + out_1857055699068035744[7] = -nom_x[7] + true_x[7]; + out_1857055699068035744[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_6693843632392876727) { - out_6693843632392876727[0] = 1.0; - out_6693843632392876727[1] = 0.0; - out_6693843632392876727[2] = 0.0; - out_6693843632392876727[3] = 0.0; - out_6693843632392876727[4] = 0.0; - out_6693843632392876727[5] = 0.0; - out_6693843632392876727[6] = 0.0; - out_6693843632392876727[7] = 0.0; - out_6693843632392876727[8] = 0.0; - out_6693843632392876727[9] = 0.0; - out_6693843632392876727[10] = 1.0; - out_6693843632392876727[11] = 0.0; - out_6693843632392876727[12] = 0.0; - out_6693843632392876727[13] = 0.0; - out_6693843632392876727[14] = 0.0; - out_6693843632392876727[15] = 0.0; - out_6693843632392876727[16] = 0.0; - out_6693843632392876727[17] = 0.0; - out_6693843632392876727[18] = 0.0; - out_6693843632392876727[19] = 0.0; - out_6693843632392876727[20] = 1.0; - out_6693843632392876727[21] = 0.0; - out_6693843632392876727[22] = 0.0; - out_6693843632392876727[23] = 0.0; - out_6693843632392876727[24] = 0.0; - out_6693843632392876727[25] = 0.0; - out_6693843632392876727[26] = 0.0; - out_6693843632392876727[27] = 0.0; - out_6693843632392876727[28] = 0.0; - out_6693843632392876727[29] = 0.0; - out_6693843632392876727[30] = 1.0; - out_6693843632392876727[31] = 0.0; - out_6693843632392876727[32] = 0.0; - out_6693843632392876727[33] = 0.0; - out_6693843632392876727[34] = 0.0; - out_6693843632392876727[35] = 0.0; - out_6693843632392876727[36] = 0.0; - out_6693843632392876727[37] = 0.0; - out_6693843632392876727[38] = 0.0; - out_6693843632392876727[39] = 0.0; - out_6693843632392876727[40] = 1.0; - out_6693843632392876727[41] = 0.0; - out_6693843632392876727[42] = 0.0; - out_6693843632392876727[43] = 0.0; - out_6693843632392876727[44] = 0.0; - out_6693843632392876727[45] = 0.0; - out_6693843632392876727[46] = 0.0; - out_6693843632392876727[47] = 0.0; - out_6693843632392876727[48] = 0.0; - out_6693843632392876727[49] = 0.0; - out_6693843632392876727[50] = 1.0; - out_6693843632392876727[51] = 0.0; - out_6693843632392876727[52] = 0.0; - out_6693843632392876727[53] = 0.0; - out_6693843632392876727[54] = 0.0; - out_6693843632392876727[55] = 0.0; - out_6693843632392876727[56] = 0.0; - out_6693843632392876727[57] = 0.0; - out_6693843632392876727[58] = 0.0; - out_6693843632392876727[59] = 0.0; - out_6693843632392876727[60] = 1.0; - out_6693843632392876727[61] = 0.0; - out_6693843632392876727[62] = 0.0; - out_6693843632392876727[63] = 0.0; - out_6693843632392876727[64] = 0.0; - out_6693843632392876727[65] = 0.0; - out_6693843632392876727[66] = 0.0; - out_6693843632392876727[67] = 0.0; - out_6693843632392876727[68] = 0.0; - out_6693843632392876727[69] = 0.0; - out_6693843632392876727[70] = 1.0; - out_6693843632392876727[71] = 0.0; - out_6693843632392876727[72] = 0.0; - out_6693843632392876727[73] = 0.0; - out_6693843632392876727[74] = 0.0; - out_6693843632392876727[75] = 0.0; - out_6693843632392876727[76] = 0.0; - out_6693843632392876727[77] = 0.0; - out_6693843632392876727[78] = 0.0; - out_6693843632392876727[79] = 0.0; - out_6693843632392876727[80] = 1.0; +void H_mod_fun(double *state, double *out_6364761287714699355) { + out_6364761287714699355[0] = 1.0; + out_6364761287714699355[1] = 0.0; + out_6364761287714699355[2] = 0.0; + out_6364761287714699355[3] = 0.0; + out_6364761287714699355[4] = 0.0; + out_6364761287714699355[5] = 0.0; + out_6364761287714699355[6] = 0.0; + out_6364761287714699355[7] = 0.0; + out_6364761287714699355[8] = 0.0; + out_6364761287714699355[9] = 0.0; + out_6364761287714699355[10] = 1.0; + out_6364761287714699355[11] = 0.0; + out_6364761287714699355[12] = 0.0; + out_6364761287714699355[13] = 0.0; + out_6364761287714699355[14] = 0.0; + out_6364761287714699355[15] = 0.0; + out_6364761287714699355[16] = 0.0; + out_6364761287714699355[17] = 0.0; + out_6364761287714699355[18] = 0.0; + out_6364761287714699355[19] = 0.0; + out_6364761287714699355[20] = 1.0; + out_6364761287714699355[21] = 0.0; + out_6364761287714699355[22] = 0.0; + out_6364761287714699355[23] = 0.0; + out_6364761287714699355[24] = 0.0; + out_6364761287714699355[25] = 0.0; + out_6364761287714699355[26] = 0.0; + out_6364761287714699355[27] = 0.0; + out_6364761287714699355[28] = 0.0; + out_6364761287714699355[29] = 0.0; + out_6364761287714699355[30] = 1.0; + out_6364761287714699355[31] = 0.0; + out_6364761287714699355[32] = 0.0; + out_6364761287714699355[33] = 0.0; + out_6364761287714699355[34] = 0.0; + out_6364761287714699355[35] = 0.0; + out_6364761287714699355[36] = 0.0; + out_6364761287714699355[37] = 0.0; + out_6364761287714699355[38] = 0.0; + out_6364761287714699355[39] = 0.0; + out_6364761287714699355[40] = 1.0; + out_6364761287714699355[41] = 0.0; + out_6364761287714699355[42] = 0.0; + out_6364761287714699355[43] = 0.0; + out_6364761287714699355[44] = 0.0; + out_6364761287714699355[45] = 0.0; + out_6364761287714699355[46] = 0.0; + out_6364761287714699355[47] = 0.0; + out_6364761287714699355[48] = 0.0; + out_6364761287714699355[49] = 0.0; + out_6364761287714699355[50] = 1.0; + out_6364761287714699355[51] = 0.0; + out_6364761287714699355[52] = 0.0; + out_6364761287714699355[53] = 0.0; + out_6364761287714699355[54] = 0.0; + out_6364761287714699355[55] = 0.0; + out_6364761287714699355[56] = 0.0; + out_6364761287714699355[57] = 0.0; + out_6364761287714699355[58] = 0.0; + out_6364761287714699355[59] = 0.0; + out_6364761287714699355[60] = 1.0; + out_6364761287714699355[61] = 0.0; + out_6364761287714699355[62] = 0.0; + out_6364761287714699355[63] = 0.0; + out_6364761287714699355[64] = 0.0; + out_6364761287714699355[65] = 0.0; + out_6364761287714699355[66] = 0.0; + out_6364761287714699355[67] = 0.0; + out_6364761287714699355[68] = 0.0; + out_6364761287714699355[69] = 0.0; + out_6364761287714699355[70] = 1.0; + out_6364761287714699355[71] = 0.0; + out_6364761287714699355[72] = 0.0; + out_6364761287714699355[73] = 0.0; + out_6364761287714699355[74] = 0.0; + out_6364761287714699355[75] = 0.0; + out_6364761287714699355[76] = 0.0; + out_6364761287714699355[77] = 0.0; + out_6364761287714699355[78] = 0.0; + out_6364761287714699355[79] = 0.0; + out_6364761287714699355[80] = 1.0; } -void f_fun(double *state, double dt, double *out_6624366898111013898) { - out_6624366898111013898[0] = state[0]; - out_6624366898111013898[1] = state[1]; - out_6624366898111013898[2] = state[2]; - out_6624366898111013898[3] = state[3]; - out_6624366898111013898[4] = state[4]; - out_6624366898111013898[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_6624366898111013898[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_6624366898111013898[7] = state[7]; - out_6624366898111013898[8] = state[8]; +void f_fun(double *state, double dt, double *out_8704640073878126620) { + out_8704640073878126620[0] = state[0]; + out_8704640073878126620[1] = state[1]; + out_8704640073878126620[2] = state[2]; + out_8704640073878126620[3] = state[3]; + out_8704640073878126620[4] = state[4]; + out_8704640073878126620[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_8704640073878126620[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_8704640073878126620[7] = state[7]; + out_8704640073878126620[8] = state[8]; } -void F_fun(double *state, double dt, double *out_6958933586889171013) { - out_6958933586889171013[0] = 1; - out_6958933586889171013[1] = 0; - out_6958933586889171013[2] = 0; - out_6958933586889171013[3] = 0; - out_6958933586889171013[4] = 0; - out_6958933586889171013[5] = 0; - out_6958933586889171013[6] = 0; - out_6958933586889171013[7] = 0; - out_6958933586889171013[8] = 0; - out_6958933586889171013[9] = 0; - out_6958933586889171013[10] = 1; - out_6958933586889171013[11] = 0; - out_6958933586889171013[12] = 0; - out_6958933586889171013[13] = 0; - out_6958933586889171013[14] = 0; - out_6958933586889171013[15] = 0; - out_6958933586889171013[16] = 0; - out_6958933586889171013[17] = 0; - out_6958933586889171013[18] = 0; - out_6958933586889171013[19] = 0; - out_6958933586889171013[20] = 1; - out_6958933586889171013[21] = 0; - out_6958933586889171013[22] = 0; - out_6958933586889171013[23] = 0; - out_6958933586889171013[24] = 0; - out_6958933586889171013[25] = 0; - out_6958933586889171013[26] = 0; - out_6958933586889171013[27] = 0; - out_6958933586889171013[28] = 0; - out_6958933586889171013[29] = 0; - out_6958933586889171013[30] = 1; - out_6958933586889171013[31] = 0; - out_6958933586889171013[32] = 0; - out_6958933586889171013[33] = 0; - out_6958933586889171013[34] = 0; - out_6958933586889171013[35] = 0; - out_6958933586889171013[36] = 0; - out_6958933586889171013[37] = 0; - out_6958933586889171013[38] = 0; - out_6958933586889171013[39] = 0; - out_6958933586889171013[40] = 1; - out_6958933586889171013[41] = 0; - out_6958933586889171013[42] = 0; - out_6958933586889171013[43] = 0; - out_6958933586889171013[44] = 0; - out_6958933586889171013[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_6958933586889171013[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_6958933586889171013[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6958933586889171013[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6958933586889171013[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_6958933586889171013[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_6958933586889171013[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_6958933586889171013[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_6958933586889171013[53] = -9.8100000000000005*dt; - out_6958933586889171013[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_6958933586889171013[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_6958933586889171013[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6958933586889171013[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6958933586889171013[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_6958933586889171013[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_6958933586889171013[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_6958933586889171013[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6958933586889171013[62] = 0; - out_6958933586889171013[63] = 0; - out_6958933586889171013[64] = 0; - out_6958933586889171013[65] = 0; - out_6958933586889171013[66] = 0; - out_6958933586889171013[67] = 0; - out_6958933586889171013[68] = 0; - out_6958933586889171013[69] = 0; - out_6958933586889171013[70] = 1; - out_6958933586889171013[71] = 0; - out_6958933586889171013[72] = 0; - out_6958933586889171013[73] = 0; - out_6958933586889171013[74] = 0; - out_6958933586889171013[75] = 0; - out_6958933586889171013[76] = 0; - out_6958933586889171013[77] = 0; - out_6958933586889171013[78] = 0; - out_6958933586889171013[79] = 0; - out_6958933586889171013[80] = 1; +void F_fun(double *state, double dt, double *out_2231700017024740017) { + out_2231700017024740017[0] = 1; + out_2231700017024740017[1] = 0; + out_2231700017024740017[2] = 0; + out_2231700017024740017[3] = 0; + out_2231700017024740017[4] = 0; + out_2231700017024740017[5] = 0; + out_2231700017024740017[6] = 0; + out_2231700017024740017[7] = 0; + out_2231700017024740017[8] = 0; + out_2231700017024740017[9] = 0; + out_2231700017024740017[10] = 1; + out_2231700017024740017[11] = 0; + out_2231700017024740017[12] = 0; + out_2231700017024740017[13] = 0; + out_2231700017024740017[14] = 0; + out_2231700017024740017[15] = 0; + out_2231700017024740017[16] = 0; + out_2231700017024740017[17] = 0; + out_2231700017024740017[18] = 0; + out_2231700017024740017[19] = 0; + out_2231700017024740017[20] = 1; + out_2231700017024740017[21] = 0; + out_2231700017024740017[22] = 0; + out_2231700017024740017[23] = 0; + out_2231700017024740017[24] = 0; + out_2231700017024740017[25] = 0; + out_2231700017024740017[26] = 0; + out_2231700017024740017[27] = 0; + out_2231700017024740017[28] = 0; + out_2231700017024740017[29] = 0; + out_2231700017024740017[30] = 1; + out_2231700017024740017[31] = 0; + out_2231700017024740017[32] = 0; + out_2231700017024740017[33] = 0; + out_2231700017024740017[34] = 0; + out_2231700017024740017[35] = 0; + out_2231700017024740017[36] = 0; + out_2231700017024740017[37] = 0; + out_2231700017024740017[38] = 0; + out_2231700017024740017[39] = 0; + out_2231700017024740017[40] = 1; + out_2231700017024740017[41] = 0; + out_2231700017024740017[42] = 0; + out_2231700017024740017[43] = 0; + out_2231700017024740017[44] = 0; + out_2231700017024740017[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_2231700017024740017[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2231700017024740017[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2231700017024740017[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2231700017024740017[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_2231700017024740017[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2231700017024740017[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2231700017024740017[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2231700017024740017[53] = -9.8100000000000005*dt; + out_2231700017024740017[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_2231700017024740017[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2231700017024740017[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2231700017024740017[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2231700017024740017[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_2231700017024740017[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2231700017024740017[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_2231700017024740017[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2231700017024740017[62] = 0; + out_2231700017024740017[63] = 0; + out_2231700017024740017[64] = 0; + out_2231700017024740017[65] = 0; + out_2231700017024740017[66] = 0; + out_2231700017024740017[67] = 0; + out_2231700017024740017[68] = 0; + out_2231700017024740017[69] = 0; + out_2231700017024740017[70] = 1; + out_2231700017024740017[71] = 0; + out_2231700017024740017[72] = 0; + out_2231700017024740017[73] = 0; + out_2231700017024740017[74] = 0; + out_2231700017024740017[75] = 0; + out_2231700017024740017[76] = 0; + out_2231700017024740017[77] = 0; + out_2231700017024740017[78] = 0; + out_2231700017024740017[79] = 0; + out_2231700017024740017[80] = 1; } -void h_25(double *state, double *unused, double *out_5696314533542565340) { - out_5696314533542565340[0] = state[6]; +void h_25(double *state, double *unused, double *out_4358124578925276600) { + out_4358124578925276600[0] = state[6]; } -void H_25(double *state, double *unused, double *out_5255877481426644938) { - out_5255877481426644938[0] = 0; - out_5255877481426644938[1] = 0; - out_5255877481426644938[2] = 0; - out_5255877481426644938[3] = 0; - out_5255877481426644938[4] = 0; - out_5255877481426644938[5] = 0; - out_5255877481426644938[6] = 1; - out_5255877481426644938[7] = 0; - out_5255877481426644938[8] = 0; +void H_25(double *state, double *unused, double *out_2841780637725207678) { + out_2841780637725207678[0] = 0; + out_2841780637725207678[1] = 0; + out_2841780637725207678[2] = 0; + out_2841780637725207678[3] = 0; + out_2841780637725207678[4] = 0; + out_2841780637725207678[5] = 0; + out_2841780637725207678[6] = 1; + out_2841780637725207678[7] = 0; + out_2841780637725207678[8] = 0; } -void h_24(double *state, double *unused, double *out_5128055596476793433) { - out_5128055596476793433[0] = state[4]; - out_5128055596476793433[1] = state[5]; +void h_24(double *state, double *unused, double *out_8493679030338339311) { + out_8493679030338339311[0] = state[4]; + out_8493679030338339311[1] = state[5]; } -void H_24(double *state, double *unused, double *out_4785419999383306214) { - out_4785419999383306214[0] = 0; - out_4785419999383306214[1] = 0; - out_4785419999383306214[2] = 0; - out_4785419999383306214[3] = 0; - out_4785419999383306214[4] = 1; - out_4785419999383306214[5] = 0; - out_4785419999383306214[6] = 0; - out_4785419999383306214[7] = 0; - out_4785419999383306214[8] = 0; - out_4785419999383306214[9] = 0; - out_4785419999383306214[10] = 0; - out_4785419999383306214[11] = 0; - out_4785419999383306214[12] = 0; - out_4785419999383306214[13] = 0; - out_4785419999383306214[14] = 1; - out_4785419999383306214[15] = 0; - out_4785419999383306214[16] = 0; - out_4785419999383306214[17] = 0; +void H_24(double *state, double *unused, double *out_716144961077115458) { + out_716144961077115458[0] = 0; + out_716144961077115458[1] = 0; + out_716144961077115458[2] = 0; + out_716144961077115458[3] = 0; + out_716144961077115458[4] = 1; + out_716144961077115458[5] = 0; + out_716144961077115458[6] = 0; + out_716144961077115458[7] = 0; + out_716144961077115458[8] = 0; + out_716144961077115458[9] = 0; + out_716144961077115458[10] = 0; + out_716144961077115458[11] = 0; + out_716144961077115458[12] = 0; + out_716144961077115458[13] = 0; + out_716144961077115458[14] = 1; + out_716144961077115458[15] = 0; + out_716144961077115458[16] = 0; + out_716144961077115458[17] = 0; } -void h_30(double *state, double *unused, double *out_8810273519341179810) { - out_8810273519341179810[0] = state[4]; +void h_30(double *state, double *unused, double *out_3523148071828772585) { + out_3523148071828772585[0] = state[4]; } -void H_30(double *state, double *unused, double *out_728181151299036740) { - out_728181151299036740[0] = 0; - out_728181151299036740[1] = 0; - out_728181151299036740[2] = 0; - out_728181151299036740[3] = 0; - out_728181151299036740[4] = 1; - out_728181151299036740[5] = 0; - out_728181151299036740[6] = 0; - out_728181151299036740[7] = 0; - out_728181151299036740[8] = 0; +void H_30(double *state, double *unused, double *out_4074909703766409077) { + out_4074909703766409077[0] = 0; + out_4074909703766409077[1] = 0; + out_4074909703766409077[2] = 0; + out_4074909703766409077[3] = 0; + out_4074909703766409077[4] = 1; + out_4074909703766409077[5] = 0; + out_4074909703766409077[6] = 0; + out_4074909703766409077[7] = 0; + out_4074909703766409077[8] = 0; } -void h_26(double *state, double *unused, double *out_93223108268356141) { - out_93223108268356141[0] = state[7]; +void h_26(double *state, double *unused, double *out_1478525195832073060) { + out_1478525195832073060[0] = state[7]; } -void H_26(double *state, double *unused, double *out_1514374162552588714) { - out_1514374162552588714[0] = 0; - out_1514374162552588714[1] = 0; - out_1514374162552588714[2] = 0; - out_1514374162552588714[3] = 0; - out_1514374162552588714[4] = 0; - out_1514374162552588714[5] = 0; - out_1514374162552588714[6] = 0; - out_1514374162552588714[7] = 1; - out_1514374162552588714[8] = 0; +void H_26(double *state, double *unused, double *out_6583283956599263902) { + out_6583283956599263902[0] = 0; + out_6583283956599263902[1] = 0; + out_6583283956599263902[2] = 0; + out_6583283956599263902[3] = 0; + out_6583283956599263902[4] = 0; + out_6583283956599263902[5] = 0; + out_6583283956599263902[6] = 0; + out_6583283956599263902[7] = 1; + out_6583283956599263902[8] = 0; } -void h_27(double *state, double *unused, double *out_3729886127726509834) { - out_3729886127726509834[0] = state[3]; +void h_27(double *state, double *unused, double *out_643548688735569045) { + out_643548688735569045[0] = state[3]; } -void H_27(double *state, double *unused, double *out_2951775222482979957) { - out_2951775222482979957[0] = 0; - out_2951775222482979957[1] = 0; - out_2951775222482979957[2] = 0; - out_2951775222482979957[3] = 1; - out_2951775222482979957[4] = 0; - out_2951775222482979957[5] = 0; - out_2951775222482979957[6] = 0; - out_2951775222482979957[7] = 0; - out_2951775222482979957[8] = 0; +void H_27(double *state, double *unused, double *out_1900146391965984166) { + out_1900146391965984166[0] = 0; + out_1900146391965984166[1] = 0; + out_1900146391965984166[2] = 0; + out_1900146391965984166[3] = 1; + out_1900146391965984166[4] = 0; + out_1900146391965984166[5] = 0; + out_1900146391965984166[6] = 0; + out_1900146391965984166[7] = 0; + out_1900146391965984166[8] = 0; } -void h_29(double *state, double *unused, double *out_3887072796077638979) { - out_3887072796077638979[0] = state[1]; +void h_29(double *state, double *unused, double *out_4280998216989591395) { + out_4280998216989591395[0] = state[1]; } -void H_29(double *state, double *unused, double *out_1238412495613428924) { - out_1238412495613428924[0] = 0; - out_1238412495613428924[1] = 1; - out_1238412495613428924[2] = 0; - out_1238412495613428924[3] = 0; - out_1238412495613428924[4] = 0; - out_1238412495613428924[5] = 0; - out_1238412495613428924[6] = 0; - out_1238412495613428924[7] = 0; - out_1238412495613428924[8] = 0; +void H_29(double *state, double *unused, double *out_4585141048080801261) { + out_4585141048080801261[0] = 0; + out_4585141048080801261[1] = 1; + out_4585141048080801261[2] = 0; + out_4585141048080801261[3] = 0; + out_4585141048080801261[4] = 0; + out_4585141048080801261[5] = 0; + out_4585141048080801261[6] = 0; + out_4585141048080801261[7] = 0; + out_4585141048080801261[8] = 0; } -void h_28(double *state, double *unused, double *out_1271787139934335811) { - out_1271787139934335811[0] = state[0]; +void h_28(double *state, double *unused, double *out_8338420483271271720) { + out_8338420483271271720[0] = state[0]; } -void H_28(double *state, double *unused, double *out_3843986521456101650) { - out_3843986521456101650[0] = 1; - out_3843986521456101650[1] = 0; - out_3843986521456101650[2] = 0; - out_3843986521456101650[3] = 0; - out_3843986521456101650[4] = 0; - out_3843986521456101650[5] = 0; - out_3843986521456101650[6] = 0; - out_3843986521456101650[7] = 0; - out_3843986521456101650[8] = 0; +void H_28(double *state, double *unused, double *out_497257968988729313) { + out_497257968988729313[0] = 1; + out_497257968988729313[1] = 0; + out_497257968988729313[2] = 0; + out_497257968988729313[3] = 0; + out_497257968988729313[4] = 0; + out_497257968988729313[5] = 0; + out_497257968988729313[6] = 0; + out_497257968988729313[7] = 0; + out_497257968988729313[8] = 0; } -void h_31(double *state, double *unused, double *out_5853501201893694485) { - out_5853501201893694485[0] = state[8]; +void h_31(double *state, double *unused, double *out_7305850569280761561) { + out_7305850569280761561[0] = state[8]; } -void H_31(double *state, double *unused, double *out_5286523443303605366) { - out_5286523443303605366[0] = 0; - out_5286523443303605366[1] = 0; - out_5286523443303605366[2] = 0; - out_5286523443303605366[3] = 0; - out_5286523443303605366[4] = 0; - out_5286523443303605366[5] = 0; - out_5286523443303605366[6] = 0; - out_5286523443303605366[7] = 0; - out_5286523443303605366[8] = 1; +void H_31(double *state, double *unused, double *out_2811134675848247250) { + out_2811134675848247250[0] = 0; + out_2811134675848247250[1] = 0; + out_2811134675848247250[2] = 0; + out_2811134675848247250[3] = 0; + out_2811134675848247250[4] = 0; + out_2811134675848247250[5] = 0; + out_2811134675848247250[6] = 0; + out_2811134675848247250[7] = 0; + out_2811134675848247250[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_4621547673265240374) { - err_fun(nom_x, delta_x, out_4621547673265240374); +void car_err_fun(double *nom_x, double *delta_x, double *out_1099503832950344155) { + err_fun(nom_x, delta_x, out_1099503832950344155); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4527069186343691966) { - inv_err_fun(nom_x, true_x, out_4527069186343691966); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1857055699068035744) { + inv_err_fun(nom_x, true_x, out_1857055699068035744); } -void car_H_mod_fun(double *state, double *out_6693843632392876727) { - H_mod_fun(state, out_6693843632392876727); +void car_H_mod_fun(double *state, double *out_6364761287714699355) { + H_mod_fun(state, out_6364761287714699355); } -void car_f_fun(double *state, double dt, double *out_6624366898111013898) { - f_fun(state, dt, out_6624366898111013898); +void car_f_fun(double *state, double dt, double *out_8704640073878126620) { + f_fun(state, dt, out_8704640073878126620); } -void car_F_fun(double *state, double dt, double *out_6958933586889171013) { - F_fun(state, dt, out_6958933586889171013); +void car_F_fun(double *state, double dt, double *out_2231700017024740017) { + F_fun(state, dt, out_2231700017024740017); } -void car_h_25(double *state, double *unused, double *out_5696314533542565340) { - h_25(state, unused, out_5696314533542565340); +void car_h_25(double *state, double *unused, double *out_4358124578925276600) { + h_25(state, unused, out_4358124578925276600); } -void car_H_25(double *state, double *unused, double *out_5255877481426644938) { - H_25(state, unused, out_5255877481426644938); +void car_H_25(double *state, double *unused, double *out_2841780637725207678) { + H_25(state, unused, out_2841780637725207678); } -void car_h_24(double *state, double *unused, double *out_5128055596476793433) { - h_24(state, unused, out_5128055596476793433); +void car_h_24(double *state, double *unused, double *out_8493679030338339311) { + h_24(state, unused, out_8493679030338339311); } -void car_H_24(double *state, double *unused, double *out_4785419999383306214) { - H_24(state, unused, out_4785419999383306214); +void car_H_24(double *state, double *unused, double *out_716144961077115458) { + H_24(state, unused, out_716144961077115458); } -void car_h_30(double *state, double *unused, double *out_8810273519341179810) { - h_30(state, unused, out_8810273519341179810); +void car_h_30(double *state, double *unused, double *out_3523148071828772585) { + h_30(state, unused, out_3523148071828772585); } -void car_H_30(double *state, double *unused, double *out_728181151299036740) { - H_30(state, unused, out_728181151299036740); +void car_H_30(double *state, double *unused, double *out_4074909703766409077) { + H_30(state, unused, out_4074909703766409077); } -void car_h_26(double *state, double *unused, double *out_93223108268356141) { - h_26(state, unused, out_93223108268356141); +void car_h_26(double *state, double *unused, double *out_1478525195832073060) { + h_26(state, unused, out_1478525195832073060); } -void car_H_26(double *state, double *unused, double *out_1514374162552588714) { - H_26(state, unused, out_1514374162552588714); +void car_H_26(double *state, double *unused, double *out_6583283956599263902) { + H_26(state, unused, out_6583283956599263902); } -void car_h_27(double *state, double *unused, double *out_3729886127726509834) { - h_27(state, unused, out_3729886127726509834); +void car_h_27(double *state, double *unused, double *out_643548688735569045) { + h_27(state, unused, out_643548688735569045); } -void car_H_27(double *state, double *unused, double *out_2951775222482979957) { - H_27(state, unused, out_2951775222482979957); +void car_H_27(double *state, double *unused, double *out_1900146391965984166) { + H_27(state, unused, out_1900146391965984166); } -void car_h_29(double *state, double *unused, double *out_3887072796077638979) { - h_29(state, unused, out_3887072796077638979); +void car_h_29(double *state, double *unused, double *out_4280998216989591395) { + h_29(state, unused, out_4280998216989591395); } -void car_H_29(double *state, double *unused, double *out_1238412495613428924) { - H_29(state, unused, out_1238412495613428924); +void car_H_29(double *state, double *unused, double *out_4585141048080801261) { + H_29(state, unused, out_4585141048080801261); } -void car_h_28(double *state, double *unused, double *out_1271787139934335811) { - h_28(state, unused, out_1271787139934335811); +void car_h_28(double *state, double *unused, double *out_8338420483271271720) { + h_28(state, unused, out_8338420483271271720); } -void car_H_28(double *state, double *unused, double *out_3843986521456101650) { - H_28(state, unused, out_3843986521456101650); +void car_H_28(double *state, double *unused, double *out_497257968988729313) { + H_28(state, unused, out_497257968988729313); } -void car_h_31(double *state, double *unused, double *out_5853501201893694485) { - h_31(state, unused, out_5853501201893694485); +void car_h_31(double *state, double *unused, double *out_7305850569280761561) { + h_31(state, unused, out_7305850569280761561); } -void car_H_31(double *state, double *unused, double *out_5286523443303605366) { - H_31(state, unused, out_5286523443303605366); +void car_H_31(double *state, double *unused, double *out_2811134675848247250) { + H_31(state, unused, out_2811134675848247250); } 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 b33767729..15ddc71ae 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_4621547673265240374); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4527069186343691966); -void car_H_mod_fun(double *state, double *out_6693843632392876727); -void car_f_fun(double *state, double dt, double *out_6624366898111013898); -void car_F_fun(double *state, double dt, double *out_6958933586889171013); -void car_h_25(double *state, double *unused, double *out_5696314533542565340); -void car_H_25(double *state, double *unused, double *out_5255877481426644938); -void car_h_24(double *state, double *unused, double *out_5128055596476793433); -void car_H_24(double *state, double *unused, double *out_4785419999383306214); -void car_h_30(double *state, double *unused, double *out_8810273519341179810); -void car_H_30(double *state, double *unused, double *out_728181151299036740); -void car_h_26(double *state, double *unused, double *out_93223108268356141); -void car_H_26(double *state, double *unused, double *out_1514374162552588714); -void car_h_27(double *state, double *unused, double *out_3729886127726509834); -void car_H_27(double *state, double *unused, double *out_2951775222482979957); -void car_h_29(double *state, double *unused, double *out_3887072796077638979); -void car_H_29(double *state, double *unused, double *out_1238412495613428924); -void car_h_28(double *state, double *unused, double *out_1271787139934335811); -void car_H_28(double *state, double *unused, double *out_3843986521456101650); -void car_h_31(double *state, double *unused, double *out_5853501201893694485); -void car_H_31(double *state, double *unused, double *out_5286523443303605366); +void car_err_fun(double *nom_x, double *delta_x, double *out_1099503832950344155); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1857055699068035744); +void car_H_mod_fun(double *state, double *out_6364761287714699355); +void car_f_fun(double *state, double dt, double *out_8704640073878126620); +void car_F_fun(double *state, double dt, double *out_2231700017024740017); +void car_h_25(double *state, double *unused, double *out_4358124578925276600); +void car_H_25(double *state, double *unused, double *out_2841780637725207678); +void car_h_24(double *state, double *unused, double *out_8493679030338339311); +void car_H_24(double *state, double *unused, double *out_716144961077115458); +void car_h_30(double *state, double *unused, double *out_3523148071828772585); +void car_H_30(double *state, double *unused, double *out_4074909703766409077); +void car_h_26(double *state, double *unused, double *out_1478525195832073060); +void car_H_26(double *state, double *unused, double *out_6583283956599263902); +void car_h_27(double *state, double *unused, double *out_643548688735569045); +void car_H_27(double *state, double *unused, double *out_1900146391965984166); +void car_h_29(double *state, double *unused, double *out_4280998216989591395); +void car_H_29(double *state, double *unused, double *out_4585141048080801261); +void car_h_28(double *state, double *unused, double *out_8338420483271271720); +void car_H_28(double *state, double *unused, double *out_497257968988729313); +void car_h_31(double *state, double *unused, double *out_7305850569280761561); +void car_H_31(double *state, double *unused, double *out_2811134675848247250); 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 78aed83f6..99dbcf1e5 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_8885367235752958450) { - out_8885367235752958450[0] = delta_x[0] + nom_x[0]; - out_8885367235752958450[1] = delta_x[1] + nom_x[1]; - out_8885367235752958450[2] = delta_x[2] + nom_x[2]; - out_8885367235752958450[3] = delta_x[3] + nom_x[3]; - out_8885367235752958450[4] = delta_x[4] + nom_x[4]; - out_8885367235752958450[5] = delta_x[5] + nom_x[5]; - out_8885367235752958450[6] = delta_x[6] + nom_x[6]; - out_8885367235752958450[7] = delta_x[7] + nom_x[7]; - out_8885367235752958450[8] = delta_x[8] + nom_x[8]; - out_8885367235752958450[9] = delta_x[9] + nom_x[9]; - out_8885367235752958450[10] = delta_x[10] + nom_x[10]; - out_8885367235752958450[11] = delta_x[11] + nom_x[11]; - out_8885367235752958450[12] = delta_x[12] + nom_x[12]; - out_8885367235752958450[13] = delta_x[13] + nom_x[13]; - out_8885367235752958450[14] = delta_x[14] + nom_x[14]; - out_8885367235752958450[15] = delta_x[15] + nom_x[15]; - out_8885367235752958450[16] = delta_x[16] + nom_x[16]; - out_8885367235752958450[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7527833659731555578) { + out_7527833659731555578[0] = delta_x[0] + nom_x[0]; + out_7527833659731555578[1] = delta_x[1] + nom_x[1]; + out_7527833659731555578[2] = delta_x[2] + nom_x[2]; + out_7527833659731555578[3] = delta_x[3] + nom_x[3]; + out_7527833659731555578[4] = delta_x[4] + nom_x[4]; + out_7527833659731555578[5] = delta_x[5] + nom_x[5]; + out_7527833659731555578[6] = delta_x[6] + nom_x[6]; + out_7527833659731555578[7] = delta_x[7] + nom_x[7]; + out_7527833659731555578[8] = delta_x[8] + nom_x[8]; + out_7527833659731555578[9] = delta_x[9] + nom_x[9]; + out_7527833659731555578[10] = delta_x[10] + nom_x[10]; + out_7527833659731555578[11] = delta_x[11] + nom_x[11]; + out_7527833659731555578[12] = delta_x[12] + nom_x[12]; + out_7527833659731555578[13] = delta_x[13] + nom_x[13]; + out_7527833659731555578[14] = delta_x[14] + nom_x[14]; + out_7527833659731555578[15] = delta_x[15] + nom_x[15]; + out_7527833659731555578[16] = delta_x[16] + nom_x[16]; + out_7527833659731555578[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4135735359507688267) { - out_4135735359507688267[0] = -nom_x[0] + true_x[0]; - out_4135735359507688267[1] = -nom_x[1] + true_x[1]; - out_4135735359507688267[2] = -nom_x[2] + true_x[2]; - out_4135735359507688267[3] = -nom_x[3] + true_x[3]; - out_4135735359507688267[4] = -nom_x[4] + true_x[4]; - out_4135735359507688267[5] = -nom_x[5] + true_x[5]; - out_4135735359507688267[6] = -nom_x[6] + true_x[6]; - out_4135735359507688267[7] = -nom_x[7] + true_x[7]; - out_4135735359507688267[8] = -nom_x[8] + true_x[8]; - out_4135735359507688267[9] = -nom_x[9] + true_x[9]; - out_4135735359507688267[10] = -nom_x[10] + true_x[10]; - out_4135735359507688267[11] = -nom_x[11] + true_x[11]; - out_4135735359507688267[12] = -nom_x[12] + true_x[12]; - out_4135735359507688267[13] = -nom_x[13] + true_x[13]; - out_4135735359507688267[14] = -nom_x[14] + true_x[14]; - out_4135735359507688267[15] = -nom_x[15] + true_x[15]; - out_4135735359507688267[16] = -nom_x[16] + true_x[16]; - out_4135735359507688267[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8121371430920178446) { + out_8121371430920178446[0] = -nom_x[0] + true_x[0]; + out_8121371430920178446[1] = -nom_x[1] + true_x[1]; + out_8121371430920178446[2] = -nom_x[2] + true_x[2]; + out_8121371430920178446[3] = -nom_x[3] + true_x[3]; + out_8121371430920178446[4] = -nom_x[4] + true_x[4]; + out_8121371430920178446[5] = -nom_x[5] + true_x[5]; + out_8121371430920178446[6] = -nom_x[6] + true_x[6]; + out_8121371430920178446[7] = -nom_x[7] + true_x[7]; + out_8121371430920178446[8] = -nom_x[8] + true_x[8]; + out_8121371430920178446[9] = -nom_x[9] + true_x[9]; + out_8121371430920178446[10] = -nom_x[10] + true_x[10]; + out_8121371430920178446[11] = -nom_x[11] + true_x[11]; + out_8121371430920178446[12] = -nom_x[12] + true_x[12]; + out_8121371430920178446[13] = -nom_x[13] + true_x[13]; + out_8121371430920178446[14] = -nom_x[14] + true_x[14]; + out_8121371430920178446[15] = -nom_x[15] + true_x[15]; + out_8121371430920178446[16] = -nom_x[16] + true_x[16]; + out_8121371430920178446[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_2496917717950831316) { - out_2496917717950831316[0] = 1.0; - out_2496917717950831316[1] = 0.0; - out_2496917717950831316[2] = 0.0; - out_2496917717950831316[3] = 0.0; - out_2496917717950831316[4] = 0.0; - out_2496917717950831316[5] = 0.0; - out_2496917717950831316[6] = 0.0; - out_2496917717950831316[7] = 0.0; - out_2496917717950831316[8] = 0.0; - out_2496917717950831316[9] = 0.0; - out_2496917717950831316[10] = 0.0; - out_2496917717950831316[11] = 0.0; - out_2496917717950831316[12] = 0.0; - out_2496917717950831316[13] = 0.0; - out_2496917717950831316[14] = 0.0; - out_2496917717950831316[15] = 0.0; - out_2496917717950831316[16] = 0.0; - out_2496917717950831316[17] = 0.0; - out_2496917717950831316[18] = 0.0; - out_2496917717950831316[19] = 1.0; - out_2496917717950831316[20] = 0.0; - out_2496917717950831316[21] = 0.0; - out_2496917717950831316[22] = 0.0; - out_2496917717950831316[23] = 0.0; - out_2496917717950831316[24] = 0.0; - out_2496917717950831316[25] = 0.0; - out_2496917717950831316[26] = 0.0; - out_2496917717950831316[27] = 0.0; - out_2496917717950831316[28] = 0.0; - out_2496917717950831316[29] = 0.0; - out_2496917717950831316[30] = 0.0; - out_2496917717950831316[31] = 0.0; - out_2496917717950831316[32] = 0.0; - out_2496917717950831316[33] = 0.0; - out_2496917717950831316[34] = 0.0; - out_2496917717950831316[35] = 0.0; - out_2496917717950831316[36] = 0.0; - out_2496917717950831316[37] = 0.0; - out_2496917717950831316[38] = 1.0; - out_2496917717950831316[39] = 0.0; - out_2496917717950831316[40] = 0.0; - out_2496917717950831316[41] = 0.0; - out_2496917717950831316[42] = 0.0; - out_2496917717950831316[43] = 0.0; - out_2496917717950831316[44] = 0.0; - out_2496917717950831316[45] = 0.0; - out_2496917717950831316[46] = 0.0; - out_2496917717950831316[47] = 0.0; - out_2496917717950831316[48] = 0.0; - out_2496917717950831316[49] = 0.0; - out_2496917717950831316[50] = 0.0; - out_2496917717950831316[51] = 0.0; - out_2496917717950831316[52] = 0.0; - out_2496917717950831316[53] = 0.0; - out_2496917717950831316[54] = 0.0; - out_2496917717950831316[55] = 0.0; - out_2496917717950831316[56] = 0.0; - out_2496917717950831316[57] = 1.0; - out_2496917717950831316[58] = 0.0; - out_2496917717950831316[59] = 0.0; - out_2496917717950831316[60] = 0.0; - out_2496917717950831316[61] = 0.0; - out_2496917717950831316[62] = 0.0; - out_2496917717950831316[63] = 0.0; - out_2496917717950831316[64] = 0.0; - out_2496917717950831316[65] = 0.0; - out_2496917717950831316[66] = 0.0; - out_2496917717950831316[67] = 0.0; - out_2496917717950831316[68] = 0.0; - out_2496917717950831316[69] = 0.0; - out_2496917717950831316[70] = 0.0; - out_2496917717950831316[71] = 0.0; - out_2496917717950831316[72] = 0.0; - out_2496917717950831316[73] = 0.0; - out_2496917717950831316[74] = 0.0; - out_2496917717950831316[75] = 0.0; - out_2496917717950831316[76] = 1.0; - out_2496917717950831316[77] = 0.0; - out_2496917717950831316[78] = 0.0; - out_2496917717950831316[79] = 0.0; - out_2496917717950831316[80] = 0.0; - out_2496917717950831316[81] = 0.0; - out_2496917717950831316[82] = 0.0; - out_2496917717950831316[83] = 0.0; - out_2496917717950831316[84] = 0.0; - out_2496917717950831316[85] = 0.0; - out_2496917717950831316[86] = 0.0; - out_2496917717950831316[87] = 0.0; - out_2496917717950831316[88] = 0.0; - out_2496917717950831316[89] = 0.0; - out_2496917717950831316[90] = 0.0; - out_2496917717950831316[91] = 0.0; - out_2496917717950831316[92] = 0.0; - out_2496917717950831316[93] = 0.0; - out_2496917717950831316[94] = 0.0; - out_2496917717950831316[95] = 1.0; - out_2496917717950831316[96] = 0.0; - out_2496917717950831316[97] = 0.0; - out_2496917717950831316[98] = 0.0; - out_2496917717950831316[99] = 0.0; - out_2496917717950831316[100] = 0.0; - out_2496917717950831316[101] = 0.0; - out_2496917717950831316[102] = 0.0; - out_2496917717950831316[103] = 0.0; - out_2496917717950831316[104] = 0.0; - out_2496917717950831316[105] = 0.0; - out_2496917717950831316[106] = 0.0; - out_2496917717950831316[107] = 0.0; - out_2496917717950831316[108] = 0.0; - out_2496917717950831316[109] = 0.0; - out_2496917717950831316[110] = 0.0; - out_2496917717950831316[111] = 0.0; - out_2496917717950831316[112] = 0.0; - out_2496917717950831316[113] = 0.0; - out_2496917717950831316[114] = 1.0; - out_2496917717950831316[115] = 0.0; - out_2496917717950831316[116] = 0.0; - out_2496917717950831316[117] = 0.0; - out_2496917717950831316[118] = 0.0; - out_2496917717950831316[119] = 0.0; - out_2496917717950831316[120] = 0.0; - out_2496917717950831316[121] = 0.0; - out_2496917717950831316[122] = 0.0; - out_2496917717950831316[123] = 0.0; - out_2496917717950831316[124] = 0.0; - out_2496917717950831316[125] = 0.0; - out_2496917717950831316[126] = 0.0; - out_2496917717950831316[127] = 0.0; - out_2496917717950831316[128] = 0.0; - out_2496917717950831316[129] = 0.0; - out_2496917717950831316[130] = 0.0; - out_2496917717950831316[131] = 0.0; - out_2496917717950831316[132] = 0.0; - out_2496917717950831316[133] = 1.0; - out_2496917717950831316[134] = 0.0; - out_2496917717950831316[135] = 0.0; - out_2496917717950831316[136] = 0.0; - out_2496917717950831316[137] = 0.0; - out_2496917717950831316[138] = 0.0; - out_2496917717950831316[139] = 0.0; - out_2496917717950831316[140] = 0.0; - out_2496917717950831316[141] = 0.0; - out_2496917717950831316[142] = 0.0; - out_2496917717950831316[143] = 0.0; - out_2496917717950831316[144] = 0.0; - out_2496917717950831316[145] = 0.0; - out_2496917717950831316[146] = 0.0; - out_2496917717950831316[147] = 0.0; - out_2496917717950831316[148] = 0.0; - out_2496917717950831316[149] = 0.0; - out_2496917717950831316[150] = 0.0; - out_2496917717950831316[151] = 0.0; - out_2496917717950831316[152] = 1.0; - out_2496917717950831316[153] = 0.0; - out_2496917717950831316[154] = 0.0; - out_2496917717950831316[155] = 0.0; - out_2496917717950831316[156] = 0.0; - out_2496917717950831316[157] = 0.0; - out_2496917717950831316[158] = 0.0; - out_2496917717950831316[159] = 0.0; - out_2496917717950831316[160] = 0.0; - out_2496917717950831316[161] = 0.0; - out_2496917717950831316[162] = 0.0; - out_2496917717950831316[163] = 0.0; - out_2496917717950831316[164] = 0.0; - out_2496917717950831316[165] = 0.0; - out_2496917717950831316[166] = 0.0; - out_2496917717950831316[167] = 0.0; - out_2496917717950831316[168] = 0.0; - out_2496917717950831316[169] = 0.0; - out_2496917717950831316[170] = 0.0; - out_2496917717950831316[171] = 1.0; - out_2496917717950831316[172] = 0.0; - out_2496917717950831316[173] = 0.0; - out_2496917717950831316[174] = 0.0; - out_2496917717950831316[175] = 0.0; - out_2496917717950831316[176] = 0.0; - out_2496917717950831316[177] = 0.0; - out_2496917717950831316[178] = 0.0; - out_2496917717950831316[179] = 0.0; - out_2496917717950831316[180] = 0.0; - out_2496917717950831316[181] = 0.0; - out_2496917717950831316[182] = 0.0; - out_2496917717950831316[183] = 0.0; - out_2496917717950831316[184] = 0.0; - out_2496917717950831316[185] = 0.0; - out_2496917717950831316[186] = 0.0; - out_2496917717950831316[187] = 0.0; - out_2496917717950831316[188] = 0.0; - out_2496917717950831316[189] = 0.0; - out_2496917717950831316[190] = 1.0; - out_2496917717950831316[191] = 0.0; - out_2496917717950831316[192] = 0.0; - out_2496917717950831316[193] = 0.0; - out_2496917717950831316[194] = 0.0; - out_2496917717950831316[195] = 0.0; - out_2496917717950831316[196] = 0.0; - out_2496917717950831316[197] = 0.0; - out_2496917717950831316[198] = 0.0; - out_2496917717950831316[199] = 0.0; - out_2496917717950831316[200] = 0.0; - out_2496917717950831316[201] = 0.0; - out_2496917717950831316[202] = 0.0; - out_2496917717950831316[203] = 0.0; - out_2496917717950831316[204] = 0.0; - out_2496917717950831316[205] = 0.0; - out_2496917717950831316[206] = 0.0; - out_2496917717950831316[207] = 0.0; - out_2496917717950831316[208] = 0.0; - out_2496917717950831316[209] = 1.0; - out_2496917717950831316[210] = 0.0; - out_2496917717950831316[211] = 0.0; - out_2496917717950831316[212] = 0.0; - out_2496917717950831316[213] = 0.0; - out_2496917717950831316[214] = 0.0; - out_2496917717950831316[215] = 0.0; - out_2496917717950831316[216] = 0.0; - out_2496917717950831316[217] = 0.0; - out_2496917717950831316[218] = 0.0; - out_2496917717950831316[219] = 0.0; - out_2496917717950831316[220] = 0.0; - out_2496917717950831316[221] = 0.0; - out_2496917717950831316[222] = 0.0; - out_2496917717950831316[223] = 0.0; - out_2496917717950831316[224] = 0.0; - out_2496917717950831316[225] = 0.0; - out_2496917717950831316[226] = 0.0; - out_2496917717950831316[227] = 0.0; - out_2496917717950831316[228] = 1.0; - out_2496917717950831316[229] = 0.0; - out_2496917717950831316[230] = 0.0; - out_2496917717950831316[231] = 0.0; - out_2496917717950831316[232] = 0.0; - out_2496917717950831316[233] = 0.0; - out_2496917717950831316[234] = 0.0; - out_2496917717950831316[235] = 0.0; - out_2496917717950831316[236] = 0.0; - out_2496917717950831316[237] = 0.0; - out_2496917717950831316[238] = 0.0; - out_2496917717950831316[239] = 0.0; - out_2496917717950831316[240] = 0.0; - out_2496917717950831316[241] = 0.0; - out_2496917717950831316[242] = 0.0; - out_2496917717950831316[243] = 0.0; - out_2496917717950831316[244] = 0.0; - out_2496917717950831316[245] = 0.0; - out_2496917717950831316[246] = 0.0; - out_2496917717950831316[247] = 1.0; - out_2496917717950831316[248] = 0.0; - out_2496917717950831316[249] = 0.0; - out_2496917717950831316[250] = 0.0; - out_2496917717950831316[251] = 0.0; - out_2496917717950831316[252] = 0.0; - out_2496917717950831316[253] = 0.0; - out_2496917717950831316[254] = 0.0; - out_2496917717950831316[255] = 0.0; - out_2496917717950831316[256] = 0.0; - out_2496917717950831316[257] = 0.0; - out_2496917717950831316[258] = 0.0; - out_2496917717950831316[259] = 0.0; - out_2496917717950831316[260] = 0.0; - out_2496917717950831316[261] = 0.0; - out_2496917717950831316[262] = 0.0; - out_2496917717950831316[263] = 0.0; - out_2496917717950831316[264] = 0.0; - out_2496917717950831316[265] = 0.0; - out_2496917717950831316[266] = 1.0; - out_2496917717950831316[267] = 0.0; - out_2496917717950831316[268] = 0.0; - out_2496917717950831316[269] = 0.0; - out_2496917717950831316[270] = 0.0; - out_2496917717950831316[271] = 0.0; - out_2496917717950831316[272] = 0.0; - out_2496917717950831316[273] = 0.0; - out_2496917717950831316[274] = 0.0; - out_2496917717950831316[275] = 0.0; - out_2496917717950831316[276] = 0.0; - out_2496917717950831316[277] = 0.0; - out_2496917717950831316[278] = 0.0; - out_2496917717950831316[279] = 0.0; - out_2496917717950831316[280] = 0.0; - out_2496917717950831316[281] = 0.0; - out_2496917717950831316[282] = 0.0; - out_2496917717950831316[283] = 0.0; - out_2496917717950831316[284] = 0.0; - out_2496917717950831316[285] = 1.0; - out_2496917717950831316[286] = 0.0; - out_2496917717950831316[287] = 0.0; - out_2496917717950831316[288] = 0.0; - out_2496917717950831316[289] = 0.0; - out_2496917717950831316[290] = 0.0; - out_2496917717950831316[291] = 0.0; - out_2496917717950831316[292] = 0.0; - out_2496917717950831316[293] = 0.0; - out_2496917717950831316[294] = 0.0; - out_2496917717950831316[295] = 0.0; - out_2496917717950831316[296] = 0.0; - out_2496917717950831316[297] = 0.0; - out_2496917717950831316[298] = 0.0; - out_2496917717950831316[299] = 0.0; - out_2496917717950831316[300] = 0.0; - out_2496917717950831316[301] = 0.0; - out_2496917717950831316[302] = 0.0; - out_2496917717950831316[303] = 0.0; - out_2496917717950831316[304] = 1.0; - out_2496917717950831316[305] = 0.0; - out_2496917717950831316[306] = 0.0; - out_2496917717950831316[307] = 0.0; - out_2496917717950831316[308] = 0.0; - out_2496917717950831316[309] = 0.0; - out_2496917717950831316[310] = 0.0; - out_2496917717950831316[311] = 0.0; - out_2496917717950831316[312] = 0.0; - out_2496917717950831316[313] = 0.0; - out_2496917717950831316[314] = 0.0; - out_2496917717950831316[315] = 0.0; - out_2496917717950831316[316] = 0.0; - out_2496917717950831316[317] = 0.0; - out_2496917717950831316[318] = 0.0; - out_2496917717950831316[319] = 0.0; - out_2496917717950831316[320] = 0.0; - out_2496917717950831316[321] = 0.0; - out_2496917717950831316[322] = 0.0; - out_2496917717950831316[323] = 1.0; +void H_mod_fun(double *state, double *out_2787357462559363540) { + out_2787357462559363540[0] = 1.0; + out_2787357462559363540[1] = 0.0; + out_2787357462559363540[2] = 0.0; + out_2787357462559363540[3] = 0.0; + out_2787357462559363540[4] = 0.0; + out_2787357462559363540[5] = 0.0; + out_2787357462559363540[6] = 0.0; + out_2787357462559363540[7] = 0.0; + out_2787357462559363540[8] = 0.0; + out_2787357462559363540[9] = 0.0; + out_2787357462559363540[10] = 0.0; + out_2787357462559363540[11] = 0.0; + out_2787357462559363540[12] = 0.0; + out_2787357462559363540[13] = 0.0; + out_2787357462559363540[14] = 0.0; + out_2787357462559363540[15] = 0.0; + out_2787357462559363540[16] = 0.0; + out_2787357462559363540[17] = 0.0; + out_2787357462559363540[18] = 0.0; + out_2787357462559363540[19] = 1.0; + out_2787357462559363540[20] = 0.0; + out_2787357462559363540[21] = 0.0; + out_2787357462559363540[22] = 0.0; + out_2787357462559363540[23] = 0.0; + out_2787357462559363540[24] = 0.0; + out_2787357462559363540[25] = 0.0; + out_2787357462559363540[26] = 0.0; + out_2787357462559363540[27] = 0.0; + out_2787357462559363540[28] = 0.0; + out_2787357462559363540[29] = 0.0; + out_2787357462559363540[30] = 0.0; + out_2787357462559363540[31] = 0.0; + out_2787357462559363540[32] = 0.0; + out_2787357462559363540[33] = 0.0; + out_2787357462559363540[34] = 0.0; + out_2787357462559363540[35] = 0.0; + out_2787357462559363540[36] = 0.0; + out_2787357462559363540[37] = 0.0; + out_2787357462559363540[38] = 1.0; + out_2787357462559363540[39] = 0.0; + out_2787357462559363540[40] = 0.0; + out_2787357462559363540[41] = 0.0; + out_2787357462559363540[42] = 0.0; + out_2787357462559363540[43] = 0.0; + out_2787357462559363540[44] = 0.0; + out_2787357462559363540[45] = 0.0; + out_2787357462559363540[46] = 0.0; + out_2787357462559363540[47] = 0.0; + out_2787357462559363540[48] = 0.0; + out_2787357462559363540[49] = 0.0; + out_2787357462559363540[50] = 0.0; + out_2787357462559363540[51] = 0.0; + out_2787357462559363540[52] = 0.0; + out_2787357462559363540[53] = 0.0; + out_2787357462559363540[54] = 0.0; + out_2787357462559363540[55] = 0.0; + out_2787357462559363540[56] = 0.0; + out_2787357462559363540[57] = 1.0; + out_2787357462559363540[58] = 0.0; + out_2787357462559363540[59] = 0.0; + out_2787357462559363540[60] = 0.0; + out_2787357462559363540[61] = 0.0; + out_2787357462559363540[62] = 0.0; + out_2787357462559363540[63] = 0.0; + out_2787357462559363540[64] = 0.0; + out_2787357462559363540[65] = 0.0; + out_2787357462559363540[66] = 0.0; + out_2787357462559363540[67] = 0.0; + out_2787357462559363540[68] = 0.0; + out_2787357462559363540[69] = 0.0; + out_2787357462559363540[70] = 0.0; + out_2787357462559363540[71] = 0.0; + out_2787357462559363540[72] = 0.0; + out_2787357462559363540[73] = 0.0; + out_2787357462559363540[74] = 0.0; + out_2787357462559363540[75] = 0.0; + out_2787357462559363540[76] = 1.0; + out_2787357462559363540[77] = 0.0; + out_2787357462559363540[78] = 0.0; + out_2787357462559363540[79] = 0.0; + out_2787357462559363540[80] = 0.0; + out_2787357462559363540[81] = 0.0; + out_2787357462559363540[82] = 0.0; + out_2787357462559363540[83] = 0.0; + out_2787357462559363540[84] = 0.0; + out_2787357462559363540[85] = 0.0; + out_2787357462559363540[86] = 0.0; + out_2787357462559363540[87] = 0.0; + out_2787357462559363540[88] = 0.0; + out_2787357462559363540[89] = 0.0; + out_2787357462559363540[90] = 0.0; + out_2787357462559363540[91] = 0.0; + out_2787357462559363540[92] = 0.0; + out_2787357462559363540[93] = 0.0; + out_2787357462559363540[94] = 0.0; + out_2787357462559363540[95] = 1.0; + out_2787357462559363540[96] = 0.0; + out_2787357462559363540[97] = 0.0; + out_2787357462559363540[98] = 0.0; + out_2787357462559363540[99] = 0.0; + out_2787357462559363540[100] = 0.0; + out_2787357462559363540[101] = 0.0; + out_2787357462559363540[102] = 0.0; + out_2787357462559363540[103] = 0.0; + out_2787357462559363540[104] = 0.0; + out_2787357462559363540[105] = 0.0; + out_2787357462559363540[106] = 0.0; + out_2787357462559363540[107] = 0.0; + out_2787357462559363540[108] = 0.0; + out_2787357462559363540[109] = 0.0; + out_2787357462559363540[110] = 0.0; + out_2787357462559363540[111] = 0.0; + out_2787357462559363540[112] = 0.0; + out_2787357462559363540[113] = 0.0; + out_2787357462559363540[114] = 1.0; + out_2787357462559363540[115] = 0.0; + out_2787357462559363540[116] = 0.0; + out_2787357462559363540[117] = 0.0; + out_2787357462559363540[118] = 0.0; + out_2787357462559363540[119] = 0.0; + out_2787357462559363540[120] = 0.0; + out_2787357462559363540[121] = 0.0; + out_2787357462559363540[122] = 0.0; + out_2787357462559363540[123] = 0.0; + out_2787357462559363540[124] = 0.0; + out_2787357462559363540[125] = 0.0; + out_2787357462559363540[126] = 0.0; + out_2787357462559363540[127] = 0.0; + out_2787357462559363540[128] = 0.0; + out_2787357462559363540[129] = 0.0; + out_2787357462559363540[130] = 0.0; + out_2787357462559363540[131] = 0.0; + out_2787357462559363540[132] = 0.0; + out_2787357462559363540[133] = 1.0; + out_2787357462559363540[134] = 0.0; + out_2787357462559363540[135] = 0.0; + out_2787357462559363540[136] = 0.0; + out_2787357462559363540[137] = 0.0; + out_2787357462559363540[138] = 0.0; + out_2787357462559363540[139] = 0.0; + out_2787357462559363540[140] = 0.0; + out_2787357462559363540[141] = 0.0; + out_2787357462559363540[142] = 0.0; + out_2787357462559363540[143] = 0.0; + out_2787357462559363540[144] = 0.0; + out_2787357462559363540[145] = 0.0; + out_2787357462559363540[146] = 0.0; + out_2787357462559363540[147] = 0.0; + out_2787357462559363540[148] = 0.0; + out_2787357462559363540[149] = 0.0; + out_2787357462559363540[150] = 0.0; + out_2787357462559363540[151] = 0.0; + out_2787357462559363540[152] = 1.0; + out_2787357462559363540[153] = 0.0; + out_2787357462559363540[154] = 0.0; + out_2787357462559363540[155] = 0.0; + out_2787357462559363540[156] = 0.0; + out_2787357462559363540[157] = 0.0; + out_2787357462559363540[158] = 0.0; + out_2787357462559363540[159] = 0.0; + out_2787357462559363540[160] = 0.0; + out_2787357462559363540[161] = 0.0; + out_2787357462559363540[162] = 0.0; + out_2787357462559363540[163] = 0.0; + out_2787357462559363540[164] = 0.0; + out_2787357462559363540[165] = 0.0; + out_2787357462559363540[166] = 0.0; + out_2787357462559363540[167] = 0.0; + out_2787357462559363540[168] = 0.0; + out_2787357462559363540[169] = 0.0; + out_2787357462559363540[170] = 0.0; + out_2787357462559363540[171] = 1.0; + out_2787357462559363540[172] = 0.0; + out_2787357462559363540[173] = 0.0; + out_2787357462559363540[174] = 0.0; + out_2787357462559363540[175] = 0.0; + out_2787357462559363540[176] = 0.0; + out_2787357462559363540[177] = 0.0; + out_2787357462559363540[178] = 0.0; + out_2787357462559363540[179] = 0.0; + out_2787357462559363540[180] = 0.0; + out_2787357462559363540[181] = 0.0; + out_2787357462559363540[182] = 0.0; + out_2787357462559363540[183] = 0.0; + out_2787357462559363540[184] = 0.0; + out_2787357462559363540[185] = 0.0; + out_2787357462559363540[186] = 0.0; + out_2787357462559363540[187] = 0.0; + out_2787357462559363540[188] = 0.0; + out_2787357462559363540[189] = 0.0; + out_2787357462559363540[190] = 1.0; + out_2787357462559363540[191] = 0.0; + out_2787357462559363540[192] = 0.0; + out_2787357462559363540[193] = 0.0; + out_2787357462559363540[194] = 0.0; + out_2787357462559363540[195] = 0.0; + out_2787357462559363540[196] = 0.0; + out_2787357462559363540[197] = 0.0; + out_2787357462559363540[198] = 0.0; + out_2787357462559363540[199] = 0.0; + out_2787357462559363540[200] = 0.0; + out_2787357462559363540[201] = 0.0; + out_2787357462559363540[202] = 0.0; + out_2787357462559363540[203] = 0.0; + out_2787357462559363540[204] = 0.0; + out_2787357462559363540[205] = 0.0; + out_2787357462559363540[206] = 0.0; + out_2787357462559363540[207] = 0.0; + out_2787357462559363540[208] = 0.0; + out_2787357462559363540[209] = 1.0; + out_2787357462559363540[210] = 0.0; + out_2787357462559363540[211] = 0.0; + out_2787357462559363540[212] = 0.0; + out_2787357462559363540[213] = 0.0; + out_2787357462559363540[214] = 0.0; + out_2787357462559363540[215] = 0.0; + out_2787357462559363540[216] = 0.0; + out_2787357462559363540[217] = 0.0; + out_2787357462559363540[218] = 0.0; + out_2787357462559363540[219] = 0.0; + out_2787357462559363540[220] = 0.0; + out_2787357462559363540[221] = 0.0; + out_2787357462559363540[222] = 0.0; + out_2787357462559363540[223] = 0.0; + out_2787357462559363540[224] = 0.0; + out_2787357462559363540[225] = 0.0; + out_2787357462559363540[226] = 0.0; + out_2787357462559363540[227] = 0.0; + out_2787357462559363540[228] = 1.0; + out_2787357462559363540[229] = 0.0; + out_2787357462559363540[230] = 0.0; + out_2787357462559363540[231] = 0.0; + out_2787357462559363540[232] = 0.0; + out_2787357462559363540[233] = 0.0; + out_2787357462559363540[234] = 0.0; + out_2787357462559363540[235] = 0.0; + out_2787357462559363540[236] = 0.0; + out_2787357462559363540[237] = 0.0; + out_2787357462559363540[238] = 0.0; + out_2787357462559363540[239] = 0.0; + out_2787357462559363540[240] = 0.0; + out_2787357462559363540[241] = 0.0; + out_2787357462559363540[242] = 0.0; + out_2787357462559363540[243] = 0.0; + out_2787357462559363540[244] = 0.0; + out_2787357462559363540[245] = 0.0; + out_2787357462559363540[246] = 0.0; + out_2787357462559363540[247] = 1.0; + out_2787357462559363540[248] = 0.0; + out_2787357462559363540[249] = 0.0; + out_2787357462559363540[250] = 0.0; + out_2787357462559363540[251] = 0.0; + out_2787357462559363540[252] = 0.0; + out_2787357462559363540[253] = 0.0; + out_2787357462559363540[254] = 0.0; + out_2787357462559363540[255] = 0.0; + out_2787357462559363540[256] = 0.0; + out_2787357462559363540[257] = 0.0; + out_2787357462559363540[258] = 0.0; + out_2787357462559363540[259] = 0.0; + out_2787357462559363540[260] = 0.0; + out_2787357462559363540[261] = 0.0; + out_2787357462559363540[262] = 0.0; + out_2787357462559363540[263] = 0.0; + out_2787357462559363540[264] = 0.0; + out_2787357462559363540[265] = 0.0; + out_2787357462559363540[266] = 1.0; + out_2787357462559363540[267] = 0.0; + out_2787357462559363540[268] = 0.0; + out_2787357462559363540[269] = 0.0; + out_2787357462559363540[270] = 0.0; + out_2787357462559363540[271] = 0.0; + out_2787357462559363540[272] = 0.0; + out_2787357462559363540[273] = 0.0; + out_2787357462559363540[274] = 0.0; + out_2787357462559363540[275] = 0.0; + out_2787357462559363540[276] = 0.0; + out_2787357462559363540[277] = 0.0; + out_2787357462559363540[278] = 0.0; + out_2787357462559363540[279] = 0.0; + out_2787357462559363540[280] = 0.0; + out_2787357462559363540[281] = 0.0; + out_2787357462559363540[282] = 0.0; + out_2787357462559363540[283] = 0.0; + out_2787357462559363540[284] = 0.0; + out_2787357462559363540[285] = 1.0; + out_2787357462559363540[286] = 0.0; + out_2787357462559363540[287] = 0.0; + out_2787357462559363540[288] = 0.0; + out_2787357462559363540[289] = 0.0; + out_2787357462559363540[290] = 0.0; + out_2787357462559363540[291] = 0.0; + out_2787357462559363540[292] = 0.0; + out_2787357462559363540[293] = 0.0; + out_2787357462559363540[294] = 0.0; + out_2787357462559363540[295] = 0.0; + out_2787357462559363540[296] = 0.0; + out_2787357462559363540[297] = 0.0; + out_2787357462559363540[298] = 0.0; + out_2787357462559363540[299] = 0.0; + out_2787357462559363540[300] = 0.0; + out_2787357462559363540[301] = 0.0; + out_2787357462559363540[302] = 0.0; + out_2787357462559363540[303] = 0.0; + out_2787357462559363540[304] = 1.0; + out_2787357462559363540[305] = 0.0; + out_2787357462559363540[306] = 0.0; + out_2787357462559363540[307] = 0.0; + out_2787357462559363540[308] = 0.0; + out_2787357462559363540[309] = 0.0; + out_2787357462559363540[310] = 0.0; + out_2787357462559363540[311] = 0.0; + out_2787357462559363540[312] = 0.0; + out_2787357462559363540[313] = 0.0; + out_2787357462559363540[314] = 0.0; + out_2787357462559363540[315] = 0.0; + out_2787357462559363540[316] = 0.0; + out_2787357462559363540[317] = 0.0; + out_2787357462559363540[318] = 0.0; + out_2787357462559363540[319] = 0.0; + out_2787357462559363540[320] = 0.0; + out_2787357462559363540[321] = 0.0; + out_2787357462559363540[322] = 0.0; + out_2787357462559363540[323] = 1.0; } -void f_fun(double *state, double dt, double *out_6905768897951283532) { - out_6905768897951283532[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_6905768897951283532[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_6905768897951283532[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_6905768897951283532[3] = dt*state[12] + state[3]; - out_6905768897951283532[4] = dt*state[13] + state[4]; - out_6905768897951283532[5] = dt*state[14] + state[5]; - out_6905768897951283532[6] = state[6]; - out_6905768897951283532[7] = state[7]; - out_6905768897951283532[8] = state[8]; - out_6905768897951283532[9] = state[9]; - out_6905768897951283532[10] = state[10]; - out_6905768897951283532[11] = state[11]; - out_6905768897951283532[12] = state[12]; - out_6905768897951283532[13] = state[13]; - out_6905768897951283532[14] = state[14]; - out_6905768897951283532[15] = state[15]; - out_6905768897951283532[16] = state[16]; - out_6905768897951283532[17] = state[17]; +void f_fun(double *state, double dt, double *out_2997254779517652533) { + out_2997254779517652533[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_2997254779517652533[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_2997254779517652533[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_2997254779517652533[3] = dt*state[12] + state[3]; + out_2997254779517652533[4] = dt*state[13] + state[4]; + out_2997254779517652533[5] = dt*state[14] + state[5]; + out_2997254779517652533[6] = state[6]; + out_2997254779517652533[7] = state[7]; + out_2997254779517652533[8] = state[8]; + out_2997254779517652533[9] = state[9]; + out_2997254779517652533[10] = state[10]; + out_2997254779517652533[11] = state[11]; + out_2997254779517652533[12] = state[12]; + out_2997254779517652533[13] = state[13]; + out_2997254779517652533[14] = state[14]; + out_2997254779517652533[15] = state[15]; + out_2997254779517652533[16] = state[16]; + out_2997254779517652533[17] = state[17]; } -void F_fun(double *state, double dt, double *out_5307423376078500036) { - out_5307423376078500036[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_5307423376078500036[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_5307423376078500036[2] = 0; - out_5307423376078500036[3] = 0; - out_5307423376078500036[4] = 0; - out_5307423376078500036[5] = 0; - out_5307423376078500036[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_5307423376078500036[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_5307423376078500036[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_5307423376078500036[9] = 0; - out_5307423376078500036[10] = 0; - out_5307423376078500036[11] = 0; - out_5307423376078500036[12] = 0; - out_5307423376078500036[13] = 0; - out_5307423376078500036[14] = 0; - out_5307423376078500036[15] = 0; - out_5307423376078500036[16] = 0; - out_5307423376078500036[17] = 0; - out_5307423376078500036[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_5307423376078500036[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_5307423376078500036[20] = 0; - out_5307423376078500036[21] = 0; - out_5307423376078500036[22] = 0; - out_5307423376078500036[23] = 0; - out_5307423376078500036[24] = 0; - out_5307423376078500036[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_5307423376078500036[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_5307423376078500036[27] = 0; - out_5307423376078500036[28] = 0; - out_5307423376078500036[29] = 0; - out_5307423376078500036[30] = 0; - out_5307423376078500036[31] = 0; - out_5307423376078500036[32] = 0; - out_5307423376078500036[33] = 0; - out_5307423376078500036[34] = 0; - out_5307423376078500036[35] = 0; - out_5307423376078500036[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_5307423376078500036[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_5307423376078500036[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_5307423376078500036[39] = 0; - out_5307423376078500036[40] = 0; - out_5307423376078500036[41] = 0; - out_5307423376078500036[42] = 0; - out_5307423376078500036[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_5307423376078500036[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_5307423376078500036[45] = 0; - out_5307423376078500036[46] = 0; - out_5307423376078500036[47] = 0; - out_5307423376078500036[48] = 0; - out_5307423376078500036[49] = 0; - out_5307423376078500036[50] = 0; - out_5307423376078500036[51] = 0; - out_5307423376078500036[52] = 0; - out_5307423376078500036[53] = 0; - out_5307423376078500036[54] = 0; - out_5307423376078500036[55] = 0; - out_5307423376078500036[56] = 0; - out_5307423376078500036[57] = 1; - out_5307423376078500036[58] = 0; - out_5307423376078500036[59] = 0; - out_5307423376078500036[60] = 0; - out_5307423376078500036[61] = 0; - out_5307423376078500036[62] = 0; - out_5307423376078500036[63] = 0; - out_5307423376078500036[64] = 0; - out_5307423376078500036[65] = 0; - out_5307423376078500036[66] = dt; - out_5307423376078500036[67] = 0; - out_5307423376078500036[68] = 0; - out_5307423376078500036[69] = 0; - out_5307423376078500036[70] = 0; - out_5307423376078500036[71] = 0; - out_5307423376078500036[72] = 0; - out_5307423376078500036[73] = 0; - out_5307423376078500036[74] = 0; - out_5307423376078500036[75] = 0; - out_5307423376078500036[76] = 1; - out_5307423376078500036[77] = 0; - out_5307423376078500036[78] = 0; - out_5307423376078500036[79] = 0; - out_5307423376078500036[80] = 0; - out_5307423376078500036[81] = 0; - out_5307423376078500036[82] = 0; - out_5307423376078500036[83] = 0; - out_5307423376078500036[84] = 0; - out_5307423376078500036[85] = dt; - out_5307423376078500036[86] = 0; - out_5307423376078500036[87] = 0; - out_5307423376078500036[88] = 0; - out_5307423376078500036[89] = 0; - out_5307423376078500036[90] = 0; - out_5307423376078500036[91] = 0; - out_5307423376078500036[92] = 0; - out_5307423376078500036[93] = 0; - out_5307423376078500036[94] = 0; - out_5307423376078500036[95] = 1; - out_5307423376078500036[96] = 0; - out_5307423376078500036[97] = 0; - out_5307423376078500036[98] = 0; - out_5307423376078500036[99] = 0; - out_5307423376078500036[100] = 0; - out_5307423376078500036[101] = 0; - out_5307423376078500036[102] = 0; - out_5307423376078500036[103] = 0; - out_5307423376078500036[104] = dt; - out_5307423376078500036[105] = 0; - out_5307423376078500036[106] = 0; - out_5307423376078500036[107] = 0; - out_5307423376078500036[108] = 0; - out_5307423376078500036[109] = 0; - out_5307423376078500036[110] = 0; - out_5307423376078500036[111] = 0; - out_5307423376078500036[112] = 0; - out_5307423376078500036[113] = 0; - out_5307423376078500036[114] = 1; - out_5307423376078500036[115] = 0; - out_5307423376078500036[116] = 0; - out_5307423376078500036[117] = 0; - out_5307423376078500036[118] = 0; - out_5307423376078500036[119] = 0; - out_5307423376078500036[120] = 0; - out_5307423376078500036[121] = 0; - out_5307423376078500036[122] = 0; - out_5307423376078500036[123] = 0; - out_5307423376078500036[124] = 0; - out_5307423376078500036[125] = 0; - out_5307423376078500036[126] = 0; - out_5307423376078500036[127] = 0; - out_5307423376078500036[128] = 0; - out_5307423376078500036[129] = 0; - out_5307423376078500036[130] = 0; - out_5307423376078500036[131] = 0; - out_5307423376078500036[132] = 0; - out_5307423376078500036[133] = 1; - out_5307423376078500036[134] = 0; - out_5307423376078500036[135] = 0; - out_5307423376078500036[136] = 0; - out_5307423376078500036[137] = 0; - out_5307423376078500036[138] = 0; - out_5307423376078500036[139] = 0; - out_5307423376078500036[140] = 0; - out_5307423376078500036[141] = 0; - out_5307423376078500036[142] = 0; - out_5307423376078500036[143] = 0; - out_5307423376078500036[144] = 0; - out_5307423376078500036[145] = 0; - out_5307423376078500036[146] = 0; - out_5307423376078500036[147] = 0; - out_5307423376078500036[148] = 0; - out_5307423376078500036[149] = 0; - out_5307423376078500036[150] = 0; - out_5307423376078500036[151] = 0; - out_5307423376078500036[152] = 1; - out_5307423376078500036[153] = 0; - out_5307423376078500036[154] = 0; - out_5307423376078500036[155] = 0; - out_5307423376078500036[156] = 0; - out_5307423376078500036[157] = 0; - out_5307423376078500036[158] = 0; - out_5307423376078500036[159] = 0; - out_5307423376078500036[160] = 0; - out_5307423376078500036[161] = 0; - out_5307423376078500036[162] = 0; - out_5307423376078500036[163] = 0; - out_5307423376078500036[164] = 0; - out_5307423376078500036[165] = 0; - out_5307423376078500036[166] = 0; - out_5307423376078500036[167] = 0; - out_5307423376078500036[168] = 0; - out_5307423376078500036[169] = 0; - out_5307423376078500036[170] = 0; - out_5307423376078500036[171] = 1; - out_5307423376078500036[172] = 0; - out_5307423376078500036[173] = 0; - out_5307423376078500036[174] = 0; - out_5307423376078500036[175] = 0; - out_5307423376078500036[176] = 0; - out_5307423376078500036[177] = 0; - out_5307423376078500036[178] = 0; - out_5307423376078500036[179] = 0; - out_5307423376078500036[180] = 0; - out_5307423376078500036[181] = 0; - out_5307423376078500036[182] = 0; - out_5307423376078500036[183] = 0; - out_5307423376078500036[184] = 0; - out_5307423376078500036[185] = 0; - out_5307423376078500036[186] = 0; - out_5307423376078500036[187] = 0; - out_5307423376078500036[188] = 0; - out_5307423376078500036[189] = 0; - out_5307423376078500036[190] = 1; - out_5307423376078500036[191] = 0; - out_5307423376078500036[192] = 0; - out_5307423376078500036[193] = 0; - out_5307423376078500036[194] = 0; - out_5307423376078500036[195] = 0; - out_5307423376078500036[196] = 0; - out_5307423376078500036[197] = 0; - out_5307423376078500036[198] = 0; - out_5307423376078500036[199] = 0; - out_5307423376078500036[200] = 0; - out_5307423376078500036[201] = 0; - out_5307423376078500036[202] = 0; - out_5307423376078500036[203] = 0; - out_5307423376078500036[204] = 0; - out_5307423376078500036[205] = 0; - out_5307423376078500036[206] = 0; - out_5307423376078500036[207] = 0; - out_5307423376078500036[208] = 0; - out_5307423376078500036[209] = 1; - out_5307423376078500036[210] = 0; - out_5307423376078500036[211] = 0; - out_5307423376078500036[212] = 0; - out_5307423376078500036[213] = 0; - out_5307423376078500036[214] = 0; - out_5307423376078500036[215] = 0; - out_5307423376078500036[216] = 0; - out_5307423376078500036[217] = 0; - out_5307423376078500036[218] = 0; - out_5307423376078500036[219] = 0; - out_5307423376078500036[220] = 0; - out_5307423376078500036[221] = 0; - out_5307423376078500036[222] = 0; - out_5307423376078500036[223] = 0; - out_5307423376078500036[224] = 0; - out_5307423376078500036[225] = 0; - out_5307423376078500036[226] = 0; - out_5307423376078500036[227] = 0; - out_5307423376078500036[228] = 1; - out_5307423376078500036[229] = 0; - out_5307423376078500036[230] = 0; - out_5307423376078500036[231] = 0; - out_5307423376078500036[232] = 0; - out_5307423376078500036[233] = 0; - out_5307423376078500036[234] = 0; - out_5307423376078500036[235] = 0; - out_5307423376078500036[236] = 0; - out_5307423376078500036[237] = 0; - out_5307423376078500036[238] = 0; - out_5307423376078500036[239] = 0; - out_5307423376078500036[240] = 0; - out_5307423376078500036[241] = 0; - out_5307423376078500036[242] = 0; - out_5307423376078500036[243] = 0; - out_5307423376078500036[244] = 0; - out_5307423376078500036[245] = 0; - out_5307423376078500036[246] = 0; - out_5307423376078500036[247] = 1; - out_5307423376078500036[248] = 0; - out_5307423376078500036[249] = 0; - out_5307423376078500036[250] = 0; - out_5307423376078500036[251] = 0; - out_5307423376078500036[252] = 0; - out_5307423376078500036[253] = 0; - out_5307423376078500036[254] = 0; - out_5307423376078500036[255] = 0; - out_5307423376078500036[256] = 0; - out_5307423376078500036[257] = 0; - out_5307423376078500036[258] = 0; - out_5307423376078500036[259] = 0; - out_5307423376078500036[260] = 0; - out_5307423376078500036[261] = 0; - out_5307423376078500036[262] = 0; - out_5307423376078500036[263] = 0; - out_5307423376078500036[264] = 0; - out_5307423376078500036[265] = 0; - out_5307423376078500036[266] = 1; - out_5307423376078500036[267] = 0; - out_5307423376078500036[268] = 0; - out_5307423376078500036[269] = 0; - out_5307423376078500036[270] = 0; - out_5307423376078500036[271] = 0; - out_5307423376078500036[272] = 0; - out_5307423376078500036[273] = 0; - out_5307423376078500036[274] = 0; - out_5307423376078500036[275] = 0; - out_5307423376078500036[276] = 0; - out_5307423376078500036[277] = 0; - out_5307423376078500036[278] = 0; - out_5307423376078500036[279] = 0; - out_5307423376078500036[280] = 0; - out_5307423376078500036[281] = 0; - out_5307423376078500036[282] = 0; - out_5307423376078500036[283] = 0; - out_5307423376078500036[284] = 0; - out_5307423376078500036[285] = 1; - out_5307423376078500036[286] = 0; - out_5307423376078500036[287] = 0; - out_5307423376078500036[288] = 0; - out_5307423376078500036[289] = 0; - out_5307423376078500036[290] = 0; - out_5307423376078500036[291] = 0; - out_5307423376078500036[292] = 0; - out_5307423376078500036[293] = 0; - out_5307423376078500036[294] = 0; - out_5307423376078500036[295] = 0; - out_5307423376078500036[296] = 0; - out_5307423376078500036[297] = 0; - out_5307423376078500036[298] = 0; - out_5307423376078500036[299] = 0; - out_5307423376078500036[300] = 0; - out_5307423376078500036[301] = 0; - out_5307423376078500036[302] = 0; - out_5307423376078500036[303] = 0; - out_5307423376078500036[304] = 1; - out_5307423376078500036[305] = 0; - out_5307423376078500036[306] = 0; - out_5307423376078500036[307] = 0; - out_5307423376078500036[308] = 0; - out_5307423376078500036[309] = 0; - out_5307423376078500036[310] = 0; - out_5307423376078500036[311] = 0; - out_5307423376078500036[312] = 0; - out_5307423376078500036[313] = 0; - out_5307423376078500036[314] = 0; - out_5307423376078500036[315] = 0; - out_5307423376078500036[316] = 0; - out_5307423376078500036[317] = 0; - out_5307423376078500036[318] = 0; - out_5307423376078500036[319] = 0; - out_5307423376078500036[320] = 0; - out_5307423376078500036[321] = 0; - out_5307423376078500036[322] = 0; - out_5307423376078500036[323] = 1; +void F_fun(double *state, double dt, double *out_3476500674504442735) { + out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[2] = 0; + out_3476500674504442735[3] = 0; + out_3476500674504442735[4] = 0; + out_3476500674504442735[5] = 0; + out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[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_3476500674504442735[9] = 0; + out_3476500674504442735[10] = 0; + out_3476500674504442735[11] = 0; + out_3476500674504442735[12] = 0; + out_3476500674504442735[13] = 0; + out_3476500674504442735[14] = 0; + out_3476500674504442735[15] = 0; + out_3476500674504442735[16] = 0; + out_3476500674504442735[17] = 0; + out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[20] = 0; + out_3476500674504442735[21] = 0; + out_3476500674504442735[22] = 0; + out_3476500674504442735[23] = 0; + out_3476500674504442735[24] = 0; + out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[27] = 0; + out_3476500674504442735[28] = 0; + out_3476500674504442735[29] = 0; + out_3476500674504442735[30] = 0; + out_3476500674504442735[31] = 0; + out_3476500674504442735[32] = 0; + out_3476500674504442735[33] = 0; + out_3476500674504442735[34] = 0; + out_3476500674504442735[35] = 0; + out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[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_3476500674504442735[39] = 0; + out_3476500674504442735[40] = 0; + out_3476500674504442735[41] = 0; + out_3476500674504442735[42] = 0; + out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[45] = 0; + out_3476500674504442735[46] = 0; + out_3476500674504442735[47] = 0; + out_3476500674504442735[48] = 0; + out_3476500674504442735[49] = 0; + out_3476500674504442735[50] = 0; + out_3476500674504442735[51] = 0; + out_3476500674504442735[52] = 0; + out_3476500674504442735[53] = 0; + out_3476500674504442735[54] = 0; + out_3476500674504442735[55] = 0; + out_3476500674504442735[56] = 0; + out_3476500674504442735[57] = 1; + out_3476500674504442735[58] = 0; + out_3476500674504442735[59] = 0; + out_3476500674504442735[60] = 0; + out_3476500674504442735[61] = 0; + out_3476500674504442735[62] = 0; + out_3476500674504442735[63] = 0; + out_3476500674504442735[64] = 0; + out_3476500674504442735[65] = 0; + out_3476500674504442735[66] = dt; + out_3476500674504442735[67] = 0; + out_3476500674504442735[68] = 0; + out_3476500674504442735[69] = 0; + out_3476500674504442735[70] = 0; + out_3476500674504442735[71] = 0; + out_3476500674504442735[72] = 0; + out_3476500674504442735[73] = 0; + out_3476500674504442735[74] = 0; + out_3476500674504442735[75] = 0; + out_3476500674504442735[76] = 1; + out_3476500674504442735[77] = 0; + out_3476500674504442735[78] = 0; + out_3476500674504442735[79] = 0; + out_3476500674504442735[80] = 0; + out_3476500674504442735[81] = 0; + out_3476500674504442735[82] = 0; + out_3476500674504442735[83] = 0; + out_3476500674504442735[84] = 0; + out_3476500674504442735[85] = dt; + out_3476500674504442735[86] = 0; + out_3476500674504442735[87] = 0; + out_3476500674504442735[88] = 0; + out_3476500674504442735[89] = 0; + out_3476500674504442735[90] = 0; + out_3476500674504442735[91] = 0; + out_3476500674504442735[92] = 0; + out_3476500674504442735[93] = 0; + out_3476500674504442735[94] = 0; + out_3476500674504442735[95] = 1; + out_3476500674504442735[96] = 0; + out_3476500674504442735[97] = 0; + out_3476500674504442735[98] = 0; + out_3476500674504442735[99] = 0; + out_3476500674504442735[100] = 0; + out_3476500674504442735[101] = 0; + out_3476500674504442735[102] = 0; + out_3476500674504442735[103] = 0; + out_3476500674504442735[104] = dt; + out_3476500674504442735[105] = 0; + out_3476500674504442735[106] = 0; + out_3476500674504442735[107] = 0; + out_3476500674504442735[108] = 0; + out_3476500674504442735[109] = 0; + out_3476500674504442735[110] = 0; + out_3476500674504442735[111] = 0; + out_3476500674504442735[112] = 0; + out_3476500674504442735[113] = 0; + out_3476500674504442735[114] = 1; + out_3476500674504442735[115] = 0; + out_3476500674504442735[116] = 0; + out_3476500674504442735[117] = 0; + out_3476500674504442735[118] = 0; + out_3476500674504442735[119] = 0; + out_3476500674504442735[120] = 0; + out_3476500674504442735[121] = 0; + out_3476500674504442735[122] = 0; + out_3476500674504442735[123] = 0; + out_3476500674504442735[124] = 0; + out_3476500674504442735[125] = 0; + out_3476500674504442735[126] = 0; + out_3476500674504442735[127] = 0; + out_3476500674504442735[128] = 0; + out_3476500674504442735[129] = 0; + out_3476500674504442735[130] = 0; + out_3476500674504442735[131] = 0; + out_3476500674504442735[132] = 0; + out_3476500674504442735[133] = 1; + out_3476500674504442735[134] = 0; + out_3476500674504442735[135] = 0; + out_3476500674504442735[136] = 0; + out_3476500674504442735[137] = 0; + out_3476500674504442735[138] = 0; + out_3476500674504442735[139] = 0; + out_3476500674504442735[140] = 0; + out_3476500674504442735[141] = 0; + out_3476500674504442735[142] = 0; + out_3476500674504442735[143] = 0; + out_3476500674504442735[144] = 0; + out_3476500674504442735[145] = 0; + out_3476500674504442735[146] = 0; + out_3476500674504442735[147] = 0; + out_3476500674504442735[148] = 0; + out_3476500674504442735[149] = 0; + out_3476500674504442735[150] = 0; + out_3476500674504442735[151] = 0; + out_3476500674504442735[152] = 1; + out_3476500674504442735[153] = 0; + out_3476500674504442735[154] = 0; + out_3476500674504442735[155] = 0; + out_3476500674504442735[156] = 0; + out_3476500674504442735[157] = 0; + out_3476500674504442735[158] = 0; + out_3476500674504442735[159] = 0; + out_3476500674504442735[160] = 0; + out_3476500674504442735[161] = 0; + out_3476500674504442735[162] = 0; + out_3476500674504442735[163] = 0; + out_3476500674504442735[164] = 0; + out_3476500674504442735[165] = 0; + out_3476500674504442735[166] = 0; + out_3476500674504442735[167] = 0; + out_3476500674504442735[168] = 0; + out_3476500674504442735[169] = 0; + out_3476500674504442735[170] = 0; + out_3476500674504442735[171] = 1; + out_3476500674504442735[172] = 0; + out_3476500674504442735[173] = 0; + out_3476500674504442735[174] = 0; + out_3476500674504442735[175] = 0; + out_3476500674504442735[176] = 0; + out_3476500674504442735[177] = 0; + out_3476500674504442735[178] = 0; + out_3476500674504442735[179] = 0; + out_3476500674504442735[180] = 0; + out_3476500674504442735[181] = 0; + out_3476500674504442735[182] = 0; + out_3476500674504442735[183] = 0; + out_3476500674504442735[184] = 0; + out_3476500674504442735[185] = 0; + out_3476500674504442735[186] = 0; + out_3476500674504442735[187] = 0; + out_3476500674504442735[188] = 0; + out_3476500674504442735[189] = 0; + out_3476500674504442735[190] = 1; + out_3476500674504442735[191] = 0; + out_3476500674504442735[192] = 0; + out_3476500674504442735[193] = 0; + out_3476500674504442735[194] = 0; + out_3476500674504442735[195] = 0; + out_3476500674504442735[196] = 0; + out_3476500674504442735[197] = 0; + out_3476500674504442735[198] = 0; + out_3476500674504442735[199] = 0; + out_3476500674504442735[200] = 0; + out_3476500674504442735[201] = 0; + out_3476500674504442735[202] = 0; + out_3476500674504442735[203] = 0; + out_3476500674504442735[204] = 0; + out_3476500674504442735[205] = 0; + out_3476500674504442735[206] = 0; + out_3476500674504442735[207] = 0; + out_3476500674504442735[208] = 0; + out_3476500674504442735[209] = 1; + out_3476500674504442735[210] = 0; + out_3476500674504442735[211] = 0; + out_3476500674504442735[212] = 0; + out_3476500674504442735[213] = 0; + out_3476500674504442735[214] = 0; + out_3476500674504442735[215] = 0; + out_3476500674504442735[216] = 0; + out_3476500674504442735[217] = 0; + out_3476500674504442735[218] = 0; + out_3476500674504442735[219] = 0; + out_3476500674504442735[220] = 0; + out_3476500674504442735[221] = 0; + out_3476500674504442735[222] = 0; + out_3476500674504442735[223] = 0; + out_3476500674504442735[224] = 0; + out_3476500674504442735[225] = 0; + out_3476500674504442735[226] = 0; + out_3476500674504442735[227] = 0; + out_3476500674504442735[228] = 1; + out_3476500674504442735[229] = 0; + out_3476500674504442735[230] = 0; + out_3476500674504442735[231] = 0; + out_3476500674504442735[232] = 0; + out_3476500674504442735[233] = 0; + out_3476500674504442735[234] = 0; + out_3476500674504442735[235] = 0; + out_3476500674504442735[236] = 0; + out_3476500674504442735[237] = 0; + out_3476500674504442735[238] = 0; + out_3476500674504442735[239] = 0; + out_3476500674504442735[240] = 0; + out_3476500674504442735[241] = 0; + out_3476500674504442735[242] = 0; + out_3476500674504442735[243] = 0; + out_3476500674504442735[244] = 0; + out_3476500674504442735[245] = 0; + out_3476500674504442735[246] = 0; + out_3476500674504442735[247] = 1; + out_3476500674504442735[248] = 0; + out_3476500674504442735[249] = 0; + out_3476500674504442735[250] = 0; + out_3476500674504442735[251] = 0; + out_3476500674504442735[252] = 0; + out_3476500674504442735[253] = 0; + out_3476500674504442735[254] = 0; + out_3476500674504442735[255] = 0; + out_3476500674504442735[256] = 0; + out_3476500674504442735[257] = 0; + out_3476500674504442735[258] = 0; + out_3476500674504442735[259] = 0; + out_3476500674504442735[260] = 0; + out_3476500674504442735[261] = 0; + out_3476500674504442735[262] = 0; + out_3476500674504442735[263] = 0; + out_3476500674504442735[264] = 0; + out_3476500674504442735[265] = 0; + out_3476500674504442735[266] = 1; + out_3476500674504442735[267] = 0; + out_3476500674504442735[268] = 0; + out_3476500674504442735[269] = 0; + out_3476500674504442735[270] = 0; + out_3476500674504442735[271] = 0; + out_3476500674504442735[272] = 0; + out_3476500674504442735[273] = 0; + out_3476500674504442735[274] = 0; + out_3476500674504442735[275] = 0; + out_3476500674504442735[276] = 0; + out_3476500674504442735[277] = 0; + out_3476500674504442735[278] = 0; + out_3476500674504442735[279] = 0; + out_3476500674504442735[280] = 0; + out_3476500674504442735[281] = 0; + out_3476500674504442735[282] = 0; + out_3476500674504442735[283] = 0; + out_3476500674504442735[284] = 0; + out_3476500674504442735[285] = 1; + out_3476500674504442735[286] = 0; + out_3476500674504442735[287] = 0; + out_3476500674504442735[288] = 0; + out_3476500674504442735[289] = 0; + out_3476500674504442735[290] = 0; + out_3476500674504442735[291] = 0; + out_3476500674504442735[292] = 0; + out_3476500674504442735[293] = 0; + out_3476500674504442735[294] = 0; + out_3476500674504442735[295] = 0; + out_3476500674504442735[296] = 0; + out_3476500674504442735[297] = 0; + out_3476500674504442735[298] = 0; + out_3476500674504442735[299] = 0; + out_3476500674504442735[300] = 0; + out_3476500674504442735[301] = 0; + out_3476500674504442735[302] = 0; + out_3476500674504442735[303] = 0; + out_3476500674504442735[304] = 1; + out_3476500674504442735[305] = 0; + out_3476500674504442735[306] = 0; + out_3476500674504442735[307] = 0; + out_3476500674504442735[308] = 0; + out_3476500674504442735[309] = 0; + out_3476500674504442735[310] = 0; + out_3476500674504442735[311] = 0; + out_3476500674504442735[312] = 0; + out_3476500674504442735[313] = 0; + out_3476500674504442735[314] = 0; + out_3476500674504442735[315] = 0; + out_3476500674504442735[316] = 0; + out_3476500674504442735[317] = 0; + out_3476500674504442735[318] = 0; + out_3476500674504442735[319] = 0; + out_3476500674504442735[320] = 0; + out_3476500674504442735[321] = 0; + out_3476500674504442735[322] = 0; + out_3476500674504442735[323] = 1; } -void h_4(double *state, double *unused, double *out_1079193932347421775) { - out_1079193932347421775[0] = state[6] + state[9]; - out_1079193932347421775[1] = state[7] + state[10]; - out_1079193932347421775[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_2259436079279725972) { + out_2259436079279725972[0] = state[6] + state[9]; + out_2259436079279725972[1] = state[7] + state[10]; + out_2259436079279725972[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_1742756715896118709) { - out_1742756715896118709[0] = 0; - out_1742756715896118709[1] = 0; - out_1742756715896118709[2] = 0; - out_1742756715896118709[3] = 0; - out_1742756715896118709[4] = 0; - out_1742756715896118709[5] = 0; - out_1742756715896118709[6] = 1; - out_1742756715896118709[7] = 0; - out_1742756715896118709[8] = 0; - out_1742756715896118709[9] = 1; - out_1742756715896118709[10] = 0; - out_1742756715896118709[11] = 0; - out_1742756715896118709[12] = 0; - out_1742756715896118709[13] = 0; - out_1742756715896118709[14] = 0; - out_1742756715896118709[15] = 0; - out_1742756715896118709[16] = 0; - out_1742756715896118709[17] = 0; - out_1742756715896118709[18] = 0; - out_1742756715896118709[19] = 0; - out_1742756715896118709[20] = 0; - out_1742756715896118709[21] = 0; - out_1742756715896118709[22] = 0; - out_1742756715896118709[23] = 0; - out_1742756715896118709[24] = 0; - out_1742756715896118709[25] = 1; - out_1742756715896118709[26] = 0; - out_1742756715896118709[27] = 0; - out_1742756715896118709[28] = 1; - out_1742756715896118709[29] = 0; - out_1742756715896118709[30] = 0; - out_1742756715896118709[31] = 0; - out_1742756715896118709[32] = 0; - out_1742756715896118709[33] = 0; - out_1742756715896118709[34] = 0; - out_1742756715896118709[35] = 0; - out_1742756715896118709[36] = 0; - out_1742756715896118709[37] = 0; - out_1742756715896118709[38] = 0; - out_1742756715896118709[39] = 0; - out_1742756715896118709[40] = 0; - out_1742756715896118709[41] = 0; - out_1742756715896118709[42] = 0; - out_1742756715896118709[43] = 0; - out_1742756715896118709[44] = 1; - out_1742756715896118709[45] = 0; - out_1742756715896118709[46] = 0; - out_1742756715896118709[47] = 1; - out_1742756715896118709[48] = 0; - out_1742756715896118709[49] = 0; - out_1742756715896118709[50] = 0; - out_1742756715896118709[51] = 0; - out_1742756715896118709[52] = 0; - out_1742756715896118709[53] = 0; +void H_4(double *state, double *unused, double *out_6715024945092366734) { + out_6715024945092366734[0] = 0; + out_6715024945092366734[1] = 0; + out_6715024945092366734[2] = 0; + out_6715024945092366734[3] = 0; + out_6715024945092366734[4] = 0; + out_6715024945092366734[5] = 0; + out_6715024945092366734[6] = 1; + out_6715024945092366734[7] = 0; + out_6715024945092366734[8] = 0; + out_6715024945092366734[9] = 1; + out_6715024945092366734[10] = 0; + out_6715024945092366734[11] = 0; + out_6715024945092366734[12] = 0; + out_6715024945092366734[13] = 0; + out_6715024945092366734[14] = 0; + out_6715024945092366734[15] = 0; + out_6715024945092366734[16] = 0; + out_6715024945092366734[17] = 0; + out_6715024945092366734[18] = 0; + out_6715024945092366734[19] = 0; + out_6715024945092366734[20] = 0; + out_6715024945092366734[21] = 0; + out_6715024945092366734[22] = 0; + out_6715024945092366734[23] = 0; + out_6715024945092366734[24] = 0; + out_6715024945092366734[25] = 1; + out_6715024945092366734[26] = 0; + out_6715024945092366734[27] = 0; + out_6715024945092366734[28] = 1; + out_6715024945092366734[29] = 0; + out_6715024945092366734[30] = 0; + out_6715024945092366734[31] = 0; + out_6715024945092366734[32] = 0; + out_6715024945092366734[33] = 0; + out_6715024945092366734[34] = 0; + out_6715024945092366734[35] = 0; + out_6715024945092366734[36] = 0; + out_6715024945092366734[37] = 0; + out_6715024945092366734[38] = 0; + out_6715024945092366734[39] = 0; + out_6715024945092366734[40] = 0; + out_6715024945092366734[41] = 0; + out_6715024945092366734[42] = 0; + out_6715024945092366734[43] = 0; + out_6715024945092366734[44] = 1; + out_6715024945092366734[45] = 0; + out_6715024945092366734[46] = 0; + out_6715024945092366734[47] = 1; + out_6715024945092366734[48] = 0; + out_6715024945092366734[49] = 0; + out_6715024945092366734[50] = 0; + out_6715024945092366734[51] = 0; + out_6715024945092366734[52] = 0; + out_6715024945092366734[53] = 0; } -void h_10(double *state, double *unused, double *out_6922910997507784152) { - out_6922910997507784152[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_6922910997507784152[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_6922910997507784152[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_4495031819694209583) { + out_4495031819694209583[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_4495031819694209583[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_4495031819694209583[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_283934102184360827) { - out_283934102184360827[0] = 0; - out_283934102184360827[1] = 9.8100000000000005*cos(state[1]); - out_283934102184360827[2] = 0; - out_283934102184360827[3] = 0; - out_283934102184360827[4] = -state[8]; - out_283934102184360827[5] = state[7]; - out_283934102184360827[6] = 0; - out_283934102184360827[7] = state[5]; - out_283934102184360827[8] = -state[4]; - out_283934102184360827[9] = 0; - out_283934102184360827[10] = 0; - out_283934102184360827[11] = 0; - out_283934102184360827[12] = 1; - out_283934102184360827[13] = 0; - out_283934102184360827[14] = 0; - out_283934102184360827[15] = 1; - out_283934102184360827[16] = 0; - out_283934102184360827[17] = 0; - out_283934102184360827[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_283934102184360827[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_283934102184360827[20] = 0; - out_283934102184360827[21] = state[8]; - out_283934102184360827[22] = 0; - out_283934102184360827[23] = -state[6]; - out_283934102184360827[24] = -state[5]; - out_283934102184360827[25] = 0; - out_283934102184360827[26] = state[3]; - out_283934102184360827[27] = 0; - out_283934102184360827[28] = 0; - out_283934102184360827[29] = 0; - out_283934102184360827[30] = 0; - out_283934102184360827[31] = 1; - out_283934102184360827[32] = 0; - out_283934102184360827[33] = 0; - out_283934102184360827[34] = 1; - out_283934102184360827[35] = 0; - out_283934102184360827[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_283934102184360827[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_283934102184360827[38] = 0; - out_283934102184360827[39] = -state[7]; - out_283934102184360827[40] = state[6]; - out_283934102184360827[41] = 0; - out_283934102184360827[42] = state[4]; - out_283934102184360827[43] = -state[3]; - out_283934102184360827[44] = 0; - out_283934102184360827[45] = 0; - out_283934102184360827[46] = 0; - out_283934102184360827[47] = 0; - out_283934102184360827[48] = 0; - out_283934102184360827[49] = 0; - out_283934102184360827[50] = 1; - out_283934102184360827[51] = 0; - out_283934102184360827[52] = 0; - out_283934102184360827[53] = 1; +void H_10(double *state, double *unused, double *out_8084729141614988184) { + out_8084729141614988184[0] = 0; + out_8084729141614988184[1] = 9.8100000000000005*cos(state[1]); + out_8084729141614988184[2] = 0; + out_8084729141614988184[3] = 0; + out_8084729141614988184[4] = -state[8]; + out_8084729141614988184[5] = state[7]; + out_8084729141614988184[6] = 0; + out_8084729141614988184[7] = state[5]; + out_8084729141614988184[8] = -state[4]; + out_8084729141614988184[9] = 0; + out_8084729141614988184[10] = 0; + out_8084729141614988184[11] = 0; + out_8084729141614988184[12] = 1; + out_8084729141614988184[13] = 0; + out_8084729141614988184[14] = 0; + out_8084729141614988184[15] = 1; + out_8084729141614988184[16] = 0; + out_8084729141614988184[17] = 0; + out_8084729141614988184[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_8084729141614988184[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_8084729141614988184[20] = 0; + out_8084729141614988184[21] = state[8]; + out_8084729141614988184[22] = 0; + out_8084729141614988184[23] = -state[6]; + out_8084729141614988184[24] = -state[5]; + out_8084729141614988184[25] = 0; + out_8084729141614988184[26] = state[3]; + out_8084729141614988184[27] = 0; + out_8084729141614988184[28] = 0; + out_8084729141614988184[29] = 0; + out_8084729141614988184[30] = 0; + out_8084729141614988184[31] = 1; + out_8084729141614988184[32] = 0; + out_8084729141614988184[33] = 0; + out_8084729141614988184[34] = 1; + out_8084729141614988184[35] = 0; + out_8084729141614988184[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_8084729141614988184[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_8084729141614988184[38] = 0; + out_8084729141614988184[39] = -state[7]; + out_8084729141614988184[40] = state[6]; + out_8084729141614988184[41] = 0; + out_8084729141614988184[42] = state[4]; + out_8084729141614988184[43] = -state[3]; + out_8084729141614988184[44] = 0; + out_8084729141614988184[45] = 0; + out_8084729141614988184[46] = 0; + out_8084729141614988184[47] = 0; + out_8084729141614988184[48] = 0; + out_8084729141614988184[49] = 0; + out_8084729141614988184[50] = 1; + out_8084729141614988184[51] = 0; + out_8084729141614988184[52] = 0; + out_8084729141614988184[53] = 1; } -void h_13(double *state, double *unused, double *out_6392911292944498098) { - out_6392911292944498098[0] = state[3]; - out_6392911292944498098[1] = state[4]; - out_6392911292944498098[2] = state[5]; +void h_13(double *state, double *unused, double *out_7265342654384269353) { + out_7265342654384269353[0] = state[3]; + out_7265342654384269353[1] = state[4]; + out_7265342654384269353[2] = state[5]; } -void H_13(double *state, double *unused, double *out_1469517109436214092) { - out_1469517109436214092[0] = 0; - out_1469517109436214092[1] = 0; - out_1469517109436214092[2] = 0; - out_1469517109436214092[3] = 1; - out_1469517109436214092[4] = 0; - out_1469517109436214092[5] = 0; - out_1469517109436214092[6] = 0; - out_1469517109436214092[7] = 0; - out_1469517109436214092[8] = 0; - out_1469517109436214092[9] = 0; - out_1469517109436214092[10] = 0; - out_1469517109436214092[11] = 0; - out_1469517109436214092[12] = 0; - out_1469517109436214092[13] = 0; - out_1469517109436214092[14] = 0; - out_1469517109436214092[15] = 0; - out_1469517109436214092[16] = 0; - out_1469517109436214092[17] = 0; - out_1469517109436214092[18] = 0; - out_1469517109436214092[19] = 0; - out_1469517109436214092[20] = 0; - out_1469517109436214092[21] = 0; - out_1469517109436214092[22] = 1; - out_1469517109436214092[23] = 0; - out_1469517109436214092[24] = 0; - out_1469517109436214092[25] = 0; - out_1469517109436214092[26] = 0; - out_1469517109436214092[27] = 0; - out_1469517109436214092[28] = 0; - out_1469517109436214092[29] = 0; - out_1469517109436214092[30] = 0; - out_1469517109436214092[31] = 0; - out_1469517109436214092[32] = 0; - out_1469517109436214092[33] = 0; - out_1469517109436214092[34] = 0; - out_1469517109436214092[35] = 0; - out_1469517109436214092[36] = 0; - out_1469517109436214092[37] = 0; - out_1469517109436214092[38] = 0; - out_1469517109436214092[39] = 0; - out_1469517109436214092[40] = 0; - out_1469517109436214092[41] = 1; - out_1469517109436214092[42] = 0; - out_1469517109436214092[43] = 0; - out_1469517109436214092[44] = 0; - out_1469517109436214092[45] = 0; - out_1469517109436214092[46] = 0; - out_1469517109436214092[47] = 0; - out_1469517109436214092[48] = 0; - out_1469517109436214092[49] = 0; - out_1469517109436214092[50] = 0; - out_1469517109436214092[51] = 0; - out_1469517109436214092[52] = 0; - out_1469517109436214092[53] = 0; +void H_13(double *state, double *unused, double *out_8519445303284852081) { + out_8519445303284852081[0] = 0; + out_8519445303284852081[1] = 0; + out_8519445303284852081[2] = 0; + out_8519445303284852081[3] = 1; + out_8519445303284852081[4] = 0; + out_8519445303284852081[5] = 0; + out_8519445303284852081[6] = 0; + out_8519445303284852081[7] = 0; + out_8519445303284852081[8] = 0; + out_8519445303284852081[9] = 0; + out_8519445303284852081[10] = 0; + out_8519445303284852081[11] = 0; + out_8519445303284852081[12] = 0; + out_8519445303284852081[13] = 0; + out_8519445303284852081[14] = 0; + out_8519445303284852081[15] = 0; + out_8519445303284852081[16] = 0; + out_8519445303284852081[17] = 0; + out_8519445303284852081[18] = 0; + out_8519445303284852081[19] = 0; + out_8519445303284852081[20] = 0; + out_8519445303284852081[21] = 0; + out_8519445303284852081[22] = 1; + out_8519445303284852081[23] = 0; + out_8519445303284852081[24] = 0; + out_8519445303284852081[25] = 0; + out_8519445303284852081[26] = 0; + out_8519445303284852081[27] = 0; + out_8519445303284852081[28] = 0; + out_8519445303284852081[29] = 0; + out_8519445303284852081[30] = 0; + out_8519445303284852081[31] = 0; + out_8519445303284852081[32] = 0; + out_8519445303284852081[33] = 0; + out_8519445303284852081[34] = 0; + out_8519445303284852081[35] = 0; + out_8519445303284852081[36] = 0; + out_8519445303284852081[37] = 0; + out_8519445303284852081[38] = 0; + out_8519445303284852081[39] = 0; + out_8519445303284852081[40] = 0; + out_8519445303284852081[41] = 1; + out_8519445303284852081[42] = 0; + out_8519445303284852081[43] = 0; + out_8519445303284852081[44] = 0; + out_8519445303284852081[45] = 0; + out_8519445303284852081[46] = 0; + out_8519445303284852081[47] = 0; + out_8519445303284852081[48] = 0; + out_8519445303284852081[49] = 0; + out_8519445303284852081[50] = 0; + out_8519445303284852081[51] = 0; + out_8519445303284852081[52] = 0; + out_8519445303284852081[53] = 0; } -void h_14(double *state, double *unused, double *out_2710339252461203529) { - out_2710339252461203529[0] = state[6]; - out_2710339252461203529[1] = state[7]; - out_2710339252461203529[2] = state[8]; +void h_14(double *state, double *unused, double *out_2527703015463387955) { + out_2527703015463387955[0] = state[6]; + out_2527703015463387955[1] = state[7]; + out_2527703015463387955[2] = state[8]; } -void H_14(double *state, double *unused, double *out_2220484140443365820) { - out_2220484140443365820[0] = 0; - out_2220484140443365820[1] = 0; - out_2220484140443365820[2] = 0; - out_2220484140443365820[3] = 0; - out_2220484140443365820[4] = 0; - out_2220484140443365820[5] = 0; - out_2220484140443365820[6] = 1; - out_2220484140443365820[7] = 0; - out_2220484140443365820[8] = 0; - out_2220484140443365820[9] = 0; - out_2220484140443365820[10] = 0; - out_2220484140443365820[11] = 0; - out_2220484140443365820[12] = 0; - out_2220484140443365820[13] = 0; - out_2220484140443365820[14] = 0; - out_2220484140443365820[15] = 0; - out_2220484140443365820[16] = 0; - out_2220484140443365820[17] = 0; - out_2220484140443365820[18] = 0; - out_2220484140443365820[19] = 0; - out_2220484140443365820[20] = 0; - out_2220484140443365820[21] = 0; - out_2220484140443365820[22] = 0; - out_2220484140443365820[23] = 0; - out_2220484140443365820[24] = 0; - out_2220484140443365820[25] = 1; - out_2220484140443365820[26] = 0; - out_2220484140443365820[27] = 0; - out_2220484140443365820[28] = 0; - out_2220484140443365820[29] = 0; - out_2220484140443365820[30] = 0; - out_2220484140443365820[31] = 0; - out_2220484140443365820[32] = 0; - out_2220484140443365820[33] = 0; - out_2220484140443365820[34] = 0; - out_2220484140443365820[35] = 0; - out_2220484140443365820[36] = 0; - out_2220484140443365820[37] = 0; - out_2220484140443365820[38] = 0; - out_2220484140443365820[39] = 0; - out_2220484140443365820[40] = 0; - out_2220484140443365820[41] = 0; - out_2220484140443365820[42] = 0; - out_2220484140443365820[43] = 0; - out_2220484140443365820[44] = 1; - out_2220484140443365820[45] = 0; - out_2220484140443365820[46] = 0; - out_2220484140443365820[47] = 0; - out_2220484140443365820[48] = 0; - out_2220484140443365820[49] = 0; - out_2220484140443365820[50] = 0; - out_2220484140443365820[51] = 0; - out_2220484140443365820[52] = 0; - out_2220484140443365820[53] = 0; +void H_14(double *state, double *unused, double *out_7768478272277700353) { + out_7768478272277700353[0] = 0; + out_7768478272277700353[1] = 0; + out_7768478272277700353[2] = 0; + out_7768478272277700353[3] = 0; + out_7768478272277700353[4] = 0; + out_7768478272277700353[5] = 0; + out_7768478272277700353[6] = 1; + out_7768478272277700353[7] = 0; + out_7768478272277700353[8] = 0; + out_7768478272277700353[9] = 0; + out_7768478272277700353[10] = 0; + out_7768478272277700353[11] = 0; + out_7768478272277700353[12] = 0; + out_7768478272277700353[13] = 0; + out_7768478272277700353[14] = 0; + out_7768478272277700353[15] = 0; + out_7768478272277700353[16] = 0; + out_7768478272277700353[17] = 0; + out_7768478272277700353[18] = 0; + out_7768478272277700353[19] = 0; + out_7768478272277700353[20] = 0; + out_7768478272277700353[21] = 0; + out_7768478272277700353[22] = 0; + out_7768478272277700353[23] = 0; + out_7768478272277700353[24] = 0; + out_7768478272277700353[25] = 1; + out_7768478272277700353[26] = 0; + out_7768478272277700353[27] = 0; + out_7768478272277700353[28] = 0; + out_7768478272277700353[29] = 0; + out_7768478272277700353[30] = 0; + out_7768478272277700353[31] = 0; + out_7768478272277700353[32] = 0; + out_7768478272277700353[33] = 0; + out_7768478272277700353[34] = 0; + out_7768478272277700353[35] = 0; + out_7768478272277700353[36] = 0; + out_7768478272277700353[37] = 0; + out_7768478272277700353[38] = 0; + out_7768478272277700353[39] = 0; + out_7768478272277700353[40] = 0; + out_7768478272277700353[41] = 0; + out_7768478272277700353[42] = 0; + out_7768478272277700353[43] = 0; + out_7768478272277700353[44] = 1; + out_7768478272277700353[45] = 0; + out_7768478272277700353[46] = 0; + out_7768478272277700353[47] = 0; + out_7768478272277700353[48] = 0; + out_7768478272277700353[49] = 0; + out_7768478272277700353[50] = 0; + out_7768478272277700353[51] = 0; + out_7768478272277700353[52] = 0; + out_7768478272277700353[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_8885367235752958450) { - err_fun(nom_x, delta_x, out_8885367235752958450); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7527833659731555578) { + err_fun(nom_x, delta_x, out_7527833659731555578); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4135735359507688267) { - inv_err_fun(nom_x, true_x, out_4135735359507688267); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8121371430920178446) { + inv_err_fun(nom_x, true_x, out_8121371430920178446); } -void pose_H_mod_fun(double *state, double *out_2496917717950831316) { - H_mod_fun(state, out_2496917717950831316); +void pose_H_mod_fun(double *state, double *out_2787357462559363540) { + H_mod_fun(state, out_2787357462559363540); } -void pose_f_fun(double *state, double dt, double *out_6905768897951283532) { - f_fun(state, dt, out_6905768897951283532); +void pose_f_fun(double *state, double dt, double *out_2997254779517652533) { + f_fun(state, dt, out_2997254779517652533); } -void pose_F_fun(double *state, double dt, double *out_5307423376078500036) { - F_fun(state, dt, out_5307423376078500036); +void pose_F_fun(double *state, double dt, double *out_3476500674504442735) { + F_fun(state, dt, out_3476500674504442735); } -void pose_h_4(double *state, double *unused, double *out_1079193932347421775) { - h_4(state, unused, out_1079193932347421775); +void pose_h_4(double *state, double *unused, double *out_2259436079279725972) { + h_4(state, unused, out_2259436079279725972); } -void pose_H_4(double *state, double *unused, double *out_1742756715896118709) { - H_4(state, unused, out_1742756715896118709); +void pose_H_4(double *state, double *unused, double *out_6715024945092366734) { + H_4(state, unused, out_6715024945092366734); } -void pose_h_10(double *state, double *unused, double *out_6922910997507784152) { - h_10(state, unused, out_6922910997507784152); +void pose_h_10(double *state, double *unused, double *out_4495031819694209583) { + h_10(state, unused, out_4495031819694209583); } -void pose_H_10(double *state, double *unused, double *out_283934102184360827) { - H_10(state, unused, out_283934102184360827); +void pose_H_10(double *state, double *unused, double *out_8084729141614988184) { + H_10(state, unused, out_8084729141614988184); } -void pose_h_13(double *state, double *unused, double *out_6392911292944498098) { - h_13(state, unused, out_6392911292944498098); +void pose_h_13(double *state, double *unused, double *out_7265342654384269353) { + h_13(state, unused, out_7265342654384269353); } -void pose_H_13(double *state, double *unused, double *out_1469517109436214092) { - H_13(state, unused, out_1469517109436214092); +void pose_H_13(double *state, double *unused, double *out_8519445303284852081) { + H_13(state, unused, out_8519445303284852081); } -void pose_h_14(double *state, double *unused, double *out_2710339252461203529) { - h_14(state, unused, out_2710339252461203529); +void pose_h_14(double *state, double *unused, double *out_2527703015463387955) { + h_14(state, unused, out_2527703015463387955); } -void pose_H_14(double *state, double *unused, double *out_2220484140443365820) { - H_14(state, unused, out_2220484140443365820); +void pose_H_14(double *state, double *unused, double *out_7768478272277700353) { + H_14(state, unused, out_7768478272277700353); } 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 1c520e617..88ab50026 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_8885367235752958450); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4135735359507688267); -void pose_H_mod_fun(double *state, double *out_2496917717950831316); -void pose_f_fun(double *state, double dt, double *out_6905768897951283532); -void pose_F_fun(double *state, double dt, double *out_5307423376078500036); -void pose_h_4(double *state, double *unused, double *out_1079193932347421775); -void pose_H_4(double *state, double *unused, double *out_1742756715896118709); -void pose_h_10(double *state, double *unused, double *out_6922910997507784152); -void pose_H_10(double *state, double *unused, double *out_283934102184360827); -void pose_h_13(double *state, double *unused, double *out_6392911292944498098); -void pose_H_13(double *state, double *unused, double *out_1469517109436214092); -void pose_h_14(double *state, double *unused, double *out_2710339252461203529); -void pose_H_14(double *state, double *unused, double *out_2220484140443365820); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7527833659731555578); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8121371430920178446); +void pose_H_mod_fun(double *state, double *out_2787357462559363540); +void pose_f_fun(double *state, double dt, double *out_2997254779517652533); +void pose_F_fun(double *state, double dt, double *out_3476500674504442735); +void pose_h_4(double *state, double *unused, double *out_2259436079279725972); +void pose_H_4(double *state, double *unused, double *out_6715024945092366734); +void pose_h_10(double *state, double *unused, double *out_4495031819694209583); +void pose_H_10(double *state, double *unused, double *out_8084729141614988184); +void pose_h_13(double *state, double *unused, double *out_7265342654384269353); +void pose_H_13(double *state, double *unused, double *out_8519445303284852081); +void pose_h_14(double *state, double *unused, double *out_2527703015463387955); +void pose_H_14(double *state, double *unused, double *out_7768478272277700353); 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 1a45d8609..19d89231b 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index b54bb9d41..a895ddcdb 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 89cf3f436..a638d31f5 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index be3ca3671..93ec906d8 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 0b34567c0..451e11ffc 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 770743d54..8a856b4ee 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ