diff --git a/cereal/libcereal.a b/cereal/libcereal.a index 5942af15e..cdc82fddd 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index fcac95e54..6391064d1 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 866ed3e20..57221136a 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 8326f2976..eadcc7fad 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 91694fe9b..2b25addc6 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 c5beaecac..8c82fd2f9 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 a4a95e3d1..ec7900e8b 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 affb4386f..90fbbd70b 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 affb4386f..90fbbd70b 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 863544f32..b8d1e507a 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 a4a95e3d1..ec7900e8b 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 5bfbc23d2..983019c9c 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-5e0a1bbe-DEBUG"; +const uint8_t gitversion[19] = "DEV-245a5eaa-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 2252cfe04..94787f1fe 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 bcb57c060..8b142ca9a 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 3139211e6..6e205a8d3 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 8398d0876..a32ed7dba 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 bb047d8cf..66be9298e 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 e373dab7e..72a5c4270 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 36b1dfa8e..8b4eec0ae 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 665588992..956b11343 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 7886b8194..2419da758 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 d7afaa17b..1e0529a76 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 236b455b2..a4b2f6de0 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 f3e6a8244..5a391a895 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 d47652dc8..ff42fcb39 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 59e640745..5b5f61317 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 66484651a..bd8a1a054 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 a5c41f18d..de4c775df 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 25b209cba..0a307f7af 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 f97773645..f41542c4a 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 ef33bc52a..af8775a05 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 ad88a973e..2222fdbad 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 ab8e74529..5dd5d42e7 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-5e0a1bbe-DEBUG \ No newline at end of file +DEV-245a5eaa-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 832bd8015..584433d8b 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_1038389500972116158) { - out_1038389500972116158[0] = delta_x[0] + nom_x[0]; - out_1038389500972116158[1] = delta_x[1] + nom_x[1]; - out_1038389500972116158[2] = delta_x[2] + nom_x[2]; - out_1038389500972116158[3] = delta_x[3] + nom_x[3]; - out_1038389500972116158[4] = delta_x[4] + nom_x[4]; - out_1038389500972116158[5] = delta_x[5] + nom_x[5]; - out_1038389500972116158[6] = delta_x[6] + nom_x[6]; - out_1038389500972116158[7] = delta_x[7] + nom_x[7]; - out_1038389500972116158[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_7715085255157723284) { + out_7715085255157723284[0] = delta_x[0] + nom_x[0]; + out_7715085255157723284[1] = delta_x[1] + nom_x[1]; + out_7715085255157723284[2] = delta_x[2] + nom_x[2]; + out_7715085255157723284[3] = delta_x[3] + nom_x[3]; + out_7715085255157723284[4] = delta_x[4] + nom_x[4]; + out_7715085255157723284[5] = delta_x[5] + nom_x[5]; + out_7715085255157723284[6] = delta_x[6] + nom_x[6]; + out_7715085255157723284[7] = delta_x[7] + nom_x[7]; + out_7715085255157723284[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_6418752408535262595) { - out_6418752408535262595[0] = -nom_x[0] + true_x[0]; - out_6418752408535262595[1] = -nom_x[1] + true_x[1]; - out_6418752408535262595[2] = -nom_x[2] + true_x[2]; - out_6418752408535262595[3] = -nom_x[3] + true_x[3]; - out_6418752408535262595[4] = -nom_x[4] + true_x[4]; - out_6418752408535262595[5] = -nom_x[5] + true_x[5]; - out_6418752408535262595[6] = -nom_x[6] + true_x[6]; - out_6418752408535262595[7] = -nom_x[7] + true_x[7]; - out_6418752408535262595[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3139976914443091875) { + out_3139976914443091875[0] = -nom_x[0] + true_x[0]; + out_3139976914443091875[1] = -nom_x[1] + true_x[1]; + out_3139976914443091875[2] = -nom_x[2] + true_x[2]; + out_3139976914443091875[3] = -nom_x[3] + true_x[3]; + out_3139976914443091875[4] = -nom_x[4] + true_x[4]; + out_3139976914443091875[5] = -nom_x[5] + true_x[5]; + out_3139976914443091875[6] = -nom_x[6] + true_x[6]; + out_3139976914443091875[7] = -nom_x[7] + true_x[7]; + out_3139976914443091875[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_2855368609596965229) { - out_2855368609596965229[0] = 1.0; - out_2855368609596965229[1] = 0.0; - out_2855368609596965229[2] = 0.0; - out_2855368609596965229[3] = 0.0; - out_2855368609596965229[4] = 0.0; - out_2855368609596965229[5] = 0.0; - out_2855368609596965229[6] = 0.0; - out_2855368609596965229[7] = 0.0; - out_2855368609596965229[8] = 0.0; - out_2855368609596965229[9] = 0.0; - out_2855368609596965229[10] = 1.0; - out_2855368609596965229[11] = 0.0; - out_2855368609596965229[12] = 0.0; - out_2855368609596965229[13] = 0.0; - out_2855368609596965229[14] = 0.0; - out_2855368609596965229[15] = 0.0; - out_2855368609596965229[16] = 0.0; - out_2855368609596965229[17] = 0.0; - out_2855368609596965229[18] = 0.0; - out_2855368609596965229[19] = 0.0; - out_2855368609596965229[20] = 1.0; - out_2855368609596965229[21] = 0.0; - out_2855368609596965229[22] = 0.0; - out_2855368609596965229[23] = 0.0; - out_2855368609596965229[24] = 0.0; - out_2855368609596965229[25] = 0.0; - out_2855368609596965229[26] = 0.0; - out_2855368609596965229[27] = 0.0; - out_2855368609596965229[28] = 0.0; - out_2855368609596965229[29] = 0.0; - out_2855368609596965229[30] = 1.0; - out_2855368609596965229[31] = 0.0; - out_2855368609596965229[32] = 0.0; - out_2855368609596965229[33] = 0.0; - out_2855368609596965229[34] = 0.0; - out_2855368609596965229[35] = 0.0; - out_2855368609596965229[36] = 0.0; - out_2855368609596965229[37] = 0.0; - out_2855368609596965229[38] = 0.0; - out_2855368609596965229[39] = 0.0; - out_2855368609596965229[40] = 1.0; - out_2855368609596965229[41] = 0.0; - out_2855368609596965229[42] = 0.0; - out_2855368609596965229[43] = 0.0; - out_2855368609596965229[44] = 0.0; - out_2855368609596965229[45] = 0.0; - out_2855368609596965229[46] = 0.0; - out_2855368609596965229[47] = 0.0; - out_2855368609596965229[48] = 0.0; - out_2855368609596965229[49] = 0.0; - out_2855368609596965229[50] = 1.0; - out_2855368609596965229[51] = 0.0; - out_2855368609596965229[52] = 0.0; - out_2855368609596965229[53] = 0.0; - out_2855368609596965229[54] = 0.0; - out_2855368609596965229[55] = 0.0; - out_2855368609596965229[56] = 0.0; - out_2855368609596965229[57] = 0.0; - out_2855368609596965229[58] = 0.0; - out_2855368609596965229[59] = 0.0; - out_2855368609596965229[60] = 1.0; - out_2855368609596965229[61] = 0.0; - out_2855368609596965229[62] = 0.0; - out_2855368609596965229[63] = 0.0; - out_2855368609596965229[64] = 0.0; - out_2855368609596965229[65] = 0.0; - out_2855368609596965229[66] = 0.0; - out_2855368609596965229[67] = 0.0; - out_2855368609596965229[68] = 0.0; - out_2855368609596965229[69] = 0.0; - out_2855368609596965229[70] = 1.0; - out_2855368609596965229[71] = 0.0; - out_2855368609596965229[72] = 0.0; - out_2855368609596965229[73] = 0.0; - out_2855368609596965229[74] = 0.0; - out_2855368609596965229[75] = 0.0; - out_2855368609596965229[76] = 0.0; - out_2855368609596965229[77] = 0.0; - out_2855368609596965229[78] = 0.0; - out_2855368609596965229[79] = 0.0; - out_2855368609596965229[80] = 1.0; +void H_mod_fun(double *state, double *out_3564524246730987791) { + out_3564524246730987791[0] = 1.0; + out_3564524246730987791[1] = 0.0; + out_3564524246730987791[2] = 0.0; + out_3564524246730987791[3] = 0.0; + out_3564524246730987791[4] = 0.0; + out_3564524246730987791[5] = 0.0; + out_3564524246730987791[6] = 0.0; + out_3564524246730987791[7] = 0.0; + out_3564524246730987791[8] = 0.0; + out_3564524246730987791[9] = 0.0; + out_3564524246730987791[10] = 1.0; + out_3564524246730987791[11] = 0.0; + out_3564524246730987791[12] = 0.0; + out_3564524246730987791[13] = 0.0; + out_3564524246730987791[14] = 0.0; + out_3564524246730987791[15] = 0.0; + out_3564524246730987791[16] = 0.0; + out_3564524246730987791[17] = 0.0; + out_3564524246730987791[18] = 0.0; + out_3564524246730987791[19] = 0.0; + out_3564524246730987791[20] = 1.0; + out_3564524246730987791[21] = 0.0; + out_3564524246730987791[22] = 0.0; + out_3564524246730987791[23] = 0.0; + out_3564524246730987791[24] = 0.0; + out_3564524246730987791[25] = 0.0; + out_3564524246730987791[26] = 0.0; + out_3564524246730987791[27] = 0.0; + out_3564524246730987791[28] = 0.0; + out_3564524246730987791[29] = 0.0; + out_3564524246730987791[30] = 1.0; + out_3564524246730987791[31] = 0.0; + out_3564524246730987791[32] = 0.0; + out_3564524246730987791[33] = 0.0; + out_3564524246730987791[34] = 0.0; + out_3564524246730987791[35] = 0.0; + out_3564524246730987791[36] = 0.0; + out_3564524246730987791[37] = 0.0; + out_3564524246730987791[38] = 0.0; + out_3564524246730987791[39] = 0.0; + out_3564524246730987791[40] = 1.0; + out_3564524246730987791[41] = 0.0; + out_3564524246730987791[42] = 0.0; + out_3564524246730987791[43] = 0.0; + out_3564524246730987791[44] = 0.0; + out_3564524246730987791[45] = 0.0; + out_3564524246730987791[46] = 0.0; + out_3564524246730987791[47] = 0.0; + out_3564524246730987791[48] = 0.0; + out_3564524246730987791[49] = 0.0; + out_3564524246730987791[50] = 1.0; + out_3564524246730987791[51] = 0.0; + out_3564524246730987791[52] = 0.0; + out_3564524246730987791[53] = 0.0; + out_3564524246730987791[54] = 0.0; + out_3564524246730987791[55] = 0.0; + out_3564524246730987791[56] = 0.0; + out_3564524246730987791[57] = 0.0; + out_3564524246730987791[58] = 0.0; + out_3564524246730987791[59] = 0.0; + out_3564524246730987791[60] = 1.0; + out_3564524246730987791[61] = 0.0; + out_3564524246730987791[62] = 0.0; + out_3564524246730987791[63] = 0.0; + out_3564524246730987791[64] = 0.0; + out_3564524246730987791[65] = 0.0; + out_3564524246730987791[66] = 0.0; + out_3564524246730987791[67] = 0.0; + out_3564524246730987791[68] = 0.0; + out_3564524246730987791[69] = 0.0; + out_3564524246730987791[70] = 1.0; + out_3564524246730987791[71] = 0.0; + out_3564524246730987791[72] = 0.0; + out_3564524246730987791[73] = 0.0; + out_3564524246730987791[74] = 0.0; + out_3564524246730987791[75] = 0.0; + out_3564524246730987791[76] = 0.0; + out_3564524246730987791[77] = 0.0; + out_3564524246730987791[78] = 0.0; + out_3564524246730987791[79] = 0.0; + out_3564524246730987791[80] = 1.0; } -void f_fun(double *state, double dt, double *out_1397092929687677159) { - out_1397092929687677159[0] = state[0]; - out_1397092929687677159[1] = state[1]; - out_1397092929687677159[2] = state[2]; - out_1397092929687677159[3] = state[3]; - out_1397092929687677159[4] = state[4]; - out_1397092929687677159[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_1397092929687677159[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_1397092929687677159[7] = state[7]; - out_1397092929687677159[8] = state[8]; +void f_fun(double *state, double dt, double *out_9199042254253526234) { + out_9199042254253526234[0] = state[0]; + out_9199042254253526234[1] = state[1]; + out_9199042254253526234[2] = state[2]; + out_9199042254253526234[3] = state[3]; + out_9199042254253526234[4] = state[4]; + out_9199042254253526234[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_9199042254253526234[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_9199042254253526234[7] = state[7]; + out_9199042254253526234[8] = state[8]; } -void F_fun(double *state, double dt, double *out_7780451684963738443) { - out_7780451684963738443[0] = 1; - out_7780451684963738443[1] = 0; - out_7780451684963738443[2] = 0; - out_7780451684963738443[3] = 0; - out_7780451684963738443[4] = 0; - out_7780451684963738443[5] = 0; - out_7780451684963738443[6] = 0; - out_7780451684963738443[7] = 0; - out_7780451684963738443[8] = 0; - out_7780451684963738443[9] = 0; - out_7780451684963738443[10] = 1; - out_7780451684963738443[11] = 0; - out_7780451684963738443[12] = 0; - out_7780451684963738443[13] = 0; - out_7780451684963738443[14] = 0; - out_7780451684963738443[15] = 0; - out_7780451684963738443[16] = 0; - out_7780451684963738443[17] = 0; - out_7780451684963738443[18] = 0; - out_7780451684963738443[19] = 0; - out_7780451684963738443[20] = 1; - out_7780451684963738443[21] = 0; - out_7780451684963738443[22] = 0; - out_7780451684963738443[23] = 0; - out_7780451684963738443[24] = 0; - out_7780451684963738443[25] = 0; - out_7780451684963738443[26] = 0; - out_7780451684963738443[27] = 0; - out_7780451684963738443[28] = 0; - out_7780451684963738443[29] = 0; - out_7780451684963738443[30] = 1; - out_7780451684963738443[31] = 0; - out_7780451684963738443[32] = 0; - out_7780451684963738443[33] = 0; - out_7780451684963738443[34] = 0; - out_7780451684963738443[35] = 0; - out_7780451684963738443[36] = 0; - out_7780451684963738443[37] = 0; - out_7780451684963738443[38] = 0; - out_7780451684963738443[39] = 0; - out_7780451684963738443[40] = 1; - out_7780451684963738443[41] = 0; - out_7780451684963738443[42] = 0; - out_7780451684963738443[43] = 0; - out_7780451684963738443[44] = 0; - out_7780451684963738443[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_7780451684963738443[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_7780451684963738443[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_7780451684963738443[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_7780451684963738443[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_7780451684963738443[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_7780451684963738443[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_7780451684963738443[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_7780451684963738443[53] = -9.8100000000000005*dt; - out_7780451684963738443[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_7780451684963738443[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_7780451684963738443[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7780451684963738443[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7780451684963738443[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_7780451684963738443[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_7780451684963738443[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_7780451684963738443[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7780451684963738443[62] = 0; - out_7780451684963738443[63] = 0; - out_7780451684963738443[64] = 0; - out_7780451684963738443[65] = 0; - out_7780451684963738443[66] = 0; - out_7780451684963738443[67] = 0; - out_7780451684963738443[68] = 0; - out_7780451684963738443[69] = 0; - out_7780451684963738443[70] = 1; - out_7780451684963738443[71] = 0; - out_7780451684963738443[72] = 0; - out_7780451684963738443[73] = 0; - out_7780451684963738443[74] = 0; - out_7780451684963738443[75] = 0; - out_7780451684963738443[76] = 0; - out_7780451684963738443[77] = 0; - out_7780451684963738443[78] = 0; - out_7780451684963738443[79] = 0; - out_7780451684963738443[80] = 1; +void F_fun(double *state, double dt, double *out_606899119426091544) { + out_606899119426091544[0] = 1; + out_606899119426091544[1] = 0; + out_606899119426091544[2] = 0; + out_606899119426091544[3] = 0; + out_606899119426091544[4] = 0; + out_606899119426091544[5] = 0; + out_606899119426091544[6] = 0; + out_606899119426091544[7] = 0; + out_606899119426091544[8] = 0; + out_606899119426091544[9] = 0; + out_606899119426091544[10] = 1; + out_606899119426091544[11] = 0; + out_606899119426091544[12] = 0; + out_606899119426091544[13] = 0; + out_606899119426091544[14] = 0; + out_606899119426091544[15] = 0; + out_606899119426091544[16] = 0; + out_606899119426091544[17] = 0; + out_606899119426091544[18] = 0; + out_606899119426091544[19] = 0; + out_606899119426091544[20] = 1; + out_606899119426091544[21] = 0; + out_606899119426091544[22] = 0; + out_606899119426091544[23] = 0; + out_606899119426091544[24] = 0; + out_606899119426091544[25] = 0; + out_606899119426091544[26] = 0; + out_606899119426091544[27] = 0; + out_606899119426091544[28] = 0; + out_606899119426091544[29] = 0; + out_606899119426091544[30] = 1; + out_606899119426091544[31] = 0; + out_606899119426091544[32] = 0; + out_606899119426091544[33] = 0; + out_606899119426091544[34] = 0; + out_606899119426091544[35] = 0; + out_606899119426091544[36] = 0; + out_606899119426091544[37] = 0; + out_606899119426091544[38] = 0; + out_606899119426091544[39] = 0; + out_606899119426091544[40] = 1; + out_606899119426091544[41] = 0; + out_606899119426091544[42] = 0; + out_606899119426091544[43] = 0; + out_606899119426091544[44] = 0; + out_606899119426091544[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_606899119426091544[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_606899119426091544[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_606899119426091544[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_606899119426091544[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_606899119426091544[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_606899119426091544[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_606899119426091544[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_606899119426091544[53] = -9.8100000000000005*dt; + out_606899119426091544[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_606899119426091544[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_606899119426091544[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_606899119426091544[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_606899119426091544[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_606899119426091544[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_606899119426091544[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_606899119426091544[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_606899119426091544[62] = 0; + out_606899119426091544[63] = 0; + out_606899119426091544[64] = 0; + out_606899119426091544[65] = 0; + out_606899119426091544[66] = 0; + out_606899119426091544[67] = 0; + out_606899119426091544[68] = 0; + out_606899119426091544[69] = 0; + out_606899119426091544[70] = 1; + out_606899119426091544[71] = 0; + out_606899119426091544[72] = 0; + out_606899119426091544[73] = 0; + out_606899119426091544[74] = 0; + out_606899119426091544[75] = 0; + out_606899119426091544[76] = 0; + out_606899119426091544[77] = 0; + out_606899119426091544[78] = 0; + out_606899119426091544[79] = 0; + out_606899119426091544[80] = 1; } -void h_25(double *state, double *unused, double *out_894848580994361563) { - out_894848580994361563[0] = state[6]; +void h_25(double *state, double *unused, double *out_2249341248542141975) { + out_2249341248542141975[0] = state[6]; } -void H_25(double *state, double *unused, double *out_6378349259586456906) { - out_6378349259586456906[0] = 0; - out_6378349259586456906[1] = 0; - out_6378349259586456906[2] = 0; - out_6378349259586456906[3] = 0; - out_6378349259586456906[4] = 0; - out_6378349259586456906[5] = 0; - out_6378349259586456906[6] = 1; - out_6378349259586456906[7] = 0; - out_6378349259586456906[8] = 0; +void H_25(double *state, double *unused, double *out_7498705153458594920) { + out_7498705153458594920[0] = 0; + out_7498705153458594920[1] = 0; + out_7498705153458594920[2] = 0; + out_7498705153458594920[3] = 0; + out_7498705153458594920[4] = 0; + out_7498705153458594920[5] = 0; + out_7498705153458594920[6] = 1; + out_7498705153458594920[7] = 0; + out_7498705153458594920[8] = 0; } -void h_24(double *state, double *unused, double *out_6711296683837743925) { - out_6711296683837743925[0] = state[4]; - out_6711296683837743925[1] = state[5]; +void h_24(double *state, double *unused, double *out_5042487363538811890) { + out_5042487363538811890[0] = state[4]; + out_5042487363538811890[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5414189356014908953) { - out_5414189356014908953[0] = 0; - out_5414189356014908953[1] = 0; - out_5414189356014908953[2] = 0; - out_5414189356014908953[3] = 0; - out_5414189356014908953[4] = 1; - out_5414189356014908953[5] = 0; - out_5414189356014908953[6] = 0; - out_5414189356014908953[7] = 0; - out_5414189356014908953[8] = 0; - out_5414189356014908953[9] = 0; - out_5414189356014908953[10] = 0; - out_5414189356014908953[11] = 0; - out_5414189356014908953[12] = 0; - out_5414189356014908953[13] = 0; - out_5414189356014908953[14] = 1; - out_5414189356014908953[15] = 0; - out_5414189356014908953[16] = 0; - out_5414189356014908953[17] = 0; +void H_24(double *state, double *unused, double *out_3529647286574029803) { + out_3529647286574029803[0] = 0; + out_3529647286574029803[1] = 0; + out_3529647286574029803[2] = 0; + out_3529647286574029803[3] = 0; + out_3529647286574029803[4] = 1; + out_3529647286574029803[5] = 0; + out_3529647286574029803[6] = 0; + out_3529647286574029803[7] = 0; + out_3529647286574029803[8] = 0; + out_3529647286574029803[9] = 0; + out_3529647286574029803[10] = 0; + out_3529647286574029803[11] = 0; + out_3529647286574029803[12] = 0; + out_3529647286574029803[13] = 0; + out_3529647286574029803[14] = 1; + out_3529647286574029803[15] = 0; + out_3529647286574029803[16] = 0; + out_3529647286574029803[17] = 0; } -void h_30(double *state, double *unused, double *out_619654518709855674) { - out_619654518709855674[0] = state[4]; +void h_30(double *state, double *unused, double *out_1974147186257636086) { + out_1974147186257636086[0] = state[4]; } -void H_30(double *state, double *unused, double *out_6249010312443216836) { - out_6249010312443216836[0] = 0; - out_6249010312443216836[1] = 0; - out_6249010312443216836[2] = 0; - out_6249010312443216836[3] = 0; - out_6249010312443216836[4] = 1; - out_6249010312443216836[5] = 0; - out_6249010312443216836[6] = 0; - out_6249010312443216836[7] = 0; - out_6249010312443216836[8] = 0; +void H_30(double *state, double *unused, double *out_4031348578759339941) { + out_4031348578759339941[0] = 0; + out_4031348578759339941[1] = 0; + out_4031348578759339941[2] = 0; + out_4031348578759339941[3] = 0; + out_4031348578759339941[4] = 1; + out_4031348578759339941[5] = 0; + out_4031348578759339941[6] = 0; + out_4031348578759339941[7] = 0; + out_4031348578759339941[8] = 0; } -void h_26(double *state, double *unused, double *out_6497940006268570762) { - out_6497940006268570762[0] = state[7]; +void h_26(double *state, double *unused, double *out_7452053475011439088) { + out_7452053475011439088[0] = state[7]; } -void H_26(double *state, double *unused, double *out_2636845940712400682) { - out_2636845940712400682[0] = 0; - out_2636845940712400682[1] = 0; - out_2636845940712400682[2] = 0; - out_2636845940712400682[3] = 0; - out_2636845940712400682[4] = 0; - out_2636845940712400682[5] = 0; - out_2636845940712400682[6] = 0; - out_2636845940712400682[7] = 1; - out_2636845940712400682[8] = 0; +void H_26(double *state, double *unused, double *out_3757201834584538696) { + out_3757201834584538696[0] = 0; + out_3757201834584538696[1] = 0; + out_3757201834584538696[2] = 0; + out_3757201834584538696[3] = 0; + out_3757201834584538696[4] = 0; + out_3757201834584538696[5] = 0; + out_3757201834584538696[6] = 0; + out_3757201834584538696[7] = 1; + out_3757201834584538696[8] = 0; } -void h_27(double *state, double *unused, double *out_7293890098408210709) { - out_7293890098408210709[0] = state[3]; +void h_27(double *state, double *unused, double *out_8648382765955991121) { + out_8648382765955991121[0] = state[3]; } -void H_27(double *state, double *unused, double *out_4074247000642791925) { - out_4074247000642791925[0] = 0; - out_4074247000642791925[1] = 0; - out_4074247000642791925[2] = 0; - out_4074247000642791925[3] = 1; - out_4074247000642791925[4] = 0; - out_4074247000642791925[5] = 0; - out_4074247000642791925[6] = 0; - out_4074247000642791925[7] = 0; - out_4074247000642791925[8] = 0; +void H_27(double *state, double *unused, double *out_6206111890559764852) { + out_6206111890559764852[0] = 0; + out_6206111890559764852[1] = 0; + out_6206111890559764852[2] = 0; + out_6206111890559764852[3] = 1; + out_6206111890559764852[4] = 0; + out_6206111890559764852[5] = 0; + out_6206111890559764852[6] = 0; + out_6206111890559764852[7] = 0; + out_6206111890559764852[8] = 0; } -void h_29(double *state, double *unused, double *out_3432206621979709057) { - out_3432206621979709057[0] = state[1]; +void h_29(double *state, double *unused, double *out_3027526081403209559) { + out_3027526081403209559[0] = state[1]; } -void H_29(double *state, double *unused, double *out_2360884273773240892) { - out_2360884273773240892[0] = 0; - out_2360884273773240892[1] = 1; - out_2360884273773240892[2] = 0; - out_2360884273773240892[3] = 0; - out_2360884273773240892[4] = 0; - out_2360884273773240892[5] = 0; - out_2360884273773240892[6] = 0; - out_2360884273773240892[7] = 0; - out_2360884273773240892[8] = 0; +void H_29(double *state, double *unused, double *out_7919474617429315885) { + out_7919474617429315885[0] = 0; + out_7919474617429315885[1] = 1; + out_7919474617429315885[2] = 0; + out_7919474617429315885[3] = 0; + out_7919474617429315885[4] = 0; + out_7919474617429315885[5] = 0; + out_7919474617429315885[6] = 0; + out_7919474617429315885[7] = 0; + out_7919474617429315885[8] = 0; } -void h_28(double *state, double *unused, double *out_5622082467174375208) { - out_5622082467174375208[0] = state[0]; +void h_28(double *state, double *unused, double *out_8854851454851898060) { + out_8854851454851898060[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2721514743296289682) { - out_2721514743296289682[0] = 1; - out_2721514743296289682[1] = 0; - out_2721514743296289682[2] = 0; - out_2721514743296289682[3] = 0; - out_2721514743296289682[4] = 0; - out_2721514743296289682[5] = 0; - out_2721514743296289682[6] = 0; - out_2721514743296289682[7] = 0; - out_2721514743296289682[8] = 0; +void H_28(double *state, double *unused, double *out_5444870439210705157) { + out_5444870439210705157[0] = 1; + out_5444870439210705157[1] = 0; + out_5444870439210705157[2] = 0; + out_5444870439210705157[3] = 0; + out_5444870439210705157[4] = 0; + out_5444870439210705157[5] = 0; + out_5444870439210705157[6] = 0; + out_5444870439210705157[7] = 0; + out_5444870439210705157[8] = 0; } -void h_31(double *state, double *unused, double *out_6998090913890161796) { - out_6998090913890161796[0] = state[8]; +void h_31(double *state, double *unused, double *out_6247319432721598017) { + out_6247319432721598017[0] = state[8]; } -void H_31(double *state, double *unused, double *out_6408995221463417334) { - out_6408995221463417334[0] = 0; - out_6408995221463417334[1] = 0; - out_6408995221463417334[2] = 0; - out_6408995221463417334[3] = 0; - out_6408995221463417334[4] = 0; - out_6408995221463417334[5] = 0; - out_6408995221463417334[6] = 0; - out_6408995221463417334[7] = 0; - out_6408995221463417334[8] = 1; +void H_31(double *state, double *unused, double *out_7529351115335555348) { + out_7529351115335555348[0] = 0; + out_7529351115335555348[1] = 0; + out_7529351115335555348[2] = 0; + out_7529351115335555348[3] = 0; + out_7529351115335555348[4] = 0; + out_7529351115335555348[5] = 0; + out_7529351115335555348[6] = 0; + out_7529351115335555348[7] = 0; + out_7529351115335555348[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_1038389500972116158) { - err_fun(nom_x, delta_x, out_1038389500972116158); +void car_err_fun(double *nom_x, double *delta_x, double *out_7715085255157723284) { + err_fun(nom_x, delta_x, out_7715085255157723284); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_6418752408535262595) { - inv_err_fun(nom_x, true_x, out_6418752408535262595); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3139976914443091875) { + inv_err_fun(nom_x, true_x, out_3139976914443091875); } -void car_H_mod_fun(double *state, double *out_2855368609596965229) { - H_mod_fun(state, out_2855368609596965229); +void car_H_mod_fun(double *state, double *out_3564524246730987791) { + H_mod_fun(state, out_3564524246730987791); } -void car_f_fun(double *state, double dt, double *out_1397092929687677159) { - f_fun(state, dt, out_1397092929687677159); +void car_f_fun(double *state, double dt, double *out_9199042254253526234) { + f_fun(state, dt, out_9199042254253526234); } -void car_F_fun(double *state, double dt, double *out_7780451684963738443) { - F_fun(state, dt, out_7780451684963738443); +void car_F_fun(double *state, double dt, double *out_606899119426091544) { + F_fun(state, dt, out_606899119426091544); } -void car_h_25(double *state, double *unused, double *out_894848580994361563) { - h_25(state, unused, out_894848580994361563); +void car_h_25(double *state, double *unused, double *out_2249341248542141975) { + h_25(state, unused, out_2249341248542141975); } -void car_H_25(double *state, double *unused, double *out_6378349259586456906) { - H_25(state, unused, out_6378349259586456906); +void car_H_25(double *state, double *unused, double *out_7498705153458594920) { + H_25(state, unused, out_7498705153458594920); } -void car_h_24(double *state, double *unused, double *out_6711296683837743925) { - h_24(state, unused, out_6711296683837743925); +void car_h_24(double *state, double *unused, double *out_5042487363538811890) { + h_24(state, unused, out_5042487363538811890); } -void car_H_24(double *state, double *unused, double *out_5414189356014908953) { - H_24(state, unused, out_5414189356014908953); +void car_H_24(double *state, double *unused, double *out_3529647286574029803) { + H_24(state, unused, out_3529647286574029803); } -void car_h_30(double *state, double *unused, double *out_619654518709855674) { - h_30(state, unused, out_619654518709855674); +void car_h_30(double *state, double *unused, double *out_1974147186257636086) { + h_30(state, unused, out_1974147186257636086); } -void car_H_30(double *state, double *unused, double *out_6249010312443216836) { - H_30(state, unused, out_6249010312443216836); +void car_H_30(double *state, double *unused, double *out_4031348578759339941) { + H_30(state, unused, out_4031348578759339941); } -void car_h_26(double *state, double *unused, double *out_6497940006268570762) { - h_26(state, unused, out_6497940006268570762); +void car_h_26(double *state, double *unused, double *out_7452053475011439088) { + h_26(state, unused, out_7452053475011439088); } -void car_H_26(double *state, double *unused, double *out_2636845940712400682) { - H_26(state, unused, out_2636845940712400682); +void car_H_26(double *state, double *unused, double *out_3757201834584538696) { + H_26(state, unused, out_3757201834584538696); } -void car_h_27(double *state, double *unused, double *out_7293890098408210709) { - h_27(state, unused, out_7293890098408210709); +void car_h_27(double *state, double *unused, double *out_8648382765955991121) { + h_27(state, unused, out_8648382765955991121); } -void car_H_27(double *state, double *unused, double *out_4074247000642791925) { - H_27(state, unused, out_4074247000642791925); +void car_H_27(double *state, double *unused, double *out_6206111890559764852) { + H_27(state, unused, out_6206111890559764852); } -void car_h_29(double *state, double *unused, double *out_3432206621979709057) { - h_29(state, unused, out_3432206621979709057); +void car_h_29(double *state, double *unused, double *out_3027526081403209559) { + h_29(state, unused, out_3027526081403209559); } -void car_H_29(double *state, double *unused, double *out_2360884273773240892) { - H_29(state, unused, out_2360884273773240892); +void car_H_29(double *state, double *unused, double *out_7919474617429315885) { + H_29(state, unused, out_7919474617429315885); } -void car_h_28(double *state, double *unused, double *out_5622082467174375208) { - h_28(state, unused, out_5622082467174375208); +void car_h_28(double *state, double *unused, double *out_8854851454851898060) { + h_28(state, unused, out_8854851454851898060); } -void car_H_28(double *state, double *unused, double *out_2721514743296289682) { - H_28(state, unused, out_2721514743296289682); +void car_H_28(double *state, double *unused, double *out_5444870439210705157) { + H_28(state, unused, out_5444870439210705157); } -void car_h_31(double *state, double *unused, double *out_6998090913890161796) { - h_31(state, unused, out_6998090913890161796); +void car_h_31(double *state, double *unused, double *out_6247319432721598017) { + h_31(state, unused, out_6247319432721598017); } -void car_H_31(double *state, double *unused, double *out_6408995221463417334) { - H_31(state, unused, out_6408995221463417334); +void car_H_31(double *state, double *unused, double *out_7529351115335555348) { + H_31(state, unused, out_7529351115335555348); } 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 1698991d0..bfdb05839 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_1038389500972116158); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_6418752408535262595); -void car_H_mod_fun(double *state, double *out_2855368609596965229); -void car_f_fun(double *state, double dt, double *out_1397092929687677159); -void car_F_fun(double *state, double dt, double *out_7780451684963738443); -void car_h_25(double *state, double *unused, double *out_894848580994361563); -void car_H_25(double *state, double *unused, double *out_6378349259586456906); -void car_h_24(double *state, double *unused, double *out_6711296683837743925); -void car_H_24(double *state, double *unused, double *out_5414189356014908953); -void car_h_30(double *state, double *unused, double *out_619654518709855674); -void car_H_30(double *state, double *unused, double *out_6249010312443216836); -void car_h_26(double *state, double *unused, double *out_6497940006268570762); -void car_H_26(double *state, double *unused, double *out_2636845940712400682); -void car_h_27(double *state, double *unused, double *out_7293890098408210709); -void car_H_27(double *state, double *unused, double *out_4074247000642791925); -void car_h_29(double *state, double *unused, double *out_3432206621979709057); -void car_H_29(double *state, double *unused, double *out_2360884273773240892); -void car_h_28(double *state, double *unused, double *out_5622082467174375208); -void car_H_28(double *state, double *unused, double *out_2721514743296289682); -void car_h_31(double *state, double *unused, double *out_6998090913890161796); -void car_H_31(double *state, double *unused, double *out_6408995221463417334); +void car_err_fun(double *nom_x, double *delta_x, double *out_7715085255157723284); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3139976914443091875); +void car_H_mod_fun(double *state, double *out_3564524246730987791); +void car_f_fun(double *state, double dt, double *out_9199042254253526234); +void car_F_fun(double *state, double dt, double *out_606899119426091544); +void car_h_25(double *state, double *unused, double *out_2249341248542141975); +void car_H_25(double *state, double *unused, double *out_7498705153458594920); +void car_h_24(double *state, double *unused, double *out_5042487363538811890); +void car_H_24(double *state, double *unused, double *out_3529647286574029803); +void car_h_30(double *state, double *unused, double *out_1974147186257636086); +void car_H_30(double *state, double *unused, double *out_4031348578759339941); +void car_h_26(double *state, double *unused, double *out_7452053475011439088); +void car_H_26(double *state, double *unused, double *out_3757201834584538696); +void car_h_27(double *state, double *unused, double *out_8648382765955991121); +void car_H_27(double *state, double *unused, double *out_6206111890559764852); +void car_h_29(double *state, double *unused, double *out_3027526081403209559); +void car_H_29(double *state, double *unused, double *out_7919474617429315885); +void car_h_28(double *state, double *unused, double *out_8854851454851898060); +void car_H_28(double *state, double *unused, double *out_5444870439210705157); +void car_h_31(double *state, double *unused, double *out_6247319432721598017); +void car_H_31(double *state, double *unused, double *out_7529351115335555348); 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 f45ad7cde..c22d7aafd 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_6395110133292861521) { - out_6395110133292861521[0] = delta_x[0] + nom_x[0]; - out_6395110133292861521[1] = delta_x[1] + nom_x[1]; - out_6395110133292861521[2] = delta_x[2] + nom_x[2]; - out_6395110133292861521[3] = delta_x[3] + nom_x[3]; - out_6395110133292861521[4] = delta_x[4] + nom_x[4]; - out_6395110133292861521[5] = delta_x[5] + nom_x[5]; - out_6395110133292861521[6] = delta_x[6] + nom_x[6]; - out_6395110133292861521[7] = delta_x[7] + nom_x[7]; - out_6395110133292861521[8] = delta_x[8] + nom_x[8]; - out_6395110133292861521[9] = delta_x[9] + nom_x[9]; - out_6395110133292861521[10] = delta_x[10] + nom_x[10]; - out_6395110133292861521[11] = delta_x[11] + nom_x[11]; - out_6395110133292861521[12] = delta_x[12] + nom_x[12]; - out_6395110133292861521[13] = delta_x[13] + nom_x[13]; - out_6395110133292861521[14] = delta_x[14] + nom_x[14]; - out_6395110133292861521[15] = delta_x[15] + nom_x[15]; - out_6395110133292861521[16] = delta_x[16] + nom_x[16]; - out_6395110133292861521[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_1416756098750413745) { + out_1416756098750413745[0] = delta_x[0] + nom_x[0]; + out_1416756098750413745[1] = delta_x[1] + nom_x[1]; + out_1416756098750413745[2] = delta_x[2] + nom_x[2]; + out_1416756098750413745[3] = delta_x[3] + nom_x[3]; + out_1416756098750413745[4] = delta_x[4] + nom_x[4]; + out_1416756098750413745[5] = delta_x[5] + nom_x[5]; + out_1416756098750413745[6] = delta_x[6] + nom_x[6]; + out_1416756098750413745[7] = delta_x[7] + nom_x[7]; + out_1416756098750413745[8] = delta_x[8] + nom_x[8]; + out_1416756098750413745[9] = delta_x[9] + nom_x[9]; + out_1416756098750413745[10] = delta_x[10] + nom_x[10]; + out_1416756098750413745[11] = delta_x[11] + nom_x[11]; + out_1416756098750413745[12] = delta_x[12] + nom_x[12]; + out_1416756098750413745[13] = delta_x[13] + nom_x[13]; + out_1416756098750413745[14] = delta_x[14] + nom_x[14]; + out_1416756098750413745[15] = delta_x[15] + nom_x[15]; + out_1416756098750413745[16] = delta_x[16] + nom_x[16]; + out_1416756098750413745[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5574902989131587335) { - out_5574902989131587335[0] = -nom_x[0] + true_x[0]; - out_5574902989131587335[1] = -nom_x[1] + true_x[1]; - out_5574902989131587335[2] = -nom_x[2] + true_x[2]; - out_5574902989131587335[3] = -nom_x[3] + true_x[3]; - out_5574902989131587335[4] = -nom_x[4] + true_x[4]; - out_5574902989131587335[5] = -nom_x[5] + true_x[5]; - out_5574902989131587335[6] = -nom_x[6] + true_x[6]; - out_5574902989131587335[7] = -nom_x[7] + true_x[7]; - out_5574902989131587335[8] = -nom_x[8] + true_x[8]; - out_5574902989131587335[9] = -nom_x[9] + true_x[9]; - out_5574902989131587335[10] = -nom_x[10] + true_x[10]; - out_5574902989131587335[11] = -nom_x[11] + true_x[11]; - out_5574902989131587335[12] = -nom_x[12] + true_x[12]; - out_5574902989131587335[13] = -nom_x[13] + true_x[13]; - out_5574902989131587335[14] = -nom_x[14] + true_x[14]; - out_5574902989131587335[15] = -nom_x[15] + true_x[15]; - out_5574902989131587335[16] = -nom_x[16] + true_x[16]; - out_5574902989131587335[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_288660278795817354) { + out_288660278795817354[0] = -nom_x[0] + true_x[0]; + out_288660278795817354[1] = -nom_x[1] + true_x[1]; + out_288660278795817354[2] = -nom_x[2] + true_x[2]; + out_288660278795817354[3] = -nom_x[3] + true_x[3]; + out_288660278795817354[4] = -nom_x[4] + true_x[4]; + out_288660278795817354[5] = -nom_x[5] + true_x[5]; + out_288660278795817354[6] = -nom_x[6] + true_x[6]; + out_288660278795817354[7] = -nom_x[7] + true_x[7]; + out_288660278795817354[8] = -nom_x[8] + true_x[8]; + out_288660278795817354[9] = -nom_x[9] + true_x[9]; + out_288660278795817354[10] = -nom_x[10] + true_x[10]; + out_288660278795817354[11] = -nom_x[11] + true_x[11]; + out_288660278795817354[12] = -nom_x[12] + true_x[12]; + out_288660278795817354[13] = -nom_x[13] + true_x[13]; + out_288660278795817354[14] = -nom_x[14] + true_x[14]; + out_288660278795817354[15] = -nom_x[15] + true_x[15]; + out_288660278795817354[16] = -nom_x[16] + true_x[16]; + out_288660278795817354[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_6860086072787592800) { - out_6860086072787592800[0] = 1.0; - out_6860086072787592800[1] = 0.0; - out_6860086072787592800[2] = 0.0; - out_6860086072787592800[3] = 0.0; - out_6860086072787592800[4] = 0.0; - out_6860086072787592800[5] = 0.0; - out_6860086072787592800[6] = 0.0; - out_6860086072787592800[7] = 0.0; - out_6860086072787592800[8] = 0.0; - out_6860086072787592800[9] = 0.0; - out_6860086072787592800[10] = 0.0; - out_6860086072787592800[11] = 0.0; - out_6860086072787592800[12] = 0.0; - out_6860086072787592800[13] = 0.0; - out_6860086072787592800[14] = 0.0; - out_6860086072787592800[15] = 0.0; - out_6860086072787592800[16] = 0.0; - out_6860086072787592800[17] = 0.0; - out_6860086072787592800[18] = 0.0; - out_6860086072787592800[19] = 1.0; - out_6860086072787592800[20] = 0.0; - out_6860086072787592800[21] = 0.0; - out_6860086072787592800[22] = 0.0; - out_6860086072787592800[23] = 0.0; - out_6860086072787592800[24] = 0.0; - out_6860086072787592800[25] = 0.0; - out_6860086072787592800[26] = 0.0; - out_6860086072787592800[27] = 0.0; - out_6860086072787592800[28] = 0.0; - out_6860086072787592800[29] = 0.0; - out_6860086072787592800[30] = 0.0; - out_6860086072787592800[31] = 0.0; - out_6860086072787592800[32] = 0.0; - out_6860086072787592800[33] = 0.0; - out_6860086072787592800[34] = 0.0; - out_6860086072787592800[35] = 0.0; - out_6860086072787592800[36] = 0.0; - out_6860086072787592800[37] = 0.0; - out_6860086072787592800[38] = 1.0; - out_6860086072787592800[39] = 0.0; - out_6860086072787592800[40] = 0.0; - out_6860086072787592800[41] = 0.0; - out_6860086072787592800[42] = 0.0; - out_6860086072787592800[43] = 0.0; - out_6860086072787592800[44] = 0.0; - out_6860086072787592800[45] = 0.0; - out_6860086072787592800[46] = 0.0; - out_6860086072787592800[47] = 0.0; - out_6860086072787592800[48] = 0.0; - out_6860086072787592800[49] = 0.0; - out_6860086072787592800[50] = 0.0; - out_6860086072787592800[51] = 0.0; - out_6860086072787592800[52] = 0.0; - out_6860086072787592800[53] = 0.0; - out_6860086072787592800[54] = 0.0; - out_6860086072787592800[55] = 0.0; - out_6860086072787592800[56] = 0.0; - out_6860086072787592800[57] = 1.0; - out_6860086072787592800[58] = 0.0; - out_6860086072787592800[59] = 0.0; - out_6860086072787592800[60] = 0.0; - out_6860086072787592800[61] = 0.0; - out_6860086072787592800[62] = 0.0; - out_6860086072787592800[63] = 0.0; - out_6860086072787592800[64] = 0.0; - out_6860086072787592800[65] = 0.0; - out_6860086072787592800[66] = 0.0; - out_6860086072787592800[67] = 0.0; - out_6860086072787592800[68] = 0.0; - out_6860086072787592800[69] = 0.0; - out_6860086072787592800[70] = 0.0; - out_6860086072787592800[71] = 0.0; - out_6860086072787592800[72] = 0.0; - out_6860086072787592800[73] = 0.0; - out_6860086072787592800[74] = 0.0; - out_6860086072787592800[75] = 0.0; - out_6860086072787592800[76] = 1.0; - out_6860086072787592800[77] = 0.0; - out_6860086072787592800[78] = 0.0; - out_6860086072787592800[79] = 0.0; - out_6860086072787592800[80] = 0.0; - out_6860086072787592800[81] = 0.0; - out_6860086072787592800[82] = 0.0; - out_6860086072787592800[83] = 0.0; - out_6860086072787592800[84] = 0.0; - out_6860086072787592800[85] = 0.0; - out_6860086072787592800[86] = 0.0; - out_6860086072787592800[87] = 0.0; - out_6860086072787592800[88] = 0.0; - out_6860086072787592800[89] = 0.0; - out_6860086072787592800[90] = 0.0; - out_6860086072787592800[91] = 0.0; - out_6860086072787592800[92] = 0.0; - out_6860086072787592800[93] = 0.0; - out_6860086072787592800[94] = 0.0; - out_6860086072787592800[95] = 1.0; - out_6860086072787592800[96] = 0.0; - out_6860086072787592800[97] = 0.0; - out_6860086072787592800[98] = 0.0; - out_6860086072787592800[99] = 0.0; - out_6860086072787592800[100] = 0.0; - out_6860086072787592800[101] = 0.0; - out_6860086072787592800[102] = 0.0; - out_6860086072787592800[103] = 0.0; - out_6860086072787592800[104] = 0.0; - out_6860086072787592800[105] = 0.0; - out_6860086072787592800[106] = 0.0; - out_6860086072787592800[107] = 0.0; - out_6860086072787592800[108] = 0.0; - out_6860086072787592800[109] = 0.0; - out_6860086072787592800[110] = 0.0; - out_6860086072787592800[111] = 0.0; - out_6860086072787592800[112] = 0.0; - out_6860086072787592800[113] = 0.0; - out_6860086072787592800[114] = 1.0; - out_6860086072787592800[115] = 0.0; - out_6860086072787592800[116] = 0.0; - out_6860086072787592800[117] = 0.0; - out_6860086072787592800[118] = 0.0; - out_6860086072787592800[119] = 0.0; - out_6860086072787592800[120] = 0.0; - out_6860086072787592800[121] = 0.0; - out_6860086072787592800[122] = 0.0; - out_6860086072787592800[123] = 0.0; - out_6860086072787592800[124] = 0.0; - out_6860086072787592800[125] = 0.0; - out_6860086072787592800[126] = 0.0; - out_6860086072787592800[127] = 0.0; - out_6860086072787592800[128] = 0.0; - out_6860086072787592800[129] = 0.0; - out_6860086072787592800[130] = 0.0; - out_6860086072787592800[131] = 0.0; - out_6860086072787592800[132] = 0.0; - out_6860086072787592800[133] = 1.0; - out_6860086072787592800[134] = 0.0; - out_6860086072787592800[135] = 0.0; - out_6860086072787592800[136] = 0.0; - out_6860086072787592800[137] = 0.0; - out_6860086072787592800[138] = 0.0; - out_6860086072787592800[139] = 0.0; - out_6860086072787592800[140] = 0.0; - out_6860086072787592800[141] = 0.0; - out_6860086072787592800[142] = 0.0; - out_6860086072787592800[143] = 0.0; - out_6860086072787592800[144] = 0.0; - out_6860086072787592800[145] = 0.0; - out_6860086072787592800[146] = 0.0; - out_6860086072787592800[147] = 0.0; - out_6860086072787592800[148] = 0.0; - out_6860086072787592800[149] = 0.0; - out_6860086072787592800[150] = 0.0; - out_6860086072787592800[151] = 0.0; - out_6860086072787592800[152] = 1.0; - out_6860086072787592800[153] = 0.0; - out_6860086072787592800[154] = 0.0; - out_6860086072787592800[155] = 0.0; - out_6860086072787592800[156] = 0.0; - out_6860086072787592800[157] = 0.0; - out_6860086072787592800[158] = 0.0; - out_6860086072787592800[159] = 0.0; - out_6860086072787592800[160] = 0.0; - out_6860086072787592800[161] = 0.0; - out_6860086072787592800[162] = 0.0; - out_6860086072787592800[163] = 0.0; - out_6860086072787592800[164] = 0.0; - out_6860086072787592800[165] = 0.0; - out_6860086072787592800[166] = 0.0; - out_6860086072787592800[167] = 0.0; - out_6860086072787592800[168] = 0.0; - out_6860086072787592800[169] = 0.0; - out_6860086072787592800[170] = 0.0; - out_6860086072787592800[171] = 1.0; - out_6860086072787592800[172] = 0.0; - out_6860086072787592800[173] = 0.0; - out_6860086072787592800[174] = 0.0; - out_6860086072787592800[175] = 0.0; - out_6860086072787592800[176] = 0.0; - out_6860086072787592800[177] = 0.0; - out_6860086072787592800[178] = 0.0; - out_6860086072787592800[179] = 0.0; - out_6860086072787592800[180] = 0.0; - out_6860086072787592800[181] = 0.0; - out_6860086072787592800[182] = 0.0; - out_6860086072787592800[183] = 0.0; - out_6860086072787592800[184] = 0.0; - out_6860086072787592800[185] = 0.0; - out_6860086072787592800[186] = 0.0; - out_6860086072787592800[187] = 0.0; - out_6860086072787592800[188] = 0.0; - out_6860086072787592800[189] = 0.0; - out_6860086072787592800[190] = 1.0; - out_6860086072787592800[191] = 0.0; - out_6860086072787592800[192] = 0.0; - out_6860086072787592800[193] = 0.0; - out_6860086072787592800[194] = 0.0; - out_6860086072787592800[195] = 0.0; - out_6860086072787592800[196] = 0.0; - out_6860086072787592800[197] = 0.0; - out_6860086072787592800[198] = 0.0; - out_6860086072787592800[199] = 0.0; - out_6860086072787592800[200] = 0.0; - out_6860086072787592800[201] = 0.0; - out_6860086072787592800[202] = 0.0; - out_6860086072787592800[203] = 0.0; - out_6860086072787592800[204] = 0.0; - out_6860086072787592800[205] = 0.0; - out_6860086072787592800[206] = 0.0; - out_6860086072787592800[207] = 0.0; - out_6860086072787592800[208] = 0.0; - out_6860086072787592800[209] = 1.0; - out_6860086072787592800[210] = 0.0; - out_6860086072787592800[211] = 0.0; - out_6860086072787592800[212] = 0.0; - out_6860086072787592800[213] = 0.0; - out_6860086072787592800[214] = 0.0; - out_6860086072787592800[215] = 0.0; - out_6860086072787592800[216] = 0.0; - out_6860086072787592800[217] = 0.0; - out_6860086072787592800[218] = 0.0; - out_6860086072787592800[219] = 0.0; - out_6860086072787592800[220] = 0.0; - out_6860086072787592800[221] = 0.0; - out_6860086072787592800[222] = 0.0; - out_6860086072787592800[223] = 0.0; - out_6860086072787592800[224] = 0.0; - out_6860086072787592800[225] = 0.0; - out_6860086072787592800[226] = 0.0; - out_6860086072787592800[227] = 0.0; - out_6860086072787592800[228] = 1.0; - out_6860086072787592800[229] = 0.0; - out_6860086072787592800[230] = 0.0; - out_6860086072787592800[231] = 0.0; - out_6860086072787592800[232] = 0.0; - out_6860086072787592800[233] = 0.0; - out_6860086072787592800[234] = 0.0; - out_6860086072787592800[235] = 0.0; - out_6860086072787592800[236] = 0.0; - out_6860086072787592800[237] = 0.0; - out_6860086072787592800[238] = 0.0; - out_6860086072787592800[239] = 0.0; - out_6860086072787592800[240] = 0.0; - out_6860086072787592800[241] = 0.0; - out_6860086072787592800[242] = 0.0; - out_6860086072787592800[243] = 0.0; - out_6860086072787592800[244] = 0.0; - out_6860086072787592800[245] = 0.0; - out_6860086072787592800[246] = 0.0; - out_6860086072787592800[247] = 1.0; - out_6860086072787592800[248] = 0.0; - out_6860086072787592800[249] = 0.0; - out_6860086072787592800[250] = 0.0; - out_6860086072787592800[251] = 0.0; - out_6860086072787592800[252] = 0.0; - out_6860086072787592800[253] = 0.0; - out_6860086072787592800[254] = 0.0; - out_6860086072787592800[255] = 0.0; - out_6860086072787592800[256] = 0.0; - out_6860086072787592800[257] = 0.0; - out_6860086072787592800[258] = 0.0; - out_6860086072787592800[259] = 0.0; - out_6860086072787592800[260] = 0.0; - out_6860086072787592800[261] = 0.0; - out_6860086072787592800[262] = 0.0; - out_6860086072787592800[263] = 0.0; - out_6860086072787592800[264] = 0.0; - out_6860086072787592800[265] = 0.0; - out_6860086072787592800[266] = 1.0; - out_6860086072787592800[267] = 0.0; - out_6860086072787592800[268] = 0.0; - out_6860086072787592800[269] = 0.0; - out_6860086072787592800[270] = 0.0; - out_6860086072787592800[271] = 0.0; - out_6860086072787592800[272] = 0.0; - out_6860086072787592800[273] = 0.0; - out_6860086072787592800[274] = 0.0; - out_6860086072787592800[275] = 0.0; - out_6860086072787592800[276] = 0.0; - out_6860086072787592800[277] = 0.0; - out_6860086072787592800[278] = 0.0; - out_6860086072787592800[279] = 0.0; - out_6860086072787592800[280] = 0.0; - out_6860086072787592800[281] = 0.0; - out_6860086072787592800[282] = 0.0; - out_6860086072787592800[283] = 0.0; - out_6860086072787592800[284] = 0.0; - out_6860086072787592800[285] = 1.0; - out_6860086072787592800[286] = 0.0; - out_6860086072787592800[287] = 0.0; - out_6860086072787592800[288] = 0.0; - out_6860086072787592800[289] = 0.0; - out_6860086072787592800[290] = 0.0; - out_6860086072787592800[291] = 0.0; - out_6860086072787592800[292] = 0.0; - out_6860086072787592800[293] = 0.0; - out_6860086072787592800[294] = 0.0; - out_6860086072787592800[295] = 0.0; - out_6860086072787592800[296] = 0.0; - out_6860086072787592800[297] = 0.0; - out_6860086072787592800[298] = 0.0; - out_6860086072787592800[299] = 0.0; - out_6860086072787592800[300] = 0.0; - out_6860086072787592800[301] = 0.0; - out_6860086072787592800[302] = 0.0; - out_6860086072787592800[303] = 0.0; - out_6860086072787592800[304] = 1.0; - out_6860086072787592800[305] = 0.0; - out_6860086072787592800[306] = 0.0; - out_6860086072787592800[307] = 0.0; - out_6860086072787592800[308] = 0.0; - out_6860086072787592800[309] = 0.0; - out_6860086072787592800[310] = 0.0; - out_6860086072787592800[311] = 0.0; - out_6860086072787592800[312] = 0.0; - out_6860086072787592800[313] = 0.0; - out_6860086072787592800[314] = 0.0; - out_6860086072787592800[315] = 0.0; - out_6860086072787592800[316] = 0.0; - out_6860086072787592800[317] = 0.0; - out_6860086072787592800[318] = 0.0; - out_6860086072787592800[319] = 0.0; - out_6860086072787592800[320] = 0.0; - out_6860086072787592800[321] = 0.0; - out_6860086072787592800[322] = 0.0; - out_6860086072787592800[323] = 1.0; +void H_mod_fun(double *state, double *out_8072626393727229290) { + out_8072626393727229290[0] = 1.0; + out_8072626393727229290[1] = 0.0; + out_8072626393727229290[2] = 0.0; + out_8072626393727229290[3] = 0.0; + out_8072626393727229290[4] = 0.0; + out_8072626393727229290[5] = 0.0; + out_8072626393727229290[6] = 0.0; + out_8072626393727229290[7] = 0.0; + out_8072626393727229290[8] = 0.0; + out_8072626393727229290[9] = 0.0; + out_8072626393727229290[10] = 0.0; + out_8072626393727229290[11] = 0.0; + out_8072626393727229290[12] = 0.0; + out_8072626393727229290[13] = 0.0; + out_8072626393727229290[14] = 0.0; + out_8072626393727229290[15] = 0.0; + out_8072626393727229290[16] = 0.0; + out_8072626393727229290[17] = 0.0; + out_8072626393727229290[18] = 0.0; + out_8072626393727229290[19] = 1.0; + out_8072626393727229290[20] = 0.0; + out_8072626393727229290[21] = 0.0; + out_8072626393727229290[22] = 0.0; + out_8072626393727229290[23] = 0.0; + out_8072626393727229290[24] = 0.0; + out_8072626393727229290[25] = 0.0; + out_8072626393727229290[26] = 0.0; + out_8072626393727229290[27] = 0.0; + out_8072626393727229290[28] = 0.0; + out_8072626393727229290[29] = 0.0; + out_8072626393727229290[30] = 0.0; + out_8072626393727229290[31] = 0.0; + out_8072626393727229290[32] = 0.0; + out_8072626393727229290[33] = 0.0; + out_8072626393727229290[34] = 0.0; + out_8072626393727229290[35] = 0.0; + out_8072626393727229290[36] = 0.0; + out_8072626393727229290[37] = 0.0; + out_8072626393727229290[38] = 1.0; + out_8072626393727229290[39] = 0.0; + out_8072626393727229290[40] = 0.0; + out_8072626393727229290[41] = 0.0; + out_8072626393727229290[42] = 0.0; + out_8072626393727229290[43] = 0.0; + out_8072626393727229290[44] = 0.0; + out_8072626393727229290[45] = 0.0; + out_8072626393727229290[46] = 0.0; + out_8072626393727229290[47] = 0.0; + out_8072626393727229290[48] = 0.0; + out_8072626393727229290[49] = 0.0; + out_8072626393727229290[50] = 0.0; + out_8072626393727229290[51] = 0.0; + out_8072626393727229290[52] = 0.0; + out_8072626393727229290[53] = 0.0; + out_8072626393727229290[54] = 0.0; + out_8072626393727229290[55] = 0.0; + out_8072626393727229290[56] = 0.0; + out_8072626393727229290[57] = 1.0; + out_8072626393727229290[58] = 0.0; + out_8072626393727229290[59] = 0.0; + out_8072626393727229290[60] = 0.0; + out_8072626393727229290[61] = 0.0; + out_8072626393727229290[62] = 0.0; + out_8072626393727229290[63] = 0.0; + out_8072626393727229290[64] = 0.0; + out_8072626393727229290[65] = 0.0; + out_8072626393727229290[66] = 0.0; + out_8072626393727229290[67] = 0.0; + out_8072626393727229290[68] = 0.0; + out_8072626393727229290[69] = 0.0; + out_8072626393727229290[70] = 0.0; + out_8072626393727229290[71] = 0.0; + out_8072626393727229290[72] = 0.0; + out_8072626393727229290[73] = 0.0; + out_8072626393727229290[74] = 0.0; + out_8072626393727229290[75] = 0.0; + out_8072626393727229290[76] = 1.0; + out_8072626393727229290[77] = 0.0; + out_8072626393727229290[78] = 0.0; + out_8072626393727229290[79] = 0.0; + out_8072626393727229290[80] = 0.0; + out_8072626393727229290[81] = 0.0; + out_8072626393727229290[82] = 0.0; + out_8072626393727229290[83] = 0.0; + out_8072626393727229290[84] = 0.0; + out_8072626393727229290[85] = 0.0; + out_8072626393727229290[86] = 0.0; + out_8072626393727229290[87] = 0.0; + out_8072626393727229290[88] = 0.0; + out_8072626393727229290[89] = 0.0; + out_8072626393727229290[90] = 0.0; + out_8072626393727229290[91] = 0.0; + out_8072626393727229290[92] = 0.0; + out_8072626393727229290[93] = 0.0; + out_8072626393727229290[94] = 0.0; + out_8072626393727229290[95] = 1.0; + out_8072626393727229290[96] = 0.0; + out_8072626393727229290[97] = 0.0; + out_8072626393727229290[98] = 0.0; + out_8072626393727229290[99] = 0.0; + out_8072626393727229290[100] = 0.0; + out_8072626393727229290[101] = 0.0; + out_8072626393727229290[102] = 0.0; + out_8072626393727229290[103] = 0.0; + out_8072626393727229290[104] = 0.0; + out_8072626393727229290[105] = 0.0; + out_8072626393727229290[106] = 0.0; + out_8072626393727229290[107] = 0.0; + out_8072626393727229290[108] = 0.0; + out_8072626393727229290[109] = 0.0; + out_8072626393727229290[110] = 0.0; + out_8072626393727229290[111] = 0.0; + out_8072626393727229290[112] = 0.0; + out_8072626393727229290[113] = 0.0; + out_8072626393727229290[114] = 1.0; + out_8072626393727229290[115] = 0.0; + out_8072626393727229290[116] = 0.0; + out_8072626393727229290[117] = 0.0; + out_8072626393727229290[118] = 0.0; + out_8072626393727229290[119] = 0.0; + out_8072626393727229290[120] = 0.0; + out_8072626393727229290[121] = 0.0; + out_8072626393727229290[122] = 0.0; + out_8072626393727229290[123] = 0.0; + out_8072626393727229290[124] = 0.0; + out_8072626393727229290[125] = 0.0; + out_8072626393727229290[126] = 0.0; + out_8072626393727229290[127] = 0.0; + out_8072626393727229290[128] = 0.0; + out_8072626393727229290[129] = 0.0; + out_8072626393727229290[130] = 0.0; + out_8072626393727229290[131] = 0.0; + out_8072626393727229290[132] = 0.0; + out_8072626393727229290[133] = 1.0; + out_8072626393727229290[134] = 0.0; + out_8072626393727229290[135] = 0.0; + out_8072626393727229290[136] = 0.0; + out_8072626393727229290[137] = 0.0; + out_8072626393727229290[138] = 0.0; + out_8072626393727229290[139] = 0.0; + out_8072626393727229290[140] = 0.0; + out_8072626393727229290[141] = 0.0; + out_8072626393727229290[142] = 0.0; + out_8072626393727229290[143] = 0.0; + out_8072626393727229290[144] = 0.0; + out_8072626393727229290[145] = 0.0; + out_8072626393727229290[146] = 0.0; + out_8072626393727229290[147] = 0.0; + out_8072626393727229290[148] = 0.0; + out_8072626393727229290[149] = 0.0; + out_8072626393727229290[150] = 0.0; + out_8072626393727229290[151] = 0.0; + out_8072626393727229290[152] = 1.0; + out_8072626393727229290[153] = 0.0; + out_8072626393727229290[154] = 0.0; + out_8072626393727229290[155] = 0.0; + out_8072626393727229290[156] = 0.0; + out_8072626393727229290[157] = 0.0; + out_8072626393727229290[158] = 0.0; + out_8072626393727229290[159] = 0.0; + out_8072626393727229290[160] = 0.0; + out_8072626393727229290[161] = 0.0; + out_8072626393727229290[162] = 0.0; + out_8072626393727229290[163] = 0.0; + out_8072626393727229290[164] = 0.0; + out_8072626393727229290[165] = 0.0; + out_8072626393727229290[166] = 0.0; + out_8072626393727229290[167] = 0.0; + out_8072626393727229290[168] = 0.0; + out_8072626393727229290[169] = 0.0; + out_8072626393727229290[170] = 0.0; + out_8072626393727229290[171] = 1.0; + out_8072626393727229290[172] = 0.0; + out_8072626393727229290[173] = 0.0; + out_8072626393727229290[174] = 0.0; + out_8072626393727229290[175] = 0.0; + out_8072626393727229290[176] = 0.0; + out_8072626393727229290[177] = 0.0; + out_8072626393727229290[178] = 0.0; + out_8072626393727229290[179] = 0.0; + out_8072626393727229290[180] = 0.0; + out_8072626393727229290[181] = 0.0; + out_8072626393727229290[182] = 0.0; + out_8072626393727229290[183] = 0.0; + out_8072626393727229290[184] = 0.0; + out_8072626393727229290[185] = 0.0; + out_8072626393727229290[186] = 0.0; + out_8072626393727229290[187] = 0.0; + out_8072626393727229290[188] = 0.0; + out_8072626393727229290[189] = 0.0; + out_8072626393727229290[190] = 1.0; + out_8072626393727229290[191] = 0.0; + out_8072626393727229290[192] = 0.0; + out_8072626393727229290[193] = 0.0; + out_8072626393727229290[194] = 0.0; + out_8072626393727229290[195] = 0.0; + out_8072626393727229290[196] = 0.0; + out_8072626393727229290[197] = 0.0; + out_8072626393727229290[198] = 0.0; + out_8072626393727229290[199] = 0.0; + out_8072626393727229290[200] = 0.0; + out_8072626393727229290[201] = 0.0; + out_8072626393727229290[202] = 0.0; + out_8072626393727229290[203] = 0.0; + out_8072626393727229290[204] = 0.0; + out_8072626393727229290[205] = 0.0; + out_8072626393727229290[206] = 0.0; + out_8072626393727229290[207] = 0.0; + out_8072626393727229290[208] = 0.0; + out_8072626393727229290[209] = 1.0; + out_8072626393727229290[210] = 0.0; + out_8072626393727229290[211] = 0.0; + out_8072626393727229290[212] = 0.0; + out_8072626393727229290[213] = 0.0; + out_8072626393727229290[214] = 0.0; + out_8072626393727229290[215] = 0.0; + out_8072626393727229290[216] = 0.0; + out_8072626393727229290[217] = 0.0; + out_8072626393727229290[218] = 0.0; + out_8072626393727229290[219] = 0.0; + out_8072626393727229290[220] = 0.0; + out_8072626393727229290[221] = 0.0; + out_8072626393727229290[222] = 0.0; + out_8072626393727229290[223] = 0.0; + out_8072626393727229290[224] = 0.0; + out_8072626393727229290[225] = 0.0; + out_8072626393727229290[226] = 0.0; + out_8072626393727229290[227] = 0.0; + out_8072626393727229290[228] = 1.0; + out_8072626393727229290[229] = 0.0; + out_8072626393727229290[230] = 0.0; + out_8072626393727229290[231] = 0.0; + out_8072626393727229290[232] = 0.0; + out_8072626393727229290[233] = 0.0; + out_8072626393727229290[234] = 0.0; + out_8072626393727229290[235] = 0.0; + out_8072626393727229290[236] = 0.0; + out_8072626393727229290[237] = 0.0; + out_8072626393727229290[238] = 0.0; + out_8072626393727229290[239] = 0.0; + out_8072626393727229290[240] = 0.0; + out_8072626393727229290[241] = 0.0; + out_8072626393727229290[242] = 0.0; + out_8072626393727229290[243] = 0.0; + out_8072626393727229290[244] = 0.0; + out_8072626393727229290[245] = 0.0; + out_8072626393727229290[246] = 0.0; + out_8072626393727229290[247] = 1.0; + out_8072626393727229290[248] = 0.0; + out_8072626393727229290[249] = 0.0; + out_8072626393727229290[250] = 0.0; + out_8072626393727229290[251] = 0.0; + out_8072626393727229290[252] = 0.0; + out_8072626393727229290[253] = 0.0; + out_8072626393727229290[254] = 0.0; + out_8072626393727229290[255] = 0.0; + out_8072626393727229290[256] = 0.0; + out_8072626393727229290[257] = 0.0; + out_8072626393727229290[258] = 0.0; + out_8072626393727229290[259] = 0.0; + out_8072626393727229290[260] = 0.0; + out_8072626393727229290[261] = 0.0; + out_8072626393727229290[262] = 0.0; + out_8072626393727229290[263] = 0.0; + out_8072626393727229290[264] = 0.0; + out_8072626393727229290[265] = 0.0; + out_8072626393727229290[266] = 1.0; + out_8072626393727229290[267] = 0.0; + out_8072626393727229290[268] = 0.0; + out_8072626393727229290[269] = 0.0; + out_8072626393727229290[270] = 0.0; + out_8072626393727229290[271] = 0.0; + out_8072626393727229290[272] = 0.0; + out_8072626393727229290[273] = 0.0; + out_8072626393727229290[274] = 0.0; + out_8072626393727229290[275] = 0.0; + out_8072626393727229290[276] = 0.0; + out_8072626393727229290[277] = 0.0; + out_8072626393727229290[278] = 0.0; + out_8072626393727229290[279] = 0.0; + out_8072626393727229290[280] = 0.0; + out_8072626393727229290[281] = 0.0; + out_8072626393727229290[282] = 0.0; + out_8072626393727229290[283] = 0.0; + out_8072626393727229290[284] = 0.0; + out_8072626393727229290[285] = 1.0; + out_8072626393727229290[286] = 0.0; + out_8072626393727229290[287] = 0.0; + out_8072626393727229290[288] = 0.0; + out_8072626393727229290[289] = 0.0; + out_8072626393727229290[290] = 0.0; + out_8072626393727229290[291] = 0.0; + out_8072626393727229290[292] = 0.0; + out_8072626393727229290[293] = 0.0; + out_8072626393727229290[294] = 0.0; + out_8072626393727229290[295] = 0.0; + out_8072626393727229290[296] = 0.0; + out_8072626393727229290[297] = 0.0; + out_8072626393727229290[298] = 0.0; + out_8072626393727229290[299] = 0.0; + out_8072626393727229290[300] = 0.0; + out_8072626393727229290[301] = 0.0; + out_8072626393727229290[302] = 0.0; + out_8072626393727229290[303] = 0.0; + out_8072626393727229290[304] = 1.0; + out_8072626393727229290[305] = 0.0; + out_8072626393727229290[306] = 0.0; + out_8072626393727229290[307] = 0.0; + out_8072626393727229290[308] = 0.0; + out_8072626393727229290[309] = 0.0; + out_8072626393727229290[310] = 0.0; + out_8072626393727229290[311] = 0.0; + out_8072626393727229290[312] = 0.0; + out_8072626393727229290[313] = 0.0; + out_8072626393727229290[314] = 0.0; + out_8072626393727229290[315] = 0.0; + out_8072626393727229290[316] = 0.0; + out_8072626393727229290[317] = 0.0; + out_8072626393727229290[318] = 0.0; + out_8072626393727229290[319] = 0.0; + out_8072626393727229290[320] = 0.0; + out_8072626393727229290[321] = 0.0; + out_8072626393727229290[322] = 0.0; + out_8072626393727229290[323] = 1.0; } -void f_fun(double *state, double dt, double *out_6157473931416548839) { - out_6157473931416548839[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_6157473931416548839[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_6157473931416548839[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_6157473931416548839[3] = dt*state[12] + state[3]; - out_6157473931416548839[4] = dt*state[13] + state[4]; - out_6157473931416548839[5] = dt*state[14] + state[5]; - out_6157473931416548839[6] = state[6]; - out_6157473931416548839[7] = state[7]; - out_6157473931416548839[8] = state[8]; - out_6157473931416548839[9] = state[9]; - out_6157473931416548839[10] = state[10]; - out_6157473931416548839[11] = state[11]; - out_6157473931416548839[12] = state[12]; - out_6157473931416548839[13] = state[13]; - out_6157473931416548839[14] = state[14]; - out_6157473931416548839[15] = state[15]; - out_6157473931416548839[16] = state[16]; - out_6157473931416548839[17] = state[17]; +void f_fun(double *state, double dt, double *out_3550739231859078198) { + out_3550739231859078198[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_3550739231859078198[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_3550739231859078198[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_3550739231859078198[3] = dt*state[12] + state[3]; + out_3550739231859078198[4] = dt*state[13] + state[4]; + out_3550739231859078198[5] = dt*state[14] + state[5]; + out_3550739231859078198[6] = state[6]; + out_3550739231859078198[7] = state[7]; + out_3550739231859078198[8] = state[8]; + out_3550739231859078198[9] = state[9]; + out_3550739231859078198[10] = state[10]; + out_3550739231859078198[11] = state[11]; + out_3550739231859078198[12] = state[12]; + out_3550739231859078198[13] = state[13]; + out_3550739231859078198[14] = state[14]; + out_3550739231859078198[15] = state[15]; + out_3550739231859078198[16] = state[16]; + out_3550739231859078198[17] = state[17]; } -void F_fun(double *state, double dt, double *out_2563232569638129684) { - out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[2] = 0; - out_2563232569638129684[3] = 0; - out_2563232569638129684[4] = 0; - out_2563232569638129684[5] = 0; - out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[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_2563232569638129684[9] = 0; - out_2563232569638129684[10] = 0; - out_2563232569638129684[11] = 0; - out_2563232569638129684[12] = 0; - out_2563232569638129684[13] = 0; - out_2563232569638129684[14] = 0; - out_2563232569638129684[15] = 0; - out_2563232569638129684[16] = 0; - out_2563232569638129684[17] = 0; - out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[20] = 0; - out_2563232569638129684[21] = 0; - out_2563232569638129684[22] = 0; - out_2563232569638129684[23] = 0; - out_2563232569638129684[24] = 0; - out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[27] = 0; - out_2563232569638129684[28] = 0; - out_2563232569638129684[29] = 0; - out_2563232569638129684[30] = 0; - out_2563232569638129684[31] = 0; - out_2563232569638129684[32] = 0; - out_2563232569638129684[33] = 0; - out_2563232569638129684[34] = 0; - out_2563232569638129684[35] = 0; - out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[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_2563232569638129684[39] = 0; - out_2563232569638129684[40] = 0; - out_2563232569638129684[41] = 0; - out_2563232569638129684[42] = 0; - out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[45] = 0; - out_2563232569638129684[46] = 0; - out_2563232569638129684[47] = 0; - out_2563232569638129684[48] = 0; - out_2563232569638129684[49] = 0; - out_2563232569638129684[50] = 0; - out_2563232569638129684[51] = 0; - out_2563232569638129684[52] = 0; - out_2563232569638129684[53] = 0; - out_2563232569638129684[54] = 0; - out_2563232569638129684[55] = 0; - out_2563232569638129684[56] = 0; - out_2563232569638129684[57] = 1; - out_2563232569638129684[58] = 0; - out_2563232569638129684[59] = 0; - out_2563232569638129684[60] = 0; - out_2563232569638129684[61] = 0; - out_2563232569638129684[62] = 0; - out_2563232569638129684[63] = 0; - out_2563232569638129684[64] = 0; - out_2563232569638129684[65] = 0; - out_2563232569638129684[66] = dt; - out_2563232569638129684[67] = 0; - out_2563232569638129684[68] = 0; - out_2563232569638129684[69] = 0; - out_2563232569638129684[70] = 0; - out_2563232569638129684[71] = 0; - out_2563232569638129684[72] = 0; - out_2563232569638129684[73] = 0; - out_2563232569638129684[74] = 0; - out_2563232569638129684[75] = 0; - out_2563232569638129684[76] = 1; - out_2563232569638129684[77] = 0; - out_2563232569638129684[78] = 0; - out_2563232569638129684[79] = 0; - out_2563232569638129684[80] = 0; - out_2563232569638129684[81] = 0; - out_2563232569638129684[82] = 0; - out_2563232569638129684[83] = 0; - out_2563232569638129684[84] = 0; - out_2563232569638129684[85] = dt; - out_2563232569638129684[86] = 0; - out_2563232569638129684[87] = 0; - out_2563232569638129684[88] = 0; - out_2563232569638129684[89] = 0; - out_2563232569638129684[90] = 0; - out_2563232569638129684[91] = 0; - out_2563232569638129684[92] = 0; - out_2563232569638129684[93] = 0; - out_2563232569638129684[94] = 0; - out_2563232569638129684[95] = 1; - out_2563232569638129684[96] = 0; - out_2563232569638129684[97] = 0; - out_2563232569638129684[98] = 0; - out_2563232569638129684[99] = 0; - out_2563232569638129684[100] = 0; - out_2563232569638129684[101] = 0; - out_2563232569638129684[102] = 0; - out_2563232569638129684[103] = 0; - out_2563232569638129684[104] = dt; - out_2563232569638129684[105] = 0; - out_2563232569638129684[106] = 0; - out_2563232569638129684[107] = 0; - out_2563232569638129684[108] = 0; - out_2563232569638129684[109] = 0; - out_2563232569638129684[110] = 0; - out_2563232569638129684[111] = 0; - out_2563232569638129684[112] = 0; - out_2563232569638129684[113] = 0; - out_2563232569638129684[114] = 1; - out_2563232569638129684[115] = 0; - out_2563232569638129684[116] = 0; - out_2563232569638129684[117] = 0; - out_2563232569638129684[118] = 0; - out_2563232569638129684[119] = 0; - out_2563232569638129684[120] = 0; - out_2563232569638129684[121] = 0; - out_2563232569638129684[122] = 0; - out_2563232569638129684[123] = 0; - out_2563232569638129684[124] = 0; - out_2563232569638129684[125] = 0; - out_2563232569638129684[126] = 0; - out_2563232569638129684[127] = 0; - out_2563232569638129684[128] = 0; - out_2563232569638129684[129] = 0; - out_2563232569638129684[130] = 0; - out_2563232569638129684[131] = 0; - out_2563232569638129684[132] = 0; - out_2563232569638129684[133] = 1; - out_2563232569638129684[134] = 0; - out_2563232569638129684[135] = 0; - out_2563232569638129684[136] = 0; - out_2563232569638129684[137] = 0; - out_2563232569638129684[138] = 0; - out_2563232569638129684[139] = 0; - out_2563232569638129684[140] = 0; - out_2563232569638129684[141] = 0; - out_2563232569638129684[142] = 0; - out_2563232569638129684[143] = 0; - out_2563232569638129684[144] = 0; - out_2563232569638129684[145] = 0; - out_2563232569638129684[146] = 0; - out_2563232569638129684[147] = 0; - out_2563232569638129684[148] = 0; - out_2563232569638129684[149] = 0; - out_2563232569638129684[150] = 0; - out_2563232569638129684[151] = 0; - out_2563232569638129684[152] = 1; - out_2563232569638129684[153] = 0; - out_2563232569638129684[154] = 0; - out_2563232569638129684[155] = 0; - out_2563232569638129684[156] = 0; - out_2563232569638129684[157] = 0; - out_2563232569638129684[158] = 0; - out_2563232569638129684[159] = 0; - out_2563232569638129684[160] = 0; - out_2563232569638129684[161] = 0; - out_2563232569638129684[162] = 0; - out_2563232569638129684[163] = 0; - out_2563232569638129684[164] = 0; - out_2563232569638129684[165] = 0; - out_2563232569638129684[166] = 0; - out_2563232569638129684[167] = 0; - out_2563232569638129684[168] = 0; - out_2563232569638129684[169] = 0; - out_2563232569638129684[170] = 0; - out_2563232569638129684[171] = 1; - out_2563232569638129684[172] = 0; - out_2563232569638129684[173] = 0; - out_2563232569638129684[174] = 0; - out_2563232569638129684[175] = 0; - out_2563232569638129684[176] = 0; - out_2563232569638129684[177] = 0; - out_2563232569638129684[178] = 0; - out_2563232569638129684[179] = 0; - out_2563232569638129684[180] = 0; - out_2563232569638129684[181] = 0; - out_2563232569638129684[182] = 0; - out_2563232569638129684[183] = 0; - out_2563232569638129684[184] = 0; - out_2563232569638129684[185] = 0; - out_2563232569638129684[186] = 0; - out_2563232569638129684[187] = 0; - out_2563232569638129684[188] = 0; - out_2563232569638129684[189] = 0; - out_2563232569638129684[190] = 1; - out_2563232569638129684[191] = 0; - out_2563232569638129684[192] = 0; - out_2563232569638129684[193] = 0; - out_2563232569638129684[194] = 0; - out_2563232569638129684[195] = 0; - out_2563232569638129684[196] = 0; - out_2563232569638129684[197] = 0; - out_2563232569638129684[198] = 0; - out_2563232569638129684[199] = 0; - out_2563232569638129684[200] = 0; - out_2563232569638129684[201] = 0; - out_2563232569638129684[202] = 0; - out_2563232569638129684[203] = 0; - out_2563232569638129684[204] = 0; - out_2563232569638129684[205] = 0; - out_2563232569638129684[206] = 0; - out_2563232569638129684[207] = 0; - out_2563232569638129684[208] = 0; - out_2563232569638129684[209] = 1; - out_2563232569638129684[210] = 0; - out_2563232569638129684[211] = 0; - out_2563232569638129684[212] = 0; - out_2563232569638129684[213] = 0; - out_2563232569638129684[214] = 0; - out_2563232569638129684[215] = 0; - out_2563232569638129684[216] = 0; - out_2563232569638129684[217] = 0; - out_2563232569638129684[218] = 0; - out_2563232569638129684[219] = 0; - out_2563232569638129684[220] = 0; - out_2563232569638129684[221] = 0; - out_2563232569638129684[222] = 0; - out_2563232569638129684[223] = 0; - out_2563232569638129684[224] = 0; - out_2563232569638129684[225] = 0; - out_2563232569638129684[226] = 0; - out_2563232569638129684[227] = 0; - out_2563232569638129684[228] = 1; - out_2563232569638129684[229] = 0; - out_2563232569638129684[230] = 0; - out_2563232569638129684[231] = 0; - out_2563232569638129684[232] = 0; - out_2563232569638129684[233] = 0; - out_2563232569638129684[234] = 0; - out_2563232569638129684[235] = 0; - out_2563232569638129684[236] = 0; - out_2563232569638129684[237] = 0; - out_2563232569638129684[238] = 0; - out_2563232569638129684[239] = 0; - out_2563232569638129684[240] = 0; - out_2563232569638129684[241] = 0; - out_2563232569638129684[242] = 0; - out_2563232569638129684[243] = 0; - out_2563232569638129684[244] = 0; - out_2563232569638129684[245] = 0; - out_2563232569638129684[246] = 0; - out_2563232569638129684[247] = 1; - out_2563232569638129684[248] = 0; - out_2563232569638129684[249] = 0; - out_2563232569638129684[250] = 0; - out_2563232569638129684[251] = 0; - out_2563232569638129684[252] = 0; - out_2563232569638129684[253] = 0; - out_2563232569638129684[254] = 0; - out_2563232569638129684[255] = 0; - out_2563232569638129684[256] = 0; - out_2563232569638129684[257] = 0; - out_2563232569638129684[258] = 0; - out_2563232569638129684[259] = 0; - out_2563232569638129684[260] = 0; - out_2563232569638129684[261] = 0; - out_2563232569638129684[262] = 0; - out_2563232569638129684[263] = 0; - out_2563232569638129684[264] = 0; - out_2563232569638129684[265] = 0; - out_2563232569638129684[266] = 1; - out_2563232569638129684[267] = 0; - out_2563232569638129684[268] = 0; - out_2563232569638129684[269] = 0; - out_2563232569638129684[270] = 0; - out_2563232569638129684[271] = 0; - out_2563232569638129684[272] = 0; - out_2563232569638129684[273] = 0; - out_2563232569638129684[274] = 0; - out_2563232569638129684[275] = 0; - out_2563232569638129684[276] = 0; - out_2563232569638129684[277] = 0; - out_2563232569638129684[278] = 0; - out_2563232569638129684[279] = 0; - out_2563232569638129684[280] = 0; - out_2563232569638129684[281] = 0; - out_2563232569638129684[282] = 0; - out_2563232569638129684[283] = 0; - out_2563232569638129684[284] = 0; - out_2563232569638129684[285] = 1; - out_2563232569638129684[286] = 0; - out_2563232569638129684[287] = 0; - out_2563232569638129684[288] = 0; - out_2563232569638129684[289] = 0; - out_2563232569638129684[290] = 0; - out_2563232569638129684[291] = 0; - out_2563232569638129684[292] = 0; - out_2563232569638129684[293] = 0; - out_2563232569638129684[294] = 0; - out_2563232569638129684[295] = 0; - out_2563232569638129684[296] = 0; - out_2563232569638129684[297] = 0; - out_2563232569638129684[298] = 0; - out_2563232569638129684[299] = 0; - out_2563232569638129684[300] = 0; - out_2563232569638129684[301] = 0; - out_2563232569638129684[302] = 0; - out_2563232569638129684[303] = 0; - out_2563232569638129684[304] = 1; - out_2563232569638129684[305] = 0; - out_2563232569638129684[306] = 0; - out_2563232569638129684[307] = 0; - out_2563232569638129684[308] = 0; - out_2563232569638129684[309] = 0; - out_2563232569638129684[310] = 0; - out_2563232569638129684[311] = 0; - out_2563232569638129684[312] = 0; - out_2563232569638129684[313] = 0; - out_2563232569638129684[314] = 0; - out_2563232569638129684[315] = 0; - out_2563232569638129684[316] = 0; - out_2563232569638129684[317] = 0; - out_2563232569638129684[318] = 0; - out_2563232569638129684[319] = 0; - out_2563232569638129684[320] = 0; - out_2563232569638129684[321] = 0; - out_2563232569638129684[322] = 0; - out_2563232569638129684[323] = 1; +void F_fun(double *state, double dt, double *out_4236350099338455825) { + out_4236350099338455825[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_4236350099338455825[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_4236350099338455825[2] = 0; + out_4236350099338455825[3] = 0; + out_4236350099338455825[4] = 0; + out_4236350099338455825[5] = 0; + out_4236350099338455825[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_4236350099338455825[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_4236350099338455825[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_4236350099338455825[9] = 0; + out_4236350099338455825[10] = 0; + out_4236350099338455825[11] = 0; + out_4236350099338455825[12] = 0; + out_4236350099338455825[13] = 0; + out_4236350099338455825[14] = 0; + out_4236350099338455825[15] = 0; + out_4236350099338455825[16] = 0; + out_4236350099338455825[17] = 0; + out_4236350099338455825[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_4236350099338455825[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_4236350099338455825[20] = 0; + out_4236350099338455825[21] = 0; + out_4236350099338455825[22] = 0; + out_4236350099338455825[23] = 0; + out_4236350099338455825[24] = 0; + out_4236350099338455825[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_4236350099338455825[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_4236350099338455825[27] = 0; + out_4236350099338455825[28] = 0; + out_4236350099338455825[29] = 0; + out_4236350099338455825[30] = 0; + out_4236350099338455825[31] = 0; + out_4236350099338455825[32] = 0; + out_4236350099338455825[33] = 0; + out_4236350099338455825[34] = 0; + out_4236350099338455825[35] = 0; + out_4236350099338455825[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_4236350099338455825[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_4236350099338455825[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_4236350099338455825[39] = 0; + out_4236350099338455825[40] = 0; + out_4236350099338455825[41] = 0; + out_4236350099338455825[42] = 0; + out_4236350099338455825[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_4236350099338455825[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_4236350099338455825[45] = 0; + out_4236350099338455825[46] = 0; + out_4236350099338455825[47] = 0; + out_4236350099338455825[48] = 0; + out_4236350099338455825[49] = 0; + out_4236350099338455825[50] = 0; + out_4236350099338455825[51] = 0; + out_4236350099338455825[52] = 0; + out_4236350099338455825[53] = 0; + out_4236350099338455825[54] = 0; + out_4236350099338455825[55] = 0; + out_4236350099338455825[56] = 0; + out_4236350099338455825[57] = 1; + out_4236350099338455825[58] = 0; + out_4236350099338455825[59] = 0; + out_4236350099338455825[60] = 0; + out_4236350099338455825[61] = 0; + out_4236350099338455825[62] = 0; + out_4236350099338455825[63] = 0; + out_4236350099338455825[64] = 0; + out_4236350099338455825[65] = 0; + out_4236350099338455825[66] = dt; + out_4236350099338455825[67] = 0; + out_4236350099338455825[68] = 0; + out_4236350099338455825[69] = 0; + out_4236350099338455825[70] = 0; + out_4236350099338455825[71] = 0; + out_4236350099338455825[72] = 0; + out_4236350099338455825[73] = 0; + out_4236350099338455825[74] = 0; + out_4236350099338455825[75] = 0; + out_4236350099338455825[76] = 1; + out_4236350099338455825[77] = 0; + out_4236350099338455825[78] = 0; + out_4236350099338455825[79] = 0; + out_4236350099338455825[80] = 0; + out_4236350099338455825[81] = 0; + out_4236350099338455825[82] = 0; + out_4236350099338455825[83] = 0; + out_4236350099338455825[84] = 0; + out_4236350099338455825[85] = dt; + out_4236350099338455825[86] = 0; + out_4236350099338455825[87] = 0; + out_4236350099338455825[88] = 0; + out_4236350099338455825[89] = 0; + out_4236350099338455825[90] = 0; + out_4236350099338455825[91] = 0; + out_4236350099338455825[92] = 0; + out_4236350099338455825[93] = 0; + out_4236350099338455825[94] = 0; + out_4236350099338455825[95] = 1; + out_4236350099338455825[96] = 0; + out_4236350099338455825[97] = 0; + out_4236350099338455825[98] = 0; + out_4236350099338455825[99] = 0; + out_4236350099338455825[100] = 0; + out_4236350099338455825[101] = 0; + out_4236350099338455825[102] = 0; + out_4236350099338455825[103] = 0; + out_4236350099338455825[104] = dt; + out_4236350099338455825[105] = 0; + out_4236350099338455825[106] = 0; + out_4236350099338455825[107] = 0; + out_4236350099338455825[108] = 0; + out_4236350099338455825[109] = 0; + out_4236350099338455825[110] = 0; + out_4236350099338455825[111] = 0; + out_4236350099338455825[112] = 0; + out_4236350099338455825[113] = 0; + out_4236350099338455825[114] = 1; + out_4236350099338455825[115] = 0; + out_4236350099338455825[116] = 0; + out_4236350099338455825[117] = 0; + out_4236350099338455825[118] = 0; + out_4236350099338455825[119] = 0; + out_4236350099338455825[120] = 0; + out_4236350099338455825[121] = 0; + out_4236350099338455825[122] = 0; + out_4236350099338455825[123] = 0; + out_4236350099338455825[124] = 0; + out_4236350099338455825[125] = 0; + out_4236350099338455825[126] = 0; + out_4236350099338455825[127] = 0; + out_4236350099338455825[128] = 0; + out_4236350099338455825[129] = 0; + out_4236350099338455825[130] = 0; + out_4236350099338455825[131] = 0; + out_4236350099338455825[132] = 0; + out_4236350099338455825[133] = 1; + out_4236350099338455825[134] = 0; + out_4236350099338455825[135] = 0; + out_4236350099338455825[136] = 0; + out_4236350099338455825[137] = 0; + out_4236350099338455825[138] = 0; + out_4236350099338455825[139] = 0; + out_4236350099338455825[140] = 0; + out_4236350099338455825[141] = 0; + out_4236350099338455825[142] = 0; + out_4236350099338455825[143] = 0; + out_4236350099338455825[144] = 0; + out_4236350099338455825[145] = 0; + out_4236350099338455825[146] = 0; + out_4236350099338455825[147] = 0; + out_4236350099338455825[148] = 0; + out_4236350099338455825[149] = 0; + out_4236350099338455825[150] = 0; + out_4236350099338455825[151] = 0; + out_4236350099338455825[152] = 1; + out_4236350099338455825[153] = 0; + out_4236350099338455825[154] = 0; + out_4236350099338455825[155] = 0; + out_4236350099338455825[156] = 0; + out_4236350099338455825[157] = 0; + out_4236350099338455825[158] = 0; + out_4236350099338455825[159] = 0; + out_4236350099338455825[160] = 0; + out_4236350099338455825[161] = 0; + out_4236350099338455825[162] = 0; + out_4236350099338455825[163] = 0; + out_4236350099338455825[164] = 0; + out_4236350099338455825[165] = 0; + out_4236350099338455825[166] = 0; + out_4236350099338455825[167] = 0; + out_4236350099338455825[168] = 0; + out_4236350099338455825[169] = 0; + out_4236350099338455825[170] = 0; + out_4236350099338455825[171] = 1; + out_4236350099338455825[172] = 0; + out_4236350099338455825[173] = 0; + out_4236350099338455825[174] = 0; + out_4236350099338455825[175] = 0; + out_4236350099338455825[176] = 0; + out_4236350099338455825[177] = 0; + out_4236350099338455825[178] = 0; + out_4236350099338455825[179] = 0; + out_4236350099338455825[180] = 0; + out_4236350099338455825[181] = 0; + out_4236350099338455825[182] = 0; + out_4236350099338455825[183] = 0; + out_4236350099338455825[184] = 0; + out_4236350099338455825[185] = 0; + out_4236350099338455825[186] = 0; + out_4236350099338455825[187] = 0; + out_4236350099338455825[188] = 0; + out_4236350099338455825[189] = 0; + out_4236350099338455825[190] = 1; + out_4236350099338455825[191] = 0; + out_4236350099338455825[192] = 0; + out_4236350099338455825[193] = 0; + out_4236350099338455825[194] = 0; + out_4236350099338455825[195] = 0; + out_4236350099338455825[196] = 0; + out_4236350099338455825[197] = 0; + out_4236350099338455825[198] = 0; + out_4236350099338455825[199] = 0; + out_4236350099338455825[200] = 0; + out_4236350099338455825[201] = 0; + out_4236350099338455825[202] = 0; + out_4236350099338455825[203] = 0; + out_4236350099338455825[204] = 0; + out_4236350099338455825[205] = 0; + out_4236350099338455825[206] = 0; + out_4236350099338455825[207] = 0; + out_4236350099338455825[208] = 0; + out_4236350099338455825[209] = 1; + out_4236350099338455825[210] = 0; + out_4236350099338455825[211] = 0; + out_4236350099338455825[212] = 0; + out_4236350099338455825[213] = 0; + out_4236350099338455825[214] = 0; + out_4236350099338455825[215] = 0; + out_4236350099338455825[216] = 0; + out_4236350099338455825[217] = 0; + out_4236350099338455825[218] = 0; + out_4236350099338455825[219] = 0; + out_4236350099338455825[220] = 0; + out_4236350099338455825[221] = 0; + out_4236350099338455825[222] = 0; + out_4236350099338455825[223] = 0; + out_4236350099338455825[224] = 0; + out_4236350099338455825[225] = 0; + out_4236350099338455825[226] = 0; + out_4236350099338455825[227] = 0; + out_4236350099338455825[228] = 1; + out_4236350099338455825[229] = 0; + out_4236350099338455825[230] = 0; + out_4236350099338455825[231] = 0; + out_4236350099338455825[232] = 0; + out_4236350099338455825[233] = 0; + out_4236350099338455825[234] = 0; + out_4236350099338455825[235] = 0; + out_4236350099338455825[236] = 0; + out_4236350099338455825[237] = 0; + out_4236350099338455825[238] = 0; + out_4236350099338455825[239] = 0; + out_4236350099338455825[240] = 0; + out_4236350099338455825[241] = 0; + out_4236350099338455825[242] = 0; + out_4236350099338455825[243] = 0; + out_4236350099338455825[244] = 0; + out_4236350099338455825[245] = 0; + out_4236350099338455825[246] = 0; + out_4236350099338455825[247] = 1; + out_4236350099338455825[248] = 0; + out_4236350099338455825[249] = 0; + out_4236350099338455825[250] = 0; + out_4236350099338455825[251] = 0; + out_4236350099338455825[252] = 0; + out_4236350099338455825[253] = 0; + out_4236350099338455825[254] = 0; + out_4236350099338455825[255] = 0; + out_4236350099338455825[256] = 0; + out_4236350099338455825[257] = 0; + out_4236350099338455825[258] = 0; + out_4236350099338455825[259] = 0; + out_4236350099338455825[260] = 0; + out_4236350099338455825[261] = 0; + out_4236350099338455825[262] = 0; + out_4236350099338455825[263] = 0; + out_4236350099338455825[264] = 0; + out_4236350099338455825[265] = 0; + out_4236350099338455825[266] = 1; + out_4236350099338455825[267] = 0; + out_4236350099338455825[268] = 0; + out_4236350099338455825[269] = 0; + out_4236350099338455825[270] = 0; + out_4236350099338455825[271] = 0; + out_4236350099338455825[272] = 0; + out_4236350099338455825[273] = 0; + out_4236350099338455825[274] = 0; + out_4236350099338455825[275] = 0; + out_4236350099338455825[276] = 0; + out_4236350099338455825[277] = 0; + out_4236350099338455825[278] = 0; + out_4236350099338455825[279] = 0; + out_4236350099338455825[280] = 0; + out_4236350099338455825[281] = 0; + out_4236350099338455825[282] = 0; + out_4236350099338455825[283] = 0; + out_4236350099338455825[284] = 0; + out_4236350099338455825[285] = 1; + out_4236350099338455825[286] = 0; + out_4236350099338455825[287] = 0; + out_4236350099338455825[288] = 0; + out_4236350099338455825[289] = 0; + out_4236350099338455825[290] = 0; + out_4236350099338455825[291] = 0; + out_4236350099338455825[292] = 0; + out_4236350099338455825[293] = 0; + out_4236350099338455825[294] = 0; + out_4236350099338455825[295] = 0; + out_4236350099338455825[296] = 0; + out_4236350099338455825[297] = 0; + out_4236350099338455825[298] = 0; + out_4236350099338455825[299] = 0; + out_4236350099338455825[300] = 0; + out_4236350099338455825[301] = 0; + out_4236350099338455825[302] = 0; + out_4236350099338455825[303] = 0; + out_4236350099338455825[304] = 1; + out_4236350099338455825[305] = 0; + out_4236350099338455825[306] = 0; + out_4236350099338455825[307] = 0; + out_4236350099338455825[308] = 0; + out_4236350099338455825[309] = 0; + out_4236350099338455825[310] = 0; + out_4236350099338455825[311] = 0; + out_4236350099338455825[312] = 0; + out_4236350099338455825[313] = 0; + out_4236350099338455825[314] = 0; + out_4236350099338455825[315] = 0; + out_4236350099338455825[316] = 0; + out_4236350099338455825[317] = 0; + out_4236350099338455825[318] = 0; + out_4236350099338455825[319] = 0; + out_4236350099338455825[320] = 0; + out_4236350099338455825[321] = 0; + out_4236350099338455825[322] = 0; + out_4236350099338455825[323] = 1; } -void h_4(double *state, double *unused, double *out_9057789463521636950) { - out_9057789463521636950[0] = state[6] + state[9]; - out_9057789463521636950[1] = state[7] + state[10]; - out_9057789463521636950[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_2031088737794184983) { + out_2031088737794184983[0] = state[6] + state[9]; + out_2031088737794184983[1] = state[7] + state[10]; + out_2031088737794184983[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_6434139615882878081) { - out_6434139615882878081[0] = 0; - out_6434139615882878081[1] = 0; - out_6434139615882878081[2] = 0; - out_6434139615882878081[3] = 0; - out_6434139615882878081[4] = 0; - out_6434139615882878081[5] = 0; - out_6434139615882878081[6] = 1; - out_6434139615882878081[7] = 0; - out_6434139615882878081[8] = 0; - out_6434139615882878081[9] = 1; - out_6434139615882878081[10] = 0; - out_6434139615882878081[11] = 0; - out_6434139615882878081[12] = 0; - out_6434139615882878081[13] = 0; - out_6434139615882878081[14] = 0; - out_6434139615882878081[15] = 0; - out_6434139615882878081[16] = 0; - out_6434139615882878081[17] = 0; - out_6434139615882878081[18] = 0; - out_6434139615882878081[19] = 0; - out_6434139615882878081[20] = 0; - out_6434139615882878081[21] = 0; - out_6434139615882878081[22] = 0; - out_6434139615882878081[23] = 0; - out_6434139615882878081[24] = 0; - out_6434139615882878081[25] = 1; - out_6434139615882878081[26] = 0; - out_6434139615882878081[27] = 0; - out_6434139615882878081[28] = 1; - out_6434139615882878081[29] = 0; - out_6434139615882878081[30] = 0; - out_6434139615882878081[31] = 0; - out_6434139615882878081[32] = 0; - out_6434139615882878081[33] = 0; - out_6434139615882878081[34] = 0; - out_6434139615882878081[35] = 0; - out_6434139615882878081[36] = 0; - out_6434139615882878081[37] = 0; - out_6434139615882878081[38] = 0; - out_6434139615882878081[39] = 0; - out_6434139615882878081[40] = 0; - out_6434139615882878081[41] = 0; - out_6434139615882878081[42] = 0; - out_6434139615882878081[43] = 0; - out_6434139615882878081[44] = 1; - out_6434139615882878081[45] = 0; - out_6434139615882878081[46] = 0; - out_6434139615882878081[47] = 1; - out_6434139615882878081[48] = 0; - out_6434139615882878081[49] = 0; - out_6434139615882878081[50] = 0; - out_6434139615882878081[51] = 0; - out_6434139615882878081[52] = 0; - out_6434139615882878081[53] = 0; +void H_4(double *state, double *unused, double *out_6602310470138414754) { + out_6602310470138414754[0] = 0; + out_6602310470138414754[1] = 0; + out_6602310470138414754[2] = 0; + out_6602310470138414754[3] = 0; + out_6602310470138414754[4] = 0; + out_6602310470138414754[5] = 0; + out_6602310470138414754[6] = 1; + out_6602310470138414754[7] = 0; + out_6602310470138414754[8] = 0; + out_6602310470138414754[9] = 1; + out_6602310470138414754[10] = 0; + out_6602310470138414754[11] = 0; + out_6602310470138414754[12] = 0; + out_6602310470138414754[13] = 0; + out_6602310470138414754[14] = 0; + out_6602310470138414754[15] = 0; + out_6602310470138414754[16] = 0; + out_6602310470138414754[17] = 0; + out_6602310470138414754[18] = 0; + out_6602310470138414754[19] = 0; + out_6602310470138414754[20] = 0; + out_6602310470138414754[21] = 0; + out_6602310470138414754[22] = 0; + out_6602310470138414754[23] = 0; + out_6602310470138414754[24] = 0; + out_6602310470138414754[25] = 1; + out_6602310470138414754[26] = 0; + out_6602310470138414754[27] = 0; + out_6602310470138414754[28] = 1; + out_6602310470138414754[29] = 0; + out_6602310470138414754[30] = 0; + out_6602310470138414754[31] = 0; + out_6602310470138414754[32] = 0; + out_6602310470138414754[33] = 0; + out_6602310470138414754[34] = 0; + out_6602310470138414754[35] = 0; + out_6602310470138414754[36] = 0; + out_6602310470138414754[37] = 0; + out_6602310470138414754[38] = 0; + out_6602310470138414754[39] = 0; + out_6602310470138414754[40] = 0; + out_6602310470138414754[41] = 0; + out_6602310470138414754[42] = 0; + out_6602310470138414754[43] = 0; + out_6602310470138414754[44] = 1; + out_6602310470138414754[45] = 0; + out_6602310470138414754[46] = 0; + out_6602310470138414754[47] = 1; + out_6602310470138414754[48] = 0; + out_6602310470138414754[49] = 0; + out_6602310470138414754[50] = 0; + out_6602310470138414754[51] = 0; + out_6602310470138414754[52] = 0; + out_6602310470138414754[53] = 0; } -void h_10(double *state, double *unused, double *out_439040734435285181) { - out_439040734435285181[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_439040734435285181[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_439040734435285181[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_2254252755933878198) { + out_2254252755933878198[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_2254252755933878198[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_2254252755933878198[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_1840785913394498883) { - out_1840785913394498883[0] = 0; - out_1840785913394498883[1] = 9.8100000000000005*cos(state[1]); - out_1840785913394498883[2] = 0; - out_1840785913394498883[3] = 0; - out_1840785913394498883[4] = -state[8]; - out_1840785913394498883[5] = state[7]; - out_1840785913394498883[6] = 0; - out_1840785913394498883[7] = state[5]; - out_1840785913394498883[8] = -state[4]; - out_1840785913394498883[9] = 0; - out_1840785913394498883[10] = 0; - out_1840785913394498883[11] = 0; - out_1840785913394498883[12] = 1; - out_1840785913394498883[13] = 0; - out_1840785913394498883[14] = 0; - out_1840785913394498883[15] = 1; - out_1840785913394498883[16] = 0; - out_1840785913394498883[17] = 0; - out_1840785913394498883[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_1840785913394498883[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_1840785913394498883[20] = 0; - out_1840785913394498883[21] = state[8]; - out_1840785913394498883[22] = 0; - out_1840785913394498883[23] = -state[6]; - out_1840785913394498883[24] = -state[5]; - out_1840785913394498883[25] = 0; - out_1840785913394498883[26] = state[3]; - out_1840785913394498883[27] = 0; - out_1840785913394498883[28] = 0; - out_1840785913394498883[29] = 0; - out_1840785913394498883[30] = 0; - out_1840785913394498883[31] = 1; - out_1840785913394498883[32] = 0; - out_1840785913394498883[33] = 0; - out_1840785913394498883[34] = 1; - out_1840785913394498883[35] = 0; - out_1840785913394498883[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_1840785913394498883[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_1840785913394498883[38] = 0; - out_1840785913394498883[39] = -state[7]; - out_1840785913394498883[40] = state[6]; - out_1840785913394498883[41] = 0; - out_1840785913394498883[42] = state[4]; - out_1840785913394498883[43] = -state[3]; - out_1840785913394498883[44] = 0; - out_1840785913394498883[45] = 0; - out_1840785913394498883[46] = 0; - out_1840785913394498883[47] = 0; - out_1840785913394498883[48] = 0; - out_1840785913394498883[49] = 0; - out_1840785913394498883[50] = 1; - out_1840785913394498883[51] = 0; - out_1840785913394498883[52] = 0; - out_1840785913394498883[53] = 1; +void H_10(double *state, double *unused, double *out_4753607725561450752) { + out_4753607725561450752[0] = 0; + out_4753607725561450752[1] = 9.8100000000000005*cos(state[1]); + out_4753607725561450752[2] = 0; + out_4753607725561450752[3] = 0; + out_4753607725561450752[4] = -state[8]; + out_4753607725561450752[5] = state[7]; + out_4753607725561450752[6] = 0; + out_4753607725561450752[7] = state[5]; + out_4753607725561450752[8] = -state[4]; + out_4753607725561450752[9] = 0; + out_4753607725561450752[10] = 0; + out_4753607725561450752[11] = 0; + out_4753607725561450752[12] = 1; + out_4753607725561450752[13] = 0; + out_4753607725561450752[14] = 0; + out_4753607725561450752[15] = 1; + out_4753607725561450752[16] = 0; + out_4753607725561450752[17] = 0; + out_4753607725561450752[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_4753607725561450752[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_4753607725561450752[20] = 0; + out_4753607725561450752[21] = state[8]; + out_4753607725561450752[22] = 0; + out_4753607725561450752[23] = -state[6]; + out_4753607725561450752[24] = -state[5]; + out_4753607725561450752[25] = 0; + out_4753607725561450752[26] = state[3]; + out_4753607725561450752[27] = 0; + out_4753607725561450752[28] = 0; + out_4753607725561450752[29] = 0; + out_4753607725561450752[30] = 0; + out_4753607725561450752[31] = 1; + out_4753607725561450752[32] = 0; + out_4753607725561450752[33] = 0; + out_4753607725561450752[34] = 1; + out_4753607725561450752[35] = 0; + out_4753607725561450752[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_4753607725561450752[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_4753607725561450752[38] = 0; + out_4753607725561450752[39] = -state[7]; + out_4753607725561450752[40] = state[6]; + out_4753607725561450752[41] = 0; + out_4753607725561450752[42] = state[4]; + out_4753607725561450752[43] = -state[3]; + out_4753607725561450752[44] = 0; + out_4753607725561450752[45] = 0; + out_4753607725561450752[46] = 0; + out_4753607725561450752[47] = 0; + out_4753607725561450752[48] = 0; + out_4753607725561450752[49] = 0; + out_4753607725561450752[50] = 1; + out_4753607725561450752[51] = 0; + out_4753607725561450752[52] = 0; + out_4753607725561450752[53] = 1; } -void h_13(double *state, double *unused, double *out_7181492765475252926) { - out_7181492765475252926[0] = state[3]; - out_7181492765475252926[1] = state[4]; - out_7181492765475252926[2] = state[5]; +void h_13(double *state, double *unused, double *out_561467872228044768) { + out_561467872228044768[0] = state[3]; + out_561467872228044768[1] = state[4]; + out_561467872228044768[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3221865790550545280) { - out_3221865790550545280[0] = 0; - out_3221865790550545280[1] = 0; - out_3221865790550545280[2] = 0; - out_3221865790550545280[3] = 1; - out_3221865790550545280[4] = 0; - out_3221865790550545280[5] = 0; - out_3221865790550545280[6] = 0; - out_3221865790550545280[7] = 0; - out_3221865790550545280[8] = 0; - out_3221865790550545280[9] = 0; - out_3221865790550545280[10] = 0; - out_3221865790550545280[11] = 0; - out_3221865790550545280[12] = 0; - out_3221865790550545280[13] = 0; - out_3221865790550545280[14] = 0; - out_3221865790550545280[15] = 0; - out_3221865790550545280[16] = 0; - out_3221865790550545280[17] = 0; - out_3221865790550545280[18] = 0; - out_3221865790550545280[19] = 0; - out_3221865790550545280[20] = 0; - out_3221865790550545280[21] = 0; - out_3221865790550545280[22] = 1; - out_3221865790550545280[23] = 0; - out_3221865790550545280[24] = 0; - out_3221865790550545280[25] = 0; - out_3221865790550545280[26] = 0; - out_3221865790550545280[27] = 0; - out_3221865790550545280[28] = 0; - out_3221865790550545280[29] = 0; - out_3221865790550545280[30] = 0; - out_3221865790550545280[31] = 0; - out_3221865790550545280[32] = 0; - out_3221865790550545280[33] = 0; - out_3221865790550545280[34] = 0; - out_3221865790550545280[35] = 0; - out_3221865790550545280[36] = 0; - out_3221865790550545280[37] = 0; - out_3221865790550545280[38] = 0; - out_3221865790550545280[39] = 0; - out_3221865790550545280[40] = 0; - out_3221865790550545280[41] = 1; - out_3221865790550545280[42] = 0; - out_3221865790550545280[43] = 0; - out_3221865790550545280[44] = 0; - out_3221865790550545280[45] = 0; - out_3221865790550545280[46] = 0; - out_3221865790550545280[47] = 0; - out_3221865790550545280[48] = 0; - out_3221865790550545280[49] = 0; - out_3221865790550545280[50] = 0; - out_3221865790550545280[51] = 0; - out_3221865790550545280[52] = 0; - out_3221865790550545280[53] = 0; +void H_13(double *state, double *unused, double *out_3390036644806081953) { + out_3390036644806081953[0] = 0; + out_3390036644806081953[1] = 0; + out_3390036644806081953[2] = 0; + out_3390036644806081953[3] = 1; + out_3390036644806081953[4] = 0; + out_3390036644806081953[5] = 0; + out_3390036644806081953[6] = 0; + out_3390036644806081953[7] = 0; + out_3390036644806081953[8] = 0; + out_3390036644806081953[9] = 0; + out_3390036644806081953[10] = 0; + out_3390036644806081953[11] = 0; + out_3390036644806081953[12] = 0; + out_3390036644806081953[13] = 0; + out_3390036644806081953[14] = 0; + out_3390036644806081953[15] = 0; + out_3390036644806081953[16] = 0; + out_3390036644806081953[17] = 0; + out_3390036644806081953[18] = 0; + out_3390036644806081953[19] = 0; + out_3390036644806081953[20] = 0; + out_3390036644806081953[21] = 0; + out_3390036644806081953[22] = 1; + out_3390036644806081953[23] = 0; + out_3390036644806081953[24] = 0; + out_3390036644806081953[25] = 0; + out_3390036644806081953[26] = 0; + out_3390036644806081953[27] = 0; + out_3390036644806081953[28] = 0; + out_3390036644806081953[29] = 0; + out_3390036644806081953[30] = 0; + out_3390036644806081953[31] = 0; + out_3390036644806081953[32] = 0; + out_3390036644806081953[33] = 0; + out_3390036644806081953[34] = 0; + out_3390036644806081953[35] = 0; + out_3390036644806081953[36] = 0; + out_3390036644806081953[37] = 0; + out_3390036644806081953[38] = 0; + out_3390036644806081953[39] = 0; + out_3390036644806081953[40] = 0; + out_3390036644806081953[41] = 1; + out_3390036644806081953[42] = 0; + out_3390036644806081953[43] = 0; + out_3390036644806081953[44] = 0; + out_3390036644806081953[45] = 0; + out_3390036644806081953[46] = 0; + out_3390036644806081953[47] = 0; + out_3390036644806081953[48] = 0; + out_3390036644806081953[49] = 0; + out_3390036644806081953[50] = 0; + out_3390036644806081953[51] = 0; + out_3390036644806081953[52] = 0; + out_3390036644806081953[53] = 0; } -void h_14(double *state, double *unused, double *out_1398557426791028034) { - out_1398557426791028034[0] = state[6]; - out_1398557426791028034[1] = state[7]; - out_1398557426791028034[2] = state[8]; +void h_14(double *state, double *unused, double *out_5074055792402957007) { + out_5074055792402957007[0] = state[6]; + out_5074055792402957007[1] = state[7]; + out_5074055792402957007[2] = state[8]; } -void H_14(double *state, double *unused, double *out_2470898759543393552) { - out_2470898759543393552[0] = 0; - out_2470898759543393552[1] = 0; - out_2470898759543393552[2] = 0; - out_2470898759543393552[3] = 0; - out_2470898759543393552[4] = 0; - out_2470898759543393552[5] = 0; - out_2470898759543393552[6] = 1; - out_2470898759543393552[7] = 0; - out_2470898759543393552[8] = 0; - out_2470898759543393552[9] = 0; - out_2470898759543393552[10] = 0; - out_2470898759543393552[11] = 0; - out_2470898759543393552[12] = 0; - out_2470898759543393552[13] = 0; - out_2470898759543393552[14] = 0; - out_2470898759543393552[15] = 0; - out_2470898759543393552[16] = 0; - out_2470898759543393552[17] = 0; - out_2470898759543393552[18] = 0; - out_2470898759543393552[19] = 0; - out_2470898759543393552[20] = 0; - out_2470898759543393552[21] = 0; - out_2470898759543393552[22] = 0; - out_2470898759543393552[23] = 0; - out_2470898759543393552[24] = 0; - out_2470898759543393552[25] = 1; - out_2470898759543393552[26] = 0; - out_2470898759543393552[27] = 0; - out_2470898759543393552[28] = 0; - out_2470898759543393552[29] = 0; - out_2470898759543393552[30] = 0; - out_2470898759543393552[31] = 0; - out_2470898759543393552[32] = 0; - out_2470898759543393552[33] = 0; - out_2470898759543393552[34] = 0; - out_2470898759543393552[35] = 0; - out_2470898759543393552[36] = 0; - out_2470898759543393552[37] = 0; - out_2470898759543393552[38] = 0; - out_2470898759543393552[39] = 0; - out_2470898759543393552[40] = 0; - out_2470898759543393552[41] = 0; - out_2470898759543393552[42] = 0; - out_2470898759543393552[43] = 0; - out_2470898759543393552[44] = 1; - out_2470898759543393552[45] = 0; - out_2470898759543393552[46] = 0; - out_2470898759543393552[47] = 0; - out_2470898759543393552[48] = 0; - out_2470898759543393552[49] = 0; - out_2470898759543393552[50] = 0; - out_2470898759543393552[51] = 0; - out_2470898759543393552[52] = 0; - out_2470898759543393552[53] = 0; +void H_14(double *state, double *unused, double *out_8761645171275764566) { + out_8761645171275764566[0] = 0; + out_8761645171275764566[1] = 0; + out_8761645171275764566[2] = 0; + out_8761645171275764566[3] = 0; + out_8761645171275764566[4] = 0; + out_8761645171275764566[5] = 0; + out_8761645171275764566[6] = 1; + out_8761645171275764566[7] = 0; + out_8761645171275764566[8] = 0; + out_8761645171275764566[9] = 0; + out_8761645171275764566[10] = 0; + out_8761645171275764566[11] = 0; + out_8761645171275764566[12] = 0; + out_8761645171275764566[13] = 0; + out_8761645171275764566[14] = 0; + out_8761645171275764566[15] = 0; + out_8761645171275764566[16] = 0; + out_8761645171275764566[17] = 0; + out_8761645171275764566[18] = 0; + out_8761645171275764566[19] = 0; + out_8761645171275764566[20] = 0; + out_8761645171275764566[21] = 0; + out_8761645171275764566[22] = 0; + out_8761645171275764566[23] = 0; + out_8761645171275764566[24] = 0; + out_8761645171275764566[25] = 1; + out_8761645171275764566[26] = 0; + out_8761645171275764566[27] = 0; + out_8761645171275764566[28] = 0; + out_8761645171275764566[29] = 0; + out_8761645171275764566[30] = 0; + out_8761645171275764566[31] = 0; + out_8761645171275764566[32] = 0; + out_8761645171275764566[33] = 0; + out_8761645171275764566[34] = 0; + out_8761645171275764566[35] = 0; + out_8761645171275764566[36] = 0; + out_8761645171275764566[37] = 0; + out_8761645171275764566[38] = 0; + out_8761645171275764566[39] = 0; + out_8761645171275764566[40] = 0; + out_8761645171275764566[41] = 0; + out_8761645171275764566[42] = 0; + out_8761645171275764566[43] = 0; + out_8761645171275764566[44] = 1; + out_8761645171275764566[45] = 0; + out_8761645171275764566[46] = 0; + out_8761645171275764566[47] = 0; + out_8761645171275764566[48] = 0; + out_8761645171275764566[49] = 0; + out_8761645171275764566[50] = 0; + out_8761645171275764566[51] = 0; + out_8761645171275764566[52] = 0; + out_8761645171275764566[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_6395110133292861521) { - err_fun(nom_x, delta_x, out_6395110133292861521); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1416756098750413745) { + err_fun(nom_x, delta_x, out_1416756098750413745); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5574902989131587335) { - inv_err_fun(nom_x, true_x, out_5574902989131587335); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_288660278795817354) { + inv_err_fun(nom_x, true_x, out_288660278795817354); } -void pose_H_mod_fun(double *state, double *out_6860086072787592800) { - H_mod_fun(state, out_6860086072787592800); +void pose_H_mod_fun(double *state, double *out_8072626393727229290) { + H_mod_fun(state, out_8072626393727229290); } -void pose_f_fun(double *state, double dt, double *out_6157473931416548839) { - f_fun(state, dt, out_6157473931416548839); +void pose_f_fun(double *state, double dt, double *out_3550739231859078198) { + f_fun(state, dt, out_3550739231859078198); } -void pose_F_fun(double *state, double dt, double *out_2563232569638129684) { - F_fun(state, dt, out_2563232569638129684); +void pose_F_fun(double *state, double dt, double *out_4236350099338455825) { + F_fun(state, dt, out_4236350099338455825); } -void pose_h_4(double *state, double *unused, double *out_9057789463521636950) { - h_4(state, unused, out_9057789463521636950); +void pose_h_4(double *state, double *unused, double *out_2031088737794184983) { + h_4(state, unused, out_2031088737794184983); } -void pose_H_4(double *state, double *unused, double *out_6434139615882878081) { - H_4(state, unused, out_6434139615882878081); +void pose_H_4(double *state, double *unused, double *out_6602310470138414754) { + H_4(state, unused, out_6602310470138414754); } -void pose_h_10(double *state, double *unused, double *out_439040734435285181) { - h_10(state, unused, out_439040734435285181); +void pose_h_10(double *state, double *unused, double *out_2254252755933878198) { + h_10(state, unused, out_2254252755933878198); } -void pose_H_10(double *state, double *unused, double *out_1840785913394498883) { - H_10(state, unused, out_1840785913394498883); +void pose_H_10(double *state, double *unused, double *out_4753607725561450752) { + H_10(state, unused, out_4753607725561450752); } -void pose_h_13(double *state, double *unused, double *out_7181492765475252926) { - h_13(state, unused, out_7181492765475252926); +void pose_h_13(double *state, double *unused, double *out_561467872228044768) { + h_13(state, unused, out_561467872228044768); } -void pose_H_13(double *state, double *unused, double *out_3221865790550545280) { - H_13(state, unused, out_3221865790550545280); +void pose_H_13(double *state, double *unused, double *out_3390036644806081953) { + H_13(state, unused, out_3390036644806081953); } -void pose_h_14(double *state, double *unused, double *out_1398557426791028034) { - h_14(state, unused, out_1398557426791028034); +void pose_h_14(double *state, double *unused, double *out_5074055792402957007) { + h_14(state, unused, out_5074055792402957007); } -void pose_H_14(double *state, double *unused, double *out_2470898759543393552) { - H_14(state, unused, out_2470898759543393552); +void pose_H_14(double *state, double *unused, double *out_8761645171275764566) { + H_14(state, unused, out_8761645171275764566); } 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 ba6e3f588..b48c3cbf7 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_6395110133292861521); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5574902989131587335); -void pose_H_mod_fun(double *state, double *out_6860086072787592800); -void pose_f_fun(double *state, double dt, double *out_6157473931416548839); -void pose_F_fun(double *state, double dt, double *out_2563232569638129684); -void pose_h_4(double *state, double *unused, double *out_9057789463521636950); -void pose_H_4(double *state, double *unused, double *out_6434139615882878081); -void pose_h_10(double *state, double *unused, double *out_439040734435285181); -void pose_H_10(double *state, double *unused, double *out_1840785913394498883); -void pose_h_13(double *state, double *unused, double *out_7181492765475252926); -void pose_H_13(double *state, double *unused, double *out_3221865790550545280); -void pose_h_14(double *state, double *unused, double *out_1398557426791028034); -void pose_H_14(double *state, double *unused, double *out_2470898759543393552); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1416756098750413745); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_288660278795817354); +void pose_H_mod_fun(double *state, double *out_8072626393727229290); +void pose_f_fun(double *state, double dt, double *out_3550739231859078198); +void pose_F_fun(double *state, double dt, double *out_4236350099338455825); +void pose_h_4(double *state, double *unused, double *out_2031088737794184983); +void pose_H_4(double *state, double *unused, double *out_6602310470138414754); +void pose_h_10(double *state, double *unused, double *out_2254252755933878198); +void pose_H_10(double *state, double *unused, double *out_4753607725561450752); +void pose_h_13(double *state, double *unused, double *out_561467872228044768); +void pose_H_13(double *state, double *unused, double *out_3390036644806081953); +void pose_h_14(double *state, double *unused, double *out_5074055792402957007); +void pose_H_14(double *state, double *unused, double *out_8761645171275764566); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 83c3311af..6a3d1b4dc 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ