diff --git a/common/libcommon.a b/common/libcommon.a index f70de9b17..c7c0a4a04 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 8b03acf06..cffd8f661 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 7d3b0db07..b1eb609f1 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 065db4bd0..da2f5a25a 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 77b723c75..0932cf9b7 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 1c8455266..785d1c658 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 1af84fbf9..f4207174c 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 a7b06c6a2..a7da27e8b 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 f2f0dd61d..7e184d782 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 f2f0dd61d..7e184d782 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 343d4bc07..72de41634 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 a7b06c6a2..a7da27e8b 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 031f5cce6..3034036b6 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-5d86b7ac-DEBUG"; +const uint8_t gitversion[19] = "DEV-fabf1226-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index da493d2c0..d01ff9401 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 f5c5b8fb3..8a2c3b790 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 4612e65bd..292196012 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 d4cf273d4..f169de79e 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 076bee071..f158e55ba 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 5db61ec0e..fa4345926 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 fa7209c93..401bfae30 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 08b3658ef..01dde74e6 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 bb9abcf50..2d02fbe76 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 2c6a253e4..cacd11336 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 b92664832..40e6cc15c 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 beade5791..7bd7da5b0 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 0e1d7cc4d..953dcd614 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 4681809cf..f6169837d 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 833870e61..afcc47bcb 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 da7ffe939..287bbb1c6 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 ba132820c..e3f434680 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 ca230f308..2a8239a3b 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 4661088bd..e38e3e2dc 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 762799d5a..d5e4cfcaf 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 4eef293e1..6d1ecdb99 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-5d86b7ac-DEBUG \ No newline at end of file +DEV-fabf1226-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 43fd583b2..339ec0e77 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_1626624007171975298) { - out_1626624007171975298[0] = delta_x[0] + nom_x[0]; - out_1626624007171975298[1] = delta_x[1] + nom_x[1]; - out_1626624007171975298[2] = delta_x[2] + nom_x[2]; - out_1626624007171975298[3] = delta_x[3] + nom_x[3]; - out_1626624007171975298[4] = delta_x[4] + nom_x[4]; - out_1626624007171975298[5] = delta_x[5] + nom_x[5]; - out_1626624007171975298[6] = delta_x[6] + nom_x[6]; - out_1626624007171975298[7] = delta_x[7] + nom_x[7]; - out_1626624007171975298[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_4189395199092511703) { + out_4189395199092511703[0] = delta_x[0] + nom_x[0]; + out_4189395199092511703[1] = delta_x[1] + nom_x[1]; + out_4189395199092511703[2] = delta_x[2] + nom_x[2]; + out_4189395199092511703[3] = delta_x[3] + nom_x[3]; + out_4189395199092511703[4] = delta_x[4] + nom_x[4]; + out_4189395199092511703[5] = delta_x[5] + nom_x[5]; + out_4189395199092511703[6] = delta_x[6] + nom_x[6]; + out_4189395199092511703[7] = delta_x[7] + nom_x[7]; + out_4189395199092511703[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3841721694315715550) { - out_3841721694315715550[0] = -nom_x[0] + true_x[0]; - out_3841721694315715550[1] = -nom_x[1] + true_x[1]; - out_3841721694315715550[2] = -nom_x[2] + true_x[2]; - out_3841721694315715550[3] = -nom_x[3] + true_x[3]; - out_3841721694315715550[4] = -nom_x[4] + true_x[4]; - out_3841721694315715550[5] = -nom_x[5] + true_x[5]; - out_3841721694315715550[6] = -nom_x[6] + true_x[6]; - out_3841721694315715550[7] = -nom_x[7] + true_x[7]; - out_3841721694315715550[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5755129993245429730) { + out_5755129993245429730[0] = -nom_x[0] + true_x[0]; + out_5755129993245429730[1] = -nom_x[1] + true_x[1]; + out_5755129993245429730[2] = -nom_x[2] + true_x[2]; + out_5755129993245429730[3] = -nom_x[3] + true_x[3]; + out_5755129993245429730[4] = -nom_x[4] + true_x[4]; + out_5755129993245429730[5] = -nom_x[5] + true_x[5]; + out_5755129993245429730[6] = -nom_x[6] + true_x[6]; + out_5755129993245429730[7] = -nom_x[7] + true_x[7]; + out_5755129993245429730[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_7646044064038577504) { - out_7646044064038577504[0] = 1.0; - out_7646044064038577504[1] = 0.0; - out_7646044064038577504[2] = 0.0; - out_7646044064038577504[3] = 0.0; - out_7646044064038577504[4] = 0.0; - out_7646044064038577504[5] = 0.0; - out_7646044064038577504[6] = 0.0; - out_7646044064038577504[7] = 0.0; - out_7646044064038577504[8] = 0.0; - out_7646044064038577504[9] = 0.0; - out_7646044064038577504[10] = 1.0; - out_7646044064038577504[11] = 0.0; - out_7646044064038577504[12] = 0.0; - out_7646044064038577504[13] = 0.0; - out_7646044064038577504[14] = 0.0; - out_7646044064038577504[15] = 0.0; - out_7646044064038577504[16] = 0.0; - out_7646044064038577504[17] = 0.0; - out_7646044064038577504[18] = 0.0; - out_7646044064038577504[19] = 0.0; - out_7646044064038577504[20] = 1.0; - out_7646044064038577504[21] = 0.0; - out_7646044064038577504[22] = 0.0; - out_7646044064038577504[23] = 0.0; - out_7646044064038577504[24] = 0.0; - out_7646044064038577504[25] = 0.0; - out_7646044064038577504[26] = 0.0; - out_7646044064038577504[27] = 0.0; - out_7646044064038577504[28] = 0.0; - out_7646044064038577504[29] = 0.0; - out_7646044064038577504[30] = 1.0; - out_7646044064038577504[31] = 0.0; - out_7646044064038577504[32] = 0.0; - out_7646044064038577504[33] = 0.0; - out_7646044064038577504[34] = 0.0; - out_7646044064038577504[35] = 0.0; - out_7646044064038577504[36] = 0.0; - out_7646044064038577504[37] = 0.0; - out_7646044064038577504[38] = 0.0; - out_7646044064038577504[39] = 0.0; - out_7646044064038577504[40] = 1.0; - out_7646044064038577504[41] = 0.0; - out_7646044064038577504[42] = 0.0; - out_7646044064038577504[43] = 0.0; - out_7646044064038577504[44] = 0.0; - out_7646044064038577504[45] = 0.0; - out_7646044064038577504[46] = 0.0; - out_7646044064038577504[47] = 0.0; - out_7646044064038577504[48] = 0.0; - out_7646044064038577504[49] = 0.0; - out_7646044064038577504[50] = 1.0; - out_7646044064038577504[51] = 0.0; - out_7646044064038577504[52] = 0.0; - out_7646044064038577504[53] = 0.0; - out_7646044064038577504[54] = 0.0; - out_7646044064038577504[55] = 0.0; - out_7646044064038577504[56] = 0.0; - out_7646044064038577504[57] = 0.0; - out_7646044064038577504[58] = 0.0; - out_7646044064038577504[59] = 0.0; - out_7646044064038577504[60] = 1.0; - out_7646044064038577504[61] = 0.0; - out_7646044064038577504[62] = 0.0; - out_7646044064038577504[63] = 0.0; - out_7646044064038577504[64] = 0.0; - out_7646044064038577504[65] = 0.0; - out_7646044064038577504[66] = 0.0; - out_7646044064038577504[67] = 0.0; - out_7646044064038577504[68] = 0.0; - out_7646044064038577504[69] = 0.0; - out_7646044064038577504[70] = 1.0; - out_7646044064038577504[71] = 0.0; - out_7646044064038577504[72] = 0.0; - out_7646044064038577504[73] = 0.0; - out_7646044064038577504[74] = 0.0; - out_7646044064038577504[75] = 0.0; - out_7646044064038577504[76] = 0.0; - out_7646044064038577504[77] = 0.0; - out_7646044064038577504[78] = 0.0; - out_7646044064038577504[79] = 0.0; - out_7646044064038577504[80] = 1.0; +void H_mod_fun(double *state, double *out_3130611196753066809) { + out_3130611196753066809[0] = 1.0; + out_3130611196753066809[1] = 0.0; + out_3130611196753066809[2] = 0.0; + out_3130611196753066809[3] = 0.0; + out_3130611196753066809[4] = 0.0; + out_3130611196753066809[5] = 0.0; + out_3130611196753066809[6] = 0.0; + out_3130611196753066809[7] = 0.0; + out_3130611196753066809[8] = 0.0; + out_3130611196753066809[9] = 0.0; + out_3130611196753066809[10] = 1.0; + out_3130611196753066809[11] = 0.0; + out_3130611196753066809[12] = 0.0; + out_3130611196753066809[13] = 0.0; + out_3130611196753066809[14] = 0.0; + out_3130611196753066809[15] = 0.0; + out_3130611196753066809[16] = 0.0; + out_3130611196753066809[17] = 0.0; + out_3130611196753066809[18] = 0.0; + out_3130611196753066809[19] = 0.0; + out_3130611196753066809[20] = 1.0; + out_3130611196753066809[21] = 0.0; + out_3130611196753066809[22] = 0.0; + out_3130611196753066809[23] = 0.0; + out_3130611196753066809[24] = 0.0; + out_3130611196753066809[25] = 0.0; + out_3130611196753066809[26] = 0.0; + out_3130611196753066809[27] = 0.0; + out_3130611196753066809[28] = 0.0; + out_3130611196753066809[29] = 0.0; + out_3130611196753066809[30] = 1.0; + out_3130611196753066809[31] = 0.0; + out_3130611196753066809[32] = 0.0; + out_3130611196753066809[33] = 0.0; + out_3130611196753066809[34] = 0.0; + out_3130611196753066809[35] = 0.0; + out_3130611196753066809[36] = 0.0; + out_3130611196753066809[37] = 0.0; + out_3130611196753066809[38] = 0.0; + out_3130611196753066809[39] = 0.0; + out_3130611196753066809[40] = 1.0; + out_3130611196753066809[41] = 0.0; + out_3130611196753066809[42] = 0.0; + out_3130611196753066809[43] = 0.0; + out_3130611196753066809[44] = 0.0; + out_3130611196753066809[45] = 0.0; + out_3130611196753066809[46] = 0.0; + out_3130611196753066809[47] = 0.0; + out_3130611196753066809[48] = 0.0; + out_3130611196753066809[49] = 0.0; + out_3130611196753066809[50] = 1.0; + out_3130611196753066809[51] = 0.0; + out_3130611196753066809[52] = 0.0; + out_3130611196753066809[53] = 0.0; + out_3130611196753066809[54] = 0.0; + out_3130611196753066809[55] = 0.0; + out_3130611196753066809[56] = 0.0; + out_3130611196753066809[57] = 0.0; + out_3130611196753066809[58] = 0.0; + out_3130611196753066809[59] = 0.0; + out_3130611196753066809[60] = 1.0; + out_3130611196753066809[61] = 0.0; + out_3130611196753066809[62] = 0.0; + out_3130611196753066809[63] = 0.0; + out_3130611196753066809[64] = 0.0; + out_3130611196753066809[65] = 0.0; + out_3130611196753066809[66] = 0.0; + out_3130611196753066809[67] = 0.0; + out_3130611196753066809[68] = 0.0; + out_3130611196753066809[69] = 0.0; + out_3130611196753066809[70] = 1.0; + out_3130611196753066809[71] = 0.0; + out_3130611196753066809[72] = 0.0; + out_3130611196753066809[73] = 0.0; + out_3130611196753066809[74] = 0.0; + out_3130611196753066809[75] = 0.0; + out_3130611196753066809[76] = 0.0; + out_3130611196753066809[77] = 0.0; + out_3130611196753066809[78] = 0.0; + out_3130611196753066809[79] = 0.0; + out_3130611196753066809[80] = 1.0; } -void f_fun(double *state, double dt, double *out_3692802059814366209) { - out_3692802059814366209[0] = state[0]; - out_3692802059814366209[1] = state[1]; - out_3692802059814366209[2] = state[2]; - out_3692802059814366209[3] = state[3]; - out_3692802059814366209[4] = state[4]; - out_3692802059814366209[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_3692802059814366209[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_3692802059814366209[7] = state[7]; - out_3692802059814366209[8] = state[8]; +void f_fun(double *state, double dt, double *out_5961640813003379683) { + out_5961640813003379683[0] = state[0]; + out_5961640813003379683[1] = state[1]; + out_5961640813003379683[2] = state[2]; + out_5961640813003379683[3] = state[3]; + out_5961640813003379683[4] = state[4]; + out_5961640813003379683[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_5961640813003379683[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_5961640813003379683[7] = state[7]; + out_5961640813003379683[8] = state[8]; } -void F_fun(double *state, double dt, double *out_6278869713612932388) { - out_6278869713612932388[0] = 1; - out_6278869713612932388[1] = 0; - out_6278869713612932388[2] = 0; - out_6278869713612932388[3] = 0; - out_6278869713612932388[4] = 0; - out_6278869713612932388[5] = 0; - out_6278869713612932388[6] = 0; - out_6278869713612932388[7] = 0; - out_6278869713612932388[8] = 0; - out_6278869713612932388[9] = 0; - out_6278869713612932388[10] = 1; - out_6278869713612932388[11] = 0; - out_6278869713612932388[12] = 0; - out_6278869713612932388[13] = 0; - out_6278869713612932388[14] = 0; - out_6278869713612932388[15] = 0; - out_6278869713612932388[16] = 0; - out_6278869713612932388[17] = 0; - out_6278869713612932388[18] = 0; - out_6278869713612932388[19] = 0; - out_6278869713612932388[20] = 1; - out_6278869713612932388[21] = 0; - out_6278869713612932388[22] = 0; - out_6278869713612932388[23] = 0; - out_6278869713612932388[24] = 0; - out_6278869713612932388[25] = 0; - out_6278869713612932388[26] = 0; - out_6278869713612932388[27] = 0; - out_6278869713612932388[28] = 0; - out_6278869713612932388[29] = 0; - out_6278869713612932388[30] = 1; - out_6278869713612932388[31] = 0; - out_6278869713612932388[32] = 0; - out_6278869713612932388[33] = 0; - out_6278869713612932388[34] = 0; - out_6278869713612932388[35] = 0; - out_6278869713612932388[36] = 0; - out_6278869713612932388[37] = 0; - out_6278869713612932388[38] = 0; - out_6278869713612932388[39] = 0; - out_6278869713612932388[40] = 1; - out_6278869713612932388[41] = 0; - out_6278869713612932388[42] = 0; - out_6278869713612932388[43] = 0; - out_6278869713612932388[44] = 0; - out_6278869713612932388[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_6278869713612932388[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_6278869713612932388[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6278869713612932388[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6278869713612932388[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_6278869713612932388[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_6278869713612932388[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_6278869713612932388[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_6278869713612932388[53] = -9.8100000000000005*dt; - out_6278869713612932388[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_6278869713612932388[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_6278869713612932388[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6278869713612932388[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6278869713612932388[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_6278869713612932388[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_6278869713612932388[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_6278869713612932388[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6278869713612932388[62] = 0; - out_6278869713612932388[63] = 0; - out_6278869713612932388[64] = 0; - out_6278869713612932388[65] = 0; - out_6278869713612932388[66] = 0; - out_6278869713612932388[67] = 0; - out_6278869713612932388[68] = 0; - out_6278869713612932388[69] = 0; - out_6278869713612932388[70] = 1; - out_6278869713612932388[71] = 0; - out_6278869713612932388[72] = 0; - out_6278869713612932388[73] = 0; - out_6278869713612932388[74] = 0; - out_6278869713612932388[75] = 0; - out_6278869713612932388[76] = 0; - out_6278869713612932388[77] = 0; - out_6278869713612932388[78] = 0; - out_6278869713612932388[79] = 0; - out_6278869713612932388[80] = 1; +void F_fun(double *state, double dt, double *out_2654082016279909676) { + out_2654082016279909676[0] = 1; + out_2654082016279909676[1] = 0; + out_2654082016279909676[2] = 0; + out_2654082016279909676[3] = 0; + out_2654082016279909676[4] = 0; + out_2654082016279909676[5] = 0; + out_2654082016279909676[6] = 0; + out_2654082016279909676[7] = 0; + out_2654082016279909676[8] = 0; + out_2654082016279909676[9] = 0; + out_2654082016279909676[10] = 1; + out_2654082016279909676[11] = 0; + out_2654082016279909676[12] = 0; + out_2654082016279909676[13] = 0; + out_2654082016279909676[14] = 0; + out_2654082016279909676[15] = 0; + out_2654082016279909676[16] = 0; + out_2654082016279909676[17] = 0; + out_2654082016279909676[18] = 0; + out_2654082016279909676[19] = 0; + out_2654082016279909676[20] = 1; + out_2654082016279909676[21] = 0; + out_2654082016279909676[22] = 0; + out_2654082016279909676[23] = 0; + out_2654082016279909676[24] = 0; + out_2654082016279909676[25] = 0; + out_2654082016279909676[26] = 0; + out_2654082016279909676[27] = 0; + out_2654082016279909676[28] = 0; + out_2654082016279909676[29] = 0; + out_2654082016279909676[30] = 1; + out_2654082016279909676[31] = 0; + out_2654082016279909676[32] = 0; + out_2654082016279909676[33] = 0; + out_2654082016279909676[34] = 0; + out_2654082016279909676[35] = 0; + out_2654082016279909676[36] = 0; + out_2654082016279909676[37] = 0; + out_2654082016279909676[38] = 0; + out_2654082016279909676[39] = 0; + out_2654082016279909676[40] = 1; + out_2654082016279909676[41] = 0; + out_2654082016279909676[42] = 0; + out_2654082016279909676[43] = 0; + out_2654082016279909676[44] = 0; + out_2654082016279909676[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_2654082016279909676[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2654082016279909676[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2654082016279909676[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2654082016279909676[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_2654082016279909676[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2654082016279909676[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2654082016279909676[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2654082016279909676[53] = -9.8100000000000005*dt; + out_2654082016279909676[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_2654082016279909676[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2654082016279909676[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2654082016279909676[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2654082016279909676[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_2654082016279909676[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2654082016279909676[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_2654082016279909676[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2654082016279909676[62] = 0; + out_2654082016279909676[63] = 0; + out_2654082016279909676[64] = 0; + out_2654082016279909676[65] = 0; + out_2654082016279909676[66] = 0; + out_2654082016279909676[67] = 0; + out_2654082016279909676[68] = 0; + out_2654082016279909676[69] = 0; + out_2654082016279909676[70] = 1; + out_2654082016279909676[71] = 0; + out_2654082016279909676[72] = 0; + out_2654082016279909676[73] = 0; + out_2654082016279909676[74] = 0; + out_2654082016279909676[75] = 0; + out_2654082016279909676[76] = 0; + out_2654082016279909676[77] = 0; + out_2654082016279909676[78] = 0; + out_2654082016279909676[79] = 0; + out_2654082016279909676[80] = 1; } -void h_25(double *state, double *unused, double *out_1903293770744417105) { - out_1903293770744417105[0] = state[6]; +void h_25(double *state, double *unused, double *out_3340180793096633183) { + out_3340180793096633183[0] = state[6]; } -void H_25(double *state, double *unused, double *out_1607511783758736875) { - out_1607511783758736875[0] = 0; - out_1607511783758736875[1] = 0; - out_1607511783758736875[2] = 0; - out_1607511783758736875[3] = 0; - out_1607511783758736875[4] = 0; - out_1607511783758736875[5] = 0; - out_1607511783758736875[6] = 1; - out_1607511783758736875[7] = 0; - out_1607511783758736875[8] = 0; +void H_25(double *state, double *unused, double *out_4434950306945038429) { + out_4434950306945038429[0] = 0; + out_4434950306945038429[1] = 0; + out_4434950306945038429[2] = 0; + out_4434950306945038429[3] = 0; + out_4434950306945038429[4] = 0; + out_4434950306945038429[5] = 0; + out_4434950306945038429[6] = 1; + out_4434950306945038429[7] = 0; + out_4434950306945038429[8] = 0; } -void h_24(double *state, double *unused, double *out_3019943109680816786) { - out_3019943109680816786[0] = state[4]; - out_3019943109680816786[1] = state[5]; +void h_24(double *state, double *unused, double *out_7854800018825826791) { + out_7854800018825826791[0] = state[4]; + out_7854800018825826791[1] = state[5]; } -void H_24(double *state, double *unused, double *out_565137815246762691) { - out_565137815246762691[0] = 0; - out_565137815246762691[1] = 0; - out_565137815246762691[2] = 0; - out_565137815246762691[3] = 0; - out_565137815246762691[4] = 1; - out_565137815246762691[5] = 0; - out_565137815246762691[6] = 0; - out_565137815246762691[7] = 0; - out_565137815246762691[8] = 0; - out_565137815246762691[9] = 0; - out_565137815246762691[10] = 0; - out_565137815246762691[11] = 0; - out_565137815246762691[12] = 0; - out_565137815246762691[13] = 0; - out_565137815246762691[14] = 1; - out_565137815246762691[15] = 0; - out_565137815246762691[16] = 0; - out_565137815246762691[17] = 0; +void H_24(double *state, double *unused, double *out_6607599905950537995) { + out_6607599905950537995[0] = 0; + out_6607599905950537995[1] = 0; + out_6607599905950537995[2] = 0; + out_6607599905950537995[3] = 0; + out_6607599905950537995[4] = 1; + out_6607599905950537995[5] = 0; + out_6607599905950537995[6] = 0; + out_6607599905950537995[7] = 0; + out_6607599905950537995[8] = 0; + out_6607599905950537995[9] = 0; + out_6607599905950537995[10] = 0; + out_6607599905950537995[11] = 0; + out_6607599905950537995[12] = 0; + out_6607599905950537995[13] = 0; + out_6607599905950537995[14] = 1; + out_6607599905950537995[15] = 0; + out_6607599905950537995[16] = 0; + out_6607599905950537995[17] = 0; } -void h_30(double *state, double *unused, double *out_5540743298998439455) { - out_5540743298998439455[0] = state[4]; +void h_30(double *state, double *unused, double *out_3104430337924860088) { + out_3104430337924860088[0] = state[4]; } -void H_30(double *state, double *unused, double *out_4125844742265985502) { - out_4125844742265985502[0] = 0; - out_4125844742265985502[1] = 0; - out_4125844742265985502[2] = 0; - out_4125844742265985502[3] = 0; - out_4125844742265985502[4] = 1; - out_4125844742265985502[5] = 0; - out_4125844742265985502[6] = 0; - out_4125844742265985502[7] = 0; - out_4125844742265985502[8] = 0; +void H_30(double *state, double *unused, double *out_1916617348437789802) { + out_1916617348437789802[0] = 0; + out_1916617348437789802[1] = 0; + out_1916617348437789802[2] = 0; + out_1916617348437789802[3] = 0; + out_1916617348437789802[4] = 1; + out_1916617348437789802[5] = 0; + out_1916617348437789802[6] = 0; + out_1916617348437789802[7] = 0; + out_1916617348437789802[8] = 0; } -void h_26(double *state, double *unused, double *out_539070031337593810) { - out_539070031337593810[0] = state[7]; +void h_26(double *state, double *unused, double *out_4544914835386474254) { + out_4544914835386474254[0] = state[7]; } -void H_26(double *state, double *unused, double *out_2133991535115319349) { - out_2133991535115319349[0] = 0; - out_2133991535115319349[1] = 0; - out_2133991535115319349[2] = 0; - out_2133991535115319349[3] = 0; - out_2133991535115319349[4] = 0; - out_2133991535115319349[5] = 0; - out_2133991535115319349[6] = 0; - out_2133991535115319349[7] = 1; - out_2133991535115319349[8] = 0; +void H_26(double *state, double *unused, double *out_8176453625819094653) { + out_8176453625819094653[0] = 0; + out_8176453625819094653[1] = 0; + out_8176453625819094653[2] = 0; + out_8176453625819094653[3] = 0; + out_8176453625819094653[4] = 0; + out_8176453625819094653[5] = 0; + out_8176453625819094653[6] = 0; + out_8176453625819094653[7] = 1; + out_8176453625819094653[8] = 0; } -void h_27(double *state, double *unused, double *out_7078675401262423660) { - out_7078675401262423660[0] = state[3]; +void h_27(double *state, double *unused, double *out_4426907441453097510) { + out_4426907441453097510[0] = state[3]; } -void H_27(double *state, double *unused, double *out_1951081430465560591) { - out_1951081430465560591[0] = 0; - out_1951081430465560591[1] = 0; - out_1951081430465560591[2] = 0; - out_1951081430465560591[3] = 1; - out_1951081430465560591[4] = 0; - out_1951081430465560591[5] = 0; - out_1951081430465560591[6] = 0; - out_1951081430465560591[7] = 0; - out_1951081430465560591[8] = 0; +void H_27(double *state, double *unused, double *out_306976722746153415) { + out_306976722746153415[0] = 0; + out_306976722746153415[1] = 0; + out_306976722746153415[2] = 0; + out_306976722746153415[3] = 1; + out_306976722746153415[4] = 0; + out_306976722746153415[5] = 0; + out_306976722746153415[6] = 0; + out_306976722746153415[7] = 0; + out_306976722746153415[8] = 0; } -void h_29(double *state, double *unused, double *out_4597233446096777020) { - out_4597233446096777020[0] = state[1]; +void h_29(double *state, double *unused, double *out_2160920485023197653) { + out_2160920485023197653[0] = state[1]; } -void H_29(double *state, double *unused, double *out_4636076086580377686) { - out_4636076086580377686[0] = 0; - out_4636076086580377686[1] = 1; - out_4636076086580377686[2] = 0; - out_4636076086580377686[3] = 0; - out_4636076086580377686[4] = 0; - out_4636076086580377686[5] = 0; - out_4636076086580377686[6] = 0; - out_4636076086580377686[7] = 0; - out_4636076086580377686[8] = 0; +void H_29(double *state, double *unused, double *out_1406386004123397618) { + out_1406386004123397618[0] = 0; + out_1406386004123397618[1] = 1; + out_1406386004123397618[2] = 0; + out_1406386004123397618[3] = 0; + out_1406386004123397618[4] = 0; + out_1406386004123397618[5] = 0; + out_1406386004123397618[6] = 0; + out_1406386004123397618[7] = 0; + out_1406386004123397618[8] = 0; } -void h_28(double *state, double *unused, double *out_2077002133601578015) { - out_2077002133601578015[0] = state[0]; +void h_28(double *state, double *unused, double *out_7017073928463388327) { + out_7017073928463388327[0] = state[0]; } -void H_28(double *state, double *unused, double *out_446322930489152888) { - out_446322930489152888[0] = 1; - out_446322930489152888[1] = 0; - out_446322930489152888[2] = 0; - out_446322930489152888[3] = 0; - out_446322930489152888[4] = 0; - out_446322930489152888[5] = 0; - out_446322930489152888[6] = 0; - out_446322930489152888[7] = 0; - out_446322930489152888[8] = 0; +void H_28(double *state, double *unused, double *out_6488785021192928192) { + out_6488785021192928192[0] = 1; + out_6488785021192928192[1] = 0; + out_6488785021192928192[2] = 0; + out_6488785021192928192[3] = 0; + out_6488785021192928192[4] = 0; + out_6488785021192928192[5] = 0; + out_6488785021192928192[6] = 0; + out_6488785021192928192[7] = 0; + out_6488785021192928192[8] = 0; } -void h_31(double *state, double *unused, double *out_7817026797415165478) { - out_7817026797415165478[0] = state[8]; +void h_31(double *state, double *unused, double *out_5156479888167106809) { + out_5156479888167106809[0] = state[8]; } -void H_31(double *state, double *unused, double *out_2760199637348670825) { - out_2760199637348670825[0] = 0; - out_2760199637348670825[1] = 0; - out_2760199637348670825[2] = 0; - out_2760199637348670825[3] = 0; - out_2760199637348670825[4] = 0; - out_2760199637348670825[5] = 0; - out_2760199637348670825[6] = 0; - out_2760199637348670825[7] = 0; - out_2760199637348670825[8] = 1; +void H_31(double *state, double *unused, double *out_4404304345068078001) { + out_4404304345068078001[0] = 0; + out_4404304345068078001[1] = 0; + out_4404304345068078001[2] = 0; + out_4404304345068078001[3] = 0; + out_4404304345068078001[4] = 0; + out_4404304345068078001[5] = 0; + out_4404304345068078001[6] = 0; + out_4404304345068078001[7] = 0; + out_4404304345068078001[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_1626624007171975298) { - err_fun(nom_x, delta_x, out_1626624007171975298); +void car_err_fun(double *nom_x, double *delta_x, double *out_4189395199092511703) { + err_fun(nom_x, delta_x, out_4189395199092511703); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3841721694315715550) { - inv_err_fun(nom_x, true_x, out_3841721694315715550); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5755129993245429730) { + inv_err_fun(nom_x, true_x, out_5755129993245429730); } -void car_H_mod_fun(double *state, double *out_7646044064038577504) { - H_mod_fun(state, out_7646044064038577504); +void car_H_mod_fun(double *state, double *out_3130611196753066809) { + H_mod_fun(state, out_3130611196753066809); } -void car_f_fun(double *state, double dt, double *out_3692802059814366209) { - f_fun(state, dt, out_3692802059814366209); +void car_f_fun(double *state, double dt, double *out_5961640813003379683) { + f_fun(state, dt, out_5961640813003379683); } -void car_F_fun(double *state, double dt, double *out_6278869713612932388) { - F_fun(state, dt, out_6278869713612932388); +void car_F_fun(double *state, double dt, double *out_2654082016279909676) { + F_fun(state, dt, out_2654082016279909676); } -void car_h_25(double *state, double *unused, double *out_1903293770744417105) { - h_25(state, unused, out_1903293770744417105); +void car_h_25(double *state, double *unused, double *out_3340180793096633183) { + h_25(state, unused, out_3340180793096633183); } -void car_H_25(double *state, double *unused, double *out_1607511783758736875) { - H_25(state, unused, out_1607511783758736875); +void car_H_25(double *state, double *unused, double *out_4434950306945038429) { + H_25(state, unused, out_4434950306945038429); } -void car_h_24(double *state, double *unused, double *out_3019943109680816786) { - h_24(state, unused, out_3019943109680816786); +void car_h_24(double *state, double *unused, double *out_7854800018825826791) { + h_24(state, unused, out_7854800018825826791); } -void car_H_24(double *state, double *unused, double *out_565137815246762691) { - H_24(state, unused, out_565137815246762691); +void car_H_24(double *state, double *unused, double *out_6607599905950537995) { + H_24(state, unused, out_6607599905950537995); } -void car_h_30(double *state, double *unused, double *out_5540743298998439455) { - h_30(state, unused, out_5540743298998439455); +void car_h_30(double *state, double *unused, double *out_3104430337924860088) { + h_30(state, unused, out_3104430337924860088); } -void car_H_30(double *state, double *unused, double *out_4125844742265985502) { - H_30(state, unused, out_4125844742265985502); +void car_H_30(double *state, double *unused, double *out_1916617348437789802) { + H_30(state, unused, out_1916617348437789802); } -void car_h_26(double *state, double *unused, double *out_539070031337593810) { - h_26(state, unused, out_539070031337593810); +void car_h_26(double *state, double *unused, double *out_4544914835386474254) { + h_26(state, unused, out_4544914835386474254); } -void car_H_26(double *state, double *unused, double *out_2133991535115319349) { - H_26(state, unused, out_2133991535115319349); +void car_H_26(double *state, double *unused, double *out_8176453625819094653) { + H_26(state, unused, out_8176453625819094653); } -void car_h_27(double *state, double *unused, double *out_7078675401262423660) { - h_27(state, unused, out_7078675401262423660); +void car_h_27(double *state, double *unused, double *out_4426907441453097510) { + h_27(state, unused, out_4426907441453097510); } -void car_H_27(double *state, double *unused, double *out_1951081430465560591) { - H_27(state, unused, out_1951081430465560591); +void car_H_27(double *state, double *unused, double *out_306976722746153415) { + H_27(state, unused, out_306976722746153415); } -void car_h_29(double *state, double *unused, double *out_4597233446096777020) { - h_29(state, unused, out_4597233446096777020); +void car_h_29(double *state, double *unused, double *out_2160920485023197653) { + h_29(state, unused, out_2160920485023197653); } -void car_H_29(double *state, double *unused, double *out_4636076086580377686) { - H_29(state, unused, out_4636076086580377686); +void car_H_29(double *state, double *unused, double *out_1406386004123397618) { + H_29(state, unused, out_1406386004123397618); } -void car_h_28(double *state, double *unused, double *out_2077002133601578015) { - h_28(state, unused, out_2077002133601578015); +void car_h_28(double *state, double *unused, double *out_7017073928463388327) { + h_28(state, unused, out_7017073928463388327); } -void car_H_28(double *state, double *unused, double *out_446322930489152888) { - H_28(state, unused, out_446322930489152888); +void car_H_28(double *state, double *unused, double *out_6488785021192928192) { + H_28(state, unused, out_6488785021192928192); } -void car_h_31(double *state, double *unused, double *out_7817026797415165478) { - h_31(state, unused, out_7817026797415165478); +void car_h_31(double *state, double *unused, double *out_5156479888167106809) { + h_31(state, unused, out_5156479888167106809); } -void car_H_31(double *state, double *unused, double *out_2760199637348670825) { - H_31(state, unused, out_2760199637348670825); +void car_H_31(double *state, double *unused, double *out_4404304345068078001) { + H_31(state, unused, out_4404304345068078001); } 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 f3916ddaf..a1a172a78 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_1626624007171975298); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3841721694315715550); -void car_H_mod_fun(double *state, double *out_7646044064038577504); -void car_f_fun(double *state, double dt, double *out_3692802059814366209); -void car_F_fun(double *state, double dt, double *out_6278869713612932388); -void car_h_25(double *state, double *unused, double *out_1903293770744417105); -void car_H_25(double *state, double *unused, double *out_1607511783758736875); -void car_h_24(double *state, double *unused, double *out_3019943109680816786); -void car_H_24(double *state, double *unused, double *out_565137815246762691); -void car_h_30(double *state, double *unused, double *out_5540743298998439455); -void car_H_30(double *state, double *unused, double *out_4125844742265985502); -void car_h_26(double *state, double *unused, double *out_539070031337593810); -void car_H_26(double *state, double *unused, double *out_2133991535115319349); -void car_h_27(double *state, double *unused, double *out_7078675401262423660); -void car_H_27(double *state, double *unused, double *out_1951081430465560591); -void car_h_29(double *state, double *unused, double *out_4597233446096777020); -void car_H_29(double *state, double *unused, double *out_4636076086580377686); -void car_h_28(double *state, double *unused, double *out_2077002133601578015); -void car_H_28(double *state, double *unused, double *out_446322930489152888); -void car_h_31(double *state, double *unused, double *out_7817026797415165478); -void car_H_31(double *state, double *unused, double *out_2760199637348670825); +void car_err_fun(double *nom_x, double *delta_x, double *out_4189395199092511703); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5755129993245429730); +void car_H_mod_fun(double *state, double *out_3130611196753066809); +void car_f_fun(double *state, double dt, double *out_5961640813003379683); +void car_F_fun(double *state, double dt, double *out_2654082016279909676); +void car_h_25(double *state, double *unused, double *out_3340180793096633183); +void car_H_25(double *state, double *unused, double *out_4434950306945038429); +void car_h_24(double *state, double *unused, double *out_7854800018825826791); +void car_H_24(double *state, double *unused, double *out_6607599905950537995); +void car_h_30(double *state, double *unused, double *out_3104430337924860088); +void car_H_30(double *state, double *unused, double *out_1916617348437789802); +void car_h_26(double *state, double *unused, double *out_4544914835386474254); +void car_H_26(double *state, double *unused, double *out_8176453625819094653); +void car_h_27(double *state, double *unused, double *out_4426907441453097510); +void car_H_27(double *state, double *unused, double *out_306976722746153415); +void car_h_29(double *state, double *unused, double *out_2160920485023197653); +void car_H_29(double *state, double *unused, double *out_1406386004123397618); +void car_h_28(double *state, double *unused, double *out_7017073928463388327); +void car_H_28(double *state, double *unused, double *out_6488785021192928192); +void car_h_31(double *state, double *unused, double *out_5156479888167106809); +void car_H_31(double *state, double *unused, double *out_4404304345068078001); 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 7e8794158..c7301555e 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_1277882425838003162) { - out_1277882425838003162[0] = delta_x[0] + nom_x[0]; - out_1277882425838003162[1] = delta_x[1] + nom_x[1]; - out_1277882425838003162[2] = delta_x[2] + nom_x[2]; - out_1277882425838003162[3] = delta_x[3] + nom_x[3]; - out_1277882425838003162[4] = delta_x[4] + nom_x[4]; - out_1277882425838003162[5] = delta_x[5] + nom_x[5]; - out_1277882425838003162[6] = delta_x[6] + nom_x[6]; - out_1277882425838003162[7] = delta_x[7] + nom_x[7]; - out_1277882425838003162[8] = delta_x[8] + nom_x[8]; - out_1277882425838003162[9] = delta_x[9] + nom_x[9]; - out_1277882425838003162[10] = delta_x[10] + nom_x[10]; - out_1277882425838003162[11] = delta_x[11] + nom_x[11]; - out_1277882425838003162[12] = delta_x[12] + nom_x[12]; - out_1277882425838003162[13] = delta_x[13] + nom_x[13]; - out_1277882425838003162[14] = delta_x[14] + nom_x[14]; - out_1277882425838003162[15] = delta_x[15] + nom_x[15]; - out_1277882425838003162[16] = delta_x[16] + nom_x[16]; - out_1277882425838003162[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_842964569186968919) { + out_842964569186968919[0] = delta_x[0] + nom_x[0]; + out_842964569186968919[1] = delta_x[1] + nom_x[1]; + out_842964569186968919[2] = delta_x[2] + nom_x[2]; + out_842964569186968919[3] = delta_x[3] + nom_x[3]; + out_842964569186968919[4] = delta_x[4] + nom_x[4]; + out_842964569186968919[5] = delta_x[5] + nom_x[5]; + out_842964569186968919[6] = delta_x[6] + nom_x[6]; + out_842964569186968919[7] = delta_x[7] + nom_x[7]; + out_842964569186968919[8] = delta_x[8] + nom_x[8]; + out_842964569186968919[9] = delta_x[9] + nom_x[9]; + out_842964569186968919[10] = delta_x[10] + nom_x[10]; + out_842964569186968919[11] = delta_x[11] + nom_x[11]; + out_842964569186968919[12] = delta_x[12] + nom_x[12]; + out_842964569186968919[13] = delta_x[13] + nom_x[13]; + out_842964569186968919[14] = delta_x[14] + nom_x[14]; + out_842964569186968919[15] = delta_x[15] + nom_x[15]; + out_842964569186968919[16] = delta_x[16] + nom_x[16]; + out_842964569186968919[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5395183656796766) { - out_5395183656796766[0] = -nom_x[0] + true_x[0]; - out_5395183656796766[1] = -nom_x[1] + true_x[1]; - out_5395183656796766[2] = -nom_x[2] + true_x[2]; - out_5395183656796766[3] = -nom_x[3] + true_x[3]; - out_5395183656796766[4] = -nom_x[4] + true_x[4]; - out_5395183656796766[5] = -nom_x[5] + true_x[5]; - out_5395183656796766[6] = -nom_x[6] + true_x[6]; - out_5395183656796766[7] = -nom_x[7] + true_x[7]; - out_5395183656796766[8] = -nom_x[8] + true_x[8]; - out_5395183656796766[9] = -nom_x[9] + true_x[9]; - out_5395183656796766[10] = -nom_x[10] + true_x[10]; - out_5395183656796766[11] = -nom_x[11] + true_x[11]; - out_5395183656796766[12] = -nom_x[12] + true_x[12]; - out_5395183656796766[13] = -nom_x[13] + true_x[13]; - out_5395183656796766[14] = -nom_x[14] + true_x[14]; - out_5395183656796766[15] = -nom_x[15] + true_x[15]; - out_5395183656796766[16] = -nom_x[16] + true_x[16]; - out_5395183656796766[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_7637995249051351300) { + out_7637995249051351300[0] = -nom_x[0] + true_x[0]; + out_7637995249051351300[1] = -nom_x[1] + true_x[1]; + out_7637995249051351300[2] = -nom_x[2] + true_x[2]; + out_7637995249051351300[3] = -nom_x[3] + true_x[3]; + out_7637995249051351300[4] = -nom_x[4] + true_x[4]; + out_7637995249051351300[5] = -nom_x[5] + true_x[5]; + out_7637995249051351300[6] = -nom_x[6] + true_x[6]; + out_7637995249051351300[7] = -nom_x[7] + true_x[7]; + out_7637995249051351300[8] = -nom_x[8] + true_x[8]; + out_7637995249051351300[9] = -nom_x[9] + true_x[9]; + out_7637995249051351300[10] = -nom_x[10] + true_x[10]; + out_7637995249051351300[11] = -nom_x[11] + true_x[11]; + out_7637995249051351300[12] = -nom_x[12] + true_x[12]; + out_7637995249051351300[13] = -nom_x[13] + true_x[13]; + out_7637995249051351300[14] = -nom_x[14] + true_x[14]; + out_7637995249051351300[15] = -nom_x[15] + true_x[15]; + out_7637995249051351300[16] = -nom_x[16] + true_x[16]; + out_7637995249051351300[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_4641299731180659481) { - out_4641299731180659481[0] = 1.0; - out_4641299731180659481[1] = 0.0; - out_4641299731180659481[2] = 0.0; - out_4641299731180659481[3] = 0.0; - out_4641299731180659481[4] = 0.0; - out_4641299731180659481[5] = 0.0; - out_4641299731180659481[6] = 0.0; - out_4641299731180659481[7] = 0.0; - out_4641299731180659481[8] = 0.0; - out_4641299731180659481[9] = 0.0; - out_4641299731180659481[10] = 0.0; - out_4641299731180659481[11] = 0.0; - out_4641299731180659481[12] = 0.0; - out_4641299731180659481[13] = 0.0; - out_4641299731180659481[14] = 0.0; - out_4641299731180659481[15] = 0.0; - out_4641299731180659481[16] = 0.0; - out_4641299731180659481[17] = 0.0; - out_4641299731180659481[18] = 0.0; - out_4641299731180659481[19] = 1.0; - out_4641299731180659481[20] = 0.0; - out_4641299731180659481[21] = 0.0; - out_4641299731180659481[22] = 0.0; - out_4641299731180659481[23] = 0.0; - out_4641299731180659481[24] = 0.0; - out_4641299731180659481[25] = 0.0; - out_4641299731180659481[26] = 0.0; - out_4641299731180659481[27] = 0.0; - out_4641299731180659481[28] = 0.0; - out_4641299731180659481[29] = 0.0; - out_4641299731180659481[30] = 0.0; - out_4641299731180659481[31] = 0.0; - out_4641299731180659481[32] = 0.0; - out_4641299731180659481[33] = 0.0; - out_4641299731180659481[34] = 0.0; - out_4641299731180659481[35] = 0.0; - out_4641299731180659481[36] = 0.0; - out_4641299731180659481[37] = 0.0; - out_4641299731180659481[38] = 1.0; - out_4641299731180659481[39] = 0.0; - out_4641299731180659481[40] = 0.0; - out_4641299731180659481[41] = 0.0; - out_4641299731180659481[42] = 0.0; - out_4641299731180659481[43] = 0.0; - out_4641299731180659481[44] = 0.0; - out_4641299731180659481[45] = 0.0; - out_4641299731180659481[46] = 0.0; - out_4641299731180659481[47] = 0.0; - out_4641299731180659481[48] = 0.0; - out_4641299731180659481[49] = 0.0; - out_4641299731180659481[50] = 0.0; - out_4641299731180659481[51] = 0.0; - out_4641299731180659481[52] = 0.0; - out_4641299731180659481[53] = 0.0; - out_4641299731180659481[54] = 0.0; - out_4641299731180659481[55] = 0.0; - out_4641299731180659481[56] = 0.0; - out_4641299731180659481[57] = 1.0; - out_4641299731180659481[58] = 0.0; - out_4641299731180659481[59] = 0.0; - out_4641299731180659481[60] = 0.0; - out_4641299731180659481[61] = 0.0; - out_4641299731180659481[62] = 0.0; - out_4641299731180659481[63] = 0.0; - out_4641299731180659481[64] = 0.0; - out_4641299731180659481[65] = 0.0; - out_4641299731180659481[66] = 0.0; - out_4641299731180659481[67] = 0.0; - out_4641299731180659481[68] = 0.0; - out_4641299731180659481[69] = 0.0; - out_4641299731180659481[70] = 0.0; - out_4641299731180659481[71] = 0.0; - out_4641299731180659481[72] = 0.0; - out_4641299731180659481[73] = 0.0; - out_4641299731180659481[74] = 0.0; - out_4641299731180659481[75] = 0.0; - out_4641299731180659481[76] = 1.0; - out_4641299731180659481[77] = 0.0; - out_4641299731180659481[78] = 0.0; - out_4641299731180659481[79] = 0.0; - out_4641299731180659481[80] = 0.0; - out_4641299731180659481[81] = 0.0; - out_4641299731180659481[82] = 0.0; - out_4641299731180659481[83] = 0.0; - out_4641299731180659481[84] = 0.0; - out_4641299731180659481[85] = 0.0; - out_4641299731180659481[86] = 0.0; - out_4641299731180659481[87] = 0.0; - out_4641299731180659481[88] = 0.0; - out_4641299731180659481[89] = 0.0; - out_4641299731180659481[90] = 0.0; - out_4641299731180659481[91] = 0.0; - out_4641299731180659481[92] = 0.0; - out_4641299731180659481[93] = 0.0; - out_4641299731180659481[94] = 0.0; - out_4641299731180659481[95] = 1.0; - out_4641299731180659481[96] = 0.0; - out_4641299731180659481[97] = 0.0; - out_4641299731180659481[98] = 0.0; - out_4641299731180659481[99] = 0.0; - out_4641299731180659481[100] = 0.0; - out_4641299731180659481[101] = 0.0; - out_4641299731180659481[102] = 0.0; - out_4641299731180659481[103] = 0.0; - out_4641299731180659481[104] = 0.0; - out_4641299731180659481[105] = 0.0; - out_4641299731180659481[106] = 0.0; - out_4641299731180659481[107] = 0.0; - out_4641299731180659481[108] = 0.0; - out_4641299731180659481[109] = 0.0; - out_4641299731180659481[110] = 0.0; - out_4641299731180659481[111] = 0.0; - out_4641299731180659481[112] = 0.0; - out_4641299731180659481[113] = 0.0; - out_4641299731180659481[114] = 1.0; - out_4641299731180659481[115] = 0.0; - out_4641299731180659481[116] = 0.0; - out_4641299731180659481[117] = 0.0; - out_4641299731180659481[118] = 0.0; - out_4641299731180659481[119] = 0.0; - out_4641299731180659481[120] = 0.0; - out_4641299731180659481[121] = 0.0; - out_4641299731180659481[122] = 0.0; - out_4641299731180659481[123] = 0.0; - out_4641299731180659481[124] = 0.0; - out_4641299731180659481[125] = 0.0; - out_4641299731180659481[126] = 0.0; - out_4641299731180659481[127] = 0.0; - out_4641299731180659481[128] = 0.0; - out_4641299731180659481[129] = 0.0; - out_4641299731180659481[130] = 0.0; - out_4641299731180659481[131] = 0.0; - out_4641299731180659481[132] = 0.0; - out_4641299731180659481[133] = 1.0; - out_4641299731180659481[134] = 0.0; - out_4641299731180659481[135] = 0.0; - out_4641299731180659481[136] = 0.0; - out_4641299731180659481[137] = 0.0; - out_4641299731180659481[138] = 0.0; - out_4641299731180659481[139] = 0.0; - out_4641299731180659481[140] = 0.0; - out_4641299731180659481[141] = 0.0; - out_4641299731180659481[142] = 0.0; - out_4641299731180659481[143] = 0.0; - out_4641299731180659481[144] = 0.0; - out_4641299731180659481[145] = 0.0; - out_4641299731180659481[146] = 0.0; - out_4641299731180659481[147] = 0.0; - out_4641299731180659481[148] = 0.0; - out_4641299731180659481[149] = 0.0; - out_4641299731180659481[150] = 0.0; - out_4641299731180659481[151] = 0.0; - out_4641299731180659481[152] = 1.0; - out_4641299731180659481[153] = 0.0; - out_4641299731180659481[154] = 0.0; - out_4641299731180659481[155] = 0.0; - out_4641299731180659481[156] = 0.0; - out_4641299731180659481[157] = 0.0; - out_4641299731180659481[158] = 0.0; - out_4641299731180659481[159] = 0.0; - out_4641299731180659481[160] = 0.0; - out_4641299731180659481[161] = 0.0; - out_4641299731180659481[162] = 0.0; - out_4641299731180659481[163] = 0.0; - out_4641299731180659481[164] = 0.0; - out_4641299731180659481[165] = 0.0; - out_4641299731180659481[166] = 0.0; - out_4641299731180659481[167] = 0.0; - out_4641299731180659481[168] = 0.0; - out_4641299731180659481[169] = 0.0; - out_4641299731180659481[170] = 0.0; - out_4641299731180659481[171] = 1.0; - out_4641299731180659481[172] = 0.0; - out_4641299731180659481[173] = 0.0; - out_4641299731180659481[174] = 0.0; - out_4641299731180659481[175] = 0.0; - out_4641299731180659481[176] = 0.0; - out_4641299731180659481[177] = 0.0; - out_4641299731180659481[178] = 0.0; - out_4641299731180659481[179] = 0.0; - out_4641299731180659481[180] = 0.0; - out_4641299731180659481[181] = 0.0; - out_4641299731180659481[182] = 0.0; - out_4641299731180659481[183] = 0.0; - out_4641299731180659481[184] = 0.0; - out_4641299731180659481[185] = 0.0; - out_4641299731180659481[186] = 0.0; - out_4641299731180659481[187] = 0.0; - out_4641299731180659481[188] = 0.0; - out_4641299731180659481[189] = 0.0; - out_4641299731180659481[190] = 1.0; - out_4641299731180659481[191] = 0.0; - out_4641299731180659481[192] = 0.0; - out_4641299731180659481[193] = 0.0; - out_4641299731180659481[194] = 0.0; - out_4641299731180659481[195] = 0.0; - out_4641299731180659481[196] = 0.0; - out_4641299731180659481[197] = 0.0; - out_4641299731180659481[198] = 0.0; - out_4641299731180659481[199] = 0.0; - out_4641299731180659481[200] = 0.0; - out_4641299731180659481[201] = 0.0; - out_4641299731180659481[202] = 0.0; - out_4641299731180659481[203] = 0.0; - out_4641299731180659481[204] = 0.0; - out_4641299731180659481[205] = 0.0; - out_4641299731180659481[206] = 0.0; - out_4641299731180659481[207] = 0.0; - out_4641299731180659481[208] = 0.0; - out_4641299731180659481[209] = 1.0; - out_4641299731180659481[210] = 0.0; - out_4641299731180659481[211] = 0.0; - out_4641299731180659481[212] = 0.0; - out_4641299731180659481[213] = 0.0; - out_4641299731180659481[214] = 0.0; - out_4641299731180659481[215] = 0.0; - out_4641299731180659481[216] = 0.0; - out_4641299731180659481[217] = 0.0; - out_4641299731180659481[218] = 0.0; - out_4641299731180659481[219] = 0.0; - out_4641299731180659481[220] = 0.0; - out_4641299731180659481[221] = 0.0; - out_4641299731180659481[222] = 0.0; - out_4641299731180659481[223] = 0.0; - out_4641299731180659481[224] = 0.0; - out_4641299731180659481[225] = 0.0; - out_4641299731180659481[226] = 0.0; - out_4641299731180659481[227] = 0.0; - out_4641299731180659481[228] = 1.0; - out_4641299731180659481[229] = 0.0; - out_4641299731180659481[230] = 0.0; - out_4641299731180659481[231] = 0.0; - out_4641299731180659481[232] = 0.0; - out_4641299731180659481[233] = 0.0; - out_4641299731180659481[234] = 0.0; - out_4641299731180659481[235] = 0.0; - out_4641299731180659481[236] = 0.0; - out_4641299731180659481[237] = 0.0; - out_4641299731180659481[238] = 0.0; - out_4641299731180659481[239] = 0.0; - out_4641299731180659481[240] = 0.0; - out_4641299731180659481[241] = 0.0; - out_4641299731180659481[242] = 0.0; - out_4641299731180659481[243] = 0.0; - out_4641299731180659481[244] = 0.0; - out_4641299731180659481[245] = 0.0; - out_4641299731180659481[246] = 0.0; - out_4641299731180659481[247] = 1.0; - out_4641299731180659481[248] = 0.0; - out_4641299731180659481[249] = 0.0; - out_4641299731180659481[250] = 0.0; - out_4641299731180659481[251] = 0.0; - out_4641299731180659481[252] = 0.0; - out_4641299731180659481[253] = 0.0; - out_4641299731180659481[254] = 0.0; - out_4641299731180659481[255] = 0.0; - out_4641299731180659481[256] = 0.0; - out_4641299731180659481[257] = 0.0; - out_4641299731180659481[258] = 0.0; - out_4641299731180659481[259] = 0.0; - out_4641299731180659481[260] = 0.0; - out_4641299731180659481[261] = 0.0; - out_4641299731180659481[262] = 0.0; - out_4641299731180659481[263] = 0.0; - out_4641299731180659481[264] = 0.0; - out_4641299731180659481[265] = 0.0; - out_4641299731180659481[266] = 1.0; - out_4641299731180659481[267] = 0.0; - out_4641299731180659481[268] = 0.0; - out_4641299731180659481[269] = 0.0; - out_4641299731180659481[270] = 0.0; - out_4641299731180659481[271] = 0.0; - out_4641299731180659481[272] = 0.0; - out_4641299731180659481[273] = 0.0; - out_4641299731180659481[274] = 0.0; - out_4641299731180659481[275] = 0.0; - out_4641299731180659481[276] = 0.0; - out_4641299731180659481[277] = 0.0; - out_4641299731180659481[278] = 0.0; - out_4641299731180659481[279] = 0.0; - out_4641299731180659481[280] = 0.0; - out_4641299731180659481[281] = 0.0; - out_4641299731180659481[282] = 0.0; - out_4641299731180659481[283] = 0.0; - out_4641299731180659481[284] = 0.0; - out_4641299731180659481[285] = 1.0; - out_4641299731180659481[286] = 0.0; - out_4641299731180659481[287] = 0.0; - out_4641299731180659481[288] = 0.0; - out_4641299731180659481[289] = 0.0; - out_4641299731180659481[290] = 0.0; - out_4641299731180659481[291] = 0.0; - out_4641299731180659481[292] = 0.0; - out_4641299731180659481[293] = 0.0; - out_4641299731180659481[294] = 0.0; - out_4641299731180659481[295] = 0.0; - out_4641299731180659481[296] = 0.0; - out_4641299731180659481[297] = 0.0; - out_4641299731180659481[298] = 0.0; - out_4641299731180659481[299] = 0.0; - out_4641299731180659481[300] = 0.0; - out_4641299731180659481[301] = 0.0; - out_4641299731180659481[302] = 0.0; - out_4641299731180659481[303] = 0.0; - out_4641299731180659481[304] = 1.0; - out_4641299731180659481[305] = 0.0; - out_4641299731180659481[306] = 0.0; - out_4641299731180659481[307] = 0.0; - out_4641299731180659481[308] = 0.0; - out_4641299731180659481[309] = 0.0; - out_4641299731180659481[310] = 0.0; - out_4641299731180659481[311] = 0.0; - out_4641299731180659481[312] = 0.0; - out_4641299731180659481[313] = 0.0; - out_4641299731180659481[314] = 0.0; - out_4641299731180659481[315] = 0.0; - out_4641299731180659481[316] = 0.0; - out_4641299731180659481[317] = 0.0; - out_4641299731180659481[318] = 0.0; - out_4641299731180659481[319] = 0.0; - out_4641299731180659481[320] = 0.0; - out_4641299731180659481[321] = 0.0; - out_4641299731180659481[322] = 0.0; - out_4641299731180659481[323] = 1.0; +void H_mod_fun(double *state, double *out_8306889626903079841) { + out_8306889626903079841[0] = 1.0; + out_8306889626903079841[1] = 0.0; + out_8306889626903079841[2] = 0.0; + out_8306889626903079841[3] = 0.0; + out_8306889626903079841[4] = 0.0; + out_8306889626903079841[5] = 0.0; + out_8306889626903079841[6] = 0.0; + out_8306889626903079841[7] = 0.0; + out_8306889626903079841[8] = 0.0; + out_8306889626903079841[9] = 0.0; + out_8306889626903079841[10] = 0.0; + out_8306889626903079841[11] = 0.0; + out_8306889626903079841[12] = 0.0; + out_8306889626903079841[13] = 0.0; + out_8306889626903079841[14] = 0.0; + out_8306889626903079841[15] = 0.0; + out_8306889626903079841[16] = 0.0; + out_8306889626903079841[17] = 0.0; + out_8306889626903079841[18] = 0.0; + out_8306889626903079841[19] = 1.0; + out_8306889626903079841[20] = 0.0; + out_8306889626903079841[21] = 0.0; + out_8306889626903079841[22] = 0.0; + out_8306889626903079841[23] = 0.0; + out_8306889626903079841[24] = 0.0; + out_8306889626903079841[25] = 0.0; + out_8306889626903079841[26] = 0.0; + out_8306889626903079841[27] = 0.0; + out_8306889626903079841[28] = 0.0; + out_8306889626903079841[29] = 0.0; + out_8306889626903079841[30] = 0.0; + out_8306889626903079841[31] = 0.0; + out_8306889626903079841[32] = 0.0; + out_8306889626903079841[33] = 0.0; + out_8306889626903079841[34] = 0.0; + out_8306889626903079841[35] = 0.0; + out_8306889626903079841[36] = 0.0; + out_8306889626903079841[37] = 0.0; + out_8306889626903079841[38] = 1.0; + out_8306889626903079841[39] = 0.0; + out_8306889626903079841[40] = 0.0; + out_8306889626903079841[41] = 0.0; + out_8306889626903079841[42] = 0.0; + out_8306889626903079841[43] = 0.0; + out_8306889626903079841[44] = 0.0; + out_8306889626903079841[45] = 0.0; + out_8306889626903079841[46] = 0.0; + out_8306889626903079841[47] = 0.0; + out_8306889626903079841[48] = 0.0; + out_8306889626903079841[49] = 0.0; + out_8306889626903079841[50] = 0.0; + out_8306889626903079841[51] = 0.0; + out_8306889626903079841[52] = 0.0; + out_8306889626903079841[53] = 0.0; + out_8306889626903079841[54] = 0.0; + out_8306889626903079841[55] = 0.0; + out_8306889626903079841[56] = 0.0; + out_8306889626903079841[57] = 1.0; + out_8306889626903079841[58] = 0.0; + out_8306889626903079841[59] = 0.0; + out_8306889626903079841[60] = 0.0; + out_8306889626903079841[61] = 0.0; + out_8306889626903079841[62] = 0.0; + out_8306889626903079841[63] = 0.0; + out_8306889626903079841[64] = 0.0; + out_8306889626903079841[65] = 0.0; + out_8306889626903079841[66] = 0.0; + out_8306889626903079841[67] = 0.0; + out_8306889626903079841[68] = 0.0; + out_8306889626903079841[69] = 0.0; + out_8306889626903079841[70] = 0.0; + out_8306889626903079841[71] = 0.0; + out_8306889626903079841[72] = 0.0; + out_8306889626903079841[73] = 0.0; + out_8306889626903079841[74] = 0.0; + out_8306889626903079841[75] = 0.0; + out_8306889626903079841[76] = 1.0; + out_8306889626903079841[77] = 0.0; + out_8306889626903079841[78] = 0.0; + out_8306889626903079841[79] = 0.0; + out_8306889626903079841[80] = 0.0; + out_8306889626903079841[81] = 0.0; + out_8306889626903079841[82] = 0.0; + out_8306889626903079841[83] = 0.0; + out_8306889626903079841[84] = 0.0; + out_8306889626903079841[85] = 0.0; + out_8306889626903079841[86] = 0.0; + out_8306889626903079841[87] = 0.0; + out_8306889626903079841[88] = 0.0; + out_8306889626903079841[89] = 0.0; + out_8306889626903079841[90] = 0.0; + out_8306889626903079841[91] = 0.0; + out_8306889626903079841[92] = 0.0; + out_8306889626903079841[93] = 0.0; + out_8306889626903079841[94] = 0.0; + out_8306889626903079841[95] = 1.0; + out_8306889626903079841[96] = 0.0; + out_8306889626903079841[97] = 0.0; + out_8306889626903079841[98] = 0.0; + out_8306889626903079841[99] = 0.0; + out_8306889626903079841[100] = 0.0; + out_8306889626903079841[101] = 0.0; + out_8306889626903079841[102] = 0.0; + out_8306889626903079841[103] = 0.0; + out_8306889626903079841[104] = 0.0; + out_8306889626903079841[105] = 0.0; + out_8306889626903079841[106] = 0.0; + out_8306889626903079841[107] = 0.0; + out_8306889626903079841[108] = 0.0; + out_8306889626903079841[109] = 0.0; + out_8306889626903079841[110] = 0.0; + out_8306889626903079841[111] = 0.0; + out_8306889626903079841[112] = 0.0; + out_8306889626903079841[113] = 0.0; + out_8306889626903079841[114] = 1.0; + out_8306889626903079841[115] = 0.0; + out_8306889626903079841[116] = 0.0; + out_8306889626903079841[117] = 0.0; + out_8306889626903079841[118] = 0.0; + out_8306889626903079841[119] = 0.0; + out_8306889626903079841[120] = 0.0; + out_8306889626903079841[121] = 0.0; + out_8306889626903079841[122] = 0.0; + out_8306889626903079841[123] = 0.0; + out_8306889626903079841[124] = 0.0; + out_8306889626903079841[125] = 0.0; + out_8306889626903079841[126] = 0.0; + out_8306889626903079841[127] = 0.0; + out_8306889626903079841[128] = 0.0; + out_8306889626903079841[129] = 0.0; + out_8306889626903079841[130] = 0.0; + out_8306889626903079841[131] = 0.0; + out_8306889626903079841[132] = 0.0; + out_8306889626903079841[133] = 1.0; + out_8306889626903079841[134] = 0.0; + out_8306889626903079841[135] = 0.0; + out_8306889626903079841[136] = 0.0; + out_8306889626903079841[137] = 0.0; + out_8306889626903079841[138] = 0.0; + out_8306889626903079841[139] = 0.0; + out_8306889626903079841[140] = 0.0; + out_8306889626903079841[141] = 0.0; + out_8306889626903079841[142] = 0.0; + out_8306889626903079841[143] = 0.0; + out_8306889626903079841[144] = 0.0; + out_8306889626903079841[145] = 0.0; + out_8306889626903079841[146] = 0.0; + out_8306889626903079841[147] = 0.0; + out_8306889626903079841[148] = 0.0; + out_8306889626903079841[149] = 0.0; + out_8306889626903079841[150] = 0.0; + out_8306889626903079841[151] = 0.0; + out_8306889626903079841[152] = 1.0; + out_8306889626903079841[153] = 0.0; + out_8306889626903079841[154] = 0.0; + out_8306889626903079841[155] = 0.0; + out_8306889626903079841[156] = 0.0; + out_8306889626903079841[157] = 0.0; + out_8306889626903079841[158] = 0.0; + out_8306889626903079841[159] = 0.0; + out_8306889626903079841[160] = 0.0; + out_8306889626903079841[161] = 0.0; + out_8306889626903079841[162] = 0.0; + out_8306889626903079841[163] = 0.0; + out_8306889626903079841[164] = 0.0; + out_8306889626903079841[165] = 0.0; + out_8306889626903079841[166] = 0.0; + out_8306889626903079841[167] = 0.0; + out_8306889626903079841[168] = 0.0; + out_8306889626903079841[169] = 0.0; + out_8306889626903079841[170] = 0.0; + out_8306889626903079841[171] = 1.0; + out_8306889626903079841[172] = 0.0; + out_8306889626903079841[173] = 0.0; + out_8306889626903079841[174] = 0.0; + out_8306889626903079841[175] = 0.0; + out_8306889626903079841[176] = 0.0; + out_8306889626903079841[177] = 0.0; + out_8306889626903079841[178] = 0.0; + out_8306889626903079841[179] = 0.0; + out_8306889626903079841[180] = 0.0; + out_8306889626903079841[181] = 0.0; + out_8306889626903079841[182] = 0.0; + out_8306889626903079841[183] = 0.0; + out_8306889626903079841[184] = 0.0; + out_8306889626903079841[185] = 0.0; + out_8306889626903079841[186] = 0.0; + out_8306889626903079841[187] = 0.0; + out_8306889626903079841[188] = 0.0; + out_8306889626903079841[189] = 0.0; + out_8306889626903079841[190] = 1.0; + out_8306889626903079841[191] = 0.0; + out_8306889626903079841[192] = 0.0; + out_8306889626903079841[193] = 0.0; + out_8306889626903079841[194] = 0.0; + out_8306889626903079841[195] = 0.0; + out_8306889626903079841[196] = 0.0; + out_8306889626903079841[197] = 0.0; + out_8306889626903079841[198] = 0.0; + out_8306889626903079841[199] = 0.0; + out_8306889626903079841[200] = 0.0; + out_8306889626903079841[201] = 0.0; + out_8306889626903079841[202] = 0.0; + out_8306889626903079841[203] = 0.0; + out_8306889626903079841[204] = 0.0; + out_8306889626903079841[205] = 0.0; + out_8306889626903079841[206] = 0.0; + out_8306889626903079841[207] = 0.0; + out_8306889626903079841[208] = 0.0; + out_8306889626903079841[209] = 1.0; + out_8306889626903079841[210] = 0.0; + out_8306889626903079841[211] = 0.0; + out_8306889626903079841[212] = 0.0; + out_8306889626903079841[213] = 0.0; + out_8306889626903079841[214] = 0.0; + out_8306889626903079841[215] = 0.0; + out_8306889626903079841[216] = 0.0; + out_8306889626903079841[217] = 0.0; + out_8306889626903079841[218] = 0.0; + out_8306889626903079841[219] = 0.0; + out_8306889626903079841[220] = 0.0; + out_8306889626903079841[221] = 0.0; + out_8306889626903079841[222] = 0.0; + out_8306889626903079841[223] = 0.0; + out_8306889626903079841[224] = 0.0; + out_8306889626903079841[225] = 0.0; + out_8306889626903079841[226] = 0.0; + out_8306889626903079841[227] = 0.0; + out_8306889626903079841[228] = 1.0; + out_8306889626903079841[229] = 0.0; + out_8306889626903079841[230] = 0.0; + out_8306889626903079841[231] = 0.0; + out_8306889626903079841[232] = 0.0; + out_8306889626903079841[233] = 0.0; + out_8306889626903079841[234] = 0.0; + out_8306889626903079841[235] = 0.0; + out_8306889626903079841[236] = 0.0; + out_8306889626903079841[237] = 0.0; + out_8306889626903079841[238] = 0.0; + out_8306889626903079841[239] = 0.0; + out_8306889626903079841[240] = 0.0; + out_8306889626903079841[241] = 0.0; + out_8306889626903079841[242] = 0.0; + out_8306889626903079841[243] = 0.0; + out_8306889626903079841[244] = 0.0; + out_8306889626903079841[245] = 0.0; + out_8306889626903079841[246] = 0.0; + out_8306889626903079841[247] = 1.0; + out_8306889626903079841[248] = 0.0; + out_8306889626903079841[249] = 0.0; + out_8306889626903079841[250] = 0.0; + out_8306889626903079841[251] = 0.0; + out_8306889626903079841[252] = 0.0; + out_8306889626903079841[253] = 0.0; + out_8306889626903079841[254] = 0.0; + out_8306889626903079841[255] = 0.0; + out_8306889626903079841[256] = 0.0; + out_8306889626903079841[257] = 0.0; + out_8306889626903079841[258] = 0.0; + out_8306889626903079841[259] = 0.0; + out_8306889626903079841[260] = 0.0; + out_8306889626903079841[261] = 0.0; + out_8306889626903079841[262] = 0.0; + out_8306889626903079841[263] = 0.0; + out_8306889626903079841[264] = 0.0; + out_8306889626903079841[265] = 0.0; + out_8306889626903079841[266] = 1.0; + out_8306889626903079841[267] = 0.0; + out_8306889626903079841[268] = 0.0; + out_8306889626903079841[269] = 0.0; + out_8306889626903079841[270] = 0.0; + out_8306889626903079841[271] = 0.0; + out_8306889626903079841[272] = 0.0; + out_8306889626903079841[273] = 0.0; + out_8306889626903079841[274] = 0.0; + out_8306889626903079841[275] = 0.0; + out_8306889626903079841[276] = 0.0; + out_8306889626903079841[277] = 0.0; + out_8306889626903079841[278] = 0.0; + out_8306889626903079841[279] = 0.0; + out_8306889626903079841[280] = 0.0; + out_8306889626903079841[281] = 0.0; + out_8306889626903079841[282] = 0.0; + out_8306889626903079841[283] = 0.0; + out_8306889626903079841[284] = 0.0; + out_8306889626903079841[285] = 1.0; + out_8306889626903079841[286] = 0.0; + out_8306889626903079841[287] = 0.0; + out_8306889626903079841[288] = 0.0; + out_8306889626903079841[289] = 0.0; + out_8306889626903079841[290] = 0.0; + out_8306889626903079841[291] = 0.0; + out_8306889626903079841[292] = 0.0; + out_8306889626903079841[293] = 0.0; + out_8306889626903079841[294] = 0.0; + out_8306889626903079841[295] = 0.0; + out_8306889626903079841[296] = 0.0; + out_8306889626903079841[297] = 0.0; + out_8306889626903079841[298] = 0.0; + out_8306889626903079841[299] = 0.0; + out_8306889626903079841[300] = 0.0; + out_8306889626903079841[301] = 0.0; + out_8306889626903079841[302] = 0.0; + out_8306889626903079841[303] = 0.0; + out_8306889626903079841[304] = 1.0; + out_8306889626903079841[305] = 0.0; + out_8306889626903079841[306] = 0.0; + out_8306889626903079841[307] = 0.0; + out_8306889626903079841[308] = 0.0; + out_8306889626903079841[309] = 0.0; + out_8306889626903079841[310] = 0.0; + out_8306889626903079841[311] = 0.0; + out_8306889626903079841[312] = 0.0; + out_8306889626903079841[313] = 0.0; + out_8306889626903079841[314] = 0.0; + out_8306889626903079841[315] = 0.0; + out_8306889626903079841[316] = 0.0; + out_8306889626903079841[317] = 0.0; + out_8306889626903079841[318] = 0.0; + out_8306889626903079841[319] = 0.0; + out_8306889626903079841[320] = 0.0; + out_8306889626903079841[321] = 0.0; + out_8306889626903079841[322] = 0.0; + out_8306889626903079841[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4475655607956648045) { - out_4475655607956648045[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_4475655607956648045[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_4475655607956648045[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_4475655607956648045[3] = dt*state[12] + state[3]; - out_4475655607956648045[4] = dt*state[13] + state[4]; - out_4475655607956648045[5] = dt*state[14] + state[5]; - out_4475655607956648045[6] = state[6]; - out_4475655607956648045[7] = state[7]; - out_4475655607956648045[8] = state[8]; - out_4475655607956648045[9] = state[9]; - out_4475655607956648045[10] = state[10]; - out_4475655607956648045[11] = state[11]; - out_4475655607956648045[12] = state[12]; - out_4475655607956648045[13] = state[13]; - out_4475655607956648045[14] = state[14]; - out_4475655607956648045[15] = state[15]; - out_4475655607956648045[16] = state[16]; - out_4475655607956648045[17] = state[17]; +void f_fun(double *state, double dt, double *out_5013225008344736149) { + out_5013225008344736149[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_5013225008344736149[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_5013225008344736149[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_5013225008344736149[3] = dt*state[12] + state[3]; + out_5013225008344736149[4] = dt*state[13] + state[4]; + out_5013225008344736149[5] = dt*state[14] + state[5]; + out_5013225008344736149[6] = state[6]; + out_5013225008344736149[7] = state[7]; + out_5013225008344736149[8] = state[8]; + out_5013225008344736149[9] = state[9]; + out_5013225008344736149[10] = state[10]; + out_5013225008344736149[11] = state[11]; + out_5013225008344736149[12] = state[12]; + out_5013225008344736149[13] = state[13]; + out_5013225008344736149[14] = state[14]; + out_5013225008344736149[15] = state[15]; + out_5013225008344736149[16] = state[16]; + out_5013225008344736149[17] = state[17]; } -void F_fun(double *state, double dt, double *out_1490364464464661376) { - out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[2] = 0; - out_1490364464464661376[3] = 0; - out_1490364464464661376[4] = 0; - out_1490364464464661376[5] = 0; - out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[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_1490364464464661376[9] = 0; - out_1490364464464661376[10] = 0; - out_1490364464464661376[11] = 0; - out_1490364464464661376[12] = 0; - out_1490364464464661376[13] = 0; - out_1490364464464661376[14] = 0; - out_1490364464464661376[15] = 0; - out_1490364464464661376[16] = 0; - out_1490364464464661376[17] = 0; - out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[20] = 0; - out_1490364464464661376[21] = 0; - out_1490364464464661376[22] = 0; - out_1490364464464661376[23] = 0; - out_1490364464464661376[24] = 0; - out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[27] = 0; - out_1490364464464661376[28] = 0; - out_1490364464464661376[29] = 0; - out_1490364464464661376[30] = 0; - out_1490364464464661376[31] = 0; - out_1490364464464661376[32] = 0; - out_1490364464464661376[33] = 0; - out_1490364464464661376[34] = 0; - out_1490364464464661376[35] = 0; - out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[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_1490364464464661376[39] = 0; - out_1490364464464661376[40] = 0; - out_1490364464464661376[41] = 0; - out_1490364464464661376[42] = 0; - out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[45] = 0; - out_1490364464464661376[46] = 0; - out_1490364464464661376[47] = 0; - out_1490364464464661376[48] = 0; - out_1490364464464661376[49] = 0; - out_1490364464464661376[50] = 0; - out_1490364464464661376[51] = 0; - out_1490364464464661376[52] = 0; - out_1490364464464661376[53] = 0; - out_1490364464464661376[54] = 0; - out_1490364464464661376[55] = 0; - out_1490364464464661376[56] = 0; - out_1490364464464661376[57] = 1; - out_1490364464464661376[58] = 0; - out_1490364464464661376[59] = 0; - out_1490364464464661376[60] = 0; - out_1490364464464661376[61] = 0; - out_1490364464464661376[62] = 0; - out_1490364464464661376[63] = 0; - out_1490364464464661376[64] = 0; - out_1490364464464661376[65] = 0; - out_1490364464464661376[66] = dt; - out_1490364464464661376[67] = 0; - out_1490364464464661376[68] = 0; - out_1490364464464661376[69] = 0; - out_1490364464464661376[70] = 0; - out_1490364464464661376[71] = 0; - out_1490364464464661376[72] = 0; - out_1490364464464661376[73] = 0; - out_1490364464464661376[74] = 0; - out_1490364464464661376[75] = 0; - out_1490364464464661376[76] = 1; - out_1490364464464661376[77] = 0; - out_1490364464464661376[78] = 0; - out_1490364464464661376[79] = 0; - out_1490364464464661376[80] = 0; - out_1490364464464661376[81] = 0; - out_1490364464464661376[82] = 0; - out_1490364464464661376[83] = 0; - out_1490364464464661376[84] = 0; - out_1490364464464661376[85] = dt; - out_1490364464464661376[86] = 0; - out_1490364464464661376[87] = 0; - out_1490364464464661376[88] = 0; - out_1490364464464661376[89] = 0; - out_1490364464464661376[90] = 0; - out_1490364464464661376[91] = 0; - out_1490364464464661376[92] = 0; - out_1490364464464661376[93] = 0; - out_1490364464464661376[94] = 0; - out_1490364464464661376[95] = 1; - out_1490364464464661376[96] = 0; - out_1490364464464661376[97] = 0; - out_1490364464464661376[98] = 0; - out_1490364464464661376[99] = 0; - out_1490364464464661376[100] = 0; - out_1490364464464661376[101] = 0; - out_1490364464464661376[102] = 0; - out_1490364464464661376[103] = 0; - out_1490364464464661376[104] = dt; - out_1490364464464661376[105] = 0; - out_1490364464464661376[106] = 0; - out_1490364464464661376[107] = 0; - out_1490364464464661376[108] = 0; - out_1490364464464661376[109] = 0; - out_1490364464464661376[110] = 0; - out_1490364464464661376[111] = 0; - out_1490364464464661376[112] = 0; - out_1490364464464661376[113] = 0; - out_1490364464464661376[114] = 1; - out_1490364464464661376[115] = 0; - out_1490364464464661376[116] = 0; - out_1490364464464661376[117] = 0; - out_1490364464464661376[118] = 0; - out_1490364464464661376[119] = 0; - out_1490364464464661376[120] = 0; - out_1490364464464661376[121] = 0; - out_1490364464464661376[122] = 0; - out_1490364464464661376[123] = 0; - out_1490364464464661376[124] = 0; - out_1490364464464661376[125] = 0; - out_1490364464464661376[126] = 0; - out_1490364464464661376[127] = 0; - out_1490364464464661376[128] = 0; - out_1490364464464661376[129] = 0; - out_1490364464464661376[130] = 0; - out_1490364464464661376[131] = 0; - out_1490364464464661376[132] = 0; - out_1490364464464661376[133] = 1; - out_1490364464464661376[134] = 0; - out_1490364464464661376[135] = 0; - out_1490364464464661376[136] = 0; - out_1490364464464661376[137] = 0; - out_1490364464464661376[138] = 0; - out_1490364464464661376[139] = 0; - out_1490364464464661376[140] = 0; - out_1490364464464661376[141] = 0; - out_1490364464464661376[142] = 0; - out_1490364464464661376[143] = 0; - out_1490364464464661376[144] = 0; - out_1490364464464661376[145] = 0; - out_1490364464464661376[146] = 0; - out_1490364464464661376[147] = 0; - out_1490364464464661376[148] = 0; - out_1490364464464661376[149] = 0; - out_1490364464464661376[150] = 0; - out_1490364464464661376[151] = 0; - out_1490364464464661376[152] = 1; - out_1490364464464661376[153] = 0; - out_1490364464464661376[154] = 0; - out_1490364464464661376[155] = 0; - out_1490364464464661376[156] = 0; - out_1490364464464661376[157] = 0; - out_1490364464464661376[158] = 0; - out_1490364464464661376[159] = 0; - out_1490364464464661376[160] = 0; - out_1490364464464661376[161] = 0; - out_1490364464464661376[162] = 0; - out_1490364464464661376[163] = 0; - out_1490364464464661376[164] = 0; - out_1490364464464661376[165] = 0; - out_1490364464464661376[166] = 0; - out_1490364464464661376[167] = 0; - out_1490364464464661376[168] = 0; - out_1490364464464661376[169] = 0; - out_1490364464464661376[170] = 0; - out_1490364464464661376[171] = 1; - out_1490364464464661376[172] = 0; - out_1490364464464661376[173] = 0; - out_1490364464464661376[174] = 0; - out_1490364464464661376[175] = 0; - out_1490364464464661376[176] = 0; - out_1490364464464661376[177] = 0; - out_1490364464464661376[178] = 0; - out_1490364464464661376[179] = 0; - out_1490364464464661376[180] = 0; - out_1490364464464661376[181] = 0; - out_1490364464464661376[182] = 0; - out_1490364464464661376[183] = 0; - out_1490364464464661376[184] = 0; - out_1490364464464661376[185] = 0; - out_1490364464464661376[186] = 0; - out_1490364464464661376[187] = 0; - out_1490364464464661376[188] = 0; - out_1490364464464661376[189] = 0; - out_1490364464464661376[190] = 1; - out_1490364464464661376[191] = 0; - out_1490364464464661376[192] = 0; - out_1490364464464661376[193] = 0; - out_1490364464464661376[194] = 0; - out_1490364464464661376[195] = 0; - out_1490364464464661376[196] = 0; - out_1490364464464661376[197] = 0; - out_1490364464464661376[198] = 0; - out_1490364464464661376[199] = 0; - out_1490364464464661376[200] = 0; - out_1490364464464661376[201] = 0; - out_1490364464464661376[202] = 0; - out_1490364464464661376[203] = 0; - out_1490364464464661376[204] = 0; - out_1490364464464661376[205] = 0; - out_1490364464464661376[206] = 0; - out_1490364464464661376[207] = 0; - out_1490364464464661376[208] = 0; - out_1490364464464661376[209] = 1; - out_1490364464464661376[210] = 0; - out_1490364464464661376[211] = 0; - out_1490364464464661376[212] = 0; - out_1490364464464661376[213] = 0; - out_1490364464464661376[214] = 0; - out_1490364464464661376[215] = 0; - out_1490364464464661376[216] = 0; - out_1490364464464661376[217] = 0; - out_1490364464464661376[218] = 0; - out_1490364464464661376[219] = 0; - out_1490364464464661376[220] = 0; - out_1490364464464661376[221] = 0; - out_1490364464464661376[222] = 0; - out_1490364464464661376[223] = 0; - out_1490364464464661376[224] = 0; - out_1490364464464661376[225] = 0; - out_1490364464464661376[226] = 0; - out_1490364464464661376[227] = 0; - out_1490364464464661376[228] = 1; - out_1490364464464661376[229] = 0; - out_1490364464464661376[230] = 0; - out_1490364464464661376[231] = 0; - out_1490364464464661376[232] = 0; - out_1490364464464661376[233] = 0; - out_1490364464464661376[234] = 0; - out_1490364464464661376[235] = 0; - out_1490364464464661376[236] = 0; - out_1490364464464661376[237] = 0; - out_1490364464464661376[238] = 0; - out_1490364464464661376[239] = 0; - out_1490364464464661376[240] = 0; - out_1490364464464661376[241] = 0; - out_1490364464464661376[242] = 0; - out_1490364464464661376[243] = 0; - out_1490364464464661376[244] = 0; - out_1490364464464661376[245] = 0; - out_1490364464464661376[246] = 0; - out_1490364464464661376[247] = 1; - out_1490364464464661376[248] = 0; - out_1490364464464661376[249] = 0; - out_1490364464464661376[250] = 0; - out_1490364464464661376[251] = 0; - out_1490364464464661376[252] = 0; - out_1490364464464661376[253] = 0; - out_1490364464464661376[254] = 0; - out_1490364464464661376[255] = 0; - out_1490364464464661376[256] = 0; - out_1490364464464661376[257] = 0; - out_1490364464464661376[258] = 0; - out_1490364464464661376[259] = 0; - out_1490364464464661376[260] = 0; - out_1490364464464661376[261] = 0; - out_1490364464464661376[262] = 0; - out_1490364464464661376[263] = 0; - out_1490364464464661376[264] = 0; - out_1490364464464661376[265] = 0; - out_1490364464464661376[266] = 1; - out_1490364464464661376[267] = 0; - out_1490364464464661376[268] = 0; - out_1490364464464661376[269] = 0; - out_1490364464464661376[270] = 0; - out_1490364464464661376[271] = 0; - out_1490364464464661376[272] = 0; - out_1490364464464661376[273] = 0; - out_1490364464464661376[274] = 0; - out_1490364464464661376[275] = 0; - out_1490364464464661376[276] = 0; - out_1490364464464661376[277] = 0; - out_1490364464464661376[278] = 0; - out_1490364464464661376[279] = 0; - out_1490364464464661376[280] = 0; - out_1490364464464661376[281] = 0; - out_1490364464464661376[282] = 0; - out_1490364464464661376[283] = 0; - out_1490364464464661376[284] = 0; - out_1490364464464661376[285] = 1; - out_1490364464464661376[286] = 0; - out_1490364464464661376[287] = 0; - out_1490364464464661376[288] = 0; - out_1490364464464661376[289] = 0; - out_1490364464464661376[290] = 0; - out_1490364464464661376[291] = 0; - out_1490364464464661376[292] = 0; - out_1490364464464661376[293] = 0; - out_1490364464464661376[294] = 0; - out_1490364464464661376[295] = 0; - out_1490364464464661376[296] = 0; - out_1490364464464661376[297] = 0; - out_1490364464464661376[298] = 0; - out_1490364464464661376[299] = 0; - out_1490364464464661376[300] = 0; - out_1490364464464661376[301] = 0; - out_1490364464464661376[302] = 0; - out_1490364464464661376[303] = 0; - out_1490364464464661376[304] = 1; - out_1490364464464661376[305] = 0; - out_1490364464464661376[306] = 0; - out_1490364464464661376[307] = 0; - out_1490364464464661376[308] = 0; - out_1490364464464661376[309] = 0; - out_1490364464464661376[310] = 0; - out_1490364464464661376[311] = 0; - out_1490364464464661376[312] = 0; - out_1490364464464661376[313] = 0; - out_1490364464464661376[314] = 0; - out_1490364464464661376[315] = 0; - out_1490364464464661376[316] = 0; - out_1490364464464661376[317] = 0; - out_1490364464464661376[318] = 0; - out_1490364464464661376[319] = 0; - out_1490364464464661376[320] = 0; - out_1490364464464661376[321] = 0; - out_1490364464464661376[322] = 0; - out_1490364464464661376[323] = 1; +void F_fun(double *state, double dt, double *out_8990818239492707931) { + out_8990818239492707931[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_8990818239492707931[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_8990818239492707931[2] = 0; + out_8990818239492707931[3] = 0; + out_8990818239492707931[4] = 0; + out_8990818239492707931[5] = 0; + out_8990818239492707931[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_8990818239492707931[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_8990818239492707931[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_8990818239492707931[9] = 0; + out_8990818239492707931[10] = 0; + out_8990818239492707931[11] = 0; + out_8990818239492707931[12] = 0; + out_8990818239492707931[13] = 0; + out_8990818239492707931[14] = 0; + out_8990818239492707931[15] = 0; + out_8990818239492707931[16] = 0; + out_8990818239492707931[17] = 0; + out_8990818239492707931[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_8990818239492707931[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_8990818239492707931[20] = 0; + out_8990818239492707931[21] = 0; + out_8990818239492707931[22] = 0; + out_8990818239492707931[23] = 0; + out_8990818239492707931[24] = 0; + out_8990818239492707931[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_8990818239492707931[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_8990818239492707931[27] = 0; + out_8990818239492707931[28] = 0; + out_8990818239492707931[29] = 0; + out_8990818239492707931[30] = 0; + out_8990818239492707931[31] = 0; + out_8990818239492707931[32] = 0; + out_8990818239492707931[33] = 0; + out_8990818239492707931[34] = 0; + out_8990818239492707931[35] = 0; + out_8990818239492707931[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_8990818239492707931[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_8990818239492707931[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_8990818239492707931[39] = 0; + out_8990818239492707931[40] = 0; + out_8990818239492707931[41] = 0; + out_8990818239492707931[42] = 0; + out_8990818239492707931[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_8990818239492707931[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_8990818239492707931[45] = 0; + out_8990818239492707931[46] = 0; + out_8990818239492707931[47] = 0; + out_8990818239492707931[48] = 0; + out_8990818239492707931[49] = 0; + out_8990818239492707931[50] = 0; + out_8990818239492707931[51] = 0; + out_8990818239492707931[52] = 0; + out_8990818239492707931[53] = 0; + out_8990818239492707931[54] = 0; + out_8990818239492707931[55] = 0; + out_8990818239492707931[56] = 0; + out_8990818239492707931[57] = 1; + out_8990818239492707931[58] = 0; + out_8990818239492707931[59] = 0; + out_8990818239492707931[60] = 0; + out_8990818239492707931[61] = 0; + out_8990818239492707931[62] = 0; + out_8990818239492707931[63] = 0; + out_8990818239492707931[64] = 0; + out_8990818239492707931[65] = 0; + out_8990818239492707931[66] = dt; + out_8990818239492707931[67] = 0; + out_8990818239492707931[68] = 0; + out_8990818239492707931[69] = 0; + out_8990818239492707931[70] = 0; + out_8990818239492707931[71] = 0; + out_8990818239492707931[72] = 0; + out_8990818239492707931[73] = 0; + out_8990818239492707931[74] = 0; + out_8990818239492707931[75] = 0; + out_8990818239492707931[76] = 1; + out_8990818239492707931[77] = 0; + out_8990818239492707931[78] = 0; + out_8990818239492707931[79] = 0; + out_8990818239492707931[80] = 0; + out_8990818239492707931[81] = 0; + out_8990818239492707931[82] = 0; + out_8990818239492707931[83] = 0; + out_8990818239492707931[84] = 0; + out_8990818239492707931[85] = dt; + out_8990818239492707931[86] = 0; + out_8990818239492707931[87] = 0; + out_8990818239492707931[88] = 0; + out_8990818239492707931[89] = 0; + out_8990818239492707931[90] = 0; + out_8990818239492707931[91] = 0; + out_8990818239492707931[92] = 0; + out_8990818239492707931[93] = 0; + out_8990818239492707931[94] = 0; + out_8990818239492707931[95] = 1; + out_8990818239492707931[96] = 0; + out_8990818239492707931[97] = 0; + out_8990818239492707931[98] = 0; + out_8990818239492707931[99] = 0; + out_8990818239492707931[100] = 0; + out_8990818239492707931[101] = 0; + out_8990818239492707931[102] = 0; + out_8990818239492707931[103] = 0; + out_8990818239492707931[104] = dt; + out_8990818239492707931[105] = 0; + out_8990818239492707931[106] = 0; + out_8990818239492707931[107] = 0; + out_8990818239492707931[108] = 0; + out_8990818239492707931[109] = 0; + out_8990818239492707931[110] = 0; + out_8990818239492707931[111] = 0; + out_8990818239492707931[112] = 0; + out_8990818239492707931[113] = 0; + out_8990818239492707931[114] = 1; + out_8990818239492707931[115] = 0; + out_8990818239492707931[116] = 0; + out_8990818239492707931[117] = 0; + out_8990818239492707931[118] = 0; + out_8990818239492707931[119] = 0; + out_8990818239492707931[120] = 0; + out_8990818239492707931[121] = 0; + out_8990818239492707931[122] = 0; + out_8990818239492707931[123] = 0; + out_8990818239492707931[124] = 0; + out_8990818239492707931[125] = 0; + out_8990818239492707931[126] = 0; + out_8990818239492707931[127] = 0; + out_8990818239492707931[128] = 0; + out_8990818239492707931[129] = 0; + out_8990818239492707931[130] = 0; + out_8990818239492707931[131] = 0; + out_8990818239492707931[132] = 0; + out_8990818239492707931[133] = 1; + out_8990818239492707931[134] = 0; + out_8990818239492707931[135] = 0; + out_8990818239492707931[136] = 0; + out_8990818239492707931[137] = 0; + out_8990818239492707931[138] = 0; + out_8990818239492707931[139] = 0; + out_8990818239492707931[140] = 0; + out_8990818239492707931[141] = 0; + out_8990818239492707931[142] = 0; + out_8990818239492707931[143] = 0; + out_8990818239492707931[144] = 0; + out_8990818239492707931[145] = 0; + out_8990818239492707931[146] = 0; + out_8990818239492707931[147] = 0; + out_8990818239492707931[148] = 0; + out_8990818239492707931[149] = 0; + out_8990818239492707931[150] = 0; + out_8990818239492707931[151] = 0; + out_8990818239492707931[152] = 1; + out_8990818239492707931[153] = 0; + out_8990818239492707931[154] = 0; + out_8990818239492707931[155] = 0; + out_8990818239492707931[156] = 0; + out_8990818239492707931[157] = 0; + out_8990818239492707931[158] = 0; + out_8990818239492707931[159] = 0; + out_8990818239492707931[160] = 0; + out_8990818239492707931[161] = 0; + out_8990818239492707931[162] = 0; + out_8990818239492707931[163] = 0; + out_8990818239492707931[164] = 0; + out_8990818239492707931[165] = 0; + out_8990818239492707931[166] = 0; + out_8990818239492707931[167] = 0; + out_8990818239492707931[168] = 0; + out_8990818239492707931[169] = 0; + out_8990818239492707931[170] = 0; + out_8990818239492707931[171] = 1; + out_8990818239492707931[172] = 0; + out_8990818239492707931[173] = 0; + out_8990818239492707931[174] = 0; + out_8990818239492707931[175] = 0; + out_8990818239492707931[176] = 0; + out_8990818239492707931[177] = 0; + out_8990818239492707931[178] = 0; + out_8990818239492707931[179] = 0; + out_8990818239492707931[180] = 0; + out_8990818239492707931[181] = 0; + out_8990818239492707931[182] = 0; + out_8990818239492707931[183] = 0; + out_8990818239492707931[184] = 0; + out_8990818239492707931[185] = 0; + out_8990818239492707931[186] = 0; + out_8990818239492707931[187] = 0; + out_8990818239492707931[188] = 0; + out_8990818239492707931[189] = 0; + out_8990818239492707931[190] = 1; + out_8990818239492707931[191] = 0; + out_8990818239492707931[192] = 0; + out_8990818239492707931[193] = 0; + out_8990818239492707931[194] = 0; + out_8990818239492707931[195] = 0; + out_8990818239492707931[196] = 0; + out_8990818239492707931[197] = 0; + out_8990818239492707931[198] = 0; + out_8990818239492707931[199] = 0; + out_8990818239492707931[200] = 0; + out_8990818239492707931[201] = 0; + out_8990818239492707931[202] = 0; + out_8990818239492707931[203] = 0; + out_8990818239492707931[204] = 0; + out_8990818239492707931[205] = 0; + out_8990818239492707931[206] = 0; + out_8990818239492707931[207] = 0; + out_8990818239492707931[208] = 0; + out_8990818239492707931[209] = 1; + out_8990818239492707931[210] = 0; + out_8990818239492707931[211] = 0; + out_8990818239492707931[212] = 0; + out_8990818239492707931[213] = 0; + out_8990818239492707931[214] = 0; + out_8990818239492707931[215] = 0; + out_8990818239492707931[216] = 0; + out_8990818239492707931[217] = 0; + out_8990818239492707931[218] = 0; + out_8990818239492707931[219] = 0; + out_8990818239492707931[220] = 0; + out_8990818239492707931[221] = 0; + out_8990818239492707931[222] = 0; + out_8990818239492707931[223] = 0; + out_8990818239492707931[224] = 0; + out_8990818239492707931[225] = 0; + out_8990818239492707931[226] = 0; + out_8990818239492707931[227] = 0; + out_8990818239492707931[228] = 1; + out_8990818239492707931[229] = 0; + out_8990818239492707931[230] = 0; + out_8990818239492707931[231] = 0; + out_8990818239492707931[232] = 0; + out_8990818239492707931[233] = 0; + out_8990818239492707931[234] = 0; + out_8990818239492707931[235] = 0; + out_8990818239492707931[236] = 0; + out_8990818239492707931[237] = 0; + out_8990818239492707931[238] = 0; + out_8990818239492707931[239] = 0; + out_8990818239492707931[240] = 0; + out_8990818239492707931[241] = 0; + out_8990818239492707931[242] = 0; + out_8990818239492707931[243] = 0; + out_8990818239492707931[244] = 0; + out_8990818239492707931[245] = 0; + out_8990818239492707931[246] = 0; + out_8990818239492707931[247] = 1; + out_8990818239492707931[248] = 0; + out_8990818239492707931[249] = 0; + out_8990818239492707931[250] = 0; + out_8990818239492707931[251] = 0; + out_8990818239492707931[252] = 0; + out_8990818239492707931[253] = 0; + out_8990818239492707931[254] = 0; + out_8990818239492707931[255] = 0; + out_8990818239492707931[256] = 0; + out_8990818239492707931[257] = 0; + out_8990818239492707931[258] = 0; + out_8990818239492707931[259] = 0; + out_8990818239492707931[260] = 0; + out_8990818239492707931[261] = 0; + out_8990818239492707931[262] = 0; + out_8990818239492707931[263] = 0; + out_8990818239492707931[264] = 0; + out_8990818239492707931[265] = 0; + out_8990818239492707931[266] = 1; + out_8990818239492707931[267] = 0; + out_8990818239492707931[268] = 0; + out_8990818239492707931[269] = 0; + out_8990818239492707931[270] = 0; + out_8990818239492707931[271] = 0; + out_8990818239492707931[272] = 0; + out_8990818239492707931[273] = 0; + out_8990818239492707931[274] = 0; + out_8990818239492707931[275] = 0; + out_8990818239492707931[276] = 0; + out_8990818239492707931[277] = 0; + out_8990818239492707931[278] = 0; + out_8990818239492707931[279] = 0; + out_8990818239492707931[280] = 0; + out_8990818239492707931[281] = 0; + out_8990818239492707931[282] = 0; + out_8990818239492707931[283] = 0; + out_8990818239492707931[284] = 0; + out_8990818239492707931[285] = 1; + out_8990818239492707931[286] = 0; + out_8990818239492707931[287] = 0; + out_8990818239492707931[288] = 0; + out_8990818239492707931[289] = 0; + out_8990818239492707931[290] = 0; + out_8990818239492707931[291] = 0; + out_8990818239492707931[292] = 0; + out_8990818239492707931[293] = 0; + out_8990818239492707931[294] = 0; + out_8990818239492707931[295] = 0; + out_8990818239492707931[296] = 0; + out_8990818239492707931[297] = 0; + out_8990818239492707931[298] = 0; + out_8990818239492707931[299] = 0; + out_8990818239492707931[300] = 0; + out_8990818239492707931[301] = 0; + out_8990818239492707931[302] = 0; + out_8990818239492707931[303] = 0; + out_8990818239492707931[304] = 1; + out_8990818239492707931[305] = 0; + out_8990818239492707931[306] = 0; + out_8990818239492707931[307] = 0; + out_8990818239492707931[308] = 0; + out_8990818239492707931[309] = 0; + out_8990818239492707931[310] = 0; + out_8990818239492707931[311] = 0; + out_8990818239492707931[312] = 0; + out_8990818239492707931[313] = 0; + out_8990818239492707931[314] = 0; + out_8990818239492707931[315] = 0; + out_8990818239492707931[316] = 0; + out_8990818239492707931[317] = 0; + out_8990818239492707931[318] = 0; + out_8990818239492707931[319] = 0; + out_8990818239492707931[320] = 0; + out_8990818239492707931[321] = 0; + out_8990818239492707931[322] = 0; + out_8990818239492707931[323] = 1; } -void h_4(double *state, double *unused, double *out_4985239385195179173) { - out_4985239385195179173[0] = state[6] + state[9]; - out_4985239385195179173[1] = state[7] + state[10]; - out_4985239385195179173[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_4553534383789500912) { + out_4553534383789500912[0] = state[6] + state[9]; + out_4553534383789500912[1] = state[7] + state[10]; + out_4553534383789500912[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_2821601005274362620) { - out_2821601005274362620[0] = 0; - out_2821601005274362620[1] = 0; - out_2821601005274362620[2] = 0; - out_2821601005274362620[3] = 0; - out_2821601005274362620[4] = 0; - out_2821601005274362620[5] = 0; - out_2821601005274362620[6] = 1; - out_2821601005274362620[7] = 0; - out_2821601005274362620[8] = 0; - out_2821601005274362620[9] = 1; - out_2821601005274362620[10] = 0; - out_2821601005274362620[11] = 0; - out_2821601005274362620[12] = 0; - out_2821601005274362620[13] = 0; - out_2821601005274362620[14] = 0; - out_2821601005274362620[15] = 0; - out_2821601005274362620[16] = 0; - out_2821601005274362620[17] = 0; - out_2821601005274362620[18] = 0; - out_2821601005274362620[19] = 0; - out_2821601005274362620[20] = 0; - out_2821601005274362620[21] = 0; - out_2821601005274362620[22] = 0; - out_2821601005274362620[23] = 0; - out_2821601005274362620[24] = 0; - out_2821601005274362620[25] = 1; - out_2821601005274362620[26] = 0; - out_2821601005274362620[27] = 0; - out_2821601005274362620[28] = 1; - out_2821601005274362620[29] = 0; - out_2821601005274362620[30] = 0; - out_2821601005274362620[31] = 0; - out_2821601005274362620[32] = 0; - out_2821601005274362620[33] = 0; - out_2821601005274362620[34] = 0; - out_2821601005274362620[35] = 0; - out_2821601005274362620[36] = 0; - out_2821601005274362620[37] = 0; - out_2821601005274362620[38] = 0; - out_2821601005274362620[39] = 0; - out_2821601005274362620[40] = 0; - out_2821601005274362620[41] = 0; - out_2821601005274362620[42] = 0; - out_2821601005274362620[43] = 0; - out_2821601005274362620[44] = 1; - out_2821601005274362620[45] = 0; - out_2821601005274362620[46] = 0; - out_2821601005274362620[47] = 1; - out_2821601005274362620[48] = 0; - out_2821601005274362620[49] = 0; - out_2821601005274362620[50] = 0; - out_2821601005274362620[51] = 0; - out_2821601005274362620[52] = 0; - out_2821601005274362620[53] = 0; +void H_4(double *state, double *unused, double *out_637472039154741501) { + out_637472039154741501[0] = 0; + out_637472039154741501[1] = 0; + out_637472039154741501[2] = 0; + out_637472039154741501[3] = 0; + out_637472039154741501[4] = 0; + out_637472039154741501[5] = 0; + out_637472039154741501[6] = 1; + out_637472039154741501[7] = 0; + out_637472039154741501[8] = 0; + out_637472039154741501[9] = 1; + out_637472039154741501[10] = 0; + out_637472039154741501[11] = 0; + out_637472039154741501[12] = 0; + out_637472039154741501[13] = 0; + out_637472039154741501[14] = 0; + out_637472039154741501[15] = 0; + out_637472039154741501[16] = 0; + out_637472039154741501[17] = 0; + out_637472039154741501[18] = 0; + out_637472039154741501[19] = 0; + out_637472039154741501[20] = 0; + out_637472039154741501[21] = 0; + out_637472039154741501[22] = 0; + out_637472039154741501[23] = 0; + out_637472039154741501[24] = 0; + out_637472039154741501[25] = 1; + out_637472039154741501[26] = 0; + out_637472039154741501[27] = 0; + out_637472039154741501[28] = 1; + out_637472039154741501[29] = 0; + out_637472039154741501[30] = 0; + out_637472039154741501[31] = 0; + out_637472039154741501[32] = 0; + out_637472039154741501[33] = 0; + out_637472039154741501[34] = 0; + out_637472039154741501[35] = 0; + out_637472039154741501[36] = 0; + out_637472039154741501[37] = 0; + out_637472039154741501[38] = 0; + out_637472039154741501[39] = 0; + out_637472039154741501[40] = 0; + out_637472039154741501[41] = 0; + out_637472039154741501[42] = 0; + out_637472039154741501[43] = 0; + out_637472039154741501[44] = 1; + out_637472039154741501[45] = 0; + out_637472039154741501[46] = 0; + out_637472039154741501[47] = 1; + out_637472039154741501[48] = 0; + out_637472039154741501[49] = 0; + out_637472039154741501[50] = 0; + out_637472039154741501[51] = 0; + out_637472039154741501[52] = 0; + out_637472039154741501[53] = 0; } -void h_10(double *state, double *unused, double *out_5766917160067306951) { - out_5766917160067306951[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_5766917160067306951[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_5766917160067306951[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_5896232866039695568) { + out_5896232866039695568[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_5896232866039695568[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_5896232866039695568[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_393429258268914973) { - out_393429258268914973[0] = 0; - out_393429258268914973[1] = 9.8100000000000005*cos(state[1]); - out_393429258268914973[2] = 0; - out_393429258268914973[3] = 0; - out_393429258268914973[4] = -state[8]; - out_393429258268914973[5] = state[7]; - out_393429258268914973[6] = 0; - out_393429258268914973[7] = state[5]; - out_393429258268914973[8] = -state[4]; - out_393429258268914973[9] = 0; - out_393429258268914973[10] = 0; - out_393429258268914973[11] = 0; - out_393429258268914973[12] = 1; - out_393429258268914973[13] = 0; - out_393429258268914973[14] = 0; - out_393429258268914973[15] = 1; - out_393429258268914973[16] = 0; - out_393429258268914973[17] = 0; - out_393429258268914973[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_393429258268914973[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_393429258268914973[20] = 0; - out_393429258268914973[21] = state[8]; - out_393429258268914973[22] = 0; - out_393429258268914973[23] = -state[6]; - out_393429258268914973[24] = -state[5]; - out_393429258268914973[25] = 0; - out_393429258268914973[26] = state[3]; - out_393429258268914973[27] = 0; - out_393429258268914973[28] = 0; - out_393429258268914973[29] = 0; - out_393429258268914973[30] = 0; - out_393429258268914973[31] = 1; - out_393429258268914973[32] = 0; - out_393429258268914973[33] = 0; - out_393429258268914973[34] = 1; - out_393429258268914973[35] = 0; - out_393429258268914973[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_393429258268914973[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_393429258268914973[38] = 0; - out_393429258268914973[39] = -state[7]; - out_393429258268914973[40] = state[6]; - out_393429258268914973[41] = 0; - out_393429258268914973[42] = state[4]; - out_393429258268914973[43] = -state[3]; - out_393429258268914973[44] = 0; - out_393429258268914973[45] = 0; - out_393429258268914973[46] = 0; - out_393429258268914973[47] = 0; - out_393429258268914973[48] = 0; - out_393429258268914973[49] = 0; - out_393429258268914973[50] = 1; - out_393429258268914973[51] = 0; - out_393429258268914973[52] = 0; - out_393429258268914973[53] = 1; +void H_10(double *state, double *unused, double *out_6470002192212990620) { + out_6470002192212990620[0] = 0; + out_6470002192212990620[1] = 9.8100000000000005*cos(state[1]); + out_6470002192212990620[2] = 0; + out_6470002192212990620[3] = 0; + out_6470002192212990620[4] = -state[8]; + out_6470002192212990620[5] = state[7]; + out_6470002192212990620[6] = 0; + out_6470002192212990620[7] = state[5]; + out_6470002192212990620[8] = -state[4]; + out_6470002192212990620[9] = 0; + out_6470002192212990620[10] = 0; + out_6470002192212990620[11] = 0; + out_6470002192212990620[12] = 1; + out_6470002192212990620[13] = 0; + out_6470002192212990620[14] = 0; + out_6470002192212990620[15] = 1; + out_6470002192212990620[16] = 0; + out_6470002192212990620[17] = 0; + out_6470002192212990620[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_6470002192212990620[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_6470002192212990620[20] = 0; + out_6470002192212990620[21] = state[8]; + out_6470002192212990620[22] = 0; + out_6470002192212990620[23] = -state[6]; + out_6470002192212990620[24] = -state[5]; + out_6470002192212990620[25] = 0; + out_6470002192212990620[26] = state[3]; + out_6470002192212990620[27] = 0; + out_6470002192212990620[28] = 0; + out_6470002192212990620[29] = 0; + out_6470002192212990620[30] = 0; + out_6470002192212990620[31] = 1; + out_6470002192212990620[32] = 0; + out_6470002192212990620[33] = 0; + out_6470002192212990620[34] = 1; + out_6470002192212990620[35] = 0; + out_6470002192212990620[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_6470002192212990620[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_6470002192212990620[38] = 0; + out_6470002192212990620[39] = -state[7]; + out_6470002192212990620[40] = state[6]; + out_6470002192212990620[41] = 0; + out_6470002192212990620[42] = state[4]; + out_6470002192212990620[43] = -state[3]; + out_6470002192212990620[44] = 0; + out_6470002192212990620[45] = 0; + out_6470002192212990620[46] = 0; + out_6470002192212990620[47] = 0; + out_6470002192212990620[48] = 0; + out_6470002192212990620[49] = 0; + out_6470002192212990620[50] = 1; + out_6470002192212990620[51] = 0; + out_6470002192212990620[52] = 0; + out_6470002192212990620[53] = 1; } -void h_13(double *state, double *unused, double *out_7359463090555700335) { - out_7359463090555700335[0] = state[3]; - out_7359463090555700335[1] = state[4]; - out_7359463090555700335[2] = state[5]; +void h_13(double *state, double *unused, double *out_5564060116117934481) { + out_5564060116117934481[0] = state[3]; + out_5564060116117934481[1] = state[4]; + out_5564060116117934481[2] = state[5]; } -void H_13(double *state, double *unused, double *out_6033874830606695421) { - out_6033874830606695421[0] = 0; - out_6033874830606695421[1] = 0; - out_6033874830606695421[2] = 0; - out_6033874830606695421[3] = 1; - out_6033874830606695421[4] = 0; - out_6033874830606695421[5] = 0; - out_6033874830606695421[6] = 0; - out_6033874830606695421[7] = 0; - out_6033874830606695421[8] = 0; - out_6033874830606695421[9] = 0; - out_6033874830606695421[10] = 0; - out_6033874830606695421[11] = 0; - out_6033874830606695421[12] = 0; - out_6033874830606695421[13] = 0; - out_6033874830606695421[14] = 0; - out_6033874830606695421[15] = 0; - out_6033874830606695421[16] = 0; - out_6033874830606695421[17] = 0; - out_6033874830606695421[18] = 0; - out_6033874830606695421[19] = 0; - out_6033874830606695421[20] = 0; - out_6033874830606695421[21] = 0; - out_6033874830606695421[22] = 1; - out_6033874830606695421[23] = 0; - out_6033874830606695421[24] = 0; - out_6033874830606695421[25] = 0; - out_6033874830606695421[26] = 0; - out_6033874830606695421[27] = 0; - out_6033874830606695421[28] = 0; - out_6033874830606695421[29] = 0; - out_6033874830606695421[30] = 0; - out_6033874830606695421[31] = 0; - out_6033874830606695421[32] = 0; - out_6033874830606695421[33] = 0; - out_6033874830606695421[34] = 0; - out_6033874830606695421[35] = 0; - out_6033874830606695421[36] = 0; - out_6033874830606695421[37] = 0; - out_6033874830606695421[38] = 0; - out_6033874830606695421[39] = 0; - out_6033874830606695421[40] = 0; - out_6033874830606695421[41] = 1; - out_6033874830606695421[42] = 0; - out_6033874830606695421[43] = 0; - out_6033874830606695421[44] = 0; - out_6033874830606695421[45] = 0; - out_6033874830606695421[46] = 0; - out_6033874830606695421[47] = 0; - out_6033874830606695421[48] = 0; - out_6033874830606695421[49] = 0; - out_6033874830606695421[50] = 0; - out_6033874830606695421[51] = 0; - out_6033874830606695421[52] = 0; - out_6033874830606695421[53] = 0; +void H_13(double *state, double *unused, double *out_2574801786177591300) { + out_2574801786177591300[0] = 0; + out_2574801786177591300[1] = 0; + out_2574801786177591300[2] = 0; + out_2574801786177591300[3] = 1; + out_2574801786177591300[4] = 0; + out_2574801786177591300[5] = 0; + out_2574801786177591300[6] = 0; + out_2574801786177591300[7] = 0; + out_2574801786177591300[8] = 0; + out_2574801786177591300[9] = 0; + out_2574801786177591300[10] = 0; + out_2574801786177591300[11] = 0; + out_2574801786177591300[12] = 0; + out_2574801786177591300[13] = 0; + out_2574801786177591300[14] = 0; + out_2574801786177591300[15] = 0; + out_2574801786177591300[16] = 0; + out_2574801786177591300[17] = 0; + out_2574801786177591300[18] = 0; + out_2574801786177591300[19] = 0; + out_2574801786177591300[20] = 0; + out_2574801786177591300[21] = 0; + out_2574801786177591300[22] = 1; + out_2574801786177591300[23] = 0; + out_2574801786177591300[24] = 0; + out_2574801786177591300[25] = 0; + out_2574801786177591300[26] = 0; + out_2574801786177591300[27] = 0; + out_2574801786177591300[28] = 0; + out_2574801786177591300[29] = 0; + out_2574801786177591300[30] = 0; + out_2574801786177591300[31] = 0; + out_2574801786177591300[32] = 0; + out_2574801786177591300[33] = 0; + out_2574801786177591300[34] = 0; + out_2574801786177591300[35] = 0; + out_2574801786177591300[36] = 0; + out_2574801786177591300[37] = 0; + out_2574801786177591300[38] = 0; + out_2574801786177591300[39] = 0; + out_2574801786177591300[40] = 0; + out_2574801786177591300[41] = 1; + out_2574801786177591300[42] = 0; + out_2574801786177591300[43] = 0; + out_2574801786177591300[44] = 0; + out_2574801786177591300[45] = 0; + out_2574801786177591300[46] = 0; + out_2574801786177591300[47] = 0; + out_2574801786177591300[48] = 0; + out_2574801786177591300[49] = 0; + out_2574801786177591300[50] = 0; + out_2574801786177591300[51] = 0; + out_2574801786177591300[52] = 0; + out_2574801786177591300[53] = 0; } -void h_14(double *state, double *unused, double *out_1914760434129654014) { - out_1914760434129654014[0] = state[6]; - out_1914760434129654014[1] = state[7]; - out_1914760434129654014[2] = state[8]; +void h_14(double *state, double *unused, double *out_9129729305013669813) { + out_9129729305013669813[0] = state[6]; + out_9129729305013669813[1] = state[7]; + out_9129729305013669813[2] = state[8]; } -void H_14(double *state, double *unused, double *out_261187427021009676) { - out_261187427021009676[0] = 0; - out_261187427021009676[1] = 0; - out_261187427021009676[2] = 0; - out_261187427021009676[3] = 0; - out_261187427021009676[4] = 0; - out_261187427021009676[5] = 0; - out_261187427021009676[6] = 1; - out_261187427021009676[7] = 0; - out_261187427021009676[8] = 0; - out_261187427021009676[9] = 0; - out_261187427021009676[10] = 0; - out_261187427021009676[11] = 0; - out_261187427021009676[12] = 0; - out_261187427021009676[13] = 0; - out_261187427021009676[14] = 0; - out_261187427021009676[15] = 0; - out_261187427021009676[16] = 0; - out_261187427021009676[17] = 0; - out_261187427021009676[18] = 0; - out_261187427021009676[19] = 0; - out_261187427021009676[20] = 0; - out_261187427021009676[21] = 0; - out_261187427021009676[22] = 0; - out_261187427021009676[23] = 0; - out_261187427021009676[24] = 0; - out_261187427021009676[25] = 1; - out_261187427021009676[26] = 0; - out_261187427021009676[27] = 0; - out_261187427021009676[28] = 0; - out_261187427021009676[29] = 0; - out_261187427021009676[30] = 0; - out_261187427021009676[31] = 0; - out_261187427021009676[32] = 0; - out_261187427021009676[33] = 0; - out_261187427021009676[34] = 0; - out_261187427021009676[35] = 0; - out_261187427021009676[36] = 0; - out_261187427021009676[37] = 0; - out_261187427021009676[38] = 0; - out_261187427021009676[39] = 0; - out_261187427021009676[40] = 0; - out_261187427021009676[41] = 0; - out_261187427021009676[42] = 0; - out_261187427021009676[43] = 0; - out_261187427021009676[44] = 1; - out_261187427021009676[45] = 0; - out_261187427021009676[46] = 0; - out_261187427021009676[47] = 0; - out_261187427021009676[48] = 0; - out_261187427021009676[49] = 0; - out_261187427021009676[50] = 0; - out_261187427021009676[51] = 0; - out_261187427021009676[52] = 0; - out_261187427021009676[53] = 0; +void H_14(double *state, double *unused, double *out_3325768817184743028) { + out_3325768817184743028[0] = 0; + out_3325768817184743028[1] = 0; + out_3325768817184743028[2] = 0; + out_3325768817184743028[3] = 0; + out_3325768817184743028[4] = 0; + out_3325768817184743028[5] = 0; + out_3325768817184743028[6] = 1; + out_3325768817184743028[7] = 0; + out_3325768817184743028[8] = 0; + out_3325768817184743028[9] = 0; + out_3325768817184743028[10] = 0; + out_3325768817184743028[11] = 0; + out_3325768817184743028[12] = 0; + out_3325768817184743028[13] = 0; + out_3325768817184743028[14] = 0; + out_3325768817184743028[15] = 0; + out_3325768817184743028[16] = 0; + out_3325768817184743028[17] = 0; + out_3325768817184743028[18] = 0; + out_3325768817184743028[19] = 0; + out_3325768817184743028[20] = 0; + out_3325768817184743028[21] = 0; + out_3325768817184743028[22] = 0; + out_3325768817184743028[23] = 0; + out_3325768817184743028[24] = 0; + out_3325768817184743028[25] = 1; + out_3325768817184743028[26] = 0; + out_3325768817184743028[27] = 0; + out_3325768817184743028[28] = 0; + out_3325768817184743028[29] = 0; + out_3325768817184743028[30] = 0; + out_3325768817184743028[31] = 0; + out_3325768817184743028[32] = 0; + out_3325768817184743028[33] = 0; + out_3325768817184743028[34] = 0; + out_3325768817184743028[35] = 0; + out_3325768817184743028[36] = 0; + out_3325768817184743028[37] = 0; + out_3325768817184743028[38] = 0; + out_3325768817184743028[39] = 0; + out_3325768817184743028[40] = 0; + out_3325768817184743028[41] = 0; + out_3325768817184743028[42] = 0; + out_3325768817184743028[43] = 0; + out_3325768817184743028[44] = 1; + out_3325768817184743028[45] = 0; + out_3325768817184743028[46] = 0; + out_3325768817184743028[47] = 0; + out_3325768817184743028[48] = 0; + out_3325768817184743028[49] = 0; + out_3325768817184743028[50] = 0; + out_3325768817184743028[51] = 0; + out_3325768817184743028[52] = 0; + out_3325768817184743028[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_1277882425838003162) { - err_fun(nom_x, delta_x, out_1277882425838003162); +void pose_err_fun(double *nom_x, double *delta_x, double *out_842964569186968919) { + err_fun(nom_x, delta_x, out_842964569186968919); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5395183656796766) { - inv_err_fun(nom_x, true_x, out_5395183656796766); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7637995249051351300) { + inv_err_fun(nom_x, true_x, out_7637995249051351300); } -void pose_H_mod_fun(double *state, double *out_4641299731180659481) { - H_mod_fun(state, out_4641299731180659481); +void pose_H_mod_fun(double *state, double *out_8306889626903079841) { + H_mod_fun(state, out_8306889626903079841); } -void pose_f_fun(double *state, double dt, double *out_4475655607956648045) { - f_fun(state, dt, out_4475655607956648045); +void pose_f_fun(double *state, double dt, double *out_5013225008344736149) { + f_fun(state, dt, out_5013225008344736149); } -void pose_F_fun(double *state, double dt, double *out_1490364464464661376) { - F_fun(state, dt, out_1490364464464661376); +void pose_F_fun(double *state, double dt, double *out_8990818239492707931) { + F_fun(state, dt, out_8990818239492707931); } -void pose_h_4(double *state, double *unused, double *out_4985239385195179173) { - h_4(state, unused, out_4985239385195179173); +void pose_h_4(double *state, double *unused, double *out_4553534383789500912) { + h_4(state, unused, out_4553534383789500912); } -void pose_H_4(double *state, double *unused, double *out_2821601005274362620) { - H_4(state, unused, out_2821601005274362620); +void pose_H_4(double *state, double *unused, double *out_637472039154741501) { + H_4(state, unused, out_637472039154741501); } -void pose_h_10(double *state, double *unused, double *out_5766917160067306951) { - h_10(state, unused, out_5766917160067306951); +void pose_h_10(double *state, double *unused, double *out_5896232866039695568) { + h_10(state, unused, out_5896232866039695568); } -void pose_H_10(double *state, double *unused, double *out_393429258268914973) { - H_10(state, unused, out_393429258268914973); +void pose_H_10(double *state, double *unused, double *out_6470002192212990620) { + H_10(state, unused, out_6470002192212990620); } -void pose_h_13(double *state, double *unused, double *out_7359463090555700335) { - h_13(state, unused, out_7359463090555700335); +void pose_h_13(double *state, double *unused, double *out_5564060116117934481) { + h_13(state, unused, out_5564060116117934481); } -void pose_H_13(double *state, double *unused, double *out_6033874830606695421) { - H_13(state, unused, out_6033874830606695421); +void pose_H_13(double *state, double *unused, double *out_2574801786177591300) { + H_13(state, unused, out_2574801786177591300); } -void pose_h_14(double *state, double *unused, double *out_1914760434129654014) { - h_14(state, unused, out_1914760434129654014); +void pose_h_14(double *state, double *unused, double *out_9129729305013669813) { + h_14(state, unused, out_9129729305013669813); } -void pose_H_14(double *state, double *unused, double *out_261187427021009676) { - H_14(state, unused, out_261187427021009676); +void pose_H_14(double *state, double *unused, double *out_3325768817184743028) { + H_14(state, unused, out_3325768817184743028); } 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 4b3598e18..998b11eca 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_1277882425838003162); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5395183656796766); -void pose_H_mod_fun(double *state, double *out_4641299731180659481); -void pose_f_fun(double *state, double dt, double *out_4475655607956648045); -void pose_F_fun(double *state, double dt, double *out_1490364464464661376); -void pose_h_4(double *state, double *unused, double *out_4985239385195179173); -void pose_H_4(double *state, double *unused, double *out_2821601005274362620); -void pose_h_10(double *state, double *unused, double *out_5766917160067306951); -void pose_H_10(double *state, double *unused, double *out_393429258268914973); -void pose_h_13(double *state, double *unused, double *out_7359463090555700335); -void pose_H_13(double *state, double *unused, double *out_6033874830606695421); -void pose_h_14(double *state, double *unused, double *out_1914760434129654014); -void pose_H_14(double *state, double *unused, double *out_261187427021009676); +void pose_err_fun(double *nom_x, double *delta_x, double *out_842964569186968919); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7637995249051351300); +void pose_H_mod_fun(double *state, double *out_8306889626903079841); +void pose_f_fun(double *state, double dt, double *out_5013225008344736149); +void pose_F_fun(double *state, double dt, double *out_8990818239492707931); +void pose_h_4(double *state, double *unused, double *out_4553534383789500912); +void pose_H_4(double *state, double *unused, double *out_637472039154741501); +void pose_h_10(double *state, double *unused, double *out_5896232866039695568); +void pose_H_10(double *state, double *unused, double *out_6470002192212990620); +void pose_h_13(double *state, double *unused, double *out_5564060116117934481); +void pose_H_13(double *state, double *unused, double *out_2574801786177591300); +void pose_h_14(double *state, double *unused, double *out_9129729305013669813); +void pose_H_14(double *state, double *unused, double *out_3325768817184743028); 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 398f86561..f0a0e535f 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 4786d948e..1af6e3578 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 887a7747a..0844709fd 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index beeda1ca3..0921984d9 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 0c1cc0706..bd4be1376 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 0f18504d3..a1a09b60c 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ