diff --git a/common/libcommon.a b/common/libcommon.a index 52e9536c7..ac32ab93e 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 f2824f863..7d0a2b111 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 acf7d8090..b11fc918f 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 d688605cf..00af6e9f5 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 2f1adcba1..d0e8681b3 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 10cff1011..53c262226 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 faf199d24..2eca9b8a6 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 cd4656d23..faf305d34 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 9afc99b74..8babaa078 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 9afc99b74..8babaa078 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 01e96ad81..f1bf70f32 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 cd4656d23..faf305d34 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 a86cc8f40..8589a089a 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-c88bebbc-DEBUG"; +const uint8_t gitversion[19] = "DEV-8a1bfb92-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index c92f1c81c..401856d5d 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 5767fa19c..f88bdbbd3 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 7af95778d..1a8fd88d0 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 f42821f93..94da6116d 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 e2f4a30d7..65844fc25 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 e8ce7ac72..124bd7bae 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 bbb687427..9dcc1acbf 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 5b2d0c0fc..b8fc1a302 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 cd696eabc..ad857df0d 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 dd525298e..694750678 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 64910b8ba..037990ad5 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 b3bfb0ce3..b4adf4f44 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 21327e988..daada0bd5 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 2f9f88778..d433aed5d 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 28d5cf4a7..fda28736d 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 19272b975..7e8a067cc 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 6653bb5a8..8d752f9e8 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 f01ad2573..8874d8115 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 da665d81f..0ee243118 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 33f6466b4..abcd65535 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 aa4841c99..4be2c976f 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-c88bebbc-DEBUG \ No newline at end of file +DEV-8a1bfb92-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index a53c5da24..3a3221f32 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_4102855695027319146) { - out_4102855695027319146[0] = delta_x[0] + nom_x[0]; - out_4102855695027319146[1] = delta_x[1] + nom_x[1]; - out_4102855695027319146[2] = delta_x[2] + nom_x[2]; - out_4102855695027319146[3] = delta_x[3] + nom_x[3]; - out_4102855695027319146[4] = delta_x[4] + nom_x[4]; - out_4102855695027319146[5] = delta_x[5] + nom_x[5]; - out_4102855695027319146[6] = delta_x[6] + nom_x[6]; - out_4102855695027319146[7] = delta_x[7] + nom_x[7]; - out_4102855695027319146[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_8250891246740759679) { + out_8250891246740759679[0] = delta_x[0] + nom_x[0]; + out_8250891246740759679[1] = delta_x[1] + nom_x[1]; + out_8250891246740759679[2] = delta_x[2] + nom_x[2]; + out_8250891246740759679[3] = delta_x[3] + nom_x[3]; + out_8250891246740759679[4] = delta_x[4] + nom_x[4]; + out_8250891246740759679[5] = delta_x[5] + nom_x[5]; + out_8250891246740759679[6] = delta_x[6] + nom_x[6]; + out_8250891246740759679[7] = delta_x[7] + nom_x[7]; + out_8250891246740759679[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_375370046560025251) { - out_375370046560025251[0] = -nom_x[0] + true_x[0]; - out_375370046560025251[1] = -nom_x[1] + true_x[1]; - out_375370046560025251[2] = -nom_x[2] + true_x[2]; - out_375370046560025251[3] = -nom_x[3] + true_x[3]; - out_375370046560025251[4] = -nom_x[4] + true_x[4]; - out_375370046560025251[5] = -nom_x[5] + true_x[5]; - out_375370046560025251[6] = -nom_x[6] + true_x[6]; - out_375370046560025251[7] = -nom_x[7] + true_x[7]; - out_375370046560025251[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6699306425357192023) { + out_6699306425357192023[0] = -nom_x[0] + true_x[0]; + out_6699306425357192023[1] = -nom_x[1] + true_x[1]; + out_6699306425357192023[2] = -nom_x[2] + true_x[2]; + out_6699306425357192023[3] = -nom_x[3] + true_x[3]; + out_6699306425357192023[4] = -nom_x[4] + true_x[4]; + out_6699306425357192023[5] = -nom_x[5] + true_x[5]; + out_6699306425357192023[6] = -nom_x[6] + true_x[6]; + out_6699306425357192023[7] = -nom_x[7] + true_x[7]; + out_6699306425357192023[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_4142214806030893418) { - out_4142214806030893418[0] = 1.0; - out_4142214806030893418[1] = 0.0; - out_4142214806030893418[2] = 0.0; - out_4142214806030893418[3] = 0.0; - out_4142214806030893418[4] = 0.0; - out_4142214806030893418[5] = 0.0; - out_4142214806030893418[6] = 0.0; - out_4142214806030893418[7] = 0.0; - out_4142214806030893418[8] = 0.0; - out_4142214806030893418[9] = 0.0; - out_4142214806030893418[10] = 1.0; - out_4142214806030893418[11] = 0.0; - out_4142214806030893418[12] = 0.0; - out_4142214806030893418[13] = 0.0; - out_4142214806030893418[14] = 0.0; - out_4142214806030893418[15] = 0.0; - out_4142214806030893418[16] = 0.0; - out_4142214806030893418[17] = 0.0; - out_4142214806030893418[18] = 0.0; - out_4142214806030893418[19] = 0.0; - out_4142214806030893418[20] = 1.0; - out_4142214806030893418[21] = 0.0; - out_4142214806030893418[22] = 0.0; - out_4142214806030893418[23] = 0.0; - out_4142214806030893418[24] = 0.0; - out_4142214806030893418[25] = 0.0; - out_4142214806030893418[26] = 0.0; - out_4142214806030893418[27] = 0.0; - out_4142214806030893418[28] = 0.0; - out_4142214806030893418[29] = 0.0; - out_4142214806030893418[30] = 1.0; - out_4142214806030893418[31] = 0.0; - out_4142214806030893418[32] = 0.0; - out_4142214806030893418[33] = 0.0; - out_4142214806030893418[34] = 0.0; - out_4142214806030893418[35] = 0.0; - out_4142214806030893418[36] = 0.0; - out_4142214806030893418[37] = 0.0; - out_4142214806030893418[38] = 0.0; - out_4142214806030893418[39] = 0.0; - out_4142214806030893418[40] = 1.0; - out_4142214806030893418[41] = 0.0; - out_4142214806030893418[42] = 0.0; - out_4142214806030893418[43] = 0.0; - out_4142214806030893418[44] = 0.0; - out_4142214806030893418[45] = 0.0; - out_4142214806030893418[46] = 0.0; - out_4142214806030893418[47] = 0.0; - out_4142214806030893418[48] = 0.0; - out_4142214806030893418[49] = 0.0; - out_4142214806030893418[50] = 1.0; - out_4142214806030893418[51] = 0.0; - out_4142214806030893418[52] = 0.0; - out_4142214806030893418[53] = 0.0; - out_4142214806030893418[54] = 0.0; - out_4142214806030893418[55] = 0.0; - out_4142214806030893418[56] = 0.0; - out_4142214806030893418[57] = 0.0; - out_4142214806030893418[58] = 0.0; - out_4142214806030893418[59] = 0.0; - out_4142214806030893418[60] = 1.0; - out_4142214806030893418[61] = 0.0; - out_4142214806030893418[62] = 0.0; - out_4142214806030893418[63] = 0.0; - out_4142214806030893418[64] = 0.0; - out_4142214806030893418[65] = 0.0; - out_4142214806030893418[66] = 0.0; - out_4142214806030893418[67] = 0.0; - out_4142214806030893418[68] = 0.0; - out_4142214806030893418[69] = 0.0; - out_4142214806030893418[70] = 1.0; - out_4142214806030893418[71] = 0.0; - out_4142214806030893418[72] = 0.0; - out_4142214806030893418[73] = 0.0; - out_4142214806030893418[74] = 0.0; - out_4142214806030893418[75] = 0.0; - out_4142214806030893418[76] = 0.0; - out_4142214806030893418[77] = 0.0; - out_4142214806030893418[78] = 0.0; - out_4142214806030893418[79] = 0.0; - out_4142214806030893418[80] = 1.0; +void H_mod_fun(double *state, double *out_2914898107951165883) { + out_2914898107951165883[0] = 1.0; + out_2914898107951165883[1] = 0.0; + out_2914898107951165883[2] = 0.0; + out_2914898107951165883[3] = 0.0; + out_2914898107951165883[4] = 0.0; + out_2914898107951165883[5] = 0.0; + out_2914898107951165883[6] = 0.0; + out_2914898107951165883[7] = 0.0; + out_2914898107951165883[8] = 0.0; + out_2914898107951165883[9] = 0.0; + out_2914898107951165883[10] = 1.0; + out_2914898107951165883[11] = 0.0; + out_2914898107951165883[12] = 0.0; + out_2914898107951165883[13] = 0.0; + out_2914898107951165883[14] = 0.0; + out_2914898107951165883[15] = 0.0; + out_2914898107951165883[16] = 0.0; + out_2914898107951165883[17] = 0.0; + out_2914898107951165883[18] = 0.0; + out_2914898107951165883[19] = 0.0; + out_2914898107951165883[20] = 1.0; + out_2914898107951165883[21] = 0.0; + out_2914898107951165883[22] = 0.0; + out_2914898107951165883[23] = 0.0; + out_2914898107951165883[24] = 0.0; + out_2914898107951165883[25] = 0.0; + out_2914898107951165883[26] = 0.0; + out_2914898107951165883[27] = 0.0; + out_2914898107951165883[28] = 0.0; + out_2914898107951165883[29] = 0.0; + out_2914898107951165883[30] = 1.0; + out_2914898107951165883[31] = 0.0; + out_2914898107951165883[32] = 0.0; + out_2914898107951165883[33] = 0.0; + out_2914898107951165883[34] = 0.0; + out_2914898107951165883[35] = 0.0; + out_2914898107951165883[36] = 0.0; + out_2914898107951165883[37] = 0.0; + out_2914898107951165883[38] = 0.0; + out_2914898107951165883[39] = 0.0; + out_2914898107951165883[40] = 1.0; + out_2914898107951165883[41] = 0.0; + out_2914898107951165883[42] = 0.0; + out_2914898107951165883[43] = 0.0; + out_2914898107951165883[44] = 0.0; + out_2914898107951165883[45] = 0.0; + out_2914898107951165883[46] = 0.0; + out_2914898107951165883[47] = 0.0; + out_2914898107951165883[48] = 0.0; + out_2914898107951165883[49] = 0.0; + out_2914898107951165883[50] = 1.0; + out_2914898107951165883[51] = 0.0; + out_2914898107951165883[52] = 0.0; + out_2914898107951165883[53] = 0.0; + out_2914898107951165883[54] = 0.0; + out_2914898107951165883[55] = 0.0; + out_2914898107951165883[56] = 0.0; + out_2914898107951165883[57] = 0.0; + out_2914898107951165883[58] = 0.0; + out_2914898107951165883[59] = 0.0; + out_2914898107951165883[60] = 1.0; + out_2914898107951165883[61] = 0.0; + out_2914898107951165883[62] = 0.0; + out_2914898107951165883[63] = 0.0; + out_2914898107951165883[64] = 0.0; + out_2914898107951165883[65] = 0.0; + out_2914898107951165883[66] = 0.0; + out_2914898107951165883[67] = 0.0; + out_2914898107951165883[68] = 0.0; + out_2914898107951165883[69] = 0.0; + out_2914898107951165883[70] = 1.0; + out_2914898107951165883[71] = 0.0; + out_2914898107951165883[72] = 0.0; + out_2914898107951165883[73] = 0.0; + out_2914898107951165883[74] = 0.0; + out_2914898107951165883[75] = 0.0; + out_2914898107951165883[76] = 0.0; + out_2914898107951165883[77] = 0.0; + out_2914898107951165883[78] = 0.0; + out_2914898107951165883[79] = 0.0; + out_2914898107951165883[80] = 1.0; } -void f_fun(double *state, double dt, double *out_1332366097619626209) { - out_1332366097619626209[0] = state[0]; - out_1332366097619626209[1] = state[1]; - out_1332366097619626209[2] = state[2]; - out_1332366097619626209[3] = state[3]; - out_1332366097619626209[4] = state[4]; - out_1332366097619626209[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_1332366097619626209[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_1332366097619626209[7] = state[7]; - out_1332366097619626209[8] = state[8]; +void f_fun(double *state, double dt, double *out_2675178968316523152) { + out_2675178968316523152[0] = state[0]; + out_2675178968316523152[1] = state[1]; + out_2675178968316523152[2] = state[2]; + out_2675178968316523152[3] = state[3]; + out_2675178968316523152[4] = state[4]; + out_2675178968316523152[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_2675178968316523152[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_2675178968316523152[7] = state[7]; + out_2675178968316523152[8] = state[8]; } -void F_fun(double *state, double dt, double *out_2970389157093285155) { - out_2970389157093285155[0] = 1; - out_2970389157093285155[1] = 0; - out_2970389157093285155[2] = 0; - out_2970389157093285155[3] = 0; - out_2970389157093285155[4] = 0; - out_2970389157093285155[5] = 0; - out_2970389157093285155[6] = 0; - out_2970389157093285155[7] = 0; - out_2970389157093285155[8] = 0; - out_2970389157093285155[9] = 0; - out_2970389157093285155[10] = 1; - out_2970389157093285155[11] = 0; - out_2970389157093285155[12] = 0; - out_2970389157093285155[13] = 0; - out_2970389157093285155[14] = 0; - out_2970389157093285155[15] = 0; - out_2970389157093285155[16] = 0; - out_2970389157093285155[17] = 0; - out_2970389157093285155[18] = 0; - out_2970389157093285155[19] = 0; - out_2970389157093285155[20] = 1; - out_2970389157093285155[21] = 0; - out_2970389157093285155[22] = 0; - out_2970389157093285155[23] = 0; - out_2970389157093285155[24] = 0; - out_2970389157093285155[25] = 0; - out_2970389157093285155[26] = 0; - out_2970389157093285155[27] = 0; - out_2970389157093285155[28] = 0; - out_2970389157093285155[29] = 0; - out_2970389157093285155[30] = 1; - out_2970389157093285155[31] = 0; - out_2970389157093285155[32] = 0; - out_2970389157093285155[33] = 0; - out_2970389157093285155[34] = 0; - out_2970389157093285155[35] = 0; - out_2970389157093285155[36] = 0; - out_2970389157093285155[37] = 0; - out_2970389157093285155[38] = 0; - out_2970389157093285155[39] = 0; - out_2970389157093285155[40] = 1; - out_2970389157093285155[41] = 0; - out_2970389157093285155[42] = 0; - out_2970389157093285155[43] = 0; - out_2970389157093285155[44] = 0; - out_2970389157093285155[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_2970389157093285155[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_2970389157093285155[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2970389157093285155[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2970389157093285155[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_2970389157093285155[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_2970389157093285155[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_2970389157093285155[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_2970389157093285155[53] = -9.8100000000000005*dt; - out_2970389157093285155[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_2970389157093285155[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_2970389157093285155[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2970389157093285155[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2970389157093285155[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_2970389157093285155[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_2970389157093285155[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_2970389157093285155[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2970389157093285155[62] = 0; - out_2970389157093285155[63] = 0; - out_2970389157093285155[64] = 0; - out_2970389157093285155[65] = 0; - out_2970389157093285155[66] = 0; - out_2970389157093285155[67] = 0; - out_2970389157093285155[68] = 0; - out_2970389157093285155[69] = 0; - out_2970389157093285155[70] = 1; - out_2970389157093285155[71] = 0; - out_2970389157093285155[72] = 0; - out_2970389157093285155[73] = 0; - out_2970389157093285155[74] = 0; - out_2970389157093285155[75] = 0; - out_2970389157093285155[76] = 0; - out_2970389157093285155[77] = 0; - out_2970389157093285155[78] = 0; - out_2970389157093285155[79] = 0; - out_2970389157093285155[80] = 1; +void F_fun(double *state, double dt, double *out_5308189961048980992) { + out_5308189961048980992[0] = 1; + out_5308189961048980992[1] = 0; + out_5308189961048980992[2] = 0; + out_5308189961048980992[3] = 0; + out_5308189961048980992[4] = 0; + out_5308189961048980992[5] = 0; + out_5308189961048980992[6] = 0; + out_5308189961048980992[7] = 0; + out_5308189961048980992[8] = 0; + out_5308189961048980992[9] = 0; + out_5308189961048980992[10] = 1; + out_5308189961048980992[11] = 0; + out_5308189961048980992[12] = 0; + out_5308189961048980992[13] = 0; + out_5308189961048980992[14] = 0; + out_5308189961048980992[15] = 0; + out_5308189961048980992[16] = 0; + out_5308189961048980992[17] = 0; + out_5308189961048980992[18] = 0; + out_5308189961048980992[19] = 0; + out_5308189961048980992[20] = 1; + out_5308189961048980992[21] = 0; + out_5308189961048980992[22] = 0; + out_5308189961048980992[23] = 0; + out_5308189961048980992[24] = 0; + out_5308189961048980992[25] = 0; + out_5308189961048980992[26] = 0; + out_5308189961048980992[27] = 0; + out_5308189961048980992[28] = 0; + out_5308189961048980992[29] = 0; + out_5308189961048980992[30] = 1; + out_5308189961048980992[31] = 0; + out_5308189961048980992[32] = 0; + out_5308189961048980992[33] = 0; + out_5308189961048980992[34] = 0; + out_5308189961048980992[35] = 0; + out_5308189961048980992[36] = 0; + out_5308189961048980992[37] = 0; + out_5308189961048980992[38] = 0; + out_5308189961048980992[39] = 0; + out_5308189961048980992[40] = 1; + out_5308189961048980992[41] = 0; + out_5308189961048980992[42] = 0; + out_5308189961048980992[43] = 0; + out_5308189961048980992[44] = 0; + out_5308189961048980992[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_5308189961048980992[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_5308189961048980992[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_5308189961048980992[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_5308189961048980992[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_5308189961048980992[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_5308189961048980992[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_5308189961048980992[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_5308189961048980992[53] = -9.8100000000000005*dt; + out_5308189961048980992[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_5308189961048980992[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_5308189961048980992[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_5308189961048980992[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_5308189961048980992[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_5308189961048980992[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_5308189961048980992[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_5308189961048980992[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_5308189961048980992[62] = 0; + out_5308189961048980992[63] = 0; + out_5308189961048980992[64] = 0; + out_5308189961048980992[65] = 0; + out_5308189961048980992[66] = 0; + out_5308189961048980992[67] = 0; + out_5308189961048980992[68] = 0; + out_5308189961048980992[69] = 0; + out_5308189961048980992[70] = 1; + out_5308189961048980992[71] = 0; + out_5308189961048980992[72] = 0; + out_5308189961048980992[73] = 0; + out_5308189961048980992[74] = 0; + out_5308189961048980992[75] = 0; + out_5308189961048980992[76] = 0; + out_5308189961048980992[77] = 0; + out_5308189961048980992[78] = 0; + out_5308189961048980992[79] = 0; + out_5308189961048980992[80] = 1; } -void h_25(double *state, double *unused, double *out_3102286071066017000) { - out_3102286071066017000[0] = state[6]; +void h_25(double *state, double *unused, double *out_6353754508244043817) { + out_6353754508244043817[0] = state[6]; } -void H_25(double *state, double *unused, double *out_7624833147842353287) { - out_7624833147842353287[0] = 0; - out_7624833147842353287[1] = 0; - out_7624833147842353287[2] = 0; - out_7624833147842353287[3] = 0; - out_7624833147842353287[4] = 0; - out_7624833147842353287[5] = 0; - out_7624833147842353287[6] = 1; - out_7624833147842353287[7] = 0; - out_7624833147842353287[8] = 0; +void H_25(double *state, double *unused, double *out_6338657739846148496) { + out_6338657739846148496[0] = 0; + out_6338657739846148496[1] = 0; + out_6338657739846148496[2] = 0; + out_6338657739846148496[3] = 0; + out_6338657739846148496[4] = 0; + out_6338657739846148496[5] = 0; + out_6338657739846148496[6] = 1; + out_6338657739846148496[7] = 0; + out_6338657739846148496[8] = 0; } -void h_24(double *state, double *unused, double *out_423096547607875781) { - out_423096547607875781[0] = state[4]; - out_423096547607875781[1] = state[5]; +void h_24(double *state, double *unused, double *out_2065669686396091997) { + out_2065669686396091997[0] = state[4]; + out_2065669686396091997[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5399125363863484725) { - out_5399125363863484725[0] = 0; - out_5399125363863484725[1] = 0; - out_5399125363863484725[2] = 0; - out_5399125363863484725[3] = 0; - out_5399125363863484725[4] = 1; - out_5399125363863484725[5] = 0; - out_5399125363863484725[6] = 0; - out_5399125363863484725[7] = 0; - out_5399125363863484725[8] = 0; - out_5399125363863484725[9] = 0; - out_5399125363863484725[10] = 0; - out_5399125363863484725[11] = 0; - out_5399125363863484725[12] = 0; - out_5399125363863484725[13] = 0; - out_5399125363863484725[14] = 1; - out_5399125363863484725[15] = 0; - out_5399125363863484725[16] = 0; - out_5399125363863484725[17] = 0; +void H_24(double *state, double *unused, double *out_1518336235190160233) { + out_1518336235190160233[0] = 0; + out_1518336235190160233[1] = 0; + out_1518336235190160233[2] = 0; + out_1518336235190160233[3] = 0; + out_1518336235190160233[4] = 1; + out_1518336235190160233[5] = 0; + out_1518336235190160233[6] = 0; + out_1518336235190160233[7] = 0; + out_1518336235190160233[8] = 0; + out_1518336235190160233[9] = 0; + out_1518336235190160233[10] = 0; + out_1518336235190160233[11] = 0; + out_1518336235190160233[12] = 0; + out_1518336235190160233[13] = 0; + out_1518336235190160233[14] = 1; + out_1518336235190160233[15] = 0; + out_1518336235190160233[16] = 0; + out_1518336235190160233[17] = 0; } -void h_30(double *state, double *unused, double *out_3668549155284333936) { - out_3668549155284333936[0] = state[4]; +void h_30(double *state, double *unused, double *out_4759906320160358758) { + out_4759906320160358758[0] = state[4]; } -void H_30(double *state, double *unused, double *out_708142806350736532) { - out_708142806350736532[0] = 0; - out_708142806350736532[1] = 0; - out_708142806350736532[2] = 0; - out_708142806350736532[3] = 0; - out_708142806350736532[4] = 1; - out_708142806350736532[5] = 0; - out_708142806350736532[6] = 0; - out_708142806350736532[7] = 0; - out_708142806350736532[8] = 0; +void H_30(double *state, double *unused, double *out_6209318792702908426) { + out_6209318792702908426[0] = 0; + out_6209318792702908426[1] = 0; + out_6209318792702908426[2] = 0; + out_6209318792702908426[3] = 0; + out_6209318792702908426[4] = 1; + out_6209318792702908426[5] = 0; + out_6209318792702908426[6] = 0; + out_6209318792702908426[7] = 0; + out_6209318792702908426[8] = 0; } -void h_26(double *state, double *unused, double *out_7985951944793485901) { - out_7985951944793485901[0] = state[7]; +void h_26(double *state, double *unused, double *out_163294403103182073) { + out_163294403103182073[0] = state[7]; } -void H_26(double *state, double *unused, double *out_7080407606993142105) { - out_7080407606993142105[0] = 0; - out_7080407606993142105[1] = 0; - out_7080407606993142105[2] = 0; - out_7080407606993142105[3] = 0; - out_7080407606993142105[4] = 0; - out_7080407606993142105[5] = 0; - out_7080407606993142105[6] = 0; - out_7080407606993142105[7] = 1; - out_7080407606993142105[8] = 0; +void H_26(double *state, double *unused, double *out_2597154420972092272) { + out_2597154420972092272[0] = 0; + out_2597154420972092272[1] = 0; + out_2597154420972092272[2] = 0; + out_2597154420972092272[3] = 0; + out_2597154420972092272[4] = 0; + out_2597154420972092272[5] = 0; + out_2597154420972092272[6] = 0; + out_2597154420972092272[7] = 1; + out_2597154420972092272[8] = 0; } -void h_27(double *state, double *unused, double *out_8103959338726862645) { - out_8103959338726862645[0] = state[3]; +void h_27(double *state, double *unused, double *out_45287009169805329) { + out_45287009169805329[0] = state[3]; } -void H_27(double *state, double *unused, double *out_2882906118151161443) { - out_2882906118151161443[0] = 0; - out_2882906118151161443[1] = 0; - out_2882906118151161443[2] = 0; - out_2882906118151161443[3] = 1; - out_2882906118151161443[4] = 0; - out_2882906118151161443[5] = 0; - out_2882906118151161443[6] = 0; - out_2882906118151161443[7] = 0; - out_2882906118151161443[8] = 0; +void H_27(double *state, double *unused, double *out_4034555480902483515) { + out_4034555480902483515[0] = 0; + out_4034555480902483515[1] = 0; + out_4034555480902483515[2] = 0; + out_4034555480902483515[3] = 1; + out_4034555480902483515[4] = 0; + out_4034555480902483515[5] = 0; + out_4034555480902483515[6] = 0; + out_4034555480902483515[7] = 0; + out_4034555480902483515[8] = 0; } -void h_29(double *state, double *unused, double *out_6653327946097979478) { - out_6653327946097979478[0] = state[1]; +void h_29(double *state, double *unused, double *out_4143590307449177193) { + out_4143590307449177193[0] = state[1]; } -void H_29(double *state, double *unused, double *out_197911462036344348) { - out_197911462036344348[0] = 0; - out_197911462036344348[1] = 1; - out_197911462036344348[2] = 0; - out_197911462036344348[3] = 0; - out_197911462036344348[4] = 0; - out_197911462036344348[5] = 0; - out_197911462036344348[6] = 0; - out_197911462036344348[7] = 0; - out_197911462036344348[8] = 0; +void H_29(double *state, double *unused, double *out_6719550137017300610) { + out_6719550137017300610[0] = 0; + out_6719550137017300610[1] = 1; + out_6719550137017300610[2] = 0; + out_6719550137017300610[3] = 0; + out_6719550137017300610[4] = 0; + out_6719550137017300610[5] = 0; + out_6719550137017300610[6] = 0; + out_6719550137017300610[7] = 0; + out_6719550137017300610[8] = 0; } -void h_28(double *state, double *unused, double *out_5499554017918164994) { - out_5499554017918164994[0] = state[0]; +void h_28(double *state, double *unused, double *out_8542875672635036079) { + out_8542875672635036079[0] = state[0]; } -void H_28(double *state, double *unused, double *out_8768076211619308566) { - out_8768076211619308566[0] = 1; - out_8768076211619308566[1] = 0; - out_8768076211619308566[2] = 0; - out_8768076211619308566[3] = 0; - out_8768076211619308566[4] = 0; - out_8768076211619308566[5] = 0; - out_8768076211619308566[6] = 0; - out_8768076211619308566[7] = 0; - out_8768076211619308566[8] = 0; +void H_28(double *state, double *unused, double *out_1637151119947770036) { + out_1637151119947770036[0] = 1; + out_1637151119947770036[1] = 0; + out_1637151119947770036[2] = 0; + out_1637151119947770036[3] = 0; + out_1637151119947770036[4] = 0; + out_1637151119947770036[5] = 0; + out_1637151119947770036[6] = 0; + out_1637151119947770036[7] = 0; + out_1637151119947770036[8] = 0; } -void h_31(double *state, double *unused, double *out_7581192745822862175) { - out_7581192745822862175[0] = state[8]; +void h_31(double *state, double *unused, double *out_5322154339115156863) { + out_5322154339115156863[0] = state[8]; } -void H_31(double *state, double *unused, double *out_7594187185965392859) { - out_7594187185965392859[0] = 0; - out_7594187185965392859[1] = 0; - out_7594187185965392859[2] = 0; - out_7594187185965392859[3] = 0; - out_7594187185965392859[4] = 0; - out_7594187185965392859[5] = 0; - out_7594187185965392859[6] = 0; - out_7594187185965392859[7] = 0; - out_7594187185965392859[8] = 1; +void H_31(double *state, double *unused, double *out_6369303701723108924) { + out_6369303701723108924[0] = 0; + out_6369303701723108924[1] = 0; + out_6369303701723108924[2] = 0; + out_6369303701723108924[3] = 0; + out_6369303701723108924[4] = 0; + out_6369303701723108924[5] = 0; + out_6369303701723108924[6] = 0; + out_6369303701723108924[7] = 0; + out_6369303701723108924[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_4102855695027319146) { - err_fun(nom_x, delta_x, out_4102855695027319146); +void car_err_fun(double *nom_x, double *delta_x, double *out_8250891246740759679) { + err_fun(nom_x, delta_x, out_8250891246740759679); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_375370046560025251) { - inv_err_fun(nom_x, true_x, out_375370046560025251); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_6699306425357192023) { + inv_err_fun(nom_x, true_x, out_6699306425357192023); } -void car_H_mod_fun(double *state, double *out_4142214806030893418) { - H_mod_fun(state, out_4142214806030893418); +void car_H_mod_fun(double *state, double *out_2914898107951165883) { + H_mod_fun(state, out_2914898107951165883); } -void car_f_fun(double *state, double dt, double *out_1332366097619626209) { - f_fun(state, dt, out_1332366097619626209); +void car_f_fun(double *state, double dt, double *out_2675178968316523152) { + f_fun(state, dt, out_2675178968316523152); } -void car_F_fun(double *state, double dt, double *out_2970389157093285155) { - F_fun(state, dt, out_2970389157093285155); +void car_F_fun(double *state, double dt, double *out_5308189961048980992) { + F_fun(state, dt, out_5308189961048980992); } -void car_h_25(double *state, double *unused, double *out_3102286071066017000) { - h_25(state, unused, out_3102286071066017000); +void car_h_25(double *state, double *unused, double *out_6353754508244043817) { + h_25(state, unused, out_6353754508244043817); } -void car_H_25(double *state, double *unused, double *out_7624833147842353287) { - H_25(state, unused, out_7624833147842353287); +void car_H_25(double *state, double *unused, double *out_6338657739846148496) { + H_25(state, unused, out_6338657739846148496); } -void car_h_24(double *state, double *unused, double *out_423096547607875781) { - h_24(state, unused, out_423096547607875781); +void car_h_24(double *state, double *unused, double *out_2065669686396091997) { + h_24(state, unused, out_2065669686396091997); } -void car_H_24(double *state, double *unused, double *out_5399125363863484725) { - H_24(state, unused, out_5399125363863484725); +void car_H_24(double *state, double *unused, double *out_1518336235190160233) { + H_24(state, unused, out_1518336235190160233); } -void car_h_30(double *state, double *unused, double *out_3668549155284333936) { - h_30(state, unused, out_3668549155284333936); +void car_h_30(double *state, double *unused, double *out_4759906320160358758) { + h_30(state, unused, out_4759906320160358758); } -void car_H_30(double *state, double *unused, double *out_708142806350736532) { - H_30(state, unused, out_708142806350736532); +void car_H_30(double *state, double *unused, double *out_6209318792702908426) { + H_30(state, unused, out_6209318792702908426); } -void car_h_26(double *state, double *unused, double *out_7985951944793485901) { - h_26(state, unused, out_7985951944793485901); +void car_h_26(double *state, double *unused, double *out_163294403103182073) { + h_26(state, unused, out_163294403103182073); } -void car_H_26(double *state, double *unused, double *out_7080407606993142105) { - H_26(state, unused, out_7080407606993142105); +void car_H_26(double *state, double *unused, double *out_2597154420972092272) { + H_26(state, unused, out_2597154420972092272); } -void car_h_27(double *state, double *unused, double *out_8103959338726862645) { - h_27(state, unused, out_8103959338726862645); +void car_h_27(double *state, double *unused, double *out_45287009169805329) { + h_27(state, unused, out_45287009169805329); } -void car_H_27(double *state, double *unused, double *out_2882906118151161443) { - H_27(state, unused, out_2882906118151161443); +void car_H_27(double *state, double *unused, double *out_4034555480902483515) { + H_27(state, unused, out_4034555480902483515); } -void car_h_29(double *state, double *unused, double *out_6653327946097979478) { - h_29(state, unused, out_6653327946097979478); +void car_h_29(double *state, double *unused, double *out_4143590307449177193) { + h_29(state, unused, out_4143590307449177193); } -void car_H_29(double *state, double *unused, double *out_197911462036344348) { - H_29(state, unused, out_197911462036344348); +void car_H_29(double *state, double *unused, double *out_6719550137017300610) { + H_29(state, unused, out_6719550137017300610); } -void car_h_28(double *state, double *unused, double *out_5499554017918164994) { - h_28(state, unused, out_5499554017918164994); +void car_h_28(double *state, double *unused, double *out_8542875672635036079) { + h_28(state, unused, out_8542875672635036079); } -void car_H_28(double *state, double *unused, double *out_8768076211619308566) { - H_28(state, unused, out_8768076211619308566); +void car_H_28(double *state, double *unused, double *out_1637151119947770036) { + H_28(state, unused, out_1637151119947770036); } -void car_h_31(double *state, double *unused, double *out_7581192745822862175) { - h_31(state, unused, out_7581192745822862175); +void car_h_31(double *state, double *unused, double *out_5322154339115156863) { + h_31(state, unused, out_5322154339115156863); } -void car_H_31(double *state, double *unused, double *out_7594187185965392859) { - H_31(state, unused, out_7594187185965392859); +void car_H_31(double *state, double *unused, double *out_6369303701723108924) { + H_31(state, unused, out_6369303701723108924); } 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 6452464c1..7ae397a61 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_4102855695027319146); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_375370046560025251); -void car_H_mod_fun(double *state, double *out_4142214806030893418); -void car_f_fun(double *state, double dt, double *out_1332366097619626209); -void car_F_fun(double *state, double dt, double *out_2970389157093285155); -void car_h_25(double *state, double *unused, double *out_3102286071066017000); -void car_H_25(double *state, double *unused, double *out_7624833147842353287); -void car_h_24(double *state, double *unused, double *out_423096547607875781); -void car_H_24(double *state, double *unused, double *out_5399125363863484725); -void car_h_30(double *state, double *unused, double *out_3668549155284333936); -void car_H_30(double *state, double *unused, double *out_708142806350736532); -void car_h_26(double *state, double *unused, double *out_7985951944793485901); -void car_H_26(double *state, double *unused, double *out_7080407606993142105); -void car_h_27(double *state, double *unused, double *out_8103959338726862645); -void car_H_27(double *state, double *unused, double *out_2882906118151161443); -void car_h_29(double *state, double *unused, double *out_6653327946097979478); -void car_H_29(double *state, double *unused, double *out_197911462036344348); -void car_h_28(double *state, double *unused, double *out_5499554017918164994); -void car_H_28(double *state, double *unused, double *out_8768076211619308566); -void car_h_31(double *state, double *unused, double *out_7581192745822862175); -void car_H_31(double *state, double *unused, double *out_7594187185965392859); +void car_err_fun(double *nom_x, double *delta_x, double *out_8250891246740759679); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_6699306425357192023); +void car_H_mod_fun(double *state, double *out_2914898107951165883); +void car_f_fun(double *state, double dt, double *out_2675178968316523152); +void car_F_fun(double *state, double dt, double *out_5308189961048980992); +void car_h_25(double *state, double *unused, double *out_6353754508244043817); +void car_H_25(double *state, double *unused, double *out_6338657739846148496); +void car_h_24(double *state, double *unused, double *out_2065669686396091997); +void car_H_24(double *state, double *unused, double *out_1518336235190160233); +void car_h_30(double *state, double *unused, double *out_4759906320160358758); +void car_H_30(double *state, double *unused, double *out_6209318792702908426); +void car_h_26(double *state, double *unused, double *out_163294403103182073); +void car_H_26(double *state, double *unused, double *out_2597154420972092272); +void car_h_27(double *state, double *unused, double *out_45287009169805329); +void car_H_27(double *state, double *unused, double *out_4034555480902483515); +void car_h_29(double *state, double *unused, double *out_4143590307449177193); +void car_H_29(double *state, double *unused, double *out_6719550137017300610); +void car_h_28(double *state, double *unused, double *out_8542875672635036079); +void car_H_28(double *state, double *unused, double *out_1637151119947770036); +void car_h_31(double *state, double *unused, double *out_5322154339115156863); +void car_H_31(double *state, double *unused, double *out_6369303701723108924); 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 a794930ff..62b453811 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_8979246808243312501) { - out_8979246808243312501[0] = delta_x[0] + nom_x[0]; - out_8979246808243312501[1] = delta_x[1] + nom_x[1]; - out_8979246808243312501[2] = delta_x[2] + nom_x[2]; - out_8979246808243312501[3] = delta_x[3] + nom_x[3]; - out_8979246808243312501[4] = delta_x[4] + nom_x[4]; - out_8979246808243312501[5] = delta_x[5] + nom_x[5]; - out_8979246808243312501[6] = delta_x[6] + nom_x[6]; - out_8979246808243312501[7] = delta_x[7] + nom_x[7]; - out_8979246808243312501[8] = delta_x[8] + nom_x[8]; - out_8979246808243312501[9] = delta_x[9] + nom_x[9]; - out_8979246808243312501[10] = delta_x[10] + nom_x[10]; - out_8979246808243312501[11] = delta_x[11] + nom_x[11]; - out_8979246808243312501[12] = delta_x[12] + nom_x[12]; - out_8979246808243312501[13] = delta_x[13] + nom_x[13]; - out_8979246808243312501[14] = delta_x[14] + nom_x[14]; - out_8979246808243312501[15] = delta_x[15] + nom_x[15]; - out_8979246808243312501[16] = delta_x[16] + nom_x[16]; - out_8979246808243312501[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_1942165964233553274) { + out_1942165964233553274[0] = delta_x[0] + nom_x[0]; + out_1942165964233553274[1] = delta_x[1] + nom_x[1]; + out_1942165964233553274[2] = delta_x[2] + nom_x[2]; + out_1942165964233553274[3] = delta_x[3] + nom_x[3]; + out_1942165964233553274[4] = delta_x[4] + nom_x[4]; + out_1942165964233553274[5] = delta_x[5] + nom_x[5]; + out_1942165964233553274[6] = delta_x[6] + nom_x[6]; + out_1942165964233553274[7] = delta_x[7] + nom_x[7]; + out_1942165964233553274[8] = delta_x[8] + nom_x[8]; + out_1942165964233553274[9] = delta_x[9] + nom_x[9]; + out_1942165964233553274[10] = delta_x[10] + nom_x[10]; + out_1942165964233553274[11] = delta_x[11] + nom_x[11]; + out_1942165964233553274[12] = delta_x[12] + nom_x[12]; + out_1942165964233553274[13] = delta_x[13] + nom_x[13]; + out_1942165964233553274[14] = delta_x[14] + nom_x[14]; + out_1942165964233553274[15] = delta_x[15] + nom_x[15]; + out_1942165964233553274[16] = delta_x[16] + nom_x[16]; + out_1942165964233553274[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_6182129478777519212) { - out_6182129478777519212[0] = -nom_x[0] + true_x[0]; - out_6182129478777519212[1] = -nom_x[1] + true_x[1]; - out_6182129478777519212[2] = -nom_x[2] + true_x[2]; - out_6182129478777519212[3] = -nom_x[3] + true_x[3]; - out_6182129478777519212[4] = -nom_x[4] + true_x[4]; - out_6182129478777519212[5] = -nom_x[5] + true_x[5]; - out_6182129478777519212[6] = -nom_x[6] + true_x[6]; - out_6182129478777519212[7] = -nom_x[7] + true_x[7]; - out_6182129478777519212[8] = -nom_x[8] + true_x[8]; - out_6182129478777519212[9] = -nom_x[9] + true_x[9]; - out_6182129478777519212[10] = -nom_x[10] + true_x[10]; - out_6182129478777519212[11] = -nom_x[11] + true_x[11]; - out_6182129478777519212[12] = -nom_x[12] + true_x[12]; - out_6182129478777519212[13] = -nom_x[13] + true_x[13]; - out_6182129478777519212[14] = -nom_x[14] + true_x[14]; - out_6182129478777519212[15] = -nom_x[15] + true_x[15]; - out_6182129478777519212[16] = -nom_x[16] + true_x[16]; - out_6182129478777519212[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2238116153961167505) { + out_2238116153961167505[0] = -nom_x[0] + true_x[0]; + out_2238116153961167505[1] = -nom_x[1] + true_x[1]; + out_2238116153961167505[2] = -nom_x[2] + true_x[2]; + out_2238116153961167505[3] = -nom_x[3] + true_x[3]; + out_2238116153961167505[4] = -nom_x[4] + true_x[4]; + out_2238116153961167505[5] = -nom_x[5] + true_x[5]; + out_2238116153961167505[6] = -nom_x[6] + true_x[6]; + out_2238116153961167505[7] = -nom_x[7] + true_x[7]; + out_2238116153961167505[8] = -nom_x[8] + true_x[8]; + out_2238116153961167505[9] = -nom_x[9] + true_x[9]; + out_2238116153961167505[10] = -nom_x[10] + true_x[10]; + out_2238116153961167505[11] = -nom_x[11] + true_x[11]; + out_2238116153961167505[12] = -nom_x[12] + true_x[12]; + out_2238116153961167505[13] = -nom_x[13] + true_x[13]; + out_2238116153961167505[14] = -nom_x[14] + true_x[14]; + out_2238116153961167505[15] = -nom_x[15] + true_x[15]; + out_2238116153961167505[16] = -nom_x[16] + true_x[16]; + out_2238116153961167505[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_5496809482113736276) { - out_5496809482113736276[0] = 1.0; - out_5496809482113736276[1] = 0.0; - out_5496809482113736276[2] = 0.0; - out_5496809482113736276[3] = 0.0; - out_5496809482113736276[4] = 0.0; - out_5496809482113736276[5] = 0.0; - out_5496809482113736276[6] = 0.0; - out_5496809482113736276[7] = 0.0; - out_5496809482113736276[8] = 0.0; - out_5496809482113736276[9] = 0.0; - out_5496809482113736276[10] = 0.0; - out_5496809482113736276[11] = 0.0; - out_5496809482113736276[12] = 0.0; - out_5496809482113736276[13] = 0.0; - out_5496809482113736276[14] = 0.0; - out_5496809482113736276[15] = 0.0; - out_5496809482113736276[16] = 0.0; - out_5496809482113736276[17] = 0.0; - out_5496809482113736276[18] = 0.0; - out_5496809482113736276[19] = 1.0; - out_5496809482113736276[20] = 0.0; - out_5496809482113736276[21] = 0.0; - out_5496809482113736276[22] = 0.0; - out_5496809482113736276[23] = 0.0; - out_5496809482113736276[24] = 0.0; - out_5496809482113736276[25] = 0.0; - out_5496809482113736276[26] = 0.0; - out_5496809482113736276[27] = 0.0; - out_5496809482113736276[28] = 0.0; - out_5496809482113736276[29] = 0.0; - out_5496809482113736276[30] = 0.0; - out_5496809482113736276[31] = 0.0; - out_5496809482113736276[32] = 0.0; - out_5496809482113736276[33] = 0.0; - out_5496809482113736276[34] = 0.0; - out_5496809482113736276[35] = 0.0; - out_5496809482113736276[36] = 0.0; - out_5496809482113736276[37] = 0.0; - out_5496809482113736276[38] = 1.0; - out_5496809482113736276[39] = 0.0; - out_5496809482113736276[40] = 0.0; - out_5496809482113736276[41] = 0.0; - out_5496809482113736276[42] = 0.0; - out_5496809482113736276[43] = 0.0; - out_5496809482113736276[44] = 0.0; - out_5496809482113736276[45] = 0.0; - out_5496809482113736276[46] = 0.0; - out_5496809482113736276[47] = 0.0; - out_5496809482113736276[48] = 0.0; - out_5496809482113736276[49] = 0.0; - out_5496809482113736276[50] = 0.0; - out_5496809482113736276[51] = 0.0; - out_5496809482113736276[52] = 0.0; - out_5496809482113736276[53] = 0.0; - out_5496809482113736276[54] = 0.0; - out_5496809482113736276[55] = 0.0; - out_5496809482113736276[56] = 0.0; - out_5496809482113736276[57] = 1.0; - out_5496809482113736276[58] = 0.0; - out_5496809482113736276[59] = 0.0; - out_5496809482113736276[60] = 0.0; - out_5496809482113736276[61] = 0.0; - out_5496809482113736276[62] = 0.0; - out_5496809482113736276[63] = 0.0; - out_5496809482113736276[64] = 0.0; - out_5496809482113736276[65] = 0.0; - out_5496809482113736276[66] = 0.0; - out_5496809482113736276[67] = 0.0; - out_5496809482113736276[68] = 0.0; - out_5496809482113736276[69] = 0.0; - out_5496809482113736276[70] = 0.0; - out_5496809482113736276[71] = 0.0; - out_5496809482113736276[72] = 0.0; - out_5496809482113736276[73] = 0.0; - out_5496809482113736276[74] = 0.0; - out_5496809482113736276[75] = 0.0; - out_5496809482113736276[76] = 1.0; - out_5496809482113736276[77] = 0.0; - out_5496809482113736276[78] = 0.0; - out_5496809482113736276[79] = 0.0; - out_5496809482113736276[80] = 0.0; - out_5496809482113736276[81] = 0.0; - out_5496809482113736276[82] = 0.0; - out_5496809482113736276[83] = 0.0; - out_5496809482113736276[84] = 0.0; - out_5496809482113736276[85] = 0.0; - out_5496809482113736276[86] = 0.0; - out_5496809482113736276[87] = 0.0; - out_5496809482113736276[88] = 0.0; - out_5496809482113736276[89] = 0.0; - out_5496809482113736276[90] = 0.0; - out_5496809482113736276[91] = 0.0; - out_5496809482113736276[92] = 0.0; - out_5496809482113736276[93] = 0.0; - out_5496809482113736276[94] = 0.0; - out_5496809482113736276[95] = 1.0; - out_5496809482113736276[96] = 0.0; - out_5496809482113736276[97] = 0.0; - out_5496809482113736276[98] = 0.0; - out_5496809482113736276[99] = 0.0; - out_5496809482113736276[100] = 0.0; - out_5496809482113736276[101] = 0.0; - out_5496809482113736276[102] = 0.0; - out_5496809482113736276[103] = 0.0; - out_5496809482113736276[104] = 0.0; - out_5496809482113736276[105] = 0.0; - out_5496809482113736276[106] = 0.0; - out_5496809482113736276[107] = 0.0; - out_5496809482113736276[108] = 0.0; - out_5496809482113736276[109] = 0.0; - out_5496809482113736276[110] = 0.0; - out_5496809482113736276[111] = 0.0; - out_5496809482113736276[112] = 0.0; - out_5496809482113736276[113] = 0.0; - out_5496809482113736276[114] = 1.0; - out_5496809482113736276[115] = 0.0; - out_5496809482113736276[116] = 0.0; - out_5496809482113736276[117] = 0.0; - out_5496809482113736276[118] = 0.0; - out_5496809482113736276[119] = 0.0; - out_5496809482113736276[120] = 0.0; - out_5496809482113736276[121] = 0.0; - out_5496809482113736276[122] = 0.0; - out_5496809482113736276[123] = 0.0; - out_5496809482113736276[124] = 0.0; - out_5496809482113736276[125] = 0.0; - out_5496809482113736276[126] = 0.0; - out_5496809482113736276[127] = 0.0; - out_5496809482113736276[128] = 0.0; - out_5496809482113736276[129] = 0.0; - out_5496809482113736276[130] = 0.0; - out_5496809482113736276[131] = 0.0; - out_5496809482113736276[132] = 0.0; - out_5496809482113736276[133] = 1.0; - out_5496809482113736276[134] = 0.0; - out_5496809482113736276[135] = 0.0; - out_5496809482113736276[136] = 0.0; - out_5496809482113736276[137] = 0.0; - out_5496809482113736276[138] = 0.0; - out_5496809482113736276[139] = 0.0; - out_5496809482113736276[140] = 0.0; - out_5496809482113736276[141] = 0.0; - out_5496809482113736276[142] = 0.0; - out_5496809482113736276[143] = 0.0; - out_5496809482113736276[144] = 0.0; - out_5496809482113736276[145] = 0.0; - out_5496809482113736276[146] = 0.0; - out_5496809482113736276[147] = 0.0; - out_5496809482113736276[148] = 0.0; - out_5496809482113736276[149] = 0.0; - out_5496809482113736276[150] = 0.0; - out_5496809482113736276[151] = 0.0; - out_5496809482113736276[152] = 1.0; - out_5496809482113736276[153] = 0.0; - out_5496809482113736276[154] = 0.0; - out_5496809482113736276[155] = 0.0; - out_5496809482113736276[156] = 0.0; - out_5496809482113736276[157] = 0.0; - out_5496809482113736276[158] = 0.0; - out_5496809482113736276[159] = 0.0; - out_5496809482113736276[160] = 0.0; - out_5496809482113736276[161] = 0.0; - out_5496809482113736276[162] = 0.0; - out_5496809482113736276[163] = 0.0; - out_5496809482113736276[164] = 0.0; - out_5496809482113736276[165] = 0.0; - out_5496809482113736276[166] = 0.0; - out_5496809482113736276[167] = 0.0; - out_5496809482113736276[168] = 0.0; - out_5496809482113736276[169] = 0.0; - out_5496809482113736276[170] = 0.0; - out_5496809482113736276[171] = 1.0; - out_5496809482113736276[172] = 0.0; - out_5496809482113736276[173] = 0.0; - out_5496809482113736276[174] = 0.0; - out_5496809482113736276[175] = 0.0; - out_5496809482113736276[176] = 0.0; - out_5496809482113736276[177] = 0.0; - out_5496809482113736276[178] = 0.0; - out_5496809482113736276[179] = 0.0; - out_5496809482113736276[180] = 0.0; - out_5496809482113736276[181] = 0.0; - out_5496809482113736276[182] = 0.0; - out_5496809482113736276[183] = 0.0; - out_5496809482113736276[184] = 0.0; - out_5496809482113736276[185] = 0.0; - out_5496809482113736276[186] = 0.0; - out_5496809482113736276[187] = 0.0; - out_5496809482113736276[188] = 0.0; - out_5496809482113736276[189] = 0.0; - out_5496809482113736276[190] = 1.0; - out_5496809482113736276[191] = 0.0; - out_5496809482113736276[192] = 0.0; - out_5496809482113736276[193] = 0.0; - out_5496809482113736276[194] = 0.0; - out_5496809482113736276[195] = 0.0; - out_5496809482113736276[196] = 0.0; - out_5496809482113736276[197] = 0.0; - out_5496809482113736276[198] = 0.0; - out_5496809482113736276[199] = 0.0; - out_5496809482113736276[200] = 0.0; - out_5496809482113736276[201] = 0.0; - out_5496809482113736276[202] = 0.0; - out_5496809482113736276[203] = 0.0; - out_5496809482113736276[204] = 0.0; - out_5496809482113736276[205] = 0.0; - out_5496809482113736276[206] = 0.0; - out_5496809482113736276[207] = 0.0; - out_5496809482113736276[208] = 0.0; - out_5496809482113736276[209] = 1.0; - out_5496809482113736276[210] = 0.0; - out_5496809482113736276[211] = 0.0; - out_5496809482113736276[212] = 0.0; - out_5496809482113736276[213] = 0.0; - out_5496809482113736276[214] = 0.0; - out_5496809482113736276[215] = 0.0; - out_5496809482113736276[216] = 0.0; - out_5496809482113736276[217] = 0.0; - out_5496809482113736276[218] = 0.0; - out_5496809482113736276[219] = 0.0; - out_5496809482113736276[220] = 0.0; - out_5496809482113736276[221] = 0.0; - out_5496809482113736276[222] = 0.0; - out_5496809482113736276[223] = 0.0; - out_5496809482113736276[224] = 0.0; - out_5496809482113736276[225] = 0.0; - out_5496809482113736276[226] = 0.0; - out_5496809482113736276[227] = 0.0; - out_5496809482113736276[228] = 1.0; - out_5496809482113736276[229] = 0.0; - out_5496809482113736276[230] = 0.0; - out_5496809482113736276[231] = 0.0; - out_5496809482113736276[232] = 0.0; - out_5496809482113736276[233] = 0.0; - out_5496809482113736276[234] = 0.0; - out_5496809482113736276[235] = 0.0; - out_5496809482113736276[236] = 0.0; - out_5496809482113736276[237] = 0.0; - out_5496809482113736276[238] = 0.0; - out_5496809482113736276[239] = 0.0; - out_5496809482113736276[240] = 0.0; - out_5496809482113736276[241] = 0.0; - out_5496809482113736276[242] = 0.0; - out_5496809482113736276[243] = 0.0; - out_5496809482113736276[244] = 0.0; - out_5496809482113736276[245] = 0.0; - out_5496809482113736276[246] = 0.0; - out_5496809482113736276[247] = 1.0; - out_5496809482113736276[248] = 0.0; - out_5496809482113736276[249] = 0.0; - out_5496809482113736276[250] = 0.0; - out_5496809482113736276[251] = 0.0; - out_5496809482113736276[252] = 0.0; - out_5496809482113736276[253] = 0.0; - out_5496809482113736276[254] = 0.0; - out_5496809482113736276[255] = 0.0; - out_5496809482113736276[256] = 0.0; - out_5496809482113736276[257] = 0.0; - out_5496809482113736276[258] = 0.0; - out_5496809482113736276[259] = 0.0; - out_5496809482113736276[260] = 0.0; - out_5496809482113736276[261] = 0.0; - out_5496809482113736276[262] = 0.0; - out_5496809482113736276[263] = 0.0; - out_5496809482113736276[264] = 0.0; - out_5496809482113736276[265] = 0.0; - out_5496809482113736276[266] = 1.0; - out_5496809482113736276[267] = 0.0; - out_5496809482113736276[268] = 0.0; - out_5496809482113736276[269] = 0.0; - out_5496809482113736276[270] = 0.0; - out_5496809482113736276[271] = 0.0; - out_5496809482113736276[272] = 0.0; - out_5496809482113736276[273] = 0.0; - out_5496809482113736276[274] = 0.0; - out_5496809482113736276[275] = 0.0; - out_5496809482113736276[276] = 0.0; - out_5496809482113736276[277] = 0.0; - out_5496809482113736276[278] = 0.0; - out_5496809482113736276[279] = 0.0; - out_5496809482113736276[280] = 0.0; - out_5496809482113736276[281] = 0.0; - out_5496809482113736276[282] = 0.0; - out_5496809482113736276[283] = 0.0; - out_5496809482113736276[284] = 0.0; - out_5496809482113736276[285] = 1.0; - out_5496809482113736276[286] = 0.0; - out_5496809482113736276[287] = 0.0; - out_5496809482113736276[288] = 0.0; - out_5496809482113736276[289] = 0.0; - out_5496809482113736276[290] = 0.0; - out_5496809482113736276[291] = 0.0; - out_5496809482113736276[292] = 0.0; - out_5496809482113736276[293] = 0.0; - out_5496809482113736276[294] = 0.0; - out_5496809482113736276[295] = 0.0; - out_5496809482113736276[296] = 0.0; - out_5496809482113736276[297] = 0.0; - out_5496809482113736276[298] = 0.0; - out_5496809482113736276[299] = 0.0; - out_5496809482113736276[300] = 0.0; - out_5496809482113736276[301] = 0.0; - out_5496809482113736276[302] = 0.0; - out_5496809482113736276[303] = 0.0; - out_5496809482113736276[304] = 1.0; - out_5496809482113736276[305] = 0.0; - out_5496809482113736276[306] = 0.0; - out_5496809482113736276[307] = 0.0; - out_5496809482113736276[308] = 0.0; - out_5496809482113736276[309] = 0.0; - out_5496809482113736276[310] = 0.0; - out_5496809482113736276[311] = 0.0; - out_5496809482113736276[312] = 0.0; - out_5496809482113736276[313] = 0.0; - out_5496809482113736276[314] = 0.0; - out_5496809482113736276[315] = 0.0; - out_5496809482113736276[316] = 0.0; - out_5496809482113736276[317] = 0.0; - out_5496809482113736276[318] = 0.0; - out_5496809482113736276[319] = 0.0; - out_5496809482113736276[320] = 0.0; - out_5496809482113736276[321] = 0.0; - out_5496809482113736276[322] = 0.0; - out_5496809482113736276[323] = 1.0; +void H_mod_fun(double *state, double *out_4990084456757481789) { + out_4990084456757481789[0] = 1.0; + out_4990084456757481789[1] = 0.0; + out_4990084456757481789[2] = 0.0; + out_4990084456757481789[3] = 0.0; + out_4990084456757481789[4] = 0.0; + out_4990084456757481789[5] = 0.0; + out_4990084456757481789[6] = 0.0; + out_4990084456757481789[7] = 0.0; + out_4990084456757481789[8] = 0.0; + out_4990084456757481789[9] = 0.0; + out_4990084456757481789[10] = 0.0; + out_4990084456757481789[11] = 0.0; + out_4990084456757481789[12] = 0.0; + out_4990084456757481789[13] = 0.0; + out_4990084456757481789[14] = 0.0; + out_4990084456757481789[15] = 0.0; + out_4990084456757481789[16] = 0.0; + out_4990084456757481789[17] = 0.0; + out_4990084456757481789[18] = 0.0; + out_4990084456757481789[19] = 1.0; + out_4990084456757481789[20] = 0.0; + out_4990084456757481789[21] = 0.0; + out_4990084456757481789[22] = 0.0; + out_4990084456757481789[23] = 0.0; + out_4990084456757481789[24] = 0.0; + out_4990084456757481789[25] = 0.0; + out_4990084456757481789[26] = 0.0; + out_4990084456757481789[27] = 0.0; + out_4990084456757481789[28] = 0.0; + out_4990084456757481789[29] = 0.0; + out_4990084456757481789[30] = 0.0; + out_4990084456757481789[31] = 0.0; + out_4990084456757481789[32] = 0.0; + out_4990084456757481789[33] = 0.0; + out_4990084456757481789[34] = 0.0; + out_4990084456757481789[35] = 0.0; + out_4990084456757481789[36] = 0.0; + out_4990084456757481789[37] = 0.0; + out_4990084456757481789[38] = 1.0; + out_4990084456757481789[39] = 0.0; + out_4990084456757481789[40] = 0.0; + out_4990084456757481789[41] = 0.0; + out_4990084456757481789[42] = 0.0; + out_4990084456757481789[43] = 0.0; + out_4990084456757481789[44] = 0.0; + out_4990084456757481789[45] = 0.0; + out_4990084456757481789[46] = 0.0; + out_4990084456757481789[47] = 0.0; + out_4990084456757481789[48] = 0.0; + out_4990084456757481789[49] = 0.0; + out_4990084456757481789[50] = 0.0; + out_4990084456757481789[51] = 0.0; + out_4990084456757481789[52] = 0.0; + out_4990084456757481789[53] = 0.0; + out_4990084456757481789[54] = 0.0; + out_4990084456757481789[55] = 0.0; + out_4990084456757481789[56] = 0.0; + out_4990084456757481789[57] = 1.0; + out_4990084456757481789[58] = 0.0; + out_4990084456757481789[59] = 0.0; + out_4990084456757481789[60] = 0.0; + out_4990084456757481789[61] = 0.0; + out_4990084456757481789[62] = 0.0; + out_4990084456757481789[63] = 0.0; + out_4990084456757481789[64] = 0.0; + out_4990084456757481789[65] = 0.0; + out_4990084456757481789[66] = 0.0; + out_4990084456757481789[67] = 0.0; + out_4990084456757481789[68] = 0.0; + out_4990084456757481789[69] = 0.0; + out_4990084456757481789[70] = 0.0; + out_4990084456757481789[71] = 0.0; + out_4990084456757481789[72] = 0.0; + out_4990084456757481789[73] = 0.0; + out_4990084456757481789[74] = 0.0; + out_4990084456757481789[75] = 0.0; + out_4990084456757481789[76] = 1.0; + out_4990084456757481789[77] = 0.0; + out_4990084456757481789[78] = 0.0; + out_4990084456757481789[79] = 0.0; + out_4990084456757481789[80] = 0.0; + out_4990084456757481789[81] = 0.0; + out_4990084456757481789[82] = 0.0; + out_4990084456757481789[83] = 0.0; + out_4990084456757481789[84] = 0.0; + out_4990084456757481789[85] = 0.0; + out_4990084456757481789[86] = 0.0; + out_4990084456757481789[87] = 0.0; + out_4990084456757481789[88] = 0.0; + out_4990084456757481789[89] = 0.0; + out_4990084456757481789[90] = 0.0; + out_4990084456757481789[91] = 0.0; + out_4990084456757481789[92] = 0.0; + out_4990084456757481789[93] = 0.0; + out_4990084456757481789[94] = 0.0; + out_4990084456757481789[95] = 1.0; + out_4990084456757481789[96] = 0.0; + out_4990084456757481789[97] = 0.0; + out_4990084456757481789[98] = 0.0; + out_4990084456757481789[99] = 0.0; + out_4990084456757481789[100] = 0.0; + out_4990084456757481789[101] = 0.0; + out_4990084456757481789[102] = 0.0; + out_4990084456757481789[103] = 0.0; + out_4990084456757481789[104] = 0.0; + out_4990084456757481789[105] = 0.0; + out_4990084456757481789[106] = 0.0; + out_4990084456757481789[107] = 0.0; + out_4990084456757481789[108] = 0.0; + out_4990084456757481789[109] = 0.0; + out_4990084456757481789[110] = 0.0; + out_4990084456757481789[111] = 0.0; + out_4990084456757481789[112] = 0.0; + out_4990084456757481789[113] = 0.0; + out_4990084456757481789[114] = 1.0; + out_4990084456757481789[115] = 0.0; + out_4990084456757481789[116] = 0.0; + out_4990084456757481789[117] = 0.0; + out_4990084456757481789[118] = 0.0; + out_4990084456757481789[119] = 0.0; + out_4990084456757481789[120] = 0.0; + out_4990084456757481789[121] = 0.0; + out_4990084456757481789[122] = 0.0; + out_4990084456757481789[123] = 0.0; + out_4990084456757481789[124] = 0.0; + out_4990084456757481789[125] = 0.0; + out_4990084456757481789[126] = 0.0; + out_4990084456757481789[127] = 0.0; + out_4990084456757481789[128] = 0.0; + out_4990084456757481789[129] = 0.0; + out_4990084456757481789[130] = 0.0; + out_4990084456757481789[131] = 0.0; + out_4990084456757481789[132] = 0.0; + out_4990084456757481789[133] = 1.0; + out_4990084456757481789[134] = 0.0; + out_4990084456757481789[135] = 0.0; + out_4990084456757481789[136] = 0.0; + out_4990084456757481789[137] = 0.0; + out_4990084456757481789[138] = 0.0; + out_4990084456757481789[139] = 0.0; + out_4990084456757481789[140] = 0.0; + out_4990084456757481789[141] = 0.0; + out_4990084456757481789[142] = 0.0; + out_4990084456757481789[143] = 0.0; + out_4990084456757481789[144] = 0.0; + out_4990084456757481789[145] = 0.0; + out_4990084456757481789[146] = 0.0; + out_4990084456757481789[147] = 0.0; + out_4990084456757481789[148] = 0.0; + out_4990084456757481789[149] = 0.0; + out_4990084456757481789[150] = 0.0; + out_4990084456757481789[151] = 0.0; + out_4990084456757481789[152] = 1.0; + out_4990084456757481789[153] = 0.0; + out_4990084456757481789[154] = 0.0; + out_4990084456757481789[155] = 0.0; + out_4990084456757481789[156] = 0.0; + out_4990084456757481789[157] = 0.0; + out_4990084456757481789[158] = 0.0; + out_4990084456757481789[159] = 0.0; + out_4990084456757481789[160] = 0.0; + out_4990084456757481789[161] = 0.0; + out_4990084456757481789[162] = 0.0; + out_4990084456757481789[163] = 0.0; + out_4990084456757481789[164] = 0.0; + out_4990084456757481789[165] = 0.0; + out_4990084456757481789[166] = 0.0; + out_4990084456757481789[167] = 0.0; + out_4990084456757481789[168] = 0.0; + out_4990084456757481789[169] = 0.0; + out_4990084456757481789[170] = 0.0; + out_4990084456757481789[171] = 1.0; + out_4990084456757481789[172] = 0.0; + out_4990084456757481789[173] = 0.0; + out_4990084456757481789[174] = 0.0; + out_4990084456757481789[175] = 0.0; + out_4990084456757481789[176] = 0.0; + out_4990084456757481789[177] = 0.0; + out_4990084456757481789[178] = 0.0; + out_4990084456757481789[179] = 0.0; + out_4990084456757481789[180] = 0.0; + out_4990084456757481789[181] = 0.0; + out_4990084456757481789[182] = 0.0; + out_4990084456757481789[183] = 0.0; + out_4990084456757481789[184] = 0.0; + out_4990084456757481789[185] = 0.0; + out_4990084456757481789[186] = 0.0; + out_4990084456757481789[187] = 0.0; + out_4990084456757481789[188] = 0.0; + out_4990084456757481789[189] = 0.0; + out_4990084456757481789[190] = 1.0; + out_4990084456757481789[191] = 0.0; + out_4990084456757481789[192] = 0.0; + out_4990084456757481789[193] = 0.0; + out_4990084456757481789[194] = 0.0; + out_4990084456757481789[195] = 0.0; + out_4990084456757481789[196] = 0.0; + out_4990084456757481789[197] = 0.0; + out_4990084456757481789[198] = 0.0; + out_4990084456757481789[199] = 0.0; + out_4990084456757481789[200] = 0.0; + out_4990084456757481789[201] = 0.0; + out_4990084456757481789[202] = 0.0; + out_4990084456757481789[203] = 0.0; + out_4990084456757481789[204] = 0.0; + out_4990084456757481789[205] = 0.0; + out_4990084456757481789[206] = 0.0; + out_4990084456757481789[207] = 0.0; + out_4990084456757481789[208] = 0.0; + out_4990084456757481789[209] = 1.0; + out_4990084456757481789[210] = 0.0; + out_4990084456757481789[211] = 0.0; + out_4990084456757481789[212] = 0.0; + out_4990084456757481789[213] = 0.0; + out_4990084456757481789[214] = 0.0; + out_4990084456757481789[215] = 0.0; + out_4990084456757481789[216] = 0.0; + out_4990084456757481789[217] = 0.0; + out_4990084456757481789[218] = 0.0; + out_4990084456757481789[219] = 0.0; + out_4990084456757481789[220] = 0.0; + out_4990084456757481789[221] = 0.0; + out_4990084456757481789[222] = 0.0; + out_4990084456757481789[223] = 0.0; + out_4990084456757481789[224] = 0.0; + out_4990084456757481789[225] = 0.0; + out_4990084456757481789[226] = 0.0; + out_4990084456757481789[227] = 0.0; + out_4990084456757481789[228] = 1.0; + out_4990084456757481789[229] = 0.0; + out_4990084456757481789[230] = 0.0; + out_4990084456757481789[231] = 0.0; + out_4990084456757481789[232] = 0.0; + out_4990084456757481789[233] = 0.0; + out_4990084456757481789[234] = 0.0; + out_4990084456757481789[235] = 0.0; + out_4990084456757481789[236] = 0.0; + out_4990084456757481789[237] = 0.0; + out_4990084456757481789[238] = 0.0; + out_4990084456757481789[239] = 0.0; + out_4990084456757481789[240] = 0.0; + out_4990084456757481789[241] = 0.0; + out_4990084456757481789[242] = 0.0; + out_4990084456757481789[243] = 0.0; + out_4990084456757481789[244] = 0.0; + out_4990084456757481789[245] = 0.0; + out_4990084456757481789[246] = 0.0; + out_4990084456757481789[247] = 1.0; + out_4990084456757481789[248] = 0.0; + out_4990084456757481789[249] = 0.0; + out_4990084456757481789[250] = 0.0; + out_4990084456757481789[251] = 0.0; + out_4990084456757481789[252] = 0.0; + out_4990084456757481789[253] = 0.0; + out_4990084456757481789[254] = 0.0; + out_4990084456757481789[255] = 0.0; + out_4990084456757481789[256] = 0.0; + out_4990084456757481789[257] = 0.0; + out_4990084456757481789[258] = 0.0; + out_4990084456757481789[259] = 0.0; + out_4990084456757481789[260] = 0.0; + out_4990084456757481789[261] = 0.0; + out_4990084456757481789[262] = 0.0; + out_4990084456757481789[263] = 0.0; + out_4990084456757481789[264] = 0.0; + out_4990084456757481789[265] = 0.0; + out_4990084456757481789[266] = 1.0; + out_4990084456757481789[267] = 0.0; + out_4990084456757481789[268] = 0.0; + out_4990084456757481789[269] = 0.0; + out_4990084456757481789[270] = 0.0; + out_4990084456757481789[271] = 0.0; + out_4990084456757481789[272] = 0.0; + out_4990084456757481789[273] = 0.0; + out_4990084456757481789[274] = 0.0; + out_4990084456757481789[275] = 0.0; + out_4990084456757481789[276] = 0.0; + out_4990084456757481789[277] = 0.0; + out_4990084456757481789[278] = 0.0; + out_4990084456757481789[279] = 0.0; + out_4990084456757481789[280] = 0.0; + out_4990084456757481789[281] = 0.0; + out_4990084456757481789[282] = 0.0; + out_4990084456757481789[283] = 0.0; + out_4990084456757481789[284] = 0.0; + out_4990084456757481789[285] = 1.0; + out_4990084456757481789[286] = 0.0; + out_4990084456757481789[287] = 0.0; + out_4990084456757481789[288] = 0.0; + out_4990084456757481789[289] = 0.0; + out_4990084456757481789[290] = 0.0; + out_4990084456757481789[291] = 0.0; + out_4990084456757481789[292] = 0.0; + out_4990084456757481789[293] = 0.0; + out_4990084456757481789[294] = 0.0; + out_4990084456757481789[295] = 0.0; + out_4990084456757481789[296] = 0.0; + out_4990084456757481789[297] = 0.0; + out_4990084456757481789[298] = 0.0; + out_4990084456757481789[299] = 0.0; + out_4990084456757481789[300] = 0.0; + out_4990084456757481789[301] = 0.0; + out_4990084456757481789[302] = 0.0; + out_4990084456757481789[303] = 0.0; + out_4990084456757481789[304] = 1.0; + out_4990084456757481789[305] = 0.0; + out_4990084456757481789[306] = 0.0; + out_4990084456757481789[307] = 0.0; + out_4990084456757481789[308] = 0.0; + out_4990084456757481789[309] = 0.0; + out_4990084456757481789[310] = 0.0; + out_4990084456757481789[311] = 0.0; + out_4990084456757481789[312] = 0.0; + out_4990084456757481789[313] = 0.0; + out_4990084456757481789[314] = 0.0; + out_4990084456757481789[315] = 0.0; + out_4990084456757481789[316] = 0.0; + out_4990084456757481789[317] = 0.0; + out_4990084456757481789[318] = 0.0; + out_4990084456757481789[319] = 0.0; + out_4990084456757481789[320] = 0.0; + out_4990084456757481789[321] = 0.0; + out_4990084456757481789[322] = 0.0; + out_4990084456757481789[323] = 1.0; } -void f_fun(double *state, double dt, double *out_76749327147481970) { - out_76749327147481970[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_76749327147481970[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_76749327147481970[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_76749327147481970[3] = dt*state[12] + state[3]; - out_76749327147481970[4] = dt*state[13] + state[4]; - out_76749327147481970[5] = dt*state[14] + state[5]; - out_76749327147481970[6] = state[6]; - out_76749327147481970[7] = state[7]; - out_76749327147481970[8] = state[8]; - out_76749327147481970[9] = state[9]; - out_76749327147481970[10] = state[10]; - out_76749327147481970[11] = state[11]; - out_76749327147481970[12] = state[12]; - out_76749327147481970[13] = state[13]; - out_76749327147481970[14] = state[14]; - out_76749327147481970[15] = state[15]; - out_76749327147481970[16] = state[16]; - out_76749327147481970[17] = state[17]; +void f_fun(double *state, double dt, double *out_6296202895308932040) { + out_6296202895308932040[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_6296202895308932040[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_6296202895308932040[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_6296202895308932040[3] = dt*state[12] + state[3]; + out_6296202895308932040[4] = dt*state[13] + state[4]; + out_6296202895308932040[5] = dt*state[14] + state[5]; + out_6296202895308932040[6] = state[6]; + out_6296202895308932040[7] = state[7]; + out_6296202895308932040[8] = state[8]; + out_6296202895308932040[9] = state[9]; + out_6296202895308932040[10] = state[10]; + out_6296202895308932040[11] = state[11]; + out_6296202895308932040[12] = state[12]; + out_6296202895308932040[13] = state[13]; + out_6296202895308932040[14] = state[14]; + out_6296202895308932040[15] = state[15]; + out_6296202895308932040[16] = state[16]; + out_6296202895308932040[17] = state[17]; } -void F_fun(double *state, double dt, double *out_7773273273846026145) { - out_7773273273846026145[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_7773273273846026145[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_7773273273846026145[2] = 0; - out_7773273273846026145[3] = 0; - out_7773273273846026145[4] = 0; - out_7773273273846026145[5] = 0; - out_7773273273846026145[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_7773273273846026145[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_7773273273846026145[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_7773273273846026145[9] = 0; - out_7773273273846026145[10] = 0; - out_7773273273846026145[11] = 0; - out_7773273273846026145[12] = 0; - out_7773273273846026145[13] = 0; - out_7773273273846026145[14] = 0; - out_7773273273846026145[15] = 0; - out_7773273273846026145[16] = 0; - out_7773273273846026145[17] = 0; - out_7773273273846026145[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_7773273273846026145[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_7773273273846026145[20] = 0; - out_7773273273846026145[21] = 0; - out_7773273273846026145[22] = 0; - out_7773273273846026145[23] = 0; - out_7773273273846026145[24] = 0; - out_7773273273846026145[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_7773273273846026145[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_7773273273846026145[27] = 0; - out_7773273273846026145[28] = 0; - out_7773273273846026145[29] = 0; - out_7773273273846026145[30] = 0; - out_7773273273846026145[31] = 0; - out_7773273273846026145[32] = 0; - out_7773273273846026145[33] = 0; - out_7773273273846026145[34] = 0; - out_7773273273846026145[35] = 0; - out_7773273273846026145[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_7773273273846026145[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_7773273273846026145[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_7773273273846026145[39] = 0; - out_7773273273846026145[40] = 0; - out_7773273273846026145[41] = 0; - out_7773273273846026145[42] = 0; - out_7773273273846026145[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_7773273273846026145[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_7773273273846026145[45] = 0; - out_7773273273846026145[46] = 0; - out_7773273273846026145[47] = 0; - out_7773273273846026145[48] = 0; - out_7773273273846026145[49] = 0; - out_7773273273846026145[50] = 0; - out_7773273273846026145[51] = 0; - out_7773273273846026145[52] = 0; - out_7773273273846026145[53] = 0; - out_7773273273846026145[54] = 0; - out_7773273273846026145[55] = 0; - out_7773273273846026145[56] = 0; - out_7773273273846026145[57] = 1; - out_7773273273846026145[58] = 0; - out_7773273273846026145[59] = 0; - out_7773273273846026145[60] = 0; - out_7773273273846026145[61] = 0; - out_7773273273846026145[62] = 0; - out_7773273273846026145[63] = 0; - out_7773273273846026145[64] = 0; - out_7773273273846026145[65] = 0; - out_7773273273846026145[66] = dt; - out_7773273273846026145[67] = 0; - out_7773273273846026145[68] = 0; - out_7773273273846026145[69] = 0; - out_7773273273846026145[70] = 0; - out_7773273273846026145[71] = 0; - out_7773273273846026145[72] = 0; - out_7773273273846026145[73] = 0; - out_7773273273846026145[74] = 0; - out_7773273273846026145[75] = 0; - out_7773273273846026145[76] = 1; - out_7773273273846026145[77] = 0; - out_7773273273846026145[78] = 0; - out_7773273273846026145[79] = 0; - out_7773273273846026145[80] = 0; - out_7773273273846026145[81] = 0; - out_7773273273846026145[82] = 0; - out_7773273273846026145[83] = 0; - out_7773273273846026145[84] = 0; - out_7773273273846026145[85] = dt; - out_7773273273846026145[86] = 0; - out_7773273273846026145[87] = 0; - out_7773273273846026145[88] = 0; - out_7773273273846026145[89] = 0; - out_7773273273846026145[90] = 0; - out_7773273273846026145[91] = 0; - out_7773273273846026145[92] = 0; - out_7773273273846026145[93] = 0; - out_7773273273846026145[94] = 0; - out_7773273273846026145[95] = 1; - out_7773273273846026145[96] = 0; - out_7773273273846026145[97] = 0; - out_7773273273846026145[98] = 0; - out_7773273273846026145[99] = 0; - out_7773273273846026145[100] = 0; - out_7773273273846026145[101] = 0; - out_7773273273846026145[102] = 0; - out_7773273273846026145[103] = 0; - out_7773273273846026145[104] = dt; - out_7773273273846026145[105] = 0; - out_7773273273846026145[106] = 0; - out_7773273273846026145[107] = 0; - out_7773273273846026145[108] = 0; - out_7773273273846026145[109] = 0; - out_7773273273846026145[110] = 0; - out_7773273273846026145[111] = 0; - out_7773273273846026145[112] = 0; - out_7773273273846026145[113] = 0; - out_7773273273846026145[114] = 1; - out_7773273273846026145[115] = 0; - out_7773273273846026145[116] = 0; - out_7773273273846026145[117] = 0; - out_7773273273846026145[118] = 0; - out_7773273273846026145[119] = 0; - out_7773273273846026145[120] = 0; - out_7773273273846026145[121] = 0; - out_7773273273846026145[122] = 0; - out_7773273273846026145[123] = 0; - out_7773273273846026145[124] = 0; - out_7773273273846026145[125] = 0; - out_7773273273846026145[126] = 0; - out_7773273273846026145[127] = 0; - out_7773273273846026145[128] = 0; - out_7773273273846026145[129] = 0; - out_7773273273846026145[130] = 0; - out_7773273273846026145[131] = 0; - out_7773273273846026145[132] = 0; - out_7773273273846026145[133] = 1; - out_7773273273846026145[134] = 0; - out_7773273273846026145[135] = 0; - out_7773273273846026145[136] = 0; - out_7773273273846026145[137] = 0; - out_7773273273846026145[138] = 0; - out_7773273273846026145[139] = 0; - out_7773273273846026145[140] = 0; - out_7773273273846026145[141] = 0; - out_7773273273846026145[142] = 0; - out_7773273273846026145[143] = 0; - out_7773273273846026145[144] = 0; - out_7773273273846026145[145] = 0; - out_7773273273846026145[146] = 0; - out_7773273273846026145[147] = 0; - out_7773273273846026145[148] = 0; - out_7773273273846026145[149] = 0; - out_7773273273846026145[150] = 0; - out_7773273273846026145[151] = 0; - out_7773273273846026145[152] = 1; - out_7773273273846026145[153] = 0; - out_7773273273846026145[154] = 0; - out_7773273273846026145[155] = 0; - out_7773273273846026145[156] = 0; - out_7773273273846026145[157] = 0; - out_7773273273846026145[158] = 0; - out_7773273273846026145[159] = 0; - out_7773273273846026145[160] = 0; - out_7773273273846026145[161] = 0; - out_7773273273846026145[162] = 0; - out_7773273273846026145[163] = 0; - out_7773273273846026145[164] = 0; - out_7773273273846026145[165] = 0; - out_7773273273846026145[166] = 0; - out_7773273273846026145[167] = 0; - out_7773273273846026145[168] = 0; - out_7773273273846026145[169] = 0; - out_7773273273846026145[170] = 0; - out_7773273273846026145[171] = 1; - out_7773273273846026145[172] = 0; - out_7773273273846026145[173] = 0; - out_7773273273846026145[174] = 0; - out_7773273273846026145[175] = 0; - out_7773273273846026145[176] = 0; - out_7773273273846026145[177] = 0; - out_7773273273846026145[178] = 0; - out_7773273273846026145[179] = 0; - out_7773273273846026145[180] = 0; - out_7773273273846026145[181] = 0; - out_7773273273846026145[182] = 0; - out_7773273273846026145[183] = 0; - out_7773273273846026145[184] = 0; - out_7773273273846026145[185] = 0; - out_7773273273846026145[186] = 0; - out_7773273273846026145[187] = 0; - out_7773273273846026145[188] = 0; - out_7773273273846026145[189] = 0; - out_7773273273846026145[190] = 1; - out_7773273273846026145[191] = 0; - out_7773273273846026145[192] = 0; - out_7773273273846026145[193] = 0; - out_7773273273846026145[194] = 0; - out_7773273273846026145[195] = 0; - out_7773273273846026145[196] = 0; - out_7773273273846026145[197] = 0; - out_7773273273846026145[198] = 0; - out_7773273273846026145[199] = 0; - out_7773273273846026145[200] = 0; - out_7773273273846026145[201] = 0; - out_7773273273846026145[202] = 0; - out_7773273273846026145[203] = 0; - out_7773273273846026145[204] = 0; - out_7773273273846026145[205] = 0; - out_7773273273846026145[206] = 0; - out_7773273273846026145[207] = 0; - out_7773273273846026145[208] = 0; - out_7773273273846026145[209] = 1; - out_7773273273846026145[210] = 0; - out_7773273273846026145[211] = 0; - out_7773273273846026145[212] = 0; - out_7773273273846026145[213] = 0; - out_7773273273846026145[214] = 0; - out_7773273273846026145[215] = 0; - out_7773273273846026145[216] = 0; - out_7773273273846026145[217] = 0; - out_7773273273846026145[218] = 0; - out_7773273273846026145[219] = 0; - out_7773273273846026145[220] = 0; - out_7773273273846026145[221] = 0; - out_7773273273846026145[222] = 0; - out_7773273273846026145[223] = 0; - out_7773273273846026145[224] = 0; - out_7773273273846026145[225] = 0; - out_7773273273846026145[226] = 0; - out_7773273273846026145[227] = 0; - out_7773273273846026145[228] = 1; - out_7773273273846026145[229] = 0; - out_7773273273846026145[230] = 0; - out_7773273273846026145[231] = 0; - out_7773273273846026145[232] = 0; - out_7773273273846026145[233] = 0; - out_7773273273846026145[234] = 0; - out_7773273273846026145[235] = 0; - out_7773273273846026145[236] = 0; - out_7773273273846026145[237] = 0; - out_7773273273846026145[238] = 0; - out_7773273273846026145[239] = 0; - out_7773273273846026145[240] = 0; - out_7773273273846026145[241] = 0; - out_7773273273846026145[242] = 0; - out_7773273273846026145[243] = 0; - out_7773273273846026145[244] = 0; - out_7773273273846026145[245] = 0; - out_7773273273846026145[246] = 0; - out_7773273273846026145[247] = 1; - out_7773273273846026145[248] = 0; - out_7773273273846026145[249] = 0; - out_7773273273846026145[250] = 0; - out_7773273273846026145[251] = 0; - out_7773273273846026145[252] = 0; - out_7773273273846026145[253] = 0; - out_7773273273846026145[254] = 0; - out_7773273273846026145[255] = 0; - out_7773273273846026145[256] = 0; - out_7773273273846026145[257] = 0; - out_7773273273846026145[258] = 0; - out_7773273273846026145[259] = 0; - out_7773273273846026145[260] = 0; - out_7773273273846026145[261] = 0; - out_7773273273846026145[262] = 0; - out_7773273273846026145[263] = 0; - out_7773273273846026145[264] = 0; - out_7773273273846026145[265] = 0; - out_7773273273846026145[266] = 1; - out_7773273273846026145[267] = 0; - out_7773273273846026145[268] = 0; - out_7773273273846026145[269] = 0; - out_7773273273846026145[270] = 0; - out_7773273273846026145[271] = 0; - out_7773273273846026145[272] = 0; - out_7773273273846026145[273] = 0; - out_7773273273846026145[274] = 0; - out_7773273273846026145[275] = 0; - out_7773273273846026145[276] = 0; - out_7773273273846026145[277] = 0; - out_7773273273846026145[278] = 0; - out_7773273273846026145[279] = 0; - out_7773273273846026145[280] = 0; - out_7773273273846026145[281] = 0; - out_7773273273846026145[282] = 0; - out_7773273273846026145[283] = 0; - out_7773273273846026145[284] = 0; - out_7773273273846026145[285] = 1; - out_7773273273846026145[286] = 0; - out_7773273273846026145[287] = 0; - out_7773273273846026145[288] = 0; - out_7773273273846026145[289] = 0; - out_7773273273846026145[290] = 0; - out_7773273273846026145[291] = 0; - out_7773273273846026145[292] = 0; - out_7773273273846026145[293] = 0; - out_7773273273846026145[294] = 0; - out_7773273273846026145[295] = 0; - out_7773273273846026145[296] = 0; - out_7773273273846026145[297] = 0; - out_7773273273846026145[298] = 0; - out_7773273273846026145[299] = 0; - out_7773273273846026145[300] = 0; - out_7773273273846026145[301] = 0; - out_7773273273846026145[302] = 0; - out_7773273273846026145[303] = 0; - out_7773273273846026145[304] = 1; - out_7773273273846026145[305] = 0; - out_7773273273846026145[306] = 0; - out_7773273273846026145[307] = 0; - out_7773273273846026145[308] = 0; - out_7773273273846026145[309] = 0; - out_7773273273846026145[310] = 0; - out_7773273273846026145[311] = 0; - out_7773273273846026145[312] = 0; - out_7773273273846026145[313] = 0; - out_7773273273846026145[314] = 0; - out_7773273273846026145[315] = 0; - out_7773273273846026145[316] = 0; - out_7773273273846026145[317] = 0; - out_7773273273846026145[318] = 0; - out_7773273273846026145[319] = 0; - out_7773273273846026145[320] = 0; - out_7773273273846026145[321] = 0; - out_7773273273846026145[322] = 0; - out_7773273273846026145[323] = 1; +void F_fun(double *state, double dt, double *out_909355394007943716) { + out_909355394007943716[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_909355394007943716[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_909355394007943716[2] = 0; + out_909355394007943716[3] = 0; + out_909355394007943716[4] = 0; + out_909355394007943716[5] = 0; + out_909355394007943716[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_909355394007943716[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_909355394007943716[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_909355394007943716[9] = 0; + out_909355394007943716[10] = 0; + out_909355394007943716[11] = 0; + out_909355394007943716[12] = 0; + out_909355394007943716[13] = 0; + out_909355394007943716[14] = 0; + out_909355394007943716[15] = 0; + out_909355394007943716[16] = 0; + out_909355394007943716[17] = 0; + out_909355394007943716[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_909355394007943716[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_909355394007943716[20] = 0; + out_909355394007943716[21] = 0; + out_909355394007943716[22] = 0; + out_909355394007943716[23] = 0; + out_909355394007943716[24] = 0; + out_909355394007943716[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_909355394007943716[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_909355394007943716[27] = 0; + out_909355394007943716[28] = 0; + out_909355394007943716[29] = 0; + out_909355394007943716[30] = 0; + out_909355394007943716[31] = 0; + out_909355394007943716[32] = 0; + out_909355394007943716[33] = 0; + out_909355394007943716[34] = 0; + out_909355394007943716[35] = 0; + out_909355394007943716[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_909355394007943716[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_909355394007943716[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_909355394007943716[39] = 0; + out_909355394007943716[40] = 0; + out_909355394007943716[41] = 0; + out_909355394007943716[42] = 0; + out_909355394007943716[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_909355394007943716[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_909355394007943716[45] = 0; + out_909355394007943716[46] = 0; + out_909355394007943716[47] = 0; + out_909355394007943716[48] = 0; + out_909355394007943716[49] = 0; + out_909355394007943716[50] = 0; + out_909355394007943716[51] = 0; + out_909355394007943716[52] = 0; + out_909355394007943716[53] = 0; + out_909355394007943716[54] = 0; + out_909355394007943716[55] = 0; + out_909355394007943716[56] = 0; + out_909355394007943716[57] = 1; + out_909355394007943716[58] = 0; + out_909355394007943716[59] = 0; + out_909355394007943716[60] = 0; + out_909355394007943716[61] = 0; + out_909355394007943716[62] = 0; + out_909355394007943716[63] = 0; + out_909355394007943716[64] = 0; + out_909355394007943716[65] = 0; + out_909355394007943716[66] = dt; + out_909355394007943716[67] = 0; + out_909355394007943716[68] = 0; + out_909355394007943716[69] = 0; + out_909355394007943716[70] = 0; + out_909355394007943716[71] = 0; + out_909355394007943716[72] = 0; + out_909355394007943716[73] = 0; + out_909355394007943716[74] = 0; + out_909355394007943716[75] = 0; + out_909355394007943716[76] = 1; + out_909355394007943716[77] = 0; + out_909355394007943716[78] = 0; + out_909355394007943716[79] = 0; + out_909355394007943716[80] = 0; + out_909355394007943716[81] = 0; + out_909355394007943716[82] = 0; + out_909355394007943716[83] = 0; + out_909355394007943716[84] = 0; + out_909355394007943716[85] = dt; + out_909355394007943716[86] = 0; + out_909355394007943716[87] = 0; + out_909355394007943716[88] = 0; + out_909355394007943716[89] = 0; + out_909355394007943716[90] = 0; + out_909355394007943716[91] = 0; + out_909355394007943716[92] = 0; + out_909355394007943716[93] = 0; + out_909355394007943716[94] = 0; + out_909355394007943716[95] = 1; + out_909355394007943716[96] = 0; + out_909355394007943716[97] = 0; + out_909355394007943716[98] = 0; + out_909355394007943716[99] = 0; + out_909355394007943716[100] = 0; + out_909355394007943716[101] = 0; + out_909355394007943716[102] = 0; + out_909355394007943716[103] = 0; + out_909355394007943716[104] = dt; + out_909355394007943716[105] = 0; + out_909355394007943716[106] = 0; + out_909355394007943716[107] = 0; + out_909355394007943716[108] = 0; + out_909355394007943716[109] = 0; + out_909355394007943716[110] = 0; + out_909355394007943716[111] = 0; + out_909355394007943716[112] = 0; + out_909355394007943716[113] = 0; + out_909355394007943716[114] = 1; + out_909355394007943716[115] = 0; + out_909355394007943716[116] = 0; + out_909355394007943716[117] = 0; + out_909355394007943716[118] = 0; + out_909355394007943716[119] = 0; + out_909355394007943716[120] = 0; + out_909355394007943716[121] = 0; + out_909355394007943716[122] = 0; + out_909355394007943716[123] = 0; + out_909355394007943716[124] = 0; + out_909355394007943716[125] = 0; + out_909355394007943716[126] = 0; + out_909355394007943716[127] = 0; + out_909355394007943716[128] = 0; + out_909355394007943716[129] = 0; + out_909355394007943716[130] = 0; + out_909355394007943716[131] = 0; + out_909355394007943716[132] = 0; + out_909355394007943716[133] = 1; + out_909355394007943716[134] = 0; + out_909355394007943716[135] = 0; + out_909355394007943716[136] = 0; + out_909355394007943716[137] = 0; + out_909355394007943716[138] = 0; + out_909355394007943716[139] = 0; + out_909355394007943716[140] = 0; + out_909355394007943716[141] = 0; + out_909355394007943716[142] = 0; + out_909355394007943716[143] = 0; + out_909355394007943716[144] = 0; + out_909355394007943716[145] = 0; + out_909355394007943716[146] = 0; + out_909355394007943716[147] = 0; + out_909355394007943716[148] = 0; + out_909355394007943716[149] = 0; + out_909355394007943716[150] = 0; + out_909355394007943716[151] = 0; + out_909355394007943716[152] = 1; + out_909355394007943716[153] = 0; + out_909355394007943716[154] = 0; + out_909355394007943716[155] = 0; + out_909355394007943716[156] = 0; + out_909355394007943716[157] = 0; + out_909355394007943716[158] = 0; + out_909355394007943716[159] = 0; + out_909355394007943716[160] = 0; + out_909355394007943716[161] = 0; + out_909355394007943716[162] = 0; + out_909355394007943716[163] = 0; + out_909355394007943716[164] = 0; + out_909355394007943716[165] = 0; + out_909355394007943716[166] = 0; + out_909355394007943716[167] = 0; + out_909355394007943716[168] = 0; + out_909355394007943716[169] = 0; + out_909355394007943716[170] = 0; + out_909355394007943716[171] = 1; + out_909355394007943716[172] = 0; + out_909355394007943716[173] = 0; + out_909355394007943716[174] = 0; + out_909355394007943716[175] = 0; + out_909355394007943716[176] = 0; + out_909355394007943716[177] = 0; + out_909355394007943716[178] = 0; + out_909355394007943716[179] = 0; + out_909355394007943716[180] = 0; + out_909355394007943716[181] = 0; + out_909355394007943716[182] = 0; + out_909355394007943716[183] = 0; + out_909355394007943716[184] = 0; + out_909355394007943716[185] = 0; + out_909355394007943716[186] = 0; + out_909355394007943716[187] = 0; + out_909355394007943716[188] = 0; + out_909355394007943716[189] = 0; + out_909355394007943716[190] = 1; + out_909355394007943716[191] = 0; + out_909355394007943716[192] = 0; + out_909355394007943716[193] = 0; + out_909355394007943716[194] = 0; + out_909355394007943716[195] = 0; + out_909355394007943716[196] = 0; + out_909355394007943716[197] = 0; + out_909355394007943716[198] = 0; + out_909355394007943716[199] = 0; + out_909355394007943716[200] = 0; + out_909355394007943716[201] = 0; + out_909355394007943716[202] = 0; + out_909355394007943716[203] = 0; + out_909355394007943716[204] = 0; + out_909355394007943716[205] = 0; + out_909355394007943716[206] = 0; + out_909355394007943716[207] = 0; + out_909355394007943716[208] = 0; + out_909355394007943716[209] = 1; + out_909355394007943716[210] = 0; + out_909355394007943716[211] = 0; + out_909355394007943716[212] = 0; + out_909355394007943716[213] = 0; + out_909355394007943716[214] = 0; + out_909355394007943716[215] = 0; + out_909355394007943716[216] = 0; + out_909355394007943716[217] = 0; + out_909355394007943716[218] = 0; + out_909355394007943716[219] = 0; + out_909355394007943716[220] = 0; + out_909355394007943716[221] = 0; + out_909355394007943716[222] = 0; + out_909355394007943716[223] = 0; + out_909355394007943716[224] = 0; + out_909355394007943716[225] = 0; + out_909355394007943716[226] = 0; + out_909355394007943716[227] = 0; + out_909355394007943716[228] = 1; + out_909355394007943716[229] = 0; + out_909355394007943716[230] = 0; + out_909355394007943716[231] = 0; + out_909355394007943716[232] = 0; + out_909355394007943716[233] = 0; + out_909355394007943716[234] = 0; + out_909355394007943716[235] = 0; + out_909355394007943716[236] = 0; + out_909355394007943716[237] = 0; + out_909355394007943716[238] = 0; + out_909355394007943716[239] = 0; + out_909355394007943716[240] = 0; + out_909355394007943716[241] = 0; + out_909355394007943716[242] = 0; + out_909355394007943716[243] = 0; + out_909355394007943716[244] = 0; + out_909355394007943716[245] = 0; + out_909355394007943716[246] = 0; + out_909355394007943716[247] = 1; + out_909355394007943716[248] = 0; + out_909355394007943716[249] = 0; + out_909355394007943716[250] = 0; + out_909355394007943716[251] = 0; + out_909355394007943716[252] = 0; + out_909355394007943716[253] = 0; + out_909355394007943716[254] = 0; + out_909355394007943716[255] = 0; + out_909355394007943716[256] = 0; + out_909355394007943716[257] = 0; + out_909355394007943716[258] = 0; + out_909355394007943716[259] = 0; + out_909355394007943716[260] = 0; + out_909355394007943716[261] = 0; + out_909355394007943716[262] = 0; + out_909355394007943716[263] = 0; + out_909355394007943716[264] = 0; + out_909355394007943716[265] = 0; + out_909355394007943716[266] = 1; + out_909355394007943716[267] = 0; + out_909355394007943716[268] = 0; + out_909355394007943716[269] = 0; + out_909355394007943716[270] = 0; + out_909355394007943716[271] = 0; + out_909355394007943716[272] = 0; + out_909355394007943716[273] = 0; + out_909355394007943716[274] = 0; + out_909355394007943716[275] = 0; + out_909355394007943716[276] = 0; + out_909355394007943716[277] = 0; + out_909355394007943716[278] = 0; + out_909355394007943716[279] = 0; + out_909355394007943716[280] = 0; + out_909355394007943716[281] = 0; + out_909355394007943716[282] = 0; + out_909355394007943716[283] = 0; + out_909355394007943716[284] = 0; + out_909355394007943716[285] = 1; + out_909355394007943716[286] = 0; + out_909355394007943716[287] = 0; + out_909355394007943716[288] = 0; + out_909355394007943716[289] = 0; + out_909355394007943716[290] = 0; + out_909355394007943716[291] = 0; + out_909355394007943716[292] = 0; + out_909355394007943716[293] = 0; + out_909355394007943716[294] = 0; + out_909355394007943716[295] = 0; + out_909355394007943716[296] = 0; + out_909355394007943716[297] = 0; + out_909355394007943716[298] = 0; + out_909355394007943716[299] = 0; + out_909355394007943716[300] = 0; + out_909355394007943716[301] = 0; + out_909355394007943716[302] = 0; + out_909355394007943716[303] = 0; + out_909355394007943716[304] = 1; + out_909355394007943716[305] = 0; + out_909355394007943716[306] = 0; + out_909355394007943716[307] = 0; + out_909355394007943716[308] = 0; + out_909355394007943716[309] = 0; + out_909355394007943716[310] = 0; + out_909355394007943716[311] = 0; + out_909355394007943716[312] = 0; + out_909355394007943716[313] = 0; + out_909355394007943716[314] = 0; + out_909355394007943716[315] = 0; + out_909355394007943716[316] = 0; + out_909355394007943716[317] = 0; + out_909355394007943716[318] = 0; + out_909355394007943716[319] = 0; + out_909355394007943716[320] = 0; + out_909355394007943716[321] = 0; + out_909355394007943716[322] = 0; + out_909355394007943716[323] = 1; } -void h_4(double *state, double *unused, double *out_8836590006675901483) { - out_8836590006675901483[0] = state[6] + state[9]; - out_8836590006675901483[1] = state[7] + state[10]; - out_8836590006675901483[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_6818977063477642683) { + out_6818977063477642683[0] = state[6] + state[9]; + out_6818977063477642683[1] = state[7] + state[10]; + out_6818977063477642683[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_8771031560904685529) { - out_8771031560904685529[0] = 0; - out_8771031560904685529[1] = 0; - out_8771031560904685529[2] = 0; - out_8771031560904685529[3] = 0; - out_8771031560904685529[4] = 0; - out_8771031560904685529[5] = 0; - out_8771031560904685529[6] = 1; - out_8771031560904685529[7] = 0; - out_8771031560904685529[8] = 0; - out_8771031560904685529[9] = 1; - out_8771031560904685529[10] = 0; - out_8771031560904685529[11] = 0; - out_8771031560904685529[12] = 0; - out_8771031560904685529[13] = 0; - out_8771031560904685529[14] = 0; - out_8771031560904685529[15] = 0; - out_8771031560904685529[16] = 0; - out_8771031560904685529[17] = 0; - out_8771031560904685529[18] = 0; - out_8771031560904685529[19] = 0; - out_8771031560904685529[20] = 0; - out_8771031560904685529[21] = 0; - out_8771031560904685529[22] = 0; - out_8771031560904685529[23] = 0; - out_8771031560904685529[24] = 0; - out_8771031560904685529[25] = 1; - out_8771031560904685529[26] = 0; - out_8771031560904685529[27] = 0; - out_8771031560904685529[28] = 1; - out_8771031560904685529[29] = 0; - out_8771031560904685529[30] = 0; - out_8771031560904685529[31] = 0; - out_8771031560904685529[32] = 0; - out_8771031560904685529[33] = 0; - out_8771031560904685529[34] = 0; - out_8771031560904685529[35] = 0; - out_8771031560904685529[36] = 0; - out_8771031560904685529[37] = 0; - out_8771031560904685529[38] = 0; - out_8771031560904685529[39] = 0; - out_8771031560904685529[40] = 0; - out_8771031560904685529[41] = 0; - out_8771031560904685529[42] = 0; - out_8771031560904685529[43] = 0; - out_8771031560904685529[44] = 1; - out_8771031560904685529[45] = 0; - out_8771031560904685529[46] = 0; - out_8771031560904685529[47] = 1; - out_8771031560904685529[48] = 0; - out_8771031560904685529[49] = 0; - out_8771031560904685529[50] = 0; - out_8771031560904685529[51] = 0; - out_8771031560904685529[52] = 0; - out_8771031560904685529[53] = 0; +void H_4(double *state, double *unused, double *out_3954277209300339553) { + out_3954277209300339553[0] = 0; + out_3954277209300339553[1] = 0; + out_3954277209300339553[2] = 0; + out_3954277209300339553[3] = 0; + out_3954277209300339553[4] = 0; + out_3954277209300339553[5] = 0; + out_3954277209300339553[6] = 1; + out_3954277209300339553[7] = 0; + out_3954277209300339553[8] = 0; + out_3954277209300339553[9] = 1; + out_3954277209300339553[10] = 0; + out_3954277209300339553[11] = 0; + out_3954277209300339553[12] = 0; + out_3954277209300339553[13] = 0; + out_3954277209300339553[14] = 0; + out_3954277209300339553[15] = 0; + out_3954277209300339553[16] = 0; + out_3954277209300339553[17] = 0; + out_3954277209300339553[18] = 0; + out_3954277209300339553[19] = 0; + out_3954277209300339553[20] = 0; + out_3954277209300339553[21] = 0; + out_3954277209300339553[22] = 0; + out_3954277209300339553[23] = 0; + out_3954277209300339553[24] = 0; + out_3954277209300339553[25] = 1; + out_3954277209300339553[26] = 0; + out_3954277209300339553[27] = 0; + out_3954277209300339553[28] = 1; + out_3954277209300339553[29] = 0; + out_3954277209300339553[30] = 0; + out_3954277209300339553[31] = 0; + out_3954277209300339553[32] = 0; + out_3954277209300339553[33] = 0; + out_3954277209300339553[34] = 0; + out_3954277209300339553[35] = 0; + out_3954277209300339553[36] = 0; + out_3954277209300339553[37] = 0; + out_3954277209300339553[38] = 0; + out_3954277209300339553[39] = 0; + out_3954277209300339553[40] = 0; + out_3954277209300339553[41] = 0; + out_3954277209300339553[42] = 0; + out_3954277209300339553[43] = 0; + out_3954277209300339553[44] = 1; + out_3954277209300339553[45] = 0; + out_3954277209300339553[46] = 0; + out_3954277209300339553[47] = 1; + out_3954277209300339553[48] = 0; + out_3954277209300339553[49] = 0; + out_3954277209300339553[50] = 0; + out_3954277209300339553[51] = 0; + out_3954277209300339553[52] = 0; + out_3954277209300339553[53] = 0; } -void h_10(double *state, double *unused, double *out_7833049906299002536) { - out_7833049906299002536[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_7833049906299002536[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_7833049906299002536[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_3834013708689822319) { + out_3834013708689822319[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_3834013708689822319[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_3834013708689822319[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_1676230657278343882) { - out_1676230657278343882[0] = 0; - out_1676230657278343882[1] = 9.8100000000000005*cos(state[1]); - out_1676230657278343882[2] = 0; - out_1676230657278343882[3] = 0; - out_1676230657278343882[4] = -state[8]; - out_1676230657278343882[5] = state[7]; - out_1676230657278343882[6] = 0; - out_1676230657278343882[7] = state[5]; - out_1676230657278343882[8] = -state[4]; - out_1676230657278343882[9] = 0; - out_1676230657278343882[10] = 0; - out_1676230657278343882[11] = 0; - out_1676230657278343882[12] = 1; - out_1676230657278343882[13] = 0; - out_1676230657278343882[14] = 0; - out_1676230657278343882[15] = 1; - out_1676230657278343882[16] = 0; - out_1676230657278343882[17] = 0; - out_1676230657278343882[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_1676230657278343882[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_1676230657278343882[20] = 0; - out_1676230657278343882[21] = state[8]; - out_1676230657278343882[22] = 0; - out_1676230657278343882[23] = -state[6]; - out_1676230657278343882[24] = -state[5]; - out_1676230657278343882[25] = 0; - out_1676230657278343882[26] = state[3]; - out_1676230657278343882[27] = 0; - out_1676230657278343882[28] = 0; - out_1676230657278343882[29] = 0; - out_1676230657278343882[30] = 0; - out_1676230657278343882[31] = 1; - out_1676230657278343882[32] = 0; - out_1676230657278343882[33] = 0; - out_1676230657278343882[34] = 1; - out_1676230657278343882[35] = 0; - out_1676230657278343882[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_1676230657278343882[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_1676230657278343882[38] = 0; - out_1676230657278343882[39] = -state[7]; - out_1676230657278343882[40] = state[6]; - out_1676230657278343882[41] = 0; - out_1676230657278343882[42] = state[4]; - out_1676230657278343882[43] = -state[3]; - out_1676230657278343882[44] = 0; - out_1676230657278343882[45] = 0; - out_1676230657278343882[46] = 0; - out_1676230657278343882[47] = 0; - out_1676230657278343882[48] = 0; - out_1676230657278343882[49] = 0; - out_1676230657278343882[50] = 1; - out_1676230657278343882[51] = 0; - out_1676230657278343882[52] = 0; - out_1676230657278343882[53] = 1; +void H_10(double *state, double *unused, double *out_3533519720904001040) { + out_3533519720904001040[0] = 0; + out_3533519720904001040[1] = 9.8100000000000005*cos(state[1]); + out_3533519720904001040[2] = 0; + out_3533519720904001040[3] = 0; + out_3533519720904001040[4] = -state[8]; + out_3533519720904001040[5] = state[7]; + out_3533519720904001040[6] = 0; + out_3533519720904001040[7] = state[5]; + out_3533519720904001040[8] = -state[4]; + out_3533519720904001040[9] = 0; + out_3533519720904001040[10] = 0; + out_3533519720904001040[11] = 0; + out_3533519720904001040[12] = 1; + out_3533519720904001040[13] = 0; + out_3533519720904001040[14] = 0; + out_3533519720904001040[15] = 1; + out_3533519720904001040[16] = 0; + out_3533519720904001040[17] = 0; + out_3533519720904001040[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_3533519720904001040[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_3533519720904001040[20] = 0; + out_3533519720904001040[21] = state[8]; + out_3533519720904001040[22] = 0; + out_3533519720904001040[23] = -state[6]; + out_3533519720904001040[24] = -state[5]; + out_3533519720904001040[25] = 0; + out_3533519720904001040[26] = state[3]; + out_3533519720904001040[27] = 0; + out_3533519720904001040[28] = 0; + out_3533519720904001040[29] = 0; + out_3533519720904001040[30] = 0; + out_3533519720904001040[31] = 1; + out_3533519720904001040[32] = 0; + out_3533519720904001040[33] = 0; + out_3533519720904001040[34] = 1; + out_3533519720904001040[35] = 0; + out_3533519720904001040[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_3533519720904001040[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_3533519720904001040[38] = 0; + out_3533519720904001040[39] = -state[7]; + out_3533519720904001040[40] = state[6]; + out_3533519720904001040[41] = 0; + out_3533519720904001040[42] = state[4]; + out_3533519720904001040[43] = -state[3]; + out_3533519720904001040[44] = 0; + out_3533519720904001040[45] = 0; + out_3533519720904001040[46] = 0; + out_3533519720904001040[47] = 0; + out_3533519720904001040[48] = 0; + out_3533519720904001040[49] = 0; + out_3533519720904001040[50] = 1; + out_3533519720904001040[51] = 0; + out_3533519720904001040[52] = 0; + out_3533519720904001040[53] = 1; } -void h_13(double *state, double *unused, double *out_8788718686171382623) { - out_8788718686171382623[0] = state[3]; - out_8788718686171382623[1] = state[4]; - out_8788718686171382623[2] = state[5]; +void h_13(double *state, double *unused, double *out_3025354993842492520) { + out_3025354993842492520[0] = state[3]; + out_3025354993842492520[1] = state[4]; + out_3025354993842492520[2] = state[5]; } -void H_13(double *state, double *unused, double *out_5558757735572352728) { - out_5558757735572352728[0] = 0; - out_5558757735572352728[1] = 0; - out_5558757735572352728[2] = 0; - out_5558757735572352728[3] = 1; - out_5558757735572352728[4] = 0; - out_5558757735572352728[5] = 0; - out_5558757735572352728[6] = 0; - out_5558757735572352728[7] = 0; - out_5558757735572352728[8] = 0; - out_5558757735572352728[9] = 0; - out_5558757735572352728[10] = 0; - out_5558757735572352728[11] = 0; - out_5558757735572352728[12] = 0; - out_5558757735572352728[13] = 0; - out_5558757735572352728[14] = 0; - out_5558757735572352728[15] = 0; - out_5558757735572352728[16] = 0; - out_5558757735572352728[17] = 0; - out_5558757735572352728[18] = 0; - out_5558757735572352728[19] = 0; - out_5558757735572352728[20] = 0; - out_5558757735572352728[21] = 0; - out_5558757735572352728[22] = 1; - out_5558757735572352728[23] = 0; - out_5558757735572352728[24] = 0; - out_5558757735572352728[25] = 0; - out_5558757735572352728[26] = 0; - out_5558757735572352728[27] = 0; - out_5558757735572352728[28] = 0; - out_5558757735572352728[29] = 0; - out_5558757735572352728[30] = 0; - out_5558757735572352728[31] = 0; - out_5558757735572352728[32] = 0; - out_5558757735572352728[33] = 0; - out_5558757735572352728[34] = 0; - out_5558757735572352728[35] = 0; - out_5558757735572352728[36] = 0; - out_5558757735572352728[37] = 0; - out_5558757735572352728[38] = 0; - out_5558757735572352728[39] = 0; - out_5558757735572352728[40] = 0; - out_5558757735572352728[41] = 1; - out_5558757735572352728[42] = 0; - out_5558757735572352728[43] = 0; - out_5558757735572352728[44] = 0; - out_5558757735572352728[45] = 0; - out_5558757735572352728[46] = 0; - out_5558757735572352728[47] = 0; - out_5558757735572352728[48] = 0; - out_5558757735572352728[49] = 0; - out_5558757735572352728[50] = 0; - out_5558757735572352728[51] = 0; - out_5558757735572352728[52] = 0; - out_5558757735572352728[53] = 0; +void H_13(double *state, double *unused, double *out_3389675289618495449) { + out_3389675289618495449[0] = 0; + out_3389675289618495449[1] = 0; + out_3389675289618495449[2] = 0; + out_3389675289618495449[3] = 1; + out_3389675289618495449[4] = 0; + out_3389675289618495449[5] = 0; + out_3389675289618495449[6] = 0; + out_3389675289618495449[7] = 0; + out_3389675289618495449[8] = 0; + out_3389675289618495449[9] = 0; + out_3389675289618495449[10] = 0; + out_3389675289618495449[11] = 0; + out_3389675289618495449[12] = 0; + out_3389675289618495449[13] = 0; + out_3389675289618495449[14] = 0; + out_3389675289618495449[15] = 0; + out_3389675289618495449[16] = 0; + out_3389675289618495449[17] = 0; + out_3389675289618495449[18] = 0; + out_3389675289618495449[19] = 0; + out_3389675289618495449[20] = 0; + out_3389675289618495449[21] = 0; + out_3389675289618495449[22] = 1; + out_3389675289618495449[23] = 0; + out_3389675289618495449[24] = 0; + out_3389675289618495449[25] = 0; + out_3389675289618495449[26] = 0; + out_3389675289618495449[27] = 0; + out_3389675289618495449[28] = 0; + out_3389675289618495449[29] = 0; + out_3389675289618495449[30] = 0; + out_3389675289618495449[31] = 0; + out_3389675289618495449[32] = 0; + out_3389675289618495449[33] = 0; + out_3389675289618495449[34] = 0; + out_3389675289618495449[35] = 0; + out_3389675289618495449[36] = 0; + out_3389675289618495449[37] = 0; + out_3389675289618495449[38] = 0; + out_3389675289618495449[39] = 0; + out_3389675289618495449[40] = 0; + out_3389675289618495449[41] = 1; + out_3389675289618495449[42] = 0; + out_3389675289618495449[43] = 0; + out_3389675289618495449[44] = 0; + out_3389675289618495449[45] = 0; + out_3389675289618495449[46] = 0; + out_3389675289618495449[47] = 0; + out_3389675289618495449[48] = 0; + out_3389675289618495449[49] = 0; + out_3389675289618495449[50] = 0; + out_3389675289618495449[51] = 0; + out_3389675289618495449[52] = 0; + out_3389675289618495449[53] = 0; } -void h_14(double *state, double *unused, double *out_4044191392879282830) { - out_4044191392879282830[0] = state[6]; - out_4044191392879282830[1] = state[7]; - out_4044191392879282830[2] = state[8]; +void h_14(double *state, double *unused, double *out_278507277292912803) { + out_278507277292912803[0] = state[6]; + out_278507277292912803[1] = state[7]; + out_278507277292912803[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4807790704565201000) { - out_4807790704565201000[0] = 0; - out_4807790704565201000[1] = 0; - out_4807790704565201000[2] = 0; - out_4807790704565201000[3] = 0; - out_4807790704565201000[4] = 0; - out_4807790704565201000[5] = 0; - out_4807790704565201000[6] = 1; - out_4807790704565201000[7] = 0; - out_4807790704565201000[8] = 0; - out_4807790704565201000[9] = 0; - out_4807790704565201000[10] = 0; - out_4807790704565201000[11] = 0; - out_4807790704565201000[12] = 0; - out_4807790704565201000[13] = 0; - out_4807790704565201000[14] = 0; - out_4807790704565201000[15] = 0; - out_4807790704565201000[16] = 0; - out_4807790704565201000[17] = 0; - out_4807790704565201000[18] = 0; - out_4807790704565201000[19] = 0; - out_4807790704565201000[20] = 0; - out_4807790704565201000[21] = 0; - out_4807790704565201000[22] = 0; - out_4807790704565201000[23] = 0; - out_4807790704565201000[24] = 0; - out_4807790704565201000[25] = 1; - out_4807790704565201000[26] = 0; - out_4807790704565201000[27] = 0; - out_4807790704565201000[28] = 0; - out_4807790704565201000[29] = 0; - out_4807790704565201000[30] = 0; - out_4807790704565201000[31] = 0; - out_4807790704565201000[32] = 0; - out_4807790704565201000[33] = 0; - out_4807790704565201000[34] = 0; - out_4807790704565201000[35] = 0; - out_4807790704565201000[36] = 0; - out_4807790704565201000[37] = 0; - out_4807790704565201000[38] = 0; - out_4807790704565201000[39] = 0; - out_4807790704565201000[40] = 0; - out_4807790704565201000[41] = 0; - out_4807790704565201000[42] = 0; - out_4807790704565201000[43] = 0; - out_4807790704565201000[44] = 1; - out_4807790704565201000[45] = 0; - out_4807790704565201000[46] = 0; - out_4807790704565201000[47] = 0; - out_4807790704565201000[48] = 0; - out_4807790704565201000[49] = 0; - out_4807790704565201000[50] = 0; - out_4807790704565201000[51] = 0; - out_4807790704565201000[52] = 0; - out_4807790704565201000[53] = 0; +void H_14(double *state, double *unused, double *out_7037065641595711849) { + out_7037065641595711849[0] = 0; + out_7037065641595711849[1] = 0; + out_7037065641595711849[2] = 0; + out_7037065641595711849[3] = 0; + out_7037065641595711849[4] = 0; + out_7037065641595711849[5] = 0; + out_7037065641595711849[6] = 1; + out_7037065641595711849[7] = 0; + out_7037065641595711849[8] = 0; + out_7037065641595711849[9] = 0; + out_7037065641595711849[10] = 0; + out_7037065641595711849[11] = 0; + out_7037065641595711849[12] = 0; + out_7037065641595711849[13] = 0; + out_7037065641595711849[14] = 0; + out_7037065641595711849[15] = 0; + out_7037065641595711849[16] = 0; + out_7037065641595711849[17] = 0; + out_7037065641595711849[18] = 0; + out_7037065641595711849[19] = 0; + out_7037065641595711849[20] = 0; + out_7037065641595711849[21] = 0; + out_7037065641595711849[22] = 0; + out_7037065641595711849[23] = 0; + out_7037065641595711849[24] = 0; + out_7037065641595711849[25] = 1; + out_7037065641595711849[26] = 0; + out_7037065641595711849[27] = 0; + out_7037065641595711849[28] = 0; + out_7037065641595711849[29] = 0; + out_7037065641595711849[30] = 0; + out_7037065641595711849[31] = 0; + out_7037065641595711849[32] = 0; + out_7037065641595711849[33] = 0; + out_7037065641595711849[34] = 0; + out_7037065641595711849[35] = 0; + out_7037065641595711849[36] = 0; + out_7037065641595711849[37] = 0; + out_7037065641595711849[38] = 0; + out_7037065641595711849[39] = 0; + out_7037065641595711849[40] = 0; + out_7037065641595711849[41] = 0; + out_7037065641595711849[42] = 0; + out_7037065641595711849[43] = 0; + out_7037065641595711849[44] = 1; + out_7037065641595711849[45] = 0; + out_7037065641595711849[46] = 0; + out_7037065641595711849[47] = 0; + out_7037065641595711849[48] = 0; + out_7037065641595711849[49] = 0; + out_7037065641595711849[50] = 0; + out_7037065641595711849[51] = 0; + out_7037065641595711849[52] = 0; + out_7037065641595711849[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_8979246808243312501) { - err_fun(nom_x, delta_x, out_8979246808243312501); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1942165964233553274) { + err_fun(nom_x, delta_x, out_1942165964233553274); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6182129478777519212) { - inv_err_fun(nom_x, true_x, out_6182129478777519212); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2238116153961167505) { + inv_err_fun(nom_x, true_x, out_2238116153961167505); } -void pose_H_mod_fun(double *state, double *out_5496809482113736276) { - H_mod_fun(state, out_5496809482113736276); +void pose_H_mod_fun(double *state, double *out_4990084456757481789) { + H_mod_fun(state, out_4990084456757481789); } -void pose_f_fun(double *state, double dt, double *out_76749327147481970) { - f_fun(state, dt, out_76749327147481970); +void pose_f_fun(double *state, double dt, double *out_6296202895308932040) { + f_fun(state, dt, out_6296202895308932040); } -void pose_F_fun(double *state, double dt, double *out_7773273273846026145) { - F_fun(state, dt, out_7773273273846026145); +void pose_F_fun(double *state, double dt, double *out_909355394007943716) { + F_fun(state, dt, out_909355394007943716); } -void pose_h_4(double *state, double *unused, double *out_8836590006675901483) { - h_4(state, unused, out_8836590006675901483); +void pose_h_4(double *state, double *unused, double *out_6818977063477642683) { + h_4(state, unused, out_6818977063477642683); } -void pose_H_4(double *state, double *unused, double *out_8771031560904685529) { - H_4(state, unused, out_8771031560904685529); +void pose_H_4(double *state, double *unused, double *out_3954277209300339553) { + H_4(state, unused, out_3954277209300339553); } -void pose_h_10(double *state, double *unused, double *out_7833049906299002536) { - h_10(state, unused, out_7833049906299002536); +void pose_h_10(double *state, double *unused, double *out_3834013708689822319) { + h_10(state, unused, out_3834013708689822319); } -void pose_H_10(double *state, double *unused, double *out_1676230657278343882) { - H_10(state, unused, out_1676230657278343882); +void pose_H_10(double *state, double *unused, double *out_3533519720904001040) { + H_10(state, unused, out_3533519720904001040); } -void pose_h_13(double *state, double *unused, double *out_8788718686171382623) { - h_13(state, unused, out_8788718686171382623); +void pose_h_13(double *state, double *unused, double *out_3025354993842492520) { + h_13(state, unused, out_3025354993842492520); } -void pose_H_13(double *state, double *unused, double *out_5558757735572352728) { - H_13(state, unused, out_5558757735572352728); +void pose_H_13(double *state, double *unused, double *out_3389675289618495449) { + H_13(state, unused, out_3389675289618495449); } -void pose_h_14(double *state, double *unused, double *out_4044191392879282830) { - h_14(state, unused, out_4044191392879282830); +void pose_h_14(double *state, double *unused, double *out_278507277292912803) { + h_14(state, unused, out_278507277292912803); } -void pose_H_14(double *state, double *unused, double *out_4807790704565201000) { - H_14(state, unused, out_4807790704565201000); +void pose_H_14(double *state, double *unused, double *out_7037065641595711849) { + H_14(state, unused, out_7037065641595711849); } 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 a705f767c..eff15490e 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_8979246808243312501); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6182129478777519212); -void pose_H_mod_fun(double *state, double *out_5496809482113736276); -void pose_f_fun(double *state, double dt, double *out_76749327147481970); -void pose_F_fun(double *state, double dt, double *out_7773273273846026145); -void pose_h_4(double *state, double *unused, double *out_8836590006675901483); -void pose_H_4(double *state, double *unused, double *out_8771031560904685529); -void pose_h_10(double *state, double *unused, double *out_7833049906299002536); -void pose_H_10(double *state, double *unused, double *out_1676230657278343882); -void pose_h_13(double *state, double *unused, double *out_8788718686171382623); -void pose_H_13(double *state, double *unused, double *out_5558757735572352728); -void pose_h_14(double *state, double *unused, double *out_4044191392879282830); -void pose_H_14(double *state, double *unused, double *out_4807790704565201000); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1942165964233553274); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2238116153961167505); +void pose_H_mod_fun(double *state, double *out_4990084456757481789); +void pose_f_fun(double *state, double dt, double *out_6296202895308932040); +void pose_F_fun(double *state, double dt, double *out_909355394007943716); +void pose_h_4(double *state, double *unused, double *out_6818977063477642683); +void pose_H_4(double *state, double *unused, double *out_3954277209300339553); +void pose_h_10(double *state, double *unused, double *out_3834013708689822319); +void pose_H_10(double *state, double *unused, double *out_3533519720904001040); +void pose_h_13(double *state, double *unused, double *out_3025354993842492520); +void pose_H_13(double *state, double *unused, double *out_3389675289618495449); +void pose_h_14(double *state, double *unused, double *out_278507277292912803); +void pose_H_14(double *state, double *unused, double *out_7037065641595711849); 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 98c7ab37f..32cc04217 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index f55f6b772..117b9fbe9 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index bce5a08c0..a19ff0791 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index bb12b9d0e..f4c10e3f2 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index a8c3d6b5c..794e7077e 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 6c416b698..1b818473f 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ