diff --git a/common/libcommon.a b/common/libcommon.a index 5ed0eacdc..e96bbf07e 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 ff164851a..f8206f032 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 07c4d2be6..ba8586098 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 a5ed97a19..60704c763 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 2cccf5882..7b942223f 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 f4d64cc90..6786f753f 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 08d2fc9a5..95b025624 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 9e25af20e..0d70c7829 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 b36931b2b..16d2c03d6 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 b36931b2b..16d2c03d6 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 c215cf5ad..620ff08e3 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 9e25af20e..0d70c7829 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 b9e6ea572..0e678651d 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-a6758eb9-DEBUG"; +const uint8_t gitversion[19] = "DEV-bec246c6-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index efb3df3cb..e753bd02d 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 06b961287..1f9b8029d 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 de2cfa972..d4dc20dde 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 21b298e7c..951965022 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 366ea201c..d745af951 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 28c0c455a..38df0060b 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 0b4c45251..765776fc7 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 3ef28d1d7..59d7ab3a8 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 17518ffaf..03dacca6d 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 e10fb434b..22e68c15d 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 fa7f634e7..f0e7cd7ba 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 7426bda24..1cae10572 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 02500de02..76b2edd5f 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 dcf6fc694..cb2de0438 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 fc5770fbb..a30f1386d 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 ee913b0bb..99f852b3c 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 d327fdf1d..841d0a482 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 877ce6665..cedacfc0b 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 1855aa0e4..a420dab7f 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 f206dc8e3..2a19fc20a 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 ad21eba03..3d1b5933c 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-a6758eb9-DEBUG \ No newline at end of file +DEV-bec246c6-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 3f0827259..24e5ef9f8 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_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 err_fun(double *nom_x, double *delta_x, double *out_3499577995962015470) { + out_3499577995962015470[0] = delta_x[0] + nom_x[0]; + out_3499577995962015470[1] = delta_x[1] + nom_x[1]; + out_3499577995962015470[2] = delta_x[2] + nom_x[2]; + out_3499577995962015470[3] = delta_x[3] + nom_x[3]; + out_3499577995962015470[4] = delta_x[4] + nom_x[4]; + out_3499577995962015470[5] = delta_x[5] + nom_x[5]; + out_3499577995962015470[6] = delta_x[6] + nom_x[6]; + out_3499577995962015470[7] = delta_x[7] + nom_x[7]; + out_3499577995962015470[8] = delta_x[8] + nom_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 inv_err_fun(double *nom_x, double *true_x, double *out_7704661140039100406) { + out_7704661140039100406[0] = -nom_x[0] + true_x[0]; + out_7704661140039100406[1] = -nom_x[1] + true_x[1]; + out_7704661140039100406[2] = -nom_x[2] + true_x[2]; + out_7704661140039100406[3] = -nom_x[3] + true_x[3]; + out_7704661140039100406[4] = -nom_x[4] + true_x[4]; + out_7704661140039100406[5] = -nom_x[5] + true_x[5]; + out_7704661140039100406[6] = -nom_x[6] + true_x[6]; + out_7704661140039100406[7] = -nom_x[7] + true_x[7]; + out_7704661140039100406[8] = -nom_x[8] + true_x[8]; } -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 H_mod_fun(double *state, double *out_3593592511346668572) { + out_3593592511346668572[0] = 1.0; + out_3593592511346668572[1] = 0.0; + out_3593592511346668572[2] = 0.0; + out_3593592511346668572[3] = 0.0; + out_3593592511346668572[4] = 0.0; + out_3593592511346668572[5] = 0.0; + out_3593592511346668572[6] = 0.0; + out_3593592511346668572[7] = 0.0; + out_3593592511346668572[8] = 0.0; + out_3593592511346668572[9] = 0.0; + out_3593592511346668572[10] = 1.0; + out_3593592511346668572[11] = 0.0; + out_3593592511346668572[12] = 0.0; + out_3593592511346668572[13] = 0.0; + out_3593592511346668572[14] = 0.0; + out_3593592511346668572[15] = 0.0; + out_3593592511346668572[16] = 0.0; + out_3593592511346668572[17] = 0.0; + out_3593592511346668572[18] = 0.0; + out_3593592511346668572[19] = 0.0; + out_3593592511346668572[20] = 1.0; + out_3593592511346668572[21] = 0.0; + out_3593592511346668572[22] = 0.0; + out_3593592511346668572[23] = 0.0; + out_3593592511346668572[24] = 0.0; + out_3593592511346668572[25] = 0.0; + out_3593592511346668572[26] = 0.0; + out_3593592511346668572[27] = 0.0; + out_3593592511346668572[28] = 0.0; + out_3593592511346668572[29] = 0.0; + out_3593592511346668572[30] = 1.0; + out_3593592511346668572[31] = 0.0; + out_3593592511346668572[32] = 0.0; + out_3593592511346668572[33] = 0.0; + out_3593592511346668572[34] = 0.0; + out_3593592511346668572[35] = 0.0; + out_3593592511346668572[36] = 0.0; + out_3593592511346668572[37] = 0.0; + out_3593592511346668572[38] = 0.0; + out_3593592511346668572[39] = 0.0; + out_3593592511346668572[40] = 1.0; + out_3593592511346668572[41] = 0.0; + out_3593592511346668572[42] = 0.0; + out_3593592511346668572[43] = 0.0; + out_3593592511346668572[44] = 0.0; + out_3593592511346668572[45] = 0.0; + out_3593592511346668572[46] = 0.0; + out_3593592511346668572[47] = 0.0; + out_3593592511346668572[48] = 0.0; + out_3593592511346668572[49] = 0.0; + out_3593592511346668572[50] = 1.0; + out_3593592511346668572[51] = 0.0; + out_3593592511346668572[52] = 0.0; + out_3593592511346668572[53] = 0.0; + out_3593592511346668572[54] = 0.0; + out_3593592511346668572[55] = 0.0; + out_3593592511346668572[56] = 0.0; + out_3593592511346668572[57] = 0.0; + out_3593592511346668572[58] = 0.0; + out_3593592511346668572[59] = 0.0; + out_3593592511346668572[60] = 1.0; + out_3593592511346668572[61] = 0.0; + out_3593592511346668572[62] = 0.0; + out_3593592511346668572[63] = 0.0; + out_3593592511346668572[64] = 0.0; + out_3593592511346668572[65] = 0.0; + out_3593592511346668572[66] = 0.0; + out_3593592511346668572[67] = 0.0; + out_3593592511346668572[68] = 0.0; + out_3593592511346668572[69] = 0.0; + out_3593592511346668572[70] = 1.0; + out_3593592511346668572[71] = 0.0; + out_3593592511346668572[72] = 0.0; + out_3593592511346668572[73] = 0.0; + out_3593592511346668572[74] = 0.0; + out_3593592511346668572[75] = 0.0; + out_3593592511346668572[76] = 0.0; + out_3593592511346668572[77] = 0.0; + out_3593592511346668572[78] = 0.0; + out_3593592511346668572[79] = 0.0; + out_3593592511346668572[80] = 1.0; } -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_5138495986546656101) { + out_5138495986546656101[0] = state[0]; + out_5138495986546656101[1] = state[1]; + out_5138495986546656101[2] = state[2]; + out_5138495986546656101[3] = state[3]; + out_5138495986546656101[4] = state[4]; + out_5138495986546656101[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_5138495986546656101[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_5138495986546656101[7] = state[7]; + out_5138495986546656101[8] = state[8]; } -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 F_fun(double *state, double dt, double *out_9094696385837044219) { + out_9094696385837044219[0] = 1; + out_9094696385837044219[1] = 0; + out_9094696385837044219[2] = 0; + out_9094696385837044219[3] = 0; + out_9094696385837044219[4] = 0; + out_9094696385837044219[5] = 0; + out_9094696385837044219[6] = 0; + out_9094696385837044219[7] = 0; + out_9094696385837044219[8] = 0; + out_9094696385837044219[9] = 0; + out_9094696385837044219[10] = 1; + out_9094696385837044219[11] = 0; + out_9094696385837044219[12] = 0; + out_9094696385837044219[13] = 0; + out_9094696385837044219[14] = 0; + out_9094696385837044219[15] = 0; + out_9094696385837044219[16] = 0; + out_9094696385837044219[17] = 0; + out_9094696385837044219[18] = 0; + out_9094696385837044219[19] = 0; + out_9094696385837044219[20] = 1; + out_9094696385837044219[21] = 0; + out_9094696385837044219[22] = 0; + out_9094696385837044219[23] = 0; + out_9094696385837044219[24] = 0; + out_9094696385837044219[25] = 0; + out_9094696385837044219[26] = 0; + out_9094696385837044219[27] = 0; + out_9094696385837044219[28] = 0; + out_9094696385837044219[29] = 0; + out_9094696385837044219[30] = 1; + out_9094696385837044219[31] = 0; + out_9094696385837044219[32] = 0; + out_9094696385837044219[33] = 0; + out_9094696385837044219[34] = 0; + out_9094696385837044219[35] = 0; + out_9094696385837044219[36] = 0; + out_9094696385837044219[37] = 0; + out_9094696385837044219[38] = 0; + out_9094696385837044219[39] = 0; + out_9094696385837044219[40] = 1; + out_9094696385837044219[41] = 0; + out_9094696385837044219[42] = 0; + out_9094696385837044219[43] = 0; + out_9094696385837044219[44] = 0; + out_9094696385837044219[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_9094696385837044219[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_9094696385837044219[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_9094696385837044219[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_9094696385837044219[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_9094696385837044219[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_9094696385837044219[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_9094696385837044219[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_9094696385837044219[53] = -9.8100000000000005*dt; + out_9094696385837044219[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_9094696385837044219[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_9094696385837044219[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_9094696385837044219[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_9094696385837044219[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_9094696385837044219[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_9094696385837044219[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_9094696385837044219[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_9094696385837044219[62] = 0; + out_9094696385837044219[63] = 0; + out_9094696385837044219[64] = 0; + out_9094696385837044219[65] = 0; + out_9094696385837044219[66] = 0; + out_9094696385837044219[67] = 0; + out_9094696385837044219[68] = 0; + out_9094696385837044219[69] = 0; + out_9094696385837044219[70] = 1; + out_9094696385837044219[71] = 0; + out_9094696385837044219[72] = 0; + out_9094696385837044219[73] = 0; + out_9094696385837044219[74] = 0; + out_9094696385837044219[75] = 0; + out_9094696385837044219[76] = 0; + out_9094696385837044219[77] = 0; + out_9094696385837044219[78] = 0; + out_9094696385837044219[79] = 0; + out_9094696385837044219[80] = 1; } -void h_25(double *state, double *unused, double *out_3236700917680503454) { - out_3236700917680503454[0] = state[6]; +void h_25(double *state, double *unused, double *out_1209975406708631015) { + out_1209975406708631015[0] = state[6]; } -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_25(double *state, double *unused, double *out_4098926953546965284) { + out_4098926953546965284[0] = 0; + out_4098926953546965284[1] = 0; + out_4098926953546965284[2] = 0; + out_4098926953546965284[3] = 0; + out_4098926953546965284[4] = 0; + out_4098926953546965284[5] = 0; + out_4098926953546965284[6] = 1; + out_4098926953546965284[7] = 0; + out_4098926953546965284[8] = 0; } -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_5058461778376183297) { + out_5058461778376183297[0] = state[4]; + out_5058461778376183297[1] = state[5]; } -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_24(double *state, double *unused, double *out_4943923562330660683) { + out_4943923562330660683[0] = 0; + out_4943923562330660683[1] = 0; + out_4943923562330660683[2] = 0; + out_4943923562330660683[3] = 0; + out_4943923562330660683[4] = 1; + out_4943923562330660683[5] = 0; + out_4943923562330660683[6] = 0; + out_4943923562330660683[7] = 0; + out_4943923562330660683[8] = 0; + out_4943923562330660683[9] = 0; + out_4943923562330660683[10] = 0; + out_4943923562330660683[11] = 0; + out_4943923562330660683[12] = 0; + out_4943923562330660683[13] = 0; + out_4943923562330660683[14] = 1; + out_4943923562330660683[15] = 0; + out_4943923562330660683[16] = 0; + out_4943923562330660683[17] = 0; } -void h_30(double *state, double *unused, double *out_7446777899208375721) { - out_7446777899208375721[0] = state[4]; +void h_30(double *state, double *unused, double *out_5560859819641719921) { + out_5560859819641719921[0] = state[4]; } -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_30(double *state, double *unused, double *out_7431126778670969577) { + out_7431126778670969577[0] = 0; + out_7431126778670969577[1] = 0; + out_7431126778670969577[2] = 0; + out_7431126778670969577[3] = 0; + out_7431126778670969577[4] = 1; + out_7431126778670969577[5] = 0; + out_7431126778670969577[6] = 0; + out_7431126778670969577[7] = 0; + out_7431126778670969577[8] = 0; } -void h_26(double *state, double *unused, double *out_8390287752110038156) { - out_8390287752110038156[0] = state[7]; +void h_26(double *state, double *unused, double *out_4617349966740057486) { + out_4617349966740057486[0] = state[7]; } -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_26(double *state, double *unused, double *out_4755781017657277188) { + out_4755781017657277188[0] = 0; + out_4755781017657277188[1] = 0; + out_4755781017657277188[2] = 0; + out_4755781017657277188[3] = 0; + out_4755781017657277188[4] = 0; + out_4755781017657277188[5] = 0; + out_4755781017657277188[6] = 0; + out_4755781017657277188[7] = 1; + out_4755781017657277188[8] = 0; } -void h_27(double *state, double *unused, double *out_7774899721600347349) { - out_7774899721600347349[0] = state[3]; +void h_27(double *state, double *unused, double *out_6211648674369476660) { + out_6211648674369476660[0] = state[3]; } -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_27(double *state, double *unused, double *out_8840853983238157128) { + out_8840853983238157128[0] = 0; + out_8840853983238157128[1] = 0; + out_8840853983238157128[2] = 0; + out_8840853983238157128[3] = 1; + out_8840853983238157128[4] = 0; + out_8840853983238157128[5] = 0; + out_8840853983238157128[6] = 0; + out_8840853983238157128[7] = 0; + out_8840853983238157128[8] = 0; } -void h_29(double *state, double *unused, double *out_8050093783884853238) { - out_8050093783884853238[0] = state[1]; +void h_29(double *state, double *unused, double *out_559186551980874276) { + out_559186551980874276[0] = state[1]; } -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_29(double *state, double *unused, double *out_6920895434356577393) { + out_6920895434356577393[0] = 0; + out_6920895434356577393[1] = 1; + out_6920895434356577393[2] = 0; + out_6920895434356577393[3] = 0; + out_6920895434356577393[4] = 0; + out_6920895434356577393[5] = 0; + out_6920895434356577393[6] = 0; + out_6920895434356577393[7] = 0; + out_6920895434356577393[8] = 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_7233422131679229311) { + out_7233422131679229311[0] = state[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_28(double *state, double *unused, double *out_6443449622283443649) { + out_6443449622283443649[0] = 1; + out_6443449622283443649[1] = 0; + out_6443449622283443649[2] = 0; + out_6443449622283443649[3] = 0; + out_6443449622283443649[4] = 0; + out_6443449622283443649[5] = 0; + out_6443449622283443649[6] = 0; + out_6443449622283443649[7] = 0; + out_6443449622283443649[8] = 0; } -void h_31(double *state, double *unused, double *out_7176856938506309920) { - out_7176856938506309920[0] = state[8]; +void h_31(double *state, double *unused, double *out_8973240663529303456) { + out_8973240663529303456[0] = state[8]; } -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; +void H_31(double *state, double *unused, double *out_4129572915423925712) { + out_4129572915423925712[0] = 0; + out_4129572915423925712[1] = 0; + out_4129572915423925712[2] = 0; + out_4129572915423925712[3] = 0; + out_4129572915423925712[4] = 0; + out_4129572915423925712[5] = 0; + out_4129572915423925712[6] = 0; + out_4129572915423925712[7] = 0; + out_4129572915423925712[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_8838314869193199803) { - err_fun(nom_x, delta_x, out_8838314869193199803); +void car_err_fun(double *nom_x, double *delta_x, double *out_3499577995962015470) { + err_fun(nom_x, delta_x, out_3499577995962015470); } -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_inv_err_fun(double *nom_x, double *true_x, double *out_7704661140039100406) { + inv_err_fun(nom_x, true_x, out_7704661140039100406); } -void car_H_mod_fun(double *state, double *out_7297179764193114402) { - H_mod_fun(state, out_7297179764193114402); +void car_H_mod_fun(double *state, double *out_3593592511346668572) { + H_mod_fun(state, out_3593592511346668572); } -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_5138495986546656101) { + f_fun(state, dt, out_5138495986546656101); } -void car_F_fun(double *state, double dt, double *out_2902406519648293307) { - F_fun(state, dt, out_2902406519648293307); +void car_F_fun(double *state, double dt, double *out_9094696385837044219) { + F_fun(state, dt, out_9094696385837044219); } -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_1209975406708631015) { + h_25(state, unused, out_1209975406708631015); } -void car_H_25(double *state, double *unused, double *out_1896008461719122835) { - H_25(state, unused, out_1896008461719122835); +void car_H_25(double *state, double *unused, double *out_4098926953546965284) { + H_25(state, unused, out_4098926953546965284); } -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_5058461778376183297) { + h_24(state, unused, out_5058461778376183297); } -void car_H_24(double *state, double *unused, double *out_4068658060724622401) { - H_24(state, unused, out_4068658060724622401); +void car_H_24(double *state, double *unused, double *out_4943923562330660683) { + H_24(state, unused, out_4943923562330660683); } -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_5560859819641719921) { + h_30(state, unused, out_5560859819641719921); } -void car_H_30(double *state, double *unused, double *out_5020681879772493920) { - H_30(state, unused, out_5020681879772493920); +void car_H_30(double *state, double *unused, double *out_7431126778670969577) { + H_30(state, unused, out_7431126778670969577); } -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_4617349966740057486) { + h_26(state, unused, out_4617349966740057486); } -void car_H_26(double *state, double *unused, double *out_5637511780593179059) { - H_26(state, unused, out_5637511780593179059); +void car_H_26(double *state, double *unused, double *out_4755781017657277188) { + H_26(state, unused, out_4755781017657277188); } -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_6211648674369476660) { + h_27(state, unused, out_6211648674369476660); } -void car_H_27(double *state, double *unused, double *out_2845918567972069009) { - H_27(state, unused, out_2845918567972069009); +void car_H_27(double *state, double *unused, double *out_8840853983238157128) { + H_27(state, unused, out_8840853983238157128); } -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_559186551980874276) { + h_29(state, unused, out_559186551980874276); } -void car_H_29(double *state, double *unused, double *out_1132555841102517976) { - H_29(state, unused, out_1132555841102517976); +void car_H_29(double *state, double *unused, double *out_6920895434356577393) { + H_29(state, unused, out_6920895434356577393); } -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_7233422131679229311) { + h_28(state, unused, out_7233422131679229311); } -void car_H_28(double *state, double *unused, double *out_3949843175967012598) { - H_28(state, unused, out_3949843175967012598); +void car_H_28(double *state, double *unused, double *out_6443449622283443649) { + H_28(state, unused, out_6443449622283443649); } -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_8973240663529303456) { + h_31(state, unused, out_8973240663529303456); } -void car_H_31(double *state, double *unused, double *out_1865362499842162407) { - H_31(state, unused, out_1865362499842162407); +void car_H_31(double *state, double *unused, double *out_4129572915423925712) { + H_31(state, unused, out_4129572915423925712); } 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 d3bd6bc90..4bde03742 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_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_err_fun(double *nom_x, double *delta_x, double *out_3499577995962015470); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_7704661140039100406); +void car_H_mod_fun(double *state, double *out_3593592511346668572); +void car_f_fun(double *state, double dt, double *out_5138495986546656101); +void car_F_fun(double *state, double dt, double *out_9094696385837044219); +void car_h_25(double *state, double *unused, double *out_1209975406708631015); +void car_H_25(double *state, double *unused, double *out_4098926953546965284); +void car_h_24(double *state, double *unused, double *out_5058461778376183297); +void car_H_24(double *state, double *unused, double *out_4943923562330660683); +void car_h_30(double *state, double *unused, double *out_5560859819641719921); +void car_H_30(double *state, double *unused, double *out_7431126778670969577); +void car_h_26(double *state, double *unused, double *out_4617349966740057486); +void car_H_26(double *state, double *unused, double *out_4755781017657277188); +void car_h_27(double *state, double *unused, double *out_6211648674369476660); +void car_H_27(double *state, double *unused, double *out_8840853983238157128); +void car_h_29(double *state, double *unused, double *out_559186551980874276); +void car_H_29(double *state, double *unused, double *out_6920895434356577393); +void car_h_28(double *state, double *unused, double *out_7233422131679229311); +void car_H_28(double *state, double *unused, double *out_6443449622283443649); +void car_h_31(double *state, double *unused, double *out_8973240663529303456); +void car_H_31(double *state, double *unused, double *out_4129572915423925712); 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 bce0de7d5..19675ff39 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_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 err_fun(double *nom_x, double *delta_x, double *out_4711793940499862270) { + out_4711793940499862270[0] = delta_x[0] + nom_x[0]; + out_4711793940499862270[1] = delta_x[1] + nom_x[1]; + out_4711793940499862270[2] = delta_x[2] + nom_x[2]; + out_4711793940499862270[3] = delta_x[3] + nom_x[3]; + out_4711793940499862270[4] = delta_x[4] + nom_x[4]; + out_4711793940499862270[5] = delta_x[5] + nom_x[5]; + out_4711793940499862270[6] = delta_x[6] + nom_x[6]; + out_4711793940499862270[7] = delta_x[7] + nom_x[7]; + out_4711793940499862270[8] = delta_x[8] + nom_x[8]; + out_4711793940499862270[9] = delta_x[9] + nom_x[9]; + out_4711793940499862270[10] = delta_x[10] + nom_x[10]; + out_4711793940499862270[11] = delta_x[11] + nom_x[11]; + out_4711793940499862270[12] = delta_x[12] + nom_x[12]; + out_4711793940499862270[13] = delta_x[13] + nom_x[13]; + out_4711793940499862270[14] = delta_x[14] + nom_x[14]; + out_4711793940499862270[15] = delta_x[15] + nom_x[15]; + out_4711793940499862270[16] = delta_x[16] + nom_x[16]; + out_4711793940499862270[17] = delta_x[17] + nom_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 inv_err_fun(double *nom_x, double *true_x, double *out_8790106481935061031) { + out_8790106481935061031[0] = -nom_x[0] + true_x[0]; + out_8790106481935061031[1] = -nom_x[1] + true_x[1]; + out_8790106481935061031[2] = -nom_x[2] + true_x[2]; + out_8790106481935061031[3] = -nom_x[3] + true_x[3]; + out_8790106481935061031[4] = -nom_x[4] + true_x[4]; + out_8790106481935061031[5] = -nom_x[5] + true_x[5]; + out_8790106481935061031[6] = -nom_x[6] + true_x[6]; + out_8790106481935061031[7] = -nom_x[7] + true_x[7]; + out_8790106481935061031[8] = -nom_x[8] + true_x[8]; + out_8790106481935061031[9] = -nom_x[9] + true_x[9]; + out_8790106481935061031[10] = -nom_x[10] + true_x[10]; + out_8790106481935061031[11] = -nom_x[11] + true_x[11]; + out_8790106481935061031[12] = -nom_x[12] + true_x[12]; + out_8790106481935061031[13] = -nom_x[13] + true_x[13]; + out_8790106481935061031[14] = -nom_x[14] + true_x[14]; + out_8790106481935061031[15] = -nom_x[15] + true_x[15]; + out_8790106481935061031[16] = -nom_x[16] + true_x[16]; + out_8790106481935061031[17] = -nom_x[17] + true_x[17]; } -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 H_mod_fun(double *state, double *out_3710500625877642434) { + out_3710500625877642434[0] = 1.0; + out_3710500625877642434[1] = 0.0; + out_3710500625877642434[2] = 0.0; + out_3710500625877642434[3] = 0.0; + out_3710500625877642434[4] = 0.0; + out_3710500625877642434[5] = 0.0; + out_3710500625877642434[6] = 0.0; + out_3710500625877642434[7] = 0.0; + out_3710500625877642434[8] = 0.0; + out_3710500625877642434[9] = 0.0; + out_3710500625877642434[10] = 0.0; + out_3710500625877642434[11] = 0.0; + out_3710500625877642434[12] = 0.0; + out_3710500625877642434[13] = 0.0; + out_3710500625877642434[14] = 0.0; + out_3710500625877642434[15] = 0.0; + out_3710500625877642434[16] = 0.0; + out_3710500625877642434[17] = 0.0; + out_3710500625877642434[18] = 0.0; + out_3710500625877642434[19] = 1.0; + out_3710500625877642434[20] = 0.0; + out_3710500625877642434[21] = 0.0; + out_3710500625877642434[22] = 0.0; + out_3710500625877642434[23] = 0.0; + out_3710500625877642434[24] = 0.0; + out_3710500625877642434[25] = 0.0; + out_3710500625877642434[26] = 0.0; + out_3710500625877642434[27] = 0.0; + out_3710500625877642434[28] = 0.0; + out_3710500625877642434[29] = 0.0; + out_3710500625877642434[30] = 0.0; + out_3710500625877642434[31] = 0.0; + out_3710500625877642434[32] = 0.0; + out_3710500625877642434[33] = 0.0; + out_3710500625877642434[34] = 0.0; + out_3710500625877642434[35] = 0.0; + out_3710500625877642434[36] = 0.0; + out_3710500625877642434[37] = 0.0; + out_3710500625877642434[38] = 1.0; + out_3710500625877642434[39] = 0.0; + out_3710500625877642434[40] = 0.0; + out_3710500625877642434[41] = 0.0; + out_3710500625877642434[42] = 0.0; + out_3710500625877642434[43] = 0.0; + out_3710500625877642434[44] = 0.0; + out_3710500625877642434[45] = 0.0; + out_3710500625877642434[46] = 0.0; + out_3710500625877642434[47] = 0.0; + out_3710500625877642434[48] = 0.0; + out_3710500625877642434[49] = 0.0; + out_3710500625877642434[50] = 0.0; + out_3710500625877642434[51] = 0.0; + out_3710500625877642434[52] = 0.0; + out_3710500625877642434[53] = 0.0; + out_3710500625877642434[54] = 0.0; + out_3710500625877642434[55] = 0.0; + out_3710500625877642434[56] = 0.0; + out_3710500625877642434[57] = 1.0; + out_3710500625877642434[58] = 0.0; + out_3710500625877642434[59] = 0.0; + out_3710500625877642434[60] = 0.0; + out_3710500625877642434[61] = 0.0; + out_3710500625877642434[62] = 0.0; + out_3710500625877642434[63] = 0.0; + out_3710500625877642434[64] = 0.0; + out_3710500625877642434[65] = 0.0; + out_3710500625877642434[66] = 0.0; + out_3710500625877642434[67] = 0.0; + out_3710500625877642434[68] = 0.0; + out_3710500625877642434[69] = 0.0; + out_3710500625877642434[70] = 0.0; + out_3710500625877642434[71] = 0.0; + out_3710500625877642434[72] = 0.0; + out_3710500625877642434[73] = 0.0; + out_3710500625877642434[74] = 0.0; + out_3710500625877642434[75] = 0.0; + out_3710500625877642434[76] = 1.0; + out_3710500625877642434[77] = 0.0; + out_3710500625877642434[78] = 0.0; + out_3710500625877642434[79] = 0.0; + out_3710500625877642434[80] = 0.0; + out_3710500625877642434[81] = 0.0; + out_3710500625877642434[82] = 0.0; + out_3710500625877642434[83] = 0.0; + out_3710500625877642434[84] = 0.0; + out_3710500625877642434[85] = 0.0; + out_3710500625877642434[86] = 0.0; + out_3710500625877642434[87] = 0.0; + out_3710500625877642434[88] = 0.0; + out_3710500625877642434[89] = 0.0; + out_3710500625877642434[90] = 0.0; + out_3710500625877642434[91] = 0.0; + out_3710500625877642434[92] = 0.0; + out_3710500625877642434[93] = 0.0; + out_3710500625877642434[94] = 0.0; + out_3710500625877642434[95] = 1.0; + out_3710500625877642434[96] = 0.0; + out_3710500625877642434[97] = 0.0; + out_3710500625877642434[98] = 0.0; + out_3710500625877642434[99] = 0.0; + out_3710500625877642434[100] = 0.0; + out_3710500625877642434[101] = 0.0; + out_3710500625877642434[102] = 0.0; + out_3710500625877642434[103] = 0.0; + out_3710500625877642434[104] = 0.0; + out_3710500625877642434[105] = 0.0; + out_3710500625877642434[106] = 0.0; + out_3710500625877642434[107] = 0.0; + out_3710500625877642434[108] = 0.0; + out_3710500625877642434[109] = 0.0; + out_3710500625877642434[110] = 0.0; + out_3710500625877642434[111] = 0.0; + out_3710500625877642434[112] = 0.0; + out_3710500625877642434[113] = 0.0; + out_3710500625877642434[114] = 1.0; + out_3710500625877642434[115] = 0.0; + out_3710500625877642434[116] = 0.0; + out_3710500625877642434[117] = 0.0; + out_3710500625877642434[118] = 0.0; + out_3710500625877642434[119] = 0.0; + out_3710500625877642434[120] = 0.0; + out_3710500625877642434[121] = 0.0; + out_3710500625877642434[122] = 0.0; + out_3710500625877642434[123] = 0.0; + out_3710500625877642434[124] = 0.0; + out_3710500625877642434[125] = 0.0; + out_3710500625877642434[126] = 0.0; + out_3710500625877642434[127] = 0.0; + out_3710500625877642434[128] = 0.0; + out_3710500625877642434[129] = 0.0; + out_3710500625877642434[130] = 0.0; + out_3710500625877642434[131] = 0.0; + out_3710500625877642434[132] = 0.0; + out_3710500625877642434[133] = 1.0; + out_3710500625877642434[134] = 0.0; + out_3710500625877642434[135] = 0.0; + out_3710500625877642434[136] = 0.0; + out_3710500625877642434[137] = 0.0; + out_3710500625877642434[138] = 0.0; + out_3710500625877642434[139] = 0.0; + out_3710500625877642434[140] = 0.0; + out_3710500625877642434[141] = 0.0; + out_3710500625877642434[142] = 0.0; + out_3710500625877642434[143] = 0.0; + out_3710500625877642434[144] = 0.0; + out_3710500625877642434[145] = 0.0; + out_3710500625877642434[146] = 0.0; + out_3710500625877642434[147] = 0.0; + out_3710500625877642434[148] = 0.0; + out_3710500625877642434[149] = 0.0; + out_3710500625877642434[150] = 0.0; + out_3710500625877642434[151] = 0.0; + out_3710500625877642434[152] = 1.0; + out_3710500625877642434[153] = 0.0; + out_3710500625877642434[154] = 0.0; + out_3710500625877642434[155] = 0.0; + out_3710500625877642434[156] = 0.0; + out_3710500625877642434[157] = 0.0; + out_3710500625877642434[158] = 0.0; + out_3710500625877642434[159] = 0.0; + out_3710500625877642434[160] = 0.0; + out_3710500625877642434[161] = 0.0; + out_3710500625877642434[162] = 0.0; + out_3710500625877642434[163] = 0.0; + out_3710500625877642434[164] = 0.0; + out_3710500625877642434[165] = 0.0; + out_3710500625877642434[166] = 0.0; + out_3710500625877642434[167] = 0.0; + out_3710500625877642434[168] = 0.0; + out_3710500625877642434[169] = 0.0; + out_3710500625877642434[170] = 0.0; + out_3710500625877642434[171] = 1.0; + out_3710500625877642434[172] = 0.0; + out_3710500625877642434[173] = 0.0; + out_3710500625877642434[174] = 0.0; + out_3710500625877642434[175] = 0.0; + out_3710500625877642434[176] = 0.0; + out_3710500625877642434[177] = 0.0; + out_3710500625877642434[178] = 0.0; + out_3710500625877642434[179] = 0.0; + out_3710500625877642434[180] = 0.0; + out_3710500625877642434[181] = 0.0; + out_3710500625877642434[182] = 0.0; + out_3710500625877642434[183] = 0.0; + out_3710500625877642434[184] = 0.0; + out_3710500625877642434[185] = 0.0; + out_3710500625877642434[186] = 0.0; + out_3710500625877642434[187] = 0.0; + out_3710500625877642434[188] = 0.0; + out_3710500625877642434[189] = 0.0; + out_3710500625877642434[190] = 1.0; + out_3710500625877642434[191] = 0.0; + out_3710500625877642434[192] = 0.0; + out_3710500625877642434[193] = 0.0; + out_3710500625877642434[194] = 0.0; + out_3710500625877642434[195] = 0.0; + out_3710500625877642434[196] = 0.0; + out_3710500625877642434[197] = 0.0; + out_3710500625877642434[198] = 0.0; + out_3710500625877642434[199] = 0.0; + out_3710500625877642434[200] = 0.0; + out_3710500625877642434[201] = 0.0; + out_3710500625877642434[202] = 0.0; + out_3710500625877642434[203] = 0.0; + out_3710500625877642434[204] = 0.0; + out_3710500625877642434[205] = 0.0; + out_3710500625877642434[206] = 0.0; + out_3710500625877642434[207] = 0.0; + out_3710500625877642434[208] = 0.0; + out_3710500625877642434[209] = 1.0; + out_3710500625877642434[210] = 0.0; + out_3710500625877642434[211] = 0.0; + out_3710500625877642434[212] = 0.0; + out_3710500625877642434[213] = 0.0; + out_3710500625877642434[214] = 0.0; + out_3710500625877642434[215] = 0.0; + out_3710500625877642434[216] = 0.0; + out_3710500625877642434[217] = 0.0; + out_3710500625877642434[218] = 0.0; + out_3710500625877642434[219] = 0.0; + out_3710500625877642434[220] = 0.0; + out_3710500625877642434[221] = 0.0; + out_3710500625877642434[222] = 0.0; + out_3710500625877642434[223] = 0.0; + out_3710500625877642434[224] = 0.0; + out_3710500625877642434[225] = 0.0; + out_3710500625877642434[226] = 0.0; + out_3710500625877642434[227] = 0.0; + out_3710500625877642434[228] = 1.0; + out_3710500625877642434[229] = 0.0; + out_3710500625877642434[230] = 0.0; + out_3710500625877642434[231] = 0.0; + out_3710500625877642434[232] = 0.0; + out_3710500625877642434[233] = 0.0; + out_3710500625877642434[234] = 0.0; + out_3710500625877642434[235] = 0.0; + out_3710500625877642434[236] = 0.0; + out_3710500625877642434[237] = 0.0; + out_3710500625877642434[238] = 0.0; + out_3710500625877642434[239] = 0.0; + out_3710500625877642434[240] = 0.0; + out_3710500625877642434[241] = 0.0; + out_3710500625877642434[242] = 0.0; + out_3710500625877642434[243] = 0.0; + out_3710500625877642434[244] = 0.0; + out_3710500625877642434[245] = 0.0; + out_3710500625877642434[246] = 0.0; + out_3710500625877642434[247] = 1.0; + out_3710500625877642434[248] = 0.0; + out_3710500625877642434[249] = 0.0; + out_3710500625877642434[250] = 0.0; + out_3710500625877642434[251] = 0.0; + out_3710500625877642434[252] = 0.0; + out_3710500625877642434[253] = 0.0; + out_3710500625877642434[254] = 0.0; + out_3710500625877642434[255] = 0.0; + out_3710500625877642434[256] = 0.0; + out_3710500625877642434[257] = 0.0; + out_3710500625877642434[258] = 0.0; + out_3710500625877642434[259] = 0.0; + out_3710500625877642434[260] = 0.0; + out_3710500625877642434[261] = 0.0; + out_3710500625877642434[262] = 0.0; + out_3710500625877642434[263] = 0.0; + out_3710500625877642434[264] = 0.0; + out_3710500625877642434[265] = 0.0; + out_3710500625877642434[266] = 1.0; + out_3710500625877642434[267] = 0.0; + out_3710500625877642434[268] = 0.0; + out_3710500625877642434[269] = 0.0; + out_3710500625877642434[270] = 0.0; + out_3710500625877642434[271] = 0.0; + out_3710500625877642434[272] = 0.0; + out_3710500625877642434[273] = 0.0; + out_3710500625877642434[274] = 0.0; + out_3710500625877642434[275] = 0.0; + out_3710500625877642434[276] = 0.0; + out_3710500625877642434[277] = 0.0; + out_3710500625877642434[278] = 0.0; + out_3710500625877642434[279] = 0.0; + out_3710500625877642434[280] = 0.0; + out_3710500625877642434[281] = 0.0; + out_3710500625877642434[282] = 0.0; + out_3710500625877642434[283] = 0.0; + out_3710500625877642434[284] = 0.0; + out_3710500625877642434[285] = 1.0; + out_3710500625877642434[286] = 0.0; + out_3710500625877642434[287] = 0.0; + out_3710500625877642434[288] = 0.0; + out_3710500625877642434[289] = 0.0; + out_3710500625877642434[290] = 0.0; + out_3710500625877642434[291] = 0.0; + out_3710500625877642434[292] = 0.0; + out_3710500625877642434[293] = 0.0; + out_3710500625877642434[294] = 0.0; + out_3710500625877642434[295] = 0.0; + out_3710500625877642434[296] = 0.0; + out_3710500625877642434[297] = 0.0; + out_3710500625877642434[298] = 0.0; + out_3710500625877642434[299] = 0.0; + out_3710500625877642434[300] = 0.0; + out_3710500625877642434[301] = 0.0; + out_3710500625877642434[302] = 0.0; + out_3710500625877642434[303] = 0.0; + out_3710500625877642434[304] = 1.0; + out_3710500625877642434[305] = 0.0; + out_3710500625877642434[306] = 0.0; + out_3710500625877642434[307] = 0.0; + out_3710500625877642434[308] = 0.0; + out_3710500625877642434[309] = 0.0; + out_3710500625877642434[310] = 0.0; + out_3710500625877642434[311] = 0.0; + out_3710500625877642434[312] = 0.0; + out_3710500625877642434[313] = 0.0; + out_3710500625877642434[314] = 0.0; + out_3710500625877642434[315] = 0.0; + out_3710500625877642434[316] = 0.0; + out_3710500625877642434[317] = 0.0; + out_3710500625877642434[318] = 0.0; + out_3710500625877642434[319] = 0.0; + out_3710500625877642434[320] = 0.0; + out_3710500625877642434[321] = 0.0; + out_3710500625877642434[322] = 0.0; + out_3710500625877642434[323] = 1.0; } -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_5725984385602899057) { + out_5725984385602899057[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_5725984385602899057[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_5725984385602899057[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_5725984385602899057[3] = dt*state[12] + state[3]; + out_5725984385602899057[4] = dt*state[13] + state[4]; + out_5725984385602899057[5] = dt*state[14] + state[5]; + out_5725984385602899057[6] = state[6]; + out_5725984385602899057[7] = state[7]; + out_5725984385602899057[8] = state[8]; + out_5725984385602899057[9] = state[9]; + out_5725984385602899057[10] = state[10]; + out_5725984385602899057[11] = state[11]; + out_5725984385602899057[12] = state[12]; + out_5725984385602899057[13] = state[13]; + out_5725984385602899057[14] = state[14]; + out_5725984385602899057[15] = state[15]; + out_5725984385602899057[16] = state[16]; + out_5725984385602899057[17] = state[17]; } -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 F_fun(double *state, double dt, double *out_3503616768446574438) { + out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[2] = 0; + out_3503616768446574438[3] = 0; + out_3503616768446574438[4] = 0; + out_3503616768446574438[5] = 0; + out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[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_3503616768446574438[9] = 0; + out_3503616768446574438[10] = 0; + out_3503616768446574438[11] = 0; + out_3503616768446574438[12] = 0; + out_3503616768446574438[13] = 0; + out_3503616768446574438[14] = 0; + out_3503616768446574438[15] = 0; + out_3503616768446574438[16] = 0; + out_3503616768446574438[17] = 0; + out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[20] = 0; + out_3503616768446574438[21] = 0; + out_3503616768446574438[22] = 0; + out_3503616768446574438[23] = 0; + out_3503616768446574438[24] = 0; + out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[27] = 0; + out_3503616768446574438[28] = 0; + out_3503616768446574438[29] = 0; + out_3503616768446574438[30] = 0; + out_3503616768446574438[31] = 0; + out_3503616768446574438[32] = 0; + out_3503616768446574438[33] = 0; + out_3503616768446574438[34] = 0; + out_3503616768446574438[35] = 0; + out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[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_3503616768446574438[39] = 0; + out_3503616768446574438[40] = 0; + out_3503616768446574438[41] = 0; + out_3503616768446574438[42] = 0; + out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[45] = 0; + out_3503616768446574438[46] = 0; + out_3503616768446574438[47] = 0; + out_3503616768446574438[48] = 0; + out_3503616768446574438[49] = 0; + out_3503616768446574438[50] = 0; + out_3503616768446574438[51] = 0; + out_3503616768446574438[52] = 0; + out_3503616768446574438[53] = 0; + out_3503616768446574438[54] = 0; + out_3503616768446574438[55] = 0; + out_3503616768446574438[56] = 0; + out_3503616768446574438[57] = 1; + out_3503616768446574438[58] = 0; + out_3503616768446574438[59] = 0; + out_3503616768446574438[60] = 0; + out_3503616768446574438[61] = 0; + out_3503616768446574438[62] = 0; + out_3503616768446574438[63] = 0; + out_3503616768446574438[64] = 0; + out_3503616768446574438[65] = 0; + out_3503616768446574438[66] = dt; + out_3503616768446574438[67] = 0; + out_3503616768446574438[68] = 0; + out_3503616768446574438[69] = 0; + out_3503616768446574438[70] = 0; + out_3503616768446574438[71] = 0; + out_3503616768446574438[72] = 0; + out_3503616768446574438[73] = 0; + out_3503616768446574438[74] = 0; + out_3503616768446574438[75] = 0; + out_3503616768446574438[76] = 1; + out_3503616768446574438[77] = 0; + out_3503616768446574438[78] = 0; + out_3503616768446574438[79] = 0; + out_3503616768446574438[80] = 0; + out_3503616768446574438[81] = 0; + out_3503616768446574438[82] = 0; + out_3503616768446574438[83] = 0; + out_3503616768446574438[84] = 0; + out_3503616768446574438[85] = dt; + out_3503616768446574438[86] = 0; + out_3503616768446574438[87] = 0; + out_3503616768446574438[88] = 0; + out_3503616768446574438[89] = 0; + out_3503616768446574438[90] = 0; + out_3503616768446574438[91] = 0; + out_3503616768446574438[92] = 0; + out_3503616768446574438[93] = 0; + out_3503616768446574438[94] = 0; + out_3503616768446574438[95] = 1; + out_3503616768446574438[96] = 0; + out_3503616768446574438[97] = 0; + out_3503616768446574438[98] = 0; + out_3503616768446574438[99] = 0; + out_3503616768446574438[100] = 0; + out_3503616768446574438[101] = 0; + out_3503616768446574438[102] = 0; + out_3503616768446574438[103] = 0; + out_3503616768446574438[104] = dt; + out_3503616768446574438[105] = 0; + out_3503616768446574438[106] = 0; + out_3503616768446574438[107] = 0; + out_3503616768446574438[108] = 0; + out_3503616768446574438[109] = 0; + out_3503616768446574438[110] = 0; + out_3503616768446574438[111] = 0; + out_3503616768446574438[112] = 0; + out_3503616768446574438[113] = 0; + out_3503616768446574438[114] = 1; + out_3503616768446574438[115] = 0; + out_3503616768446574438[116] = 0; + out_3503616768446574438[117] = 0; + out_3503616768446574438[118] = 0; + out_3503616768446574438[119] = 0; + out_3503616768446574438[120] = 0; + out_3503616768446574438[121] = 0; + out_3503616768446574438[122] = 0; + out_3503616768446574438[123] = 0; + out_3503616768446574438[124] = 0; + out_3503616768446574438[125] = 0; + out_3503616768446574438[126] = 0; + out_3503616768446574438[127] = 0; + out_3503616768446574438[128] = 0; + out_3503616768446574438[129] = 0; + out_3503616768446574438[130] = 0; + out_3503616768446574438[131] = 0; + out_3503616768446574438[132] = 0; + out_3503616768446574438[133] = 1; + out_3503616768446574438[134] = 0; + out_3503616768446574438[135] = 0; + out_3503616768446574438[136] = 0; + out_3503616768446574438[137] = 0; + out_3503616768446574438[138] = 0; + out_3503616768446574438[139] = 0; + out_3503616768446574438[140] = 0; + out_3503616768446574438[141] = 0; + out_3503616768446574438[142] = 0; + out_3503616768446574438[143] = 0; + out_3503616768446574438[144] = 0; + out_3503616768446574438[145] = 0; + out_3503616768446574438[146] = 0; + out_3503616768446574438[147] = 0; + out_3503616768446574438[148] = 0; + out_3503616768446574438[149] = 0; + out_3503616768446574438[150] = 0; + out_3503616768446574438[151] = 0; + out_3503616768446574438[152] = 1; + out_3503616768446574438[153] = 0; + out_3503616768446574438[154] = 0; + out_3503616768446574438[155] = 0; + out_3503616768446574438[156] = 0; + out_3503616768446574438[157] = 0; + out_3503616768446574438[158] = 0; + out_3503616768446574438[159] = 0; + out_3503616768446574438[160] = 0; + out_3503616768446574438[161] = 0; + out_3503616768446574438[162] = 0; + out_3503616768446574438[163] = 0; + out_3503616768446574438[164] = 0; + out_3503616768446574438[165] = 0; + out_3503616768446574438[166] = 0; + out_3503616768446574438[167] = 0; + out_3503616768446574438[168] = 0; + out_3503616768446574438[169] = 0; + out_3503616768446574438[170] = 0; + out_3503616768446574438[171] = 1; + out_3503616768446574438[172] = 0; + out_3503616768446574438[173] = 0; + out_3503616768446574438[174] = 0; + out_3503616768446574438[175] = 0; + out_3503616768446574438[176] = 0; + out_3503616768446574438[177] = 0; + out_3503616768446574438[178] = 0; + out_3503616768446574438[179] = 0; + out_3503616768446574438[180] = 0; + out_3503616768446574438[181] = 0; + out_3503616768446574438[182] = 0; + out_3503616768446574438[183] = 0; + out_3503616768446574438[184] = 0; + out_3503616768446574438[185] = 0; + out_3503616768446574438[186] = 0; + out_3503616768446574438[187] = 0; + out_3503616768446574438[188] = 0; + out_3503616768446574438[189] = 0; + out_3503616768446574438[190] = 1; + out_3503616768446574438[191] = 0; + out_3503616768446574438[192] = 0; + out_3503616768446574438[193] = 0; + out_3503616768446574438[194] = 0; + out_3503616768446574438[195] = 0; + out_3503616768446574438[196] = 0; + out_3503616768446574438[197] = 0; + out_3503616768446574438[198] = 0; + out_3503616768446574438[199] = 0; + out_3503616768446574438[200] = 0; + out_3503616768446574438[201] = 0; + out_3503616768446574438[202] = 0; + out_3503616768446574438[203] = 0; + out_3503616768446574438[204] = 0; + out_3503616768446574438[205] = 0; + out_3503616768446574438[206] = 0; + out_3503616768446574438[207] = 0; + out_3503616768446574438[208] = 0; + out_3503616768446574438[209] = 1; + out_3503616768446574438[210] = 0; + out_3503616768446574438[211] = 0; + out_3503616768446574438[212] = 0; + out_3503616768446574438[213] = 0; + out_3503616768446574438[214] = 0; + out_3503616768446574438[215] = 0; + out_3503616768446574438[216] = 0; + out_3503616768446574438[217] = 0; + out_3503616768446574438[218] = 0; + out_3503616768446574438[219] = 0; + out_3503616768446574438[220] = 0; + out_3503616768446574438[221] = 0; + out_3503616768446574438[222] = 0; + out_3503616768446574438[223] = 0; + out_3503616768446574438[224] = 0; + out_3503616768446574438[225] = 0; + out_3503616768446574438[226] = 0; + out_3503616768446574438[227] = 0; + out_3503616768446574438[228] = 1; + out_3503616768446574438[229] = 0; + out_3503616768446574438[230] = 0; + out_3503616768446574438[231] = 0; + out_3503616768446574438[232] = 0; + out_3503616768446574438[233] = 0; + out_3503616768446574438[234] = 0; + out_3503616768446574438[235] = 0; + out_3503616768446574438[236] = 0; + out_3503616768446574438[237] = 0; + out_3503616768446574438[238] = 0; + out_3503616768446574438[239] = 0; + out_3503616768446574438[240] = 0; + out_3503616768446574438[241] = 0; + out_3503616768446574438[242] = 0; + out_3503616768446574438[243] = 0; + out_3503616768446574438[244] = 0; + out_3503616768446574438[245] = 0; + out_3503616768446574438[246] = 0; + out_3503616768446574438[247] = 1; + out_3503616768446574438[248] = 0; + out_3503616768446574438[249] = 0; + out_3503616768446574438[250] = 0; + out_3503616768446574438[251] = 0; + out_3503616768446574438[252] = 0; + out_3503616768446574438[253] = 0; + out_3503616768446574438[254] = 0; + out_3503616768446574438[255] = 0; + out_3503616768446574438[256] = 0; + out_3503616768446574438[257] = 0; + out_3503616768446574438[258] = 0; + out_3503616768446574438[259] = 0; + out_3503616768446574438[260] = 0; + out_3503616768446574438[261] = 0; + out_3503616768446574438[262] = 0; + out_3503616768446574438[263] = 0; + out_3503616768446574438[264] = 0; + out_3503616768446574438[265] = 0; + out_3503616768446574438[266] = 1; + out_3503616768446574438[267] = 0; + out_3503616768446574438[268] = 0; + out_3503616768446574438[269] = 0; + out_3503616768446574438[270] = 0; + out_3503616768446574438[271] = 0; + out_3503616768446574438[272] = 0; + out_3503616768446574438[273] = 0; + out_3503616768446574438[274] = 0; + out_3503616768446574438[275] = 0; + out_3503616768446574438[276] = 0; + out_3503616768446574438[277] = 0; + out_3503616768446574438[278] = 0; + out_3503616768446574438[279] = 0; + out_3503616768446574438[280] = 0; + out_3503616768446574438[281] = 0; + out_3503616768446574438[282] = 0; + out_3503616768446574438[283] = 0; + out_3503616768446574438[284] = 0; + out_3503616768446574438[285] = 1; + out_3503616768446574438[286] = 0; + out_3503616768446574438[287] = 0; + out_3503616768446574438[288] = 0; + out_3503616768446574438[289] = 0; + out_3503616768446574438[290] = 0; + out_3503616768446574438[291] = 0; + out_3503616768446574438[292] = 0; + out_3503616768446574438[293] = 0; + out_3503616768446574438[294] = 0; + out_3503616768446574438[295] = 0; + out_3503616768446574438[296] = 0; + out_3503616768446574438[297] = 0; + out_3503616768446574438[298] = 0; + out_3503616768446574438[299] = 0; + out_3503616768446574438[300] = 0; + out_3503616768446574438[301] = 0; + out_3503616768446574438[302] = 0; + out_3503616768446574438[303] = 0; + out_3503616768446574438[304] = 1; + out_3503616768446574438[305] = 0; + out_3503616768446574438[306] = 0; + out_3503616768446574438[307] = 0; + out_3503616768446574438[308] = 0; + out_3503616768446574438[309] = 0; + out_3503616768446574438[310] = 0; + out_3503616768446574438[311] = 0; + out_3503616768446574438[312] = 0; + out_3503616768446574438[313] = 0; + out_3503616768446574438[314] = 0; + out_3503616768446574438[315] = 0; + out_3503616768446574438[316] = 0; + out_3503616768446574438[317] = 0; + out_3503616768446574438[318] = 0; + out_3503616768446574438[319] = 0; + out_3503616768446574438[320] = 0; + out_3503616768446574438[321] = 0; + out_3503616768446574438[322] = 0; + out_3503616768446574438[323] = 1; } -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_3803432647309987328) { + out_3803432647309987328[0] = state[6] + state[9]; + out_3803432647309987328[1] = state[7] + state[10]; + out_3803432647309987328[2] = state[8] + state[11]; } -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_4(double *state, double *unused, double *out_7950155699686530755) { + out_7950155699686530755[0] = 0; + out_7950155699686530755[1] = 0; + out_7950155699686530755[2] = 0; + out_7950155699686530755[3] = 0; + out_7950155699686530755[4] = 0; + out_7950155699686530755[5] = 0; + out_7950155699686530755[6] = 1; + out_7950155699686530755[7] = 0; + out_7950155699686530755[8] = 0; + out_7950155699686530755[9] = 1; + out_7950155699686530755[10] = 0; + out_7950155699686530755[11] = 0; + out_7950155699686530755[12] = 0; + out_7950155699686530755[13] = 0; + out_7950155699686530755[14] = 0; + out_7950155699686530755[15] = 0; + out_7950155699686530755[16] = 0; + out_7950155699686530755[17] = 0; + out_7950155699686530755[18] = 0; + out_7950155699686530755[19] = 0; + out_7950155699686530755[20] = 0; + out_7950155699686530755[21] = 0; + out_7950155699686530755[22] = 0; + out_7950155699686530755[23] = 0; + out_7950155699686530755[24] = 0; + out_7950155699686530755[25] = 1; + out_7950155699686530755[26] = 0; + out_7950155699686530755[27] = 0; + out_7950155699686530755[28] = 1; + out_7950155699686530755[29] = 0; + out_7950155699686530755[30] = 0; + out_7950155699686530755[31] = 0; + out_7950155699686530755[32] = 0; + out_7950155699686530755[33] = 0; + out_7950155699686530755[34] = 0; + out_7950155699686530755[35] = 0; + out_7950155699686530755[36] = 0; + out_7950155699686530755[37] = 0; + out_7950155699686530755[38] = 0; + out_7950155699686530755[39] = 0; + out_7950155699686530755[40] = 0; + out_7950155699686530755[41] = 0; + out_7950155699686530755[42] = 0; + out_7950155699686530755[43] = 0; + out_7950155699686530755[44] = 1; + out_7950155699686530755[45] = 0; + out_7950155699686530755[46] = 0; + out_7950155699686530755[47] = 1; + out_7950155699686530755[48] = 0; + out_7950155699686530755[49] = 0; + out_7950155699686530755[50] = 0; + out_7950155699686530755[51] = 0; + out_7950155699686530755[52] = 0; + out_7950155699686530755[53] = 0; } -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_477421004614984104) { + out_477421004614984104[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_477421004614984104[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_477421004614984104[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_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_10(double *state, double *unused, double *out_8536057142137197060) { + out_8536057142137197060[0] = 0; + out_8536057142137197060[1] = 9.8100000000000005*cos(state[1]); + out_8536057142137197060[2] = 0; + out_8536057142137197060[3] = 0; + out_8536057142137197060[4] = -state[8]; + out_8536057142137197060[5] = state[7]; + out_8536057142137197060[6] = 0; + out_8536057142137197060[7] = state[5]; + out_8536057142137197060[8] = -state[4]; + out_8536057142137197060[9] = 0; + out_8536057142137197060[10] = 0; + out_8536057142137197060[11] = 0; + out_8536057142137197060[12] = 1; + out_8536057142137197060[13] = 0; + out_8536057142137197060[14] = 0; + out_8536057142137197060[15] = 1; + out_8536057142137197060[16] = 0; + out_8536057142137197060[17] = 0; + out_8536057142137197060[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_8536057142137197060[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_8536057142137197060[20] = 0; + out_8536057142137197060[21] = state[8]; + out_8536057142137197060[22] = 0; + out_8536057142137197060[23] = -state[6]; + out_8536057142137197060[24] = -state[5]; + out_8536057142137197060[25] = 0; + out_8536057142137197060[26] = state[3]; + out_8536057142137197060[27] = 0; + out_8536057142137197060[28] = 0; + out_8536057142137197060[29] = 0; + out_8536057142137197060[30] = 0; + out_8536057142137197060[31] = 1; + out_8536057142137197060[32] = 0; + out_8536057142137197060[33] = 0; + out_8536057142137197060[34] = 1; + out_8536057142137197060[35] = 0; + out_8536057142137197060[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_8536057142137197060[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_8536057142137197060[38] = 0; + out_8536057142137197060[39] = -state[7]; + out_8536057142137197060[40] = state[6]; + out_8536057142137197060[41] = 0; + out_8536057142137197060[42] = state[4]; + out_8536057142137197060[43] = -state[3]; + out_8536057142137197060[44] = 0; + out_8536057142137197060[45] = 0; + out_8536057142137197060[46] = 0; + out_8536057142137197060[47] = 0; + out_8536057142137197060[48] = 0; + out_8536057142137197060[49] = 0; + out_8536057142137197060[50] = 1; + out_8536057142137197060[51] = 0; + out_8536057142137197060[52] = 0; + out_8536057142137197060[53] = 1; } -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_1243321013145270066) { + out_1243321013145270066[0] = state[3]; + out_1243321013145270066[1] = state[4]; + out_1243321013145270066[2] = state[5]; } -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_13(double *state, double *unused, double *out_8514757619368374859) { + out_8514757619368374859[0] = 0; + out_8514757619368374859[1] = 0; + out_8514757619368374859[2] = 0; + out_8514757619368374859[3] = 1; + out_8514757619368374859[4] = 0; + out_8514757619368374859[5] = 0; + out_8514757619368374859[6] = 0; + out_8514757619368374859[7] = 0; + out_8514757619368374859[8] = 0; + out_8514757619368374859[9] = 0; + out_8514757619368374859[10] = 0; + out_8514757619368374859[11] = 0; + out_8514757619368374859[12] = 0; + out_8514757619368374859[13] = 0; + out_8514757619368374859[14] = 0; + out_8514757619368374859[15] = 0; + out_8514757619368374859[16] = 0; + out_8514757619368374859[17] = 0; + out_8514757619368374859[18] = 0; + out_8514757619368374859[19] = 0; + out_8514757619368374859[20] = 0; + out_8514757619368374859[21] = 0; + out_8514757619368374859[22] = 1; + out_8514757619368374859[23] = 0; + out_8514757619368374859[24] = 0; + out_8514757619368374859[25] = 0; + out_8514757619368374859[26] = 0; + out_8514757619368374859[27] = 0; + out_8514757619368374859[28] = 0; + out_8514757619368374859[29] = 0; + out_8514757619368374859[30] = 0; + out_8514757619368374859[31] = 0; + out_8514757619368374859[32] = 0; + out_8514757619368374859[33] = 0; + out_8514757619368374859[34] = 0; + out_8514757619368374859[35] = 0; + out_8514757619368374859[36] = 0; + out_8514757619368374859[37] = 0; + out_8514757619368374859[38] = 0; + out_8514757619368374859[39] = 0; + out_8514757619368374859[40] = 0; + out_8514757619368374859[41] = 1; + out_8514757619368374859[42] = 0; + out_8514757619368374859[43] = 0; + out_8514757619368374859[44] = 0; + out_8514757619368374859[45] = 0; + out_8514757619368374859[46] = 0; + out_8514757619368374859[47] = 0; + out_8514757619368374859[48] = 0; + out_8514757619368374859[49] = 0; + out_8514757619368374859[50] = 0; + out_8514757619368374859[51] = 0; + out_8514757619368374859[52] = 0; + out_8514757619368374859[53] = 0; } -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_7694485530011040401) { + out_7694485530011040401[0] = state[6]; + out_7694485530011040401[1] = state[7]; + out_7694485530011040401[2] = state[8]; } -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; +void H_14(double *state, double *unused, double *out_4867367267391158459) { + out_4867367267391158459[0] = 0; + out_4867367267391158459[1] = 0; + out_4867367267391158459[2] = 0; + out_4867367267391158459[3] = 0; + out_4867367267391158459[4] = 0; + out_4867367267391158459[5] = 0; + out_4867367267391158459[6] = 1; + out_4867367267391158459[7] = 0; + out_4867367267391158459[8] = 0; + out_4867367267391158459[9] = 0; + out_4867367267391158459[10] = 0; + out_4867367267391158459[11] = 0; + out_4867367267391158459[12] = 0; + out_4867367267391158459[13] = 0; + out_4867367267391158459[14] = 0; + out_4867367267391158459[15] = 0; + out_4867367267391158459[16] = 0; + out_4867367267391158459[17] = 0; + out_4867367267391158459[18] = 0; + out_4867367267391158459[19] = 0; + out_4867367267391158459[20] = 0; + out_4867367267391158459[21] = 0; + out_4867367267391158459[22] = 0; + out_4867367267391158459[23] = 0; + out_4867367267391158459[24] = 0; + out_4867367267391158459[25] = 1; + out_4867367267391158459[26] = 0; + out_4867367267391158459[27] = 0; + out_4867367267391158459[28] = 0; + out_4867367267391158459[29] = 0; + out_4867367267391158459[30] = 0; + out_4867367267391158459[31] = 0; + out_4867367267391158459[32] = 0; + out_4867367267391158459[33] = 0; + out_4867367267391158459[34] = 0; + out_4867367267391158459[35] = 0; + out_4867367267391158459[36] = 0; + out_4867367267391158459[37] = 0; + out_4867367267391158459[38] = 0; + out_4867367267391158459[39] = 0; + out_4867367267391158459[40] = 0; + out_4867367267391158459[41] = 0; + out_4867367267391158459[42] = 0; + out_4867367267391158459[43] = 0; + out_4867367267391158459[44] = 1; + out_4867367267391158459[45] = 0; + out_4867367267391158459[46] = 0; + out_4867367267391158459[47] = 0; + out_4867367267391158459[48] = 0; + out_4867367267391158459[49] = 0; + out_4867367267391158459[50] = 0; + out_4867367267391158459[51] = 0; + out_4867367267391158459[52] = 0; + out_4867367267391158459[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_4833077353004324467) { - err_fun(nom_x, delta_x, out_4833077353004324467); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4711793940499862270) { + err_fun(nom_x, delta_x, out_4711793940499862270); } -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_inv_err_fun(double *nom_x, double *true_x, double *out_8790106481935061031) { + inv_err_fun(nom_x, true_x, out_8790106481935061031); } -void pose_H_mod_fun(double *state, double *out_4718726566615345673) { - H_mod_fun(state, out_4718726566615345673); +void pose_H_mod_fun(double *state, double *out_3710500625877642434) { + H_mod_fun(state, out_3710500625877642434); } -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_5725984385602899057) { + f_fun(state, dt, out_5725984385602899057); } -void pose_F_fun(double *state, double dt, double *out_7597852127146355968) { - F_fun(state, dt, out_7597852127146355968); +void pose_F_fun(double *state, double dt, double *out_3503616768446574438) { + F_fun(state, dt, out_3503616768446574438); } -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_3803432647309987328) { + h_4(state, unused, out_3803432647309987328); } -void pose_H_4(double *state, double *unused, double *out_946919356771438101) { - H_4(state, unused, out_946919356771438101); +void pose_H_4(double *state, double *unused, double *out_7950155699686530755) { + H_4(state, unused, out_7950155699686530755); } -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_477421004614984104) { + h_10(state, unused, out_477421004614984104); } -void pose_H_10(double *state, double *unused, double *out_8679455089612648559) { - H_10(state, unused, out_8679455089612648559); +void pose_H_10(double *state, double *unused, double *out_8536057142137197060) { + H_10(state, unused, out_8536057142137197060); } -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_1243321013145270066) { + h_13(state, unused, out_1243321013145270066); } -void pose_H_13(double *state, double *unused, double *out_2265354468560894700) { - H_13(state, unused, out_2265354468560894700); +void pose_H_13(double *state, double *unused, double *out_8514757619368374859) { + H_13(state, unused, out_8514757619368374859); } -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_7694485530011040401) { + h_14(state, unused, out_7694485530011040401); } -void pose_H_14(double *state, double *unused, double *out_3016321499568046428) { - H_14(state, unused, out_3016321499568046428); +void pose_H_14(double *state, double *unused, double *out_4867367267391158459) { + H_14(state, unused, out_4867367267391158459); } 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 8f8c8c533..030a926d9 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_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_err_fun(double *nom_x, double *delta_x, double *out_4711793940499862270); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8790106481935061031); +void pose_H_mod_fun(double *state, double *out_3710500625877642434); +void pose_f_fun(double *state, double dt, double *out_5725984385602899057); +void pose_F_fun(double *state, double dt, double *out_3503616768446574438); +void pose_h_4(double *state, double *unused, double *out_3803432647309987328); +void pose_H_4(double *state, double *unused, double *out_7950155699686530755); +void pose_h_10(double *state, double *unused, double *out_477421004614984104); +void pose_H_10(double *state, double *unused, double *out_8536057142137197060); +void pose_h_13(double *state, double *unused, double *out_1243321013145270066); +void pose_H_13(double *state, double *unused, double *out_8514757619368374859); +void pose_h_14(double *state, double *unused, double *out_7694485530011040401); +void pose_H_14(double *state, double *unused, double *out_4867367267391158459); 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 e796f620c..039f9a996 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 582c7144a..8940bc2f5 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 911729054..1885d55f1 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 9f056f95d..2464eb7c2 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 8f42f98fc..b7f024f70 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index b9626588d..cd78b5605 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ