diff --git a/common/libcommon.a b/common/libcommon.a index ac32ab93ee..dfcfa93748 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 7d0a2b1118..76e123f8e5 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 b11fc918f9..a31f4fa0b9 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 00af6e9f52..54dc56d49d 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 d0e8681b37..b505335b6e 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 53c262226e..ff0007bdd3 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 2eca9b8a66..8f396165c9 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 faf305d347..19d81659f9 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 8babaa078a..16486dcf87 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 8babaa078a..16486dcf87 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 f1bf70f32f..6266784796 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 faf305d347..19d81659f9 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 8589a089a5..b2b78fb13c 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-8a1bfb92-DEBUG"; +const uint8_t gitversion[19] = "DEV-7a17c21f-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 401856d5df..6375b7e779 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 f88bdbbd39..b26999f288 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 1a8fd88d0b..a094d70625 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 94da6116dd..78773d2070 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 65844fc25c..cd11af645a 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 124bd7bae7..bef3a4ce52 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 9dcc1acbf5..4bb20a4bf0 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 b8fc1a3024..66ef0ac1d0 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 ad857df0de..6ffdcffc09 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 694750678f..1915a5573e 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 037990ad58..b0e96a4fcc 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 b4adf4f440..11c2e5e237 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 daada0bd54..3e26a367fe 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 d433aed5d8..48297d538e 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 fda28736da..5b8567ae84 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 7e8a067cc6..84d40c9c8f 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 8d752f9e86..e5eeac0468 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 8874d81157..460a165964 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 0ee2431184..a9f75f76a0 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 abcd655355..af121604e5 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 4be2c976fe..ffc04fd7fa 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-8a1bfb92-DEBUG \ No newline at end of file +DEV-7a17c21f-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 3a3221f328..153f1a3842 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_8250891246740759679) { - out_8250891246740759679[0] = delta_x[0] + nom_x[0]; - out_8250891246740759679[1] = delta_x[1] + nom_x[1]; - out_8250891246740759679[2] = delta_x[2] + nom_x[2]; - out_8250891246740759679[3] = delta_x[3] + nom_x[3]; - out_8250891246740759679[4] = delta_x[4] + nom_x[4]; - out_8250891246740759679[5] = delta_x[5] + nom_x[5]; - out_8250891246740759679[6] = delta_x[6] + nom_x[6]; - out_8250891246740759679[7] = delta_x[7] + nom_x[7]; - out_8250891246740759679[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_4271848112759039807) { + out_4271848112759039807[0] = delta_x[0] + nom_x[0]; + out_4271848112759039807[1] = delta_x[1] + nom_x[1]; + out_4271848112759039807[2] = delta_x[2] + nom_x[2]; + out_4271848112759039807[3] = delta_x[3] + nom_x[3]; + out_4271848112759039807[4] = delta_x[4] + nom_x[4]; + out_4271848112759039807[5] = delta_x[5] + nom_x[5]; + out_4271848112759039807[6] = delta_x[6] + nom_x[6]; + out_4271848112759039807[7] = delta_x[7] + nom_x[7]; + out_4271848112759039807[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_6699306425357192023) { - out_6699306425357192023[0] = -nom_x[0] + true_x[0]; - out_6699306425357192023[1] = -nom_x[1] + true_x[1]; - out_6699306425357192023[2] = -nom_x[2] + true_x[2]; - out_6699306425357192023[3] = -nom_x[3] + true_x[3]; - out_6699306425357192023[4] = -nom_x[4] + true_x[4]; - out_6699306425357192023[5] = -nom_x[5] + true_x[5]; - out_6699306425357192023[6] = -nom_x[6] + true_x[6]; - out_6699306425357192023[7] = -nom_x[7] + true_x[7]; - out_6699306425357192023[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2031572598342150124) { + out_2031572598342150124[0] = -nom_x[0] + true_x[0]; + out_2031572598342150124[1] = -nom_x[1] + true_x[1]; + out_2031572598342150124[2] = -nom_x[2] + true_x[2]; + out_2031572598342150124[3] = -nom_x[3] + true_x[3]; + out_2031572598342150124[4] = -nom_x[4] + true_x[4]; + out_2031572598342150124[5] = -nom_x[5] + true_x[5]; + out_2031572598342150124[6] = -nom_x[6] + true_x[6]; + out_2031572598342150124[7] = -nom_x[7] + true_x[7]; + out_2031572598342150124[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_2914898107951165883) { - out_2914898107951165883[0] = 1.0; - out_2914898107951165883[1] = 0.0; - out_2914898107951165883[2] = 0.0; - out_2914898107951165883[3] = 0.0; - out_2914898107951165883[4] = 0.0; - out_2914898107951165883[5] = 0.0; - out_2914898107951165883[6] = 0.0; - out_2914898107951165883[7] = 0.0; - out_2914898107951165883[8] = 0.0; - out_2914898107951165883[9] = 0.0; - out_2914898107951165883[10] = 1.0; - out_2914898107951165883[11] = 0.0; - out_2914898107951165883[12] = 0.0; - out_2914898107951165883[13] = 0.0; - out_2914898107951165883[14] = 0.0; - out_2914898107951165883[15] = 0.0; - out_2914898107951165883[16] = 0.0; - out_2914898107951165883[17] = 0.0; - out_2914898107951165883[18] = 0.0; - out_2914898107951165883[19] = 0.0; - out_2914898107951165883[20] = 1.0; - out_2914898107951165883[21] = 0.0; - out_2914898107951165883[22] = 0.0; - out_2914898107951165883[23] = 0.0; - out_2914898107951165883[24] = 0.0; - out_2914898107951165883[25] = 0.0; - out_2914898107951165883[26] = 0.0; - out_2914898107951165883[27] = 0.0; - out_2914898107951165883[28] = 0.0; - out_2914898107951165883[29] = 0.0; - out_2914898107951165883[30] = 1.0; - out_2914898107951165883[31] = 0.0; - out_2914898107951165883[32] = 0.0; - out_2914898107951165883[33] = 0.0; - out_2914898107951165883[34] = 0.0; - out_2914898107951165883[35] = 0.0; - out_2914898107951165883[36] = 0.0; - out_2914898107951165883[37] = 0.0; - out_2914898107951165883[38] = 0.0; - out_2914898107951165883[39] = 0.0; - out_2914898107951165883[40] = 1.0; - out_2914898107951165883[41] = 0.0; - out_2914898107951165883[42] = 0.0; - out_2914898107951165883[43] = 0.0; - out_2914898107951165883[44] = 0.0; - out_2914898107951165883[45] = 0.0; - out_2914898107951165883[46] = 0.0; - out_2914898107951165883[47] = 0.0; - out_2914898107951165883[48] = 0.0; - out_2914898107951165883[49] = 0.0; - out_2914898107951165883[50] = 1.0; - out_2914898107951165883[51] = 0.0; - out_2914898107951165883[52] = 0.0; - out_2914898107951165883[53] = 0.0; - out_2914898107951165883[54] = 0.0; - out_2914898107951165883[55] = 0.0; - out_2914898107951165883[56] = 0.0; - out_2914898107951165883[57] = 0.0; - out_2914898107951165883[58] = 0.0; - out_2914898107951165883[59] = 0.0; - out_2914898107951165883[60] = 1.0; - out_2914898107951165883[61] = 0.0; - out_2914898107951165883[62] = 0.0; - out_2914898107951165883[63] = 0.0; - out_2914898107951165883[64] = 0.0; - out_2914898107951165883[65] = 0.0; - out_2914898107951165883[66] = 0.0; - out_2914898107951165883[67] = 0.0; - out_2914898107951165883[68] = 0.0; - out_2914898107951165883[69] = 0.0; - out_2914898107951165883[70] = 1.0; - out_2914898107951165883[71] = 0.0; - out_2914898107951165883[72] = 0.0; - out_2914898107951165883[73] = 0.0; - out_2914898107951165883[74] = 0.0; - out_2914898107951165883[75] = 0.0; - out_2914898107951165883[76] = 0.0; - out_2914898107951165883[77] = 0.0; - out_2914898107951165883[78] = 0.0; - out_2914898107951165883[79] = 0.0; - out_2914898107951165883[80] = 1.0; +void H_mod_fun(double *state, double *out_177684180657030354) { + out_177684180657030354[0] = 1.0; + out_177684180657030354[1] = 0.0; + out_177684180657030354[2] = 0.0; + out_177684180657030354[3] = 0.0; + out_177684180657030354[4] = 0.0; + out_177684180657030354[5] = 0.0; + out_177684180657030354[6] = 0.0; + out_177684180657030354[7] = 0.0; + out_177684180657030354[8] = 0.0; + out_177684180657030354[9] = 0.0; + out_177684180657030354[10] = 1.0; + out_177684180657030354[11] = 0.0; + out_177684180657030354[12] = 0.0; + out_177684180657030354[13] = 0.0; + out_177684180657030354[14] = 0.0; + out_177684180657030354[15] = 0.0; + out_177684180657030354[16] = 0.0; + out_177684180657030354[17] = 0.0; + out_177684180657030354[18] = 0.0; + out_177684180657030354[19] = 0.0; + out_177684180657030354[20] = 1.0; + out_177684180657030354[21] = 0.0; + out_177684180657030354[22] = 0.0; + out_177684180657030354[23] = 0.0; + out_177684180657030354[24] = 0.0; + out_177684180657030354[25] = 0.0; + out_177684180657030354[26] = 0.0; + out_177684180657030354[27] = 0.0; + out_177684180657030354[28] = 0.0; + out_177684180657030354[29] = 0.0; + out_177684180657030354[30] = 1.0; + out_177684180657030354[31] = 0.0; + out_177684180657030354[32] = 0.0; + out_177684180657030354[33] = 0.0; + out_177684180657030354[34] = 0.0; + out_177684180657030354[35] = 0.0; + out_177684180657030354[36] = 0.0; + out_177684180657030354[37] = 0.0; + out_177684180657030354[38] = 0.0; + out_177684180657030354[39] = 0.0; + out_177684180657030354[40] = 1.0; + out_177684180657030354[41] = 0.0; + out_177684180657030354[42] = 0.0; + out_177684180657030354[43] = 0.0; + out_177684180657030354[44] = 0.0; + out_177684180657030354[45] = 0.0; + out_177684180657030354[46] = 0.0; + out_177684180657030354[47] = 0.0; + out_177684180657030354[48] = 0.0; + out_177684180657030354[49] = 0.0; + out_177684180657030354[50] = 1.0; + out_177684180657030354[51] = 0.0; + out_177684180657030354[52] = 0.0; + out_177684180657030354[53] = 0.0; + out_177684180657030354[54] = 0.0; + out_177684180657030354[55] = 0.0; + out_177684180657030354[56] = 0.0; + out_177684180657030354[57] = 0.0; + out_177684180657030354[58] = 0.0; + out_177684180657030354[59] = 0.0; + out_177684180657030354[60] = 1.0; + out_177684180657030354[61] = 0.0; + out_177684180657030354[62] = 0.0; + out_177684180657030354[63] = 0.0; + out_177684180657030354[64] = 0.0; + out_177684180657030354[65] = 0.0; + out_177684180657030354[66] = 0.0; + out_177684180657030354[67] = 0.0; + out_177684180657030354[68] = 0.0; + out_177684180657030354[69] = 0.0; + out_177684180657030354[70] = 1.0; + out_177684180657030354[71] = 0.0; + out_177684180657030354[72] = 0.0; + out_177684180657030354[73] = 0.0; + out_177684180657030354[74] = 0.0; + out_177684180657030354[75] = 0.0; + out_177684180657030354[76] = 0.0; + out_177684180657030354[77] = 0.0; + out_177684180657030354[78] = 0.0; + out_177684180657030354[79] = 0.0; + out_177684180657030354[80] = 1.0; } -void f_fun(double *state, double dt, double *out_2675178968316523152) { - out_2675178968316523152[0] = state[0]; - out_2675178968316523152[1] = state[1]; - out_2675178968316523152[2] = state[2]; - out_2675178968316523152[3] = state[3]; - out_2675178968316523152[4] = state[4]; - out_2675178968316523152[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5]; - out_2675178968316523152[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6]; - out_2675178968316523152[7] = state[7]; - out_2675178968316523152[8] = state[8]; +void f_fun(double *state, double dt, double *out_6521764996120993087) { + out_6521764996120993087[0] = state[0]; + out_6521764996120993087[1] = state[1]; + out_6521764996120993087[2] = state[2]; + out_6521764996120993087[3] = state[3]; + out_6521764996120993087[4] = state[4]; + out_6521764996120993087[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_6521764996120993087[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_6521764996120993087[7] = state[7]; + out_6521764996120993087[8] = state[8]; } -void F_fun(double *state, double dt, double *out_5308189961048980992) { - out_5308189961048980992[0] = 1; - out_5308189961048980992[1] = 0; - out_5308189961048980992[2] = 0; - out_5308189961048980992[3] = 0; - out_5308189961048980992[4] = 0; - out_5308189961048980992[5] = 0; - out_5308189961048980992[6] = 0; - out_5308189961048980992[7] = 0; - out_5308189961048980992[8] = 0; - out_5308189961048980992[9] = 0; - out_5308189961048980992[10] = 1; - out_5308189961048980992[11] = 0; - out_5308189961048980992[12] = 0; - out_5308189961048980992[13] = 0; - out_5308189961048980992[14] = 0; - out_5308189961048980992[15] = 0; - out_5308189961048980992[16] = 0; - out_5308189961048980992[17] = 0; - out_5308189961048980992[18] = 0; - out_5308189961048980992[19] = 0; - out_5308189961048980992[20] = 1; - out_5308189961048980992[21] = 0; - out_5308189961048980992[22] = 0; - out_5308189961048980992[23] = 0; - out_5308189961048980992[24] = 0; - out_5308189961048980992[25] = 0; - out_5308189961048980992[26] = 0; - out_5308189961048980992[27] = 0; - out_5308189961048980992[28] = 0; - out_5308189961048980992[29] = 0; - out_5308189961048980992[30] = 1; - out_5308189961048980992[31] = 0; - out_5308189961048980992[32] = 0; - out_5308189961048980992[33] = 0; - out_5308189961048980992[34] = 0; - out_5308189961048980992[35] = 0; - out_5308189961048980992[36] = 0; - out_5308189961048980992[37] = 0; - out_5308189961048980992[38] = 0; - out_5308189961048980992[39] = 0; - out_5308189961048980992[40] = 1; - out_5308189961048980992[41] = 0; - out_5308189961048980992[42] = 0; - out_5308189961048980992[43] = 0; - out_5308189961048980992[44] = 0; - out_5308189961048980992[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4])); - out_5308189961048980992[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_5308189961048980992[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_5308189961048980992[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_5308189961048980992[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2))); - out_5308189961048980992[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_5308189961048980992[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_5308189961048980992[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_5308189961048980992[53] = -9.8100000000000005*dt; - out_5308189961048980992[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4])); - out_5308189961048980992[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_5308189961048980992[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_5308189961048980992[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_5308189961048980992[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2))); - out_5308189961048980992[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_5308189961048980992[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1; - out_5308189961048980992[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_5308189961048980992[62] = 0; - out_5308189961048980992[63] = 0; - out_5308189961048980992[64] = 0; - out_5308189961048980992[65] = 0; - out_5308189961048980992[66] = 0; - out_5308189961048980992[67] = 0; - out_5308189961048980992[68] = 0; - out_5308189961048980992[69] = 0; - out_5308189961048980992[70] = 1; - out_5308189961048980992[71] = 0; - out_5308189961048980992[72] = 0; - out_5308189961048980992[73] = 0; - out_5308189961048980992[74] = 0; - out_5308189961048980992[75] = 0; - out_5308189961048980992[76] = 0; - out_5308189961048980992[77] = 0; - out_5308189961048980992[78] = 0; - out_5308189961048980992[79] = 0; - out_5308189961048980992[80] = 1; +void F_fun(double *state, double dt, double *out_4624398414228863854) { + out_4624398414228863854[0] = 1; + out_4624398414228863854[1] = 0; + out_4624398414228863854[2] = 0; + out_4624398414228863854[3] = 0; + out_4624398414228863854[4] = 0; + out_4624398414228863854[5] = 0; + out_4624398414228863854[6] = 0; + out_4624398414228863854[7] = 0; + out_4624398414228863854[8] = 0; + out_4624398414228863854[9] = 0; + out_4624398414228863854[10] = 1; + out_4624398414228863854[11] = 0; + out_4624398414228863854[12] = 0; + out_4624398414228863854[13] = 0; + out_4624398414228863854[14] = 0; + out_4624398414228863854[15] = 0; + out_4624398414228863854[16] = 0; + out_4624398414228863854[17] = 0; + out_4624398414228863854[18] = 0; + out_4624398414228863854[19] = 0; + out_4624398414228863854[20] = 1; + out_4624398414228863854[21] = 0; + out_4624398414228863854[22] = 0; + out_4624398414228863854[23] = 0; + out_4624398414228863854[24] = 0; + out_4624398414228863854[25] = 0; + out_4624398414228863854[26] = 0; + out_4624398414228863854[27] = 0; + out_4624398414228863854[28] = 0; + out_4624398414228863854[29] = 0; + out_4624398414228863854[30] = 1; + out_4624398414228863854[31] = 0; + out_4624398414228863854[32] = 0; + out_4624398414228863854[33] = 0; + out_4624398414228863854[34] = 0; + out_4624398414228863854[35] = 0; + out_4624398414228863854[36] = 0; + out_4624398414228863854[37] = 0; + out_4624398414228863854[38] = 0; + out_4624398414228863854[39] = 0; + out_4624398414228863854[40] = 1; + out_4624398414228863854[41] = 0; + out_4624398414228863854[42] = 0; + out_4624398414228863854[43] = 0; + out_4624398414228863854[44] = 0; + out_4624398414228863854[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_4624398414228863854[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_4624398414228863854[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4624398414228863854[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4624398414228863854[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_4624398414228863854[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_4624398414228863854[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_4624398414228863854[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_4624398414228863854[53] = -9.8100000000000005*dt; + out_4624398414228863854[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_4624398414228863854[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_4624398414228863854[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4624398414228863854[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4624398414228863854[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_4624398414228863854[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_4624398414228863854[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_4624398414228863854[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4624398414228863854[62] = 0; + out_4624398414228863854[63] = 0; + out_4624398414228863854[64] = 0; + out_4624398414228863854[65] = 0; + out_4624398414228863854[66] = 0; + out_4624398414228863854[67] = 0; + out_4624398414228863854[68] = 0; + out_4624398414228863854[69] = 0; + out_4624398414228863854[70] = 1; + out_4624398414228863854[71] = 0; + out_4624398414228863854[72] = 0; + out_4624398414228863854[73] = 0; + out_4624398414228863854[74] = 0; + out_4624398414228863854[75] = 0; + out_4624398414228863854[76] = 0; + out_4624398414228863854[77] = 0; + out_4624398414228863854[78] = 0; + out_4624398414228863854[79] = 0; + out_4624398414228863854[80] = 1; } -void h_25(double *state, double *unused, double *out_6353754508244043817) { - out_6353754508244043817[0] = state[6]; +void h_25(double *state, double *unused, double *out_5192262937518193843) { + out_5192262937518193843[0] = state[6]; } -void H_25(double *state, double *unused, double *out_6338657739846148496) { - out_6338657739846148496[0] = 0; - out_6338657739846148496[1] = 0; - out_6338657739846148496[2] = 0; - out_6338657739846148496[3] = 0; - out_6338657739846148496[4] = 0; - out_6338657739846148496[5] = 0; - out_6338657739846148496[6] = 1; - out_6338657739846148496[7] = 0; - out_6338657739846148496[8] = 0; +void H_25(double *state, double *unused, double *out_6857380300493335265) { + out_6857380300493335265[0] = 0; + out_6857380300493335265[1] = 0; + out_6857380300493335265[2] = 0; + out_6857380300493335265[3] = 0; + out_6857380300493335265[4] = 0; + out_6857380300493335265[5] = 0; + out_6857380300493335265[6] = 1; + out_6857380300493335265[7] = 0; + out_6857380300493335265[8] = 0; } -void h_24(double *state, double *unused, double *out_2065669686396091997) { - out_2065669686396091997[0] = state[4]; - out_2065669686396091997[1] = state[5]; +void h_24(double *state, double *unused, double *out_6066922532331985124) { + out_6066922532331985124[0] = state[4]; + out_6066922532331985124[1] = state[5]; } -void H_24(double *state, double *unused, double *out_1518336235190160233) { - out_1518336235190160233[0] = 0; - out_1518336235190160233[1] = 0; - out_1518336235190160233[2] = 0; - out_1518336235190160233[3] = 0; - out_1518336235190160233[4] = 1; - out_1518336235190160233[5] = 0; - out_1518336235190160233[6] = 0; - out_1518336235190160233[7] = 0; - out_1518336235190160233[8] = 0; - out_1518336235190160233[9] = 0; - out_1518336235190160233[10] = 0; - out_1518336235190160233[11] = 0; - out_1518336235190160233[12] = 0; - out_1518336235190160233[13] = 0; - out_1518336235190160233[14] = 1; - out_1518336235190160233[15] = 0; - out_1518336235190160233[16] = 0; - out_1518336235190160233[17] = 0; +void H_24(double *state, double *unused, double *out_4684730701487835699) { + out_4684730701487835699[0] = 0; + out_4684730701487835699[1] = 0; + out_4684730701487835699[2] = 0; + out_4684730701487835699[3] = 0; + out_4684730701487835699[4] = 1; + out_4684730701487835699[5] = 0; + out_4684730701487835699[6] = 0; + out_4684730701487835699[7] = 0; + out_4684730701487835699[8] = 0; + out_4684730701487835699[9] = 0; + out_4684730701487835699[10] = 0; + out_4684730701487835699[11] = 0; + out_4684730701487835699[12] = 0; + out_4684730701487835699[13] = 0; + out_4684730701487835699[14] = 1; + out_4684730701487835699[15] = 0; + out_4684730701487835699[16] = 0; + out_4684730701487835699[17] = 0; } -void h_30(double *state, double *unused, double *out_4759906320160358758) { - out_4759906320160358758[0] = state[4]; +void h_30(double *state, double *unused, double *out_3741631544889310676) { + out_3741631544889310676[0] = state[4]; } -void H_30(double *state, double *unused, double *out_6209318792702908426) { - out_6209318792702908426[0] = 0; - out_6209318792702908426[1] = 0; - out_6209318792702908426[2] = 0; - out_6209318792702908426[3] = 0; - out_6209318792702908426[4] = 1; - out_6209318792702908426[5] = 0; - out_6209318792702908426[6] = 0; - out_6209318792702908426[7] = 0; - out_6209318792702908426[8] = 0; +void H_30(double *state, double *unused, double *out_9071030814708967724) { + out_9071030814708967724[0] = 0; + out_9071030814708967724[1] = 0; + out_9071030814708967724[2] = 0; + out_9071030814708967724[3] = 0; + out_9071030814708967724[4] = 1; + out_9071030814708967724[5] = 0; + out_9071030814708967724[6] = 0; + out_9071030814708967724[7] = 0; + out_9071030814708967724[8] = 0; } -void h_26(double *state, double *unused, double *out_163294403103182073) { - out_163294403103182073[0] = state[7]; +void h_26(double *state, double *unused, double *out_7702349825027175161) { + out_7702349825027175161[0] = state[7]; } -void H_26(double *state, double *unused, double *out_2597154420972092272) { - out_2597154420972092272[0] = 0; - out_2597154420972092272[1] = 0; - out_2597154420972092272[2] = 0; - out_2597154420972092272[3] = 0; - out_2597154420972092272[4] = 0; - out_2597154420972092272[5] = 0; - out_2597154420972092272[6] = 0; - out_2597154420972092272[7] = 1; - out_2597154420972092272[8] = 0; +void H_26(double *state, double *unused, double *out_3115876981619279041) { + out_3115876981619279041[0] = 0; + out_3115876981619279041[1] = 0; + out_3115876981619279041[2] = 0; + out_3115876981619279041[3] = 0; + out_3115876981619279041[4] = 0; + out_3115876981619279041[5] = 0; + out_3115876981619279041[6] = 0; + out_3115876981619279041[7] = 1; + out_3115876981619279041[8] = 0; } -void h_27(double *state, double *unused, double *out_45287009169805329) { - out_45287009169805329[0] = state[3]; +void h_27(double *state, double *unused, double *out_2155476886073861158) { + out_2155476886073861158[0] = state[3]; } -void H_27(double *state, double *unused, double *out_4034555480902483515) { - out_4034555480902483515[0] = 0; - out_4034555480902483515[1] = 0; - out_4034555480902483515[2] = 0; - out_4034555480902483515[3] = 1; - out_4034555480902483515[4] = 0; - out_4034555480902483515[5] = 0; - out_4034555480902483515[6] = 0; - out_4034555480902483515[7] = 0; - out_4034555480902483515[8] = 0; +void H_27(double *state, double *unused, double *out_7200949947200158981) { + out_7200949947200158981[0] = 0; + out_7200949947200158981[1] = 0; + out_7200949947200158981[2] = 0; + out_7200949947200158981[3] = 1; + out_7200949947200158981[4] = 0; + out_7200949947200158981[5] = 0; + out_7200949947200158981[6] = 0; + out_7200949947200158981[7] = 0; + out_7200949947200158981[8] = 0; } -void h_29(double *state, double *unused, double *out_4143590307449177193) { - out_4143590307449177193[0] = state[1]; +void h_29(double *state, double *unused, double *out_8743304812550156321) { + out_8743304812550156321[0] = state[1]; } -void H_29(double *state, double *unused, double *out_6719550137017300610) { - out_6719550137017300610[0] = 0; - out_6719550137017300610[1] = 1; - out_6719550137017300610[2] = 0; - out_6719550137017300610[3] = 0; - out_6719550137017300610[4] = 0; - out_6719550137017300610[5] = 0; - out_6719550137017300610[6] = 0; - out_6719550137017300610[7] = 0; - out_6719550137017300610[8] = 0; +void H_29(double *state, double *unused, double *out_8560799470394575540) { + out_8560799470394575540[0] = 0; + out_8560799470394575540[1] = 1; + out_8560799470394575540[2] = 0; + out_8560799470394575540[3] = 0; + out_8560799470394575540[4] = 0; + out_8560799470394575540[5] = 0; + out_8560799470394575540[6] = 0; + out_8560799470394575540[7] = 0; + out_8560799470394575540[8] = 0; } -void h_28(double *state, double *unused, double *out_8542875672635036079) { - out_8542875672635036079[0] = state[0]; +void h_28(double *state, double *unused, double *out_1211967033172198723) { + out_1211967033172198723[0] = state[0]; } -void H_28(double *state, double *unused, double *out_1637151119947770036) { - out_1637151119947770036[0] = 1; - out_1637151119947770036[1] = 0; - out_1637151119947770036[2] = 0; - out_1637151119947770036[3] = 0; - out_1637151119947770036[4] = 0; - out_1637151119947770036[5] = 0; - out_1637151119947770036[6] = 0; - out_1637151119947770036[7] = 0; - out_1637151119947770036[8] = 0; +void H_28(double *state, double *unused, double *out_4803545586245445502) { + out_4803545586245445502[0] = 1; + out_4803545586245445502[1] = 0; + out_4803545586245445502[2] = 0; + out_4803545586245445502[3] = 0; + out_4803545586245445502[4] = 0; + out_4803545586245445502[5] = 0; + out_4803545586245445502[6] = 0; + out_4803545586245445502[7] = 0; + out_4803545586245445502[8] = 0; } -void h_31(double *state, double *unused, double *out_5322154339115156863) { - out_5322154339115156863[0] = state[8]; +void h_31(double *state, double *unused, double *out_6483645909841006837) { + out_6483645909841006837[0] = state[8]; } -void H_31(double *state, double *unused, double *out_6369303701723108924) { - out_6369303701723108924[0] = 0; - out_6369303701723108924[1] = 0; - out_6369303701723108924[2] = 0; - out_6369303701723108924[3] = 0; - out_6369303701723108924[4] = 0; - out_6369303701723108924[5] = 0; - out_6369303701723108924[6] = 0; - out_6369303701723108924[7] = 0; - out_6369303701723108924[8] = 1; +void H_31(double *state, double *unused, double *out_2489668879385927565) { + out_2489668879385927565[0] = 0; + out_2489668879385927565[1] = 0; + out_2489668879385927565[2] = 0; + out_2489668879385927565[3] = 0; + out_2489668879385927565[4] = 0; + out_2489668879385927565[5] = 0; + out_2489668879385927565[6] = 0; + out_2489668879385927565[7] = 0; + out_2489668879385927565[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_8250891246740759679) { - err_fun(nom_x, delta_x, out_8250891246740759679); +void car_err_fun(double *nom_x, double *delta_x, double *out_4271848112759039807) { + err_fun(nom_x, delta_x, out_4271848112759039807); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_6699306425357192023) { - inv_err_fun(nom_x, true_x, out_6699306425357192023); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_2031572598342150124) { + inv_err_fun(nom_x, true_x, out_2031572598342150124); } -void car_H_mod_fun(double *state, double *out_2914898107951165883) { - H_mod_fun(state, out_2914898107951165883); +void car_H_mod_fun(double *state, double *out_177684180657030354) { + H_mod_fun(state, out_177684180657030354); } -void car_f_fun(double *state, double dt, double *out_2675178968316523152) { - f_fun(state, dt, out_2675178968316523152); +void car_f_fun(double *state, double dt, double *out_6521764996120993087) { + f_fun(state, dt, out_6521764996120993087); } -void car_F_fun(double *state, double dt, double *out_5308189961048980992) { - F_fun(state, dt, out_5308189961048980992); +void car_F_fun(double *state, double dt, double *out_4624398414228863854) { + F_fun(state, dt, out_4624398414228863854); } -void car_h_25(double *state, double *unused, double *out_6353754508244043817) { - h_25(state, unused, out_6353754508244043817); +void car_h_25(double *state, double *unused, double *out_5192262937518193843) { + h_25(state, unused, out_5192262937518193843); } -void car_H_25(double *state, double *unused, double *out_6338657739846148496) { - H_25(state, unused, out_6338657739846148496); +void car_H_25(double *state, double *unused, double *out_6857380300493335265) { + H_25(state, unused, out_6857380300493335265); } -void car_h_24(double *state, double *unused, double *out_2065669686396091997) { - h_24(state, unused, out_2065669686396091997); +void car_h_24(double *state, double *unused, double *out_6066922532331985124) { + h_24(state, unused, out_6066922532331985124); } -void car_H_24(double *state, double *unused, double *out_1518336235190160233) { - H_24(state, unused, out_1518336235190160233); +void car_H_24(double *state, double *unused, double *out_4684730701487835699) { + H_24(state, unused, out_4684730701487835699); } -void car_h_30(double *state, double *unused, double *out_4759906320160358758) { - h_30(state, unused, out_4759906320160358758); +void car_h_30(double *state, double *unused, double *out_3741631544889310676) { + h_30(state, unused, out_3741631544889310676); } -void car_H_30(double *state, double *unused, double *out_6209318792702908426) { - H_30(state, unused, out_6209318792702908426); +void car_H_30(double *state, double *unused, double *out_9071030814708967724) { + H_30(state, unused, out_9071030814708967724); } -void car_h_26(double *state, double *unused, double *out_163294403103182073) { - h_26(state, unused, out_163294403103182073); +void car_h_26(double *state, double *unused, double *out_7702349825027175161) { + h_26(state, unused, out_7702349825027175161); } -void car_H_26(double *state, double *unused, double *out_2597154420972092272) { - H_26(state, unused, out_2597154420972092272); +void car_H_26(double *state, double *unused, double *out_3115876981619279041) { + H_26(state, unused, out_3115876981619279041); } -void car_h_27(double *state, double *unused, double *out_45287009169805329) { - h_27(state, unused, out_45287009169805329); +void car_h_27(double *state, double *unused, double *out_2155476886073861158) { + h_27(state, unused, out_2155476886073861158); } -void car_H_27(double *state, double *unused, double *out_4034555480902483515) { - H_27(state, unused, out_4034555480902483515); +void car_H_27(double *state, double *unused, double *out_7200949947200158981) { + H_27(state, unused, out_7200949947200158981); } -void car_h_29(double *state, double *unused, double *out_4143590307449177193) { - h_29(state, unused, out_4143590307449177193); +void car_h_29(double *state, double *unused, double *out_8743304812550156321) { + h_29(state, unused, out_8743304812550156321); } -void car_H_29(double *state, double *unused, double *out_6719550137017300610) { - H_29(state, unused, out_6719550137017300610); +void car_H_29(double *state, double *unused, double *out_8560799470394575540) { + H_29(state, unused, out_8560799470394575540); } -void car_h_28(double *state, double *unused, double *out_8542875672635036079) { - h_28(state, unused, out_8542875672635036079); +void car_h_28(double *state, double *unused, double *out_1211967033172198723) { + h_28(state, unused, out_1211967033172198723); } -void car_H_28(double *state, double *unused, double *out_1637151119947770036) { - H_28(state, unused, out_1637151119947770036); +void car_H_28(double *state, double *unused, double *out_4803545586245445502) { + H_28(state, unused, out_4803545586245445502); } -void car_h_31(double *state, double *unused, double *out_5322154339115156863) { - h_31(state, unused, out_5322154339115156863); +void car_h_31(double *state, double *unused, double *out_6483645909841006837) { + h_31(state, unused, out_6483645909841006837); } -void car_H_31(double *state, double *unused, double *out_6369303701723108924) { - H_31(state, unused, out_6369303701723108924); +void car_H_31(double *state, double *unused, double *out_2489668879385927565) { + H_31(state, unused, out_2489668879385927565); } 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 7ae397a614..9aa00f2915 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_8250891246740759679); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_6699306425357192023); -void car_H_mod_fun(double *state, double *out_2914898107951165883); -void car_f_fun(double *state, double dt, double *out_2675178968316523152); -void car_F_fun(double *state, double dt, double *out_5308189961048980992); -void car_h_25(double *state, double *unused, double *out_6353754508244043817); -void car_H_25(double *state, double *unused, double *out_6338657739846148496); -void car_h_24(double *state, double *unused, double *out_2065669686396091997); -void car_H_24(double *state, double *unused, double *out_1518336235190160233); -void car_h_30(double *state, double *unused, double *out_4759906320160358758); -void car_H_30(double *state, double *unused, double *out_6209318792702908426); -void car_h_26(double *state, double *unused, double *out_163294403103182073); -void car_H_26(double *state, double *unused, double *out_2597154420972092272); -void car_h_27(double *state, double *unused, double *out_45287009169805329); -void car_H_27(double *state, double *unused, double *out_4034555480902483515); -void car_h_29(double *state, double *unused, double *out_4143590307449177193); -void car_H_29(double *state, double *unused, double *out_6719550137017300610); -void car_h_28(double *state, double *unused, double *out_8542875672635036079); -void car_H_28(double *state, double *unused, double *out_1637151119947770036); -void car_h_31(double *state, double *unused, double *out_5322154339115156863); -void car_H_31(double *state, double *unused, double *out_6369303701723108924); +void car_err_fun(double *nom_x, double *delta_x, double *out_4271848112759039807); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_2031572598342150124); +void car_H_mod_fun(double *state, double *out_177684180657030354); +void car_f_fun(double *state, double dt, double *out_6521764996120993087); +void car_F_fun(double *state, double dt, double *out_4624398414228863854); +void car_h_25(double *state, double *unused, double *out_5192262937518193843); +void car_H_25(double *state, double *unused, double *out_6857380300493335265); +void car_h_24(double *state, double *unused, double *out_6066922532331985124); +void car_H_24(double *state, double *unused, double *out_4684730701487835699); +void car_h_30(double *state, double *unused, double *out_3741631544889310676); +void car_H_30(double *state, double *unused, double *out_9071030814708967724); +void car_h_26(double *state, double *unused, double *out_7702349825027175161); +void car_H_26(double *state, double *unused, double *out_3115876981619279041); +void car_h_27(double *state, double *unused, double *out_2155476886073861158); +void car_H_27(double *state, double *unused, double *out_7200949947200158981); +void car_h_29(double *state, double *unused, double *out_8743304812550156321); +void car_H_29(double *state, double *unused, double *out_8560799470394575540); +void car_h_28(double *state, double *unused, double *out_1211967033172198723); +void car_H_28(double *state, double *unused, double *out_4803545586245445502); +void car_h_31(double *state, double *unused, double *out_6483645909841006837); +void car_H_31(double *state, double *unused, double *out_2489668879385927565); 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 62b4538119..49a2289d23 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_1942165964233553274) { - out_1942165964233553274[0] = delta_x[0] + nom_x[0]; - out_1942165964233553274[1] = delta_x[1] + nom_x[1]; - out_1942165964233553274[2] = delta_x[2] + nom_x[2]; - out_1942165964233553274[3] = delta_x[3] + nom_x[3]; - out_1942165964233553274[4] = delta_x[4] + nom_x[4]; - out_1942165964233553274[5] = delta_x[5] + nom_x[5]; - out_1942165964233553274[6] = delta_x[6] + nom_x[6]; - out_1942165964233553274[7] = delta_x[7] + nom_x[7]; - out_1942165964233553274[8] = delta_x[8] + nom_x[8]; - out_1942165964233553274[9] = delta_x[9] + nom_x[9]; - out_1942165964233553274[10] = delta_x[10] + nom_x[10]; - out_1942165964233553274[11] = delta_x[11] + nom_x[11]; - out_1942165964233553274[12] = delta_x[12] + nom_x[12]; - out_1942165964233553274[13] = delta_x[13] + nom_x[13]; - out_1942165964233553274[14] = delta_x[14] + nom_x[14]; - out_1942165964233553274[15] = delta_x[15] + nom_x[15]; - out_1942165964233553274[16] = delta_x[16] + nom_x[16]; - out_1942165964233553274[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_6891956137394554809) { + out_6891956137394554809[0] = delta_x[0] + nom_x[0]; + out_6891956137394554809[1] = delta_x[1] + nom_x[1]; + out_6891956137394554809[2] = delta_x[2] + nom_x[2]; + out_6891956137394554809[3] = delta_x[3] + nom_x[3]; + out_6891956137394554809[4] = delta_x[4] + nom_x[4]; + out_6891956137394554809[5] = delta_x[5] + nom_x[5]; + out_6891956137394554809[6] = delta_x[6] + nom_x[6]; + out_6891956137394554809[7] = delta_x[7] + nom_x[7]; + out_6891956137394554809[8] = delta_x[8] + nom_x[8]; + out_6891956137394554809[9] = delta_x[9] + nom_x[9]; + out_6891956137394554809[10] = delta_x[10] + nom_x[10]; + out_6891956137394554809[11] = delta_x[11] + nom_x[11]; + out_6891956137394554809[12] = delta_x[12] + nom_x[12]; + out_6891956137394554809[13] = delta_x[13] + nom_x[13]; + out_6891956137394554809[14] = delta_x[14] + nom_x[14]; + out_6891956137394554809[15] = delta_x[15] + nom_x[15]; + out_6891956137394554809[16] = delta_x[16] + nom_x[16]; + out_6891956137394554809[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_2238116153961167505) { - out_2238116153961167505[0] = -nom_x[0] + true_x[0]; - out_2238116153961167505[1] = -nom_x[1] + true_x[1]; - out_2238116153961167505[2] = -nom_x[2] + true_x[2]; - out_2238116153961167505[3] = -nom_x[3] + true_x[3]; - out_2238116153961167505[4] = -nom_x[4] + true_x[4]; - out_2238116153961167505[5] = -nom_x[5] + true_x[5]; - out_2238116153961167505[6] = -nom_x[6] + true_x[6]; - out_2238116153961167505[7] = -nom_x[7] + true_x[7]; - out_2238116153961167505[8] = -nom_x[8] + true_x[8]; - out_2238116153961167505[9] = -nom_x[9] + true_x[9]; - out_2238116153961167505[10] = -nom_x[10] + true_x[10]; - out_2238116153961167505[11] = -nom_x[11] + true_x[11]; - out_2238116153961167505[12] = -nom_x[12] + true_x[12]; - out_2238116153961167505[13] = -nom_x[13] + true_x[13]; - out_2238116153961167505[14] = -nom_x[14] + true_x[14]; - out_2238116153961167505[15] = -nom_x[15] + true_x[15]; - out_2238116153961167505[16] = -nom_x[16] + true_x[16]; - out_2238116153961167505[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_352074767866048570) { + out_352074767866048570[0] = -nom_x[0] + true_x[0]; + out_352074767866048570[1] = -nom_x[1] + true_x[1]; + out_352074767866048570[2] = -nom_x[2] + true_x[2]; + out_352074767866048570[3] = -nom_x[3] + true_x[3]; + out_352074767866048570[4] = -nom_x[4] + true_x[4]; + out_352074767866048570[5] = -nom_x[5] + true_x[5]; + out_352074767866048570[6] = -nom_x[6] + true_x[6]; + out_352074767866048570[7] = -nom_x[7] + true_x[7]; + out_352074767866048570[8] = -nom_x[8] + true_x[8]; + out_352074767866048570[9] = -nom_x[9] + true_x[9]; + out_352074767866048570[10] = -nom_x[10] + true_x[10]; + out_352074767866048570[11] = -nom_x[11] + true_x[11]; + out_352074767866048570[12] = -nom_x[12] + true_x[12]; + out_352074767866048570[13] = -nom_x[13] + true_x[13]; + out_352074767866048570[14] = -nom_x[14] + true_x[14]; + out_352074767866048570[15] = -nom_x[15] + true_x[15]; + out_352074767866048570[16] = -nom_x[16] + true_x[16]; + out_352074767866048570[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_4990084456757481789) { - out_4990084456757481789[0] = 1.0; - out_4990084456757481789[1] = 0.0; - out_4990084456757481789[2] = 0.0; - out_4990084456757481789[3] = 0.0; - out_4990084456757481789[4] = 0.0; - out_4990084456757481789[5] = 0.0; - out_4990084456757481789[6] = 0.0; - out_4990084456757481789[7] = 0.0; - out_4990084456757481789[8] = 0.0; - out_4990084456757481789[9] = 0.0; - out_4990084456757481789[10] = 0.0; - out_4990084456757481789[11] = 0.0; - out_4990084456757481789[12] = 0.0; - out_4990084456757481789[13] = 0.0; - out_4990084456757481789[14] = 0.0; - out_4990084456757481789[15] = 0.0; - out_4990084456757481789[16] = 0.0; - out_4990084456757481789[17] = 0.0; - out_4990084456757481789[18] = 0.0; - out_4990084456757481789[19] = 1.0; - out_4990084456757481789[20] = 0.0; - out_4990084456757481789[21] = 0.0; - out_4990084456757481789[22] = 0.0; - out_4990084456757481789[23] = 0.0; - out_4990084456757481789[24] = 0.0; - out_4990084456757481789[25] = 0.0; - out_4990084456757481789[26] = 0.0; - out_4990084456757481789[27] = 0.0; - out_4990084456757481789[28] = 0.0; - out_4990084456757481789[29] = 0.0; - out_4990084456757481789[30] = 0.0; - out_4990084456757481789[31] = 0.0; - out_4990084456757481789[32] = 0.0; - out_4990084456757481789[33] = 0.0; - out_4990084456757481789[34] = 0.0; - out_4990084456757481789[35] = 0.0; - out_4990084456757481789[36] = 0.0; - out_4990084456757481789[37] = 0.0; - out_4990084456757481789[38] = 1.0; - out_4990084456757481789[39] = 0.0; - out_4990084456757481789[40] = 0.0; - out_4990084456757481789[41] = 0.0; - out_4990084456757481789[42] = 0.0; - out_4990084456757481789[43] = 0.0; - out_4990084456757481789[44] = 0.0; - out_4990084456757481789[45] = 0.0; - out_4990084456757481789[46] = 0.0; - out_4990084456757481789[47] = 0.0; - out_4990084456757481789[48] = 0.0; - out_4990084456757481789[49] = 0.0; - out_4990084456757481789[50] = 0.0; - out_4990084456757481789[51] = 0.0; - out_4990084456757481789[52] = 0.0; - out_4990084456757481789[53] = 0.0; - out_4990084456757481789[54] = 0.0; - out_4990084456757481789[55] = 0.0; - out_4990084456757481789[56] = 0.0; - out_4990084456757481789[57] = 1.0; - out_4990084456757481789[58] = 0.0; - out_4990084456757481789[59] = 0.0; - out_4990084456757481789[60] = 0.0; - out_4990084456757481789[61] = 0.0; - out_4990084456757481789[62] = 0.0; - out_4990084456757481789[63] = 0.0; - out_4990084456757481789[64] = 0.0; - out_4990084456757481789[65] = 0.0; - out_4990084456757481789[66] = 0.0; - out_4990084456757481789[67] = 0.0; - out_4990084456757481789[68] = 0.0; - out_4990084456757481789[69] = 0.0; - out_4990084456757481789[70] = 0.0; - out_4990084456757481789[71] = 0.0; - out_4990084456757481789[72] = 0.0; - out_4990084456757481789[73] = 0.0; - out_4990084456757481789[74] = 0.0; - out_4990084456757481789[75] = 0.0; - out_4990084456757481789[76] = 1.0; - out_4990084456757481789[77] = 0.0; - out_4990084456757481789[78] = 0.0; - out_4990084456757481789[79] = 0.0; - out_4990084456757481789[80] = 0.0; - out_4990084456757481789[81] = 0.0; - out_4990084456757481789[82] = 0.0; - out_4990084456757481789[83] = 0.0; - out_4990084456757481789[84] = 0.0; - out_4990084456757481789[85] = 0.0; - out_4990084456757481789[86] = 0.0; - out_4990084456757481789[87] = 0.0; - out_4990084456757481789[88] = 0.0; - out_4990084456757481789[89] = 0.0; - out_4990084456757481789[90] = 0.0; - out_4990084456757481789[91] = 0.0; - out_4990084456757481789[92] = 0.0; - out_4990084456757481789[93] = 0.0; - out_4990084456757481789[94] = 0.0; - out_4990084456757481789[95] = 1.0; - out_4990084456757481789[96] = 0.0; - out_4990084456757481789[97] = 0.0; - out_4990084456757481789[98] = 0.0; - out_4990084456757481789[99] = 0.0; - out_4990084456757481789[100] = 0.0; - out_4990084456757481789[101] = 0.0; - out_4990084456757481789[102] = 0.0; - out_4990084456757481789[103] = 0.0; - out_4990084456757481789[104] = 0.0; - out_4990084456757481789[105] = 0.0; - out_4990084456757481789[106] = 0.0; - out_4990084456757481789[107] = 0.0; - out_4990084456757481789[108] = 0.0; - out_4990084456757481789[109] = 0.0; - out_4990084456757481789[110] = 0.0; - out_4990084456757481789[111] = 0.0; - out_4990084456757481789[112] = 0.0; - out_4990084456757481789[113] = 0.0; - out_4990084456757481789[114] = 1.0; - out_4990084456757481789[115] = 0.0; - out_4990084456757481789[116] = 0.0; - out_4990084456757481789[117] = 0.0; - out_4990084456757481789[118] = 0.0; - out_4990084456757481789[119] = 0.0; - out_4990084456757481789[120] = 0.0; - out_4990084456757481789[121] = 0.0; - out_4990084456757481789[122] = 0.0; - out_4990084456757481789[123] = 0.0; - out_4990084456757481789[124] = 0.0; - out_4990084456757481789[125] = 0.0; - out_4990084456757481789[126] = 0.0; - out_4990084456757481789[127] = 0.0; - out_4990084456757481789[128] = 0.0; - out_4990084456757481789[129] = 0.0; - out_4990084456757481789[130] = 0.0; - out_4990084456757481789[131] = 0.0; - out_4990084456757481789[132] = 0.0; - out_4990084456757481789[133] = 1.0; - out_4990084456757481789[134] = 0.0; - out_4990084456757481789[135] = 0.0; - out_4990084456757481789[136] = 0.0; - out_4990084456757481789[137] = 0.0; - out_4990084456757481789[138] = 0.0; - out_4990084456757481789[139] = 0.0; - out_4990084456757481789[140] = 0.0; - out_4990084456757481789[141] = 0.0; - out_4990084456757481789[142] = 0.0; - out_4990084456757481789[143] = 0.0; - out_4990084456757481789[144] = 0.0; - out_4990084456757481789[145] = 0.0; - out_4990084456757481789[146] = 0.0; - out_4990084456757481789[147] = 0.0; - out_4990084456757481789[148] = 0.0; - out_4990084456757481789[149] = 0.0; - out_4990084456757481789[150] = 0.0; - out_4990084456757481789[151] = 0.0; - out_4990084456757481789[152] = 1.0; - out_4990084456757481789[153] = 0.0; - out_4990084456757481789[154] = 0.0; - out_4990084456757481789[155] = 0.0; - out_4990084456757481789[156] = 0.0; - out_4990084456757481789[157] = 0.0; - out_4990084456757481789[158] = 0.0; - out_4990084456757481789[159] = 0.0; - out_4990084456757481789[160] = 0.0; - out_4990084456757481789[161] = 0.0; - out_4990084456757481789[162] = 0.0; - out_4990084456757481789[163] = 0.0; - out_4990084456757481789[164] = 0.0; - out_4990084456757481789[165] = 0.0; - out_4990084456757481789[166] = 0.0; - out_4990084456757481789[167] = 0.0; - out_4990084456757481789[168] = 0.0; - out_4990084456757481789[169] = 0.0; - out_4990084456757481789[170] = 0.0; - out_4990084456757481789[171] = 1.0; - out_4990084456757481789[172] = 0.0; - out_4990084456757481789[173] = 0.0; - out_4990084456757481789[174] = 0.0; - out_4990084456757481789[175] = 0.0; - out_4990084456757481789[176] = 0.0; - out_4990084456757481789[177] = 0.0; - out_4990084456757481789[178] = 0.0; - out_4990084456757481789[179] = 0.0; - out_4990084456757481789[180] = 0.0; - out_4990084456757481789[181] = 0.0; - out_4990084456757481789[182] = 0.0; - out_4990084456757481789[183] = 0.0; - out_4990084456757481789[184] = 0.0; - out_4990084456757481789[185] = 0.0; - out_4990084456757481789[186] = 0.0; - out_4990084456757481789[187] = 0.0; - out_4990084456757481789[188] = 0.0; - out_4990084456757481789[189] = 0.0; - out_4990084456757481789[190] = 1.0; - out_4990084456757481789[191] = 0.0; - out_4990084456757481789[192] = 0.0; - out_4990084456757481789[193] = 0.0; - out_4990084456757481789[194] = 0.0; - out_4990084456757481789[195] = 0.0; - out_4990084456757481789[196] = 0.0; - out_4990084456757481789[197] = 0.0; - out_4990084456757481789[198] = 0.0; - out_4990084456757481789[199] = 0.0; - out_4990084456757481789[200] = 0.0; - out_4990084456757481789[201] = 0.0; - out_4990084456757481789[202] = 0.0; - out_4990084456757481789[203] = 0.0; - out_4990084456757481789[204] = 0.0; - out_4990084456757481789[205] = 0.0; - out_4990084456757481789[206] = 0.0; - out_4990084456757481789[207] = 0.0; - out_4990084456757481789[208] = 0.0; - out_4990084456757481789[209] = 1.0; - out_4990084456757481789[210] = 0.0; - out_4990084456757481789[211] = 0.0; - out_4990084456757481789[212] = 0.0; - out_4990084456757481789[213] = 0.0; - out_4990084456757481789[214] = 0.0; - out_4990084456757481789[215] = 0.0; - out_4990084456757481789[216] = 0.0; - out_4990084456757481789[217] = 0.0; - out_4990084456757481789[218] = 0.0; - out_4990084456757481789[219] = 0.0; - out_4990084456757481789[220] = 0.0; - out_4990084456757481789[221] = 0.0; - out_4990084456757481789[222] = 0.0; - out_4990084456757481789[223] = 0.0; - out_4990084456757481789[224] = 0.0; - out_4990084456757481789[225] = 0.0; - out_4990084456757481789[226] = 0.0; - out_4990084456757481789[227] = 0.0; - out_4990084456757481789[228] = 1.0; - out_4990084456757481789[229] = 0.0; - out_4990084456757481789[230] = 0.0; - out_4990084456757481789[231] = 0.0; - out_4990084456757481789[232] = 0.0; - out_4990084456757481789[233] = 0.0; - out_4990084456757481789[234] = 0.0; - out_4990084456757481789[235] = 0.0; - out_4990084456757481789[236] = 0.0; - out_4990084456757481789[237] = 0.0; - out_4990084456757481789[238] = 0.0; - out_4990084456757481789[239] = 0.0; - out_4990084456757481789[240] = 0.0; - out_4990084456757481789[241] = 0.0; - out_4990084456757481789[242] = 0.0; - out_4990084456757481789[243] = 0.0; - out_4990084456757481789[244] = 0.0; - out_4990084456757481789[245] = 0.0; - out_4990084456757481789[246] = 0.0; - out_4990084456757481789[247] = 1.0; - out_4990084456757481789[248] = 0.0; - out_4990084456757481789[249] = 0.0; - out_4990084456757481789[250] = 0.0; - out_4990084456757481789[251] = 0.0; - out_4990084456757481789[252] = 0.0; - out_4990084456757481789[253] = 0.0; - out_4990084456757481789[254] = 0.0; - out_4990084456757481789[255] = 0.0; - out_4990084456757481789[256] = 0.0; - out_4990084456757481789[257] = 0.0; - out_4990084456757481789[258] = 0.0; - out_4990084456757481789[259] = 0.0; - out_4990084456757481789[260] = 0.0; - out_4990084456757481789[261] = 0.0; - out_4990084456757481789[262] = 0.0; - out_4990084456757481789[263] = 0.0; - out_4990084456757481789[264] = 0.0; - out_4990084456757481789[265] = 0.0; - out_4990084456757481789[266] = 1.0; - out_4990084456757481789[267] = 0.0; - out_4990084456757481789[268] = 0.0; - out_4990084456757481789[269] = 0.0; - out_4990084456757481789[270] = 0.0; - out_4990084456757481789[271] = 0.0; - out_4990084456757481789[272] = 0.0; - out_4990084456757481789[273] = 0.0; - out_4990084456757481789[274] = 0.0; - out_4990084456757481789[275] = 0.0; - out_4990084456757481789[276] = 0.0; - out_4990084456757481789[277] = 0.0; - out_4990084456757481789[278] = 0.0; - out_4990084456757481789[279] = 0.0; - out_4990084456757481789[280] = 0.0; - out_4990084456757481789[281] = 0.0; - out_4990084456757481789[282] = 0.0; - out_4990084456757481789[283] = 0.0; - out_4990084456757481789[284] = 0.0; - out_4990084456757481789[285] = 1.0; - out_4990084456757481789[286] = 0.0; - out_4990084456757481789[287] = 0.0; - out_4990084456757481789[288] = 0.0; - out_4990084456757481789[289] = 0.0; - out_4990084456757481789[290] = 0.0; - out_4990084456757481789[291] = 0.0; - out_4990084456757481789[292] = 0.0; - out_4990084456757481789[293] = 0.0; - out_4990084456757481789[294] = 0.0; - out_4990084456757481789[295] = 0.0; - out_4990084456757481789[296] = 0.0; - out_4990084456757481789[297] = 0.0; - out_4990084456757481789[298] = 0.0; - out_4990084456757481789[299] = 0.0; - out_4990084456757481789[300] = 0.0; - out_4990084456757481789[301] = 0.0; - out_4990084456757481789[302] = 0.0; - out_4990084456757481789[303] = 0.0; - out_4990084456757481789[304] = 1.0; - out_4990084456757481789[305] = 0.0; - out_4990084456757481789[306] = 0.0; - out_4990084456757481789[307] = 0.0; - out_4990084456757481789[308] = 0.0; - out_4990084456757481789[309] = 0.0; - out_4990084456757481789[310] = 0.0; - out_4990084456757481789[311] = 0.0; - out_4990084456757481789[312] = 0.0; - out_4990084456757481789[313] = 0.0; - out_4990084456757481789[314] = 0.0; - out_4990084456757481789[315] = 0.0; - out_4990084456757481789[316] = 0.0; - out_4990084456757481789[317] = 0.0; - out_4990084456757481789[318] = 0.0; - out_4990084456757481789[319] = 0.0; - out_4990084456757481789[320] = 0.0; - out_4990084456757481789[321] = 0.0; - out_4990084456757481789[322] = 0.0; - out_4990084456757481789[323] = 1.0; +void H_mod_fun(double *state, double *out_57344037476075931) { + out_57344037476075931[0] = 1.0; + out_57344037476075931[1] = 0.0; + out_57344037476075931[2] = 0.0; + out_57344037476075931[3] = 0.0; + out_57344037476075931[4] = 0.0; + out_57344037476075931[5] = 0.0; + out_57344037476075931[6] = 0.0; + out_57344037476075931[7] = 0.0; + out_57344037476075931[8] = 0.0; + out_57344037476075931[9] = 0.0; + out_57344037476075931[10] = 0.0; + out_57344037476075931[11] = 0.0; + out_57344037476075931[12] = 0.0; + out_57344037476075931[13] = 0.0; + out_57344037476075931[14] = 0.0; + out_57344037476075931[15] = 0.0; + out_57344037476075931[16] = 0.0; + out_57344037476075931[17] = 0.0; + out_57344037476075931[18] = 0.0; + out_57344037476075931[19] = 1.0; + out_57344037476075931[20] = 0.0; + out_57344037476075931[21] = 0.0; + out_57344037476075931[22] = 0.0; + out_57344037476075931[23] = 0.0; + out_57344037476075931[24] = 0.0; + out_57344037476075931[25] = 0.0; + out_57344037476075931[26] = 0.0; + out_57344037476075931[27] = 0.0; + out_57344037476075931[28] = 0.0; + out_57344037476075931[29] = 0.0; + out_57344037476075931[30] = 0.0; + out_57344037476075931[31] = 0.0; + out_57344037476075931[32] = 0.0; + out_57344037476075931[33] = 0.0; + out_57344037476075931[34] = 0.0; + out_57344037476075931[35] = 0.0; + out_57344037476075931[36] = 0.0; + out_57344037476075931[37] = 0.0; + out_57344037476075931[38] = 1.0; + out_57344037476075931[39] = 0.0; + out_57344037476075931[40] = 0.0; + out_57344037476075931[41] = 0.0; + out_57344037476075931[42] = 0.0; + out_57344037476075931[43] = 0.0; + out_57344037476075931[44] = 0.0; + out_57344037476075931[45] = 0.0; + out_57344037476075931[46] = 0.0; + out_57344037476075931[47] = 0.0; + out_57344037476075931[48] = 0.0; + out_57344037476075931[49] = 0.0; + out_57344037476075931[50] = 0.0; + out_57344037476075931[51] = 0.0; + out_57344037476075931[52] = 0.0; + out_57344037476075931[53] = 0.0; + out_57344037476075931[54] = 0.0; + out_57344037476075931[55] = 0.0; + out_57344037476075931[56] = 0.0; + out_57344037476075931[57] = 1.0; + out_57344037476075931[58] = 0.0; + out_57344037476075931[59] = 0.0; + out_57344037476075931[60] = 0.0; + out_57344037476075931[61] = 0.0; + out_57344037476075931[62] = 0.0; + out_57344037476075931[63] = 0.0; + out_57344037476075931[64] = 0.0; + out_57344037476075931[65] = 0.0; + out_57344037476075931[66] = 0.0; + out_57344037476075931[67] = 0.0; + out_57344037476075931[68] = 0.0; + out_57344037476075931[69] = 0.0; + out_57344037476075931[70] = 0.0; + out_57344037476075931[71] = 0.0; + out_57344037476075931[72] = 0.0; + out_57344037476075931[73] = 0.0; + out_57344037476075931[74] = 0.0; + out_57344037476075931[75] = 0.0; + out_57344037476075931[76] = 1.0; + out_57344037476075931[77] = 0.0; + out_57344037476075931[78] = 0.0; + out_57344037476075931[79] = 0.0; + out_57344037476075931[80] = 0.0; + out_57344037476075931[81] = 0.0; + out_57344037476075931[82] = 0.0; + out_57344037476075931[83] = 0.0; + out_57344037476075931[84] = 0.0; + out_57344037476075931[85] = 0.0; + out_57344037476075931[86] = 0.0; + out_57344037476075931[87] = 0.0; + out_57344037476075931[88] = 0.0; + out_57344037476075931[89] = 0.0; + out_57344037476075931[90] = 0.0; + out_57344037476075931[91] = 0.0; + out_57344037476075931[92] = 0.0; + out_57344037476075931[93] = 0.0; + out_57344037476075931[94] = 0.0; + out_57344037476075931[95] = 1.0; + out_57344037476075931[96] = 0.0; + out_57344037476075931[97] = 0.0; + out_57344037476075931[98] = 0.0; + out_57344037476075931[99] = 0.0; + out_57344037476075931[100] = 0.0; + out_57344037476075931[101] = 0.0; + out_57344037476075931[102] = 0.0; + out_57344037476075931[103] = 0.0; + out_57344037476075931[104] = 0.0; + out_57344037476075931[105] = 0.0; + out_57344037476075931[106] = 0.0; + out_57344037476075931[107] = 0.0; + out_57344037476075931[108] = 0.0; + out_57344037476075931[109] = 0.0; + out_57344037476075931[110] = 0.0; + out_57344037476075931[111] = 0.0; + out_57344037476075931[112] = 0.0; + out_57344037476075931[113] = 0.0; + out_57344037476075931[114] = 1.0; + out_57344037476075931[115] = 0.0; + out_57344037476075931[116] = 0.0; + out_57344037476075931[117] = 0.0; + out_57344037476075931[118] = 0.0; + out_57344037476075931[119] = 0.0; + out_57344037476075931[120] = 0.0; + out_57344037476075931[121] = 0.0; + out_57344037476075931[122] = 0.0; + out_57344037476075931[123] = 0.0; + out_57344037476075931[124] = 0.0; + out_57344037476075931[125] = 0.0; + out_57344037476075931[126] = 0.0; + out_57344037476075931[127] = 0.0; + out_57344037476075931[128] = 0.0; + out_57344037476075931[129] = 0.0; + out_57344037476075931[130] = 0.0; + out_57344037476075931[131] = 0.0; + out_57344037476075931[132] = 0.0; + out_57344037476075931[133] = 1.0; + out_57344037476075931[134] = 0.0; + out_57344037476075931[135] = 0.0; + out_57344037476075931[136] = 0.0; + out_57344037476075931[137] = 0.0; + out_57344037476075931[138] = 0.0; + out_57344037476075931[139] = 0.0; + out_57344037476075931[140] = 0.0; + out_57344037476075931[141] = 0.0; + out_57344037476075931[142] = 0.0; + out_57344037476075931[143] = 0.0; + out_57344037476075931[144] = 0.0; + out_57344037476075931[145] = 0.0; + out_57344037476075931[146] = 0.0; + out_57344037476075931[147] = 0.0; + out_57344037476075931[148] = 0.0; + out_57344037476075931[149] = 0.0; + out_57344037476075931[150] = 0.0; + out_57344037476075931[151] = 0.0; + out_57344037476075931[152] = 1.0; + out_57344037476075931[153] = 0.0; + out_57344037476075931[154] = 0.0; + out_57344037476075931[155] = 0.0; + out_57344037476075931[156] = 0.0; + out_57344037476075931[157] = 0.0; + out_57344037476075931[158] = 0.0; + out_57344037476075931[159] = 0.0; + out_57344037476075931[160] = 0.0; + out_57344037476075931[161] = 0.0; + out_57344037476075931[162] = 0.0; + out_57344037476075931[163] = 0.0; + out_57344037476075931[164] = 0.0; + out_57344037476075931[165] = 0.0; + out_57344037476075931[166] = 0.0; + out_57344037476075931[167] = 0.0; + out_57344037476075931[168] = 0.0; + out_57344037476075931[169] = 0.0; + out_57344037476075931[170] = 0.0; + out_57344037476075931[171] = 1.0; + out_57344037476075931[172] = 0.0; + out_57344037476075931[173] = 0.0; + out_57344037476075931[174] = 0.0; + out_57344037476075931[175] = 0.0; + out_57344037476075931[176] = 0.0; + out_57344037476075931[177] = 0.0; + out_57344037476075931[178] = 0.0; + out_57344037476075931[179] = 0.0; + out_57344037476075931[180] = 0.0; + out_57344037476075931[181] = 0.0; + out_57344037476075931[182] = 0.0; + out_57344037476075931[183] = 0.0; + out_57344037476075931[184] = 0.0; + out_57344037476075931[185] = 0.0; + out_57344037476075931[186] = 0.0; + out_57344037476075931[187] = 0.0; + out_57344037476075931[188] = 0.0; + out_57344037476075931[189] = 0.0; + out_57344037476075931[190] = 1.0; + out_57344037476075931[191] = 0.0; + out_57344037476075931[192] = 0.0; + out_57344037476075931[193] = 0.0; + out_57344037476075931[194] = 0.0; + out_57344037476075931[195] = 0.0; + out_57344037476075931[196] = 0.0; + out_57344037476075931[197] = 0.0; + out_57344037476075931[198] = 0.0; + out_57344037476075931[199] = 0.0; + out_57344037476075931[200] = 0.0; + out_57344037476075931[201] = 0.0; + out_57344037476075931[202] = 0.0; + out_57344037476075931[203] = 0.0; + out_57344037476075931[204] = 0.0; + out_57344037476075931[205] = 0.0; + out_57344037476075931[206] = 0.0; + out_57344037476075931[207] = 0.0; + out_57344037476075931[208] = 0.0; + out_57344037476075931[209] = 1.0; + out_57344037476075931[210] = 0.0; + out_57344037476075931[211] = 0.0; + out_57344037476075931[212] = 0.0; + out_57344037476075931[213] = 0.0; + out_57344037476075931[214] = 0.0; + out_57344037476075931[215] = 0.0; + out_57344037476075931[216] = 0.0; + out_57344037476075931[217] = 0.0; + out_57344037476075931[218] = 0.0; + out_57344037476075931[219] = 0.0; + out_57344037476075931[220] = 0.0; + out_57344037476075931[221] = 0.0; + out_57344037476075931[222] = 0.0; + out_57344037476075931[223] = 0.0; + out_57344037476075931[224] = 0.0; + out_57344037476075931[225] = 0.0; + out_57344037476075931[226] = 0.0; + out_57344037476075931[227] = 0.0; + out_57344037476075931[228] = 1.0; + out_57344037476075931[229] = 0.0; + out_57344037476075931[230] = 0.0; + out_57344037476075931[231] = 0.0; + out_57344037476075931[232] = 0.0; + out_57344037476075931[233] = 0.0; + out_57344037476075931[234] = 0.0; + out_57344037476075931[235] = 0.0; + out_57344037476075931[236] = 0.0; + out_57344037476075931[237] = 0.0; + out_57344037476075931[238] = 0.0; + out_57344037476075931[239] = 0.0; + out_57344037476075931[240] = 0.0; + out_57344037476075931[241] = 0.0; + out_57344037476075931[242] = 0.0; + out_57344037476075931[243] = 0.0; + out_57344037476075931[244] = 0.0; + out_57344037476075931[245] = 0.0; + out_57344037476075931[246] = 0.0; + out_57344037476075931[247] = 1.0; + out_57344037476075931[248] = 0.0; + out_57344037476075931[249] = 0.0; + out_57344037476075931[250] = 0.0; + out_57344037476075931[251] = 0.0; + out_57344037476075931[252] = 0.0; + out_57344037476075931[253] = 0.0; + out_57344037476075931[254] = 0.0; + out_57344037476075931[255] = 0.0; + out_57344037476075931[256] = 0.0; + out_57344037476075931[257] = 0.0; + out_57344037476075931[258] = 0.0; + out_57344037476075931[259] = 0.0; + out_57344037476075931[260] = 0.0; + out_57344037476075931[261] = 0.0; + out_57344037476075931[262] = 0.0; + out_57344037476075931[263] = 0.0; + out_57344037476075931[264] = 0.0; + out_57344037476075931[265] = 0.0; + out_57344037476075931[266] = 1.0; + out_57344037476075931[267] = 0.0; + out_57344037476075931[268] = 0.0; + out_57344037476075931[269] = 0.0; + out_57344037476075931[270] = 0.0; + out_57344037476075931[271] = 0.0; + out_57344037476075931[272] = 0.0; + out_57344037476075931[273] = 0.0; + out_57344037476075931[274] = 0.0; + out_57344037476075931[275] = 0.0; + out_57344037476075931[276] = 0.0; + out_57344037476075931[277] = 0.0; + out_57344037476075931[278] = 0.0; + out_57344037476075931[279] = 0.0; + out_57344037476075931[280] = 0.0; + out_57344037476075931[281] = 0.0; + out_57344037476075931[282] = 0.0; + out_57344037476075931[283] = 0.0; + out_57344037476075931[284] = 0.0; + out_57344037476075931[285] = 1.0; + out_57344037476075931[286] = 0.0; + out_57344037476075931[287] = 0.0; + out_57344037476075931[288] = 0.0; + out_57344037476075931[289] = 0.0; + out_57344037476075931[290] = 0.0; + out_57344037476075931[291] = 0.0; + out_57344037476075931[292] = 0.0; + out_57344037476075931[293] = 0.0; + out_57344037476075931[294] = 0.0; + out_57344037476075931[295] = 0.0; + out_57344037476075931[296] = 0.0; + out_57344037476075931[297] = 0.0; + out_57344037476075931[298] = 0.0; + out_57344037476075931[299] = 0.0; + out_57344037476075931[300] = 0.0; + out_57344037476075931[301] = 0.0; + out_57344037476075931[302] = 0.0; + out_57344037476075931[303] = 0.0; + out_57344037476075931[304] = 1.0; + out_57344037476075931[305] = 0.0; + out_57344037476075931[306] = 0.0; + out_57344037476075931[307] = 0.0; + out_57344037476075931[308] = 0.0; + out_57344037476075931[309] = 0.0; + out_57344037476075931[310] = 0.0; + out_57344037476075931[311] = 0.0; + out_57344037476075931[312] = 0.0; + out_57344037476075931[313] = 0.0; + out_57344037476075931[314] = 0.0; + out_57344037476075931[315] = 0.0; + out_57344037476075931[316] = 0.0; + out_57344037476075931[317] = 0.0; + out_57344037476075931[318] = 0.0; + out_57344037476075931[319] = 0.0; + out_57344037476075931[320] = 0.0; + out_57344037476075931[321] = 0.0; + out_57344037476075931[322] = 0.0; + out_57344037476075931[323] = 1.0; } -void f_fun(double *state, double dt, double *out_6296202895308932040) { - out_6296202895308932040[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); - out_6296202895308932040[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); - out_6296202895308932040[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); - out_6296202895308932040[3] = dt*state[12] + state[3]; - out_6296202895308932040[4] = dt*state[13] + state[4]; - out_6296202895308932040[5] = dt*state[14] + state[5]; - out_6296202895308932040[6] = state[6]; - out_6296202895308932040[7] = state[7]; - out_6296202895308932040[8] = state[8]; - out_6296202895308932040[9] = state[9]; - out_6296202895308932040[10] = state[10]; - out_6296202895308932040[11] = state[11]; - out_6296202895308932040[12] = state[12]; - out_6296202895308932040[13] = state[13]; - out_6296202895308932040[14] = state[14]; - out_6296202895308932040[15] = state[15]; - out_6296202895308932040[16] = state[16]; - out_6296202895308932040[17] = state[17]; +void f_fun(double *state, double dt, double *out_8328845329683204295) { + out_8328845329683204295[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_8328845329683204295[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_8328845329683204295[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_8328845329683204295[3] = dt*state[12] + state[3]; + out_8328845329683204295[4] = dt*state[13] + state[4]; + out_8328845329683204295[5] = dt*state[14] + state[5]; + out_8328845329683204295[6] = state[6]; + out_8328845329683204295[7] = state[7]; + out_8328845329683204295[8] = state[8]; + out_8328845329683204295[9] = state[9]; + out_8328845329683204295[10] = state[10]; + out_8328845329683204295[11] = state[11]; + out_8328845329683204295[12] = state[12]; + out_8328845329683204295[13] = state[13]; + out_8328845329683204295[14] = state[14]; + out_8328845329683204295[15] = state[15]; + out_8328845329683204295[16] = state[16]; + out_8328845329683204295[17] = state[17]; } -void F_fun(double *state, double dt, double *out_909355394007943716) { - out_909355394007943716[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_909355394007943716[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_909355394007943716[2] = 0; - out_909355394007943716[3] = 0; - out_909355394007943716[4] = 0; - out_909355394007943716[5] = 0; - out_909355394007943716[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_909355394007943716[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_909355394007943716[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_909355394007943716[9] = 0; - out_909355394007943716[10] = 0; - out_909355394007943716[11] = 0; - out_909355394007943716[12] = 0; - out_909355394007943716[13] = 0; - out_909355394007943716[14] = 0; - out_909355394007943716[15] = 0; - out_909355394007943716[16] = 0; - out_909355394007943716[17] = 0; - out_909355394007943716[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_909355394007943716[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_909355394007943716[20] = 0; - out_909355394007943716[21] = 0; - out_909355394007943716[22] = 0; - out_909355394007943716[23] = 0; - out_909355394007943716[24] = 0; - out_909355394007943716[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_909355394007943716[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_909355394007943716[27] = 0; - out_909355394007943716[28] = 0; - out_909355394007943716[29] = 0; - out_909355394007943716[30] = 0; - out_909355394007943716[31] = 0; - out_909355394007943716[32] = 0; - out_909355394007943716[33] = 0; - out_909355394007943716[34] = 0; - out_909355394007943716[35] = 0; - out_909355394007943716[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_909355394007943716[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_909355394007943716[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_909355394007943716[39] = 0; - out_909355394007943716[40] = 0; - out_909355394007943716[41] = 0; - out_909355394007943716[42] = 0; - out_909355394007943716[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_909355394007943716[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_909355394007943716[45] = 0; - out_909355394007943716[46] = 0; - out_909355394007943716[47] = 0; - out_909355394007943716[48] = 0; - out_909355394007943716[49] = 0; - out_909355394007943716[50] = 0; - out_909355394007943716[51] = 0; - out_909355394007943716[52] = 0; - out_909355394007943716[53] = 0; - out_909355394007943716[54] = 0; - out_909355394007943716[55] = 0; - out_909355394007943716[56] = 0; - out_909355394007943716[57] = 1; - out_909355394007943716[58] = 0; - out_909355394007943716[59] = 0; - out_909355394007943716[60] = 0; - out_909355394007943716[61] = 0; - out_909355394007943716[62] = 0; - out_909355394007943716[63] = 0; - out_909355394007943716[64] = 0; - out_909355394007943716[65] = 0; - out_909355394007943716[66] = dt; - out_909355394007943716[67] = 0; - out_909355394007943716[68] = 0; - out_909355394007943716[69] = 0; - out_909355394007943716[70] = 0; - out_909355394007943716[71] = 0; - out_909355394007943716[72] = 0; - out_909355394007943716[73] = 0; - out_909355394007943716[74] = 0; - out_909355394007943716[75] = 0; - out_909355394007943716[76] = 1; - out_909355394007943716[77] = 0; - out_909355394007943716[78] = 0; - out_909355394007943716[79] = 0; - out_909355394007943716[80] = 0; - out_909355394007943716[81] = 0; - out_909355394007943716[82] = 0; - out_909355394007943716[83] = 0; - out_909355394007943716[84] = 0; - out_909355394007943716[85] = dt; - out_909355394007943716[86] = 0; - out_909355394007943716[87] = 0; - out_909355394007943716[88] = 0; - out_909355394007943716[89] = 0; - out_909355394007943716[90] = 0; - out_909355394007943716[91] = 0; - out_909355394007943716[92] = 0; - out_909355394007943716[93] = 0; - out_909355394007943716[94] = 0; - out_909355394007943716[95] = 1; - out_909355394007943716[96] = 0; - out_909355394007943716[97] = 0; - out_909355394007943716[98] = 0; - out_909355394007943716[99] = 0; - out_909355394007943716[100] = 0; - out_909355394007943716[101] = 0; - out_909355394007943716[102] = 0; - out_909355394007943716[103] = 0; - out_909355394007943716[104] = dt; - out_909355394007943716[105] = 0; - out_909355394007943716[106] = 0; - out_909355394007943716[107] = 0; - out_909355394007943716[108] = 0; - out_909355394007943716[109] = 0; - out_909355394007943716[110] = 0; - out_909355394007943716[111] = 0; - out_909355394007943716[112] = 0; - out_909355394007943716[113] = 0; - out_909355394007943716[114] = 1; - out_909355394007943716[115] = 0; - out_909355394007943716[116] = 0; - out_909355394007943716[117] = 0; - out_909355394007943716[118] = 0; - out_909355394007943716[119] = 0; - out_909355394007943716[120] = 0; - out_909355394007943716[121] = 0; - out_909355394007943716[122] = 0; - out_909355394007943716[123] = 0; - out_909355394007943716[124] = 0; - out_909355394007943716[125] = 0; - out_909355394007943716[126] = 0; - out_909355394007943716[127] = 0; - out_909355394007943716[128] = 0; - out_909355394007943716[129] = 0; - out_909355394007943716[130] = 0; - out_909355394007943716[131] = 0; - out_909355394007943716[132] = 0; - out_909355394007943716[133] = 1; - out_909355394007943716[134] = 0; - out_909355394007943716[135] = 0; - out_909355394007943716[136] = 0; - out_909355394007943716[137] = 0; - out_909355394007943716[138] = 0; - out_909355394007943716[139] = 0; - out_909355394007943716[140] = 0; - out_909355394007943716[141] = 0; - out_909355394007943716[142] = 0; - out_909355394007943716[143] = 0; - out_909355394007943716[144] = 0; - out_909355394007943716[145] = 0; - out_909355394007943716[146] = 0; - out_909355394007943716[147] = 0; - out_909355394007943716[148] = 0; - out_909355394007943716[149] = 0; - out_909355394007943716[150] = 0; - out_909355394007943716[151] = 0; - out_909355394007943716[152] = 1; - out_909355394007943716[153] = 0; - out_909355394007943716[154] = 0; - out_909355394007943716[155] = 0; - out_909355394007943716[156] = 0; - out_909355394007943716[157] = 0; - out_909355394007943716[158] = 0; - out_909355394007943716[159] = 0; - out_909355394007943716[160] = 0; - out_909355394007943716[161] = 0; - out_909355394007943716[162] = 0; - out_909355394007943716[163] = 0; - out_909355394007943716[164] = 0; - out_909355394007943716[165] = 0; - out_909355394007943716[166] = 0; - out_909355394007943716[167] = 0; - out_909355394007943716[168] = 0; - out_909355394007943716[169] = 0; - out_909355394007943716[170] = 0; - out_909355394007943716[171] = 1; - out_909355394007943716[172] = 0; - out_909355394007943716[173] = 0; - out_909355394007943716[174] = 0; - out_909355394007943716[175] = 0; - out_909355394007943716[176] = 0; - out_909355394007943716[177] = 0; - out_909355394007943716[178] = 0; - out_909355394007943716[179] = 0; - out_909355394007943716[180] = 0; - out_909355394007943716[181] = 0; - out_909355394007943716[182] = 0; - out_909355394007943716[183] = 0; - out_909355394007943716[184] = 0; - out_909355394007943716[185] = 0; - out_909355394007943716[186] = 0; - out_909355394007943716[187] = 0; - out_909355394007943716[188] = 0; - out_909355394007943716[189] = 0; - out_909355394007943716[190] = 1; - out_909355394007943716[191] = 0; - out_909355394007943716[192] = 0; - out_909355394007943716[193] = 0; - out_909355394007943716[194] = 0; - out_909355394007943716[195] = 0; - out_909355394007943716[196] = 0; - out_909355394007943716[197] = 0; - out_909355394007943716[198] = 0; - out_909355394007943716[199] = 0; - out_909355394007943716[200] = 0; - out_909355394007943716[201] = 0; - out_909355394007943716[202] = 0; - out_909355394007943716[203] = 0; - out_909355394007943716[204] = 0; - out_909355394007943716[205] = 0; - out_909355394007943716[206] = 0; - out_909355394007943716[207] = 0; - out_909355394007943716[208] = 0; - out_909355394007943716[209] = 1; - out_909355394007943716[210] = 0; - out_909355394007943716[211] = 0; - out_909355394007943716[212] = 0; - out_909355394007943716[213] = 0; - out_909355394007943716[214] = 0; - out_909355394007943716[215] = 0; - out_909355394007943716[216] = 0; - out_909355394007943716[217] = 0; - out_909355394007943716[218] = 0; - out_909355394007943716[219] = 0; - out_909355394007943716[220] = 0; - out_909355394007943716[221] = 0; - out_909355394007943716[222] = 0; - out_909355394007943716[223] = 0; - out_909355394007943716[224] = 0; - out_909355394007943716[225] = 0; - out_909355394007943716[226] = 0; - out_909355394007943716[227] = 0; - out_909355394007943716[228] = 1; - out_909355394007943716[229] = 0; - out_909355394007943716[230] = 0; - out_909355394007943716[231] = 0; - out_909355394007943716[232] = 0; - out_909355394007943716[233] = 0; - out_909355394007943716[234] = 0; - out_909355394007943716[235] = 0; - out_909355394007943716[236] = 0; - out_909355394007943716[237] = 0; - out_909355394007943716[238] = 0; - out_909355394007943716[239] = 0; - out_909355394007943716[240] = 0; - out_909355394007943716[241] = 0; - out_909355394007943716[242] = 0; - out_909355394007943716[243] = 0; - out_909355394007943716[244] = 0; - out_909355394007943716[245] = 0; - out_909355394007943716[246] = 0; - out_909355394007943716[247] = 1; - out_909355394007943716[248] = 0; - out_909355394007943716[249] = 0; - out_909355394007943716[250] = 0; - out_909355394007943716[251] = 0; - out_909355394007943716[252] = 0; - out_909355394007943716[253] = 0; - out_909355394007943716[254] = 0; - out_909355394007943716[255] = 0; - out_909355394007943716[256] = 0; - out_909355394007943716[257] = 0; - out_909355394007943716[258] = 0; - out_909355394007943716[259] = 0; - out_909355394007943716[260] = 0; - out_909355394007943716[261] = 0; - out_909355394007943716[262] = 0; - out_909355394007943716[263] = 0; - out_909355394007943716[264] = 0; - out_909355394007943716[265] = 0; - out_909355394007943716[266] = 1; - out_909355394007943716[267] = 0; - out_909355394007943716[268] = 0; - out_909355394007943716[269] = 0; - out_909355394007943716[270] = 0; - out_909355394007943716[271] = 0; - out_909355394007943716[272] = 0; - out_909355394007943716[273] = 0; - out_909355394007943716[274] = 0; - out_909355394007943716[275] = 0; - out_909355394007943716[276] = 0; - out_909355394007943716[277] = 0; - out_909355394007943716[278] = 0; - out_909355394007943716[279] = 0; - out_909355394007943716[280] = 0; - out_909355394007943716[281] = 0; - out_909355394007943716[282] = 0; - out_909355394007943716[283] = 0; - out_909355394007943716[284] = 0; - out_909355394007943716[285] = 1; - out_909355394007943716[286] = 0; - out_909355394007943716[287] = 0; - out_909355394007943716[288] = 0; - out_909355394007943716[289] = 0; - out_909355394007943716[290] = 0; - out_909355394007943716[291] = 0; - out_909355394007943716[292] = 0; - out_909355394007943716[293] = 0; - out_909355394007943716[294] = 0; - out_909355394007943716[295] = 0; - out_909355394007943716[296] = 0; - out_909355394007943716[297] = 0; - out_909355394007943716[298] = 0; - out_909355394007943716[299] = 0; - out_909355394007943716[300] = 0; - out_909355394007943716[301] = 0; - out_909355394007943716[302] = 0; - out_909355394007943716[303] = 0; - out_909355394007943716[304] = 1; - out_909355394007943716[305] = 0; - out_909355394007943716[306] = 0; - out_909355394007943716[307] = 0; - out_909355394007943716[308] = 0; - out_909355394007943716[309] = 0; - out_909355394007943716[310] = 0; - out_909355394007943716[311] = 0; - out_909355394007943716[312] = 0; - out_909355394007943716[313] = 0; - out_909355394007943716[314] = 0; - out_909355394007943716[315] = 0; - out_909355394007943716[316] = 0; - out_909355394007943716[317] = 0; - out_909355394007943716[318] = 0; - out_909355394007943716[319] = 0; - out_909355394007943716[320] = 0; - out_909355394007943716[321] = 0; - out_909355394007943716[322] = 0; - out_909355394007943716[323] = 1; +void F_fun(double *state, double dt, double *out_6465495001911852221) { + out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[2] = 0; + out_6465495001911852221[3] = 0; + out_6465495001911852221[4] = 0; + out_6465495001911852221[5] = 0; + out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[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_6465495001911852221[9] = 0; + out_6465495001911852221[10] = 0; + out_6465495001911852221[11] = 0; + out_6465495001911852221[12] = 0; + out_6465495001911852221[13] = 0; + out_6465495001911852221[14] = 0; + out_6465495001911852221[15] = 0; + out_6465495001911852221[16] = 0; + out_6465495001911852221[17] = 0; + out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[20] = 0; + out_6465495001911852221[21] = 0; + out_6465495001911852221[22] = 0; + out_6465495001911852221[23] = 0; + out_6465495001911852221[24] = 0; + out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[27] = 0; + out_6465495001911852221[28] = 0; + out_6465495001911852221[29] = 0; + out_6465495001911852221[30] = 0; + out_6465495001911852221[31] = 0; + out_6465495001911852221[32] = 0; + out_6465495001911852221[33] = 0; + out_6465495001911852221[34] = 0; + out_6465495001911852221[35] = 0; + out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[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_6465495001911852221[39] = 0; + out_6465495001911852221[40] = 0; + out_6465495001911852221[41] = 0; + out_6465495001911852221[42] = 0; + out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[45] = 0; + out_6465495001911852221[46] = 0; + out_6465495001911852221[47] = 0; + out_6465495001911852221[48] = 0; + out_6465495001911852221[49] = 0; + out_6465495001911852221[50] = 0; + out_6465495001911852221[51] = 0; + out_6465495001911852221[52] = 0; + out_6465495001911852221[53] = 0; + out_6465495001911852221[54] = 0; + out_6465495001911852221[55] = 0; + out_6465495001911852221[56] = 0; + out_6465495001911852221[57] = 1; + out_6465495001911852221[58] = 0; + out_6465495001911852221[59] = 0; + out_6465495001911852221[60] = 0; + out_6465495001911852221[61] = 0; + out_6465495001911852221[62] = 0; + out_6465495001911852221[63] = 0; + out_6465495001911852221[64] = 0; + out_6465495001911852221[65] = 0; + out_6465495001911852221[66] = dt; + out_6465495001911852221[67] = 0; + out_6465495001911852221[68] = 0; + out_6465495001911852221[69] = 0; + out_6465495001911852221[70] = 0; + out_6465495001911852221[71] = 0; + out_6465495001911852221[72] = 0; + out_6465495001911852221[73] = 0; + out_6465495001911852221[74] = 0; + out_6465495001911852221[75] = 0; + out_6465495001911852221[76] = 1; + out_6465495001911852221[77] = 0; + out_6465495001911852221[78] = 0; + out_6465495001911852221[79] = 0; + out_6465495001911852221[80] = 0; + out_6465495001911852221[81] = 0; + out_6465495001911852221[82] = 0; + out_6465495001911852221[83] = 0; + out_6465495001911852221[84] = 0; + out_6465495001911852221[85] = dt; + out_6465495001911852221[86] = 0; + out_6465495001911852221[87] = 0; + out_6465495001911852221[88] = 0; + out_6465495001911852221[89] = 0; + out_6465495001911852221[90] = 0; + out_6465495001911852221[91] = 0; + out_6465495001911852221[92] = 0; + out_6465495001911852221[93] = 0; + out_6465495001911852221[94] = 0; + out_6465495001911852221[95] = 1; + out_6465495001911852221[96] = 0; + out_6465495001911852221[97] = 0; + out_6465495001911852221[98] = 0; + out_6465495001911852221[99] = 0; + out_6465495001911852221[100] = 0; + out_6465495001911852221[101] = 0; + out_6465495001911852221[102] = 0; + out_6465495001911852221[103] = 0; + out_6465495001911852221[104] = dt; + out_6465495001911852221[105] = 0; + out_6465495001911852221[106] = 0; + out_6465495001911852221[107] = 0; + out_6465495001911852221[108] = 0; + out_6465495001911852221[109] = 0; + out_6465495001911852221[110] = 0; + out_6465495001911852221[111] = 0; + out_6465495001911852221[112] = 0; + out_6465495001911852221[113] = 0; + out_6465495001911852221[114] = 1; + out_6465495001911852221[115] = 0; + out_6465495001911852221[116] = 0; + out_6465495001911852221[117] = 0; + out_6465495001911852221[118] = 0; + out_6465495001911852221[119] = 0; + out_6465495001911852221[120] = 0; + out_6465495001911852221[121] = 0; + out_6465495001911852221[122] = 0; + out_6465495001911852221[123] = 0; + out_6465495001911852221[124] = 0; + out_6465495001911852221[125] = 0; + out_6465495001911852221[126] = 0; + out_6465495001911852221[127] = 0; + out_6465495001911852221[128] = 0; + out_6465495001911852221[129] = 0; + out_6465495001911852221[130] = 0; + out_6465495001911852221[131] = 0; + out_6465495001911852221[132] = 0; + out_6465495001911852221[133] = 1; + out_6465495001911852221[134] = 0; + out_6465495001911852221[135] = 0; + out_6465495001911852221[136] = 0; + out_6465495001911852221[137] = 0; + out_6465495001911852221[138] = 0; + out_6465495001911852221[139] = 0; + out_6465495001911852221[140] = 0; + out_6465495001911852221[141] = 0; + out_6465495001911852221[142] = 0; + out_6465495001911852221[143] = 0; + out_6465495001911852221[144] = 0; + out_6465495001911852221[145] = 0; + out_6465495001911852221[146] = 0; + out_6465495001911852221[147] = 0; + out_6465495001911852221[148] = 0; + out_6465495001911852221[149] = 0; + out_6465495001911852221[150] = 0; + out_6465495001911852221[151] = 0; + out_6465495001911852221[152] = 1; + out_6465495001911852221[153] = 0; + out_6465495001911852221[154] = 0; + out_6465495001911852221[155] = 0; + out_6465495001911852221[156] = 0; + out_6465495001911852221[157] = 0; + out_6465495001911852221[158] = 0; + out_6465495001911852221[159] = 0; + out_6465495001911852221[160] = 0; + out_6465495001911852221[161] = 0; + out_6465495001911852221[162] = 0; + out_6465495001911852221[163] = 0; + out_6465495001911852221[164] = 0; + out_6465495001911852221[165] = 0; + out_6465495001911852221[166] = 0; + out_6465495001911852221[167] = 0; + out_6465495001911852221[168] = 0; + out_6465495001911852221[169] = 0; + out_6465495001911852221[170] = 0; + out_6465495001911852221[171] = 1; + out_6465495001911852221[172] = 0; + out_6465495001911852221[173] = 0; + out_6465495001911852221[174] = 0; + out_6465495001911852221[175] = 0; + out_6465495001911852221[176] = 0; + out_6465495001911852221[177] = 0; + out_6465495001911852221[178] = 0; + out_6465495001911852221[179] = 0; + out_6465495001911852221[180] = 0; + out_6465495001911852221[181] = 0; + out_6465495001911852221[182] = 0; + out_6465495001911852221[183] = 0; + out_6465495001911852221[184] = 0; + out_6465495001911852221[185] = 0; + out_6465495001911852221[186] = 0; + out_6465495001911852221[187] = 0; + out_6465495001911852221[188] = 0; + out_6465495001911852221[189] = 0; + out_6465495001911852221[190] = 1; + out_6465495001911852221[191] = 0; + out_6465495001911852221[192] = 0; + out_6465495001911852221[193] = 0; + out_6465495001911852221[194] = 0; + out_6465495001911852221[195] = 0; + out_6465495001911852221[196] = 0; + out_6465495001911852221[197] = 0; + out_6465495001911852221[198] = 0; + out_6465495001911852221[199] = 0; + out_6465495001911852221[200] = 0; + out_6465495001911852221[201] = 0; + out_6465495001911852221[202] = 0; + out_6465495001911852221[203] = 0; + out_6465495001911852221[204] = 0; + out_6465495001911852221[205] = 0; + out_6465495001911852221[206] = 0; + out_6465495001911852221[207] = 0; + out_6465495001911852221[208] = 0; + out_6465495001911852221[209] = 1; + out_6465495001911852221[210] = 0; + out_6465495001911852221[211] = 0; + out_6465495001911852221[212] = 0; + out_6465495001911852221[213] = 0; + out_6465495001911852221[214] = 0; + out_6465495001911852221[215] = 0; + out_6465495001911852221[216] = 0; + out_6465495001911852221[217] = 0; + out_6465495001911852221[218] = 0; + out_6465495001911852221[219] = 0; + out_6465495001911852221[220] = 0; + out_6465495001911852221[221] = 0; + out_6465495001911852221[222] = 0; + out_6465495001911852221[223] = 0; + out_6465495001911852221[224] = 0; + out_6465495001911852221[225] = 0; + out_6465495001911852221[226] = 0; + out_6465495001911852221[227] = 0; + out_6465495001911852221[228] = 1; + out_6465495001911852221[229] = 0; + out_6465495001911852221[230] = 0; + out_6465495001911852221[231] = 0; + out_6465495001911852221[232] = 0; + out_6465495001911852221[233] = 0; + out_6465495001911852221[234] = 0; + out_6465495001911852221[235] = 0; + out_6465495001911852221[236] = 0; + out_6465495001911852221[237] = 0; + out_6465495001911852221[238] = 0; + out_6465495001911852221[239] = 0; + out_6465495001911852221[240] = 0; + out_6465495001911852221[241] = 0; + out_6465495001911852221[242] = 0; + out_6465495001911852221[243] = 0; + out_6465495001911852221[244] = 0; + out_6465495001911852221[245] = 0; + out_6465495001911852221[246] = 0; + out_6465495001911852221[247] = 1; + out_6465495001911852221[248] = 0; + out_6465495001911852221[249] = 0; + out_6465495001911852221[250] = 0; + out_6465495001911852221[251] = 0; + out_6465495001911852221[252] = 0; + out_6465495001911852221[253] = 0; + out_6465495001911852221[254] = 0; + out_6465495001911852221[255] = 0; + out_6465495001911852221[256] = 0; + out_6465495001911852221[257] = 0; + out_6465495001911852221[258] = 0; + out_6465495001911852221[259] = 0; + out_6465495001911852221[260] = 0; + out_6465495001911852221[261] = 0; + out_6465495001911852221[262] = 0; + out_6465495001911852221[263] = 0; + out_6465495001911852221[264] = 0; + out_6465495001911852221[265] = 0; + out_6465495001911852221[266] = 1; + out_6465495001911852221[267] = 0; + out_6465495001911852221[268] = 0; + out_6465495001911852221[269] = 0; + out_6465495001911852221[270] = 0; + out_6465495001911852221[271] = 0; + out_6465495001911852221[272] = 0; + out_6465495001911852221[273] = 0; + out_6465495001911852221[274] = 0; + out_6465495001911852221[275] = 0; + out_6465495001911852221[276] = 0; + out_6465495001911852221[277] = 0; + out_6465495001911852221[278] = 0; + out_6465495001911852221[279] = 0; + out_6465495001911852221[280] = 0; + out_6465495001911852221[281] = 0; + out_6465495001911852221[282] = 0; + out_6465495001911852221[283] = 0; + out_6465495001911852221[284] = 0; + out_6465495001911852221[285] = 1; + out_6465495001911852221[286] = 0; + out_6465495001911852221[287] = 0; + out_6465495001911852221[288] = 0; + out_6465495001911852221[289] = 0; + out_6465495001911852221[290] = 0; + out_6465495001911852221[291] = 0; + out_6465495001911852221[292] = 0; + out_6465495001911852221[293] = 0; + out_6465495001911852221[294] = 0; + out_6465495001911852221[295] = 0; + out_6465495001911852221[296] = 0; + out_6465495001911852221[297] = 0; + out_6465495001911852221[298] = 0; + out_6465495001911852221[299] = 0; + out_6465495001911852221[300] = 0; + out_6465495001911852221[301] = 0; + out_6465495001911852221[302] = 0; + out_6465495001911852221[303] = 0; + out_6465495001911852221[304] = 1; + out_6465495001911852221[305] = 0; + out_6465495001911852221[306] = 0; + out_6465495001911852221[307] = 0; + out_6465495001911852221[308] = 0; + out_6465495001911852221[309] = 0; + out_6465495001911852221[310] = 0; + out_6465495001911852221[311] = 0; + out_6465495001911852221[312] = 0; + out_6465495001911852221[313] = 0; + out_6465495001911852221[314] = 0; + out_6465495001911852221[315] = 0; + out_6465495001911852221[316] = 0; + out_6465495001911852221[317] = 0; + out_6465495001911852221[318] = 0; + out_6465495001911852221[319] = 0; + out_6465495001911852221[320] = 0; + out_6465495001911852221[321] = 0; + out_6465495001911852221[322] = 0; + out_6465495001911852221[323] = 1; } -void h_4(double *state, double *unused, double *out_6818977063477642683) { - out_6818977063477642683[0] = state[6] + state[9]; - out_6818977063477642683[1] = state[7] + state[10]; - out_6818977063477642683[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_7442631847161232120) { + out_7442631847161232120[0] = state[6] + state[9]; + out_7442631847161232120[1] = state[7] + state[10]; + out_7442631847161232120[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_3954277209300339553) { - out_3954277209300339553[0] = 0; - out_3954277209300339553[1] = 0; - out_3954277209300339553[2] = 0; - out_3954277209300339553[3] = 0; - out_3954277209300339553[4] = 0; - out_3954277209300339553[5] = 0; - out_3954277209300339553[6] = 1; - out_3954277209300339553[7] = 0; - out_3954277209300339553[8] = 0; - out_3954277209300339553[9] = 1; - out_3954277209300339553[10] = 0; - out_3954277209300339553[11] = 0; - out_3954277209300339553[12] = 0; - out_3954277209300339553[13] = 0; - out_3954277209300339553[14] = 0; - out_3954277209300339553[15] = 0; - out_3954277209300339553[16] = 0; - out_3954277209300339553[17] = 0; - out_3954277209300339553[18] = 0; - out_3954277209300339553[19] = 0; - out_3954277209300339553[20] = 0; - out_3954277209300339553[21] = 0; - out_3954277209300339553[22] = 0; - out_3954277209300339553[23] = 0; - out_3954277209300339553[24] = 0; - out_3954277209300339553[25] = 1; - out_3954277209300339553[26] = 0; - out_3954277209300339553[27] = 0; - out_3954277209300339553[28] = 1; - out_3954277209300339553[29] = 0; - out_3954277209300339553[30] = 0; - out_3954277209300339553[31] = 0; - out_3954277209300339553[32] = 0; - out_3954277209300339553[33] = 0; - out_3954277209300339553[34] = 0; - out_3954277209300339553[35] = 0; - out_3954277209300339553[36] = 0; - out_3954277209300339553[37] = 0; - out_3954277209300339553[38] = 0; - out_3954277209300339553[39] = 0; - out_3954277209300339553[40] = 0; - out_3954277209300339553[41] = 0; - out_3954277209300339553[42] = 0; - out_3954277209300339553[43] = 0; - out_3954277209300339553[44] = 1; - out_3954277209300339553[45] = 0; - out_3954277209300339553[46] = 0; - out_3954277209300339553[47] = 1; - out_3954277209300339553[48] = 0; - out_3954277209300339553[49] = 0; - out_3954277209300339553[50] = 0; - out_3954277209300339553[51] = 0; - out_3954277209300339553[52] = 0; - out_3954277209300339553[53] = 0; +void H_4(double *state, double *unused, double *out_8142960846497148836) { + out_8142960846497148836[0] = 0; + out_8142960846497148836[1] = 0; + out_8142960846497148836[2] = 0; + out_8142960846497148836[3] = 0; + out_8142960846497148836[4] = 0; + out_8142960846497148836[5] = 0; + out_8142960846497148836[6] = 1; + out_8142960846497148836[7] = 0; + out_8142960846497148836[8] = 0; + out_8142960846497148836[9] = 1; + out_8142960846497148836[10] = 0; + out_8142960846497148836[11] = 0; + out_8142960846497148836[12] = 0; + out_8142960846497148836[13] = 0; + out_8142960846497148836[14] = 0; + out_8142960846497148836[15] = 0; + out_8142960846497148836[16] = 0; + out_8142960846497148836[17] = 0; + out_8142960846497148836[18] = 0; + out_8142960846497148836[19] = 0; + out_8142960846497148836[20] = 0; + out_8142960846497148836[21] = 0; + out_8142960846497148836[22] = 0; + out_8142960846497148836[23] = 0; + out_8142960846497148836[24] = 0; + out_8142960846497148836[25] = 1; + out_8142960846497148836[26] = 0; + out_8142960846497148836[27] = 0; + out_8142960846497148836[28] = 1; + out_8142960846497148836[29] = 0; + out_8142960846497148836[30] = 0; + out_8142960846497148836[31] = 0; + out_8142960846497148836[32] = 0; + out_8142960846497148836[33] = 0; + out_8142960846497148836[34] = 0; + out_8142960846497148836[35] = 0; + out_8142960846497148836[36] = 0; + out_8142960846497148836[37] = 0; + out_8142960846497148836[38] = 0; + out_8142960846497148836[39] = 0; + out_8142960846497148836[40] = 0; + out_8142960846497148836[41] = 0; + out_8142960846497148836[42] = 0; + out_8142960846497148836[43] = 0; + out_8142960846497148836[44] = 1; + out_8142960846497148836[45] = 0; + out_8142960846497148836[46] = 0; + out_8142960846497148836[47] = 1; + out_8142960846497148836[48] = 0; + out_8142960846497148836[49] = 0; + out_8142960846497148836[50] = 0; + out_8142960846497148836[51] = 0; + out_8142960846497148836[52] = 0; + out_8142960846497148836[53] = 0; } -void h_10(double *state, double *unused, double *out_3834013708689822319) { - out_3834013708689822319[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_3834013708689822319[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_3834013708689822319[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; +void h_10(double *state, double *unused, double *out_6865183365695080269) { + out_6865183365695080269[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_6865183365695080269[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_6865183365695080269[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_3533519720904001040) { - out_3533519720904001040[0] = 0; - out_3533519720904001040[1] = 9.8100000000000005*cos(state[1]); - out_3533519720904001040[2] = 0; - out_3533519720904001040[3] = 0; - out_3533519720904001040[4] = -state[8]; - out_3533519720904001040[5] = state[7]; - out_3533519720904001040[6] = 0; - out_3533519720904001040[7] = state[5]; - out_3533519720904001040[8] = -state[4]; - out_3533519720904001040[9] = 0; - out_3533519720904001040[10] = 0; - out_3533519720904001040[11] = 0; - out_3533519720904001040[12] = 1; - out_3533519720904001040[13] = 0; - out_3533519720904001040[14] = 0; - out_3533519720904001040[15] = 1; - out_3533519720904001040[16] = 0; - out_3533519720904001040[17] = 0; - out_3533519720904001040[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_3533519720904001040[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_3533519720904001040[20] = 0; - out_3533519720904001040[21] = state[8]; - out_3533519720904001040[22] = 0; - out_3533519720904001040[23] = -state[6]; - out_3533519720904001040[24] = -state[5]; - out_3533519720904001040[25] = 0; - out_3533519720904001040[26] = state[3]; - out_3533519720904001040[27] = 0; - out_3533519720904001040[28] = 0; - out_3533519720904001040[29] = 0; - out_3533519720904001040[30] = 0; - out_3533519720904001040[31] = 1; - out_3533519720904001040[32] = 0; - out_3533519720904001040[33] = 0; - out_3533519720904001040[34] = 1; - out_3533519720904001040[35] = 0; - out_3533519720904001040[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_3533519720904001040[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_3533519720904001040[38] = 0; - out_3533519720904001040[39] = -state[7]; - out_3533519720904001040[40] = state[6]; - out_3533519720904001040[41] = 0; - out_3533519720904001040[42] = state[4]; - out_3533519720904001040[43] = -state[3]; - out_3533519720904001040[44] = 0; - out_3533519720904001040[45] = 0; - out_3533519720904001040[46] = 0; - out_3533519720904001040[47] = 0; - out_3533519720904001040[48] = 0; - out_3533519720904001040[49] = 0; - out_3533519720904001040[50] = 1; - out_3533519720904001040[51] = 0; - out_3533519720904001040[52] = 0; - out_3533519720904001040[53] = 1; +void H_10(double *state, double *unused, double *out_456103805919410821) { + out_456103805919410821[0] = 0; + out_456103805919410821[1] = 9.8100000000000005*cos(state[1]); + out_456103805919410821[2] = 0; + out_456103805919410821[3] = 0; + out_456103805919410821[4] = -state[8]; + out_456103805919410821[5] = state[7]; + out_456103805919410821[6] = 0; + out_456103805919410821[7] = state[5]; + out_456103805919410821[8] = -state[4]; + out_456103805919410821[9] = 0; + out_456103805919410821[10] = 0; + out_456103805919410821[11] = 0; + out_456103805919410821[12] = 1; + out_456103805919410821[13] = 0; + out_456103805919410821[14] = 0; + out_456103805919410821[15] = 1; + out_456103805919410821[16] = 0; + out_456103805919410821[17] = 0; + out_456103805919410821[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_456103805919410821[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_456103805919410821[20] = 0; + out_456103805919410821[21] = state[8]; + out_456103805919410821[22] = 0; + out_456103805919410821[23] = -state[6]; + out_456103805919410821[24] = -state[5]; + out_456103805919410821[25] = 0; + out_456103805919410821[26] = state[3]; + out_456103805919410821[27] = 0; + out_456103805919410821[28] = 0; + out_456103805919410821[29] = 0; + out_456103805919410821[30] = 0; + out_456103805919410821[31] = 1; + out_456103805919410821[32] = 0; + out_456103805919410821[33] = 0; + out_456103805919410821[34] = 1; + out_456103805919410821[35] = 0; + out_456103805919410821[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_456103805919410821[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_456103805919410821[38] = 0; + out_456103805919410821[39] = -state[7]; + out_456103805919410821[40] = state[6]; + out_456103805919410821[41] = 0; + out_456103805919410821[42] = state[4]; + out_456103805919410821[43] = -state[3]; + out_456103805919410821[44] = 0; + out_456103805919410821[45] = 0; + out_456103805919410821[46] = 0; + out_456103805919410821[47] = 0; + out_456103805919410821[48] = 0; + out_456103805919410821[49] = 0; + out_456103805919410821[50] = 1; + out_456103805919410821[51] = 0; + out_456103805919410821[52] = 0; + out_456103805919410821[53] = 1; } -void h_13(double *state, double *unused, double *out_3025354993842492520) { - out_3025354993842492520[0] = state[3]; - out_3025354993842492520[1] = state[4]; - out_3025354993842492520[2] = state[5]; +void h_13(double *state, double *unused, double *out_2130091715364414233) { + out_2130091715364414233[0] = state[3]; + out_2130091715364414233[1] = state[4]; + out_2130091715364414233[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3389675289618495449) { - out_3389675289618495449[0] = 0; - out_3389675289618495449[1] = 0; - out_3389675289618495449[2] = 0; - out_3389675289618495449[3] = 1; - out_3389675289618495449[4] = 0; - out_3389675289618495449[5] = 0; - out_3389675289618495449[6] = 0; - out_3389675289618495449[7] = 0; - out_3389675289618495449[8] = 0; - out_3389675289618495449[9] = 0; - out_3389675289618495449[10] = 0; - out_3389675289618495449[11] = 0; - out_3389675289618495449[12] = 0; - out_3389675289618495449[13] = 0; - out_3389675289618495449[14] = 0; - out_3389675289618495449[15] = 0; - out_3389675289618495449[16] = 0; - out_3389675289618495449[17] = 0; - out_3389675289618495449[18] = 0; - out_3389675289618495449[19] = 0; - out_3389675289618495449[20] = 0; - out_3389675289618495449[21] = 0; - out_3389675289618495449[22] = 1; - out_3389675289618495449[23] = 0; - out_3389675289618495449[24] = 0; - out_3389675289618495449[25] = 0; - out_3389675289618495449[26] = 0; - out_3389675289618495449[27] = 0; - out_3389675289618495449[28] = 0; - out_3389675289618495449[29] = 0; - out_3389675289618495449[30] = 0; - out_3389675289618495449[31] = 0; - out_3389675289618495449[32] = 0; - out_3389675289618495449[33] = 0; - out_3389675289618495449[34] = 0; - out_3389675289618495449[35] = 0; - out_3389675289618495449[36] = 0; - out_3389675289618495449[37] = 0; - out_3389675289618495449[38] = 0; - out_3389675289618495449[39] = 0; - out_3389675289618495449[40] = 0; - out_3389675289618495449[41] = 1; - out_3389675289618495449[42] = 0; - out_3389675289618495449[43] = 0; - out_3389675289618495449[44] = 0; - out_3389675289618495449[45] = 0; - out_3389675289618495449[46] = 0; - out_3389675289618495449[47] = 0; - out_3389675289618495449[48] = 0; - out_3389675289618495449[49] = 0; - out_3389675289618495449[50] = 0; - out_3389675289618495449[51] = 0; - out_3389675289618495449[52] = 0; - out_3389675289618495449[53] = 0; +void H_13(double *state, double *unused, double *out_2693152018895701851) { + out_2693152018895701851[0] = 0; + out_2693152018895701851[1] = 0; + out_2693152018895701851[2] = 0; + out_2693152018895701851[3] = 1; + out_2693152018895701851[4] = 0; + out_2693152018895701851[5] = 0; + out_2693152018895701851[6] = 0; + out_2693152018895701851[7] = 0; + out_2693152018895701851[8] = 0; + out_2693152018895701851[9] = 0; + out_2693152018895701851[10] = 0; + out_2693152018895701851[11] = 0; + out_2693152018895701851[12] = 0; + out_2693152018895701851[13] = 0; + out_2693152018895701851[14] = 0; + out_2693152018895701851[15] = 0; + out_2693152018895701851[16] = 0; + out_2693152018895701851[17] = 0; + out_2693152018895701851[18] = 0; + out_2693152018895701851[19] = 0; + out_2693152018895701851[20] = 0; + out_2693152018895701851[21] = 0; + out_2693152018895701851[22] = 1; + out_2693152018895701851[23] = 0; + out_2693152018895701851[24] = 0; + out_2693152018895701851[25] = 0; + out_2693152018895701851[26] = 0; + out_2693152018895701851[27] = 0; + out_2693152018895701851[28] = 0; + out_2693152018895701851[29] = 0; + out_2693152018895701851[30] = 0; + out_2693152018895701851[31] = 0; + out_2693152018895701851[32] = 0; + out_2693152018895701851[33] = 0; + out_2693152018895701851[34] = 0; + out_2693152018895701851[35] = 0; + out_2693152018895701851[36] = 0; + out_2693152018895701851[37] = 0; + out_2693152018895701851[38] = 0; + out_2693152018895701851[39] = 0; + out_2693152018895701851[40] = 0; + out_2693152018895701851[41] = 1; + out_2693152018895701851[42] = 0; + out_2693152018895701851[43] = 0; + out_2693152018895701851[44] = 0; + out_2693152018895701851[45] = 0; + out_2693152018895701851[46] = 0; + out_2693152018895701851[47] = 0; + out_2693152018895701851[48] = 0; + out_2693152018895701851[49] = 0; + out_2693152018895701851[50] = 0; + out_2693152018895701851[51] = 0; + out_2693152018895701851[52] = 0; + out_2693152018895701851[53] = 0; } -void h_14(double *state, double *unused, double *out_278507277292912803) { - out_278507277292912803[0] = state[6]; - out_278507277292912803[1] = state[7]; - out_278507277292912803[2] = state[8]; +void h_14(double *state, double *unused, double *out_2314638522999502046) { + out_2314638522999502046[0] = state[6]; + out_2314638522999502046[1] = state[7]; + out_2314638522999502046[2] = state[8]; } -void H_14(double *state, double *unused, double *out_7037065641595711849) { - out_7037065641595711849[0] = 0; - out_7037065641595711849[1] = 0; - out_7037065641595711849[2] = 0; - out_7037065641595711849[3] = 0; - out_7037065641595711849[4] = 0; - out_7037065641595711849[5] = 0; - out_7037065641595711849[6] = 1; - out_7037065641595711849[7] = 0; - out_7037065641595711849[8] = 0; - out_7037065641595711849[9] = 0; - out_7037065641595711849[10] = 0; - out_7037065641595711849[11] = 0; - out_7037065641595711849[12] = 0; - out_7037065641595711849[13] = 0; - out_7037065641595711849[14] = 0; - out_7037065641595711849[15] = 0; - out_7037065641595711849[16] = 0; - out_7037065641595711849[17] = 0; - out_7037065641595711849[18] = 0; - out_7037065641595711849[19] = 0; - out_7037065641595711849[20] = 0; - out_7037065641595711849[21] = 0; - out_7037065641595711849[22] = 0; - out_7037065641595711849[23] = 0; - out_7037065641595711849[24] = 0; - out_7037065641595711849[25] = 1; - out_7037065641595711849[26] = 0; - out_7037065641595711849[27] = 0; - out_7037065641595711849[28] = 0; - out_7037065641595711849[29] = 0; - out_7037065641595711849[30] = 0; - out_7037065641595711849[31] = 0; - out_7037065641595711849[32] = 0; - out_7037065641595711849[33] = 0; - out_7037065641595711849[34] = 0; - out_7037065641595711849[35] = 0; - out_7037065641595711849[36] = 0; - out_7037065641595711849[37] = 0; - out_7037065641595711849[38] = 0; - out_7037065641595711849[39] = 0; - out_7037065641595711849[40] = 0; - out_7037065641595711849[41] = 0; - out_7037065641595711849[42] = 0; - out_7037065641595711849[43] = 0; - out_7037065641595711849[44] = 1; - out_7037065641595711849[45] = 0; - out_7037065641595711849[46] = 0; - out_7037065641595711849[47] = 0; - out_7037065641595711849[48] = 0; - out_7037065641595711849[49] = 0; - out_7037065641595711849[50] = 0; - out_7037065641595711849[51] = 0; - out_7037065641595711849[52] = 0; - out_7037065641595711849[53] = 0; +void H_14(double *state, double *unused, double *out_6340542370872918251) { + out_6340542370872918251[0] = 0; + out_6340542370872918251[1] = 0; + out_6340542370872918251[2] = 0; + out_6340542370872918251[3] = 0; + out_6340542370872918251[4] = 0; + out_6340542370872918251[5] = 0; + out_6340542370872918251[6] = 1; + out_6340542370872918251[7] = 0; + out_6340542370872918251[8] = 0; + out_6340542370872918251[9] = 0; + out_6340542370872918251[10] = 0; + out_6340542370872918251[11] = 0; + out_6340542370872918251[12] = 0; + out_6340542370872918251[13] = 0; + out_6340542370872918251[14] = 0; + out_6340542370872918251[15] = 0; + out_6340542370872918251[16] = 0; + out_6340542370872918251[17] = 0; + out_6340542370872918251[18] = 0; + out_6340542370872918251[19] = 0; + out_6340542370872918251[20] = 0; + out_6340542370872918251[21] = 0; + out_6340542370872918251[22] = 0; + out_6340542370872918251[23] = 0; + out_6340542370872918251[24] = 0; + out_6340542370872918251[25] = 1; + out_6340542370872918251[26] = 0; + out_6340542370872918251[27] = 0; + out_6340542370872918251[28] = 0; + out_6340542370872918251[29] = 0; + out_6340542370872918251[30] = 0; + out_6340542370872918251[31] = 0; + out_6340542370872918251[32] = 0; + out_6340542370872918251[33] = 0; + out_6340542370872918251[34] = 0; + out_6340542370872918251[35] = 0; + out_6340542370872918251[36] = 0; + out_6340542370872918251[37] = 0; + out_6340542370872918251[38] = 0; + out_6340542370872918251[39] = 0; + out_6340542370872918251[40] = 0; + out_6340542370872918251[41] = 0; + out_6340542370872918251[42] = 0; + out_6340542370872918251[43] = 0; + out_6340542370872918251[44] = 1; + out_6340542370872918251[45] = 0; + out_6340542370872918251[46] = 0; + out_6340542370872918251[47] = 0; + out_6340542370872918251[48] = 0; + out_6340542370872918251[49] = 0; + out_6340542370872918251[50] = 0; + out_6340542370872918251[51] = 0; + out_6340542370872918251[52] = 0; + out_6340542370872918251[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_1942165964233553274) { - err_fun(nom_x, delta_x, out_1942165964233553274); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6891956137394554809) { + err_fun(nom_x, delta_x, out_6891956137394554809); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2238116153961167505) { - inv_err_fun(nom_x, true_x, out_2238116153961167505); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_352074767866048570) { + inv_err_fun(nom_x, true_x, out_352074767866048570); } -void pose_H_mod_fun(double *state, double *out_4990084456757481789) { - H_mod_fun(state, out_4990084456757481789); +void pose_H_mod_fun(double *state, double *out_57344037476075931) { + H_mod_fun(state, out_57344037476075931); } -void pose_f_fun(double *state, double dt, double *out_6296202895308932040) { - f_fun(state, dt, out_6296202895308932040); +void pose_f_fun(double *state, double dt, double *out_8328845329683204295) { + f_fun(state, dt, out_8328845329683204295); } -void pose_F_fun(double *state, double dt, double *out_909355394007943716) { - F_fun(state, dt, out_909355394007943716); +void pose_F_fun(double *state, double dt, double *out_6465495001911852221) { + F_fun(state, dt, out_6465495001911852221); } -void pose_h_4(double *state, double *unused, double *out_6818977063477642683) { - h_4(state, unused, out_6818977063477642683); +void pose_h_4(double *state, double *unused, double *out_7442631847161232120) { + h_4(state, unused, out_7442631847161232120); } -void pose_H_4(double *state, double *unused, double *out_3954277209300339553) { - H_4(state, unused, out_3954277209300339553); +void pose_H_4(double *state, double *unused, double *out_8142960846497148836) { + H_4(state, unused, out_8142960846497148836); } -void pose_h_10(double *state, double *unused, double *out_3834013708689822319) { - h_10(state, unused, out_3834013708689822319); +void pose_h_10(double *state, double *unused, double *out_6865183365695080269) { + h_10(state, unused, out_6865183365695080269); } -void pose_H_10(double *state, double *unused, double *out_3533519720904001040) { - H_10(state, unused, out_3533519720904001040); +void pose_H_10(double *state, double *unused, double *out_456103805919410821) { + H_10(state, unused, out_456103805919410821); } -void pose_h_13(double *state, double *unused, double *out_3025354993842492520) { - h_13(state, unused, out_3025354993842492520); +void pose_h_13(double *state, double *unused, double *out_2130091715364414233) { + h_13(state, unused, out_2130091715364414233); } -void pose_H_13(double *state, double *unused, double *out_3389675289618495449) { - H_13(state, unused, out_3389675289618495449); +void pose_H_13(double *state, double *unused, double *out_2693152018895701851) { + H_13(state, unused, out_2693152018895701851); } -void pose_h_14(double *state, double *unused, double *out_278507277292912803) { - h_14(state, unused, out_278507277292912803); +void pose_h_14(double *state, double *unused, double *out_2314638522999502046) { + h_14(state, unused, out_2314638522999502046); } -void pose_H_14(double *state, double *unused, double *out_7037065641595711849) { - H_14(state, unused, out_7037065641595711849); +void pose_H_14(double *state, double *unused, double *out_6340542370872918251) { + H_14(state, unused, out_6340542370872918251); } 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 eff15490e0..cd6119db30 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_1942165964233553274); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2238116153961167505); -void pose_H_mod_fun(double *state, double *out_4990084456757481789); -void pose_f_fun(double *state, double dt, double *out_6296202895308932040); -void pose_F_fun(double *state, double dt, double *out_909355394007943716); -void pose_h_4(double *state, double *unused, double *out_6818977063477642683); -void pose_H_4(double *state, double *unused, double *out_3954277209300339553); -void pose_h_10(double *state, double *unused, double *out_3834013708689822319); -void pose_H_10(double *state, double *unused, double *out_3533519720904001040); -void pose_h_13(double *state, double *unused, double *out_3025354993842492520); -void pose_H_13(double *state, double *unused, double *out_3389675289618495449); -void pose_h_14(double *state, double *unused, double *out_278507277292912803); -void pose_H_14(double *state, double *unused, double *out_7037065641595711849); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6891956137394554809); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_352074767866048570); +void pose_H_mod_fun(double *state, double *out_57344037476075931); +void pose_f_fun(double *state, double dt, double *out_8328845329683204295); +void pose_F_fun(double *state, double dt, double *out_6465495001911852221); +void pose_h_4(double *state, double *unused, double *out_7442631847161232120); +void pose_H_4(double *state, double *unused, double *out_8142960846497148836); +void pose_h_10(double *state, double *unused, double *out_6865183365695080269); +void pose_H_10(double *state, double *unused, double *out_456103805919410821); +void pose_h_13(double *state, double *unused, double *out_2130091715364414233); +void pose_H_13(double *state, double *unused, double *out_2693152018895701851); +void pose_h_14(double *state, double *unused, double *out_2314638522999502046); +void pose_H_14(double *state, double *unused, double *out_6340542370872918251); 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 32cc042178..d25bb5325c 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 117b9fbe9c..f497e7e3f6 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index a19ff0791e..435247293d 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index f4c10e3f2e..09f431ed03 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 794e7077ef..401f7d44ea 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 1b818473fa..da108f5b46 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ