diff --git a/common/libcommon.a b/common/libcommon.a index a6dca1234..52e9536c7 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 b5e8953ee..f2824f863 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 b505bf53e..acf7d8090 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 de58de734..d688605cf 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 ea939aae9..2f1adcba1 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 94e12c3a2..10cff1011 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 6197fd071..faf199d24 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 97bf5b8d5..cd4656d23 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 ce3f8169f..9afc99b74 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 ce3f8169f..9afc99b74 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 a8157dace..01e96ad81 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 97bf5b8d5..cd4656d23 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 287412014..a86cc8f40 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-cf1c99cf-DEBUG"; +const uint8_t gitversion[19] = "DEV-c88bebbc-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 8a62c12d8..c92f1c81c 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 912996ee2..5767fa19c 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 221562edb..7af95778d 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 9b2bd61be..f42821f93 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 ffbf53155..e2f4a30d7 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 c6e209a97..e8ce7ac72 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 f365adca4..bbb687427 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 b191170a9..5b2d0c0fc 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 8a2a01ec2..cd696eabc 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 81254563c..dd525298e 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 38b80994a..64910b8ba 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 6a5f1f31b..b3bfb0ce3 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 e5426fbc7..21327e988 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 10af1b047..2f9f88778 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 935a481db..28d5cf4a7 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 026ab6bac..19272b975 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 7346f7356..6653bb5a8 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 9216d7fc1..f01ad2573 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 a37232ec0..da665d81f 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 a0d68aa4d..33f6466b4 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 1b8335a7e..aa4841c99 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-cf1c99cf-DEBUG \ No newline at end of file +DEV-c88bebbc-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 485df0582..a53c5da24 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_5319131797443970272) { - out_5319131797443970272[0] = delta_x[0] + nom_x[0]; - out_5319131797443970272[1] = delta_x[1] + nom_x[1]; - out_5319131797443970272[2] = delta_x[2] + nom_x[2]; - out_5319131797443970272[3] = delta_x[3] + nom_x[3]; - out_5319131797443970272[4] = delta_x[4] + nom_x[4]; - out_5319131797443970272[5] = delta_x[5] + nom_x[5]; - out_5319131797443970272[6] = delta_x[6] + nom_x[6]; - out_5319131797443970272[7] = delta_x[7] + nom_x[7]; - out_5319131797443970272[8] = delta_x[8] + nom_x[8]; +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 inv_err_fun(double *nom_x, double *true_x, double *out_3644993322176215676) { - out_3644993322176215676[0] = -nom_x[0] + true_x[0]; - out_3644993322176215676[1] = -nom_x[1] + true_x[1]; - out_3644993322176215676[2] = -nom_x[2] + true_x[2]; - out_3644993322176215676[3] = -nom_x[3] + true_x[3]; - out_3644993322176215676[4] = -nom_x[4] + true_x[4]; - out_3644993322176215676[5] = -nom_x[5] + true_x[5]; - out_3644993322176215676[6] = -nom_x[6] + true_x[6]; - out_3644993322176215676[7] = -nom_x[7] + true_x[7]; - out_3644993322176215676[8] = -nom_x[8] + true_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 H_mod_fun(double *state, double *out_3163511227028018782) { - out_3163511227028018782[0] = 1.0; - out_3163511227028018782[1] = 0.0; - out_3163511227028018782[2] = 0.0; - out_3163511227028018782[3] = 0.0; - out_3163511227028018782[4] = 0.0; - out_3163511227028018782[5] = 0.0; - out_3163511227028018782[6] = 0.0; - out_3163511227028018782[7] = 0.0; - out_3163511227028018782[8] = 0.0; - out_3163511227028018782[9] = 0.0; - out_3163511227028018782[10] = 1.0; - out_3163511227028018782[11] = 0.0; - out_3163511227028018782[12] = 0.0; - out_3163511227028018782[13] = 0.0; - out_3163511227028018782[14] = 0.0; - out_3163511227028018782[15] = 0.0; - out_3163511227028018782[16] = 0.0; - out_3163511227028018782[17] = 0.0; - out_3163511227028018782[18] = 0.0; - out_3163511227028018782[19] = 0.0; - out_3163511227028018782[20] = 1.0; - out_3163511227028018782[21] = 0.0; - out_3163511227028018782[22] = 0.0; - out_3163511227028018782[23] = 0.0; - out_3163511227028018782[24] = 0.0; - out_3163511227028018782[25] = 0.0; - out_3163511227028018782[26] = 0.0; - out_3163511227028018782[27] = 0.0; - out_3163511227028018782[28] = 0.0; - out_3163511227028018782[29] = 0.0; - out_3163511227028018782[30] = 1.0; - out_3163511227028018782[31] = 0.0; - out_3163511227028018782[32] = 0.0; - out_3163511227028018782[33] = 0.0; - out_3163511227028018782[34] = 0.0; - out_3163511227028018782[35] = 0.0; - out_3163511227028018782[36] = 0.0; - out_3163511227028018782[37] = 0.0; - out_3163511227028018782[38] = 0.0; - out_3163511227028018782[39] = 0.0; - out_3163511227028018782[40] = 1.0; - out_3163511227028018782[41] = 0.0; - out_3163511227028018782[42] = 0.0; - out_3163511227028018782[43] = 0.0; - out_3163511227028018782[44] = 0.0; - out_3163511227028018782[45] = 0.0; - out_3163511227028018782[46] = 0.0; - out_3163511227028018782[47] = 0.0; - out_3163511227028018782[48] = 0.0; - out_3163511227028018782[49] = 0.0; - out_3163511227028018782[50] = 1.0; - out_3163511227028018782[51] = 0.0; - out_3163511227028018782[52] = 0.0; - out_3163511227028018782[53] = 0.0; - out_3163511227028018782[54] = 0.0; - out_3163511227028018782[55] = 0.0; - out_3163511227028018782[56] = 0.0; - out_3163511227028018782[57] = 0.0; - out_3163511227028018782[58] = 0.0; - out_3163511227028018782[59] = 0.0; - out_3163511227028018782[60] = 1.0; - out_3163511227028018782[61] = 0.0; - out_3163511227028018782[62] = 0.0; - out_3163511227028018782[63] = 0.0; - out_3163511227028018782[64] = 0.0; - out_3163511227028018782[65] = 0.0; - out_3163511227028018782[66] = 0.0; - out_3163511227028018782[67] = 0.0; - out_3163511227028018782[68] = 0.0; - out_3163511227028018782[69] = 0.0; - out_3163511227028018782[70] = 1.0; - out_3163511227028018782[71] = 0.0; - out_3163511227028018782[72] = 0.0; - out_3163511227028018782[73] = 0.0; - out_3163511227028018782[74] = 0.0; - out_3163511227028018782[75] = 0.0; - out_3163511227028018782[76] = 0.0; - out_3163511227028018782[77] = 0.0; - out_3163511227028018782[78] = 0.0; - out_3163511227028018782[79] = 0.0; - out_3163511227028018782[80] = 1.0; +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 f_fun(double *state, double dt, double *out_1546760495327089109) { - out_1546760495327089109[0] = state[0]; - out_1546760495327089109[1] = state[1]; - out_1546760495327089109[2] = state[2]; - out_1546760495327089109[3] = state[3]; - out_1546760495327089109[4] = state[4]; - out_1546760495327089109[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_1546760495327089109[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_1546760495327089109[7] = state[7]; - out_1546760495327089109[8] = state[8]; +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_3348391828080031822) { - out_3348391828080031822[0] = 1; - out_3348391828080031822[1] = 0; - out_3348391828080031822[2] = 0; - out_3348391828080031822[3] = 0; - out_3348391828080031822[4] = 0; - out_3348391828080031822[5] = 0; - out_3348391828080031822[6] = 0; - out_3348391828080031822[7] = 0; - out_3348391828080031822[8] = 0; - out_3348391828080031822[9] = 0; - out_3348391828080031822[10] = 1; - out_3348391828080031822[11] = 0; - out_3348391828080031822[12] = 0; - out_3348391828080031822[13] = 0; - out_3348391828080031822[14] = 0; - out_3348391828080031822[15] = 0; - out_3348391828080031822[16] = 0; - out_3348391828080031822[17] = 0; - out_3348391828080031822[18] = 0; - out_3348391828080031822[19] = 0; - out_3348391828080031822[20] = 1; - out_3348391828080031822[21] = 0; - out_3348391828080031822[22] = 0; - out_3348391828080031822[23] = 0; - out_3348391828080031822[24] = 0; - out_3348391828080031822[25] = 0; - out_3348391828080031822[26] = 0; - out_3348391828080031822[27] = 0; - out_3348391828080031822[28] = 0; - out_3348391828080031822[29] = 0; - out_3348391828080031822[30] = 1; - out_3348391828080031822[31] = 0; - out_3348391828080031822[32] = 0; - out_3348391828080031822[33] = 0; - out_3348391828080031822[34] = 0; - out_3348391828080031822[35] = 0; - out_3348391828080031822[36] = 0; - out_3348391828080031822[37] = 0; - out_3348391828080031822[38] = 0; - out_3348391828080031822[39] = 0; - out_3348391828080031822[40] = 1; - out_3348391828080031822[41] = 0; - out_3348391828080031822[42] = 0; - out_3348391828080031822[43] = 0; - out_3348391828080031822[44] = 0; - out_3348391828080031822[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_3348391828080031822[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_3348391828080031822[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3348391828080031822[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3348391828080031822[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_3348391828080031822[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_3348391828080031822[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_3348391828080031822[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_3348391828080031822[53] = -9.8100000000000005*dt; - out_3348391828080031822[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_3348391828080031822[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_3348391828080031822[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3348391828080031822[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3348391828080031822[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_3348391828080031822[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_3348391828080031822[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_3348391828080031822[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3348391828080031822[62] = 0; - out_3348391828080031822[63] = 0; - out_3348391828080031822[64] = 0; - out_3348391828080031822[65] = 0; - out_3348391828080031822[66] = 0; - out_3348391828080031822[67] = 0; - out_3348391828080031822[68] = 0; - out_3348391828080031822[69] = 0; - out_3348391828080031822[70] = 1; - out_3348391828080031822[71] = 0; - out_3348391828080031822[72] = 0; - out_3348391828080031822[73] = 0; - out_3348391828080031822[74] = 0; - out_3348391828080031822[75] = 0; - out_3348391828080031822[76] = 0; - out_3348391828080031822[77] = 0; - out_3348391828080031822[78] = 0; - out_3348391828080031822[79] = 0; - out_3348391828080031822[80] = 1; +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 h_25(double *state, double *unused, double *out_4471759016602712561) { - out_4471759016602712561[0] = state[6]; +void h_25(double *state, double *unused, double *out_3102286071066017000) { + out_3102286071066017000[0] = state[6]; } -void H_25(double *state, double *unused, double *out_1740180598156646058) { - out_1740180598156646058[0] = 0; - out_1740180598156646058[1] = 0; - out_1740180598156646058[2] = 0; - out_1740180598156646058[3] = 0; - out_1740180598156646058[4] = 0; - out_1740180598156646058[5] = 0; - out_1740180598156646058[6] = 1; - out_1740180598156646058[7] = 0; - out_1740180598156646058[8] = 0; +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_24(double *state, double *unused, double *out_3126254991737006136) { - out_3126254991737006136[0] = state[4]; - out_3126254991737006136[1] = state[5]; +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_1269723116113307334) { - out_1269723116113307334[0] = 0; - out_1269723116113307334[1] = 0; - out_1269723116113307334[2] = 0; - out_1269723116113307334[3] = 0; - out_1269723116113307334[4] = 1; - out_1269723116113307334[5] = 0; - out_1269723116113307334[6] = 0; - out_1269723116113307334[7] = 0; - out_1269723116113307334[8] = 0; - out_1269723116113307334[9] = 0; - out_1269723116113307334[10] = 0; - out_1269723116113307334[11] = 0; - out_1269723116113307334[12] = 0; - out_1269723116113307334[13] = 0; - out_1269723116113307334[14] = 1; - out_1269723116113307334[15] = 0; - out_1269723116113307334[16] = 0; - out_1269723116113307334[17] = 0; +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_30(double *state, double *unused, double *out_4168118460115829323) { - out_4168118460115829323[0] = state[4]; +void h_30(double *state, double *unused, double *out_3668549155284333936) { + out_3668549155284333936[0] = state[4]; } -void H_30(double *state, double *unused, double *out_1610841651013405988) { - out_1610841651013405988[0] = 0; - out_1610841651013405988[1] = 0; - out_1610841651013405988[2] = 0; - out_1610841651013405988[3] = 0; - out_1610841651013405988[4] = 1; - out_1610841651013405988[5] = 0; - out_1610841651013405988[6] = 0; - out_1610841651013405988[7] = 0; - out_1610841651013405988[8] = 0; +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_26(double *state, double *unused, double *out_2045289894744513329) { - out_2045289894744513329[0] = state[7]; +void h_26(double *state, double *unused, double *out_7985951944793485901) { + out_7985951944793485901[0] = state[7]; } -void H_26(double *state, double *unused, double *out_2001322720717410166) { - out_2001322720717410166[0] = 0; - out_2001322720717410166[1] = 0; - out_2001322720717410166[2] = 0; - out_2001322720717410166[3] = 0; - out_2001322720717410166[4] = 0; - out_2001322720717410166[5] = 0; - out_2001322720717410166[6] = 0; - out_2001322720717410166[7] = 1; - out_2001322720717410166[8] = 0; +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_27(double *state, double *unused, double *out_1927282500811136585) { - out_1927282500811136585[0] = state[3]; +void h_27(double *state, double *unused, double *out_8103959338726862645) { + out_8103959338726862645[0] = state[3]; } -void H_27(double *state, double *unused, double *out_563921660787018923) { - out_563921660787018923[0] = 0; - out_563921660787018923[1] = 0; - out_563921660787018923[2] = 0; - out_563921660787018923[3] = 1; - out_563921660787018923[4] = 0; - out_563921660787018923[5] = 0; - out_563921660787018923[6] = 0; - out_563921660787018923[7] = 0; - out_563921660787018923[8] = 0; +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_29(double *state, double *unused, double *out_8022800891634675039) { - out_8022800891634675039[0] = state[1]; +void h_29(double *state, double *unused, double *out_6653327946097979478) { + out_6653327946097979478[0] = state[1]; } -void H_29(double *state, double *unused, double *out_2277284387656569956) { - out_2277284387656569956[0] = 0; - out_2277284387656569956[1] = 1; - out_2277284387656569956[2] = 0; - out_2277284387656569956[3] = 0; - out_2277284387656569956[4] = 0; - out_2277284387656569956[5] = 0; - out_2277284387656569956[6] = 0; - out_2277284387656569956[7] = 0; - out_2277284387656569956[8] = 0; +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_28(double *state, double *unused, double *out_4964068552255469270) { - out_4964068552255469270[0] = state[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_313654116091243705) { - out_313654116091243705[0] = 1; - out_313654116091243705[1] = 0; - out_313654116091243705[2] = 0; - out_313654116091243705[3] = 0; - out_313654116091243705[4] = 0; - out_313654116091243705[5] = 0; - out_313654116091243705[6] = 0; - out_313654116091243705[7] = 0; - out_313654116091243705[8] = 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_31(double *state, double *unused, double *out_834309488348690211) { - out_834309488348690211[0] = state[8]; +void h_31(double *state, double *unused, double *out_7581192745822862175) { + out_7581192745822862175[0] = state[8]; } -void H_31(double *state, double *unused, double *out_1770826560033606486) { - out_1770826560033606486[0] = 0; - out_1770826560033606486[1] = 0; - out_1770826560033606486[2] = 0; - out_1770826560033606486[3] = 0; - out_1770826560033606486[4] = 0; - out_1770826560033606486[5] = 0; - out_1770826560033606486[6] = 0; - out_1770826560033606486[7] = 0; - out_1770826560033606486[8] = 1; +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; } #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_5319131797443970272) { - err_fun(nom_x, delta_x, out_5319131797443970272); +void car_err_fun(double *nom_x, double *delta_x, double *out_4102855695027319146) { + err_fun(nom_x, delta_x, out_4102855695027319146); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3644993322176215676) { - inv_err_fun(nom_x, true_x, out_3644993322176215676); +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_H_mod_fun(double *state, double *out_3163511227028018782) { - H_mod_fun(state, out_3163511227028018782); +void car_H_mod_fun(double *state, double *out_4142214806030893418) { + H_mod_fun(state, out_4142214806030893418); } -void car_f_fun(double *state, double dt, double *out_1546760495327089109) { - f_fun(state, dt, out_1546760495327089109); +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_3348391828080031822) { - F_fun(state, dt, out_3348391828080031822); +void car_F_fun(double *state, double dt, double *out_2970389157093285155) { + F_fun(state, dt, out_2970389157093285155); } -void car_h_25(double *state, double *unused, double *out_4471759016602712561) { - h_25(state, unused, out_4471759016602712561); +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_1740180598156646058) { - H_25(state, unused, out_1740180598156646058); +void car_H_25(double *state, double *unused, double *out_7624833147842353287) { + H_25(state, unused, out_7624833147842353287); } -void car_h_24(double *state, double *unused, double *out_3126254991737006136) { - h_24(state, unused, out_3126254991737006136); +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_1269723116113307334) { - H_24(state, unused, out_1269723116113307334); +void car_H_24(double *state, double *unused, double *out_5399125363863484725) { + H_24(state, unused, out_5399125363863484725); } -void car_h_30(double *state, double *unused, double *out_4168118460115829323) { - h_30(state, unused, out_4168118460115829323); +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_1610841651013405988) { - H_30(state, unused, out_1610841651013405988); +void car_H_30(double *state, double *unused, double *out_708142806350736532) { + H_30(state, unused, out_708142806350736532); } -void car_h_26(double *state, double *unused, double *out_2045289894744513329) { - h_26(state, unused, out_2045289894744513329); +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_2001322720717410166) { - H_26(state, unused, out_2001322720717410166); +void car_H_26(double *state, double *unused, double *out_7080407606993142105) { + H_26(state, unused, out_7080407606993142105); } -void car_h_27(double *state, double *unused, double *out_1927282500811136585) { - h_27(state, unused, out_1927282500811136585); +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_563921660787018923) { - H_27(state, unused, out_563921660787018923); +void car_H_27(double *state, double *unused, double *out_2882906118151161443) { + H_27(state, unused, out_2882906118151161443); } -void car_h_29(double *state, double *unused, double *out_8022800891634675039) { - h_29(state, unused, out_8022800891634675039); +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_2277284387656569956) { - H_29(state, unused, out_2277284387656569956); +void car_H_29(double *state, double *unused, double *out_197911462036344348) { + H_29(state, unused, out_197911462036344348); } -void car_h_28(double *state, double *unused, double *out_4964068552255469270) { - h_28(state, unused, out_4964068552255469270); +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_313654116091243705) { - H_28(state, unused, out_313654116091243705); +void car_H_28(double *state, double *unused, double *out_8768076211619308566) { + H_28(state, unused, out_8768076211619308566); } -void car_h_31(double *state, double *unused, double *out_834309488348690211) { - h_31(state, unused, out_834309488348690211); +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_1770826560033606486) { - H_31(state, unused, out_1770826560033606486); +void car_H_31(double *state, double *unused, double *out_7594187185965392859) { + H_31(state, unused, out_7594187185965392859); } 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 7dd345d4f..6452464c1 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_5319131797443970272); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3644993322176215676); -void car_H_mod_fun(double *state, double *out_3163511227028018782); -void car_f_fun(double *state, double dt, double *out_1546760495327089109); -void car_F_fun(double *state, double dt, double *out_3348391828080031822); -void car_h_25(double *state, double *unused, double *out_4471759016602712561); -void car_H_25(double *state, double *unused, double *out_1740180598156646058); -void car_h_24(double *state, double *unused, double *out_3126254991737006136); -void car_H_24(double *state, double *unused, double *out_1269723116113307334); -void car_h_30(double *state, double *unused, double *out_4168118460115829323); -void car_H_30(double *state, double *unused, double *out_1610841651013405988); -void car_h_26(double *state, double *unused, double *out_2045289894744513329); -void car_H_26(double *state, double *unused, double *out_2001322720717410166); -void car_h_27(double *state, double *unused, double *out_1927282500811136585); -void car_H_27(double *state, double *unused, double *out_563921660787018923); -void car_h_29(double *state, double *unused, double *out_8022800891634675039); -void car_H_29(double *state, double *unused, double *out_2277284387656569956); -void car_h_28(double *state, double *unused, double *out_4964068552255469270); -void car_H_28(double *state, double *unused, double *out_313654116091243705); -void car_h_31(double *state, double *unused, double *out_834309488348690211); -void car_H_31(double *state, double *unused, double *out_1770826560033606486); +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_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 26ea1ddf6..a794930ff 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_9044617002367965712) { - out_9044617002367965712[0] = delta_x[0] + nom_x[0]; - out_9044617002367965712[1] = delta_x[1] + nom_x[1]; - out_9044617002367965712[2] = delta_x[2] + nom_x[2]; - out_9044617002367965712[3] = delta_x[3] + nom_x[3]; - out_9044617002367965712[4] = delta_x[4] + nom_x[4]; - out_9044617002367965712[5] = delta_x[5] + nom_x[5]; - out_9044617002367965712[6] = delta_x[6] + nom_x[6]; - out_9044617002367965712[7] = delta_x[7] + nom_x[7]; - out_9044617002367965712[8] = delta_x[8] + nom_x[8]; - out_9044617002367965712[9] = delta_x[9] + nom_x[9]; - out_9044617002367965712[10] = delta_x[10] + nom_x[10]; - out_9044617002367965712[11] = delta_x[11] + nom_x[11]; - out_9044617002367965712[12] = delta_x[12] + nom_x[12]; - out_9044617002367965712[13] = delta_x[13] + nom_x[13]; - out_9044617002367965712[14] = delta_x[14] + nom_x[14]; - out_9044617002367965712[15] = delta_x[15] + nom_x[15]; - out_9044617002367965712[16] = delta_x[16] + nom_x[16]; - out_9044617002367965712[17] = delta_x[17] + nom_x[17]; +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 inv_err_fun(double *nom_x, double *true_x, double *out_2986388781113821274) { - out_2986388781113821274[0] = -nom_x[0] + true_x[0]; - out_2986388781113821274[1] = -nom_x[1] + true_x[1]; - out_2986388781113821274[2] = -nom_x[2] + true_x[2]; - out_2986388781113821274[3] = -nom_x[3] + true_x[3]; - out_2986388781113821274[4] = -nom_x[4] + true_x[4]; - out_2986388781113821274[5] = -nom_x[5] + true_x[5]; - out_2986388781113821274[6] = -nom_x[6] + true_x[6]; - out_2986388781113821274[7] = -nom_x[7] + true_x[7]; - out_2986388781113821274[8] = -nom_x[8] + true_x[8]; - out_2986388781113821274[9] = -nom_x[9] + true_x[9]; - out_2986388781113821274[10] = -nom_x[10] + true_x[10]; - out_2986388781113821274[11] = -nom_x[11] + true_x[11]; - out_2986388781113821274[12] = -nom_x[12] + true_x[12]; - out_2986388781113821274[13] = -nom_x[13] + true_x[13]; - out_2986388781113821274[14] = -nom_x[14] + true_x[14]; - out_2986388781113821274[15] = -nom_x[15] + true_x[15]; - out_2986388781113821274[16] = -nom_x[16] + true_x[16]; - out_2986388781113821274[17] = -nom_x[17] + true_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 H_mod_fun(double *state, double *out_5449976102936300367) { - out_5449976102936300367[0] = 1.0; - out_5449976102936300367[1] = 0.0; - out_5449976102936300367[2] = 0.0; - out_5449976102936300367[3] = 0.0; - out_5449976102936300367[4] = 0.0; - out_5449976102936300367[5] = 0.0; - out_5449976102936300367[6] = 0.0; - out_5449976102936300367[7] = 0.0; - out_5449976102936300367[8] = 0.0; - out_5449976102936300367[9] = 0.0; - out_5449976102936300367[10] = 0.0; - out_5449976102936300367[11] = 0.0; - out_5449976102936300367[12] = 0.0; - out_5449976102936300367[13] = 0.0; - out_5449976102936300367[14] = 0.0; - out_5449976102936300367[15] = 0.0; - out_5449976102936300367[16] = 0.0; - out_5449976102936300367[17] = 0.0; - out_5449976102936300367[18] = 0.0; - out_5449976102936300367[19] = 1.0; - out_5449976102936300367[20] = 0.0; - out_5449976102936300367[21] = 0.0; - out_5449976102936300367[22] = 0.0; - out_5449976102936300367[23] = 0.0; - out_5449976102936300367[24] = 0.0; - out_5449976102936300367[25] = 0.0; - out_5449976102936300367[26] = 0.0; - out_5449976102936300367[27] = 0.0; - out_5449976102936300367[28] = 0.0; - out_5449976102936300367[29] = 0.0; - out_5449976102936300367[30] = 0.0; - out_5449976102936300367[31] = 0.0; - out_5449976102936300367[32] = 0.0; - out_5449976102936300367[33] = 0.0; - out_5449976102936300367[34] = 0.0; - out_5449976102936300367[35] = 0.0; - out_5449976102936300367[36] = 0.0; - out_5449976102936300367[37] = 0.0; - out_5449976102936300367[38] = 1.0; - out_5449976102936300367[39] = 0.0; - out_5449976102936300367[40] = 0.0; - out_5449976102936300367[41] = 0.0; - out_5449976102936300367[42] = 0.0; - out_5449976102936300367[43] = 0.0; - out_5449976102936300367[44] = 0.0; - out_5449976102936300367[45] = 0.0; - out_5449976102936300367[46] = 0.0; - out_5449976102936300367[47] = 0.0; - out_5449976102936300367[48] = 0.0; - out_5449976102936300367[49] = 0.0; - out_5449976102936300367[50] = 0.0; - out_5449976102936300367[51] = 0.0; - out_5449976102936300367[52] = 0.0; - out_5449976102936300367[53] = 0.0; - out_5449976102936300367[54] = 0.0; - out_5449976102936300367[55] = 0.0; - out_5449976102936300367[56] = 0.0; - out_5449976102936300367[57] = 1.0; - out_5449976102936300367[58] = 0.0; - out_5449976102936300367[59] = 0.0; - out_5449976102936300367[60] = 0.0; - out_5449976102936300367[61] = 0.0; - out_5449976102936300367[62] = 0.0; - out_5449976102936300367[63] = 0.0; - out_5449976102936300367[64] = 0.0; - out_5449976102936300367[65] = 0.0; - out_5449976102936300367[66] = 0.0; - out_5449976102936300367[67] = 0.0; - out_5449976102936300367[68] = 0.0; - out_5449976102936300367[69] = 0.0; - out_5449976102936300367[70] = 0.0; - out_5449976102936300367[71] = 0.0; - out_5449976102936300367[72] = 0.0; - out_5449976102936300367[73] = 0.0; - out_5449976102936300367[74] = 0.0; - out_5449976102936300367[75] = 0.0; - out_5449976102936300367[76] = 1.0; - out_5449976102936300367[77] = 0.0; - out_5449976102936300367[78] = 0.0; - out_5449976102936300367[79] = 0.0; - out_5449976102936300367[80] = 0.0; - out_5449976102936300367[81] = 0.0; - out_5449976102936300367[82] = 0.0; - out_5449976102936300367[83] = 0.0; - out_5449976102936300367[84] = 0.0; - out_5449976102936300367[85] = 0.0; - out_5449976102936300367[86] = 0.0; - out_5449976102936300367[87] = 0.0; - out_5449976102936300367[88] = 0.0; - out_5449976102936300367[89] = 0.0; - out_5449976102936300367[90] = 0.0; - out_5449976102936300367[91] = 0.0; - out_5449976102936300367[92] = 0.0; - out_5449976102936300367[93] = 0.0; - out_5449976102936300367[94] = 0.0; - out_5449976102936300367[95] = 1.0; - out_5449976102936300367[96] = 0.0; - out_5449976102936300367[97] = 0.0; - out_5449976102936300367[98] = 0.0; - out_5449976102936300367[99] = 0.0; - out_5449976102936300367[100] = 0.0; - out_5449976102936300367[101] = 0.0; - out_5449976102936300367[102] = 0.0; - out_5449976102936300367[103] = 0.0; - out_5449976102936300367[104] = 0.0; - out_5449976102936300367[105] = 0.0; - out_5449976102936300367[106] = 0.0; - out_5449976102936300367[107] = 0.0; - out_5449976102936300367[108] = 0.0; - out_5449976102936300367[109] = 0.0; - out_5449976102936300367[110] = 0.0; - out_5449976102936300367[111] = 0.0; - out_5449976102936300367[112] = 0.0; - out_5449976102936300367[113] = 0.0; - out_5449976102936300367[114] = 1.0; - out_5449976102936300367[115] = 0.0; - out_5449976102936300367[116] = 0.0; - out_5449976102936300367[117] = 0.0; - out_5449976102936300367[118] = 0.0; - out_5449976102936300367[119] = 0.0; - out_5449976102936300367[120] = 0.0; - out_5449976102936300367[121] = 0.0; - out_5449976102936300367[122] = 0.0; - out_5449976102936300367[123] = 0.0; - out_5449976102936300367[124] = 0.0; - out_5449976102936300367[125] = 0.0; - out_5449976102936300367[126] = 0.0; - out_5449976102936300367[127] = 0.0; - out_5449976102936300367[128] = 0.0; - out_5449976102936300367[129] = 0.0; - out_5449976102936300367[130] = 0.0; - out_5449976102936300367[131] = 0.0; - out_5449976102936300367[132] = 0.0; - out_5449976102936300367[133] = 1.0; - out_5449976102936300367[134] = 0.0; - out_5449976102936300367[135] = 0.0; - out_5449976102936300367[136] = 0.0; - out_5449976102936300367[137] = 0.0; - out_5449976102936300367[138] = 0.0; - out_5449976102936300367[139] = 0.0; - out_5449976102936300367[140] = 0.0; - out_5449976102936300367[141] = 0.0; - out_5449976102936300367[142] = 0.0; - out_5449976102936300367[143] = 0.0; - out_5449976102936300367[144] = 0.0; - out_5449976102936300367[145] = 0.0; - out_5449976102936300367[146] = 0.0; - out_5449976102936300367[147] = 0.0; - out_5449976102936300367[148] = 0.0; - out_5449976102936300367[149] = 0.0; - out_5449976102936300367[150] = 0.0; - out_5449976102936300367[151] = 0.0; - out_5449976102936300367[152] = 1.0; - out_5449976102936300367[153] = 0.0; - out_5449976102936300367[154] = 0.0; - out_5449976102936300367[155] = 0.0; - out_5449976102936300367[156] = 0.0; - out_5449976102936300367[157] = 0.0; - out_5449976102936300367[158] = 0.0; - out_5449976102936300367[159] = 0.0; - out_5449976102936300367[160] = 0.0; - out_5449976102936300367[161] = 0.0; - out_5449976102936300367[162] = 0.0; - out_5449976102936300367[163] = 0.0; - out_5449976102936300367[164] = 0.0; - out_5449976102936300367[165] = 0.0; - out_5449976102936300367[166] = 0.0; - out_5449976102936300367[167] = 0.0; - out_5449976102936300367[168] = 0.0; - out_5449976102936300367[169] = 0.0; - out_5449976102936300367[170] = 0.0; - out_5449976102936300367[171] = 1.0; - out_5449976102936300367[172] = 0.0; - out_5449976102936300367[173] = 0.0; - out_5449976102936300367[174] = 0.0; - out_5449976102936300367[175] = 0.0; - out_5449976102936300367[176] = 0.0; - out_5449976102936300367[177] = 0.0; - out_5449976102936300367[178] = 0.0; - out_5449976102936300367[179] = 0.0; - out_5449976102936300367[180] = 0.0; - out_5449976102936300367[181] = 0.0; - out_5449976102936300367[182] = 0.0; - out_5449976102936300367[183] = 0.0; - out_5449976102936300367[184] = 0.0; - out_5449976102936300367[185] = 0.0; - out_5449976102936300367[186] = 0.0; - out_5449976102936300367[187] = 0.0; - out_5449976102936300367[188] = 0.0; - out_5449976102936300367[189] = 0.0; - out_5449976102936300367[190] = 1.0; - out_5449976102936300367[191] = 0.0; - out_5449976102936300367[192] = 0.0; - out_5449976102936300367[193] = 0.0; - out_5449976102936300367[194] = 0.0; - out_5449976102936300367[195] = 0.0; - out_5449976102936300367[196] = 0.0; - out_5449976102936300367[197] = 0.0; - out_5449976102936300367[198] = 0.0; - out_5449976102936300367[199] = 0.0; - out_5449976102936300367[200] = 0.0; - out_5449976102936300367[201] = 0.0; - out_5449976102936300367[202] = 0.0; - out_5449976102936300367[203] = 0.0; - out_5449976102936300367[204] = 0.0; - out_5449976102936300367[205] = 0.0; - out_5449976102936300367[206] = 0.0; - out_5449976102936300367[207] = 0.0; - out_5449976102936300367[208] = 0.0; - out_5449976102936300367[209] = 1.0; - out_5449976102936300367[210] = 0.0; - out_5449976102936300367[211] = 0.0; - out_5449976102936300367[212] = 0.0; - out_5449976102936300367[213] = 0.0; - out_5449976102936300367[214] = 0.0; - out_5449976102936300367[215] = 0.0; - out_5449976102936300367[216] = 0.0; - out_5449976102936300367[217] = 0.0; - out_5449976102936300367[218] = 0.0; - out_5449976102936300367[219] = 0.0; - out_5449976102936300367[220] = 0.0; - out_5449976102936300367[221] = 0.0; - out_5449976102936300367[222] = 0.0; - out_5449976102936300367[223] = 0.0; - out_5449976102936300367[224] = 0.0; - out_5449976102936300367[225] = 0.0; - out_5449976102936300367[226] = 0.0; - out_5449976102936300367[227] = 0.0; - out_5449976102936300367[228] = 1.0; - out_5449976102936300367[229] = 0.0; - out_5449976102936300367[230] = 0.0; - out_5449976102936300367[231] = 0.0; - out_5449976102936300367[232] = 0.0; - out_5449976102936300367[233] = 0.0; - out_5449976102936300367[234] = 0.0; - out_5449976102936300367[235] = 0.0; - out_5449976102936300367[236] = 0.0; - out_5449976102936300367[237] = 0.0; - out_5449976102936300367[238] = 0.0; - out_5449976102936300367[239] = 0.0; - out_5449976102936300367[240] = 0.0; - out_5449976102936300367[241] = 0.0; - out_5449976102936300367[242] = 0.0; - out_5449976102936300367[243] = 0.0; - out_5449976102936300367[244] = 0.0; - out_5449976102936300367[245] = 0.0; - out_5449976102936300367[246] = 0.0; - out_5449976102936300367[247] = 1.0; - out_5449976102936300367[248] = 0.0; - out_5449976102936300367[249] = 0.0; - out_5449976102936300367[250] = 0.0; - out_5449976102936300367[251] = 0.0; - out_5449976102936300367[252] = 0.0; - out_5449976102936300367[253] = 0.0; - out_5449976102936300367[254] = 0.0; - out_5449976102936300367[255] = 0.0; - out_5449976102936300367[256] = 0.0; - out_5449976102936300367[257] = 0.0; - out_5449976102936300367[258] = 0.0; - out_5449976102936300367[259] = 0.0; - out_5449976102936300367[260] = 0.0; - out_5449976102936300367[261] = 0.0; - out_5449976102936300367[262] = 0.0; - out_5449976102936300367[263] = 0.0; - out_5449976102936300367[264] = 0.0; - out_5449976102936300367[265] = 0.0; - out_5449976102936300367[266] = 1.0; - out_5449976102936300367[267] = 0.0; - out_5449976102936300367[268] = 0.0; - out_5449976102936300367[269] = 0.0; - out_5449976102936300367[270] = 0.0; - out_5449976102936300367[271] = 0.0; - out_5449976102936300367[272] = 0.0; - out_5449976102936300367[273] = 0.0; - out_5449976102936300367[274] = 0.0; - out_5449976102936300367[275] = 0.0; - out_5449976102936300367[276] = 0.0; - out_5449976102936300367[277] = 0.0; - out_5449976102936300367[278] = 0.0; - out_5449976102936300367[279] = 0.0; - out_5449976102936300367[280] = 0.0; - out_5449976102936300367[281] = 0.0; - out_5449976102936300367[282] = 0.0; - out_5449976102936300367[283] = 0.0; - out_5449976102936300367[284] = 0.0; - out_5449976102936300367[285] = 1.0; - out_5449976102936300367[286] = 0.0; - out_5449976102936300367[287] = 0.0; - out_5449976102936300367[288] = 0.0; - out_5449976102936300367[289] = 0.0; - out_5449976102936300367[290] = 0.0; - out_5449976102936300367[291] = 0.0; - out_5449976102936300367[292] = 0.0; - out_5449976102936300367[293] = 0.0; - out_5449976102936300367[294] = 0.0; - out_5449976102936300367[295] = 0.0; - out_5449976102936300367[296] = 0.0; - out_5449976102936300367[297] = 0.0; - out_5449976102936300367[298] = 0.0; - out_5449976102936300367[299] = 0.0; - out_5449976102936300367[300] = 0.0; - out_5449976102936300367[301] = 0.0; - out_5449976102936300367[302] = 0.0; - out_5449976102936300367[303] = 0.0; - out_5449976102936300367[304] = 1.0; - out_5449976102936300367[305] = 0.0; - out_5449976102936300367[306] = 0.0; - out_5449976102936300367[307] = 0.0; - out_5449976102936300367[308] = 0.0; - out_5449976102936300367[309] = 0.0; - out_5449976102936300367[310] = 0.0; - out_5449976102936300367[311] = 0.0; - out_5449976102936300367[312] = 0.0; - out_5449976102936300367[313] = 0.0; - out_5449976102936300367[314] = 0.0; - out_5449976102936300367[315] = 0.0; - out_5449976102936300367[316] = 0.0; - out_5449976102936300367[317] = 0.0; - out_5449976102936300367[318] = 0.0; - out_5449976102936300367[319] = 0.0; - out_5449976102936300367[320] = 0.0; - out_5449976102936300367[321] = 0.0; - out_5449976102936300367[322] = 0.0; - out_5449976102936300367[323] = 1.0; +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 f_fun(double *state, double dt, double *out_2001314774583974236) { - out_2001314774583974236[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_2001314774583974236[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_2001314774583974236[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_2001314774583974236[3] = dt*state[12] + state[3]; - out_2001314774583974236[4] = dt*state[13] + state[4]; - out_2001314774583974236[5] = dt*state[14] + state[5]; - out_2001314774583974236[6] = state[6]; - out_2001314774583974236[7] = state[7]; - out_2001314774583974236[8] = state[8]; - out_2001314774583974236[9] = state[9]; - out_2001314774583974236[10] = state[10]; - out_2001314774583974236[11] = state[11]; - out_2001314774583974236[12] = state[12]; - out_2001314774583974236[13] = state[13]; - out_2001314774583974236[14] = state[14]; - out_2001314774583974236[15] = state[15]; - out_2001314774583974236[16] = state[16]; - out_2001314774583974236[17] = state[17]; +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_173210708380746968) { - out_173210708380746968[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_173210708380746968[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_173210708380746968[2] = 0; - out_173210708380746968[3] = 0; - out_173210708380746968[4] = 0; - out_173210708380746968[5] = 0; - out_173210708380746968[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_173210708380746968[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_173210708380746968[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_173210708380746968[9] = 0; - out_173210708380746968[10] = 0; - out_173210708380746968[11] = 0; - out_173210708380746968[12] = 0; - out_173210708380746968[13] = 0; - out_173210708380746968[14] = 0; - out_173210708380746968[15] = 0; - out_173210708380746968[16] = 0; - out_173210708380746968[17] = 0; - out_173210708380746968[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_173210708380746968[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_173210708380746968[20] = 0; - out_173210708380746968[21] = 0; - out_173210708380746968[22] = 0; - out_173210708380746968[23] = 0; - out_173210708380746968[24] = 0; - out_173210708380746968[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_173210708380746968[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_173210708380746968[27] = 0; - out_173210708380746968[28] = 0; - out_173210708380746968[29] = 0; - out_173210708380746968[30] = 0; - out_173210708380746968[31] = 0; - out_173210708380746968[32] = 0; - out_173210708380746968[33] = 0; - out_173210708380746968[34] = 0; - out_173210708380746968[35] = 0; - out_173210708380746968[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_173210708380746968[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_173210708380746968[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_173210708380746968[39] = 0; - out_173210708380746968[40] = 0; - out_173210708380746968[41] = 0; - out_173210708380746968[42] = 0; - out_173210708380746968[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_173210708380746968[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_173210708380746968[45] = 0; - out_173210708380746968[46] = 0; - out_173210708380746968[47] = 0; - out_173210708380746968[48] = 0; - out_173210708380746968[49] = 0; - out_173210708380746968[50] = 0; - out_173210708380746968[51] = 0; - out_173210708380746968[52] = 0; - out_173210708380746968[53] = 0; - out_173210708380746968[54] = 0; - out_173210708380746968[55] = 0; - out_173210708380746968[56] = 0; - out_173210708380746968[57] = 1; - out_173210708380746968[58] = 0; - out_173210708380746968[59] = 0; - out_173210708380746968[60] = 0; - out_173210708380746968[61] = 0; - out_173210708380746968[62] = 0; - out_173210708380746968[63] = 0; - out_173210708380746968[64] = 0; - out_173210708380746968[65] = 0; - out_173210708380746968[66] = dt; - out_173210708380746968[67] = 0; - out_173210708380746968[68] = 0; - out_173210708380746968[69] = 0; - out_173210708380746968[70] = 0; - out_173210708380746968[71] = 0; - out_173210708380746968[72] = 0; - out_173210708380746968[73] = 0; - out_173210708380746968[74] = 0; - out_173210708380746968[75] = 0; - out_173210708380746968[76] = 1; - out_173210708380746968[77] = 0; - out_173210708380746968[78] = 0; - out_173210708380746968[79] = 0; - out_173210708380746968[80] = 0; - out_173210708380746968[81] = 0; - out_173210708380746968[82] = 0; - out_173210708380746968[83] = 0; - out_173210708380746968[84] = 0; - out_173210708380746968[85] = dt; - out_173210708380746968[86] = 0; - out_173210708380746968[87] = 0; - out_173210708380746968[88] = 0; - out_173210708380746968[89] = 0; - out_173210708380746968[90] = 0; - out_173210708380746968[91] = 0; - out_173210708380746968[92] = 0; - out_173210708380746968[93] = 0; - out_173210708380746968[94] = 0; - out_173210708380746968[95] = 1; - out_173210708380746968[96] = 0; - out_173210708380746968[97] = 0; - out_173210708380746968[98] = 0; - out_173210708380746968[99] = 0; - out_173210708380746968[100] = 0; - out_173210708380746968[101] = 0; - out_173210708380746968[102] = 0; - out_173210708380746968[103] = 0; - out_173210708380746968[104] = dt; - out_173210708380746968[105] = 0; - out_173210708380746968[106] = 0; - out_173210708380746968[107] = 0; - out_173210708380746968[108] = 0; - out_173210708380746968[109] = 0; - out_173210708380746968[110] = 0; - out_173210708380746968[111] = 0; - out_173210708380746968[112] = 0; - out_173210708380746968[113] = 0; - out_173210708380746968[114] = 1; - out_173210708380746968[115] = 0; - out_173210708380746968[116] = 0; - out_173210708380746968[117] = 0; - out_173210708380746968[118] = 0; - out_173210708380746968[119] = 0; - out_173210708380746968[120] = 0; - out_173210708380746968[121] = 0; - out_173210708380746968[122] = 0; - out_173210708380746968[123] = 0; - out_173210708380746968[124] = 0; - out_173210708380746968[125] = 0; - out_173210708380746968[126] = 0; - out_173210708380746968[127] = 0; - out_173210708380746968[128] = 0; - out_173210708380746968[129] = 0; - out_173210708380746968[130] = 0; - out_173210708380746968[131] = 0; - out_173210708380746968[132] = 0; - out_173210708380746968[133] = 1; - out_173210708380746968[134] = 0; - out_173210708380746968[135] = 0; - out_173210708380746968[136] = 0; - out_173210708380746968[137] = 0; - out_173210708380746968[138] = 0; - out_173210708380746968[139] = 0; - out_173210708380746968[140] = 0; - out_173210708380746968[141] = 0; - out_173210708380746968[142] = 0; - out_173210708380746968[143] = 0; - out_173210708380746968[144] = 0; - out_173210708380746968[145] = 0; - out_173210708380746968[146] = 0; - out_173210708380746968[147] = 0; - out_173210708380746968[148] = 0; - out_173210708380746968[149] = 0; - out_173210708380746968[150] = 0; - out_173210708380746968[151] = 0; - out_173210708380746968[152] = 1; - out_173210708380746968[153] = 0; - out_173210708380746968[154] = 0; - out_173210708380746968[155] = 0; - out_173210708380746968[156] = 0; - out_173210708380746968[157] = 0; - out_173210708380746968[158] = 0; - out_173210708380746968[159] = 0; - out_173210708380746968[160] = 0; - out_173210708380746968[161] = 0; - out_173210708380746968[162] = 0; - out_173210708380746968[163] = 0; - out_173210708380746968[164] = 0; - out_173210708380746968[165] = 0; - out_173210708380746968[166] = 0; - out_173210708380746968[167] = 0; - out_173210708380746968[168] = 0; - out_173210708380746968[169] = 0; - out_173210708380746968[170] = 0; - out_173210708380746968[171] = 1; - out_173210708380746968[172] = 0; - out_173210708380746968[173] = 0; - out_173210708380746968[174] = 0; - out_173210708380746968[175] = 0; - out_173210708380746968[176] = 0; - out_173210708380746968[177] = 0; - out_173210708380746968[178] = 0; - out_173210708380746968[179] = 0; - out_173210708380746968[180] = 0; - out_173210708380746968[181] = 0; - out_173210708380746968[182] = 0; - out_173210708380746968[183] = 0; - out_173210708380746968[184] = 0; - out_173210708380746968[185] = 0; - out_173210708380746968[186] = 0; - out_173210708380746968[187] = 0; - out_173210708380746968[188] = 0; - out_173210708380746968[189] = 0; - out_173210708380746968[190] = 1; - out_173210708380746968[191] = 0; - out_173210708380746968[192] = 0; - out_173210708380746968[193] = 0; - out_173210708380746968[194] = 0; - out_173210708380746968[195] = 0; - out_173210708380746968[196] = 0; - out_173210708380746968[197] = 0; - out_173210708380746968[198] = 0; - out_173210708380746968[199] = 0; - out_173210708380746968[200] = 0; - out_173210708380746968[201] = 0; - out_173210708380746968[202] = 0; - out_173210708380746968[203] = 0; - out_173210708380746968[204] = 0; - out_173210708380746968[205] = 0; - out_173210708380746968[206] = 0; - out_173210708380746968[207] = 0; - out_173210708380746968[208] = 0; - out_173210708380746968[209] = 1; - out_173210708380746968[210] = 0; - out_173210708380746968[211] = 0; - out_173210708380746968[212] = 0; - out_173210708380746968[213] = 0; - out_173210708380746968[214] = 0; - out_173210708380746968[215] = 0; - out_173210708380746968[216] = 0; - out_173210708380746968[217] = 0; - out_173210708380746968[218] = 0; - out_173210708380746968[219] = 0; - out_173210708380746968[220] = 0; - out_173210708380746968[221] = 0; - out_173210708380746968[222] = 0; - out_173210708380746968[223] = 0; - out_173210708380746968[224] = 0; - out_173210708380746968[225] = 0; - out_173210708380746968[226] = 0; - out_173210708380746968[227] = 0; - out_173210708380746968[228] = 1; - out_173210708380746968[229] = 0; - out_173210708380746968[230] = 0; - out_173210708380746968[231] = 0; - out_173210708380746968[232] = 0; - out_173210708380746968[233] = 0; - out_173210708380746968[234] = 0; - out_173210708380746968[235] = 0; - out_173210708380746968[236] = 0; - out_173210708380746968[237] = 0; - out_173210708380746968[238] = 0; - out_173210708380746968[239] = 0; - out_173210708380746968[240] = 0; - out_173210708380746968[241] = 0; - out_173210708380746968[242] = 0; - out_173210708380746968[243] = 0; - out_173210708380746968[244] = 0; - out_173210708380746968[245] = 0; - out_173210708380746968[246] = 0; - out_173210708380746968[247] = 1; - out_173210708380746968[248] = 0; - out_173210708380746968[249] = 0; - out_173210708380746968[250] = 0; - out_173210708380746968[251] = 0; - out_173210708380746968[252] = 0; - out_173210708380746968[253] = 0; - out_173210708380746968[254] = 0; - out_173210708380746968[255] = 0; - out_173210708380746968[256] = 0; - out_173210708380746968[257] = 0; - out_173210708380746968[258] = 0; - out_173210708380746968[259] = 0; - out_173210708380746968[260] = 0; - out_173210708380746968[261] = 0; - out_173210708380746968[262] = 0; - out_173210708380746968[263] = 0; - out_173210708380746968[264] = 0; - out_173210708380746968[265] = 0; - out_173210708380746968[266] = 1; - out_173210708380746968[267] = 0; - out_173210708380746968[268] = 0; - out_173210708380746968[269] = 0; - out_173210708380746968[270] = 0; - out_173210708380746968[271] = 0; - out_173210708380746968[272] = 0; - out_173210708380746968[273] = 0; - out_173210708380746968[274] = 0; - out_173210708380746968[275] = 0; - out_173210708380746968[276] = 0; - out_173210708380746968[277] = 0; - out_173210708380746968[278] = 0; - out_173210708380746968[279] = 0; - out_173210708380746968[280] = 0; - out_173210708380746968[281] = 0; - out_173210708380746968[282] = 0; - out_173210708380746968[283] = 0; - out_173210708380746968[284] = 0; - out_173210708380746968[285] = 1; - out_173210708380746968[286] = 0; - out_173210708380746968[287] = 0; - out_173210708380746968[288] = 0; - out_173210708380746968[289] = 0; - out_173210708380746968[290] = 0; - out_173210708380746968[291] = 0; - out_173210708380746968[292] = 0; - out_173210708380746968[293] = 0; - out_173210708380746968[294] = 0; - out_173210708380746968[295] = 0; - out_173210708380746968[296] = 0; - out_173210708380746968[297] = 0; - out_173210708380746968[298] = 0; - out_173210708380746968[299] = 0; - out_173210708380746968[300] = 0; - out_173210708380746968[301] = 0; - out_173210708380746968[302] = 0; - out_173210708380746968[303] = 0; - out_173210708380746968[304] = 1; - out_173210708380746968[305] = 0; - out_173210708380746968[306] = 0; - out_173210708380746968[307] = 0; - out_173210708380746968[308] = 0; - out_173210708380746968[309] = 0; - out_173210708380746968[310] = 0; - out_173210708380746968[311] = 0; - out_173210708380746968[312] = 0; - out_173210708380746968[313] = 0; - out_173210708380746968[314] = 0; - out_173210708380746968[315] = 0; - out_173210708380746968[316] = 0; - out_173210708380746968[317] = 0; - out_173210708380746968[318] = 0; - out_173210708380746968[319] = 0; - out_173210708380746968[320] = 0; - out_173210708380746968[321] = 0; - out_173210708380746968[322] = 0; - out_173210708380746968[323] = 1; +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 h_4(double *state, double *unused, double *out_4290451809527946304) { - out_4290451809527946304[0] = state[6] + state[9]; - out_4290451809527946304[1] = state[7] + state[10]; - out_4290451809527946304[2] = state[8] + state[11]; +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_4695815100881587760) { - out_4695815100881587760[0] = 0; - out_4695815100881587760[1] = 0; - out_4695815100881587760[2] = 0; - out_4695815100881587760[3] = 0; - out_4695815100881587760[4] = 0; - out_4695815100881587760[5] = 0; - out_4695815100881587760[6] = 1; - out_4695815100881587760[7] = 0; - out_4695815100881587760[8] = 0; - out_4695815100881587760[9] = 1; - out_4695815100881587760[10] = 0; - out_4695815100881587760[11] = 0; - out_4695815100881587760[12] = 0; - out_4695815100881587760[13] = 0; - out_4695815100881587760[14] = 0; - out_4695815100881587760[15] = 0; - out_4695815100881587760[16] = 0; - out_4695815100881587760[17] = 0; - out_4695815100881587760[18] = 0; - out_4695815100881587760[19] = 0; - out_4695815100881587760[20] = 0; - out_4695815100881587760[21] = 0; - out_4695815100881587760[22] = 0; - out_4695815100881587760[23] = 0; - out_4695815100881587760[24] = 0; - out_4695815100881587760[25] = 1; - out_4695815100881587760[26] = 0; - out_4695815100881587760[27] = 0; - out_4695815100881587760[28] = 1; - out_4695815100881587760[29] = 0; - out_4695815100881587760[30] = 0; - out_4695815100881587760[31] = 0; - out_4695815100881587760[32] = 0; - out_4695815100881587760[33] = 0; - out_4695815100881587760[34] = 0; - out_4695815100881587760[35] = 0; - out_4695815100881587760[36] = 0; - out_4695815100881587760[37] = 0; - out_4695815100881587760[38] = 0; - out_4695815100881587760[39] = 0; - out_4695815100881587760[40] = 0; - out_4695815100881587760[41] = 0; - out_4695815100881587760[42] = 0; - out_4695815100881587760[43] = 0; - out_4695815100881587760[44] = 1; - out_4695815100881587760[45] = 0; - out_4695815100881587760[46] = 0; - out_4695815100881587760[47] = 1; - out_4695815100881587760[48] = 0; - out_4695815100881587760[49] = 0; - out_4695815100881587760[50] = 0; - out_4695815100881587760[51] = 0; - out_4695815100881587760[52] = 0; - out_4695815100881587760[53] = 0; +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_10(double *state, double *unused, double *out_6723857678965398132) { - out_6723857678965398132[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_6723857678965398132[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_6723857678965398132[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_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_984080189925557249) { - out_984080189925557249[0] = 0; - out_984080189925557249[1] = 9.8100000000000005*cos(state[1]); - out_984080189925557249[2] = 0; - out_984080189925557249[3] = 0; - out_984080189925557249[4] = -state[8]; - out_984080189925557249[5] = state[7]; - out_984080189925557249[6] = 0; - out_984080189925557249[7] = state[5]; - out_984080189925557249[8] = -state[4]; - out_984080189925557249[9] = 0; - out_984080189925557249[10] = 0; - out_984080189925557249[11] = 0; - out_984080189925557249[12] = 1; - out_984080189925557249[13] = 0; - out_984080189925557249[14] = 0; - out_984080189925557249[15] = 1; - out_984080189925557249[16] = 0; - out_984080189925557249[17] = 0; - out_984080189925557249[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_984080189925557249[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_984080189925557249[20] = 0; - out_984080189925557249[21] = state[8]; - out_984080189925557249[22] = 0; - out_984080189925557249[23] = -state[6]; - out_984080189925557249[24] = -state[5]; - out_984080189925557249[25] = 0; - out_984080189925557249[26] = state[3]; - out_984080189925557249[27] = 0; - out_984080189925557249[28] = 0; - out_984080189925557249[29] = 0; - out_984080189925557249[30] = 0; - out_984080189925557249[31] = 1; - out_984080189925557249[32] = 0; - out_984080189925557249[33] = 0; - out_984080189925557249[34] = 1; - out_984080189925557249[35] = 0; - out_984080189925557249[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_984080189925557249[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_984080189925557249[38] = 0; - out_984080189925557249[39] = -state[7]; - out_984080189925557249[40] = state[6]; - out_984080189925557249[41] = 0; - out_984080189925557249[42] = state[4]; - out_984080189925557249[43] = -state[3]; - out_984080189925557249[44] = 0; - out_984080189925557249[45] = 0; - out_984080189925557249[46] = 0; - out_984080189925557249[47] = 0; - out_984080189925557249[48] = 0; - out_984080189925557249[49] = 0; - out_984080189925557249[50] = 1; - out_984080189925557249[51] = 0; - out_984080189925557249[52] = 0; - out_984080189925557249[53] = 1; +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_13(double *state, double *unused, double *out_1838091537859337419) { - out_1838091537859337419[0] = state[3]; - out_1838091537859337419[1] = state[4]; - out_1838091537859337419[2] = state[5]; +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_1483541275549254959) { - out_1483541275549254959[0] = 0; - out_1483541275549254959[1] = 0; - out_1483541275549254959[2] = 0; - out_1483541275549254959[3] = 1; - out_1483541275549254959[4] = 0; - out_1483541275549254959[5] = 0; - out_1483541275549254959[6] = 0; - out_1483541275549254959[7] = 0; - out_1483541275549254959[8] = 0; - out_1483541275549254959[9] = 0; - out_1483541275549254959[10] = 0; - out_1483541275549254959[11] = 0; - out_1483541275549254959[12] = 0; - out_1483541275549254959[13] = 0; - out_1483541275549254959[14] = 0; - out_1483541275549254959[15] = 0; - out_1483541275549254959[16] = 0; - out_1483541275549254959[17] = 0; - out_1483541275549254959[18] = 0; - out_1483541275549254959[19] = 0; - out_1483541275549254959[20] = 0; - out_1483541275549254959[21] = 0; - out_1483541275549254959[22] = 1; - out_1483541275549254959[23] = 0; - out_1483541275549254959[24] = 0; - out_1483541275549254959[25] = 0; - out_1483541275549254959[26] = 0; - out_1483541275549254959[27] = 0; - out_1483541275549254959[28] = 0; - out_1483541275549254959[29] = 0; - out_1483541275549254959[30] = 0; - out_1483541275549254959[31] = 0; - out_1483541275549254959[32] = 0; - out_1483541275549254959[33] = 0; - out_1483541275549254959[34] = 0; - out_1483541275549254959[35] = 0; - out_1483541275549254959[36] = 0; - out_1483541275549254959[37] = 0; - out_1483541275549254959[38] = 0; - out_1483541275549254959[39] = 0; - out_1483541275549254959[40] = 0; - out_1483541275549254959[41] = 1; - out_1483541275549254959[42] = 0; - out_1483541275549254959[43] = 0; - out_1483541275549254959[44] = 0; - out_1483541275549254959[45] = 0; - out_1483541275549254959[46] = 0; - out_1483541275549254959[47] = 0; - out_1483541275549254959[48] = 0; - out_1483541275549254959[49] = 0; - out_1483541275549254959[50] = 0; - out_1483541275549254959[51] = 0; - out_1483541275549254959[52] = 0; - out_1483541275549254959[53] = 0; +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_14(double *state, double *unused, double *out_849503921054780572) { - out_849503921054780572[0] = state[6]; - out_849503921054780572[1] = state[7]; - out_849503921054780572[2] = state[8]; +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_732574244542103231) { - out_732574244542103231[0] = 0; - out_732574244542103231[1] = 0; - out_732574244542103231[2] = 0; - out_732574244542103231[3] = 0; - out_732574244542103231[4] = 0; - out_732574244542103231[5] = 0; - out_732574244542103231[6] = 1; - out_732574244542103231[7] = 0; - out_732574244542103231[8] = 0; - out_732574244542103231[9] = 0; - out_732574244542103231[10] = 0; - out_732574244542103231[11] = 0; - out_732574244542103231[12] = 0; - out_732574244542103231[13] = 0; - out_732574244542103231[14] = 0; - out_732574244542103231[15] = 0; - out_732574244542103231[16] = 0; - out_732574244542103231[17] = 0; - out_732574244542103231[18] = 0; - out_732574244542103231[19] = 0; - out_732574244542103231[20] = 0; - out_732574244542103231[21] = 0; - out_732574244542103231[22] = 0; - out_732574244542103231[23] = 0; - out_732574244542103231[24] = 0; - out_732574244542103231[25] = 1; - out_732574244542103231[26] = 0; - out_732574244542103231[27] = 0; - out_732574244542103231[28] = 0; - out_732574244542103231[29] = 0; - out_732574244542103231[30] = 0; - out_732574244542103231[31] = 0; - out_732574244542103231[32] = 0; - out_732574244542103231[33] = 0; - out_732574244542103231[34] = 0; - out_732574244542103231[35] = 0; - out_732574244542103231[36] = 0; - out_732574244542103231[37] = 0; - out_732574244542103231[38] = 0; - out_732574244542103231[39] = 0; - out_732574244542103231[40] = 0; - out_732574244542103231[41] = 0; - out_732574244542103231[42] = 0; - out_732574244542103231[43] = 0; - out_732574244542103231[44] = 1; - out_732574244542103231[45] = 0; - out_732574244542103231[46] = 0; - out_732574244542103231[47] = 0; - out_732574244542103231[48] = 0; - out_732574244542103231[49] = 0; - out_732574244542103231[50] = 0; - out_732574244542103231[51] = 0; - out_732574244542103231[52] = 0; - out_732574244542103231[53] = 0; +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; } #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_9044617002367965712) { - err_fun(nom_x, delta_x, out_9044617002367965712); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8979246808243312501) { + err_fun(nom_x, delta_x, out_8979246808243312501); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2986388781113821274) { - inv_err_fun(nom_x, true_x, out_2986388781113821274); +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_H_mod_fun(double *state, double *out_5449976102936300367) { - H_mod_fun(state, out_5449976102936300367); +void pose_H_mod_fun(double *state, double *out_5496809482113736276) { + H_mod_fun(state, out_5496809482113736276); } -void pose_f_fun(double *state, double dt, double *out_2001314774583974236) { - f_fun(state, dt, out_2001314774583974236); +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_173210708380746968) { - F_fun(state, dt, out_173210708380746968); +void pose_F_fun(double *state, double dt, double *out_7773273273846026145) { + F_fun(state, dt, out_7773273273846026145); } -void pose_h_4(double *state, double *unused, double *out_4290451809527946304) { - h_4(state, unused, out_4290451809527946304); +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_4695815100881587760) { - H_4(state, unused, out_4695815100881587760); +void pose_H_4(double *state, double *unused, double *out_8771031560904685529) { + H_4(state, unused, out_8771031560904685529); } -void pose_h_10(double *state, double *unused, double *out_6723857678965398132) { - h_10(state, unused, out_6723857678965398132); +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_984080189925557249) { - H_10(state, unused, out_984080189925557249); +void pose_H_10(double *state, double *unused, double *out_1676230657278343882) { + H_10(state, unused, out_1676230657278343882); } -void pose_h_13(double *state, double *unused, double *out_1838091537859337419) { - h_13(state, unused, out_1838091537859337419); +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_1483541275549254959) { - H_13(state, unused, out_1483541275549254959); +void pose_H_13(double *state, double *unused, double *out_5558757735572352728) { + H_13(state, unused, out_5558757735572352728); } -void pose_h_14(double *state, double *unused, double *out_849503921054780572) { - h_14(state, unused, out_849503921054780572); +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_732574244542103231) { - H_14(state, unused, out_732574244542103231); +void pose_H_14(double *state, double *unused, double *out_4807790704565201000) { + H_14(state, unused, out_4807790704565201000); } 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 77d9b509e..a705f767c 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_9044617002367965712); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2986388781113821274); -void pose_H_mod_fun(double *state, double *out_5449976102936300367); -void pose_f_fun(double *state, double dt, double *out_2001314774583974236); -void pose_F_fun(double *state, double dt, double *out_173210708380746968); -void pose_h_4(double *state, double *unused, double *out_4290451809527946304); -void pose_H_4(double *state, double *unused, double *out_4695815100881587760); -void pose_h_10(double *state, double *unused, double *out_6723857678965398132); -void pose_H_10(double *state, double *unused, double *out_984080189925557249); -void pose_h_13(double *state, double *unused, double *out_1838091537859337419); -void pose_H_13(double *state, double *unused, double *out_1483541275549254959); -void pose_h_14(double *state, double *unused, double *out_849503921054780572); -void pose_H_14(double *state, double *unused, double *out_732574244542103231); +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_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 329b71766..98c7ab37f 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 85774c2e7..f55f6b772 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index f73b32266..bce5a08c0 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 6026381b0..bb12b9d0e 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 039b0c728..a8c3d6b5c 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 9dab54c83..6c416b698 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ