diff --git a/common/libcommon.a b/common/libcommon.a index 5dc99c65b..5ed0eacdc 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 ade4b70c0..ff164851a 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 8aabe9782..07c4d2be6 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 cfb10fc67..a5ed97a19 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 43ff4ffc1..2cccf5882 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 69c4fad86..f4d64cc90 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 ecf35d2bd..08d2fc9a5 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 c796f5eaf..9e25af20e 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 138851fa0..b36931b2b 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 138851fa0..b36931b2b 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 d9588a0b2..c215cf5ad 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 c796f5eaf..9e25af20e 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 516eb068a..b9e6ea572 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-301a45c7-DEBUG"; +const uint8_t gitversion[19] = "DEV-a6758eb9-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 9aaa51526..efb3df3cb 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 e2844bbad..06b961287 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 b5db35774..de2cfa972 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 6f2d5be2c..21b298e7c 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 58068583e..366ea201c 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 40cc2325b..28c0c455a 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 fa45b7d95..0b4c45251 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 fc317c393..3ef28d1d7 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 de1e68881..17518ffaf 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 f664f7f0a..e10fb434b 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 4195cccb0..fa7f634e7 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 1a2194213..7426bda24 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 0f96144cb..02500de02 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 80d4878db..dcf6fc694 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 5bef145f9..fc5770fbb 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 c8942cdea..ee913b0bb 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 6093e91a0..d327fdf1d 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 a670e9d6c..877ce6665 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 d0187d8d4..1855aa0e4 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 c2be48e77..f206dc8e3 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 7d5532868..ad21eba03 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-301a45c7-DEBUG \ No newline at end of file +DEV-a6758eb9-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 525813374..3f0827259 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_3959736040406304353) { - out_3959736040406304353[0] = delta_x[0] + nom_x[0]; - out_3959736040406304353[1] = delta_x[1] + nom_x[1]; - out_3959736040406304353[2] = delta_x[2] + nom_x[2]; - out_3959736040406304353[3] = delta_x[3] + nom_x[3]; - out_3959736040406304353[4] = delta_x[4] + nom_x[4]; - out_3959736040406304353[5] = delta_x[5] + nom_x[5]; - out_3959736040406304353[6] = delta_x[6] + nom_x[6]; - out_3959736040406304353[7] = delta_x[7] + nom_x[7]; - out_3959736040406304353[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_8838314869193199803) { + out_8838314869193199803[0] = delta_x[0] + nom_x[0]; + out_8838314869193199803[1] = delta_x[1] + nom_x[1]; + out_8838314869193199803[2] = delta_x[2] + nom_x[2]; + out_8838314869193199803[3] = delta_x[3] + nom_x[3]; + out_8838314869193199803[4] = delta_x[4] + nom_x[4]; + out_8838314869193199803[5] = delta_x[5] + nom_x[5]; + out_8838314869193199803[6] = delta_x[6] + nom_x[6]; + out_8838314869193199803[7] = delta_x[7] + nom_x[7]; + out_8838314869193199803[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7899142377712818401) { - out_7899142377712818401[0] = -nom_x[0] + true_x[0]; - out_7899142377712818401[1] = -nom_x[1] + true_x[1]; - out_7899142377712818401[2] = -nom_x[2] + true_x[2]; - out_7899142377712818401[3] = -nom_x[3] + true_x[3]; - out_7899142377712818401[4] = -nom_x[4] + true_x[4]; - out_7899142377712818401[5] = -nom_x[5] + true_x[5]; - out_7899142377712818401[6] = -nom_x[6] + true_x[6]; - out_7899142377712818401[7] = -nom_x[7] + true_x[7]; - out_7899142377712818401[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1156535968876491631) { + out_1156535968876491631[0] = -nom_x[0] + true_x[0]; + out_1156535968876491631[1] = -nom_x[1] + true_x[1]; + out_1156535968876491631[2] = -nom_x[2] + true_x[2]; + out_1156535968876491631[3] = -nom_x[3] + true_x[3]; + out_1156535968876491631[4] = -nom_x[4] + true_x[4]; + out_1156535968876491631[5] = -nom_x[5] + true_x[5]; + out_1156535968876491631[6] = -nom_x[6] + true_x[6]; + out_1156535968876491631[7] = -nom_x[7] + true_x[7]; + out_1156535968876491631[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_8853016966935855995) { - out_8853016966935855995[0] = 1.0; - out_8853016966935855995[1] = 0.0; - out_8853016966935855995[2] = 0.0; - out_8853016966935855995[3] = 0.0; - out_8853016966935855995[4] = 0.0; - out_8853016966935855995[5] = 0.0; - out_8853016966935855995[6] = 0.0; - out_8853016966935855995[7] = 0.0; - out_8853016966935855995[8] = 0.0; - out_8853016966935855995[9] = 0.0; - out_8853016966935855995[10] = 1.0; - out_8853016966935855995[11] = 0.0; - out_8853016966935855995[12] = 0.0; - out_8853016966935855995[13] = 0.0; - out_8853016966935855995[14] = 0.0; - out_8853016966935855995[15] = 0.0; - out_8853016966935855995[16] = 0.0; - out_8853016966935855995[17] = 0.0; - out_8853016966935855995[18] = 0.0; - out_8853016966935855995[19] = 0.0; - out_8853016966935855995[20] = 1.0; - out_8853016966935855995[21] = 0.0; - out_8853016966935855995[22] = 0.0; - out_8853016966935855995[23] = 0.0; - out_8853016966935855995[24] = 0.0; - out_8853016966935855995[25] = 0.0; - out_8853016966935855995[26] = 0.0; - out_8853016966935855995[27] = 0.0; - out_8853016966935855995[28] = 0.0; - out_8853016966935855995[29] = 0.0; - out_8853016966935855995[30] = 1.0; - out_8853016966935855995[31] = 0.0; - out_8853016966935855995[32] = 0.0; - out_8853016966935855995[33] = 0.0; - out_8853016966935855995[34] = 0.0; - out_8853016966935855995[35] = 0.0; - out_8853016966935855995[36] = 0.0; - out_8853016966935855995[37] = 0.0; - out_8853016966935855995[38] = 0.0; - out_8853016966935855995[39] = 0.0; - out_8853016966935855995[40] = 1.0; - out_8853016966935855995[41] = 0.0; - out_8853016966935855995[42] = 0.0; - out_8853016966935855995[43] = 0.0; - out_8853016966935855995[44] = 0.0; - out_8853016966935855995[45] = 0.0; - out_8853016966935855995[46] = 0.0; - out_8853016966935855995[47] = 0.0; - out_8853016966935855995[48] = 0.0; - out_8853016966935855995[49] = 0.0; - out_8853016966935855995[50] = 1.0; - out_8853016966935855995[51] = 0.0; - out_8853016966935855995[52] = 0.0; - out_8853016966935855995[53] = 0.0; - out_8853016966935855995[54] = 0.0; - out_8853016966935855995[55] = 0.0; - out_8853016966935855995[56] = 0.0; - out_8853016966935855995[57] = 0.0; - out_8853016966935855995[58] = 0.0; - out_8853016966935855995[59] = 0.0; - out_8853016966935855995[60] = 1.0; - out_8853016966935855995[61] = 0.0; - out_8853016966935855995[62] = 0.0; - out_8853016966935855995[63] = 0.0; - out_8853016966935855995[64] = 0.0; - out_8853016966935855995[65] = 0.0; - out_8853016966935855995[66] = 0.0; - out_8853016966935855995[67] = 0.0; - out_8853016966935855995[68] = 0.0; - out_8853016966935855995[69] = 0.0; - out_8853016966935855995[70] = 1.0; - out_8853016966935855995[71] = 0.0; - out_8853016966935855995[72] = 0.0; - out_8853016966935855995[73] = 0.0; - out_8853016966935855995[74] = 0.0; - out_8853016966935855995[75] = 0.0; - out_8853016966935855995[76] = 0.0; - out_8853016966935855995[77] = 0.0; - out_8853016966935855995[78] = 0.0; - out_8853016966935855995[79] = 0.0; - out_8853016966935855995[80] = 1.0; +void H_mod_fun(double *state, double *out_7297179764193114402) { + out_7297179764193114402[0] = 1.0; + out_7297179764193114402[1] = 0.0; + out_7297179764193114402[2] = 0.0; + out_7297179764193114402[3] = 0.0; + out_7297179764193114402[4] = 0.0; + out_7297179764193114402[5] = 0.0; + out_7297179764193114402[6] = 0.0; + out_7297179764193114402[7] = 0.0; + out_7297179764193114402[8] = 0.0; + out_7297179764193114402[9] = 0.0; + out_7297179764193114402[10] = 1.0; + out_7297179764193114402[11] = 0.0; + out_7297179764193114402[12] = 0.0; + out_7297179764193114402[13] = 0.0; + out_7297179764193114402[14] = 0.0; + out_7297179764193114402[15] = 0.0; + out_7297179764193114402[16] = 0.0; + out_7297179764193114402[17] = 0.0; + out_7297179764193114402[18] = 0.0; + out_7297179764193114402[19] = 0.0; + out_7297179764193114402[20] = 1.0; + out_7297179764193114402[21] = 0.0; + out_7297179764193114402[22] = 0.0; + out_7297179764193114402[23] = 0.0; + out_7297179764193114402[24] = 0.0; + out_7297179764193114402[25] = 0.0; + out_7297179764193114402[26] = 0.0; + out_7297179764193114402[27] = 0.0; + out_7297179764193114402[28] = 0.0; + out_7297179764193114402[29] = 0.0; + out_7297179764193114402[30] = 1.0; + out_7297179764193114402[31] = 0.0; + out_7297179764193114402[32] = 0.0; + out_7297179764193114402[33] = 0.0; + out_7297179764193114402[34] = 0.0; + out_7297179764193114402[35] = 0.0; + out_7297179764193114402[36] = 0.0; + out_7297179764193114402[37] = 0.0; + out_7297179764193114402[38] = 0.0; + out_7297179764193114402[39] = 0.0; + out_7297179764193114402[40] = 1.0; + out_7297179764193114402[41] = 0.0; + out_7297179764193114402[42] = 0.0; + out_7297179764193114402[43] = 0.0; + out_7297179764193114402[44] = 0.0; + out_7297179764193114402[45] = 0.0; + out_7297179764193114402[46] = 0.0; + out_7297179764193114402[47] = 0.0; + out_7297179764193114402[48] = 0.0; + out_7297179764193114402[49] = 0.0; + out_7297179764193114402[50] = 1.0; + out_7297179764193114402[51] = 0.0; + out_7297179764193114402[52] = 0.0; + out_7297179764193114402[53] = 0.0; + out_7297179764193114402[54] = 0.0; + out_7297179764193114402[55] = 0.0; + out_7297179764193114402[56] = 0.0; + out_7297179764193114402[57] = 0.0; + out_7297179764193114402[58] = 0.0; + out_7297179764193114402[59] = 0.0; + out_7297179764193114402[60] = 1.0; + out_7297179764193114402[61] = 0.0; + out_7297179764193114402[62] = 0.0; + out_7297179764193114402[63] = 0.0; + out_7297179764193114402[64] = 0.0; + out_7297179764193114402[65] = 0.0; + out_7297179764193114402[66] = 0.0; + out_7297179764193114402[67] = 0.0; + out_7297179764193114402[68] = 0.0; + out_7297179764193114402[69] = 0.0; + out_7297179764193114402[70] = 1.0; + out_7297179764193114402[71] = 0.0; + out_7297179764193114402[72] = 0.0; + out_7297179764193114402[73] = 0.0; + out_7297179764193114402[74] = 0.0; + out_7297179764193114402[75] = 0.0; + out_7297179764193114402[76] = 0.0; + out_7297179764193114402[77] = 0.0; + out_7297179764193114402[78] = 0.0; + out_7297179764193114402[79] = 0.0; + out_7297179764193114402[80] = 1.0; } -void f_fun(double *state, double dt, double *out_8070155753173713130) { - out_8070155753173713130[0] = state[0]; - out_8070155753173713130[1] = state[1]; - out_8070155753173713130[2] = state[2]; - out_8070155753173713130[3] = state[3]; - out_8070155753173713130[4] = state[4]; - out_8070155753173713130[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_8070155753173713130[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_8070155753173713130[7] = state[7]; - out_8070155753173713130[8] = state[8]; +void f_fun(double *state, double dt, double *out_6025394122966474126) { + out_6025394122966474126[0] = state[0]; + out_6025394122966474126[1] = state[1]; + out_6025394122966474126[2] = state[2]; + out_6025394122966474126[3] = state[3]; + out_6025394122966474126[4] = state[4]; + out_6025394122966474126[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_6025394122966474126[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_6025394122966474126[7] = state[7]; + out_6025394122966474126[8] = state[8]; } -void F_fun(double *state, double dt, double *out_2319703520718919116) { - out_2319703520718919116[0] = 1; - out_2319703520718919116[1] = 0; - out_2319703520718919116[2] = 0; - out_2319703520718919116[3] = 0; - out_2319703520718919116[4] = 0; - out_2319703520718919116[5] = 0; - out_2319703520718919116[6] = 0; - out_2319703520718919116[7] = 0; - out_2319703520718919116[8] = 0; - out_2319703520718919116[9] = 0; - out_2319703520718919116[10] = 1; - out_2319703520718919116[11] = 0; - out_2319703520718919116[12] = 0; - out_2319703520718919116[13] = 0; - out_2319703520718919116[14] = 0; - out_2319703520718919116[15] = 0; - out_2319703520718919116[16] = 0; - out_2319703520718919116[17] = 0; - out_2319703520718919116[18] = 0; - out_2319703520718919116[19] = 0; - out_2319703520718919116[20] = 1; - out_2319703520718919116[21] = 0; - out_2319703520718919116[22] = 0; - out_2319703520718919116[23] = 0; - out_2319703520718919116[24] = 0; - out_2319703520718919116[25] = 0; - out_2319703520718919116[26] = 0; - out_2319703520718919116[27] = 0; - out_2319703520718919116[28] = 0; - out_2319703520718919116[29] = 0; - out_2319703520718919116[30] = 1; - out_2319703520718919116[31] = 0; - out_2319703520718919116[32] = 0; - out_2319703520718919116[33] = 0; - out_2319703520718919116[34] = 0; - out_2319703520718919116[35] = 0; - out_2319703520718919116[36] = 0; - out_2319703520718919116[37] = 0; - out_2319703520718919116[38] = 0; - out_2319703520718919116[39] = 0; - out_2319703520718919116[40] = 1; - out_2319703520718919116[41] = 0; - out_2319703520718919116[42] = 0; - out_2319703520718919116[43] = 0; - out_2319703520718919116[44] = 0; - out_2319703520718919116[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_2319703520718919116[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_2319703520718919116[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2319703520718919116[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2319703520718919116[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_2319703520718919116[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_2319703520718919116[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_2319703520718919116[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_2319703520718919116[53] = -9.8100000000000005*dt; - out_2319703520718919116[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_2319703520718919116[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_2319703520718919116[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2319703520718919116[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2319703520718919116[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_2319703520718919116[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_2319703520718919116[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_2319703520718919116[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2319703520718919116[62] = 0; - out_2319703520718919116[63] = 0; - out_2319703520718919116[64] = 0; - out_2319703520718919116[65] = 0; - out_2319703520718919116[66] = 0; - out_2319703520718919116[67] = 0; - out_2319703520718919116[68] = 0; - out_2319703520718919116[69] = 0; - out_2319703520718919116[70] = 1; - out_2319703520718919116[71] = 0; - out_2319703520718919116[72] = 0; - out_2319703520718919116[73] = 0; - out_2319703520718919116[74] = 0; - out_2319703520718919116[75] = 0; - out_2319703520718919116[76] = 0; - out_2319703520718919116[77] = 0; - out_2319703520718919116[78] = 0; - out_2319703520718919116[79] = 0; - out_2319703520718919116[80] = 1; +void F_fun(double *state, double dt, double *out_2902406519648293307) { + out_2902406519648293307[0] = 1; + out_2902406519648293307[1] = 0; + out_2902406519648293307[2] = 0; + out_2902406519648293307[3] = 0; + out_2902406519648293307[4] = 0; + out_2902406519648293307[5] = 0; + out_2902406519648293307[6] = 0; + out_2902406519648293307[7] = 0; + out_2902406519648293307[8] = 0; + out_2902406519648293307[9] = 0; + out_2902406519648293307[10] = 1; + out_2902406519648293307[11] = 0; + out_2902406519648293307[12] = 0; + out_2902406519648293307[13] = 0; + out_2902406519648293307[14] = 0; + out_2902406519648293307[15] = 0; + out_2902406519648293307[16] = 0; + out_2902406519648293307[17] = 0; + out_2902406519648293307[18] = 0; + out_2902406519648293307[19] = 0; + out_2902406519648293307[20] = 1; + out_2902406519648293307[21] = 0; + out_2902406519648293307[22] = 0; + out_2902406519648293307[23] = 0; + out_2902406519648293307[24] = 0; + out_2902406519648293307[25] = 0; + out_2902406519648293307[26] = 0; + out_2902406519648293307[27] = 0; + out_2902406519648293307[28] = 0; + out_2902406519648293307[29] = 0; + out_2902406519648293307[30] = 1; + out_2902406519648293307[31] = 0; + out_2902406519648293307[32] = 0; + out_2902406519648293307[33] = 0; + out_2902406519648293307[34] = 0; + out_2902406519648293307[35] = 0; + out_2902406519648293307[36] = 0; + out_2902406519648293307[37] = 0; + out_2902406519648293307[38] = 0; + out_2902406519648293307[39] = 0; + out_2902406519648293307[40] = 1; + out_2902406519648293307[41] = 0; + out_2902406519648293307[42] = 0; + out_2902406519648293307[43] = 0; + out_2902406519648293307[44] = 0; + out_2902406519648293307[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_2902406519648293307[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2902406519648293307[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2902406519648293307[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2902406519648293307[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_2902406519648293307[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2902406519648293307[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2902406519648293307[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2902406519648293307[53] = -9.8100000000000005*dt; + out_2902406519648293307[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_2902406519648293307[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2902406519648293307[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2902406519648293307[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2902406519648293307[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_2902406519648293307[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2902406519648293307[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_2902406519648293307[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2902406519648293307[62] = 0; + out_2902406519648293307[63] = 0; + out_2902406519648293307[64] = 0; + out_2902406519648293307[65] = 0; + out_2902406519648293307[66] = 0; + out_2902406519648293307[67] = 0; + out_2902406519648293307[68] = 0; + out_2902406519648293307[69] = 0; + out_2902406519648293307[70] = 1; + out_2902406519648293307[71] = 0; + out_2902406519648293307[72] = 0; + out_2902406519648293307[73] = 0; + out_2902406519648293307[74] = 0; + out_2902406519648293307[75] = 0; + out_2902406519648293307[76] = 0; + out_2902406519648293307[77] = 0; + out_2902406519648293307[78] = 0; + out_2902406519648293307[79] = 0; + out_2902406519648293307[80] = 1; } -void h_25(double *state, double *unused, double *out_4616275488682323747) { - out_4616275488682323747[0] = state[6]; +void h_25(double *state, double *unused, double *out_3236700917680503454) { + out_3236700917680503454[0] = state[6]; } -void H_25(double *state, double *unused, double *out_7903896044907373786) { - out_7903896044907373786[0] = 0; - out_7903896044907373786[1] = 0; - out_7903896044907373786[2] = 0; - out_7903896044907373786[3] = 0; - out_7903896044907373786[4] = 0; - out_7903896044907373786[5] = 0; - out_7903896044907373786[6] = 1; - out_7903896044907373786[7] = 0; - out_7903896044907373786[8] = 0; +void H_25(double *state, double *unused, double *out_1896008461719122835) { + out_1896008461719122835[0] = 0; + out_1896008461719122835[1] = 0; + out_1896008461719122835[2] = 0; + out_1896008461719122835[3] = 0; + out_1896008461719122835[4] = 0; + out_1896008461719122835[5] = 0; + out_1896008461719122835[6] = 1; + out_1896008461719122835[7] = 0; + out_1896008461719122835[8] = 0; } -void h_24(double *state, double *unused, double *out_1240821179762925085) { - out_1240821179762925085[0] = state[4]; - out_1240821179762925085[1] = state[5]; +void h_24(double *state, double *unused, double *out_457181241716230531) { + out_457181241716230531[0] = state[4]; + out_457181241716230531[1] = state[5]; } -void H_24(double *state, double *unused, double *out_4345970446105050650) { - out_4345970446105050650[0] = 0; - out_4345970446105050650[1] = 0; - out_4345970446105050650[2] = 0; - out_4345970446105050650[3] = 0; - out_4345970446105050650[4] = 1; - out_4345970446105050650[5] = 0; - out_4345970446105050650[6] = 0; - out_4345970446105050650[7] = 0; - out_4345970446105050650[8] = 0; - out_4345970446105050650[9] = 0; - out_4345970446105050650[10] = 0; - out_4345970446105050650[11] = 0; - out_4345970446105050650[12] = 0; - out_4345970446105050650[13] = 0; - out_4345970446105050650[14] = 1; - out_4345970446105050650[15] = 0; - out_4345970446105050650[16] = 0; - out_4345970446105050650[17] = 0; +void H_24(double *state, double *unused, double *out_4068658060724622401) { + out_4068658060724622401[0] = 0; + out_4068658060724622401[1] = 0; + out_4068658060724622401[2] = 0; + out_4068658060724622401[3] = 0; + out_4068658060724622401[4] = 1; + out_4068658060724622401[5] = 0; + out_4068658060724622401[6] = 0; + out_4068658060724622401[7] = 0; + out_4068658060724622401[8] = 0; + out_4068658060724622401[9] = 0; + out_4068658060724622401[10] = 0; + out_4068658060724622401[11] = 0; + out_4068658060724622401[12] = 0; + out_4068658060724622401[13] = 0; + out_4068658060724622401[14] = 1; + out_4068658060724622401[15] = 0; + out_4068658060724622401[16] = 0; + out_4068658060724622401[17] = 0; } -void h_30(double *state, double *unused, double *out_978825960428301397) { - out_978825960428301397[0] = state[4]; +void h_30(double *state, double *unused, double *out_7446777899208375721) { + out_7446777899208375721[0] = state[4]; } -void H_30(double *state, double *unused, double *out_8033234992050613856) { - out_8033234992050613856[0] = 0; - out_8033234992050613856[1] = 0; - out_8033234992050613856[2] = 0; - out_8033234992050613856[3] = 0; - out_8033234992050613856[4] = 1; - out_8033234992050613856[5] = 0; - out_8033234992050613856[6] = 0; - out_8033234992050613856[7] = 0; - out_8033234992050613856[8] = 0; +void H_30(double *state, double *unused, double *out_5020681879772493920) { + out_5020681879772493920[0] = 0; + out_5020681879772493920[1] = 0; + out_5020681879772493920[2] = 0; + out_5020681879772493920[3] = 0; + out_5020681879772493920[4] = 1; + out_5020681879772493920[5] = 0; + out_5020681879772493920[6] = 0; + out_5020681879772493920[7] = 0; + out_5020681879772493920[8] = 0; } -void h_26(double *state, double *unused, double *out_35316107526638962) { - out_35316107526638962[0] = state[7]; +void h_26(double *state, double *unused, double *out_8390287752110038156) { + out_8390287752110038156[0] = state[7]; } -void H_26(double *state, double *unused, double *out_6801344709928121606) { - out_6801344709928121606[0] = 0; - out_6801344709928121606[1] = 0; - out_6801344709928121606[2] = 0; - out_6801344709928121606[3] = 0; - out_6801344709928121606[4] = 0; - out_6801344709928121606[5] = 0; - out_6801344709928121606[6] = 0; - out_6801344709928121606[7] = 1; - out_6801344709928121606[8] = 0; +void H_26(double *state, double *unused, double *out_5637511780593179059) { + out_5637511780593179059[0] = 0; + out_5637511780593179059[1] = 0; + out_5637511780593179059[2] = 0; + out_5637511780593179059[3] = 0; + out_5637511780593179059[4] = 0; + out_5637511780593179059[5] = 0; + out_5637511780593179059[6] = 0; + out_5637511780593179059[7] = 1; + out_5637511780593179059[8] = 0; } -void h_27(double *state, double *unused, double *out_8256782726505873065) { - out_8256782726505873065[0] = state[3]; +void h_27(double *state, double *unused, double *out_7774899721600347349) { + out_7774899721600347349[0] = state[3]; } -void H_27(double *state, double *unused, double *out_8238745769858512849) { - out_8238745769858512849[0] = 0; - out_8238745769858512849[1] = 0; - out_8238745769858512849[2] = 0; - out_8238745769858512849[3] = 1; - out_8238745769858512849[4] = 0; - out_8238745769858512849[5] = 0; - out_8238745769858512849[6] = 0; - out_8238745769858512849[7] = 0; - out_8238745769858512849[8] = 0; +void H_27(double *state, double *unused, double *out_2845918567972069009) { + out_2845918567972069009[0] = 0; + out_2845918567972069009[1] = 0; + out_2845918567972069009[2] = 0; + out_2845918567972069009[3] = 1; + out_2845918567972069009[4] = 0; + out_2845918567972069009[5] = 0; + out_2845918567972069009[6] = 0; + out_2845918567972069009[7] = 0; + out_2845918567972069009[8] = 0; } -void h_29(double *state, double *unused, double *out_4022847307232544248) { - out_4022847307232544248[0] = state[1]; +void h_29(double *state, double *unused, double *out_8050093783884853238) { + out_8050093783884853238[0] = state[1]; } -void H_29(double *state, double *unused, double *out_7523003647736221672) { - out_7523003647736221672[0] = 0; - out_7523003647736221672[1] = 1; - out_7523003647736221672[2] = 0; - out_7523003647736221672[3] = 0; - out_7523003647736221672[4] = 0; - out_7523003647736221672[5] = 0; - out_7523003647736221672[6] = 0; - out_7523003647736221672[7] = 0; - out_7523003647736221672[8] = 0; +void H_29(double *state, double *unused, double *out_1132555841102517976) { + out_1132555841102517976[0] = 0; + out_1132555841102517976[1] = 1; + out_1132555841102517976[2] = 0; + out_1132555841102517976[3] = 0; + out_1132555841102517976[4] = 0; + out_1132555841102517976[5] = 0; + out_1132555841102517976[6] = 0; + out_1132555841102517976[7] = 0; + out_1132555841102517976[8] = 0; } -void h_28(double *state, double *unused, double *out_3255109458845027420) { - out_3255109458845027420[0] = state[0]; +void h_28(double *state, double *unused, double *out_1375858204186498203) { + out_1375858204186498203[0] = state[0]; } -void H_28(double *state, double *unused, double *out_5841341408903799370) { - out_5841341408903799370[0] = 1; - out_5841341408903799370[1] = 0; - out_5841341408903799370[2] = 0; - out_5841341408903799370[3] = 0; - out_5841341408903799370[4] = 0; - out_5841341408903799370[5] = 0; - out_5841341408903799370[6] = 0; - out_5841341408903799370[7] = 0; - out_5841341408903799370[8] = 0; +void H_28(double *state, double *unused, double *out_3949843175967012598) { + out_3949843175967012598[0] = 1; + out_3949843175967012598[1] = 0; + out_3949843175967012598[2] = 0; + out_3949843175967012598[3] = 0; + out_3949843175967012598[4] = 0; + out_3949843175967012598[5] = 0; + out_3949843175967012598[6] = 0; + out_3949843175967012598[7] = 0; + out_3949843175967012598[8] = 0; } -void h_31(double *state, double *unused, double *out_6638919472171716073) { - out_6638919472171716073[0] = state[8]; +void h_31(double *state, double *unused, double *out_7176856938506309920) { + out_7176856938506309920[0] = state[8]; } -void H_31(double *state, double *unused, double *out_7873250083030413358) { - out_7873250083030413358[0] = 0; - out_7873250083030413358[1] = 0; - out_7873250083030413358[2] = 0; - out_7873250083030413358[3] = 0; - out_7873250083030413358[4] = 0; - out_7873250083030413358[5] = 0; - out_7873250083030413358[6] = 0; - out_7873250083030413358[7] = 0; - out_7873250083030413358[8] = 1; +void H_31(double *state, double *unused, double *out_1865362499842162407) { + out_1865362499842162407[0] = 0; + out_1865362499842162407[1] = 0; + out_1865362499842162407[2] = 0; + out_1865362499842162407[3] = 0; + out_1865362499842162407[4] = 0; + out_1865362499842162407[5] = 0; + out_1865362499842162407[6] = 0; + out_1865362499842162407[7] = 0; + out_1865362499842162407[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_3959736040406304353) { - err_fun(nom_x, delta_x, out_3959736040406304353); +void car_err_fun(double *nom_x, double *delta_x, double *out_8838314869193199803) { + err_fun(nom_x, delta_x, out_8838314869193199803); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7899142377712818401) { - inv_err_fun(nom_x, true_x, out_7899142377712818401); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1156535968876491631) { + inv_err_fun(nom_x, true_x, out_1156535968876491631); } -void car_H_mod_fun(double *state, double *out_8853016966935855995) { - H_mod_fun(state, out_8853016966935855995); +void car_H_mod_fun(double *state, double *out_7297179764193114402) { + H_mod_fun(state, out_7297179764193114402); } -void car_f_fun(double *state, double dt, double *out_8070155753173713130) { - f_fun(state, dt, out_8070155753173713130); +void car_f_fun(double *state, double dt, double *out_6025394122966474126) { + f_fun(state, dt, out_6025394122966474126); } -void car_F_fun(double *state, double dt, double *out_2319703520718919116) { - F_fun(state, dt, out_2319703520718919116); +void car_F_fun(double *state, double dt, double *out_2902406519648293307) { + F_fun(state, dt, out_2902406519648293307); } -void car_h_25(double *state, double *unused, double *out_4616275488682323747) { - h_25(state, unused, out_4616275488682323747); +void car_h_25(double *state, double *unused, double *out_3236700917680503454) { + h_25(state, unused, out_3236700917680503454); } -void car_H_25(double *state, double *unused, double *out_7903896044907373786) { - H_25(state, unused, out_7903896044907373786); +void car_H_25(double *state, double *unused, double *out_1896008461719122835) { + H_25(state, unused, out_1896008461719122835); } -void car_h_24(double *state, double *unused, double *out_1240821179762925085) { - h_24(state, unused, out_1240821179762925085); +void car_h_24(double *state, double *unused, double *out_457181241716230531) { + h_24(state, unused, out_457181241716230531); } -void car_H_24(double *state, double *unused, double *out_4345970446105050650) { - H_24(state, unused, out_4345970446105050650); +void car_H_24(double *state, double *unused, double *out_4068658060724622401) { + H_24(state, unused, out_4068658060724622401); } -void car_h_30(double *state, double *unused, double *out_978825960428301397) { - h_30(state, unused, out_978825960428301397); +void car_h_30(double *state, double *unused, double *out_7446777899208375721) { + h_30(state, unused, out_7446777899208375721); } -void car_H_30(double *state, double *unused, double *out_8033234992050613856) { - H_30(state, unused, out_8033234992050613856); +void car_H_30(double *state, double *unused, double *out_5020681879772493920) { + H_30(state, unused, out_5020681879772493920); } -void car_h_26(double *state, double *unused, double *out_35316107526638962) { - h_26(state, unused, out_35316107526638962); +void car_h_26(double *state, double *unused, double *out_8390287752110038156) { + h_26(state, unused, out_8390287752110038156); } -void car_H_26(double *state, double *unused, double *out_6801344709928121606) { - H_26(state, unused, out_6801344709928121606); +void car_H_26(double *state, double *unused, double *out_5637511780593179059) { + H_26(state, unused, out_5637511780593179059); } -void car_h_27(double *state, double *unused, double *out_8256782726505873065) { - h_27(state, unused, out_8256782726505873065); +void car_h_27(double *state, double *unused, double *out_7774899721600347349) { + h_27(state, unused, out_7774899721600347349); } -void car_H_27(double *state, double *unused, double *out_8238745769858512849) { - H_27(state, unused, out_8238745769858512849); +void car_H_27(double *state, double *unused, double *out_2845918567972069009) { + H_27(state, unused, out_2845918567972069009); } -void car_h_29(double *state, double *unused, double *out_4022847307232544248) { - h_29(state, unused, out_4022847307232544248); +void car_h_29(double *state, double *unused, double *out_8050093783884853238) { + h_29(state, unused, out_8050093783884853238); } -void car_H_29(double *state, double *unused, double *out_7523003647736221672) { - H_29(state, unused, out_7523003647736221672); +void car_H_29(double *state, double *unused, double *out_1132555841102517976) { + H_29(state, unused, out_1132555841102517976); } -void car_h_28(double *state, double *unused, double *out_3255109458845027420) { - h_28(state, unused, out_3255109458845027420); +void car_h_28(double *state, double *unused, double *out_1375858204186498203) { + h_28(state, unused, out_1375858204186498203); } -void car_H_28(double *state, double *unused, double *out_5841341408903799370) { - H_28(state, unused, out_5841341408903799370); +void car_H_28(double *state, double *unused, double *out_3949843175967012598) { + H_28(state, unused, out_3949843175967012598); } -void car_h_31(double *state, double *unused, double *out_6638919472171716073) { - h_31(state, unused, out_6638919472171716073); +void car_h_31(double *state, double *unused, double *out_7176856938506309920) { + h_31(state, unused, out_7176856938506309920); } -void car_H_31(double *state, double *unused, double *out_7873250083030413358) { - H_31(state, unused, out_7873250083030413358); +void car_H_31(double *state, double *unused, double *out_1865362499842162407) { + H_31(state, unused, out_1865362499842162407); } 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 f801f0e6b..d3bd6bc90 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_3959736040406304353); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7899142377712818401); -void car_H_mod_fun(double *state, double *out_8853016966935855995); -void car_f_fun(double *state, double dt, double *out_8070155753173713130); -void car_F_fun(double *state, double dt, double *out_2319703520718919116); -void car_h_25(double *state, double *unused, double *out_4616275488682323747); -void car_H_25(double *state, double *unused, double *out_7903896044907373786); -void car_h_24(double *state, double *unused, double *out_1240821179762925085); -void car_H_24(double *state, double *unused, double *out_4345970446105050650); -void car_h_30(double *state, double *unused, double *out_978825960428301397); -void car_H_30(double *state, double *unused, double *out_8033234992050613856); -void car_h_26(double *state, double *unused, double *out_35316107526638962); -void car_H_26(double *state, double *unused, double *out_6801344709928121606); -void car_h_27(double *state, double *unused, double *out_8256782726505873065); -void car_H_27(double *state, double *unused, double *out_8238745769858512849); -void car_h_29(double *state, double *unused, double *out_4022847307232544248); -void car_H_29(double *state, double *unused, double *out_7523003647736221672); -void car_h_28(double *state, double *unused, double *out_3255109458845027420); -void car_H_28(double *state, double *unused, double *out_5841341408903799370); -void car_h_31(double *state, double *unused, double *out_6638919472171716073); -void car_H_31(double *state, double *unused, double *out_7873250083030413358); +void car_err_fun(double *nom_x, double *delta_x, double *out_8838314869193199803); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1156535968876491631); +void car_H_mod_fun(double *state, double *out_7297179764193114402); +void car_f_fun(double *state, double dt, double *out_6025394122966474126); +void car_F_fun(double *state, double dt, double *out_2902406519648293307); +void car_h_25(double *state, double *unused, double *out_3236700917680503454); +void car_H_25(double *state, double *unused, double *out_1896008461719122835); +void car_h_24(double *state, double *unused, double *out_457181241716230531); +void car_H_24(double *state, double *unused, double *out_4068658060724622401); +void car_h_30(double *state, double *unused, double *out_7446777899208375721); +void car_H_30(double *state, double *unused, double *out_5020681879772493920); +void car_h_26(double *state, double *unused, double *out_8390287752110038156); +void car_H_26(double *state, double *unused, double *out_5637511780593179059); +void car_h_27(double *state, double *unused, double *out_7774899721600347349); +void car_H_27(double *state, double *unused, double *out_2845918567972069009); +void car_h_29(double *state, double *unused, double *out_8050093783884853238); +void car_H_29(double *state, double *unused, double *out_1132555841102517976); +void car_h_28(double *state, double *unused, double *out_1375858204186498203); +void car_H_28(double *state, double *unused, double *out_3949843175967012598); +void car_h_31(double *state, double *unused, double *out_7176856938506309920); +void car_H_31(double *state, double *unused, double *out_1865362499842162407); 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 bd2fd7c40..bce0de7d5 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_3650099251769799718) { - out_3650099251769799718[0] = delta_x[0] + nom_x[0]; - out_3650099251769799718[1] = delta_x[1] + nom_x[1]; - out_3650099251769799718[2] = delta_x[2] + nom_x[2]; - out_3650099251769799718[3] = delta_x[3] + nom_x[3]; - out_3650099251769799718[4] = delta_x[4] + nom_x[4]; - out_3650099251769799718[5] = delta_x[5] + nom_x[5]; - out_3650099251769799718[6] = delta_x[6] + nom_x[6]; - out_3650099251769799718[7] = delta_x[7] + nom_x[7]; - out_3650099251769799718[8] = delta_x[8] + nom_x[8]; - out_3650099251769799718[9] = delta_x[9] + nom_x[9]; - out_3650099251769799718[10] = delta_x[10] + nom_x[10]; - out_3650099251769799718[11] = delta_x[11] + nom_x[11]; - out_3650099251769799718[12] = delta_x[12] + nom_x[12]; - out_3650099251769799718[13] = delta_x[13] + nom_x[13]; - out_3650099251769799718[14] = delta_x[14] + nom_x[14]; - out_3650099251769799718[15] = delta_x[15] + nom_x[15]; - out_3650099251769799718[16] = delta_x[16] + nom_x[16]; - out_3650099251769799718[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_4833077353004324467) { + out_4833077353004324467[0] = delta_x[0] + nom_x[0]; + out_4833077353004324467[1] = delta_x[1] + nom_x[1]; + out_4833077353004324467[2] = delta_x[2] + nom_x[2]; + out_4833077353004324467[3] = delta_x[3] + nom_x[3]; + out_4833077353004324467[4] = delta_x[4] + nom_x[4]; + out_4833077353004324467[5] = delta_x[5] + nom_x[5]; + out_4833077353004324467[6] = delta_x[6] + nom_x[6]; + out_4833077353004324467[7] = delta_x[7] + nom_x[7]; + out_4833077353004324467[8] = delta_x[8] + nom_x[8]; + out_4833077353004324467[9] = delta_x[9] + nom_x[9]; + out_4833077353004324467[10] = delta_x[10] + nom_x[10]; + out_4833077353004324467[11] = delta_x[11] + nom_x[11]; + out_4833077353004324467[12] = delta_x[12] + nom_x[12]; + out_4833077353004324467[13] = delta_x[13] + nom_x[13]; + out_4833077353004324467[14] = delta_x[14] + nom_x[14]; + out_4833077353004324467[15] = delta_x[15] + nom_x[15]; + out_4833077353004324467[16] = delta_x[16] + nom_x[16]; + out_4833077353004324467[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4549899801136771746) { - out_4549899801136771746[0] = -nom_x[0] + true_x[0]; - out_4549899801136771746[1] = -nom_x[1] + true_x[1]; - out_4549899801136771746[2] = -nom_x[2] + true_x[2]; - out_4549899801136771746[3] = -nom_x[3] + true_x[3]; - out_4549899801136771746[4] = -nom_x[4] + true_x[4]; - out_4549899801136771746[5] = -nom_x[5] + true_x[5]; - out_4549899801136771746[6] = -nom_x[6] + true_x[6]; - out_4549899801136771746[7] = -nom_x[7] + true_x[7]; - out_4549899801136771746[8] = -nom_x[8] + true_x[8]; - out_4549899801136771746[9] = -nom_x[9] + true_x[9]; - out_4549899801136771746[10] = -nom_x[10] + true_x[10]; - out_4549899801136771746[11] = -nom_x[11] + true_x[11]; - out_4549899801136771746[12] = -nom_x[12] + true_x[12]; - out_4549899801136771746[13] = -nom_x[13] + true_x[13]; - out_4549899801136771746[14] = -nom_x[14] + true_x[14]; - out_4549899801136771746[15] = -nom_x[15] + true_x[15]; - out_4549899801136771746[16] = -nom_x[16] + true_x[16]; - out_4549899801136771746[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_4980437301453189923) { + out_4980437301453189923[0] = -nom_x[0] + true_x[0]; + out_4980437301453189923[1] = -nom_x[1] + true_x[1]; + out_4980437301453189923[2] = -nom_x[2] + true_x[2]; + out_4980437301453189923[3] = -nom_x[3] + true_x[3]; + out_4980437301453189923[4] = -nom_x[4] + true_x[4]; + out_4980437301453189923[5] = -nom_x[5] + true_x[5]; + out_4980437301453189923[6] = -nom_x[6] + true_x[6]; + out_4980437301453189923[7] = -nom_x[7] + true_x[7]; + out_4980437301453189923[8] = -nom_x[8] + true_x[8]; + out_4980437301453189923[9] = -nom_x[9] + true_x[9]; + out_4980437301453189923[10] = -nom_x[10] + true_x[10]; + out_4980437301453189923[11] = -nom_x[11] + true_x[11]; + out_4980437301453189923[12] = -nom_x[12] + true_x[12]; + out_4980437301453189923[13] = -nom_x[13] + true_x[13]; + out_4980437301453189923[14] = -nom_x[14] + true_x[14]; + out_4980437301453189923[15] = -nom_x[15] + true_x[15]; + out_4980437301453189923[16] = -nom_x[16] + true_x[16]; + out_4980437301453189923[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_6815214623410224821) { - out_6815214623410224821[0] = 1.0; - out_6815214623410224821[1] = 0.0; - out_6815214623410224821[2] = 0.0; - out_6815214623410224821[3] = 0.0; - out_6815214623410224821[4] = 0.0; - out_6815214623410224821[5] = 0.0; - out_6815214623410224821[6] = 0.0; - out_6815214623410224821[7] = 0.0; - out_6815214623410224821[8] = 0.0; - out_6815214623410224821[9] = 0.0; - out_6815214623410224821[10] = 0.0; - out_6815214623410224821[11] = 0.0; - out_6815214623410224821[12] = 0.0; - out_6815214623410224821[13] = 0.0; - out_6815214623410224821[14] = 0.0; - out_6815214623410224821[15] = 0.0; - out_6815214623410224821[16] = 0.0; - out_6815214623410224821[17] = 0.0; - out_6815214623410224821[18] = 0.0; - out_6815214623410224821[19] = 1.0; - out_6815214623410224821[20] = 0.0; - out_6815214623410224821[21] = 0.0; - out_6815214623410224821[22] = 0.0; - out_6815214623410224821[23] = 0.0; - out_6815214623410224821[24] = 0.0; - out_6815214623410224821[25] = 0.0; - out_6815214623410224821[26] = 0.0; - out_6815214623410224821[27] = 0.0; - out_6815214623410224821[28] = 0.0; - out_6815214623410224821[29] = 0.0; - out_6815214623410224821[30] = 0.0; - out_6815214623410224821[31] = 0.0; - out_6815214623410224821[32] = 0.0; - out_6815214623410224821[33] = 0.0; - out_6815214623410224821[34] = 0.0; - out_6815214623410224821[35] = 0.0; - out_6815214623410224821[36] = 0.0; - out_6815214623410224821[37] = 0.0; - out_6815214623410224821[38] = 1.0; - out_6815214623410224821[39] = 0.0; - out_6815214623410224821[40] = 0.0; - out_6815214623410224821[41] = 0.0; - out_6815214623410224821[42] = 0.0; - out_6815214623410224821[43] = 0.0; - out_6815214623410224821[44] = 0.0; - out_6815214623410224821[45] = 0.0; - out_6815214623410224821[46] = 0.0; - out_6815214623410224821[47] = 0.0; - out_6815214623410224821[48] = 0.0; - out_6815214623410224821[49] = 0.0; - out_6815214623410224821[50] = 0.0; - out_6815214623410224821[51] = 0.0; - out_6815214623410224821[52] = 0.0; - out_6815214623410224821[53] = 0.0; - out_6815214623410224821[54] = 0.0; - out_6815214623410224821[55] = 0.0; - out_6815214623410224821[56] = 0.0; - out_6815214623410224821[57] = 1.0; - out_6815214623410224821[58] = 0.0; - out_6815214623410224821[59] = 0.0; - out_6815214623410224821[60] = 0.0; - out_6815214623410224821[61] = 0.0; - out_6815214623410224821[62] = 0.0; - out_6815214623410224821[63] = 0.0; - out_6815214623410224821[64] = 0.0; - out_6815214623410224821[65] = 0.0; - out_6815214623410224821[66] = 0.0; - out_6815214623410224821[67] = 0.0; - out_6815214623410224821[68] = 0.0; - out_6815214623410224821[69] = 0.0; - out_6815214623410224821[70] = 0.0; - out_6815214623410224821[71] = 0.0; - out_6815214623410224821[72] = 0.0; - out_6815214623410224821[73] = 0.0; - out_6815214623410224821[74] = 0.0; - out_6815214623410224821[75] = 0.0; - out_6815214623410224821[76] = 1.0; - out_6815214623410224821[77] = 0.0; - out_6815214623410224821[78] = 0.0; - out_6815214623410224821[79] = 0.0; - out_6815214623410224821[80] = 0.0; - out_6815214623410224821[81] = 0.0; - out_6815214623410224821[82] = 0.0; - out_6815214623410224821[83] = 0.0; - out_6815214623410224821[84] = 0.0; - out_6815214623410224821[85] = 0.0; - out_6815214623410224821[86] = 0.0; - out_6815214623410224821[87] = 0.0; - out_6815214623410224821[88] = 0.0; - out_6815214623410224821[89] = 0.0; - out_6815214623410224821[90] = 0.0; - out_6815214623410224821[91] = 0.0; - out_6815214623410224821[92] = 0.0; - out_6815214623410224821[93] = 0.0; - out_6815214623410224821[94] = 0.0; - out_6815214623410224821[95] = 1.0; - out_6815214623410224821[96] = 0.0; - out_6815214623410224821[97] = 0.0; - out_6815214623410224821[98] = 0.0; - out_6815214623410224821[99] = 0.0; - out_6815214623410224821[100] = 0.0; - out_6815214623410224821[101] = 0.0; - out_6815214623410224821[102] = 0.0; - out_6815214623410224821[103] = 0.0; - out_6815214623410224821[104] = 0.0; - out_6815214623410224821[105] = 0.0; - out_6815214623410224821[106] = 0.0; - out_6815214623410224821[107] = 0.0; - out_6815214623410224821[108] = 0.0; - out_6815214623410224821[109] = 0.0; - out_6815214623410224821[110] = 0.0; - out_6815214623410224821[111] = 0.0; - out_6815214623410224821[112] = 0.0; - out_6815214623410224821[113] = 0.0; - out_6815214623410224821[114] = 1.0; - out_6815214623410224821[115] = 0.0; - out_6815214623410224821[116] = 0.0; - out_6815214623410224821[117] = 0.0; - out_6815214623410224821[118] = 0.0; - out_6815214623410224821[119] = 0.0; - out_6815214623410224821[120] = 0.0; - out_6815214623410224821[121] = 0.0; - out_6815214623410224821[122] = 0.0; - out_6815214623410224821[123] = 0.0; - out_6815214623410224821[124] = 0.0; - out_6815214623410224821[125] = 0.0; - out_6815214623410224821[126] = 0.0; - out_6815214623410224821[127] = 0.0; - out_6815214623410224821[128] = 0.0; - out_6815214623410224821[129] = 0.0; - out_6815214623410224821[130] = 0.0; - out_6815214623410224821[131] = 0.0; - out_6815214623410224821[132] = 0.0; - out_6815214623410224821[133] = 1.0; - out_6815214623410224821[134] = 0.0; - out_6815214623410224821[135] = 0.0; - out_6815214623410224821[136] = 0.0; - out_6815214623410224821[137] = 0.0; - out_6815214623410224821[138] = 0.0; - out_6815214623410224821[139] = 0.0; - out_6815214623410224821[140] = 0.0; - out_6815214623410224821[141] = 0.0; - out_6815214623410224821[142] = 0.0; - out_6815214623410224821[143] = 0.0; - out_6815214623410224821[144] = 0.0; - out_6815214623410224821[145] = 0.0; - out_6815214623410224821[146] = 0.0; - out_6815214623410224821[147] = 0.0; - out_6815214623410224821[148] = 0.0; - out_6815214623410224821[149] = 0.0; - out_6815214623410224821[150] = 0.0; - out_6815214623410224821[151] = 0.0; - out_6815214623410224821[152] = 1.0; - out_6815214623410224821[153] = 0.0; - out_6815214623410224821[154] = 0.0; - out_6815214623410224821[155] = 0.0; - out_6815214623410224821[156] = 0.0; - out_6815214623410224821[157] = 0.0; - out_6815214623410224821[158] = 0.0; - out_6815214623410224821[159] = 0.0; - out_6815214623410224821[160] = 0.0; - out_6815214623410224821[161] = 0.0; - out_6815214623410224821[162] = 0.0; - out_6815214623410224821[163] = 0.0; - out_6815214623410224821[164] = 0.0; - out_6815214623410224821[165] = 0.0; - out_6815214623410224821[166] = 0.0; - out_6815214623410224821[167] = 0.0; - out_6815214623410224821[168] = 0.0; - out_6815214623410224821[169] = 0.0; - out_6815214623410224821[170] = 0.0; - out_6815214623410224821[171] = 1.0; - out_6815214623410224821[172] = 0.0; - out_6815214623410224821[173] = 0.0; - out_6815214623410224821[174] = 0.0; - out_6815214623410224821[175] = 0.0; - out_6815214623410224821[176] = 0.0; - out_6815214623410224821[177] = 0.0; - out_6815214623410224821[178] = 0.0; - out_6815214623410224821[179] = 0.0; - out_6815214623410224821[180] = 0.0; - out_6815214623410224821[181] = 0.0; - out_6815214623410224821[182] = 0.0; - out_6815214623410224821[183] = 0.0; - out_6815214623410224821[184] = 0.0; - out_6815214623410224821[185] = 0.0; - out_6815214623410224821[186] = 0.0; - out_6815214623410224821[187] = 0.0; - out_6815214623410224821[188] = 0.0; - out_6815214623410224821[189] = 0.0; - out_6815214623410224821[190] = 1.0; - out_6815214623410224821[191] = 0.0; - out_6815214623410224821[192] = 0.0; - out_6815214623410224821[193] = 0.0; - out_6815214623410224821[194] = 0.0; - out_6815214623410224821[195] = 0.0; - out_6815214623410224821[196] = 0.0; - out_6815214623410224821[197] = 0.0; - out_6815214623410224821[198] = 0.0; - out_6815214623410224821[199] = 0.0; - out_6815214623410224821[200] = 0.0; - out_6815214623410224821[201] = 0.0; - out_6815214623410224821[202] = 0.0; - out_6815214623410224821[203] = 0.0; - out_6815214623410224821[204] = 0.0; - out_6815214623410224821[205] = 0.0; - out_6815214623410224821[206] = 0.0; - out_6815214623410224821[207] = 0.0; - out_6815214623410224821[208] = 0.0; - out_6815214623410224821[209] = 1.0; - out_6815214623410224821[210] = 0.0; - out_6815214623410224821[211] = 0.0; - out_6815214623410224821[212] = 0.0; - out_6815214623410224821[213] = 0.0; - out_6815214623410224821[214] = 0.0; - out_6815214623410224821[215] = 0.0; - out_6815214623410224821[216] = 0.0; - out_6815214623410224821[217] = 0.0; - out_6815214623410224821[218] = 0.0; - out_6815214623410224821[219] = 0.0; - out_6815214623410224821[220] = 0.0; - out_6815214623410224821[221] = 0.0; - out_6815214623410224821[222] = 0.0; - out_6815214623410224821[223] = 0.0; - out_6815214623410224821[224] = 0.0; - out_6815214623410224821[225] = 0.0; - out_6815214623410224821[226] = 0.0; - out_6815214623410224821[227] = 0.0; - out_6815214623410224821[228] = 1.0; - out_6815214623410224821[229] = 0.0; - out_6815214623410224821[230] = 0.0; - out_6815214623410224821[231] = 0.0; - out_6815214623410224821[232] = 0.0; - out_6815214623410224821[233] = 0.0; - out_6815214623410224821[234] = 0.0; - out_6815214623410224821[235] = 0.0; - out_6815214623410224821[236] = 0.0; - out_6815214623410224821[237] = 0.0; - out_6815214623410224821[238] = 0.0; - out_6815214623410224821[239] = 0.0; - out_6815214623410224821[240] = 0.0; - out_6815214623410224821[241] = 0.0; - out_6815214623410224821[242] = 0.0; - out_6815214623410224821[243] = 0.0; - out_6815214623410224821[244] = 0.0; - out_6815214623410224821[245] = 0.0; - out_6815214623410224821[246] = 0.0; - out_6815214623410224821[247] = 1.0; - out_6815214623410224821[248] = 0.0; - out_6815214623410224821[249] = 0.0; - out_6815214623410224821[250] = 0.0; - out_6815214623410224821[251] = 0.0; - out_6815214623410224821[252] = 0.0; - out_6815214623410224821[253] = 0.0; - out_6815214623410224821[254] = 0.0; - out_6815214623410224821[255] = 0.0; - out_6815214623410224821[256] = 0.0; - out_6815214623410224821[257] = 0.0; - out_6815214623410224821[258] = 0.0; - out_6815214623410224821[259] = 0.0; - out_6815214623410224821[260] = 0.0; - out_6815214623410224821[261] = 0.0; - out_6815214623410224821[262] = 0.0; - out_6815214623410224821[263] = 0.0; - out_6815214623410224821[264] = 0.0; - out_6815214623410224821[265] = 0.0; - out_6815214623410224821[266] = 1.0; - out_6815214623410224821[267] = 0.0; - out_6815214623410224821[268] = 0.0; - out_6815214623410224821[269] = 0.0; - out_6815214623410224821[270] = 0.0; - out_6815214623410224821[271] = 0.0; - out_6815214623410224821[272] = 0.0; - out_6815214623410224821[273] = 0.0; - out_6815214623410224821[274] = 0.0; - out_6815214623410224821[275] = 0.0; - out_6815214623410224821[276] = 0.0; - out_6815214623410224821[277] = 0.0; - out_6815214623410224821[278] = 0.0; - out_6815214623410224821[279] = 0.0; - out_6815214623410224821[280] = 0.0; - out_6815214623410224821[281] = 0.0; - out_6815214623410224821[282] = 0.0; - out_6815214623410224821[283] = 0.0; - out_6815214623410224821[284] = 0.0; - out_6815214623410224821[285] = 1.0; - out_6815214623410224821[286] = 0.0; - out_6815214623410224821[287] = 0.0; - out_6815214623410224821[288] = 0.0; - out_6815214623410224821[289] = 0.0; - out_6815214623410224821[290] = 0.0; - out_6815214623410224821[291] = 0.0; - out_6815214623410224821[292] = 0.0; - out_6815214623410224821[293] = 0.0; - out_6815214623410224821[294] = 0.0; - out_6815214623410224821[295] = 0.0; - out_6815214623410224821[296] = 0.0; - out_6815214623410224821[297] = 0.0; - out_6815214623410224821[298] = 0.0; - out_6815214623410224821[299] = 0.0; - out_6815214623410224821[300] = 0.0; - out_6815214623410224821[301] = 0.0; - out_6815214623410224821[302] = 0.0; - out_6815214623410224821[303] = 0.0; - out_6815214623410224821[304] = 1.0; - out_6815214623410224821[305] = 0.0; - out_6815214623410224821[306] = 0.0; - out_6815214623410224821[307] = 0.0; - out_6815214623410224821[308] = 0.0; - out_6815214623410224821[309] = 0.0; - out_6815214623410224821[310] = 0.0; - out_6815214623410224821[311] = 0.0; - out_6815214623410224821[312] = 0.0; - out_6815214623410224821[313] = 0.0; - out_6815214623410224821[314] = 0.0; - out_6815214623410224821[315] = 0.0; - out_6815214623410224821[316] = 0.0; - out_6815214623410224821[317] = 0.0; - out_6815214623410224821[318] = 0.0; - out_6815214623410224821[319] = 0.0; - out_6815214623410224821[320] = 0.0; - out_6815214623410224821[321] = 0.0; - out_6815214623410224821[322] = 0.0; - out_6815214623410224821[323] = 1.0; +void H_mod_fun(double *state, double *out_4718726566615345673) { + out_4718726566615345673[0] = 1.0; + out_4718726566615345673[1] = 0.0; + out_4718726566615345673[2] = 0.0; + out_4718726566615345673[3] = 0.0; + out_4718726566615345673[4] = 0.0; + out_4718726566615345673[5] = 0.0; + out_4718726566615345673[6] = 0.0; + out_4718726566615345673[7] = 0.0; + out_4718726566615345673[8] = 0.0; + out_4718726566615345673[9] = 0.0; + out_4718726566615345673[10] = 0.0; + out_4718726566615345673[11] = 0.0; + out_4718726566615345673[12] = 0.0; + out_4718726566615345673[13] = 0.0; + out_4718726566615345673[14] = 0.0; + out_4718726566615345673[15] = 0.0; + out_4718726566615345673[16] = 0.0; + out_4718726566615345673[17] = 0.0; + out_4718726566615345673[18] = 0.0; + out_4718726566615345673[19] = 1.0; + out_4718726566615345673[20] = 0.0; + out_4718726566615345673[21] = 0.0; + out_4718726566615345673[22] = 0.0; + out_4718726566615345673[23] = 0.0; + out_4718726566615345673[24] = 0.0; + out_4718726566615345673[25] = 0.0; + out_4718726566615345673[26] = 0.0; + out_4718726566615345673[27] = 0.0; + out_4718726566615345673[28] = 0.0; + out_4718726566615345673[29] = 0.0; + out_4718726566615345673[30] = 0.0; + out_4718726566615345673[31] = 0.0; + out_4718726566615345673[32] = 0.0; + out_4718726566615345673[33] = 0.0; + out_4718726566615345673[34] = 0.0; + out_4718726566615345673[35] = 0.0; + out_4718726566615345673[36] = 0.0; + out_4718726566615345673[37] = 0.0; + out_4718726566615345673[38] = 1.0; + out_4718726566615345673[39] = 0.0; + out_4718726566615345673[40] = 0.0; + out_4718726566615345673[41] = 0.0; + out_4718726566615345673[42] = 0.0; + out_4718726566615345673[43] = 0.0; + out_4718726566615345673[44] = 0.0; + out_4718726566615345673[45] = 0.0; + out_4718726566615345673[46] = 0.0; + out_4718726566615345673[47] = 0.0; + out_4718726566615345673[48] = 0.0; + out_4718726566615345673[49] = 0.0; + out_4718726566615345673[50] = 0.0; + out_4718726566615345673[51] = 0.0; + out_4718726566615345673[52] = 0.0; + out_4718726566615345673[53] = 0.0; + out_4718726566615345673[54] = 0.0; + out_4718726566615345673[55] = 0.0; + out_4718726566615345673[56] = 0.0; + out_4718726566615345673[57] = 1.0; + out_4718726566615345673[58] = 0.0; + out_4718726566615345673[59] = 0.0; + out_4718726566615345673[60] = 0.0; + out_4718726566615345673[61] = 0.0; + out_4718726566615345673[62] = 0.0; + out_4718726566615345673[63] = 0.0; + out_4718726566615345673[64] = 0.0; + out_4718726566615345673[65] = 0.0; + out_4718726566615345673[66] = 0.0; + out_4718726566615345673[67] = 0.0; + out_4718726566615345673[68] = 0.0; + out_4718726566615345673[69] = 0.0; + out_4718726566615345673[70] = 0.0; + out_4718726566615345673[71] = 0.0; + out_4718726566615345673[72] = 0.0; + out_4718726566615345673[73] = 0.0; + out_4718726566615345673[74] = 0.0; + out_4718726566615345673[75] = 0.0; + out_4718726566615345673[76] = 1.0; + out_4718726566615345673[77] = 0.0; + out_4718726566615345673[78] = 0.0; + out_4718726566615345673[79] = 0.0; + out_4718726566615345673[80] = 0.0; + out_4718726566615345673[81] = 0.0; + out_4718726566615345673[82] = 0.0; + out_4718726566615345673[83] = 0.0; + out_4718726566615345673[84] = 0.0; + out_4718726566615345673[85] = 0.0; + out_4718726566615345673[86] = 0.0; + out_4718726566615345673[87] = 0.0; + out_4718726566615345673[88] = 0.0; + out_4718726566615345673[89] = 0.0; + out_4718726566615345673[90] = 0.0; + out_4718726566615345673[91] = 0.0; + out_4718726566615345673[92] = 0.0; + out_4718726566615345673[93] = 0.0; + out_4718726566615345673[94] = 0.0; + out_4718726566615345673[95] = 1.0; + out_4718726566615345673[96] = 0.0; + out_4718726566615345673[97] = 0.0; + out_4718726566615345673[98] = 0.0; + out_4718726566615345673[99] = 0.0; + out_4718726566615345673[100] = 0.0; + out_4718726566615345673[101] = 0.0; + out_4718726566615345673[102] = 0.0; + out_4718726566615345673[103] = 0.0; + out_4718726566615345673[104] = 0.0; + out_4718726566615345673[105] = 0.0; + out_4718726566615345673[106] = 0.0; + out_4718726566615345673[107] = 0.0; + out_4718726566615345673[108] = 0.0; + out_4718726566615345673[109] = 0.0; + out_4718726566615345673[110] = 0.0; + out_4718726566615345673[111] = 0.0; + out_4718726566615345673[112] = 0.0; + out_4718726566615345673[113] = 0.0; + out_4718726566615345673[114] = 1.0; + out_4718726566615345673[115] = 0.0; + out_4718726566615345673[116] = 0.0; + out_4718726566615345673[117] = 0.0; + out_4718726566615345673[118] = 0.0; + out_4718726566615345673[119] = 0.0; + out_4718726566615345673[120] = 0.0; + out_4718726566615345673[121] = 0.0; + out_4718726566615345673[122] = 0.0; + out_4718726566615345673[123] = 0.0; + out_4718726566615345673[124] = 0.0; + out_4718726566615345673[125] = 0.0; + out_4718726566615345673[126] = 0.0; + out_4718726566615345673[127] = 0.0; + out_4718726566615345673[128] = 0.0; + out_4718726566615345673[129] = 0.0; + out_4718726566615345673[130] = 0.0; + out_4718726566615345673[131] = 0.0; + out_4718726566615345673[132] = 0.0; + out_4718726566615345673[133] = 1.0; + out_4718726566615345673[134] = 0.0; + out_4718726566615345673[135] = 0.0; + out_4718726566615345673[136] = 0.0; + out_4718726566615345673[137] = 0.0; + out_4718726566615345673[138] = 0.0; + out_4718726566615345673[139] = 0.0; + out_4718726566615345673[140] = 0.0; + out_4718726566615345673[141] = 0.0; + out_4718726566615345673[142] = 0.0; + out_4718726566615345673[143] = 0.0; + out_4718726566615345673[144] = 0.0; + out_4718726566615345673[145] = 0.0; + out_4718726566615345673[146] = 0.0; + out_4718726566615345673[147] = 0.0; + out_4718726566615345673[148] = 0.0; + out_4718726566615345673[149] = 0.0; + out_4718726566615345673[150] = 0.0; + out_4718726566615345673[151] = 0.0; + out_4718726566615345673[152] = 1.0; + out_4718726566615345673[153] = 0.0; + out_4718726566615345673[154] = 0.0; + out_4718726566615345673[155] = 0.0; + out_4718726566615345673[156] = 0.0; + out_4718726566615345673[157] = 0.0; + out_4718726566615345673[158] = 0.0; + out_4718726566615345673[159] = 0.0; + out_4718726566615345673[160] = 0.0; + out_4718726566615345673[161] = 0.0; + out_4718726566615345673[162] = 0.0; + out_4718726566615345673[163] = 0.0; + out_4718726566615345673[164] = 0.0; + out_4718726566615345673[165] = 0.0; + out_4718726566615345673[166] = 0.0; + out_4718726566615345673[167] = 0.0; + out_4718726566615345673[168] = 0.0; + out_4718726566615345673[169] = 0.0; + out_4718726566615345673[170] = 0.0; + out_4718726566615345673[171] = 1.0; + out_4718726566615345673[172] = 0.0; + out_4718726566615345673[173] = 0.0; + out_4718726566615345673[174] = 0.0; + out_4718726566615345673[175] = 0.0; + out_4718726566615345673[176] = 0.0; + out_4718726566615345673[177] = 0.0; + out_4718726566615345673[178] = 0.0; + out_4718726566615345673[179] = 0.0; + out_4718726566615345673[180] = 0.0; + out_4718726566615345673[181] = 0.0; + out_4718726566615345673[182] = 0.0; + out_4718726566615345673[183] = 0.0; + out_4718726566615345673[184] = 0.0; + out_4718726566615345673[185] = 0.0; + out_4718726566615345673[186] = 0.0; + out_4718726566615345673[187] = 0.0; + out_4718726566615345673[188] = 0.0; + out_4718726566615345673[189] = 0.0; + out_4718726566615345673[190] = 1.0; + out_4718726566615345673[191] = 0.0; + out_4718726566615345673[192] = 0.0; + out_4718726566615345673[193] = 0.0; + out_4718726566615345673[194] = 0.0; + out_4718726566615345673[195] = 0.0; + out_4718726566615345673[196] = 0.0; + out_4718726566615345673[197] = 0.0; + out_4718726566615345673[198] = 0.0; + out_4718726566615345673[199] = 0.0; + out_4718726566615345673[200] = 0.0; + out_4718726566615345673[201] = 0.0; + out_4718726566615345673[202] = 0.0; + out_4718726566615345673[203] = 0.0; + out_4718726566615345673[204] = 0.0; + out_4718726566615345673[205] = 0.0; + out_4718726566615345673[206] = 0.0; + out_4718726566615345673[207] = 0.0; + out_4718726566615345673[208] = 0.0; + out_4718726566615345673[209] = 1.0; + out_4718726566615345673[210] = 0.0; + out_4718726566615345673[211] = 0.0; + out_4718726566615345673[212] = 0.0; + out_4718726566615345673[213] = 0.0; + out_4718726566615345673[214] = 0.0; + out_4718726566615345673[215] = 0.0; + out_4718726566615345673[216] = 0.0; + out_4718726566615345673[217] = 0.0; + out_4718726566615345673[218] = 0.0; + out_4718726566615345673[219] = 0.0; + out_4718726566615345673[220] = 0.0; + out_4718726566615345673[221] = 0.0; + out_4718726566615345673[222] = 0.0; + out_4718726566615345673[223] = 0.0; + out_4718726566615345673[224] = 0.0; + out_4718726566615345673[225] = 0.0; + out_4718726566615345673[226] = 0.0; + out_4718726566615345673[227] = 0.0; + out_4718726566615345673[228] = 1.0; + out_4718726566615345673[229] = 0.0; + out_4718726566615345673[230] = 0.0; + out_4718726566615345673[231] = 0.0; + out_4718726566615345673[232] = 0.0; + out_4718726566615345673[233] = 0.0; + out_4718726566615345673[234] = 0.0; + out_4718726566615345673[235] = 0.0; + out_4718726566615345673[236] = 0.0; + out_4718726566615345673[237] = 0.0; + out_4718726566615345673[238] = 0.0; + out_4718726566615345673[239] = 0.0; + out_4718726566615345673[240] = 0.0; + out_4718726566615345673[241] = 0.0; + out_4718726566615345673[242] = 0.0; + out_4718726566615345673[243] = 0.0; + out_4718726566615345673[244] = 0.0; + out_4718726566615345673[245] = 0.0; + out_4718726566615345673[246] = 0.0; + out_4718726566615345673[247] = 1.0; + out_4718726566615345673[248] = 0.0; + out_4718726566615345673[249] = 0.0; + out_4718726566615345673[250] = 0.0; + out_4718726566615345673[251] = 0.0; + out_4718726566615345673[252] = 0.0; + out_4718726566615345673[253] = 0.0; + out_4718726566615345673[254] = 0.0; + out_4718726566615345673[255] = 0.0; + out_4718726566615345673[256] = 0.0; + out_4718726566615345673[257] = 0.0; + out_4718726566615345673[258] = 0.0; + out_4718726566615345673[259] = 0.0; + out_4718726566615345673[260] = 0.0; + out_4718726566615345673[261] = 0.0; + out_4718726566615345673[262] = 0.0; + out_4718726566615345673[263] = 0.0; + out_4718726566615345673[264] = 0.0; + out_4718726566615345673[265] = 0.0; + out_4718726566615345673[266] = 1.0; + out_4718726566615345673[267] = 0.0; + out_4718726566615345673[268] = 0.0; + out_4718726566615345673[269] = 0.0; + out_4718726566615345673[270] = 0.0; + out_4718726566615345673[271] = 0.0; + out_4718726566615345673[272] = 0.0; + out_4718726566615345673[273] = 0.0; + out_4718726566615345673[274] = 0.0; + out_4718726566615345673[275] = 0.0; + out_4718726566615345673[276] = 0.0; + out_4718726566615345673[277] = 0.0; + out_4718726566615345673[278] = 0.0; + out_4718726566615345673[279] = 0.0; + out_4718726566615345673[280] = 0.0; + out_4718726566615345673[281] = 0.0; + out_4718726566615345673[282] = 0.0; + out_4718726566615345673[283] = 0.0; + out_4718726566615345673[284] = 0.0; + out_4718726566615345673[285] = 1.0; + out_4718726566615345673[286] = 0.0; + out_4718726566615345673[287] = 0.0; + out_4718726566615345673[288] = 0.0; + out_4718726566615345673[289] = 0.0; + out_4718726566615345673[290] = 0.0; + out_4718726566615345673[291] = 0.0; + out_4718726566615345673[292] = 0.0; + out_4718726566615345673[293] = 0.0; + out_4718726566615345673[294] = 0.0; + out_4718726566615345673[295] = 0.0; + out_4718726566615345673[296] = 0.0; + out_4718726566615345673[297] = 0.0; + out_4718726566615345673[298] = 0.0; + out_4718726566615345673[299] = 0.0; + out_4718726566615345673[300] = 0.0; + out_4718726566615345673[301] = 0.0; + out_4718726566615345673[302] = 0.0; + out_4718726566615345673[303] = 0.0; + out_4718726566615345673[304] = 1.0; + out_4718726566615345673[305] = 0.0; + out_4718726566615345673[306] = 0.0; + out_4718726566615345673[307] = 0.0; + out_4718726566615345673[308] = 0.0; + out_4718726566615345673[309] = 0.0; + out_4718726566615345673[310] = 0.0; + out_4718726566615345673[311] = 0.0; + out_4718726566615345673[312] = 0.0; + out_4718726566615345673[313] = 0.0; + out_4718726566615345673[314] = 0.0; + out_4718726566615345673[315] = 0.0; + out_4718726566615345673[316] = 0.0; + out_4718726566615345673[317] = 0.0; + out_4718726566615345673[318] = 0.0; + out_4718726566615345673[319] = 0.0; + out_4718726566615345673[320] = 0.0; + out_4718726566615345673[321] = 0.0; + out_4718726566615345673[322] = 0.0; + out_4718726566615345673[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4724171785691844407) { - out_4724171785691844407[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_4724171785691844407[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_4724171785691844407[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_4724171785691844407[3] = dt*state[12] + state[3]; - out_4724171785691844407[4] = dt*state[13] + state[4]; - out_4724171785691844407[5] = dt*state[14] + state[5]; - out_4724171785691844407[6] = state[6]; - out_4724171785691844407[7] = state[7]; - out_4724171785691844407[8] = state[8]; - out_4724171785691844407[9] = state[9]; - out_4724171785691844407[10] = state[10]; - out_4724171785691844407[11] = state[11]; - out_4724171785691844407[12] = state[12]; - out_4724171785691844407[13] = state[13]; - out_4724171785691844407[14] = state[14]; - out_4724171785691844407[15] = state[15]; - out_4724171785691844407[16] = state[16]; - out_4724171785691844407[17] = state[17]; +void f_fun(double *state, double dt, double *out_2093259032196707743) { + out_2093259032196707743[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_2093259032196707743[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_2093259032196707743[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_2093259032196707743[3] = dt*state[12] + state[3]; + out_2093259032196707743[4] = dt*state[13] + state[4]; + out_2093259032196707743[5] = dt*state[14] + state[5]; + out_2093259032196707743[6] = state[6]; + out_2093259032196707743[7] = state[7]; + out_2093259032196707743[8] = state[8]; + out_2093259032196707743[9] = state[9]; + out_2093259032196707743[10] = state[10]; + out_2093259032196707743[11] = state[11]; + out_2093259032196707743[12] = state[12]; + out_2093259032196707743[13] = state[13]; + out_2093259032196707743[14] = state[14]; + out_2093259032196707743[15] = state[15]; + out_2093259032196707743[16] = state[16]; + out_2093259032196707743[17] = state[17]; } -void F_fun(double *state, double dt, double *out_638456415896569458) { - out_638456415896569458[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_638456415896569458[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_638456415896569458[2] = 0; - out_638456415896569458[3] = 0; - out_638456415896569458[4] = 0; - out_638456415896569458[5] = 0; - out_638456415896569458[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_638456415896569458[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_638456415896569458[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_638456415896569458[9] = 0; - out_638456415896569458[10] = 0; - out_638456415896569458[11] = 0; - out_638456415896569458[12] = 0; - out_638456415896569458[13] = 0; - out_638456415896569458[14] = 0; - out_638456415896569458[15] = 0; - out_638456415896569458[16] = 0; - out_638456415896569458[17] = 0; - out_638456415896569458[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_638456415896569458[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_638456415896569458[20] = 0; - out_638456415896569458[21] = 0; - out_638456415896569458[22] = 0; - out_638456415896569458[23] = 0; - out_638456415896569458[24] = 0; - out_638456415896569458[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_638456415896569458[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_638456415896569458[27] = 0; - out_638456415896569458[28] = 0; - out_638456415896569458[29] = 0; - out_638456415896569458[30] = 0; - out_638456415896569458[31] = 0; - out_638456415896569458[32] = 0; - out_638456415896569458[33] = 0; - out_638456415896569458[34] = 0; - out_638456415896569458[35] = 0; - out_638456415896569458[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_638456415896569458[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_638456415896569458[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_638456415896569458[39] = 0; - out_638456415896569458[40] = 0; - out_638456415896569458[41] = 0; - out_638456415896569458[42] = 0; - out_638456415896569458[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_638456415896569458[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_638456415896569458[45] = 0; - out_638456415896569458[46] = 0; - out_638456415896569458[47] = 0; - out_638456415896569458[48] = 0; - out_638456415896569458[49] = 0; - out_638456415896569458[50] = 0; - out_638456415896569458[51] = 0; - out_638456415896569458[52] = 0; - out_638456415896569458[53] = 0; - out_638456415896569458[54] = 0; - out_638456415896569458[55] = 0; - out_638456415896569458[56] = 0; - out_638456415896569458[57] = 1; - out_638456415896569458[58] = 0; - out_638456415896569458[59] = 0; - out_638456415896569458[60] = 0; - out_638456415896569458[61] = 0; - out_638456415896569458[62] = 0; - out_638456415896569458[63] = 0; - out_638456415896569458[64] = 0; - out_638456415896569458[65] = 0; - out_638456415896569458[66] = dt; - out_638456415896569458[67] = 0; - out_638456415896569458[68] = 0; - out_638456415896569458[69] = 0; - out_638456415896569458[70] = 0; - out_638456415896569458[71] = 0; - out_638456415896569458[72] = 0; - out_638456415896569458[73] = 0; - out_638456415896569458[74] = 0; - out_638456415896569458[75] = 0; - out_638456415896569458[76] = 1; - out_638456415896569458[77] = 0; - out_638456415896569458[78] = 0; - out_638456415896569458[79] = 0; - out_638456415896569458[80] = 0; - out_638456415896569458[81] = 0; - out_638456415896569458[82] = 0; - out_638456415896569458[83] = 0; - out_638456415896569458[84] = 0; - out_638456415896569458[85] = dt; - out_638456415896569458[86] = 0; - out_638456415896569458[87] = 0; - out_638456415896569458[88] = 0; - out_638456415896569458[89] = 0; - out_638456415896569458[90] = 0; - out_638456415896569458[91] = 0; - out_638456415896569458[92] = 0; - out_638456415896569458[93] = 0; - out_638456415896569458[94] = 0; - out_638456415896569458[95] = 1; - out_638456415896569458[96] = 0; - out_638456415896569458[97] = 0; - out_638456415896569458[98] = 0; - out_638456415896569458[99] = 0; - out_638456415896569458[100] = 0; - out_638456415896569458[101] = 0; - out_638456415896569458[102] = 0; - out_638456415896569458[103] = 0; - out_638456415896569458[104] = dt; - out_638456415896569458[105] = 0; - out_638456415896569458[106] = 0; - out_638456415896569458[107] = 0; - out_638456415896569458[108] = 0; - out_638456415896569458[109] = 0; - out_638456415896569458[110] = 0; - out_638456415896569458[111] = 0; - out_638456415896569458[112] = 0; - out_638456415896569458[113] = 0; - out_638456415896569458[114] = 1; - out_638456415896569458[115] = 0; - out_638456415896569458[116] = 0; - out_638456415896569458[117] = 0; - out_638456415896569458[118] = 0; - out_638456415896569458[119] = 0; - out_638456415896569458[120] = 0; - out_638456415896569458[121] = 0; - out_638456415896569458[122] = 0; - out_638456415896569458[123] = 0; - out_638456415896569458[124] = 0; - out_638456415896569458[125] = 0; - out_638456415896569458[126] = 0; - out_638456415896569458[127] = 0; - out_638456415896569458[128] = 0; - out_638456415896569458[129] = 0; - out_638456415896569458[130] = 0; - out_638456415896569458[131] = 0; - out_638456415896569458[132] = 0; - out_638456415896569458[133] = 1; - out_638456415896569458[134] = 0; - out_638456415896569458[135] = 0; - out_638456415896569458[136] = 0; - out_638456415896569458[137] = 0; - out_638456415896569458[138] = 0; - out_638456415896569458[139] = 0; - out_638456415896569458[140] = 0; - out_638456415896569458[141] = 0; - out_638456415896569458[142] = 0; - out_638456415896569458[143] = 0; - out_638456415896569458[144] = 0; - out_638456415896569458[145] = 0; - out_638456415896569458[146] = 0; - out_638456415896569458[147] = 0; - out_638456415896569458[148] = 0; - out_638456415896569458[149] = 0; - out_638456415896569458[150] = 0; - out_638456415896569458[151] = 0; - out_638456415896569458[152] = 1; - out_638456415896569458[153] = 0; - out_638456415896569458[154] = 0; - out_638456415896569458[155] = 0; - out_638456415896569458[156] = 0; - out_638456415896569458[157] = 0; - out_638456415896569458[158] = 0; - out_638456415896569458[159] = 0; - out_638456415896569458[160] = 0; - out_638456415896569458[161] = 0; - out_638456415896569458[162] = 0; - out_638456415896569458[163] = 0; - out_638456415896569458[164] = 0; - out_638456415896569458[165] = 0; - out_638456415896569458[166] = 0; - out_638456415896569458[167] = 0; - out_638456415896569458[168] = 0; - out_638456415896569458[169] = 0; - out_638456415896569458[170] = 0; - out_638456415896569458[171] = 1; - out_638456415896569458[172] = 0; - out_638456415896569458[173] = 0; - out_638456415896569458[174] = 0; - out_638456415896569458[175] = 0; - out_638456415896569458[176] = 0; - out_638456415896569458[177] = 0; - out_638456415896569458[178] = 0; - out_638456415896569458[179] = 0; - out_638456415896569458[180] = 0; - out_638456415896569458[181] = 0; - out_638456415896569458[182] = 0; - out_638456415896569458[183] = 0; - out_638456415896569458[184] = 0; - out_638456415896569458[185] = 0; - out_638456415896569458[186] = 0; - out_638456415896569458[187] = 0; - out_638456415896569458[188] = 0; - out_638456415896569458[189] = 0; - out_638456415896569458[190] = 1; - out_638456415896569458[191] = 0; - out_638456415896569458[192] = 0; - out_638456415896569458[193] = 0; - out_638456415896569458[194] = 0; - out_638456415896569458[195] = 0; - out_638456415896569458[196] = 0; - out_638456415896569458[197] = 0; - out_638456415896569458[198] = 0; - out_638456415896569458[199] = 0; - out_638456415896569458[200] = 0; - out_638456415896569458[201] = 0; - out_638456415896569458[202] = 0; - out_638456415896569458[203] = 0; - out_638456415896569458[204] = 0; - out_638456415896569458[205] = 0; - out_638456415896569458[206] = 0; - out_638456415896569458[207] = 0; - out_638456415896569458[208] = 0; - out_638456415896569458[209] = 1; - out_638456415896569458[210] = 0; - out_638456415896569458[211] = 0; - out_638456415896569458[212] = 0; - out_638456415896569458[213] = 0; - out_638456415896569458[214] = 0; - out_638456415896569458[215] = 0; - out_638456415896569458[216] = 0; - out_638456415896569458[217] = 0; - out_638456415896569458[218] = 0; - out_638456415896569458[219] = 0; - out_638456415896569458[220] = 0; - out_638456415896569458[221] = 0; - out_638456415896569458[222] = 0; - out_638456415896569458[223] = 0; - out_638456415896569458[224] = 0; - out_638456415896569458[225] = 0; - out_638456415896569458[226] = 0; - out_638456415896569458[227] = 0; - out_638456415896569458[228] = 1; - out_638456415896569458[229] = 0; - out_638456415896569458[230] = 0; - out_638456415896569458[231] = 0; - out_638456415896569458[232] = 0; - out_638456415896569458[233] = 0; - out_638456415896569458[234] = 0; - out_638456415896569458[235] = 0; - out_638456415896569458[236] = 0; - out_638456415896569458[237] = 0; - out_638456415896569458[238] = 0; - out_638456415896569458[239] = 0; - out_638456415896569458[240] = 0; - out_638456415896569458[241] = 0; - out_638456415896569458[242] = 0; - out_638456415896569458[243] = 0; - out_638456415896569458[244] = 0; - out_638456415896569458[245] = 0; - out_638456415896569458[246] = 0; - out_638456415896569458[247] = 1; - out_638456415896569458[248] = 0; - out_638456415896569458[249] = 0; - out_638456415896569458[250] = 0; - out_638456415896569458[251] = 0; - out_638456415896569458[252] = 0; - out_638456415896569458[253] = 0; - out_638456415896569458[254] = 0; - out_638456415896569458[255] = 0; - out_638456415896569458[256] = 0; - out_638456415896569458[257] = 0; - out_638456415896569458[258] = 0; - out_638456415896569458[259] = 0; - out_638456415896569458[260] = 0; - out_638456415896569458[261] = 0; - out_638456415896569458[262] = 0; - out_638456415896569458[263] = 0; - out_638456415896569458[264] = 0; - out_638456415896569458[265] = 0; - out_638456415896569458[266] = 1; - out_638456415896569458[267] = 0; - out_638456415896569458[268] = 0; - out_638456415896569458[269] = 0; - out_638456415896569458[270] = 0; - out_638456415896569458[271] = 0; - out_638456415896569458[272] = 0; - out_638456415896569458[273] = 0; - out_638456415896569458[274] = 0; - out_638456415896569458[275] = 0; - out_638456415896569458[276] = 0; - out_638456415896569458[277] = 0; - out_638456415896569458[278] = 0; - out_638456415896569458[279] = 0; - out_638456415896569458[280] = 0; - out_638456415896569458[281] = 0; - out_638456415896569458[282] = 0; - out_638456415896569458[283] = 0; - out_638456415896569458[284] = 0; - out_638456415896569458[285] = 1; - out_638456415896569458[286] = 0; - out_638456415896569458[287] = 0; - out_638456415896569458[288] = 0; - out_638456415896569458[289] = 0; - out_638456415896569458[290] = 0; - out_638456415896569458[291] = 0; - out_638456415896569458[292] = 0; - out_638456415896569458[293] = 0; - out_638456415896569458[294] = 0; - out_638456415896569458[295] = 0; - out_638456415896569458[296] = 0; - out_638456415896569458[297] = 0; - out_638456415896569458[298] = 0; - out_638456415896569458[299] = 0; - out_638456415896569458[300] = 0; - out_638456415896569458[301] = 0; - out_638456415896569458[302] = 0; - out_638456415896569458[303] = 0; - out_638456415896569458[304] = 1; - out_638456415896569458[305] = 0; - out_638456415896569458[306] = 0; - out_638456415896569458[307] = 0; - out_638456415896569458[308] = 0; - out_638456415896569458[309] = 0; - out_638456415896569458[310] = 0; - out_638456415896569458[311] = 0; - out_638456415896569458[312] = 0; - out_638456415896569458[313] = 0; - out_638456415896569458[314] = 0; - out_638456415896569458[315] = 0; - out_638456415896569458[316] = 0; - out_638456415896569458[317] = 0; - out_638456415896569458[318] = 0; - out_638456415896569458[319] = 0; - out_638456415896569458[320] = 0; - out_638456415896569458[321] = 0; - out_638456415896569458[322] = 0; - out_638456415896569458[323] = 1; +void F_fun(double *state, double dt, double *out_7597852127146355968) { + out_7597852127146355968[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_7597852127146355968[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_7597852127146355968[2] = 0; + out_7597852127146355968[3] = 0; + out_7597852127146355968[4] = 0; + out_7597852127146355968[5] = 0; + out_7597852127146355968[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_7597852127146355968[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_7597852127146355968[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_7597852127146355968[9] = 0; + out_7597852127146355968[10] = 0; + out_7597852127146355968[11] = 0; + out_7597852127146355968[12] = 0; + out_7597852127146355968[13] = 0; + out_7597852127146355968[14] = 0; + out_7597852127146355968[15] = 0; + out_7597852127146355968[16] = 0; + out_7597852127146355968[17] = 0; + out_7597852127146355968[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_7597852127146355968[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_7597852127146355968[20] = 0; + out_7597852127146355968[21] = 0; + out_7597852127146355968[22] = 0; + out_7597852127146355968[23] = 0; + out_7597852127146355968[24] = 0; + out_7597852127146355968[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_7597852127146355968[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_7597852127146355968[27] = 0; + out_7597852127146355968[28] = 0; + out_7597852127146355968[29] = 0; + out_7597852127146355968[30] = 0; + out_7597852127146355968[31] = 0; + out_7597852127146355968[32] = 0; + out_7597852127146355968[33] = 0; + out_7597852127146355968[34] = 0; + out_7597852127146355968[35] = 0; + out_7597852127146355968[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_7597852127146355968[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_7597852127146355968[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_7597852127146355968[39] = 0; + out_7597852127146355968[40] = 0; + out_7597852127146355968[41] = 0; + out_7597852127146355968[42] = 0; + out_7597852127146355968[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_7597852127146355968[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_7597852127146355968[45] = 0; + out_7597852127146355968[46] = 0; + out_7597852127146355968[47] = 0; + out_7597852127146355968[48] = 0; + out_7597852127146355968[49] = 0; + out_7597852127146355968[50] = 0; + out_7597852127146355968[51] = 0; + out_7597852127146355968[52] = 0; + out_7597852127146355968[53] = 0; + out_7597852127146355968[54] = 0; + out_7597852127146355968[55] = 0; + out_7597852127146355968[56] = 0; + out_7597852127146355968[57] = 1; + out_7597852127146355968[58] = 0; + out_7597852127146355968[59] = 0; + out_7597852127146355968[60] = 0; + out_7597852127146355968[61] = 0; + out_7597852127146355968[62] = 0; + out_7597852127146355968[63] = 0; + out_7597852127146355968[64] = 0; + out_7597852127146355968[65] = 0; + out_7597852127146355968[66] = dt; + out_7597852127146355968[67] = 0; + out_7597852127146355968[68] = 0; + out_7597852127146355968[69] = 0; + out_7597852127146355968[70] = 0; + out_7597852127146355968[71] = 0; + out_7597852127146355968[72] = 0; + out_7597852127146355968[73] = 0; + out_7597852127146355968[74] = 0; + out_7597852127146355968[75] = 0; + out_7597852127146355968[76] = 1; + out_7597852127146355968[77] = 0; + out_7597852127146355968[78] = 0; + out_7597852127146355968[79] = 0; + out_7597852127146355968[80] = 0; + out_7597852127146355968[81] = 0; + out_7597852127146355968[82] = 0; + out_7597852127146355968[83] = 0; + out_7597852127146355968[84] = 0; + out_7597852127146355968[85] = dt; + out_7597852127146355968[86] = 0; + out_7597852127146355968[87] = 0; + out_7597852127146355968[88] = 0; + out_7597852127146355968[89] = 0; + out_7597852127146355968[90] = 0; + out_7597852127146355968[91] = 0; + out_7597852127146355968[92] = 0; + out_7597852127146355968[93] = 0; + out_7597852127146355968[94] = 0; + out_7597852127146355968[95] = 1; + out_7597852127146355968[96] = 0; + out_7597852127146355968[97] = 0; + out_7597852127146355968[98] = 0; + out_7597852127146355968[99] = 0; + out_7597852127146355968[100] = 0; + out_7597852127146355968[101] = 0; + out_7597852127146355968[102] = 0; + out_7597852127146355968[103] = 0; + out_7597852127146355968[104] = dt; + out_7597852127146355968[105] = 0; + out_7597852127146355968[106] = 0; + out_7597852127146355968[107] = 0; + out_7597852127146355968[108] = 0; + out_7597852127146355968[109] = 0; + out_7597852127146355968[110] = 0; + out_7597852127146355968[111] = 0; + out_7597852127146355968[112] = 0; + out_7597852127146355968[113] = 0; + out_7597852127146355968[114] = 1; + out_7597852127146355968[115] = 0; + out_7597852127146355968[116] = 0; + out_7597852127146355968[117] = 0; + out_7597852127146355968[118] = 0; + out_7597852127146355968[119] = 0; + out_7597852127146355968[120] = 0; + out_7597852127146355968[121] = 0; + out_7597852127146355968[122] = 0; + out_7597852127146355968[123] = 0; + out_7597852127146355968[124] = 0; + out_7597852127146355968[125] = 0; + out_7597852127146355968[126] = 0; + out_7597852127146355968[127] = 0; + out_7597852127146355968[128] = 0; + out_7597852127146355968[129] = 0; + out_7597852127146355968[130] = 0; + out_7597852127146355968[131] = 0; + out_7597852127146355968[132] = 0; + out_7597852127146355968[133] = 1; + out_7597852127146355968[134] = 0; + out_7597852127146355968[135] = 0; + out_7597852127146355968[136] = 0; + out_7597852127146355968[137] = 0; + out_7597852127146355968[138] = 0; + out_7597852127146355968[139] = 0; + out_7597852127146355968[140] = 0; + out_7597852127146355968[141] = 0; + out_7597852127146355968[142] = 0; + out_7597852127146355968[143] = 0; + out_7597852127146355968[144] = 0; + out_7597852127146355968[145] = 0; + out_7597852127146355968[146] = 0; + out_7597852127146355968[147] = 0; + out_7597852127146355968[148] = 0; + out_7597852127146355968[149] = 0; + out_7597852127146355968[150] = 0; + out_7597852127146355968[151] = 0; + out_7597852127146355968[152] = 1; + out_7597852127146355968[153] = 0; + out_7597852127146355968[154] = 0; + out_7597852127146355968[155] = 0; + out_7597852127146355968[156] = 0; + out_7597852127146355968[157] = 0; + out_7597852127146355968[158] = 0; + out_7597852127146355968[159] = 0; + out_7597852127146355968[160] = 0; + out_7597852127146355968[161] = 0; + out_7597852127146355968[162] = 0; + out_7597852127146355968[163] = 0; + out_7597852127146355968[164] = 0; + out_7597852127146355968[165] = 0; + out_7597852127146355968[166] = 0; + out_7597852127146355968[167] = 0; + out_7597852127146355968[168] = 0; + out_7597852127146355968[169] = 0; + out_7597852127146355968[170] = 0; + out_7597852127146355968[171] = 1; + out_7597852127146355968[172] = 0; + out_7597852127146355968[173] = 0; + out_7597852127146355968[174] = 0; + out_7597852127146355968[175] = 0; + out_7597852127146355968[176] = 0; + out_7597852127146355968[177] = 0; + out_7597852127146355968[178] = 0; + out_7597852127146355968[179] = 0; + out_7597852127146355968[180] = 0; + out_7597852127146355968[181] = 0; + out_7597852127146355968[182] = 0; + out_7597852127146355968[183] = 0; + out_7597852127146355968[184] = 0; + out_7597852127146355968[185] = 0; + out_7597852127146355968[186] = 0; + out_7597852127146355968[187] = 0; + out_7597852127146355968[188] = 0; + out_7597852127146355968[189] = 0; + out_7597852127146355968[190] = 1; + out_7597852127146355968[191] = 0; + out_7597852127146355968[192] = 0; + out_7597852127146355968[193] = 0; + out_7597852127146355968[194] = 0; + out_7597852127146355968[195] = 0; + out_7597852127146355968[196] = 0; + out_7597852127146355968[197] = 0; + out_7597852127146355968[198] = 0; + out_7597852127146355968[199] = 0; + out_7597852127146355968[200] = 0; + out_7597852127146355968[201] = 0; + out_7597852127146355968[202] = 0; + out_7597852127146355968[203] = 0; + out_7597852127146355968[204] = 0; + out_7597852127146355968[205] = 0; + out_7597852127146355968[206] = 0; + out_7597852127146355968[207] = 0; + out_7597852127146355968[208] = 0; + out_7597852127146355968[209] = 1; + out_7597852127146355968[210] = 0; + out_7597852127146355968[211] = 0; + out_7597852127146355968[212] = 0; + out_7597852127146355968[213] = 0; + out_7597852127146355968[214] = 0; + out_7597852127146355968[215] = 0; + out_7597852127146355968[216] = 0; + out_7597852127146355968[217] = 0; + out_7597852127146355968[218] = 0; + out_7597852127146355968[219] = 0; + out_7597852127146355968[220] = 0; + out_7597852127146355968[221] = 0; + out_7597852127146355968[222] = 0; + out_7597852127146355968[223] = 0; + out_7597852127146355968[224] = 0; + out_7597852127146355968[225] = 0; + out_7597852127146355968[226] = 0; + out_7597852127146355968[227] = 0; + out_7597852127146355968[228] = 1; + out_7597852127146355968[229] = 0; + out_7597852127146355968[230] = 0; + out_7597852127146355968[231] = 0; + out_7597852127146355968[232] = 0; + out_7597852127146355968[233] = 0; + out_7597852127146355968[234] = 0; + out_7597852127146355968[235] = 0; + out_7597852127146355968[236] = 0; + out_7597852127146355968[237] = 0; + out_7597852127146355968[238] = 0; + out_7597852127146355968[239] = 0; + out_7597852127146355968[240] = 0; + out_7597852127146355968[241] = 0; + out_7597852127146355968[242] = 0; + out_7597852127146355968[243] = 0; + out_7597852127146355968[244] = 0; + out_7597852127146355968[245] = 0; + out_7597852127146355968[246] = 0; + out_7597852127146355968[247] = 1; + out_7597852127146355968[248] = 0; + out_7597852127146355968[249] = 0; + out_7597852127146355968[250] = 0; + out_7597852127146355968[251] = 0; + out_7597852127146355968[252] = 0; + out_7597852127146355968[253] = 0; + out_7597852127146355968[254] = 0; + out_7597852127146355968[255] = 0; + out_7597852127146355968[256] = 0; + out_7597852127146355968[257] = 0; + out_7597852127146355968[258] = 0; + out_7597852127146355968[259] = 0; + out_7597852127146355968[260] = 0; + out_7597852127146355968[261] = 0; + out_7597852127146355968[262] = 0; + out_7597852127146355968[263] = 0; + out_7597852127146355968[264] = 0; + out_7597852127146355968[265] = 0; + out_7597852127146355968[266] = 1; + out_7597852127146355968[267] = 0; + out_7597852127146355968[268] = 0; + out_7597852127146355968[269] = 0; + out_7597852127146355968[270] = 0; + out_7597852127146355968[271] = 0; + out_7597852127146355968[272] = 0; + out_7597852127146355968[273] = 0; + out_7597852127146355968[274] = 0; + out_7597852127146355968[275] = 0; + out_7597852127146355968[276] = 0; + out_7597852127146355968[277] = 0; + out_7597852127146355968[278] = 0; + out_7597852127146355968[279] = 0; + out_7597852127146355968[280] = 0; + out_7597852127146355968[281] = 0; + out_7597852127146355968[282] = 0; + out_7597852127146355968[283] = 0; + out_7597852127146355968[284] = 0; + out_7597852127146355968[285] = 1; + out_7597852127146355968[286] = 0; + out_7597852127146355968[287] = 0; + out_7597852127146355968[288] = 0; + out_7597852127146355968[289] = 0; + out_7597852127146355968[290] = 0; + out_7597852127146355968[291] = 0; + out_7597852127146355968[292] = 0; + out_7597852127146355968[293] = 0; + out_7597852127146355968[294] = 0; + out_7597852127146355968[295] = 0; + out_7597852127146355968[296] = 0; + out_7597852127146355968[297] = 0; + out_7597852127146355968[298] = 0; + out_7597852127146355968[299] = 0; + out_7597852127146355968[300] = 0; + out_7597852127146355968[301] = 0; + out_7597852127146355968[302] = 0; + out_7597852127146355968[303] = 0; + out_7597852127146355968[304] = 1; + out_7597852127146355968[305] = 0; + out_7597852127146355968[306] = 0; + out_7597852127146355968[307] = 0; + out_7597852127146355968[308] = 0; + out_7597852127146355968[309] = 0; + out_7597852127146355968[310] = 0; + out_7597852127146355968[311] = 0; + out_7597852127146355968[312] = 0; + out_7597852127146355968[313] = 0; + out_7597852127146355968[314] = 0; + out_7597852127146355968[315] = 0; + out_7597852127146355968[316] = 0; + out_7597852127146355968[317] = 0; + out_7597852127146355968[318] = 0; + out_7597852127146355968[319] = 0; + out_7597852127146355968[320] = 0; + out_7597852127146355968[321] = 0; + out_7597852127146355968[322] = 0; + out_7597852127146355968[323] = 1; } -void h_4(double *state, double *unused, double *out_3715383509872023288) { - out_3715383509872023288[0] = state[6] + state[9]; - out_3715383509872023288[1] = state[7] + state[10]; - out_3715383509872023288[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_89080864518885864) { + out_89080864518885864[0] = state[6] + state[9]; + out_89080864518885864[1] = state[7] + state[10]; + out_89080864518885864[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_7859722240455419223) { - out_7859722240455419223[0] = 0; - out_7859722240455419223[1] = 0; - out_7859722240455419223[2] = 0; - out_7859722240455419223[3] = 0; - out_7859722240455419223[4] = 0; - out_7859722240455419223[5] = 0; - out_7859722240455419223[6] = 1; - out_7859722240455419223[7] = 0; - out_7859722240455419223[8] = 0; - out_7859722240455419223[9] = 1; - out_7859722240455419223[10] = 0; - out_7859722240455419223[11] = 0; - out_7859722240455419223[12] = 0; - out_7859722240455419223[13] = 0; - out_7859722240455419223[14] = 0; - out_7859722240455419223[15] = 0; - out_7859722240455419223[16] = 0; - out_7859722240455419223[17] = 0; - out_7859722240455419223[18] = 0; - out_7859722240455419223[19] = 0; - out_7859722240455419223[20] = 0; - out_7859722240455419223[21] = 0; - out_7859722240455419223[22] = 0; - out_7859722240455419223[23] = 0; - out_7859722240455419223[24] = 0; - out_7859722240455419223[25] = 1; - out_7859722240455419223[26] = 0; - out_7859722240455419223[27] = 0; - out_7859722240455419223[28] = 1; - out_7859722240455419223[29] = 0; - out_7859722240455419223[30] = 0; - out_7859722240455419223[31] = 0; - out_7859722240455419223[32] = 0; - out_7859722240455419223[33] = 0; - out_7859722240455419223[34] = 0; - out_7859722240455419223[35] = 0; - out_7859722240455419223[36] = 0; - out_7859722240455419223[37] = 0; - out_7859722240455419223[38] = 0; - out_7859722240455419223[39] = 0; - out_7859722240455419223[40] = 0; - out_7859722240455419223[41] = 0; - out_7859722240455419223[42] = 0; - out_7859722240455419223[43] = 0; - out_7859722240455419223[44] = 1; - out_7859722240455419223[45] = 0; - out_7859722240455419223[46] = 0; - out_7859722240455419223[47] = 1; - out_7859722240455419223[48] = 0; - out_7859722240455419223[49] = 0; - out_7859722240455419223[50] = 0; - out_7859722240455419223[51] = 0; - out_7859722240455419223[52] = 0; - out_7859722240455419223[53] = 0; +void H_4(double *state, double *unused, double *out_946919356771438101) { + out_946919356771438101[0] = 0; + out_946919356771438101[1] = 0; + out_946919356771438101[2] = 0; + out_946919356771438101[3] = 0; + out_946919356771438101[4] = 0; + out_946919356771438101[5] = 0; + out_946919356771438101[6] = 1; + out_946919356771438101[7] = 0; + out_946919356771438101[8] = 0; + out_946919356771438101[9] = 1; + out_946919356771438101[10] = 0; + out_946919356771438101[11] = 0; + out_946919356771438101[12] = 0; + out_946919356771438101[13] = 0; + out_946919356771438101[14] = 0; + out_946919356771438101[15] = 0; + out_946919356771438101[16] = 0; + out_946919356771438101[17] = 0; + out_946919356771438101[18] = 0; + out_946919356771438101[19] = 0; + out_946919356771438101[20] = 0; + out_946919356771438101[21] = 0; + out_946919356771438101[22] = 0; + out_946919356771438101[23] = 0; + out_946919356771438101[24] = 0; + out_946919356771438101[25] = 1; + out_946919356771438101[26] = 0; + out_946919356771438101[27] = 0; + out_946919356771438101[28] = 1; + out_946919356771438101[29] = 0; + out_946919356771438101[30] = 0; + out_946919356771438101[31] = 0; + out_946919356771438101[32] = 0; + out_946919356771438101[33] = 0; + out_946919356771438101[34] = 0; + out_946919356771438101[35] = 0; + out_946919356771438101[36] = 0; + out_946919356771438101[37] = 0; + out_946919356771438101[38] = 0; + out_946919356771438101[39] = 0; + out_946919356771438101[40] = 0; + out_946919356771438101[41] = 0; + out_946919356771438101[42] = 0; + out_946919356771438101[43] = 0; + out_946919356771438101[44] = 1; + out_946919356771438101[45] = 0; + out_946919356771438101[46] = 0; + out_946919356771438101[47] = 1; + out_946919356771438101[48] = 0; + out_946919356771438101[49] = 0; + out_946919356771438101[50] = 0; + out_946919356771438101[51] = 0; + out_946919356771438101[52] = 0; + out_946919356771438101[53] = 0; } -void h_10(double *state, double *unused, double *out_1493881979826544307) { - out_1493881979826544307[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_1493881979826544307[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_1493881979826544307[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_2699740651616676276) { + out_2699740651616676276[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_2699740651616676276[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_2699740651616676276[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_8226181140571901267) { - out_8226181140571901267[0] = 0; - out_8226181140571901267[1] = 9.8100000000000005*cos(state[1]); - out_8226181140571901267[2] = 0; - out_8226181140571901267[3] = 0; - out_8226181140571901267[4] = -state[8]; - out_8226181140571901267[5] = state[7]; - out_8226181140571901267[6] = 0; - out_8226181140571901267[7] = state[5]; - out_8226181140571901267[8] = -state[4]; - out_8226181140571901267[9] = 0; - out_8226181140571901267[10] = 0; - out_8226181140571901267[11] = 0; - out_8226181140571901267[12] = 1; - out_8226181140571901267[13] = 0; - out_8226181140571901267[14] = 0; - out_8226181140571901267[15] = 1; - out_8226181140571901267[16] = 0; - out_8226181140571901267[17] = 0; - out_8226181140571901267[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_8226181140571901267[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_8226181140571901267[20] = 0; - out_8226181140571901267[21] = state[8]; - out_8226181140571901267[22] = 0; - out_8226181140571901267[23] = -state[6]; - out_8226181140571901267[24] = -state[5]; - out_8226181140571901267[25] = 0; - out_8226181140571901267[26] = state[3]; - out_8226181140571901267[27] = 0; - out_8226181140571901267[28] = 0; - out_8226181140571901267[29] = 0; - out_8226181140571901267[30] = 0; - out_8226181140571901267[31] = 1; - out_8226181140571901267[32] = 0; - out_8226181140571901267[33] = 0; - out_8226181140571901267[34] = 1; - out_8226181140571901267[35] = 0; - out_8226181140571901267[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_8226181140571901267[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_8226181140571901267[38] = 0; - out_8226181140571901267[39] = -state[7]; - out_8226181140571901267[40] = state[6]; - out_8226181140571901267[41] = 0; - out_8226181140571901267[42] = state[4]; - out_8226181140571901267[43] = -state[3]; - out_8226181140571901267[44] = 0; - out_8226181140571901267[45] = 0; - out_8226181140571901267[46] = 0; - out_8226181140571901267[47] = 0; - out_8226181140571901267[48] = 0; - out_8226181140571901267[49] = 0; - out_8226181140571901267[50] = 1; - out_8226181140571901267[51] = 0; - out_8226181140571901267[52] = 0; - out_8226181140571901267[53] = 1; +void H_10(double *state, double *unused, double *out_8679455089612648559) { + out_8679455089612648559[0] = 0; + out_8679455089612648559[1] = 9.8100000000000005*cos(state[1]); + out_8679455089612648559[2] = 0; + out_8679455089612648559[3] = 0; + out_8679455089612648559[4] = -state[8]; + out_8679455089612648559[5] = state[7]; + out_8679455089612648559[6] = 0; + out_8679455089612648559[7] = state[5]; + out_8679455089612648559[8] = -state[4]; + out_8679455089612648559[9] = 0; + out_8679455089612648559[10] = 0; + out_8679455089612648559[11] = 0; + out_8679455089612648559[12] = 1; + out_8679455089612648559[13] = 0; + out_8679455089612648559[14] = 0; + out_8679455089612648559[15] = 1; + out_8679455089612648559[16] = 0; + out_8679455089612648559[17] = 0; + out_8679455089612648559[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_8679455089612648559[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_8679455089612648559[20] = 0; + out_8679455089612648559[21] = state[8]; + out_8679455089612648559[22] = 0; + out_8679455089612648559[23] = -state[6]; + out_8679455089612648559[24] = -state[5]; + out_8679455089612648559[25] = 0; + out_8679455089612648559[26] = state[3]; + out_8679455089612648559[27] = 0; + out_8679455089612648559[28] = 0; + out_8679455089612648559[29] = 0; + out_8679455089612648559[30] = 0; + out_8679455089612648559[31] = 1; + out_8679455089612648559[32] = 0; + out_8679455089612648559[33] = 0; + out_8679455089612648559[34] = 1; + out_8679455089612648559[35] = 0; + out_8679455089612648559[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_8679455089612648559[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_8679455089612648559[38] = 0; + out_8679455089612648559[39] = -state[7]; + out_8679455089612648559[40] = state[6]; + out_8679455089612648559[41] = 0; + out_8679455089612648559[42] = state[4]; + out_8679455089612648559[43] = -state[3]; + out_8679455089612648559[44] = 0; + out_8679455089612648559[45] = 0; + out_8679455089612648559[46] = 0; + out_8679455089612648559[47] = 0; + out_8679455089612648559[48] = 0; + out_8679455089612648559[49] = 0; + out_8679455089612648559[50] = 1; + out_8679455089612648559[51] = 0; + out_8679455089612648559[52] = 0; + out_8679455089612648559[53] = 1; } -void h_13(double *state, double *unused, double *out_820596947016455055) { - out_820596947016455055[0] = state[3]; - out_820596947016455055[1] = state[4]; - out_820596947016455055[2] = state[5]; +void h_13(double *state, double *unused, double *out_622335723485041042) { + out_622335723485041042[0] = state[3]; + out_622335723485041042[1] = state[4]; + out_622335723485041042[2] = state[5]; } -void H_13(double *state, double *unused, double *out_4647448415123086422) { - out_4647448415123086422[0] = 0; - out_4647448415123086422[1] = 0; - out_4647448415123086422[2] = 0; - out_4647448415123086422[3] = 1; - out_4647448415123086422[4] = 0; - out_4647448415123086422[5] = 0; - out_4647448415123086422[6] = 0; - out_4647448415123086422[7] = 0; - out_4647448415123086422[8] = 0; - out_4647448415123086422[9] = 0; - out_4647448415123086422[10] = 0; - out_4647448415123086422[11] = 0; - out_4647448415123086422[12] = 0; - out_4647448415123086422[13] = 0; - out_4647448415123086422[14] = 0; - out_4647448415123086422[15] = 0; - out_4647448415123086422[16] = 0; - out_4647448415123086422[17] = 0; - out_4647448415123086422[18] = 0; - out_4647448415123086422[19] = 0; - out_4647448415123086422[20] = 0; - out_4647448415123086422[21] = 0; - out_4647448415123086422[22] = 1; - out_4647448415123086422[23] = 0; - out_4647448415123086422[24] = 0; - out_4647448415123086422[25] = 0; - out_4647448415123086422[26] = 0; - out_4647448415123086422[27] = 0; - out_4647448415123086422[28] = 0; - out_4647448415123086422[29] = 0; - out_4647448415123086422[30] = 0; - out_4647448415123086422[31] = 0; - out_4647448415123086422[32] = 0; - out_4647448415123086422[33] = 0; - out_4647448415123086422[34] = 0; - out_4647448415123086422[35] = 0; - out_4647448415123086422[36] = 0; - out_4647448415123086422[37] = 0; - out_4647448415123086422[38] = 0; - out_4647448415123086422[39] = 0; - out_4647448415123086422[40] = 0; - out_4647448415123086422[41] = 1; - out_4647448415123086422[42] = 0; - out_4647448415123086422[43] = 0; - out_4647448415123086422[44] = 0; - out_4647448415123086422[45] = 0; - out_4647448415123086422[46] = 0; - out_4647448415123086422[47] = 0; - out_4647448415123086422[48] = 0; - out_4647448415123086422[49] = 0; - out_4647448415123086422[50] = 0; - out_4647448415123086422[51] = 0; - out_4647448415123086422[52] = 0; - out_4647448415123086422[53] = 0; +void H_13(double *state, double *unused, double *out_2265354468560894700) { + out_2265354468560894700[0] = 0; + out_2265354468560894700[1] = 0; + out_2265354468560894700[2] = 0; + out_2265354468560894700[3] = 1; + out_2265354468560894700[4] = 0; + out_2265354468560894700[5] = 0; + out_2265354468560894700[6] = 0; + out_2265354468560894700[7] = 0; + out_2265354468560894700[8] = 0; + out_2265354468560894700[9] = 0; + out_2265354468560894700[10] = 0; + out_2265354468560894700[11] = 0; + out_2265354468560894700[12] = 0; + out_2265354468560894700[13] = 0; + out_2265354468560894700[14] = 0; + out_2265354468560894700[15] = 0; + out_2265354468560894700[16] = 0; + out_2265354468560894700[17] = 0; + out_2265354468560894700[18] = 0; + out_2265354468560894700[19] = 0; + out_2265354468560894700[20] = 0; + out_2265354468560894700[21] = 0; + out_2265354468560894700[22] = 1; + out_2265354468560894700[23] = 0; + out_2265354468560894700[24] = 0; + out_2265354468560894700[25] = 0; + out_2265354468560894700[26] = 0; + out_2265354468560894700[27] = 0; + out_2265354468560894700[28] = 0; + out_2265354468560894700[29] = 0; + out_2265354468560894700[30] = 0; + out_2265354468560894700[31] = 0; + out_2265354468560894700[32] = 0; + out_2265354468560894700[33] = 0; + out_2265354468560894700[34] = 0; + out_2265354468560894700[35] = 0; + out_2265354468560894700[36] = 0; + out_2265354468560894700[37] = 0; + out_2265354468560894700[38] = 0; + out_2265354468560894700[39] = 0; + out_2265354468560894700[40] = 0; + out_2265354468560894700[41] = 1; + out_2265354468560894700[42] = 0; + out_2265354468560894700[43] = 0; + out_2265354468560894700[44] = 0; + out_2265354468560894700[45] = 0; + out_2265354468560894700[46] = 0; + out_2265354468560894700[47] = 0; + out_2265354468560894700[48] = 0; + out_2265354468560894700[49] = 0; + out_2265354468560894700[50] = 0; + out_2265354468560894700[51] = 0; + out_2265354468560894700[52] = 0; + out_2265354468560894700[53] = 0; } -void h_14(double *state, double *unused, double *out_7488710011373447915) { - out_7488710011373447915[0] = state[6]; - out_7488710011373447915[1] = state[7]; - out_7488710011373447915[2] = state[8]; +void h_14(double *state, double *unused, double *out_299516174209740344) { + out_299516174209740344[0] = state[6]; + out_299516174209740344[1] = state[7]; + out_299516174209740344[2] = state[8]; } -void H_14(double *state, double *unused, double *out_3896481384115934694) { - out_3896481384115934694[0] = 0; - out_3896481384115934694[1] = 0; - out_3896481384115934694[2] = 0; - out_3896481384115934694[3] = 0; - out_3896481384115934694[4] = 0; - out_3896481384115934694[5] = 0; - out_3896481384115934694[6] = 1; - out_3896481384115934694[7] = 0; - out_3896481384115934694[8] = 0; - out_3896481384115934694[9] = 0; - out_3896481384115934694[10] = 0; - out_3896481384115934694[11] = 0; - out_3896481384115934694[12] = 0; - out_3896481384115934694[13] = 0; - out_3896481384115934694[14] = 0; - out_3896481384115934694[15] = 0; - out_3896481384115934694[16] = 0; - out_3896481384115934694[17] = 0; - out_3896481384115934694[18] = 0; - out_3896481384115934694[19] = 0; - out_3896481384115934694[20] = 0; - out_3896481384115934694[21] = 0; - out_3896481384115934694[22] = 0; - out_3896481384115934694[23] = 0; - out_3896481384115934694[24] = 0; - out_3896481384115934694[25] = 1; - out_3896481384115934694[26] = 0; - out_3896481384115934694[27] = 0; - out_3896481384115934694[28] = 0; - out_3896481384115934694[29] = 0; - out_3896481384115934694[30] = 0; - out_3896481384115934694[31] = 0; - out_3896481384115934694[32] = 0; - out_3896481384115934694[33] = 0; - out_3896481384115934694[34] = 0; - out_3896481384115934694[35] = 0; - out_3896481384115934694[36] = 0; - out_3896481384115934694[37] = 0; - out_3896481384115934694[38] = 0; - out_3896481384115934694[39] = 0; - out_3896481384115934694[40] = 0; - out_3896481384115934694[41] = 0; - out_3896481384115934694[42] = 0; - out_3896481384115934694[43] = 0; - out_3896481384115934694[44] = 1; - out_3896481384115934694[45] = 0; - out_3896481384115934694[46] = 0; - out_3896481384115934694[47] = 0; - out_3896481384115934694[48] = 0; - out_3896481384115934694[49] = 0; - out_3896481384115934694[50] = 0; - out_3896481384115934694[51] = 0; - out_3896481384115934694[52] = 0; - out_3896481384115934694[53] = 0; +void H_14(double *state, double *unused, double *out_3016321499568046428) { + out_3016321499568046428[0] = 0; + out_3016321499568046428[1] = 0; + out_3016321499568046428[2] = 0; + out_3016321499568046428[3] = 0; + out_3016321499568046428[4] = 0; + out_3016321499568046428[5] = 0; + out_3016321499568046428[6] = 1; + out_3016321499568046428[7] = 0; + out_3016321499568046428[8] = 0; + out_3016321499568046428[9] = 0; + out_3016321499568046428[10] = 0; + out_3016321499568046428[11] = 0; + out_3016321499568046428[12] = 0; + out_3016321499568046428[13] = 0; + out_3016321499568046428[14] = 0; + out_3016321499568046428[15] = 0; + out_3016321499568046428[16] = 0; + out_3016321499568046428[17] = 0; + out_3016321499568046428[18] = 0; + out_3016321499568046428[19] = 0; + out_3016321499568046428[20] = 0; + out_3016321499568046428[21] = 0; + out_3016321499568046428[22] = 0; + out_3016321499568046428[23] = 0; + out_3016321499568046428[24] = 0; + out_3016321499568046428[25] = 1; + out_3016321499568046428[26] = 0; + out_3016321499568046428[27] = 0; + out_3016321499568046428[28] = 0; + out_3016321499568046428[29] = 0; + out_3016321499568046428[30] = 0; + out_3016321499568046428[31] = 0; + out_3016321499568046428[32] = 0; + out_3016321499568046428[33] = 0; + out_3016321499568046428[34] = 0; + out_3016321499568046428[35] = 0; + out_3016321499568046428[36] = 0; + out_3016321499568046428[37] = 0; + out_3016321499568046428[38] = 0; + out_3016321499568046428[39] = 0; + out_3016321499568046428[40] = 0; + out_3016321499568046428[41] = 0; + out_3016321499568046428[42] = 0; + out_3016321499568046428[43] = 0; + out_3016321499568046428[44] = 1; + out_3016321499568046428[45] = 0; + out_3016321499568046428[46] = 0; + out_3016321499568046428[47] = 0; + out_3016321499568046428[48] = 0; + out_3016321499568046428[49] = 0; + out_3016321499568046428[50] = 0; + out_3016321499568046428[51] = 0; + out_3016321499568046428[52] = 0; + out_3016321499568046428[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_3650099251769799718) { - err_fun(nom_x, delta_x, out_3650099251769799718); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4833077353004324467) { + err_fun(nom_x, delta_x, out_4833077353004324467); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4549899801136771746) { - inv_err_fun(nom_x, true_x, out_4549899801136771746); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4980437301453189923) { + inv_err_fun(nom_x, true_x, out_4980437301453189923); } -void pose_H_mod_fun(double *state, double *out_6815214623410224821) { - H_mod_fun(state, out_6815214623410224821); +void pose_H_mod_fun(double *state, double *out_4718726566615345673) { + H_mod_fun(state, out_4718726566615345673); } -void pose_f_fun(double *state, double dt, double *out_4724171785691844407) { - f_fun(state, dt, out_4724171785691844407); +void pose_f_fun(double *state, double dt, double *out_2093259032196707743) { + f_fun(state, dt, out_2093259032196707743); } -void pose_F_fun(double *state, double dt, double *out_638456415896569458) { - F_fun(state, dt, out_638456415896569458); +void pose_F_fun(double *state, double dt, double *out_7597852127146355968) { + F_fun(state, dt, out_7597852127146355968); } -void pose_h_4(double *state, double *unused, double *out_3715383509872023288) { - h_4(state, unused, out_3715383509872023288); +void pose_h_4(double *state, double *unused, double *out_89080864518885864) { + h_4(state, unused, out_89080864518885864); } -void pose_H_4(double *state, double *unused, double *out_7859722240455419223) { - H_4(state, unused, out_7859722240455419223); +void pose_H_4(double *state, double *unused, double *out_946919356771438101) { + H_4(state, unused, out_946919356771438101); } -void pose_h_10(double *state, double *unused, double *out_1493881979826544307) { - h_10(state, unused, out_1493881979826544307); +void pose_h_10(double *state, double *unused, double *out_2699740651616676276) { + h_10(state, unused, out_2699740651616676276); } -void pose_H_10(double *state, double *unused, double *out_8226181140571901267) { - H_10(state, unused, out_8226181140571901267); +void pose_H_10(double *state, double *unused, double *out_8679455089612648559) { + H_10(state, unused, out_8679455089612648559); } -void pose_h_13(double *state, double *unused, double *out_820596947016455055) { - h_13(state, unused, out_820596947016455055); +void pose_h_13(double *state, double *unused, double *out_622335723485041042) { + h_13(state, unused, out_622335723485041042); } -void pose_H_13(double *state, double *unused, double *out_4647448415123086422) { - H_13(state, unused, out_4647448415123086422); +void pose_H_13(double *state, double *unused, double *out_2265354468560894700) { + H_13(state, unused, out_2265354468560894700); } -void pose_h_14(double *state, double *unused, double *out_7488710011373447915) { - h_14(state, unused, out_7488710011373447915); +void pose_h_14(double *state, double *unused, double *out_299516174209740344) { + h_14(state, unused, out_299516174209740344); } -void pose_H_14(double *state, double *unused, double *out_3896481384115934694) { - H_14(state, unused, out_3896481384115934694); +void pose_H_14(double *state, double *unused, double *out_3016321499568046428) { + H_14(state, unused, out_3016321499568046428); } 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 d782dc726..8f8c8c533 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_3650099251769799718); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4549899801136771746); -void pose_H_mod_fun(double *state, double *out_6815214623410224821); -void pose_f_fun(double *state, double dt, double *out_4724171785691844407); -void pose_F_fun(double *state, double dt, double *out_638456415896569458); -void pose_h_4(double *state, double *unused, double *out_3715383509872023288); -void pose_H_4(double *state, double *unused, double *out_7859722240455419223); -void pose_h_10(double *state, double *unused, double *out_1493881979826544307); -void pose_H_10(double *state, double *unused, double *out_8226181140571901267); -void pose_h_13(double *state, double *unused, double *out_820596947016455055); -void pose_H_13(double *state, double *unused, double *out_4647448415123086422); -void pose_h_14(double *state, double *unused, double *out_7488710011373447915); -void pose_H_14(double *state, double *unused, double *out_3896481384115934694); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4833077353004324467); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4980437301453189923); +void pose_H_mod_fun(double *state, double *out_4718726566615345673); +void pose_f_fun(double *state, double dt, double *out_2093259032196707743); +void pose_F_fun(double *state, double dt, double *out_7597852127146355968); +void pose_h_4(double *state, double *unused, double *out_89080864518885864); +void pose_H_4(double *state, double *unused, double *out_946919356771438101); +void pose_h_10(double *state, double *unused, double *out_2699740651616676276); +void pose_H_10(double *state, double *unused, double *out_8679455089612648559); +void pose_h_13(double *state, double *unused, double *out_622335723485041042); +void pose_H_13(double *state, double *unused, double *out_2265354468560894700); +void pose_h_14(double *state, double *unused, double *out_299516174209740344); +void pose_H_14(double *state, double *unused, double *out_3016321499568046428); 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 05d7ccaf2..e796f620c 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index b870a7090..582c7144a 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 118f0da00..911729054 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 8a0b327a3..9f056f95d 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 7858d22a5..8f42f98fc 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index a7c7dcb88..b9626588d 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ