diff --git a/common/libcommon.a b/common/libcommon.a index 438ceb633..574846580 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 5efe195a2..ca1593d31 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 001e406e3..569d01e3a 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 b81a8223e..abedac540 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 8976513a9..fbd48be45 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 aee4a357c..c79e270e3 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 a8cffc805..33e684d8c 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 7607bc3e9..cfbf89a94 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 79b9a36df..d1cd2b715 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 79b9a36df..d1cd2b715 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 811c1eb46..060418c11 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 7607bc3e9..cfbf89a94 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 e715f23a6..6454a1cea 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-d8c79a97-DEBUG"; +const uint8_t gitversion[19] = "DEV-63db3908-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 4f55ed142..c14e7eb96 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 03814e91b..090bad869 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 b2a36a024..e7f39a6b8 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 dc5b949df..5b5c1c6e6 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 bdd85acfc..366a2f19a 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 2d95638fb..b4c511d49 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 f47329187..14fb45e24 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 e4d7a5c8e..fbc975342 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 2a7e1d0c2..2c9815429 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 d90699525..03932f5d2 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 a2e0cb8b3..d9a9c2536 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 cb8f0b680..5b3400583 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 b25958a13..90f6b05be 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 c8b083e64..4f4ea6b19 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 b12e2a27f..b52898e75 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 3dd886c58..0dbbb45c9 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 15baff923..20d6fb063 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 d319318fb..c16f5190f 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 d9e9f6a9c..c23e3510c 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 fcfc6b7cd..965a49af9 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 02dacd073..c718ca270 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-d8c79a97-DEBUG \ No newline at end of file +DEV-63db3908-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index e0890f46b..4146cd6db 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_3281150270443704084) { - out_3281150270443704084[0] = delta_x[0] + nom_x[0]; - out_3281150270443704084[1] = delta_x[1] + nom_x[1]; - out_3281150270443704084[2] = delta_x[2] + nom_x[2]; - out_3281150270443704084[3] = delta_x[3] + nom_x[3]; - out_3281150270443704084[4] = delta_x[4] + nom_x[4]; - out_3281150270443704084[5] = delta_x[5] + nom_x[5]; - out_3281150270443704084[6] = delta_x[6] + nom_x[6]; - out_3281150270443704084[7] = delta_x[7] + nom_x[7]; - out_3281150270443704084[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_5171593721555630090) { + out_5171593721555630090[0] = delta_x[0] + nom_x[0]; + out_5171593721555630090[1] = delta_x[1] + nom_x[1]; + out_5171593721555630090[2] = delta_x[2] + nom_x[2]; + out_5171593721555630090[3] = delta_x[3] + nom_x[3]; + out_5171593721555630090[4] = delta_x[4] + nom_x[4]; + out_5171593721555630090[5] = delta_x[5] + nom_x[5]; + out_5171593721555630090[6] = delta_x[6] + nom_x[6]; + out_5171593721555630090[7] = delta_x[7] + nom_x[7]; + out_5171593721555630090[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3048073845424101937) { - out_3048073845424101937[0] = -nom_x[0] + true_x[0]; - out_3048073845424101937[1] = -nom_x[1] + true_x[1]; - out_3048073845424101937[2] = -nom_x[2] + true_x[2]; - out_3048073845424101937[3] = -nom_x[3] + true_x[3]; - out_3048073845424101937[4] = -nom_x[4] + true_x[4]; - out_3048073845424101937[5] = -nom_x[5] + true_x[5]; - out_3048073845424101937[6] = -nom_x[6] + true_x[6]; - out_3048073845424101937[7] = -nom_x[7] + true_x[7]; - out_3048073845424101937[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8159896824054378862) { + out_8159896824054378862[0] = -nom_x[0] + true_x[0]; + out_8159896824054378862[1] = -nom_x[1] + true_x[1]; + out_8159896824054378862[2] = -nom_x[2] + true_x[2]; + out_8159896824054378862[3] = -nom_x[3] + true_x[3]; + out_8159896824054378862[4] = -nom_x[4] + true_x[4]; + out_8159896824054378862[5] = -nom_x[5] + true_x[5]; + out_8159896824054378862[6] = -nom_x[6] + true_x[6]; + out_8159896824054378862[7] = -nom_x[7] + true_x[7]; + out_8159896824054378862[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_5861733525303608227) { - out_5861733525303608227[0] = 1.0; - out_5861733525303608227[1] = 0.0; - out_5861733525303608227[2] = 0.0; - out_5861733525303608227[3] = 0.0; - out_5861733525303608227[4] = 0.0; - out_5861733525303608227[5] = 0.0; - out_5861733525303608227[6] = 0.0; - out_5861733525303608227[7] = 0.0; - out_5861733525303608227[8] = 0.0; - out_5861733525303608227[9] = 0.0; - out_5861733525303608227[10] = 1.0; - out_5861733525303608227[11] = 0.0; - out_5861733525303608227[12] = 0.0; - out_5861733525303608227[13] = 0.0; - out_5861733525303608227[14] = 0.0; - out_5861733525303608227[15] = 0.0; - out_5861733525303608227[16] = 0.0; - out_5861733525303608227[17] = 0.0; - out_5861733525303608227[18] = 0.0; - out_5861733525303608227[19] = 0.0; - out_5861733525303608227[20] = 1.0; - out_5861733525303608227[21] = 0.0; - out_5861733525303608227[22] = 0.0; - out_5861733525303608227[23] = 0.0; - out_5861733525303608227[24] = 0.0; - out_5861733525303608227[25] = 0.0; - out_5861733525303608227[26] = 0.0; - out_5861733525303608227[27] = 0.0; - out_5861733525303608227[28] = 0.0; - out_5861733525303608227[29] = 0.0; - out_5861733525303608227[30] = 1.0; - out_5861733525303608227[31] = 0.0; - out_5861733525303608227[32] = 0.0; - out_5861733525303608227[33] = 0.0; - out_5861733525303608227[34] = 0.0; - out_5861733525303608227[35] = 0.0; - out_5861733525303608227[36] = 0.0; - out_5861733525303608227[37] = 0.0; - out_5861733525303608227[38] = 0.0; - out_5861733525303608227[39] = 0.0; - out_5861733525303608227[40] = 1.0; - out_5861733525303608227[41] = 0.0; - out_5861733525303608227[42] = 0.0; - out_5861733525303608227[43] = 0.0; - out_5861733525303608227[44] = 0.0; - out_5861733525303608227[45] = 0.0; - out_5861733525303608227[46] = 0.0; - out_5861733525303608227[47] = 0.0; - out_5861733525303608227[48] = 0.0; - out_5861733525303608227[49] = 0.0; - out_5861733525303608227[50] = 1.0; - out_5861733525303608227[51] = 0.0; - out_5861733525303608227[52] = 0.0; - out_5861733525303608227[53] = 0.0; - out_5861733525303608227[54] = 0.0; - out_5861733525303608227[55] = 0.0; - out_5861733525303608227[56] = 0.0; - out_5861733525303608227[57] = 0.0; - out_5861733525303608227[58] = 0.0; - out_5861733525303608227[59] = 0.0; - out_5861733525303608227[60] = 1.0; - out_5861733525303608227[61] = 0.0; - out_5861733525303608227[62] = 0.0; - out_5861733525303608227[63] = 0.0; - out_5861733525303608227[64] = 0.0; - out_5861733525303608227[65] = 0.0; - out_5861733525303608227[66] = 0.0; - out_5861733525303608227[67] = 0.0; - out_5861733525303608227[68] = 0.0; - out_5861733525303608227[69] = 0.0; - out_5861733525303608227[70] = 1.0; - out_5861733525303608227[71] = 0.0; - out_5861733525303608227[72] = 0.0; - out_5861733525303608227[73] = 0.0; - out_5861733525303608227[74] = 0.0; - out_5861733525303608227[75] = 0.0; - out_5861733525303608227[76] = 0.0; - out_5861733525303608227[77] = 0.0; - out_5861733525303608227[78] = 0.0; - out_5861733525303608227[79] = 0.0; - out_5861733525303608227[80] = 1.0; +void H_mod_fun(double *state, double *out_2621434864577473374) { + out_2621434864577473374[0] = 1.0; + out_2621434864577473374[1] = 0.0; + out_2621434864577473374[2] = 0.0; + out_2621434864577473374[3] = 0.0; + out_2621434864577473374[4] = 0.0; + out_2621434864577473374[5] = 0.0; + out_2621434864577473374[6] = 0.0; + out_2621434864577473374[7] = 0.0; + out_2621434864577473374[8] = 0.0; + out_2621434864577473374[9] = 0.0; + out_2621434864577473374[10] = 1.0; + out_2621434864577473374[11] = 0.0; + out_2621434864577473374[12] = 0.0; + out_2621434864577473374[13] = 0.0; + out_2621434864577473374[14] = 0.0; + out_2621434864577473374[15] = 0.0; + out_2621434864577473374[16] = 0.0; + out_2621434864577473374[17] = 0.0; + out_2621434864577473374[18] = 0.0; + out_2621434864577473374[19] = 0.0; + out_2621434864577473374[20] = 1.0; + out_2621434864577473374[21] = 0.0; + out_2621434864577473374[22] = 0.0; + out_2621434864577473374[23] = 0.0; + out_2621434864577473374[24] = 0.0; + out_2621434864577473374[25] = 0.0; + out_2621434864577473374[26] = 0.0; + out_2621434864577473374[27] = 0.0; + out_2621434864577473374[28] = 0.0; + out_2621434864577473374[29] = 0.0; + out_2621434864577473374[30] = 1.0; + out_2621434864577473374[31] = 0.0; + out_2621434864577473374[32] = 0.0; + out_2621434864577473374[33] = 0.0; + out_2621434864577473374[34] = 0.0; + out_2621434864577473374[35] = 0.0; + out_2621434864577473374[36] = 0.0; + out_2621434864577473374[37] = 0.0; + out_2621434864577473374[38] = 0.0; + out_2621434864577473374[39] = 0.0; + out_2621434864577473374[40] = 1.0; + out_2621434864577473374[41] = 0.0; + out_2621434864577473374[42] = 0.0; + out_2621434864577473374[43] = 0.0; + out_2621434864577473374[44] = 0.0; + out_2621434864577473374[45] = 0.0; + out_2621434864577473374[46] = 0.0; + out_2621434864577473374[47] = 0.0; + out_2621434864577473374[48] = 0.0; + out_2621434864577473374[49] = 0.0; + out_2621434864577473374[50] = 1.0; + out_2621434864577473374[51] = 0.0; + out_2621434864577473374[52] = 0.0; + out_2621434864577473374[53] = 0.0; + out_2621434864577473374[54] = 0.0; + out_2621434864577473374[55] = 0.0; + out_2621434864577473374[56] = 0.0; + out_2621434864577473374[57] = 0.0; + out_2621434864577473374[58] = 0.0; + out_2621434864577473374[59] = 0.0; + out_2621434864577473374[60] = 1.0; + out_2621434864577473374[61] = 0.0; + out_2621434864577473374[62] = 0.0; + out_2621434864577473374[63] = 0.0; + out_2621434864577473374[64] = 0.0; + out_2621434864577473374[65] = 0.0; + out_2621434864577473374[66] = 0.0; + out_2621434864577473374[67] = 0.0; + out_2621434864577473374[68] = 0.0; + out_2621434864577473374[69] = 0.0; + out_2621434864577473374[70] = 1.0; + out_2621434864577473374[71] = 0.0; + out_2621434864577473374[72] = 0.0; + out_2621434864577473374[73] = 0.0; + out_2621434864577473374[74] = 0.0; + out_2621434864577473374[75] = 0.0; + out_2621434864577473374[76] = 0.0; + out_2621434864577473374[77] = 0.0; + out_2621434864577473374[78] = 0.0; + out_2621434864577473374[79] = 0.0; + out_2621434864577473374[80] = 1.0; } -void f_fun(double *state, double dt, double *out_6520674902136446798) { - out_6520674902136446798[0] = state[0]; - out_6520674902136446798[1] = state[1]; - out_6520674902136446798[2] = state[2]; - out_6520674902136446798[3] = state[3]; - out_6520674902136446798[4] = state[4]; - out_6520674902136446798[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_6520674902136446798[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_6520674902136446798[7] = state[7]; - out_6520674902136446798[8] = state[8]; +void f_fun(double *state, double dt, double *out_5898225613213012342) { + out_5898225613213012342[0] = state[0]; + out_5898225613213012342[1] = state[1]; + out_5898225613213012342[2] = state[2]; + out_5898225613213012342[3] = state[3]; + out_5898225613213012342[4] = state[4]; + out_5898225613213012342[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_5898225613213012342[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_5898225613213012342[7] = state[7]; + out_5898225613213012342[8] = state[8]; } -void F_fun(double *state, double dt, double *out_2036702211381872187) { - out_2036702211381872187[0] = 1; - out_2036702211381872187[1] = 0; - out_2036702211381872187[2] = 0; - out_2036702211381872187[3] = 0; - out_2036702211381872187[4] = 0; - out_2036702211381872187[5] = 0; - out_2036702211381872187[6] = 0; - out_2036702211381872187[7] = 0; - out_2036702211381872187[8] = 0; - out_2036702211381872187[9] = 0; - out_2036702211381872187[10] = 1; - out_2036702211381872187[11] = 0; - out_2036702211381872187[12] = 0; - out_2036702211381872187[13] = 0; - out_2036702211381872187[14] = 0; - out_2036702211381872187[15] = 0; - out_2036702211381872187[16] = 0; - out_2036702211381872187[17] = 0; - out_2036702211381872187[18] = 0; - out_2036702211381872187[19] = 0; - out_2036702211381872187[20] = 1; - out_2036702211381872187[21] = 0; - out_2036702211381872187[22] = 0; - out_2036702211381872187[23] = 0; - out_2036702211381872187[24] = 0; - out_2036702211381872187[25] = 0; - out_2036702211381872187[26] = 0; - out_2036702211381872187[27] = 0; - out_2036702211381872187[28] = 0; - out_2036702211381872187[29] = 0; - out_2036702211381872187[30] = 1; - out_2036702211381872187[31] = 0; - out_2036702211381872187[32] = 0; - out_2036702211381872187[33] = 0; - out_2036702211381872187[34] = 0; - out_2036702211381872187[35] = 0; - out_2036702211381872187[36] = 0; - out_2036702211381872187[37] = 0; - out_2036702211381872187[38] = 0; - out_2036702211381872187[39] = 0; - out_2036702211381872187[40] = 1; - out_2036702211381872187[41] = 0; - out_2036702211381872187[42] = 0; - out_2036702211381872187[43] = 0; - out_2036702211381872187[44] = 0; - out_2036702211381872187[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_2036702211381872187[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_2036702211381872187[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2036702211381872187[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2036702211381872187[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_2036702211381872187[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_2036702211381872187[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_2036702211381872187[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_2036702211381872187[53] = -9.8100000000000005*dt; - out_2036702211381872187[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_2036702211381872187[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_2036702211381872187[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2036702211381872187[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2036702211381872187[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_2036702211381872187[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_2036702211381872187[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_2036702211381872187[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2036702211381872187[62] = 0; - out_2036702211381872187[63] = 0; - out_2036702211381872187[64] = 0; - out_2036702211381872187[65] = 0; - out_2036702211381872187[66] = 0; - out_2036702211381872187[67] = 0; - out_2036702211381872187[68] = 0; - out_2036702211381872187[69] = 0; - out_2036702211381872187[70] = 1; - out_2036702211381872187[71] = 0; - out_2036702211381872187[72] = 0; - out_2036702211381872187[73] = 0; - out_2036702211381872187[74] = 0; - out_2036702211381872187[75] = 0; - out_2036702211381872187[76] = 0; - out_2036702211381872187[77] = 0; - out_2036702211381872187[78] = 0; - out_2036702211381872187[79] = 0; - out_2036702211381872187[80] = 1; +void F_fun(double *state, double dt, double *out_289209120563918309) { + out_289209120563918309[0] = 1; + out_289209120563918309[1] = 0; + out_289209120563918309[2] = 0; + out_289209120563918309[3] = 0; + out_289209120563918309[4] = 0; + out_289209120563918309[5] = 0; + out_289209120563918309[6] = 0; + out_289209120563918309[7] = 0; + out_289209120563918309[8] = 0; + out_289209120563918309[9] = 0; + out_289209120563918309[10] = 1; + out_289209120563918309[11] = 0; + out_289209120563918309[12] = 0; + out_289209120563918309[13] = 0; + out_289209120563918309[14] = 0; + out_289209120563918309[15] = 0; + out_289209120563918309[16] = 0; + out_289209120563918309[17] = 0; + out_289209120563918309[18] = 0; + out_289209120563918309[19] = 0; + out_289209120563918309[20] = 1; + out_289209120563918309[21] = 0; + out_289209120563918309[22] = 0; + out_289209120563918309[23] = 0; + out_289209120563918309[24] = 0; + out_289209120563918309[25] = 0; + out_289209120563918309[26] = 0; + out_289209120563918309[27] = 0; + out_289209120563918309[28] = 0; + out_289209120563918309[29] = 0; + out_289209120563918309[30] = 1; + out_289209120563918309[31] = 0; + out_289209120563918309[32] = 0; + out_289209120563918309[33] = 0; + out_289209120563918309[34] = 0; + out_289209120563918309[35] = 0; + out_289209120563918309[36] = 0; + out_289209120563918309[37] = 0; + out_289209120563918309[38] = 0; + out_289209120563918309[39] = 0; + out_289209120563918309[40] = 1; + out_289209120563918309[41] = 0; + out_289209120563918309[42] = 0; + out_289209120563918309[43] = 0; + out_289209120563918309[44] = 0; + out_289209120563918309[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_289209120563918309[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_289209120563918309[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_289209120563918309[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_289209120563918309[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_289209120563918309[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_289209120563918309[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_289209120563918309[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_289209120563918309[53] = -9.8100000000000005*dt; + out_289209120563918309[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_289209120563918309[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_289209120563918309[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_289209120563918309[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_289209120563918309[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_289209120563918309[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_289209120563918309[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_289209120563918309[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_289209120563918309[62] = 0; + out_289209120563918309[63] = 0; + out_289209120563918309[64] = 0; + out_289209120563918309[65] = 0; + out_289209120563918309[66] = 0; + out_289209120563918309[67] = 0; + out_289209120563918309[68] = 0; + out_289209120563918309[69] = 0; + out_289209120563918309[70] = 1; + out_289209120563918309[71] = 0; + out_289209120563918309[72] = 0; + out_289209120563918309[73] = 0; + out_289209120563918309[74] = 0; + out_289209120563918309[75] = 0; + out_289209120563918309[76] = 0; + out_289209120563918309[77] = 0; + out_289209120563918309[78] = 0; + out_289209120563918309[79] = 0; + out_289209120563918309[80] = 1; } -void h_25(double *state, double *unused, double *out_7733856010300149046) { - out_7733856010300149046[0] = state[6]; +void h_25(double *state, double *unused, double *out_1048536752434567181) { + out_1048536752434567181[0] = state[6]; } -void H_25(double *state, double *unused, double *out_6810854447332090436) { - out_6810854447332090436[0] = 0; - out_6810854447332090436[1] = 0; - out_6810854447332090436[2] = 0; - out_6810854447332090436[3] = 0; - out_6810854447332090436[4] = 0; - out_6810854447332090436[5] = 0; - out_6810854447332090436[6] = 1; - out_6810854447332090436[7] = 0; - out_6810854447332090436[8] = 0; +void H_25(double *state, double *unused, double *out_6632120983219841005) { + out_6632120983219841005[0] = 0; + out_6632120983219841005[1] = 0; + out_6632120983219841005[2] = 0; + out_6632120983219841005[3] = 0; + out_6632120983219841005[4] = 0; + out_6632120983219841005[5] = 0; + out_6632120983219841005[6] = 1; + out_6632120983219841005[7] = 0; + out_6632120983219841005[8] = 0; } -void h_24(double *state, double *unused, double *out_8048847696560640687) { - out_8048847696560640687[0] = state[4]; - out_8048847696560640687[1] = state[5]; +void h_24(double *state, double *unused, double *out_7601461491001714773) { + out_7601461491001714773[0] = state[4]; + out_7601461491001714773[1] = state[5]; } -void H_24(double *state, double *unused, double *out_2407824440308265955) { - out_2407824440308265955[0] = 0; - out_2407824440308265955[1] = 0; - out_2407824440308265955[2] = 0; - out_2407824440308265955[3] = 0; - out_2407824440308265955[4] = 1; - out_2407824440308265955[5] = 0; - out_2407824440308265955[6] = 0; - out_2407824440308265955[7] = 0; - out_2407824440308265955[8] = 0; - out_2407824440308265955[9] = 0; - out_2407824440308265955[10] = 0; - out_2407824440308265955[11] = 0; - out_2407824440308265955[12] = 0; - out_2407824440308265955[13] = 0; - out_2407824440308265955[14] = 1; - out_2407824440308265955[15] = 0; - out_2407824440308265955[16] = 0; - out_2407824440308265955[17] = 0; +void H_24(double *state, double *unused, double *out_4459471384214341439) { + out_4459471384214341439[0] = 0; + out_4459471384214341439[1] = 0; + out_4459471384214341439[2] = 0; + out_4459471384214341439[3] = 0; + out_4459471384214341439[4] = 1; + out_4459471384214341439[5] = 0; + out_4459471384214341439[6] = 0; + out_4459471384214341439[7] = 0; + out_4459471384214341439[8] = 0; + out_4459471384214341439[9] = 0; + out_4459471384214341439[10] = 0; + out_4459471384214341439[11] = 0; + out_4459471384214341439[12] = 0; + out_4459471384214341439[13] = 0; + out_4459471384214341439[14] = 1; + out_4459471384214341439[15] = 0; + out_4459471384214341439[16] = 0; + out_4459471384214341439[17] = 0; } -void h_30(double *state, double *unused, double *out_7458661948015643157) { - out_7458661948015643157[0] = state[4]; +void h_30(double *state, double *unused, double *out_4685986280688589531) { + out_4685986280688589531[0] = state[4]; } -void H_30(double *state, double *unused, double *out_2283158117204482238) { - out_2283158117204482238[0] = 0; - out_2283158117204482238[1] = 0; - out_2283158117204482238[2] = 0; - out_2283158117204482238[3] = 0; - out_2283158117204482238[4] = 1; - out_2283158117204482238[5] = 0; - out_2283158117204482238[6] = 0; - out_2283158117204482238[7] = 0; - out_2283158117204482238[8] = 0; +void H_30(double *state, double *unused, double *out_9150453941727089632) { + out_9150453941727089632[0] = 0; + out_9150453941727089632[1] = 0; + out_9150453941727089632[2] = 0; + out_9150453941727089632[3] = 0; + out_9150453941727089632[4] = 1; + out_9150453941727089632[5] = 0; + out_9150453941727089632[6] = 0; + out_9150453941727089632[7] = 0; + out_9150453941727089632[8] = 0; } -void h_26(double *state, double *unused, double *out_5109796638135193371) { - out_5109796638135193371[0] = state[7]; +void h_26(double *state, double *unused, double *out_4082670396012112014) { + out_4082670396012112014[0] = state[7]; } -void H_26(double *state, double *unused, double *out_3069351128458034212) { - out_3069351128458034212[0] = 0; - out_3069351128458034212[1] = 0; - out_3069351128458034212[2] = 0; - out_3069351128458034212[3] = 0; - out_3069351128458034212[4] = 0; - out_3069351128458034212[5] = 0; - out_3069351128458034212[6] = 0; - out_3069351128458034212[7] = 1; - out_3069351128458034212[8] = 0; +void H_26(double *state, double *unused, double *out_8510097120728910010) { + out_8510097120728910010[0] = 0; + out_8510097120728910010[1] = 0; + out_8510097120728910010[2] = 0; + out_8510097120728910010[3] = 0; + out_8510097120728910010[4] = 0; + out_8510097120728910010[5] = 0; + out_8510097120728910010[6] = 0; + out_8510097120728910010[7] = 1; + out_8510097120728910010[8] = 0; } -void h_27(double *state, double *unused, double *out_2732182742639303401) { - out_2732182742639303401[0] = state[3]; +void h_27(double *state, double *unused, double *out_9178210209371601407) { + out_9178210209371601407[0] = state[3]; } -void H_27(double *state, double *unused, double *out_108394805404057327) { - out_108394805404057327[0] = 0; - out_108394805404057327[1] = 0; - out_108394805404057327[2] = 0; - out_108394805404057327[3] = 1; - out_108394805404057327[4] = 0; - out_108394805404057327[5] = 0; - out_108394805404057327[6] = 0; - out_108394805404057327[7] = 0; - out_108394805404057327[8] = 0; +void H_27(double *state, double *unused, double *out_7072696060798518767) { + out_7072696060798518767[0] = 0; + out_7072696060798518767[1] = 0; + out_7072696060798518767[2] = 0; + out_7072696060798518767[3] = 1; + out_7072696060798518767[4] = 0; + out_7072696060798518767[5] = 0; + out_7072696060798518767[6] = 0; + out_7072696060798518767[7] = 0; + out_7072696060798518767[8] = 0; } -void h_29(double *state, double *unused, double *out_1129500733789198251) { - out_1129500733789198251[0] = state[1]; +void h_29(double *state, double *unused, double *out_8534850577185637138) { + out_8534850577185637138[0] = state[1]; } -void H_29(double *state, double *unused, double *out_2793389461518874422) { - out_2793389461518874422[0] = 0; - out_2793389461518874422[1] = 1; - out_2793389461518874422[2] = 0; - out_2793389461518874422[3] = 0; - out_2793389461518874422[4] = 0; - out_2793389461518874422[5] = 0; - out_2793389461518874422[6] = 0; - out_2793389461518874422[7] = 0; - out_2793389461518874422[8] = 0; +void H_29(double *state, double *unused, double *out_8786058787668069800) { + out_8786058787668069800[0] = 0; + out_8786058787668069800[1] = 1; + out_8786058787668069800[2] = 0; + out_8786058787668069800[3] = 0; + out_8786058787668069800[4] = 0; + out_8786058787668069800[5] = 0; + out_8786058787668069800[6] = 0; + out_8786058787668069800[7] = 0; + out_8786058787668069800[8] = 0; } -void h_28(double *state, double *unused, double *out_4422630577374984361) { - out_4422630577374984361[0] = state[0]; +void h_28(double *state, double *unused, double *out_2931759151911427939) { + out_2931759151911427939[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2289009555550656152) { - out_2289009555550656152[0] = 1; - out_2289009555550656152[1] = 0; - out_2289009555550656152[2] = 0; - out_2289009555550656152[3] = 0; - out_2289009555550656152[4] = 0; - out_2289009555550656152[5] = 0; - out_2289009555550656152[6] = 0; - out_2289009555550656152[7] = 0; - out_2289009555550656152[8] = 0; +void H_28(double *state, double *unused, double *out_4578286268971951242) { + out_4578286268971951242[0] = 1; + out_4578286268971951242[1] = 0; + out_4578286268971951242[2] = 0; + out_4578286268971951242[3] = 0; + out_4578286268971951242[4] = 0; + out_4578286268971951242[5] = 0; + out_4578286268971951242[6] = 0; + out_4578286268971951242[7] = 0; + out_4578286268971951242[8] = 0; } -void h_31(double *state, double *unused, double *out_7765162073053917609) { - out_7765162073053917609[0] = state[8]; +void h_31(double *state, double *unused, double *out_5027768451407450342) { + out_5027768451407450342[0] = state[8]; } -void H_31(double *state, double *unused, double *out_2443143026224682736) { - out_2443143026224682736[0] = 0; - out_2443143026224682736[1] = 0; - out_2443143026224682736[2] = 0; - out_2443143026224682736[3] = 0; - out_2443143026224682736[4] = 0; - out_2443143026224682736[5] = 0; - out_2443143026224682736[6] = 0; - out_2443143026224682736[7] = 0; - out_2443143026224682736[8] = 1; +void H_31(double *state, double *unused, double *out_4737947839977893358) { + out_4737947839977893358[0] = 0; + out_4737947839977893358[1] = 0; + out_4737947839977893358[2] = 0; + out_4737947839977893358[3] = 0; + out_4737947839977893358[4] = 0; + out_4737947839977893358[5] = 0; + out_4737947839977893358[6] = 0; + out_4737947839977893358[7] = 0; + out_4737947839977893358[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_3281150270443704084) { - err_fun(nom_x, delta_x, out_3281150270443704084); +void car_err_fun(double *nom_x, double *delta_x, double *out_5171593721555630090) { + err_fun(nom_x, delta_x, out_5171593721555630090); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3048073845424101937) { - inv_err_fun(nom_x, true_x, out_3048073845424101937); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8159896824054378862) { + inv_err_fun(nom_x, true_x, out_8159896824054378862); } -void car_H_mod_fun(double *state, double *out_5861733525303608227) { - H_mod_fun(state, out_5861733525303608227); +void car_H_mod_fun(double *state, double *out_2621434864577473374) { + H_mod_fun(state, out_2621434864577473374); } -void car_f_fun(double *state, double dt, double *out_6520674902136446798) { - f_fun(state, dt, out_6520674902136446798); +void car_f_fun(double *state, double dt, double *out_5898225613213012342) { + f_fun(state, dt, out_5898225613213012342); } -void car_F_fun(double *state, double dt, double *out_2036702211381872187) { - F_fun(state, dt, out_2036702211381872187); +void car_F_fun(double *state, double dt, double *out_289209120563918309) { + F_fun(state, dt, out_289209120563918309); } -void car_h_25(double *state, double *unused, double *out_7733856010300149046) { - h_25(state, unused, out_7733856010300149046); +void car_h_25(double *state, double *unused, double *out_1048536752434567181) { + h_25(state, unused, out_1048536752434567181); } -void car_H_25(double *state, double *unused, double *out_6810854447332090436) { - H_25(state, unused, out_6810854447332090436); +void car_H_25(double *state, double *unused, double *out_6632120983219841005) { + H_25(state, unused, out_6632120983219841005); } -void car_h_24(double *state, double *unused, double *out_8048847696560640687) { - h_24(state, unused, out_8048847696560640687); +void car_h_24(double *state, double *unused, double *out_7601461491001714773) { + h_24(state, unused, out_7601461491001714773); } -void car_H_24(double *state, double *unused, double *out_2407824440308265955) { - H_24(state, unused, out_2407824440308265955); +void car_H_24(double *state, double *unused, double *out_4459471384214341439) { + H_24(state, unused, out_4459471384214341439); } -void car_h_30(double *state, double *unused, double *out_7458661948015643157) { - h_30(state, unused, out_7458661948015643157); +void car_h_30(double *state, double *unused, double *out_4685986280688589531) { + h_30(state, unused, out_4685986280688589531); } -void car_H_30(double *state, double *unused, double *out_2283158117204482238) { - H_30(state, unused, out_2283158117204482238); +void car_H_30(double *state, double *unused, double *out_9150453941727089632) { + H_30(state, unused, out_9150453941727089632); } -void car_h_26(double *state, double *unused, double *out_5109796638135193371) { - h_26(state, unused, out_5109796638135193371); +void car_h_26(double *state, double *unused, double *out_4082670396012112014) { + h_26(state, unused, out_4082670396012112014); } -void car_H_26(double *state, double *unused, double *out_3069351128458034212) { - H_26(state, unused, out_3069351128458034212); +void car_H_26(double *state, double *unused, double *out_8510097120728910010) { + H_26(state, unused, out_8510097120728910010); } -void car_h_27(double *state, double *unused, double *out_2732182742639303401) { - h_27(state, unused, out_2732182742639303401); +void car_h_27(double *state, double *unused, double *out_9178210209371601407) { + h_27(state, unused, out_9178210209371601407); } -void car_H_27(double *state, double *unused, double *out_108394805404057327) { - H_27(state, unused, out_108394805404057327); +void car_H_27(double *state, double *unused, double *out_7072696060798518767) { + H_27(state, unused, out_7072696060798518767); } -void car_h_29(double *state, double *unused, double *out_1129500733789198251) { - h_29(state, unused, out_1129500733789198251); +void car_h_29(double *state, double *unused, double *out_8534850577185637138) { + h_29(state, unused, out_8534850577185637138); } -void car_H_29(double *state, double *unused, double *out_2793389461518874422) { - H_29(state, unused, out_2793389461518874422); +void car_H_29(double *state, double *unused, double *out_8786058787668069800) { + H_29(state, unused, out_8786058787668069800); } -void car_h_28(double *state, double *unused, double *out_4422630577374984361) { - h_28(state, unused, out_4422630577374984361); +void car_h_28(double *state, double *unused, double *out_2931759151911427939) { + h_28(state, unused, out_2931759151911427939); } -void car_H_28(double *state, double *unused, double *out_2289009555550656152) { - H_28(state, unused, out_2289009555550656152); +void car_H_28(double *state, double *unused, double *out_4578286268971951242) { + H_28(state, unused, out_4578286268971951242); } -void car_h_31(double *state, double *unused, double *out_7765162073053917609) { - h_31(state, unused, out_7765162073053917609); +void car_h_31(double *state, double *unused, double *out_5027768451407450342) { + h_31(state, unused, out_5027768451407450342); } -void car_H_31(double *state, double *unused, double *out_2443143026224682736) { - H_31(state, unused, out_2443143026224682736); +void car_H_31(double *state, double *unused, double *out_4737947839977893358) { + H_31(state, unused, out_4737947839977893358); } 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 455985ab6..d0e70c29b 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_3281150270443704084); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3048073845424101937); -void car_H_mod_fun(double *state, double *out_5861733525303608227); -void car_f_fun(double *state, double dt, double *out_6520674902136446798); -void car_F_fun(double *state, double dt, double *out_2036702211381872187); -void car_h_25(double *state, double *unused, double *out_7733856010300149046); -void car_H_25(double *state, double *unused, double *out_6810854447332090436); -void car_h_24(double *state, double *unused, double *out_8048847696560640687); -void car_H_24(double *state, double *unused, double *out_2407824440308265955); -void car_h_30(double *state, double *unused, double *out_7458661948015643157); -void car_H_30(double *state, double *unused, double *out_2283158117204482238); -void car_h_26(double *state, double *unused, double *out_5109796638135193371); -void car_H_26(double *state, double *unused, double *out_3069351128458034212); -void car_h_27(double *state, double *unused, double *out_2732182742639303401); -void car_H_27(double *state, double *unused, double *out_108394805404057327); -void car_h_29(double *state, double *unused, double *out_1129500733789198251); -void car_H_29(double *state, double *unused, double *out_2793389461518874422); -void car_h_28(double *state, double *unused, double *out_4422630577374984361); -void car_H_28(double *state, double *unused, double *out_2289009555550656152); -void car_h_31(double *state, double *unused, double *out_7765162073053917609); -void car_H_31(double *state, double *unused, double *out_2443143026224682736); +void car_err_fun(double *nom_x, double *delta_x, double *out_5171593721555630090); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8159896824054378862); +void car_H_mod_fun(double *state, double *out_2621434864577473374); +void car_f_fun(double *state, double dt, double *out_5898225613213012342); +void car_F_fun(double *state, double dt, double *out_289209120563918309); +void car_h_25(double *state, double *unused, double *out_1048536752434567181); +void car_H_25(double *state, double *unused, double *out_6632120983219841005); +void car_h_24(double *state, double *unused, double *out_7601461491001714773); +void car_H_24(double *state, double *unused, double *out_4459471384214341439); +void car_h_30(double *state, double *unused, double *out_4685986280688589531); +void car_H_30(double *state, double *unused, double *out_9150453941727089632); +void car_h_26(double *state, double *unused, double *out_4082670396012112014); +void car_H_26(double *state, double *unused, double *out_8510097120728910010); +void car_h_27(double *state, double *unused, double *out_9178210209371601407); +void car_H_27(double *state, double *unused, double *out_7072696060798518767); +void car_h_29(double *state, double *unused, double *out_8534850577185637138); +void car_H_29(double *state, double *unused, double *out_8786058787668069800); +void car_h_28(double *state, double *unused, double *out_2931759151911427939); +void car_H_28(double *state, double *unused, double *out_4578286268971951242); +void car_h_31(double *state, double *unused, double *out_5027768451407450342); +void car_H_31(double *state, double *unused, double *out_4737947839977893358); 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 e1ede770a..292540c84 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_6531280423659383110) { - out_6531280423659383110[0] = delta_x[0] + nom_x[0]; - out_6531280423659383110[1] = delta_x[1] + nom_x[1]; - out_6531280423659383110[2] = delta_x[2] + nom_x[2]; - out_6531280423659383110[3] = delta_x[3] + nom_x[3]; - out_6531280423659383110[4] = delta_x[4] + nom_x[4]; - out_6531280423659383110[5] = delta_x[5] + nom_x[5]; - out_6531280423659383110[6] = delta_x[6] + nom_x[6]; - out_6531280423659383110[7] = delta_x[7] + nom_x[7]; - out_6531280423659383110[8] = delta_x[8] + nom_x[8]; - out_6531280423659383110[9] = delta_x[9] + nom_x[9]; - out_6531280423659383110[10] = delta_x[10] + nom_x[10]; - out_6531280423659383110[11] = delta_x[11] + nom_x[11]; - out_6531280423659383110[12] = delta_x[12] + nom_x[12]; - out_6531280423659383110[13] = delta_x[13] + nom_x[13]; - out_6531280423659383110[14] = delta_x[14] + nom_x[14]; - out_6531280423659383110[15] = delta_x[15] + nom_x[15]; - out_6531280423659383110[16] = delta_x[16] + nom_x[16]; - out_6531280423659383110[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_3892827861815398533) { + out_3892827861815398533[0] = delta_x[0] + nom_x[0]; + out_3892827861815398533[1] = delta_x[1] + nom_x[1]; + out_3892827861815398533[2] = delta_x[2] + nom_x[2]; + out_3892827861815398533[3] = delta_x[3] + nom_x[3]; + out_3892827861815398533[4] = delta_x[4] + nom_x[4]; + out_3892827861815398533[5] = delta_x[5] + nom_x[5]; + out_3892827861815398533[6] = delta_x[6] + nom_x[6]; + out_3892827861815398533[7] = delta_x[7] + nom_x[7]; + out_3892827861815398533[8] = delta_x[8] + nom_x[8]; + out_3892827861815398533[9] = delta_x[9] + nom_x[9]; + out_3892827861815398533[10] = delta_x[10] + nom_x[10]; + out_3892827861815398533[11] = delta_x[11] + nom_x[11]; + out_3892827861815398533[12] = delta_x[12] + nom_x[12]; + out_3892827861815398533[13] = delta_x[13] + nom_x[13]; + out_3892827861815398533[14] = delta_x[14] + nom_x[14]; + out_3892827861815398533[15] = delta_x[15] + nom_x[15]; + out_3892827861815398533[16] = delta_x[16] + nom_x[16]; + out_3892827861815398533[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_2847042605170284069) { - out_2847042605170284069[0] = -nom_x[0] + true_x[0]; - out_2847042605170284069[1] = -nom_x[1] + true_x[1]; - out_2847042605170284069[2] = -nom_x[2] + true_x[2]; - out_2847042605170284069[3] = -nom_x[3] + true_x[3]; - out_2847042605170284069[4] = -nom_x[4] + true_x[4]; - out_2847042605170284069[5] = -nom_x[5] + true_x[5]; - out_2847042605170284069[6] = -nom_x[6] + true_x[6]; - out_2847042605170284069[7] = -nom_x[7] + true_x[7]; - out_2847042605170284069[8] = -nom_x[8] + true_x[8]; - out_2847042605170284069[9] = -nom_x[9] + true_x[9]; - out_2847042605170284069[10] = -nom_x[10] + true_x[10]; - out_2847042605170284069[11] = -nom_x[11] + true_x[11]; - out_2847042605170284069[12] = -nom_x[12] + true_x[12]; - out_2847042605170284069[13] = -nom_x[13] + true_x[13]; - out_2847042605170284069[14] = -nom_x[14] + true_x[14]; - out_2847042605170284069[15] = -nom_x[15] + true_x[15]; - out_2847042605170284069[16] = -nom_x[16] + true_x[16]; - out_2847042605170284069[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_341451131886162609) { + out_341451131886162609[0] = -nom_x[0] + true_x[0]; + out_341451131886162609[1] = -nom_x[1] + true_x[1]; + out_341451131886162609[2] = -nom_x[2] + true_x[2]; + out_341451131886162609[3] = -nom_x[3] + true_x[3]; + out_341451131886162609[4] = -nom_x[4] + true_x[4]; + out_341451131886162609[5] = -nom_x[5] + true_x[5]; + out_341451131886162609[6] = -nom_x[6] + true_x[6]; + out_341451131886162609[7] = -nom_x[7] + true_x[7]; + out_341451131886162609[8] = -nom_x[8] + true_x[8]; + out_341451131886162609[9] = -nom_x[9] + true_x[9]; + out_341451131886162609[10] = -nom_x[10] + true_x[10]; + out_341451131886162609[11] = -nom_x[11] + true_x[11]; + out_341451131886162609[12] = -nom_x[12] + true_x[12]; + out_341451131886162609[13] = -nom_x[13] + true_x[13]; + out_341451131886162609[14] = -nom_x[14] + true_x[14]; + out_341451131886162609[15] = -nom_x[15] + true_x[15]; + out_341451131886162609[16] = -nom_x[16] + true_x[16]; + out_341451131886162609[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_1201664963462517165) { - out_1201664963462517165[0] = 1.0; - out_1201664963462517165[1] = 0.0; - out_1201664963462517165[2] = 0.0; - out_1201664963462517165[3] = 0.0; - out_1201664963462517165[4] = 0.0; - out_1201664963462517165[5] = 0.0; - out_1201664963462517165[6] = 0.0; - out_1201664963462517165[7] = 0.0; - out_1201664963462517165[8] = 0.0; - out_1201664963462517165[9] = 0.0; - out_1201664963462517165[10] = 0.0; - out_1201664963462517165[11] = 0.0; - out_1201664963462517165[12] = 0.0; - out_1201664963462517165[13] = 0.0; - out_1201664963462517165[14] = 0.0; - out_1201664963462517165[15] = 0.0; - out_1201664963462517165[16] = 0.0; - out_1201664963462517165[17] = 0.0; - out_1201664963462517165[18] = 0.0; - out_1201664963462517165[19] = 1.0; - out_1201664963462517165[20] = 0.0; - out_1201664963462517165[21] = 0.0; - out_1201664963462517165[22] = 0.0; - out_1201664963462517165[23] = 0.0; - out_1201664963462517165[24] = 0.0; - out_1201664963462517165[25] = 0.0; - out_1201664963462517165[26] = 0.0; - out_1201664963462517165[27] = 0.0; - out_1201664963462517165[28] = 0.0; - out_1201664963462517165[29] = 0.0; - out_1201664963462517165[30] = 0.0; - out_1201664963462517165[31] = 0.0; - out_1201664963462517165[32] = 0.0; - out_1201664963462517165[33] = 0.0; - out_1201664963462517165[34] = 0.0; - out_1201664963462517165[35] = 0.0; - out_1201664963462517165[36] = 0.0; - out_1201664963462517165[37] = 0.0; - out_1201664963462517165[38] = 1.0; - out_1201664963462517165[39] = 0.0; - out_1201664963462517165[40] = 0.0; - out_1201664963462517165[41] = 0.0; - out_1201664963462517165[42] = 0.0; - out_1201664963462517165[43] = 0.0; - out_1201664963462517165[44] = 0.0; - out_1201664963462517165[45] = 0.0; - out_1201664963462517165[46] = 0.0; - out_1201664963462517165[47] = 0.0; - out_1201664963462517165[48] = 0.0; - out_1201664963462517165[49] = 0.0; - out_1201664963462517165[50] = 0.0; - out_1201664963462517165[51] = 0.0; - out_1201664963462517165[52] = 0.0; - out_1201664963462517165[53] = 0.0; - out_1201664963462517165[54] = 0.0; - out_1201664963462517165[55] = 0.0; - out_1201664963462517165[56] = 0.0; - out_1201664963462517165[57] = 1.0; - out_1201664963462517165[58] = 0.0; - out_1201664963462517165[59] = 0.0; - out_1201664963462517165[60] = 0.0; - out_1201664963462517165[61] = 0.0; - out_1201664963462517165[62] = 0.0; - out_1201664963462517165[63] = 0.0; - out_1201664963462517165[64] = 0.0; - out_1201664963462517165[65] = 0.0; - out_1201664963462517165[66] = 0.0; - out_1201664963462517165[67] = 0.0; - out_1201664963462517165[68] = 0.0; - out_1201664963462517165[69] = 0.0; - out_1201664963462517165[70] = 0.0; - out_1201664963462517165[71] = 0.0; - out_1201664963462517165[72] = 0.0; - out_1201664963462517165[73] = 0.0; - out_1201664963462517165[74] = 0.0; - out_1201664963462517165[75] = 0.0; - out_1201664963462517165[76] = 1.0; - out_1201664963462517165[77] = 0.0; - out_1201664963462517165[78] = 0.0; - out_1201664963462517165[79] = 0.0; - out_1201664963462517165[80] = 0.0; - out_1201664963462517165[81] = 0.0; - out_1201664963462517165[82] = 0.0; - out_1201664963462517165[83] = 0.0; - out_1201664963462517165[84] = 0.0; - out_1201664963462517165[85] = 0.0; - out_1201664963462517165[86] = 0.0; - out_1201664963462517165[87] = 0.0; - out_1201664963462517165[88] = 0.0; - out_1201664963462517165[89] = 0.0; - out_1201664963462517165[90] = 0.0; - out_1201664963462517165[91] = 0.0; - out_1201664963462517165[92] = 0.0; - out_1201664963462517165[93] = 0.0; - out_1201664963462517165[94] = 0.0; - out_1201664963462517165[95] = 1.0; - out_1201664963462517165[96] = 0.0; - out_1201664963462517165[97] = 0.0; - out_1201664963462517165[98] = 0.0; - out_1201664963462517165[99] = 0.0; - out_1201664963462517165[100] = 0.0; - out_1201664963462517165[101] = 0.0; - out_1201664963462517165[102] = 0.0; - out_1201664963462517165[103] = 0.0; - out_1201664963462517165[104] = 0.0; - out_1201664963462517165[105] = 0.0; - out_1201664963462517165[106] = 0.0; - out_1201664963462517165[107] = 0.0; - out_1201664963462517165[108] = 0.0; - out_1201664963462517165[109] = 0.0; - out_1201664963462517165[110] = 0.0; - out_1201664963462517165[111] = 0.0; - out_1201664963462517165[112] = 0.0; - out_1201664963462517165[113] = 0.0; - out_1201664963462517165[114] = 1.0; - out_1201664963462517165[115] = 0.0; - out_1201664963462517165[116] = 0.0; - out_1201664963462517165[117] = 0.0; - out_1201664963462517165[118] = 0.0; - out_1201664963462517165[119] = 0.0; - out_1201664963462517165[120] = 0.0; - out_1201664963462517165[121] = 0.0; - out_1201664963462517165[122] = 0.0; - out_1201664963462517165[123] = 0.0; - out_1201664963462517165[124] = 0.0; - out_1201664963462517165[125] = 0.0; - out_1201664963462517165[126] = 0.0; - out_1201664963462517165[127] = 0.0; - out_1201664963462517165[128] = 0.0; - out_1201664963462517165[129] = 0.0; - out_1201664963462517165[130] = 0.0; - out_1201664963462517165[131] = 0.0; - out_1201664963462517165[132] = 0.0; - out_1201664963462517165[133] = 1.0; - out_1201664963462517165[134] = 0.0; - out_1201664963462517165[135] = 0.0; - out_1201664963462517165[136] = 0.0; - out_1201664963462517165[137] = 0.0; - out_1201664963462517165[138] = 0.0; - out_1201664963462517165[139] = 0.0; - out_1201664963462517165[140] = 0.0; - out_1201664963462517165[141] = 0.0; - out_1201664963462517165[142] = 0.0; - out_1201664963462517165[143] = 0.0; - out_1201664963462517165[144] = 0.0; - out_1201664963462517165[145] = 0.0; - out_1201664963462517165[146] = 0.0; - out_1201664963462517165[147] = 0.0; - out_1201664963462517165[148] = 0.0; - out_1201664963462517165[149] = 0.0; - out_1201664963462517165[150] = 0.0; - out_1201664963462517165[151] = 0.0; - out_1201664963462517165[152] = 1.0; - out_1201664963462517165[153] = 0.0; - out_1201664963462517165[154] = 0.0; - out_1201664963462517165[155] = 0.0; - out_1201664963462517165[156] = 0.0; - out_1201664963462517165[157] = 0.0; - out_1201664963462517165[158] = 0.0; - out_1201664963462517165[159] = 0.0; - out_1201664963462517165[160] = 0.0; - out_1201664963462517165[161] = 0.0; - out_1201664963462517165[162] = 0.0; - out_1201664963462517165[163] = 0.0; - out_1201664963462517165[164] = 0.0; - out_1201664963462517165[165] = 0.0; - out_1201664963462517165[166] = 0.0; - out_1201664963462517165[167] = 0.0; - out_1201664963462517165[168] = 0.0; - out_1201664963462517165[169] = 0.0; - out_1201664963462517165[170] = 0.0; - out_1201664963462517165[171] = 1.0; - out_1201664963462517165[172] = 0.0; - out_1201664963462517165[173] = 0.0; - out_1201664963462517165[174] = 0.0; - out_1201664963462517165[175] = 0.0; - out_1201664963462517165[176] = 0.0; - out_1201664963462517165[177] = 0.0; - out_1201664963462517165[178] = 0.0; - out_1201664963462517165[179] = 0.0; - out_1201664963462517165[180] = 0.0; - out_1201664963462517165[181] = 0.0; - out_1201664963462517165[182] = 0.0; - out_1201664963462517165[183] = 0.0; - out_1201664963462517165[184] = 0.0; - out_1201664963462517165[185] = 0.0; - out_1201664963462517165[186] = 0.0; - out_1201664963462517165[187] = 0.0; - out_1201664963462517165[188] = 0.0; - out_1201664963462517165[189] = 0.0; - out_1201664963462517165[190] = 1.0; - out_1201664963462517165[191] = 0.0; - out_1201664963462517165[192] = 0.0; - out_1201664963462517165[193] = 0.0; - out_1201664963462517165[194] = 0.0; - out_1201664963462517165[195] = 0.0; - out_1201664963462517165[196] = 0.0; - out_1201664963462517165[197] = 0.0; - out_1201664963462517165[198] = 0.0; - out_1201664963462517165[199] = 0.0; - out_1201664963462517165[200] = 0.0; - out_1201664963462517165[201] = 0.0; - out_1201664963462517165[202] = 0.0; - out_1201664963462517165[203] = 0.0; - out_1201664963462517165[204] = 0.0; - out_1201664963462517165[205] = 0.0; - out_1201664963462517165[206] = 0.0; - out_1201664963462517165[207] = 0.0; - out_1201664963462517165[208] = 0.0; - out_1201664963462517165[209] = 1.0; - out_1201664963462517165[210] = 0.0; - out_1201664963462517165[211] = 0.0; - out_1201664963462517165[212] = 0.0; - out_1201664963462517165[213] = 0.0; - out_1201664963462517165[214] = 0.0; - out_1201664963462517165[215] = 0.0; - out_1201664963462517165[216] = 0.0; - out_1201664963462517165[217] = 0.0; - out_1201664963462517165[218] = 0.0; - out_1201664963462517165[219] = 0.0; - out_1201664963462517165[220] = 0.0; - out_1201664963462517165[221] = 0.0; - out_1201664963462517165[222] = 0.0; - out_1201664963462517165[223] = 0.0; - out_1201664963462517165[224] = 0.0; - out_1201664963462517165[225] = 0.0; - out_1201664963462517165[226] = 0.0; - out_1201664963462517165[227] = 0.0; - out_1201664963462517165[228] = 1.0; - out_1201664963462517165[229] = 0.0; - out_1201664963462517165[230] = 0.0; - out_1201664963462517165[231] = 0.0; - out_1201664963462517165[232] = 0.0; - out_1201664963462517165[233] = 0.0; - out_1201664963462517165[234] = 0.0; - out_1201664963462517165[235] = 0.0; - out_1201664963462517165[236] = 0.0; - out_1201664963462517165[237] = 0.0; - out_1201664963462517165[238] = 0.0; - out_1201664963462517165[239] = 0.0; - out_1201664963462517165[240] = 0.0; - out_1201664963462517165[241] = 0.0; - out_1201664963462517165[242] = 0.0; - out_1201664963462517165[243] = 0.0; - out_1201664963462517165[244] = 0.0; - out_1201664963462517165[245] = 0.0; - out_1201664963462517165[246] = 0.0; - out_1201664963462517165[247] = 1.0; - out_1201664963462517165[248] = 0.0; - out_1201664963462517165[249] = 0.0; - out_1201664963462517165[250] = 0.0; - out_1201664963462517165[251] = 0.0; - out_1201664963462517165[252] = 0.0; - out_1201664963462517165[253] = 0.0; - out_1201664963462517165[254] = 0.0; - out_1201664963462517165[255] = 0.0; - out_1201664963462517165[256] = 0.0; - out_1201664963462517165[257] = 0.0; - out_1201664963462517165[258] = 0.0; - out_1201664963462517165[259] = 0.0; - out_1201664963462517165[260] = 0.0; - out_1201664963462517165[261] = 0.0; - out_1201664963462517165[262] = 0.0; - out_1201664963462517165[263] = 0.0; - out_1201664963462517165[264] = 0.0; - out_1201664963462517165[265] = 0.0; - out_1201664963462517165[266] = 1.0; - out_1201664963462517165[267] = 0.0; - out_1201664963462517165[268] = 0.0; - out_1201664963462517165[269] = 0.0; - out_1201664963462517165[270] = 0.0; - out_1201664963462517165[271] = 0.0; - out_1201664963462517165[272] = 0.0; - out_1201664963462517165[273] = 0.0; - out_1201664963462517165[274] = 0.0; - out_1201664963462517165[275] = 0.0; - out_1201664963462517165[276] = 0.0; - out_1201664963462517165[277] = 0.0; - out_1201664963462517165[278] = 0.0; - out_1201664963462517165[279] = 0.0; - out_1201664963462517165[280] = 0.0; - out_1201664963462517165[281] = 0.0; - out_1201664963462517165[282] = 0.0; - out_1201664963462517165[283] = 0.0; - out_1201664963462517165[284] = 0.0; - out_1201664963462517165[285] = 1.0; - out_1201664963462517165[286] = 0.0; - out_1201664963462517165[287] = 0.0; - out_1201664963462517165[288] = 0.0; - out_1201664963462517165[289] = 0.0; - out_1201664963462517165[290] = 0.0; - out_1201664963462517165[291] = 0.0; - out_1201664963462517165[292] = 0.0; - out_1201664963462517165[293] = 0.0; - out_1201664963462517165[294] = 0.0; - out_1201664963462517165[295] = 0.0; - out_1201664963462517165[296] = 0.0; - out_1201664963462517165[297] = 0.0; - out_1201664963462517165[298] = 0.0; - out_1201664963462517165[299] = 0.0; - out_1201664963462517165[300] = 0.0; - out_1201664963462517165[301] = 0.0; - out_1201664963462517165[302] = 0.0; - out_1201664963462517165[303] = 0.0; - out_1201664963462517165[304] = 1.0; - out_1201664963462517165[305] = 0.0; - out_1201664963462517165[306] = 0.0; - out_1201664963462517165[307] = 0.0; - out_1201664963462517165[308] = 0.0; - out_1201664963462517165[309] = 0.0; - out_1201664963462517165[310] = 0.0; - out_1201664963462517165[311] = 0.0; - out_1201664963462517165[312] = 0.0; - out_1201664963462517165[313] = 0.0; - out_1201664963462517165[314] = 0.0; - out_1201664963462517165[315] = 0.0; - out_1201664963462517165[316] = 0.0; - out_1201664963462517165[317] = 0.0; - out_1201664963462517165[318] = 0.0; - out_1201664963462517165[319] = 0.0; - out_1201664963462517165[320] = 0.0; - out_1201664963462517165[321] = 0.0; - out_1201664963462517165[322] = 0.0; - out_1201664963462517165[323] = 1.0; +void H_mod_fun(double *state, double *out_328613363132085737) { + out_328613363132085737[0] = 1.0; + out_328613363132085737[1] = 0.0; + out_328613363132085737[2] = 0.0; + out_328613363132085737[3] = 0.0; + out_328613363132085737[4] = 0.0; + out_328613363132085737[5] = 0.0; + out_328613363132085737[6] = 0.0; + out_328613363132085737[7] = 0.0; + out_328613363132085737[8] = 0.0; + out_328613363132085737[9] = 0.0; + out_328613363132085737[10] = 0.0; + out_328613363132085737[11] = 0.0; + out_328613363132085737[12] = 0.0; + out_328613363132085737[13] = 0.0; + out_328613363132085737[14] = 0.0; + out_328613363132085737[15] = 0.0; + out_328613363132085737[16] = 0.0; + out_328613363132085737[17] = 0.0; + out_328613363132085737[18] = 0.0; + out_328613363132085737[19] = 1.0; + out_328613363132085737[20] = 0.0; + out_328613363132085737[21] = 0.0; + out_328613363132085737[22] = 0.0; + out_328613363132085737[23] = 0.0; + out_328613363132085737[24] = 0.0; + out_328613363132085737[25] = 0.0; + out_328613363132085737[26] = 0.0; + out_328613363132085737[27] = 0.0; + out_328613363132085737[28] = 0.0; + out_328613363132085737[29] = 0.0; + out_328613363132085737[30] = 0.0; + out_328613363132085737[31] = 0.0; + out_328613363132085737[32] = 0.0; + out_328613363132085737[33] = 0.0; + out_328613363132085737[34] = 0.0; + out_328613363132085737[35] = 0.0; + out_328613363132085737[36] = 0.0; + out_328613363132085737[37] = 0.0; + out_328613363132085737[38] = 1.0; + out_328613363132085737[39] = 0.0; + out_328613363132085737[40] = 0.0; + out_328613363132085737[41] = 0.0; + out_328613363132085737[42] = 0.0; + out_328613363132085737[43] = 0.0; + out_328613363132085737[44] = 0.0; + out_328613363132085737[45] = 0.0; + out_328613363132085737[46] = 0.0; + out_328613363132085737[47] = 0.0; + out_328613363132085737[48] = 0.0; + out_328613363132085737[49] = 0.0; + out_328613363132085737[50] = 0.0; + out_328613363132085737[51] = 0.0; + out_328613363132085737[52] = 0.0; + out_328613363132085737[53] = 0.0; + out_328613363132085737[54] = 0.0; + out_328613363132085737[55] = 0.0; + out_328613363132085737[56] = 0.0; + out_328613363132085737[57] = 1.0; + out_328613363132085737[58] = 0.0; + out_328613363132085737[59] = 0.0; + out_328613363132085737[60] = 0.0; + out_328613363132085737[61] = 0.0; + out_328613363132085737[62] = 0.0; + out_328613363132085737[63] = 0.0; + out_328613363132085737[64] = 0.0; + out_328613363132085737[65] = 0.0; + out_328613363132085737[66] = 0.0; + out_328613363132085737[67] = 0.0; + out_328613363132085737[68] = 0.0; + out_328613363132085737[69] = 0.0; + out_328613363132085737[70] = 0.0; + out_328613363132085737[71] = 0.0; + out_328613363132085737[72] = 0.0; + out_328613363132085737[73] = 0.0; + out_328613363132085737[74] = 0.0; + out_328613363132085737[75] = 0.0; + out_328613363132085737[76] = 1.0; + out_328613363132085737[77] = 0.0; + out_328613363132085737[78] = 0.0; + out_328613363132085737[79] = 0.0; + out_328613363132085737[80] = 0.0; + out_328613363132085737[81] = 0.0; + out_328613363132085737[82] = 0.0; + out_328613363132085737[83] = 0.0; + out_328613363132085737[84] = 0.0; + out_328613363132085737[85] = 0.0; + out_328613363132085737[86] = 0.0; + out_328613363132085737[87] = 0.0; + out_328613363132085737[88] = 0.0; + out_328613363132085737[89] = 0.0; + out_328613363132085737[90] = 0.0; + out_328613363132085737[91] = 0.0; + out_328613363132085737[92] = 0.0; + out_328613363132085737[93] = 0.0; + out_328613363132085737[94] = 0.0; + out_328613363132085737[95] = 1.0; + out_328613363132085737[96] = 0.0; + out_328613363132085737[97] = 0.0; + out_328613363132085737[98] = 0.0; + out_328613363132085737[99] = 0.0; + out_328613363132085737[100] = 0.0; + out_328613363132085737[101] = 0.0; + out_328613363132085737[102] = 0.0; + out_328613363132085737[103] = 0.0; + out_328613363132085737[104] = 0.0; + out_328613363132085737[105] = 0.0; + out_328613363132085737[106] = 0.0; + out_328613363132085737[107] = 0.0; + out_328613363132085737[108] = 0.0; + out_328613363132085737[109] = 0.0; + out_328613363132085737[110] = 0.0; + out_328613363132085737[111] = 0.0; + out_328613363132085737[112] = 0.0; + out_328613363132085737[113] = 0.0; + out_328613363132085737[114] = 1.0; + out_328613363132085737[115] = 0.0; + out_328613363132085737[116] = 0.0; + out_328613363132085737[117] = 0.0; + out_328613363132085737[118] = 0.0; + out_328613363132085737[119] = 0.0; + out_328613363132085737[120] = 0.0; + out_328613363132085737[121] = 0.0; + out_328613363132085737[122] = 0.0; + out_328613363132085737[123] = 0.0; + out_328613363132085737[124] = 0.0; + out_328613363132085737[125] = 0.0; + out_328613363132085737[126] = 0.0; + out_328613363132085737[127] = 0.0; + out_328613363132085737[128] = 0.0; + out_328613363132085737[129] = 0.0; + out_328613363132085737[130] = 0.0; + out_328613363132085737[131] = 0.0; + out_328613363132085737[132] = 0.0; + out_328613363132085737[133] = 1.0; + out_328613363132085737[134] = 0.0; + out_328613363132085737[135] = 0.0; + out_328613363132085737[136] = 0.0; + out_328613363132085737[137] = 0.0; + out_328613363132085737[138] = 0.0; + out_328613363132085737[139] = 0.0; + out_328613363132085737[140] = 0.0; + out_328613363132085737[141] = 0.0; + out_328613363132085737[142] = 0.0; + out_328613363132085737[143] = 0.0; + out_328613363132085737[144] = 0.0; + out_328613363132085737[145] = 0.0; + out_328613363132085737[146] = 0.0; + out_328613363132085737[147] = 0.0; + out_328613363132085737[148] = 0.0; + out_328613363132085737[149] = 0.0; + out_328613363132085737[150] = 0.0; + out_328613363132085737[151] = 0.0; + out_328613363132085737[152] = 1.0; + out_328613363132085737[153] = 0.0; + out_328613363132085737[154] = 0.0; + out_328613363132085737[155] = 0.0; + out_328613363132085737[156] = 0.0; + out_328613363132085737[157] = 0.0; + out_328613363132085737[158] = 0.0; + out_328613363132085737[159] = 0.0; + out_328613363132085737[160] = 0.0; + out_328613363132085737[161] = 0.0; + out_328613363132085737[162] = 0.0; + out_328613363132085737[163] = 0.0; + out_328613363132085737[164] = 0.0; + out_328613363132085737[165] = 0.0; + out_328613363132085737[166] = 0.0; + out_328613363132085737[167] = 0.0; + out_328613363132085737[168] = 0.0; + out_328613363132085737[169] = 0.0; + out_328613363132085737[170] = 0.0; + out_328613363132085737[171] = 1.0; + out_328613363132085737[172] = 0.0; + out_328613363132085737[173] = 0.0; + out_328613363132085737[174] = 0.0; + out_328613363132085737[175] = 0.0; + out_328613363132085737[176] = 0.0; + out_328613363132085737[177] = 0.0; + out_328613363132085737[178] = 0.0; + out_328613363132085737[179] = 0.0; + out_328613363132085737[180] = 0.0; + out_328613363132085737[181] = 0.0; + out_328613363132085737[182] = 0.0; + out_328613363132085737[183] = 0.0; + out_328613363132085737[184] = 0.0; + out_328613363132085737[185] = 0.0; + out_328613363132085737[186] = 0.0; + out_328613363132085737[187] = 0.0; + out_328613363132085737[188] = 0.0; + out_328613363132085737[189] = 0.0; + out_328613363132085737[190] = 1.0; + out_328613363132085737[191] = 0.0; + out_328613363132085737[192] = 0.0; + out_328613363132085737[193] = 0.0; + out_328613363132085737[194] = 0.0; + out_328613363132085737[195] = 0.0; + out_328613363132085737[196] = 0.0; + out_328613363132085737[197] = 0.0; + out_328613363132085737[198] = 0.0; + out_328613363132085737[199] = 0.0; + out_328613363132085737[200] = 0.0; + out_328613363132085737[201] = 0.0; + out_328613363132085737[202] = 0.0; + out_328613363132085737[203] = 0.0; + out_328613363132085737[204] = 0.0; + out_328613363132085737[205] = 0.0; + out_328613363132085737[206] = 0.0; + out_328613363132085737[207] = 0.0; + out_328613363132085737[208] = 0.0; + out_328613363132085737[209] = 1.0; + out_328613363132085737[210] = 0.0; + out_328613363132085737[211] = 0.0; + out_328613363132085737[212] = 0.0; + out_328613363132085737[213] = 0.0; + out_328613363132085737[214] = 0.0; + out_328613363132085737[215] = 0.0; + out_328613363132085737[216] = 0.0; + out_328613363132085737[217] = 0.0; + out_328613363132085737[218] = 0.0; + out_328613363132085737[219] = 0.0; + out_328613363132085737[220] = 0.0; + out_328613363132085737[221] = 0.0; + out_328613363132085737[222] = 0.0; + out_328613363132085737[223] = 0.0; + out_328613363132085737[224] = 0.0; + out_328613363132085737[225] = 0.0; + out_328613363132085737[226] = 0.0; + out_328613363132085737[227] = 0.0; + out_328613363132085737[228] = 1.0; + out_328613363132085737[229] = 0.0; + out_328613363132085737[230] = 0.0; + out_328613363132085737[231] = 0.0; + out_328613363132085737[232] = 0.0; + out_328613363132085737[233] = 0.0; + out_328613363132085737[234] = 0.0; + out_328613363132085737[235] = 0.0; + out_328613363132085737[236] = 0.0; + out_328613363132085737[237] = 0.0; + out_328613363132085737[238] = 0.0; + out_328613363132085737[239] = 0.0; + out_328613363132085737[240] = 0.0; + out_328613363132085737[241] = 0.0; + out_328613363132085737[242] = 0.0; + out_328613363132085737[243] = 0.0; + out_328613363132085737[244] = 0.0; + out_328613363132085737[245] = 0.0; + out_328613363132085737[246] = 0.0; + out_328613363132085737[247] = 1.0; + out_328613363132085737[248] = 0.0; + out_328613363132085737[249] = 0.0; + out_328613363132085737[250] = 0.0; + out_328613363132085737[251] = 0.0; + out_328613363132085737[252] = 0.0; + out_328613363132085737[253] = 0.0; + out_328613363132085737[254] = 0.0; + out_328613363132085737[255] = 0.0; + out_328613363132085737[256] = 0.0; + out_328613363132085737[257] = 0.0; + out_328613363132085737[258] = 0.0; + out_328613363132085737[259] = 0.0; + out_328613363132085737[260] = 0.0; + out_328613363132085737[261] = 0.0; + out_328613363132085737[262] = 0.0; + out_328613363132085737[263] = 0.0; + out_328613363132085737[264] = 0.0; + out_328613363132085737[265] = 0.0; + out_328613363132085737[266] = 1.0; + out_328613363132085737[267] = 0.0; + out_328613363132085737[268] = 0.0; + out_328613363132085737[269] = 0.0; + out_328613363132085737[270] = 0.0; + out_328613363132085737[271] = 0.0; + out_328613363132085737[272] = 0.0; + out_328613363132085737[273] = 0.0; + out_328613363132085737[274] = 0.0; + out_328613363132085737[275] = 0.0; + out_328613363132085737[276] = 0.0; + out_328613363132085737[277] = 0.0; + out_328613363132085737[278] = 0.0; + out_328613363132085737[279] = 0.0; + out_328613363132085737[280] = 0.0; + out_328613363132085737[281] = 0.0; + out_328613363132085737[282] = 0.0; + out_328613363132085737[283] = 0.0; + out_328613363132085737[284] = 0.0; + out_328613363132085737[285] = 1.0; + out_328613363132085737[286] = 0.0; + out_328613363132085737[287] = 0.0; + out_328613363132085737[288] = 0.0; + out_328613363132085737[289] = 0.0; + out_328613363132085737[290] = 0.0; + out_328613363132085737[291] = 0.0; + out_328613363132085737[292] = 0.0; + out_328613363132085737[293] = 0.0; + out_328613363132085737[294] = 0.0; + out_328613363132085737[295] = 0.0; + out_328613363132085737[296] = 0.0; + out_328613363132085737[297] = 0.0; + out_328613363132085737[298] = 0.0; + out_328613363132085737[299] = 0.0; + out_328613363132085737[300] = 0.0; + out_328613363132085737[301] = 0.0; + out_328613363132085737[302] = 0.0; + out_328613363132085737[303] = 0.0; + out_328613363132085737[304] = 1.0; + out_328613363132085737[305] = 0.0; + out_328613363132085737[306] = 0.0; + out_328613363132085737[307] = 0.0; + out_328613363132085737[308] = 0.0; + out_328613363132085737[309] = 0.0; + out_328613363132085737[310] = 0.0; + out_328613363132085737[311] = 0.0; + out_328613363132085737[312] = 0.0; + out_328613363132085737[313] = 0.0; + out_328613363132085737[314] = 0.0; + out_328613363132085737[315] = 0.0; + out_328613363132085737[316] = 0.0; + out_328613363132085737[317] = 0.0; + out_328613363132085737[318] = 0.0; + out_328613363132085737[319] = 0.0; + out_328613363132085737[320] = 0.0; + out_328613363132085737[321] = 0.0; + out_328613363132085737[322] = 0.0; + out_328613363132085737[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4925442091395050631) { - out_4925442091395050631[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_4925442091395050631[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_4925442091395050631[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_4925442091395050631[3] = dt*state[12] + state[3]; - out_4925442091395050631[4] = dt*state[13] + state[4]; - out_4925442091395050631[5] = dt*state[14] + state[5]; - out_4925442091395050631[6] = state[6]; - out_4925442091395050631[7] = state[7]; - out_4925442091395050631[8] = state[8]; - out_4925442091395050631[9] = state[9]; - out_4925442091395050631[10] = state[10]; - out_4925442091395050631[11] = state[11]; - out_4925442091395050631[12] = state[12]; - out_4925442091395050631[13] = state[13]; - out_4925442091395050631[14] = state[14]; - out_4925442091395050631[15] = state[15]; - out_4925442091395050631[16] = state[16]; - out_4925442091395050631[17] = state[17]; +void f_fun(double *state, double dt, double *out_2411932017416861366) { + out_2411932017416861366[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_2411932017416861366[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_2411932017416861366[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_2411932017416861366[3] = dt*state[12] + state[3]; + out_2411932017416861366[4] = dt*state[13] + state[4]; + out_2411932017416861366[5] = dt*state[14] + state[5]; + out_2411932017416861366[6] = state[6]; + out_2411932017416861366[7] = state[7]; + out_2411932017416861366[8] = state[8]; + out_2411932017416861366[9] = state[9]; + out_2411932017416861366[10] = state[10]; + out_2411932017416861366[11] = state[11]; + out_2411932017416861366[12] = state[12]; + out_2411932017416861366[13] = state[13]; + out_2411932017416861366[14] = state[14]; + out_2411932017416861366[15] = state[15]; + out_2411932017416861366[16] = state[16]; + out_2411932017416861366[17] = state[17]; } -void F_fun(double *state, double dt, double *out_3854196706556040023) { - out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[2] = 0; - out_3854196706556040023[3] = 0; - out_3854196706556040023[4] = 0; - out_3854196706556040023[5] = 0; - out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[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_3854196706556040023[9] = 0; - out_3854196706556040023[10] = 0; - out_3854196706556040023[11] = 0; - out_3854196706556040023[12] = 0; - out_3854196706556040023[13] = 0; - out_3854196706556040023[14] = 0; - out_3854196706556040023[15] = 0; - out_3854196706556040023[16] = 0; - out_3854196706556040023[17] = 0; - out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[20] = 0; - out_3854196706556040023[21] = 0; - out_3854196706556040023[22] = 0; - out_3854196706556040023[23] = 0; - out_3854196706556040023[24] = 0; - out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[27] = 0; - out_3854196706556040023[28] = 0; - out_3854196706556040023[29] = 0; - out_3854196706556040023[30] = 0; - out_3854196706556040023[31] = 0; - out_3854196706556040023[32] = 0; - out_3854196706556040023[33] = 0; - out_3854196706556040023[34] = 0; - out_3854196706556040023[35] = 0; - out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[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_3854196706556040023[39] = 0; - out_3854196706556040023[40] = 0; - out_3854196706556040023[41] = 0; - out_3854196706556040023[42] = 0; - out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[45] = 0; - out_3854196706556040023[46] = 0; - out_3854196706556040023[47] = 0; - out_3854196706556040023[48] = 0; - out_3854196706556040023[49] = 0; - out_3854196706556040023[50] = 0; - out_3854196706556040023[51] = 0; - out_3854196706556040023[52] = 0; - out_3854196706556040023[53] = 0; - out_3854196706556040023[54] = 0; - out_3854196706556040023[55] = 0; - out_3854196706556040023[56] = 0; - out_3854196706556040023[57] = 1; - out_3854196706556040023[58] = 0; - out_3854196706556040023[59] = 0; - out_3854196706556040023[60] = 0; - out_3854196706556040023[61] = 0; - out_3854196706556040023[62] = 0; - out_3854196706556040023[63] = 0; - out_3854196706556040023[64] = 0; - out_3854196706556040023[65] = 0; - out_3854196706556040023[66] = dt; - out_3854196706556040023[67] = 0; - out_3854196706556040023[68] = 0; - out_3854196706556040023[69] = 0; - out_3854196706556040023[70] = 0; - out_3854196706556040023[71] = 0; - out_3854196706556040023[72] = 0; - out_3854196706556040023[73] = 0; - out_3854196706556040023[74] = 0; - out_3854196706556040023[75] = 0; - out_3854196706556040023[76] = 1; - out_3854196706556040023[77] = 0; - out_3854196706556040023[78] = 0; - out_3854196706556040023[79] = 0; - out_3854196706556040023[80] = 0; - out_3854196706556040023[81] = 0; - out_3854196706556040023[82] = 0; - out_3854196706556040023[83] = 0; - out_3854196706556040023[84] = 0; - out_3854196706556040023[85] = dt; - out_3854196706556040023[86] = 0; - out_3854196706556040023[87] = 0; - out_3854196706556040023[88] = 0; - out_3854196706556040023[89] = 0; - out_3854196706556040023[90] = 0; - out_3854196706556040023[91] = 0; - out_3854196706556040023[92] = 0; - out_3854196706556040023[93] = 0; - out_3854196706556040023[94] = 0; - out_3854196706556040023[95] = 1; - out_3854196706556040023[96] = 0; - out_3854196706556040023[97] = 0; - out_3854196706556040023[98] = 0; - out_3854196706556040023[99] = 0; - out_3854196706556040023[100] = 0; - out_3854196706556040023[101] = 0; - out_3854196706556040023[102] = 0; - out_3854196706556040023[103] = 0; - out_3854196706556040023[104] = dt; - out_3854196706556040023[105] = 0; - out_3854196706556040023[106] = 0; - out_3854196706556040023[107] = 0; - out_3854196706556040023[108] = 0; - out_3854196706556040023[109] = 0; - out_3854196706556040023[110] = 0; - out_3854196706556040023[111] = 0; - out_3854196706556040023[112] = 0; - out_3854196706556040023[113] = 0; - out_3854196706556040023[114] = 1; - out_3854196706556040023[115] = 0; - out_3854196706556040023[116] = 0; - out_3854196706556040023[117] = 0; - out_3854196706556040023[118] = 0; - out_3854196706556040023[119] = 0; - out_3854196706556040023[120] = 0; - out_3854196706556040023[121] = 0; - out_3854196706556040023[122] = 0; - out_3854196706556040023[123] = 0; - out_3854196706556040023[124] = 0; - out_3854196706556040023[125] = 0; - out_3854196706556040023[126] = 0; - out_3854196706556040023[127] = 0; - out_3854196706556040023[128] = 0; - out_3854196706556040023[129] = 0; - out_3854196706556040023[130] = 0; - out_3854196706556040023[131] = 0; - out_3854196706556040023[132] = 0; - out_3854196706556040023[133] = 1; - out_3854196706556040023[134] = 0; - out_3854196706556040023[135] = 0; - out_3854196706556040023[136] = 0; - out_3854196706556040023[137] = 0; - out_3854196706556040023[138] = 0; - out_3854196706556040023[139] = 0; - out_3854196706556040023[140] = 0; - out_3854196706556040023[141] = 0; - out_3854196706556040023[142] = 0; - out_3854196706556040023[143] = 0; - out_3854196706556040023[144] = 0; - out_3854196706556040023[145] = 0; - out_3854196706556040023[146] = 0; - out_3854196706556040023[147] = 0; - out_3854196706556040023[148] = 0; - out_3854196706556040023[149] = 0; - out_3854196706556040023[150] = 0; - out_3854196706556040023[151] = 0; - out_3854196706556040023[152] = 1; - out_3854196706556040023[153] = 0; - out_3854196706556040023[154] = 0; - out_3854196706556040023[155] = 0; - out_3854196706556040023[156] = 0; - out_3854196706556040023[157] = 0; - out_3854196706556040023[158] = 0; - out_3854196706556040023[159] = 0; - out_3854196706556040023[160] = 0; - out_3854196706556040023[161] = 0; - out_3854196706556040023[162] = 0; - out_3854196706556040023[163] = 0; - out_3854196706556040023[164] = 0; - out_3854196706556040023[165] = 0; - out_3854196706556040023[166] = 0; - out_3854196706556040023[167] = 0; - out_3854196706556040023[168] = 0; - out_3854196706556040023[169] = 0; - out_3854196706556040023[170] = 0; - out_3854196706556040023[171] = 1; - out_3854196706556040023[172] = 0; - out_3854196706556040023[173] = 0; - out_3854196706556040023[174] = 0; - out_3854196706556040023[175] = 0; - out_3854196706556040023[176] = 0; - out_3854196706556040023[177] = 0; - out_3854196706556040023[178] = 0; - out_3854196706556040023[179] = 0; - out_3854196706556040023[180] = 0; - out_3854196706556040023[181] = 0; - out_3854196706556040023[182] = 0; - out_3854196706556040023[183] = 0; - out_3854196706556040023[184] = 0; - out_3854196706556040023[185] = 0; - out_3854196706556040023[186] = 0; - out_3854196706556040023[187] = 0; - out_3854196706556040023[188] = 0; - out_3854196706556040023[189] = 0; - out_3854196706556040023[190] = 1; - out_3854196706556040023[191] = 0; - out_3854196706556040023[192] = 0; - out_3854196706556040023[193] = 0; - out_3854196706556040023[194] = 0; - out_3854196706556040023[195] = 0; - out_3854196706556040023[196] = 0; - out_3854196706556040023[197] = 0; - out_3854196706556040023[198] = 0; - out_3854196706556040023[199] = 0; - out_3854196706556040023[200] = 0; - out_3854196706556040023[201] = 0; - out_3854196706556040023[202] = 0; - out_3854196706556040023[203] = 0; - out_3854196706556040023[204] = 0; - out_3854196706556040023[205] = 0; - out_3854196706556040023[206] = 0; - out_3854196706556040023[207] = 0; - out_3854196706556040023[208] = 0; - out_3854196706556040023[209] = 1; - out_3854196706556040023[210] = 0; - out_3854196706556040023[211] = 0; - out_3854196706556040023[212] = 0; - out_3854196706556040023[213] = 0; - out_3854196706556040023[214] = 0; - out_3854196706556040023[215] = 0; - out_3854196706556040023[216] = 0; - out_3854196706556040023[217] = 0; - out_3854196706556040023[218] = 0; - out_3854196706556040023[219] = 0; - out_3854196706556040023[220] = 0; - out_3854196706556040023[221] = 0; - out_3854196706556040023[222] = 0; - out_3854196706556040023[223] = 0; - out_3854196706556040023[224] = 0; - out_3854196706556040023[225] = 0; - out_3854196706556040023[226] = 0; - out_3854196706556040023[227] = 0; - out_3854196706556040023[228] = 1; - out_3854196706556040023[229] = 0; - out_3854196706556040023[230] = 0; - out_3854196706556040023[231] = 0; - out_3854196706556040023[232] = 0; - out_3854196706556040023[233] = 0; - out_3854196706556040023[234] = 0; - out_3854196706556040023[235] = 0; - out_3854196706556040023[236] = 0; - out_3854196706556040023[237] = 0; - out_3854196706556040023[238] = 0; - out_3854196706556040023[239] = 0; - out_3854196706556040023[240] = 0; - out_3854196706556040023[241] = 0; - out_3854196706556040023[242] = 0; - out_3854196706556040023[243] = 0; - out_3854196706556040023[244] = 0; - out_3854196706556040023[245] = 0; - out_3854196706556040023[246] = 0; - out_3854196706556040023[247] = 1; - out_3854196706556040023[248] = 0; - out_3854196706556040023[249] = 0; - out_3854196706556040023[250] = 0; - out_3854196706556040023[251] = 0; - out_3854196706556040023[252] = 0; - out_3854196706556040023[253] = 0; - out_3854196706556040023[254] = 0; - out_3854196706556040023[255] = 0; - out_3854196706556040023[256] = 0; - out_3854196706556040023[257] = 0; - out_3854196706556040023[258] = 0; - out_3854196706556040023[259] = 0; - out_3854196706556040023[260] = 0; - out_3854196706556040023[261] = 0; - out_3854196706556040023[262] = 0; - out_3854196706556040023[263] = 0; - out_3854196706556040023[264] = 0; - out_3854196706556040023[265] = 0; - out_3854196706556040023[266] = 1; - out_3854196706556040023[267] = 0; - out_3854196706556040023[268] = 0; - out_3854196706556040023[269] = 0; - out_3854196706556040023[270] = 0; - out_3854196706556040023[271] = 0; - out_3854196706556040023[272] = 0; - out_3854196706556040023[273] = 0; - out_3854196706556040023[274] = 0; - out_3854196706556040023[275] = 0; - out_3854196706556040023[276] = 0; - out_3854196706556040023[277] = 0; - out_3854196706556040023[278] = 0; - out_3854196706556040023[279] = 0; - out_3854196706556040023[280] = 0; - out_3854196706556040023[281] = 0; - out_3854196706556040023[282] = 0; - out_3854196706556040023[283] = 0; - out_3854196706556040023[284] = 0; - out_3854196706556040023[285] = 1; - out_3854196706556040023[286] = 0; - out_3854196706556040023[287] = 0; - out_3854196706556040023[288] = 0; - out_3854196706556040023[289] = 0; - out_3854196706556040023[290] = 0; - out_3854196706556040023[291] = 0; - out_3854196706556040023[292] = 0; - out_3854196706556040023[293] = 0; - out_3854196706556040023[294] = 0; - out_3854196706556040023[295] = 0; - out_3854196706556040023[296] = 0; - out_3854196706556040023[297] = 0; - out_3854196706556040023[298] = 0; - out_3854196706556040023[299] = 0; - out_3854196706556040023[300] = 0; - out_3854196706556040023[301] = 0; - out_3854196706556040023[302] = 0; - out_3854196706556040023[303] = 0; - out_3854196706556040023[304] = 1; - out_3854196706556040023[305] = 0; - out_3854196706556040023[306] = 0; - out_3854196706556040023[307] = 0; - out_3854196706556040023[308] = 0; - out_3854196706556040023[309] = 0; - out_3854196706556040023[310] = 0; - out_3854196706556040023[311] = 0; - out_3854196706556040023[312] = 0; - out_3854196706556040023[313] = 0; - out_3854196706556040023[314] = 0; - out_3854196706556040023[315] = 0; - out_3854196706556040023[316] = 0; - out_3854196706556040023[317] = 0; - out_3854196706556040023[318] = 0; - out_3854196706556040023[319] = 0; - out_3854196706556040023[320] = 0; - out_3854196706556040023[321] = 0; - out_3854196706556040023[322] = 0; - out_3854196706556040023[323] = 1; +void F_fun(double *state, double dt, double *out_8127227481367081246) { + out_8127227481367081246[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_8127227481367081246[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_8127227481367081246[2] = 0; + out_8127227481367081246[3] = 0; + out_8127227481367081246[4] = 0; + out_8127227481367081246[5] = 0; + out_8127227481367081246[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_8127227481367081246[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_8127227481367081246[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_8127227481367081246[9] = 0; + out_8127227481367081246[10] = 0; + out_8127227481367081246[11] = 0; + out_8127227481367081246[12] = 0; + out_8127227481367081246[13] = 0; + out_8127227481367081246[14] = 0; + out_8127227481367081246[15] = 0; + out_8127227481367081246[16] = 0; + out_8127227481367081246[17] = 0; + out_8127227481367081246[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_8127227481367081246[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_8127227481367081246[20] = 0; + out_8127227481367081246[21] = 0; + out_8127227481367081246[22] = 0; + out_8127227481367081246[23] = 0; + out_8127227481367081246[24] = 0; + out_8127227481367081246[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_8127227481367081246[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_8127227481367081246[27] = 0; + out_8127227481367081246[28] = 0; + out_8127227481367081246[29] = 0; + out_8127227481367081246[30] = 0; + out_8127227481367081246[31] = 0; + out_8127227481367081246[32] = 0; + out_8127227481367081246[33] = 0; + out_8127227481367081246[34] = 0; + out_8127227481367081246[35] = 0; + out_8127227481367081246[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_8127227481367081246[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_8127227481367081246[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_8127227481367081246[39] = 0; + out_8127227481367081246[40] = 0; + out_8127227481367081246[41] = 0; + out_8127227481367081246[42] = 0; + out_8127227481367081246[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_8127227481367081246[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_8127227481367081246[45] = 0; + out_8127227481367081246[46] = 0; + out_8127227481367081246[47] = 0; + out_8127227481367081246[48] = 0; + out_8127227481367081246[49] = 0; + out_8127227481367081246[50] = 0; + out_8127227481367081246[51] = 0; + out_8127227481367081246[52] = 0; + out_8127227481367081246[53] = 0; + out_8127227481367081246[54] = 0; + out_8127227481367081246[55] = 0; + out_8127227481367081246[56] = 0; + out_8127227481367081246[57] = 1; + out_8127227481367081246[58] = 0; + out_8127227481367081246[59] = 0; + out_8127227481367081246[60] = 0; + out_8127227481367081246[61] = 0; + out_8127227481367081246[62] = 0; + out_8127227481367081246[63] = 0; + out_8127227481367081246[64] = 0; + out_8127227481367081246[65] = 0; + out_8127227481367081246[66] = dt; + out_8127227481367081246[67] = 0; + out_8127227481367081246[68] = 0; + out_8127227481367081246[69] = 0; + out_8127227481367081246[70] = 0; + out_8127227481367081246[71] = 0; + out_8127227481367081246[72] = 0; + out_8127227481367081246[73] = 0; + out_8127227481367081246[74] = 0; + out_8127227481367081246[75] = 0; + out_8127227481367081246[76] = 1; + out_8127227481367081246[77] = 0; + out_8127227481367081246[78] = 0; + out_8127227481367081246[79] = 0; + out_8127227481367081246[80] = 0; + out_8127227481367081246[81] = 0; + out_8127227481367081246[82] = 0; + out_8127227481367081246[83] = 0; + out_8127227481367081246[84] = 0; + out_8127227481367081246[85] = dt; + out_8127227481367081246[86] = 0; + out_8127227481367081246[87] = 0; + out_8127227481367081246[88] = 0; + out_8127227481367081246[89] = 0; + out_8127227481367081246[90] = 0; + out_8127227481367081246[91] = 0; + out_8127227481367081246[92] = 0; + out_8127227481367081246[93] = 0; + out_8127227481367081246[94] = 0; + out_8127227481367081246[95] = 1; + out_8127227481367081246[96] = 0; + out_8127227481367081246[97] = 0; + out_8127227481367081246[98] = 0; + out_8127227481367081246[99] = 0; + out_8127227481367081246[100] = 0; + out_8127227481367081246[101] = 0; + out_8127227481367081246[102] = 0; + out_8127227481367081246[103] = 0; + out_8127227481367081246[104] = dt; + out_8127227481367081246[105] = 0; + out_8127227481367081246[106] = 0; + out_8127227481367081246[107] = 0; + out_8127227481367081246[108] = 0; + out_8127227481367081246[109] = 0; + out_8127227481367081246[110] = 0; + out_8127227481367081246[111] = 0; + out_8127227481367081246[112] = 0; + out_8127227481367081246[113] = 0; + out_8127227481367081246[114] = 1; + out_8127227481367081246[115] = 0; + out_8127227481367081246[116] = 0; + out_8127227481367081246[117] = 0; + out_8127227481367081246[118] = 0; + out_8127227481367081246[119] = 0; + out_8127227481367081246[120] = 0; + out_8127227481367081246[121] = 0; + out_8127227481367081246[122] = 0; + out_8127227481367081246[123] = 0; + out_8127227481367081246[124] = 0; + out_8127227481367081246[125] = 0; + out_8127227481367081246[126] = 0; + out_8127227481367081246[127] = 0; + out_8127227481367081246[128] = 0; + out_8127227481367081246[129] = 0; + out_8127227481367081246[130] = 0; + out_8127227481367081246[131] = 0; + out_8127227481367081246[132] = 0; + out_8127227481367081246[133] = 1; + out_8127227481367081246[134] = 0; + out_8127227481367081246[135] = 0; + out_8127227481367081246[136] = 0; + out_8127227481367081246[137] = 0; + out_8127227481367081246[138] = 0; + out_8127227481367081246[139] = 0; + out_8127227481367081246[140] = 0; + out_8127227481367081246[141] = 0; + out_8127227481367081246[142] = 0; + out_8127227481367081246[143] = 0; + out_8127227481367081246[144] = 0; + out_8127227481367081246[145] = 0; + out_8127227481367081246[146] = 0; + out_8127227481367081246[147] = 0; + out_8127227481367081246[148] = 0; + out_8127227481367081246[149] = 0; + out_8127227481367081246[150] = 0; + out_8127227481367081246[151] = 0; + out_8127227481367081246[152] = 1; + out_8127227481367081246[153] = 0; + out_8127227481367081246[154] = 0; + out_8127227481367081246[155] = 0; + out_8127227481367081246[156] = 0; + out_8127227481367081246[157] = 0; + out_8127227481367081246[158] = 0; + out_8127227481367081246[159] = 0; + out_8127227481367081246[160] = 0; + out_8127227481367081246[161] = 0; + out_8127227481367081246[162] = 0; + out_8127227481367081246[163] = 0; + out_8127227481367081246[164] = 0; + out_8127227481367081246[165] = 0; + out_8127227481367081246[166] = 0; + out_8127227481367081246[167] = 0; + out_8127227481367081246[168] = 0; + out_8127227481367081246[169] = 0; + out_8127227481367081246[170] = 0; + out_8127227481367081246[171] = 1; + out_8127227481367081246[172] = 0; + out_8127227481367081246[173] = 0; + out_8127227481367081246[174] = 0; + out_8127227481367081246[175] = 0; + out_8127227481367081246[176] = 0; + out_8127227481367081246[177] = 0; + out_8127227481367081246[178] = 0; + out_8127227481367081246[179] = 0; + out_8127227481367081246[180] = 0; + out_8127227481367081246[181] = 0; + out_8127227481367081246[182] = 0; + out_8127227481367081246[183] = 0; + out_8127227481367081246[184] = 0; + out_8127227481367081246[185] = 0; + out_8127227481367081246[186] = 0; + out_8127227481367081246[187] = 0; + out_8127227481367081246[188] = 0; + out_8127227481367081246[189] = 0; + out_8127227481367081246[190] = 1; + out_8127227481367081246[191] = 0; + out_8127227481367081246[192] = 0; + out_8127227481367081246[193] = 0; + out_8127227481367081246[194] = 0; + out_8127227481367081246[195] = 0; + out_8127227481367081246[196] = 0; + out_8127227481367081246[197] = 0; + out_8127227481367081246[198] = 0; + out_8127227481367081246[199] = 0; + out_8127227481367081246[200] = 0; + out_8127227481367081246[201] = 0; + out_8127227481367081246[202] = 0; + out_8127227481367081246[203] = 0; + out_8127227481367081246[204] = 0; + out_8127227481367081246[205] = 0; + out_8127227481367081246[206] = 0; + out_8127227481367081246[207] = 0; + out_8127227481367081246[208] = 0; + out_8127227481367081246[209] = 1; + out_8127227481367081246[210] = 0; + out_8127227481367081246[211] = 0; + out_8127227481367081246[212] = 0; + out_8127227481367081246[213] = 0; + out_8127227481367081246[214] = 0; + out_8127227481367081246[215] = 0; + out_8127227481367081246[216] = 0; + out_8127227481367081246[217] = 0; + out_8127227481367081246[218] = 0; + out_8127227481367081246[219] = 0; + out_8127227481367081246[220] = 0; + out_8127227481367081246[221] = 0; + out_8127227481367081246[222] = 0; + out_8127227481367081246[223] = 0; + out_8127227481367081246[224] = 0; + out_8127227481367081246[225] = 0; + out_8127227481367081246[226] = 0; + out_8127227481367081246[227] = 0; + out_8127227481367081246[228] = 1; + out_8127227481367081246[229] = 0; + out_8127227481367081246[230] = 0; + out_8127227481367081246[231] = 0; + out_8127227481367081246[232] = 0; + out_8127227481367081246[233] = 0; + out_8127227481367081246[234] = 0; + out_8127227481367081246[235] = 0; + out_8127227481367081246[236] = 0; + out_8127227481367081246[237] = 0; + out_8127227481367081246[238] = 0; + out_8127227481367081246[239] = 0; + out_8127227481367081246[240] = 0; + out_8127227481367081246[241] = 0; + out_8127227481367081246[242] = 0; + out_8127227481367081246[243] = 0; + out_8127227481367081246[244] = 0; + out_8127227481367081246[245] = 0; + out_8127227481367081246[246] = 0; + out_8127227481367081246[247] = 1; + out_8127227481367081246[248] = 0; + out_8127227481367081246[249] = 0; + out_8127227481367081246[250] = 0; + out_8127227481367081246[251] = 0; + out_8127227481367081246[252] = 0; + out_8127227481367081246[253] = 0; + out_8127227481367081246[254] = 0; + out_8127227481367081246[255] = 0; + out_8127227481367081246[256] = 0; + out_8127227481367081246[257] = 0; + out_8127227481367081246[258] = 0; + out_8127227481367081246[259] = 0; + out_8127227481367081246[260] = 0; + out_8127227481367081246[261] = 0; + out_8127227481367081246[262] = 0; + out_8127227481367081246[263] = 0; + out_8127227481367081246[264] = 0; + out_8127227481367081246[265] = 0; + out_8127227481367081246[266] = 1; + out_8127227481367081246[267] = 0; + out_8127227481367081246[268] = 0; + out_8127227481367081246[269] = 0; + out_8127227481367081246[270] = 0; + out_8127227481367081246[271] = 0; + out_8127227481367081246[272] = 0; + out_8127227481367081246[273] = 0; + out_8127227481367081246[274] = 0; + out_8127227481367081246[275] = 0; + out_8127227481367081246[276] = 0; + out_8127227481367081246[277] = 0; + out_8127227481367081246[278] = 0; + out_8127227481367081246[279] = 0; + out_8127227481367081246[280] = 0; + out_8127227481367081246[281] = 0; + out_8127227481367081246[282] = 0; + out_8127227481367081246[283] = 0; + out_8127227481367081246[284] = 0; + out_8127227481367081246[285] = 1; + out_8127227481367081246[286] = 0; + out_8127227481367081246[287] = 0; + out_8127227481367081246[288] = 0; + out_8127227481367081246[289] = 0; + out_8127227481367081246[290] = 0; + out_8127227481367081246[291] = 0; + out_8127227481367081246[292] = 0; + out_8127227481367081246[293] = 0; + out_8127227481367081246[294] = 0; + out_8127227481367081246[295] = 0; + out_8127227481367081246[296] = 0; + out_8127227481367081246[297] = 0; + out_8127227481367081246[298] = 0; + out_8127227481367081246[299] = 0; + out_8127227481367081246[300] = 0; + out_8127227481367081246[301] = 0; + out_8127227481367081246[302] = 0; + out_8127227481367081246[303] = 0; + out_8127227481367081246[304] = 1; + out_8127227481367081246[305] = 0; + out_8127227481367081246[306] = 0; + out_8127227481367081246[307] = 0; + out_8127227481367081246[308] = 0; + out_8127227481367081246[309] = 0; + out_8127227481367081246[310] = 0; + out_8127227481367081246[311] = 0; + out_8127227481367081246[312] = 0; + out_8127227481367081246[313] = 0; + out_8127227481367081246[314] = 0; + out_8127227481367081246[315] = 0; + out_8127227481367081246[316] = 0; + out_8127227481367081246[317] = 0; + out_8127227481367081246[318] = 0; + out_8127227481367081246[319] = 0; + out_8127227481367081246[320] = 0; + out_8127227481367081246[321] = 0; + out_8127227481367081246[322] = 0; + out_8127227481367081246[323] = 1; } -void h_4(double *state, double *unused, double *out_6044371799568265897) { - out_6044371799568265897[0] = state[6] + state[9]; - out_6044371799568265897[1] = state[7] + state[10]; - out_6044371799568265897[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_3493540261963132576) { + out_3493540261963132576[0] = state[6] + state[9]; + out_3493540261963132576[1] = state[7] + state[10]; + out_3493540261963132576[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_8751938887176636728) { - out_8751938887176636728[0] = 0; - out_8751938887176636728[1] = 0; - out_8751938887176636728[2] = 0; - out_8751938887176636728[3] = 0; - out_8751938887176636728[4] = 0; - out_8751938887176636728[5] = 0; - out_8751938887176636728[6] = 1; - out_8751938887176636728[7] = 0; - out_8751938887176636728[8] = 0; - out_8751938887176636728[9] = 1; - out_8751938887176636728[10] = 0; - out_8751938887176636728[11] = 0; - out_8751938887176636728[12] = 0; - out_8751938887176636728[13] = 0; - out_8751938887176636728[14] = 0; - out_8751938887176636728[15] = 0; - out_8751938887176636728[16] = 0; - out_8751938887176636728[17] = 0; - out_8751938887176636728[18] = 0; - out_8751938887176636728[19] = 0; - out_8751938887176636728[20] = 0; - out_8751938887176636728[21] = 0; - out_8751938887176636728[22] = 0; - out_8751938887176636728[23] = 0; - out_8751938887176636728[24] = 0; - out_8751938887176636728[25] = 1; - out_8751938887176636728[26] = 0; - out_8751938887176636728[27] = 0; - out_8751938887176636728[28] = 1; - out_8751938887176636728[29] = 0; - out_8751938887176636728[30] = 0; - out_8751938887176636728[31] = 0; - out_8751938887176636728[32] = 0; - out_8751938887176636728[33] = 0; - out_8751938887176636728[34] = 0; - out_8751938887176636728[35] = 0; - out_8751938887176636728[36] = 0; - out_8751938887176636728[37] = 0; - out_8751938887176636728[38] = 0; - out_8751938887176636728[39] = 0; - out_8751938887176636728[40] = 0; - out_8751938887176636728[41] = 0; - out_8751938887176636728[42] = 0; - out_8751938887176636728[43] = 0; - out_8751938887176636728[44] = 1; - out_8751938887176636728[45] = 0; - out_8751938887176636728[46] = 0; - out_8751938887176636728[47] = 1; - out_8751938887176636728[48] = 0; - out_8751938887176636728[49] = 0; - out_8751938887176636728[50] = 0; - out_8751938887176636728[51] = 0; - out_8751938887176636728[52] = 0; - out_8751938887176636728[53] = 0; +void H_4(double *state, double *unused, double *out_425547638922626870) { + out_425547638922626870[0] = 0; + out_425547638922626870[1] = 0; + out_425547638922626870[2] = 0; + out_425547638922626870[3] = 0; + out_425547638922626870[4] = 0; + out_425547638922626870[5] = 0; + out_425547638922626870[6] = 1; + out_425547638922626870[7] = 0; + out_425547638922626870[8] = 0; + out_425547638922626870[9] = 1; + out_425547638922626870[10] = 0; + out_425547638922626870[11] = 0; + out_425547638922626870[12] = 0; + out_425547638922626870[13] = 0; + out_425547638922626870[14] = 0; + out_425547638922626870[15] = 0; + out_425547638922626870[16] = 0; + out_425547638922626870[17] = 0; + out_425547638922626870[18] = 0; + out_425547638922626870[19] = 0; + out_425547638922626870[20] = 0; + out_425547638922626870[21] = 0; + out_425547638922626870[22] = 0; + out_425547638922626870[23] = 0; + out_425547638922626870[24] = 0; + out_425547638922626870[25] = 1; + out_425547638922626870[26] = 0; + out_425547638922626870[27] = 0; + out_425547638922626870[28] = 1; + out_425547638922626870[29] = 0; + out_425547638922626870[30] = 0; + out_425547638922626870[31] = 0; + out_425547638922626870[32] = 0; + out_425547638922626870[33] = 0; + out_425547638922626870[34] = 0; + out_425547638922626870[35] = 0; + out_425547638922626870[36] = 0; + out_425547638922626870[37] = 0; + out_425547638922626870[38] = 0; + out_425547638922626870[39] = 0; + out_425547638922626870[40] = 0; + out_425547638922626870[41] = 0; + out_425547638922626870[42] = 0; + out_425547638922626870[43] = 0; + out_425547638922626870[44] = 1; + out_425547638922626870[45] = 0; + out_425547638922626870[46] = 0; + out_425547638922626870[47] = 1; + out_425547638922626870[48] = 0; + out_425547638922626870[49] = 0; + out_425547638922626870[50] = 0; + out_425547638922626870[51] = 0; + out_425547638922626870[52] = 0; + out_425547638922626870[53] = 0; } -void h_10(double *state, double *unused, double *out_9126782512040865980) { - out_9126782512040865980[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_9126782512040865980[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_9126782512040865980[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_6421583794262292879) { + out_6421583794262292879[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_6421583794262292879[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_6421583794262292879[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_2728048388349900962) { - out_2728048388349900962[0] = 0; - out_2728048388349900962[1] = 9.8100000000000005*cos(state[1]); - out_2728048388349900962[2] = 0; - out_2728048388349900962[3] = 0; - out_2728048388349900962[4] = -state[8]; - out_2728048388349900962[5] = state[7]; - out_2728048388349900962[6] = 0; - out_2728048388349900962[7] = state[5]; - out_2728048388349900962[8] = -state[4]; - out_2728048388349900962[9] = 0; - out_2728048388349900962[10] = 0; - out_2728048388349900962[11] = 0; - out_2728048388349900962[12] = 1; - out_2728048388349900962[13] = 0; - out_2728048388349900962[14] = 0; - out_2728048388349900962[15] = 1; - out_2728048388349900962[16] = 0; - out_2728048388349900962[17] = 0; - out_2728048388349900962[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_2728048388349900962[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_2728048388349900962[20] = 0; - out_2728048388349900962[21] = state[8]; - out_2728048388349900962[22] = 0; - out_2728048388349900962[23] = -state[6]; - out_2728048388349900962[24] = -state[5]; - out_2728048388349900962[25] = 0; - out_2728048388349900962[26] = state[3]; - out_2728048388349900962[27] = 0; - out_2728048388349900962[28] = 0; - out_2728048388349900962[29] = 0; - out_2728048388349900962[30] = 0; - out_2728048388349900962[31] = 1; - out_2728048388349900962[32] = 0; - out_2728048388349900962[33] = 0; - out_2728048388349900962[34] = 1; - out_2728048388349900962[35] = 0; - out_2728048388349900962[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_2728048388349900962[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_2728048388349900962[38] = 0; - out_2728048388349900962[39] = -state[7]; - out_2728048388349900962[40] = state[6]; - out_2728048388349900962[41] = 0; - out_2728048388349900962[42] = state[4]; - out_2728048388349900962[43] = -state[3]; - out_2728048388349900962[44] = 0; - out_2728048388349900962[45] = 0; - out_2728048388349900962[46] = 0; - out_2728048388349900962[47] = 0; - out_2728048388349900962[48] = 0; - out_2728048388349900962[49] = 0; - out_2728048388349900962[50] = 1; - out_2728048388349900962[51] = 0; - out_2728048388349900962[52] = 0; - out_2728048388349900962[53] = 1; +void H_10(double *state, double *unused, double *out_2093218466854338741) { + out_2093218466854338741[0] = 0; + out_2093218466854338741[1] = 9.8100000000000005*cos(state[1]); + out_2093218466854338741[2] = 0; + out_2093218466854338741[3] = 0; + out_2093218466854338741[4] = -state[8]; + out_2093218466854338741[5] = state[7]; + out_2093218466854338741[6] = 0; + out_2093218466854338741[7] = state[5]; + out_2093218466854338741[8] = -state[4]; + out_2093218466854338741[9] = 0; + out_2093218466854338741[10] = 0; + out_2093218466854338741[11] = 0; + out_2093218466854338741[12] = 1; + out_2093218466854338741[13] = 0; + out_2093218466854338741[14] = 0; + out_2093218466854338741[15] = 1; + out_2093218466854338741[16] = 0; + out_2093218466854338741[17] = 0; + out_2093218466854338741[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_2093218466854338741[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_2093218466854338741[20] = 0; + out_2093218466854338741[21] = state[8]; + out_2093218466854338741[22] = 0; + out_2093218466854338741[23] = -state[6]; + out_2093218466854338741[24] = -state[5]; + out_2093218466854338741[25] = 0; + out_2093218466854338741[26] = state[3]; + out_2093218466854338741[27] = 0; + out_2093218466854338741[28] = 0; + out_2093218466854338741[29] = 0; + out_2093218466854338741[30] = 0; + out_2093218466854338741[31] = 1; + out_2093218466854338741[32] = 0; + out_2093218466854338741[33] = 0; + out_2093218466854338741[34] = 1; + out_2093218466854338741[35] = 0; + out_2093218466854338741[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_2093218466854338741[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_2093218466854338741[38] = 0; + out_2093218466854338741[39] = -state[7]; + out_2093218466854338741[40] = state[6]; + out_2093218466854338741[41] = 0; + out_2093218466854338741[42] = state[4]; + out_2093218466854338741[43] = -state[3]; + out_2093218466854338741[44] = 0; + out_2093218466854338741[45] = 0; + out_2093218466854338741[46] = 0; + out_2093218466854338741[47] = 0; + out_2093218466854338741[48] = 0; + out_2093218466854338741[49] = 0; + out_2093218466854338741[50] = 1; + out_2093218466854338741[51] = 0; + out_2093218466854338741[52] = 0; + out_2093218466854338741[53] = 1; } -void h_13(double *state, double *unused, double *out_6648711810487729870) { - out_6648711810487729870[0] = state[3]; - out_6648711810487729870[1] = state[4]; - out_6648711810487729870[2] = state[5]; +void h_13(double *state, double *unused, double *out_3950551688454018720) { + out_3950551688454018720[0] = state[3]; + out_3950551688454018720[1] = state[4]; + out_3950551688454018720[2] = state[5]; } -void H_13(double *state, double *unused, double *out_1141307678859935799) { - out_1141307678859935799[0] = 0; - out_1141307678859935799[1] = 0; - out_1141307678859935799[2] = 0; - out_1141307678859935799[3] = 1; - out_1141307678859935799[4] = 0; - out_1141307678859935799[5] = 0; - out_1141307678859935799[6] = 0; - out_1141307678859935799[7] = 0; - out_1141307678859935799[8] = 0; - out_1141307678859935799[9] = 0; - out_1141307678859935799[10] = 0; - out_1141307678859935799[11] = 0; - out_1141307678859935799[12] = 0; - out_1141307678859935799[13] = 0; - out_1141307678859935799[14] = 0; - out_1141307678859935799[15] = 0; - out_1141307678859935799[16] = 0; - out_1141307678859935799[17] = 0; - out_1141307678859935799[18] = 0; - out_1141307678859935799[19] = 0; - out_1141307678859935799[20] = 0; - out_1141307678859935799[21] = 0; - out_1141307678859935799[22] = 1; - out_1141307678859935799[23] = 0; - out_1141307678859935799[24] = 0; - out_1141307678859935799[25] = 0; - out_1141307678859935799[26] = 0; - out_1141307678859935799[27] = 0; - out_1141307678859935799[28] = 0; - out_1141307678859935799[29] = 0; - out_1141307678859935799[30] = 0; - out_1141307678859935799[31] = 0; - out_1141307678859935799[32] = 0; - out_1141307678859935799[33] = 0; - out_1141307678859935799[34] = 0; - out_1141307678859935799[35] = 0; - out_1141307678859935799[36] = 0; - out_1141307678859935799[37] = 0; - out_1141307678859935799[38] = 0; - out_1141307678859935799[39] = 0; - out_1141307678859935799[40] = 0; - out_1141307678859935799[41] = 1; - out_1141307678859935799[42] = 0; - out_1141307678859935799[43] = 0; - out_1141307678859935799[44] = 0; - out_1141307678859935799[45] = 0; - out_1141307678859935799[46] = 0; - out_1141307678859935799[47] = 0; - out_1141307678859935799[48] = 0; - out_1141307678859935799[49] = 0; - out_1141307678859935799[50] = 0; - out_1141307678859935799[51] = 0; - out_1141307678859935799[52] = 0; - out_1141307678859935799[53] = 0; +void H_13(double *state, double *unused, double *out_3637821464254959671) { + out_3637821464254959671[0] = 0; + out_3637821464254959671[1] = 0; + out_3637821464254959671[2] = 0; + out_3637821464254959671[3] = 1; + out_3637821464254959671[4] = 0; + out_3637821464254959671[5] = 0; + out_3637821464254959671[6] = 0; + out_3637821464254959671[7] = 0; + out_3637821464254959671[8] = 0; + out_3637821464254959671[9] = 0; + out_3637821464254959671[10] = 0; + out_3637821464254959671[11] = 0; + out_3637821464254959671[12] = 0; + out_3637821464254959671[13] = 0; + out_3637821464254959671[14] = 0; + out_3637821464254959671[15] = 0; + out_3637821464254959671[16] = 0; + out_3637821464254959671[17] = 0; + out_3637821464254959671[18] = 0; + out_3637821464254959671[19] = 0; + out_3637821464254959671[20] = 0; + out_3637821464254959671[21] = 0; + out_3637821464254959671[22] = 1; + out_3637821464254959671[23] = 0; + out_3637821464254959671[24] = 0; + out_3637821464254959671[25] = 0; + out_3637821464254959671[26] = 0; + out_3637821464254959671[27] = 0; + out_3637821464254959671[28] = 0; + out_3637821464254959671[29] = 0; + out_3637821464254959671[30] = 0; + out_3637821464254959671[31] = 0; + out_3637821464254959671[32] = 0; + out_3637821464254959671[33] = 0; + out_3637821464254959671[34] = 0; + out_3637821464254959671[35] = 0; + out_3637821464254959671[36] = 0; + out_3637821464254959671[37] = 0; + out_3637821464254959671[38] = 0; + out_3637821464254959671[39] = 0; + out_3637821464254959671[40] = 0; + out_3637821464254959671[41] = 1; + out_3637821464254959671[42] = 0; + out_3637821464254959671[43] = 0; + out_3637821464254959671[44] = 0; + out_3637821464254959671[45] = 0; + out_3637821464254959671[46] = 0; + out_3637821464254959671[47] = 0; + out_3637821464254959671[48] = 0; + out_3637821464254959671[49] = 0; + out_3637821464254959671[50] = 0; + out_3637821464254959671[51] = 0; + out_3637821464254959671[52] = 0; + out_3637821464254959671[53] = 0; } -void h_14(double *state, double *unused, double *out_6901319929397273887) { - out_6901319929397273887[0] = state[6]; - out_6901319929397273887[1] = state[7]; - out_6901319929397273887[2] = state[8]; +void h_14(double *state, double *unused, double *out_4675773390607303658) { + out_4675773390607303658[0] = state[6]; + out_4675773390607303658[1] = state[7]; + out_4675773390607303658[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4788698030837152199) { - out_4788698030837152199[0] = 0; - out_4788698030837152199[1] = 0; - out_4788698030837152199[2] = 0; - out_4788698030837152199[3] = 0; - out_4788698030837152199[4] = 0; - out_4788698030837152199[5] = 0; - out_4788698030837152199[6] = 1; - out_4788698030837152199[7] = 0; - out_4788698030837152199[8] = 0; - out_4788698030837152199[9] = 0; - out_4788698030837152199[10] = 0; - out_4788698030837152199[11] = 0; - out_4788698030837152199[12] = 0; - out_4788698030837152199[13] = 0; - out_4788698030837152199[14] = 0; - out_4788698030837152199[15] = 0; - out_4788698030837152199[16] = 0; - out_4788698030837152199[17] = 0; - out_4788698030837152199[18] = 0; - out_4788698030837152199[19] = 0; - out_4788698030837152199[20] = 0; - out_4788698030837152199[21] = 0; - out_4788698030837152199[22] = 0; - out_4788698030837152199[23] = 0; - out_4788698030837152199[24] = 0; - out_4788698030837152199[25] = 1; - out_4788698030837152199[26] = 0; - out_4788698030837152199[27] = 0; - out_4788698030837152199[28] = 0; - out_4788698030837152199[29] = 0; - out_4788698030837152199[30] = 0; - out_4788698030837152199[31] = 0; - out_4788698030837152199[32] = 0; - out_4788698030837152199[33] = 0; - out_4788698030837152199[34] = 0; - out_4788698030837152199[35] = 0; - out_4788698030837152199[36] = 0; - out_4788698030837152199[37] = 0; - out_4788698030837152199[38] = 0; - out_4788698030837152199[39] = 0; - out_4788698030837152199[40] = 0; - out_4788698030837152199[41] = 0; - out_4788698030837152199[42] = 0; - out_4788698030837152199[43] = 0; - out_4788698030837152199[44] = 1; - out_4788698030837152199[45] = 0; - out_4788698030837152199[46] = 0; - out_4788698030837152199[47] = 0; - out_4788698030837152199[48] = 0; - out_4788698030837152199[49] = 0; - out_4788698030837152199[50] = 0; - out_4788698030837152199[51] = 0; - out_4788698030837152199[52] = 0; - out_4788698030837152199[53] = 0; +void H_14(double *state, double *unused, double *out_2657240793372745426) { + out_2657240793372745426[0] = 0; + out_2657240793372745426[1] = 0; + out_2657240793372745426[2] = 0; + out_2657240793372745426[3] = 0; + out_2657240793372745426[4] = 0; + out_2657240793372745426[5] = 0; + out_2657240793372745426[6] = 1; + out_2657240793372745426[7] = 0; + out_2657240793372745426[8] = 0; + out_2657240793372745426[9] = 0; + out_2657240793372745426[10] = 0; + out_2657240793372745426[11] = 0; + out_2657240793372745426[12] = 0; + out_2657240793372745426[13] = 0; + out_2657240793372745426[14] = 0; + out_2657240793372745426[15] = 0; + out_2657240793372745426[16] = 0; + out_2657240793372745426[17] = 0; + out_2657240793372745426[18] = 0; + out_2657240793372745426[19] = 0; + out_2657240793372745426[20] = 0; + out_2657240793372745426[21] = 0; + out_2657240793372745426[22] = 0; + out_2657240793372745426[23] = 0; + out_2657240793372745426[24] = 0; + out_2657240793372745426[25] = 1; + out_2657240793372745426[26] = 0; + out_2657240793372745426[27] = 0; + out_2657240793372745426[28] = 0; + out_2657240793372745426[29] = 0; + out_2657240793372745426[30] = 0; + out_2657240793372745426[31] = 0; + out_2657240793372745426[32] = 0; + out_2657240793372745426[33] = 0; + out_2657240793372745426[34] = 0; + out_2657240793372745426[35] = 0; + out_2657240793372745426[36] = 0; + out_2657240793372745426[37] = 0; + out_2657240793372745426[38] = 0; + out_2657240793372745426[39] = 0; + out_2657240793372745426[40] = 0; + out_2657240793372745426[41] = 0; + out_2657240793372745426[42] = 0; + out_2657240793372745426[43] = 0; + out_2657240793372745426[44] = 1; + out_2657240793372745426[45] = 0; + out_2657240793372745426[46] = 0; + out_2657240793372745426[47] = 0; + out_2657240793372745426[48] = 0; + out_2657240793372745426[49] = 0; + out_2657240793372745426[50] = 0; + out_2657240793372745426[51] = 0; + out_2657240793372745426[52] = 0; + out_2657240793372745426[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_6531280423659383110) { - err_fun(nom_x, delta_x, out_6531280423659383110); +void pose_err_fun(double *nom_x, double *delta_x, double *out_3892827861815398533) { + err_fun(nom_x, delta_x, out_3892827861815398533); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2847042605170284069) { - inv_err_fun(nom_x, true_x, out_2847042605170284069); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_341451131886162609) { + inv_err_fun(nom_x, true_x, out_341451131886162609); } -void pose_H_mod_fun(double *state, double *out_1201664963462517165) { - H_mod_fun(state, out_1201664963462517165); +void pose_H_mod_fun(double *state, double *out_328613363132085737) { + H_mod_fun(state, out_328613363132085737); } -void pose_f_fun(double *state, double dt, double *out_4925442091395050631) { - f_fun(state, dt, out_4925442091395050631); +void pose_f_fun(double *state, double dt, double *out_2411932017416861366) { + f_fun(state, dt, out_2411932017416861366); } -void pose_F_fun(double *state, double dt, double *out_3854196706556040023) { - F_fun(state, dt, out_3854196706556040023); +void pose_F_fun(double *state, double dt, double *out_8127227481367081246) { + F_fun(state, dt, out_8127227481367081246); } -void pose_h_4(double *state, double *unused, double *out_6044371799568265897) { - h_4(state, unused, out_6044371799568265897); +void pose_h_4(double *state, double *unused, double *out_3493540261963132576) { + h_4(state, unused, out_3493540261963132576); } -void pose_H_4(double *state, double *unused, double *out_8751938887176636728) { - H_4(state, unused, out_8751938887176636728); +void pose_H_4(double *state, double *unused, double *out_425547638922626870) { + H_4(state, unused, out_425547638922626870); } -void pose_h_10(double *state, double *unused, double *out_9126782512040865980) { - h_10(state, unused, out_9126782512040865980); +void pose_h_10(double *state, double *unused, double *out_6421583794262292879) { + h_10(state, unused, out_6421583794262292879); } -void pose_H_10(double *state, double *unused, double *out_2728048388349900962) { - H_10(state, unused, out_2728048388349900962); +void pose_H_10(double *state, double *unused, double *out_2093218466854338741) { + H_10(state, unused, out_2093218466854338741); } -void pose_h_13(double *state, double *unused, double *out_6648711810487729870) { - h_13(state, unused, out_6648711810487729870); +void pose_h_13(double *state, double *unused, double *out_3950551688454018720) { + h_13(state, unused, out_3950551688454018720); } -void pose_H_13(double *state, double *unused, double *out_1141307678859935799) { - H_13(state, unused, out_1141307678859935799); +void pose_H_13(double *state, double *unused, double *out_3637821464254959671) { + H_13(state, unused, out_3637821464254959671); } -void pose_h_14(double *state, double *unused, double *out_6901319929397273887) { - h_14(state, unused, out_6901319929397273887); +void pose_h_14(double *state, double *unused, double *out_4675773390607303658) { + h_14(state, unused, out_4675773390607303658); } -void pose_H_14(double *state, double *unused, double *out_4788698030837152199) { - H_14(state, unused, out_4788698030837152199); +void pose_H_14(double *state, double *unused, double *out_2657240793372745426) { + H_14(state, unused, out_2657240793372745426); } 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 bc3ed9a96..e0a528369 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_6531280423659383110); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2847042605170284069); -void pose_H_mod_fun(double *state, double *out_1201664963462517165); -void pose_f_fun(double *state, double dt, double *out_4925442091395050631); -void pose_F_fun(double *state, double dt, double *out_3854196706556040023); -void pose_h_4(double *state, double *unused, double *out_6044371799568265897); -void pose_H_4(double *state, double *unused, double *out_8751938887176636728); -void pose_h_10(double *state, double *unused, double *out_9126782512040865980); -void pose_H_10(double *state, double *unused, double *out_2728048388349900962); -void pose_h_13(double *state, double *unused, double *out_6648711810487729870); -void pose_H_13(double *state, double *unused, double *out_1141307678859935799); -void pose_h_14(double *state, double *unused, double *out_6901319929397273887); -void pose_H_14(double *state, double *unused, double *out_4788698030837152199); +void pose_err_fun(double *nom_x, double *delta_x, double *out_3892827861815398533); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_341451131886162609); +void pose_H_mod_fun(double *state, double *out_328613363132085737); +void pose_f_fun(double *state, double dt, double *out_2411932017416861366); +void pose_F_fun(double *state, double dt, double *out_8127227481367081246); +void pose_h_4(double *state, double *unused, double *out_3493540261963132576); +void pose_H_4(double *state, double *unused, double *out_425547638922626870); +void pose_h_10(double *state, double *unused, double *out_6421583794262292879); +void pose_H_10(double *state, double *unused, double *out_2093218466854338741); +void pose_h_13(double *state, double *unused, double *out_3950551688454018720); +void pose_H_13(double *state, double *unused, double *out_3637821464254959671); +void pose_h_14(double *state, double *unused, double *out_4675773390607303658); +void pose_H_14(double *state, double *unused, double *out_2657240793372745426); 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 1b95da792..be2072ac0 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 8fbba96b1..611894cb8 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 1d46a13ad..f775a030c 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 15f5104f0..9072a8449 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 29e1ba83e..6073f5152 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index b8011cc33..318ab235b 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ