diff --git a/common/libcommon.a b/common/libcommon.a index 79a984d38..acd6bcd5a 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 5e97323a4..95d99f928 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 733fa2710..49f031069 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 95cfa785d..00fe4d62b 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 58bcb21b1..f5179f48c 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 045c92224..53e5e6e5b 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 36ad99c97..5b0e79416 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 5225b82f9..9a46321e3 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 09c1e9cf5..b48442e1a 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 09c1e9cf5..b48442e1a 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 fb11c0434..ff50e65de 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 5225b82f9..9a46321e3 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 1ce3a787e..50b239f78 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-e5cc7460-DEBUG"; +const uint8_t gitversion[19] = "DEV-743ff679-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 1023134df..7da142e85 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 dbd53064c..1dd4a24b5 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 bfc239ed5..7e9286d7b 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 2936260bc..fd07e049b 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 8907aef0e..d8697af59 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 c1c1dae5e..ae226c5c2 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 8047a27da..33229b754 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 744190b3b..7da020d81 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 4ba1eb791..3e2324256 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 59ff4ccfb..626a774c1 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 13e46a3df..d961172b4 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 3d2df1174..f497ddef2 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 be4a4d972..2e2542c20 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 5b7ebbf5f..f54c98011 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 c228035c3..504bb1a5a 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 0e82a2dc6..7c44a8402 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 e0b25f7cf..a264c45a2 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 e4ccbadb9..b3b63ec12 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 f787ea248..5b4c53607 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 f0c8f4a22..be2760c4c 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 7a7d0a5d1..8406a40d4 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-e5cc7460-DEBUG \ No newline at end of file +DEV-743ff679-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 9c144a3ac..26ceedb43 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_2765937094321424100) { - out_2765937094321424100[0] = delta_x[0] + nom_x[0]; - out_2765937094321424100[1] = delta_x[1] + nom_x[1]; - out_2765937094321424100[2] = delta_x[2] + nom_x[2]; - out_2765937094321424100[3] = delta_x[3] + nom_x[3]; - out_2765937094321424100[4] = delta_x[4] + nom_x[4]; - out_2765937094321424100[5] = delta_x[5] + nom_x[5]; - out_2765937094321424100[6] = delta_x[6] + nom_x[6]; - out_2765937094321424100[7] = delta_x[7] + nom_x[7]; - out_2765937094321424100[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_6669412735228971409) { + out_6669412735228971409[0] = delta_x[0] + nom_x[0]; + out_6669412735228971409[1] = delta_x[1] + nom_x[1]; + out_6669412735228971409[2] = delta_x[2] + nom_x[2]; + out_6669412735228971409[3] = delta_x[3] + nom_x[3]; + out_6669412735228971409[4] = delta_x[4] + nom_x[4]; + out_6669412735228971409[5] = delta_x[5] + nom_x[5]; + out_6669412735228971409[6] = delta_x[6] + nom_x[6]; + out_6669412735228971409[7] = delta_x[7] + nom_x[7]; + out_6669412735228971409[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1382255689110899984) { - out_1382255689110899984[0] = -nom_x[0] + true_x[0]; - out_1382255689110899984[1] = -nom_x[1] + true_x[1]; - out_1382255689110899984[2] = -nom_x[2] + true_x[2]; - out_1382255689110899984[3] = -nom_x[3] + true_x[3]; - out_1382255689110899984[4] = -nom_x[4] + true_x[4]; - out_1382255689110899984[5] = -nom_x[5] + true_x[5]; - out_1382255689110899984[6] = -nom_x[6] + true_x[6]; - out_1382255689110899984[7] = -nom_x[7] + true_x[7]; - out_1382255689110899984[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6779967762183071907) { + out_6779967762183071907[0] = -nom_x[0] + true_x[0]; + out_6779967762183071907[1] = -nom_x[1] + true_x[1]; + out_6779967762183071907[2] = -nom_x[2] + true_x[2]; + out_6779967762183071907[3] = -nom_x[3] + true_x[3]; + out_6779967762183071907[4] = -nom_x[4] + true_x[4]; + out_6779967762183071907[5] = -nom_x[5] + true_x[5]; + out_6779967762183071907[6] = -nom_x[6] + true_x[6]; + out_6779967762183071907[7] = -nom_x[7] + true_x[7]; + out_6779967762183071907[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_6175643942212596402) { - out_6175643942212596402[0] = 1.0; - out_6175643942212596402[1] = 0.0; - out_6175643942212596402[2] = 0.0; - out_6175643942212596402[3] = 0.0; - out_6175643942212596402[4] = 0.0; - out_6175643942212596402[5] = 0.0; - out_6175643942212596402[6] = 0.0; - out_6175643942212596402[7] = 0.0; - out_6175643942212596402[8] = 0.0; - out_6175643942212596402[9] = 0.0; - out_6175643942212596402[10] = 1.0; - out_6175643942212596402[11] = 0.0; - out_6175643942212596402[12] = 0.0; - out_6175643942212596402[13] = 0.0; - out_6175643942212596402[14] = 0.0; - out_6175643942212596402[15] = 0.0; - out_6175643942212596402[16] = 0.0; - out_6175643942212596402[17] = 0.0; - out_6175643942212596402[18] = 0.0; - out_6175643942212596402[19] = 0.0; - out_6175643942212596402[20] = 1.0; - out_6175643942212596402[21] = 0.0; - out_6175643942212596402[22] = 0.0; - out_6175643942212596402[23] = 0.0; - out_6175643942212596402[24] = 0.0; - out_6175643942212596402[25] = 0.0; - out_6175643942212596402[26] = 0.0; - out_6175643942212596402[27] = 0.0; - out_6175643942212596402[28] = 0.0; - out_6175643942212596402[29] = 0.0; - out_6175643942212596402[30] = 1.0; - out_6175643942212596402[31] = 0.0; - out_6175643942212596402[32] = 0.0; - out_6175643942212596402[33] = 0.0; - out_6175643942212596402[34] = 0.0; - out_6175643942212596402[35] = 0.0; - out_6175643942212596402[36] = 0.0; - out_6175643942212596402[37] = 0.0; - out_6175643942212596402[38] = 0.0; - out_6175643942212596402[39] = 0.0; - out_6175643942212596402[40] = 1.0; - out_6175643942212596402[41] = 0.0; - out_6175643942212596402[42] = 0.0; - out_6175643942212596402[43] = 0.0; - out_6175643942212596402[44] = 0.0; - out_6175643942212596402[45] = 0.0; - out_6175643942212596402[46] = 0.0; - out_6175643942212596402[47] = 0.0; - out_6175643942212596402[48] = 0.0; - out_6175643942212596402[49] = 0.0; - out_6175643942212596402[50] = 1.0; - out_6175643942212596402[51] = 0.0; - out_6175643942212596402[52] = 0.0; - out_6175643942212596402[53] = 0.0; - out_6175643942212596402[54] = 0.0; - out_6175643942212596402[55] = 0.0; - out_6175643942212596402[56] = 0.0; - out_6175643942212596402[57] = 0.0; - out_6175643942212596402[58] = 0.0; - out_6175643942212596402[59] = 0.0; - out_6175643942212596402[60] = 1.0; - out_6175643942212596402[61] = 0.0; - out_6175643942212596402[62] = 0.0; - out_6175643942212596402[63] = 0.0; - out_6175643942212596402[64] = 0.0; - out_6175643942212596402[65] = 0.0; - out_6175643942212596402[66] = 0.0; - out_6175643942212596402[67] = 0.0; - out_6175643942212596402[68] = 0.0; - out_6175643942212596402[69] = 0.0; - out_6175643942212596402[70] = 1.0; - out_6175643942212596402[71] = 0.0; - out_6175643942212596402[72] = 0.0; - out_6175643942212596402[73] = 0.0; - out_6175643942212596402[74] = 0.0; - out_6175643942212596402[75] = 0.0; - out_6175643942212596402[76] = 0.0; - out_6175643942212596402[77] = 0.0; - out_6175643942212596402[78] = 0.0; - out_6175643942212596402[79] = 0.0; - out_6175643942212596402[80] = 1.0; +void H_mod_fun(double *state, double *out_6491699675187271682) { + out_6491699675187271682[0] = 1.0; + out_6491699675187271682[1] = 0.0; + out_6491699675187271682[2] = 0.0; + out_6491699675187271682[3] = 0.0; + out_6491699675187271682[4] = 0.0; + out_6491699675187271682[5] = 0.0; + out_6491699675187271682[6] = 0.0; + out_6491699675187271682[7] = 0.0; + out_6491699675187271682[8] = 0.0; + out_6491699675187271682[9] = 0.0; + out_6491699675187271682[10] = 1.0; + out_6491699675187271682[11] = 0.0; + out_6491699675187271682[12] = 0.0; + out_6491699675187271682[13] = 0.0; + out_6491699675187271682[14] = 0.0; + out_6491699675187271682[15] = 0.0; + out_6491699675187271682[16] = 0.0; + out_6491699675187271682[17] = 0.0; + out_6491699675187271682[18] = 0.0; + out_6491699675187271682[19] = 0.0; + out_6491699675187271682[20] = 1.0; + out_6491699675187271682[21] = 0.0; + out_6491699675187271682[22] = 0.0; + out_6491699675187271682[23] = 0.0; + out_6491699675187271682[24] = 0.0; + out_6491699675187271682[25] = 0.0; + out_6491699675187271682[26] = 0.0; + out_6491699675187271682[27] = 0.0; + out_6491699675187271682[28] = 0.0; + out_6491699675187271682[29] = 0.0; + out_6491699675187271682[30] = 1.0; + out_6491699675187271682[31] = 0.0; + out_6491699675187271682[32] = 0.0; + out_6491699675187271682[33] = 0.0; + out_6491699675187271682[34] = 0.0; + out_6491699675187271682[35] = 0.0; + out_6491699675187271682[36] = 0.0; + out_6491699675187271682[37] = 0.0; + out_6491699675187271682[38] = 0.0; + out_6491699675187271682[39] = 0.0; + out_6491699675187271682[40] = 1.0; + out_6491699675187271682[41] = 0.0; + out_6491699675187271682[42] = 0.0; + out_6491699675187271682[43] = 0.0; + out_6491699675187271682[44] = 0.0; + out_6491699675187271682[45] = 0.0; + out_6491699675187271682[46] = 0.0; + out_6491699675187271682[47] = 0.0; + out_6491699675187271682[48] = 0.0; + out_6491699675187271682[49] = 0.0; + out_6491699675187271682[50] = 1.0; + out_6491699675187271682[51] = 0.0; + out_6491699675187271682[52] = 0.0; + out_6491699675187271682[53] = 0.0; + out_6491699675187271682[54] = 0.0; + out_6491699675187271682[55] = 0.0; + out_6491699675187271682[56] = 0.0; + out_6491699675187271682[57] = 0.0; + out_6491699675187271682[58] = 0.0; + out_6491699675187271682[59] = 0.0; + out_6491699675187271682[60] = 1.0; + out_6491699675187271682[61] = 0.0; + out_6491699675187271682[62] = 0.0; + out_6491699675187271682[63] = 0.0; + out_6491699675187271682[64] = 0.0; + out_6491699675187271682[65] = 0.0; + out_6491699675187271682[66] = 0.0; + out_6491699675187271682[67] = 0.0; + out_6491699675187271682[68] = 0.0; + out_6491699675187271682[69] = 0.0; + out_6491699675187271682[70] = 1.0; + out_6491699675187271682[71] = 0.0; + out_6491699675187271682[72] = 0.0; + out_6491699675187271682[73] = 0.0; + out_6491699675187271682[74] = 0.0; + out_6491699675187271682[75] = 0.0; + out_6491699675187271682[76] = 0.0; + out_6491699675187271682[77] = 0.0; + out_6491699675187271682[78] = 0.0; + out_6491699675187271682[79] = 0.0; + out_6491699675187271682[80] = 1.0; } -void f_fun(double *state, double dt, double *out_2437881867604815540) { - out_2437881867604815540[0] = state[0]; - out_2437881867604815540[1] = state[1]; - out_2437881867604815540[2] = state[2]; - out_2437881867604815540[3] = state[3]; - out_2437881867604815540[4] = state[4]; - out_2437881867604815540[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_2437881867604815540[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_2437881867604815540[7] = state[7]; - out_2437881867604815540[8] = state[8]; +void f_fun(double *state, double dt, double *out_2858314681790673269) { + out_2858314681790673269[0] = state[0]; + out_2858314681790673269[1] = state[1]; + out_2858314681790673269[2] = state[2]; + out_2858314681790673269[3] = state[3]; + out_2858314681790673269[4] = state[4]; + out_2858314681790673269[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_2858314681790673269[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_2858314681790673269[7] = state[7]; + out_2858314681790673269[8] = state[8]; } -void F_fun(double *state, double dt, double *out_3645925778664752399) { - out_3645925778664752399[0] = 1; - out_3645925778664752399[1] = 0; - out_3645925778664752399[2] = 0; - out_3645925778664752399[3] = 0; - out_3645925778664752399[4] = 0; - out_3645925778664752399[5] = 0; - out_3645925778664752399[6] = 0; - out_3645925778664752399[7] = 0; - out_3645925778664752399[8] = 0; - out_3645925778664752399[9] = 0; - out_3645925778664752399[10] = 1; - out_3645925778664752399[11] = 0; - out_3645925778664752399[12] = 0; - out_3645925778664752399[13] = 0; - out_3645925778664752399[14] = 0; - out_3645925778664752399[15] = 0; - out_3645925778664752399[16] = 0; - out_3645925778664752399[17] = 0; - out_3645925778664752399[18] = 0; - out_3645925778664752399[19] = 0; - out_3645925778664752399[20] = 1; - out_3645925778664752399[21] = 0; - out_3645925778664752399[22] = 0; - out_3645925778664752399[23] = 0; - out_3645925778664752399[24] = 0; - out_3645925778664752399[25] = 0; - out_3645925778664752399[26] = 0; - out_3645925778664752399[27] = 0; - out_3645925778664752399[28] = 0; - out_3645925778664752399[29] = 0; - out_3645925778664752399[30] = 1; - out_3645925778664752399[31] = 0; - out_3645925778664752399[32] = 0; - out_3645925778664752399[33] = 0; - out_3645925778664752399[34] = 0; - out_3645925778664752399[35] = 0; - out_3645925778664752399[36] = 0; - out_3645925778664752399[37] = 0; - out_3645925778664752399[38] = 0; - out_3645925778664752399[39] = 0; - out_3645925778664752399[40] = 1; - out_3645925778664752399[41] = 0; - out_3645925778664752399[42] = 0; - out_3645925778664752399[43] = 0; - out_3645925778664752399[44] = 0; - out_3645925778664752399[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_3645925778664752399[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_3645925778664752399[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3645925778664752399[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3645925778664752399[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_3645925778664752399[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_3645925778664752399[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_3645925778664752399[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_3645925778664752399[53] = -9.8100000000000005*dt; - out_3645925778664752399[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_3645925778664752399[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_3645925778664752399[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3645925778664752399[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3645925778664752399[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_3645925778664752399[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_3645925778664752399[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_3645925778664752399[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3645925778664752399[62] = 0; - out_3645925778664752399[63] = 0; - out_3645925778664752399[64] = 0; - out_3645925778664752399[65] = 0; - out_3645925778664752399[66] = 0; - out_3645925778664752399[67] = 0; - out_3645925778664752399[68] = 0; - out_3645925778664752399[69] = 0; - out_3645925778664752399[70] = 1; - out_3645925778664752399[71] = 0; - out_3645925778664752399[72] = 0; - out_3645925778664752399[73] = 0; - out_3645925778664752399[74] = 0; - out_3645925778664752399[75] = 0; - out_3645925778664752399[76] = 0; - out_3645925778664752399[77] = 0; - out_3645925778664752399[78] = 0; - out_3645925778664752399[79] = 0; - out_3645925778664752399[80] = 1; +void F_fun(double *state, double dt, double *out_1519249616957804461) { + out_1519249616957804461[0] = 1; + out_1519249616957804461[1] = 0; + out_1519249616957804461[2] = 0; + out_1519249616957804461[3] = 0; + out_1519249616957804461[4] = 0; + out_1519249616957804461[5] = 0; + out_1519249616957804461[6] = 0; + out_1519249616957804461[7] = 0; + out_1519249616957804461[8] = 0; + out_1519249616957804461[9] = 0; + out_1519249616957804461[10] = 1; + out_1519249616957804461[11] = 0; + out_1519249616957804461[12] = 0; + out_1519249616957804461[13] = 0; + out_1519249616957804461[14] = 0; + out_1519249616957804461[15] = 0; + out_1519249616957804461[16] = 0; + out_1519249616957804461[17] = 0; + out_1519249616957804461[18] = 0; + out_1519249616957804461[19] = 0; + out_1519249616957804461[20] = 1; + out_1519249616957804461[21] = 0; + out_1519249616957804461[22] = 0; + out_1519249616957804461[23] = 0; + out_1519249616957804461[24] = 0; + out_1519249616957804461[25] = 0; + out_1519249616957804461[26] = 0; + out_1519249616957804461[27] = 0; + out_1519249616957804461[28] = 0; + out_1519249616957804461[29] = 0; + out_1519249616957804461[30] = 1; + out_1519249616957804461[31] = 0; + out_1519249616957804461[32] = 0; + out_1519249616957804461[33] = 0; + out_1519249616957804461[34] = 0; + out_1519249616957804461[35] = 0; + out_1519249616957804461[36] = 0; + out_1519249616957804461[37] = 0; + out_1519249616957804461[38] = 0; + out_1519249616957804461[39] = 0; + out_1519249616957804461[40] = 1; + out_1519249616957804461[41] = 0; + out_1519249616957804461[42] = 0; + out_1519249616957804461[43] = 0; + out_1519249616957804461[44] = 0; + out_1519249616957804461[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_1519249616957804461[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_1519249616957804461[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_1519249616957804461[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_1519249616957804461[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_1519249616957804461[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_1519249616957804461[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_1519249616957804461[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_1519249616957804461[53] = -9.8100000000000005*dt; + out_1519249616957804461[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_1519249616957804461[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_1519249616957804461[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1519249616957804461[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1519249616957804461[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_1519249616957804461[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_1519249616957804461[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_1519249616957804461[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1519249616957804461[62] = 0; + out_1519249616957804461[63] = 0; + out_1519249616957804461[64] = 0; + out_1519249616957804461[65] = 0; + out_1519249616957804461[66] = 0; + out_1519249616957804461[67] = 0; + out_1519249616957804461[68] = 0; + out_1519249616957804461[69] = 0; + out_1519249616957804461[70] = 1; + out_1519249616957804461[71] = 0; + out_1519249616957804461[72] = 0; + out_1519249616957804461[73] = 0; + out_1519249616957804461[74] = 0; + out_1519249616957804461[75] = 0; + out_1519249616957804461[76] = 0; + out_1519249616957804461[77] = 0; + out_1519249616957804461[78] = 0; + out_1519249616957804461[79] = 0; + out_1519249616957804461[80] = 1; } -void h_25(double *state, double *unused, double *out_1347552076965832871) { - out_1347552076965832871[0] = state[6]; +void h_25(double *state, double *unused, double *out_7032479070992890953) { + out_7032479070992890953[0] = state[6]; } -void H_25(double *state, double *unused, double *out_6035190491488835800) { - out_6035190491488835800[0] = 0; - out_6035190491488835800[1] = 0; - out_6035190491488835800[2] = 0; - out_6035190491488835800[3] = 0; - out_6035190491488835800[4] = 0; - out_6035190491488835800[5] = 0; - out_6035190491488835800[6] = 1; - out_6035190491488835800[7] = 0; - out_6035190491488835800[8] = 0; +void H_25(double *state, double *unused, double *out_2968651036541906534) { + out_2968651036541906534[0] = 0; + out_2968651036541906534[1] = 0; + out_2968651036541906534[2] = 0; + out_2968651036541906534[3] = 0; + out_2968651036541906534[4] = 0; + out_2968651036541906534[5] = 0; + out_2968651036541906534[6] = 1; + out_2968651036541906534[7] = 0; + out_2968651036541906534[8] = 0; } -void h_24(double *state, double *unused, double *out_7176999251413506765) { - out_7176999251413506765[0] = state[4]; - out_7176999251413506765[1] = state[5]; +void h_24(double *state, double *unused, double *out_5672934374792452147) { + out_5672934374792452147[0] = state[4]; + out_5672934374792452147[1] = state[5]; } -void H_24(double *state, double *unused, double *out_6210520902369554390) { - out_6210520902369554390[0] = 0; - out_6210520902369554390[1] = 0; - out_6210520902369554390[2] = 0; - out_6210520902369554390[3] = 0; - out_6210520902369554390[4] = 1; - out_6210520902369554390[5] = 0; - out_6210520902369554390[6] = 0; - out_6210520902369554390[7] = 0; - out_6210520902369554390[8] = 0; - out_6210520902369554390[9] = 0; - out_6210520902369554390[10] = 0; - out_6210520902369554390[11] = 0; - out_6210520902369554390[12] = 0; - out_6210520902369554390[13] = 0; - out_6210520902369554390[14] = 1; - out_6210520902369554390[15] = 0; - out_6210520902369554390[16] = 0; - out_6210520902369554390[17] = 0; +void H_24(double *state, double *unused, double *out_5194358820520775096) { + out_5194358820520775096[0] = 0; + out_5194358820520775096[1] = 0; + out_5194358820520775096[2] = 0; + out_5194358820520775096[3] = 0; + out_5194358820520775096[4] = 1; + out_5194358820520775096[5] = 0; + out_5194358820520775096[6] = 0; + out_5194358820520775096[7] = 0; + out_5194358820520775096[8] = 0; + out_5194358820520775096[9] = 0; + out_5194358820520775096[10] = 0; + out_5194358820520775096[11] = 0; + out_5194358820520775096[12] = 0; + out_5194358820520775096[13] = 0; + out_5194358820520775096[14] = 1; + out_5194358820520775096[15] = 0; + out_5194358820520775096[16] = 0; + out_5194358820520775096[17] = 0; } -void h_30(double *state, double *unused, double *out_2798183469594716038) { - out_2798183469594716038[0] = state[4]; +void h_30(double *state, double *unused, double *out_6875292402641761808) { + out_6875292402641761808[0] = state[4]; } -void H_30(double *state, double *unused, double *out_881499850002780955) { - out_881499850002780955[0] = 0; - out_881499850002780955[1] = 0; - out_881499850002780955[2] = 0; - out_881499850002780955[3] = 0; - out_881499850002780955[4] = 1; - out_881499850002780955[5] = 0; - out_881499850002780955[6] = 0; - out_881499850002780955[7] = 0; - out_881499850002780955[8] = 0; +void H_30(double *state, double *unused, double *out_8561402695676028327) { + out_8561402695676028327[0] = 0; + out_8561402695676028327[1] = 0; + out_8561402695676028327[2] = 0; + out_8561402695676028327[3] = 0; + out_8561402695676028327[4] = 1; + out_8561402695676028327[5] = 0; + out_8561402695676028327[6] = 0; + out_8561402695676028327[7] = 0; + out_8561402695676028327[8] = 0; } -void h_26(double *state, double *unused, double *out_7428563492211452550) { - out_7428563492211452550[0] = state[7]; +void h_26(double *state, double *unused, double *out_5172410153653940662) { + out_5172410153653940662[0] = state[7]; } -void H_26(double *state, double *unused, double *out_8670050263346659592) { - out_8670050263346659592[0] = 0; - out_8670050263346659592[1] = 0; - out_8670050263346659592[2] = 0; - out_8670050263346659592[3] = 0; - out_8670050263346659592[4] = 0; - out_8670050263346659592[5] = 0; - out_8670050263346659592[6] = 0; - out_8670050263346659592[7] = 1; - out_8670050263346659592[8] = 0; +void H_26(double *state, double *unused, double *out_6273177006302707135) { + out_6273177006302707135[0] = 0; + out_6273177006302707135[1] = 0; + out_6273177006302707135[2] = 0; + out_6273177006302707135[3] = 0; + out_6273177006302707135[4] = 0; + out_6273177006302707135[5] = 0; + out_6273177006302707135[6] = 0; + out_6273177006302707135[7] = 1; + out_6273177006302707135[8] = 0; } -void h_27(double *state, double *unused, double *out_3654121190695012774) { - out_3654121190695012774[0] = state[3]; +void h_27(double *state, double *unused, double *out_402664363435809860) { + out_402664363435809860[0] = state[3]; } -void H_27(double *state, double *unused, double *out_8339292750432500781) { - out_8339292750432500781[0] = 0; - out_8339292750432500781[1] = 0; - out_8339292750432500781[2] = 0; - out_8339292750432500781[3] = 1; - out_8339292750432500781[4] = 0; - out_8339292750432500781[5] = 0; - out_8339292750432500781[6] = 0; - out_8339292750432500781[7] = 0; - out_8339292750432500781[8] = 0; +void H_27(double *state, double *unused, double *out_7710578066233098378) { + out_7710578066233098378[0] = 0; + out_7710578066233098378[1] = 0; + out_7710578066233098378[2] = 0; + out_7710578066233098378[3] = 1; + out_7710578066233098378[4] = 0; + out_7710578066233098378[5] = 0; + out_7710578066233098378[6] = 0; + out_7710578066233098378[7] = 0; + out_7710578066233098378[8] = 0; } -void h_29(double *state, double *unused, double *out_2203489798066129607) { - out_2203489798066129607[0] = state[1]; +void h_29(double *state, double *unused, double *out_7173499589786160796) { + out_7173499589786160796[0] = state[1]; } -void H_29(double *state, double *unused, double *out_5654298094317683686) { - out_5654298094317683686[0] = 0; - out_5654298094317683686[1] = 1; - out_5654298094317683686[2] = 0; - out_5654298094317683686[3] = 0; - out_5654298094317683686[4] = 0; - out_5654298094317683686[5] = 0; - out_5654298094317683686[6] = 0; - out_5654298094317683686[7] = 0; - out_5654298094317683686[8] = 0; +void H_29(double *state, double *unused, double *out_8051171351361636143) { + out_8051171351361636143[0] = 0; + out_8051171351361636143[1] = 1; + out_8051171351361636143[2] = 0; + out_8051171351361636143[3] = 0; + out_8051171351361636143[4] = 0; + out_8051171351361636143[5] = 0; + out_8051171351361636143[6] = 0; + out_8051171351361636143[7] = 0; + out_8051171351361636143[8] = 0; } -void h_28(double *state, double *unused, double *out_3781022243733688039) { - out_3781022243733688039[0] = state[0]; +void h_28(double *state, double *unused, double *out_1346174216337472295) { + out_1346174216337472295[0] = state[0]; } -void H_28(double *state, double *unused, double *out_3690667822752357435) { - out_3690667822752357435[0] = 1; - out_3690667822752357435[1] = 0; - out_3690667822752357435[2] = 0; - out_3690667822752357435[3] = 0; - out_3690667822752357435[4] = 0; - out_3690667822752357435[5] = 0; - out_3690667822752357435[6] = 0; - out_3690667822752357435[7] = 0; - out_3690667822752357435[8] = 0; +void H_28(double *state, double *unused, double *out_5313173705278384899) { + out_5313173705278384899[0] = 1; + out_5313173705278384899[1] = 0; + out_5313173705278384899[2] = 0; + out_5313173705278384899[3] = 0; + out_5313173705278384899[4] = 0; + out_5313173705278384899[5] = 0; + out_5313173705278384899[6] = 0; + out_5313173705278384899[7] = 0; + out_5313173705278384899[8] = 0; } -void h_31(double *state, double *unused, double *out_5423283149384518065) { - out_5423283149384518065[0] = state[8]; +void h_31(double *state, double *unused, double *out_6757285008708385064) { + out_6757285008708385064[0] = state[8]; } -void H_31(double *state, double *unused, double *out_6004544529611875372) { - out_6004544529611875372[0] = 0; - out_6004544529611875372[1] = 0; - out_6004544529611875372[2] = 0; - out_6004544529611875372[3] = 0; - out_6004544529611875372[4] = 0; - out_6004544529611875372[5] = 0; - out_6004544529611875372[6] = 0; - out_6004544529611875372[7] = 0; - out_6004544529611875372[8] = 1; +void H_31(double *state, double *unused, double *out_2999296998418866962) { + out_2999296998418866962[0] = 0; + out_2999296998418866962[1] = 0; + out_2999296998418866962[2] = 0; + out_2999296998418866962[3] = 0; + out_2999296998418866962[4] = 0; + out_2999296998418866962[5] = 0; + out_2999296998418866962[6] = 0; + out_2999296998418866962[7] = 0; + out_2999296998418866962[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_2765937094321424100) { - err_fun(nom_x, delta_x, out_2765937094321424100); +void car_err_fun(double *nom_x, double *delta_x, double *out_6669412735228971409) { + err_fun(nom_x, delta_x, out_6669412735228971409); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1382255689110899984) { - inv_err_fun(nom_x, true_x, out_1382255689110899984); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_6779967762183071907) { + inv_err_fun(nom_x, true_x, out_6779967762183071907); } -void car_H_mod_fun(double *state, double *out_6175643942212596402) { - H_mod_fun(state, out_6175643942212596402); +void car_H_mod_fun(double *state, double *out_6491699675187271682) { + H_mod_fun(state, out_6491699675187271682); } -void car_f_fun(double *state, double dt, double *out_2437881867604815540) { - f_fun(state, dt, out_2437881867604815540); +void car_f_fun(double *state, double dt, double *out_2858314681790673269) { + f_fun(state, dt, out_2858314681790673269); } -void car_F_fun(double *state, double dt, double *out_3645925778664752399) { - F_fun(state, dt, out_3645925778664752399); +void car_F_fun(double *state, double dt, double *out_1519249616957804461) { + F_fun(state, dt, out_1519249616957804461); } -void car_h_25(double *state, double *unused, double *out_1347552076965832871) { - h_25(state, unused, out_1347552076965832871); +void car_h_25(double *state, double *unused, double *out_7032479070992890953) { + h_25(state, unused, out_7032479070992890953); } -void car_H_25(double *state, double *unused, double *out_6035190491488835800) { - H_25(state, unused, out_6035190491488835800); +void car_H_25(double *state, double *unused, double *out_2968651036541906534) { + H_25(state, unused, out_2968651036541906534); } -void car_h_24(double *state, double *unused, double *out_7176999251413506765) { - h_24(state, unused, out_7176999251413506765); +void car_h_24(double *state, double *unused, double *out_5672934374792452147) { + h_24(state, unused, out_5672934374792452147); } -void car_H_24(double *state, double *unused, double *out_6210520902369554390) { - H_24(state, unused, out_6210520902369554390); +void car_H_24(double *state, double *unused, double *out_5194358820520775096) { + H_24(state, unused, out_5194358820520775096); } -void car_h_30(double *state, double *unused, double *out_2798183469594716038) { - h_30(state, unused, out_2798183469594716038); +void car_h_30(double *state, double *unused, double *out_6875292402641761808) { + h_30(state, unused, out_6875292402641761808); } -void car_H_30(double *state, double *unused, double *out_881499850002780955) { - H_30(state, unused, out_881499850002780955); +void car_H_30(double *state, double *unused, double *out_8561402695676028327) { + H_30(state, unused, out_8561402695676028327); } -void car_h_26(double *state, double *unused, double *out_7428563492211452550) { - h_26(state, unused, out_7428563492211452550); +void car_h_26(double *state, double *unused, double *out_5172410153653940662) { + h_26(state, unused, out_5172410153653940662); } -void car_H_26(double *state, double *unused, double *out_8670050263346659592) { - H_26(state, unused, out_8670050263346659592); +void car_H_26(double *state, double *unused, double *out_6273177006302707135) { + H_26(state, unused, out_6273177006302707135); } -void car_h_27(double *state, double *unused, double *out_3654121190695012774) { - h_27(state, unused, out_3654121190695012774); +void car_h_27(double *state, double *unused, double *out_402664363435809860) { + h_27(state, unused, out_402664363435809860); } -void car_H_27(double *state, double *unused, double *out_8339292750432500781) { - H_27(state, unused, out_8339292750432500781); +void car_H_27(double *state, double *unused, double *out_7710578066233098378) { + H_27(state, unused, out_7710578066233098378); } -void car_h_29(double *state, double *unused, double *out_2203489798066129607) { - h_29(state, unused, out_2203489798066129607); +void car_h_29(double *state, double *unused, double *out_7173499589786160796) { + h_29(state, unused, out_7173499589786160796); } -void car_H_29(double *state, double *unused, double *out_5654298094317683686) { - H_29(state, unused, out_5654298094317683686); +void car_H_29(double *state, double *unused, double *out_8051171351361636143) { + H_29(state, unused, out_8051171351361636143); } -void car_h_28(double *state, double *unused, double *out_3781022243733688039) { - h_28(state, unused, out_3781022243733688039); +void car_h_28(double *state, double *unused, double *out_1346174216337472295) { + h_28(state, unused, out_1346174216337472295); } -void car_H_28(double *state, double *unused, double *out_3690667822752357435) { - H_28(state, unused, out_3690667822752357435); +void car_H_28(double *state, double *unused, double *out_5313173705278384899) { + H_28(state, unused, out_5313173705278384899); } -void car_h_31(double *state, double *unused, double *out_5423283149384518065) { - h_31(state, unused, out_5423283149384518065); +void car_h_31(double *state, double *unused, double *out_6757285008708385064) { + h_31(state, unused, out_6757285008708385064); } -void car_H_31(double *state, double *unused, double *out_6004544529611875372) { - H_31(state, unused, out_6004544529611875372); +void car_H_31(double *state, double *unused, double *out_2999296998418866962) { + H_31(state, unused, out_2999296998418866962); } 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 627c90f74..16e5a8d50 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_2765937094321424100); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1382255689110899984); -void car_H_mod_fun(double *state, double *out_6175643942212596402); -void car_f_fun(double *state, double dt, double *out_2437881867604815540); -void car_F_fun(double *state, double dt, double *out_3645925778664752399); -void car_h_25(double *state, double *unused, double *out_1347552076965832871); -void car_H_25(double *state, double *unused, double *out_6035190491488835800); -void car_h_24(double *state, double *unused, double *out_7176999251413506765); -void car_H_24(double *state, double *unused, double *out_6210520902369554390); -void car_h_30(double *state, double *unused, double *out_2798183469594716038); -void car_H_30(double *state, double *unused, double *out_881499850002780955); -void car_h_26(double *state, double *unused, double *out_7428563492211452550); -void car_H_26(double *state, double *unused, double *out_8670050263346659592); -void car_h_27(double *state, double *unused, double *out_3654121190695012774); -void car_H_27(double *state, double *unused, double *out_8339292750432500781); -void car_h_29(double *state, double *unused, double *out_2203489798066129607); -void car_H_29(double *state, double *unused, double *out_5654298094317683686); -void car_h_28(double *state, double *unused, double *out_3781022243733688039); -void car_H_28(double *state, double *unused, double *out_3690667822752357435); -void car_h_31(double *state, double *unused, double *out_5423283149384518065); -void car_H_31(double *state, double *unused, double *out_6004544529611875372); +void car_err_fun(double *nom_x, double *delta_x, double *out_6669412735228971409); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_6779967762183071907); +void car_H_mod_fun(double *state, double *out_6491699675187271682); +void car_f_fun(double *state, double dt, double *out_2858314681790673269); +void car_F_fun(double *state, double dt, double *out_1519249616957804461); +void car_h_25(double *state, double *unused, double *out_7032479070992890953); +void car_H_25(double *state, double *unused, double *out_2968651036541906534); +void car_h_24(double *state, double *unused, double *out_5672934374792452147); +void car_H_24(double *state, double *unused, double *out_5194358820520775096); +void car_h_30(double *state, double *unused, double *out_6875292402641761808); +void car_H_30(double *state, double *unused, double *out_8561402695676028327); +void car_h_26(double *state, double *unused, double *out_5172410153653940662); +void car_H_26(double *state, double *unused, double *out_6273177006302707135); +void car_h_27(double *state, double *unused, double *out_402664363435809860); +void car_H_27(double *state, double *unused, double *out_7710578066233098378); +void car_h_29(double *state, double *unused, double *out_7173499589786160796); +void car_H_29(double *state, double *unused, double *out_8051171351361636143); +void car_h_28(double *state, double *unused, double *out_1346174216337472295); +void car_H_28(double *state, double *unused, double *out_5313173705278384899); +void car_h_31(double *state, double *unused, double *out_6757285008708385064); +void car_H_31(double *state, double *unused, double *out_2999296998418866962); 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 1366d16bb..25c7df537 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_2822233615381903085) { - out_2822233615381903085[0] = delta_x[0] + nom_x[0]; - out_2822233615381903085[1] = delta_x[1] + nom_x[1]; - out_2822233615381903085[2] = delta_x[2] + nom_x[2]; - out_2822233615381903085[3] = delta_x[3] + nom_x[3]; - out_2822233615381903085[4] = delta_x[4] + nom_x[4]; - out_2822233615381903085[5] = delta_x[5] + nom_x[5]; - out_2822233615381903085[6] = delta_x[6] + nom_x[6]; - out_2822233615381903085[7] = delta_x[7] + nom_x[7]; - out_2822233615381903085[8] = delta_x[8] + nom_x[8]; - out_2822233615381903085[9] = delta_x[9] + nom_x[9]; - out_2822233615381903085[10] = delta_x[10] + nom_x[10]; - out_2822233615381903085[11] = delta_x[11] + nom_x[11]; - out_2822233615381903085[12] = delta_x[12] + nom_x[12]; - out_2822233615381903085[13] = delta_x[13] + nom_x[13]; - out_2822233615381903085[14] = delta_x[14] + nom_x[14]; - out_2822233615381903085[15] = delta_x[15] + nom_x[15]; - out_2822233615381903085[16] = delta_x[16] + nom_x[16]; - out_2822233615381903085[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_609976965150865312) { + out_609976965150865312[0] = delta_x[0] + nom_x[0]; + out_609976965150865312[1] = delta_x[1] + nom_x[1]; + out_609976965150865312[2] = delta_x[2] + nom_x[2]; + out_609976965150865312[3] = delta_x[3] + nom_x[3]; + out_609976965150865312[4] = delta_x[4] + nom_x[4]; + out_609976965150865312[5] = delta_x[5] + nom_x[5]; + out_609976965150865312[6] = delta_x[6] + nom_x[6]; + out_609976965150865312[7] = delta_x[7] + nom_x[7]; + out_609976965150865312[8] = delta_x[8] + nom_x[8]; + out_609976965150865312[9] = delta_x[9] + nom_x[9]; + out_609976965150865312[10] = delta_x[10] + nom_x[10]; + out_609976965150865312[11] = delta_x[11] + nom_x[11]; + out_609976965150865312[12] = delta_x[12] + nom_x[12]; + out_609976965150865312[13] = delta_x[13] + nom_x[13]; + out_609976965150865312[14] = delta_x[14] + nom_x[14]; + out_609976965150865312[15] = delta_x[15] + nom_x[15]; + out_609976965150865312[16] = delta_x[16] + nom_x[16]; + out_609976965150865312[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3556797172859207138) { - out_3556797172859207138[0] = -nom_x[0] + true_x[0]; - out_3556797172859207138[1] = -nom_x[1] + true_x[1]; - out_3556797172859207138[2] = -nom_x[2] + true_x[2]; - out_3556797172859207138[3] = -nom_x[3] + true_x[3]; - out_3556797172859207138[4] = -nom_x[4] + true_x[4]; - out_3556797172859207138[5] = -nom_x[5] + true_x[5]; - out_3556797172859207138[6] = -nom_x[6] + true_x[6]; - out_3556797172859207138[7] = -nom_x[7] + true_x[7]; - out_3556797172859207138[8] = -nom_x[8] + true_x[8]; - out_3556797172859207138[9] = -nom_x[9] + true_x[9]; - out_3556797172859207138[10] = -nom_x[10] + true_x[10]; - out_3556797172859207138[11] = -nom_x[11] + true_x[11]; - out_3556797172859207138[12] = -nom_x[12] + true_x[12]; - out_3556797172859207138[13] = -nom_x[13] + true_x[13]; - out_3556797172859207138[14] = -nom_x[14] + true_x[14]; - out_3556797172859207138[15] = -nom_x[15] + true_x[15]; - out_3556797172859207138[16] = -nom_x[16] + true_x[16]; - out_3556797172859207138[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8281732551506992950) { + out_8281732551506992950[0] = -nom_x[0] + true_x[0]; + out_8281732551506992950[1] = -nom_x[1] + true_x[1]; + out_8281732551506992950[2] = -nom_x[2] + true_x[2]; + out_8281732551506992950[3] = -nom_x[3] + true_x[3]; + out_8281732551506992950[4] = -nom_x[4] + true_x[4]; + out_8281732551506992950[5] = -nom_x[5] + true_x[5]; + out_8281732551506992950[6] = -nom_x[6] + true_x[6]; + out_8281732551506992950[7] = -nom_x[7] + true_x[7]; + out_8281732551506992950[8] = -nom_x[8] + true_x[8]; + out_8281732551506992950[9] = -nom_x[9] + true_x[9]; + out_8281732551506992950[10] = -nom_x[10] + true_x[10]; + out_8281732551506992950[11] = -nom_x[11] + true_x[11]; + out_8281732551506992950[12] = -nom_x[12] + true_x[12]; + out_8281732551506992950[13] = -nom_x[13] + true_x[13]; + out_8281732551506992950[14] = -nom_x[14] + true_x[14]; + out_8281732551506992950[15] = -nom_x[15] + true_x[15]; + out_8281732551506992950[16] = -nom_x[16] + true_x[16]; + out_8281732551506992950[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_4158952900173024917) { - out_4158952900173024917[0] = 1.0; - out_4158952900173024917[1] = 0.0; - out_4158952900173024917[2] = 0.0; - out_4158952900173024917[3] = 0.0; - out_4158952900173024917[4] = 0.0; - out_4158952900173024917[5] = 0.0; - out_4158952900173024917[6] = 0.0; - out_4158952900173024917[7] = 0.0; - out_4158952900173024917[8] = 0.0; - out_4158952900173024917[9] = 0.0; - out_4158952900173024917[10] = 0.0; - out_4158952900173024917[11] = 0.0; - out_4158952900173024917[12] = 0.0; - out_4158952900173024917[13] = 0.0; - out_4158952900173024917[14] = 0.0; - out_4158952900173024917[15] = 0.0; - out_4158952900173024917[16] = 0.0; - out_4158952900173024917[17] = 0.0; - out_4158952900173024917[18] = 0.0; - out_4158952900173024917[19] = 1.0; - out_4158952900173024917[20] = 0.0; - out_4158952900173024917[21] = 0.0; - out_4158952900173024917[22] = 0.0; - out_4158952900173024917[23] = 0.0; - out_4158952900173024917[24] = 0.0; - out_4158952900173024917[25] = 0.0; - out_4158952900173024917[26] = 0.0; - out_4158952900173024917[27] = 0.0; - out_4158952900173024917[28] = 0.0; - out_4158952900173024917[29] = 0.0; - out_4158952900173024917[30] = 0.0; - out_4158952900173024917[31] = 0.0; - out_4158952900173024917[32] = 0.0; - out_4158952900173024917[33] = 0.0; - out_4158952900173024917[34] = 0.0; - out_4158952900173024917[35] = 0.0; - out_4158952900173024917[36] = 0.0; - out_4158952900173024917[37] = 0.0; - out_4158952900173024917[38] = 1.0; - out_4158952900173024917[39] = 0.0; - out_4158952900173024917[40] = 0.0; - out_4158952900173024917[41] = 0.0; - out_4158952900173024917[42] = 0.0; - out_4158952900173024917[43] = 0.0; - out_4158952900173024917[44] = 0.0; - out_4158952900173024917[45] = 0.0; - out_4158952900173024917[46] = 0.0; - out_4158952900173024917[47] = 0.0; - out_4158952900173024917[48] = 0.0; - out_4158952900173024917[49] = 0.0; - out_4158952900173024917[50] = 0.0; - out_4158952900173024917[51] = 0.0; - out_4158952900173024917[52] = 0.0; - out_4158952900173024917[53] = 0.0; - out_4158952900173024917[54] = 0.0; - out_4158952900173024917[55] = 0.0; - out_4158952900173024917[56] = 0.0; - out_4158952900173024917[57] = 1.0; - out_4158952900173024917[58] = 0.0; - out_4158952900173024917[59] = 0.0; - out_4158952900173024917[60] = 0.0; - out_4158952900173024917[61] = 0.0; - out_4158952900173024917[62] = 0.0; - out_4158952900173024917[63] = 0.0; - out_4158952900173024917[64] = 0.0; - out_4158952900173024917[65] = 0.0; - out_4158952900173024917[66] = 0.0; - out_4158952900173024917[67] = 0.0; - out_4158952900173024917[68] = 0.0; - out_4158952900173024917[69] = 0.0; - out_4158952900173024917[70] = 0.0; - out_4158952900173024917[71] = 0.0; - out_4158952900173024917[72] = 0.0; - out_4158952900173024917[73] = 0.0; - out_4158952900173024917[74] = 0.0; - out_4158952900173024917[75] = 0.0; - out_4158952900173024917[76] = 1.0; - out_4158952900173024917[77] = 0.0; - out_4158952900173024917[78] = 0.0; - out_4158952900173024917[79] = 0.0; - out_4158952900173024917[80] = 0.0; - out_4158952900173024917[81] = 0.0; - out_4158952900173024917[82] = 0.0; - out_4158952900173024917[83] = 0.0; - out_4158952900173024917[84] = 0.0; - out_4158952900173024917[85] = 0.0; - out_4158952900173024917[86] = 0.0; - out_4158952900173024917[87] = 0.0; - out_4158952900173024917[88] = 0.0; - out_4158952900173024917[89] = 0.0; - out_4158952900173024917[90] = 0.0; - out_4158952900173024917[91] = 0.0; - out_4158952900173024917[92] = 0.0; - out_4158952900173024917[93] = 0.0; - out_4158952900173024917[94] = 0.0; - out_4158952900173024917[95] = 1.0; - out_4158952900173024917[96] = 0.0; - out_4158952900173024917[97] = 0.0; - out_4158952900173024917[98] = 0.0; - out_4158952900173024917[99] = 0.0; - out_4158952900173024917[100] = 0.0; - out_4158952900173024917[101] = 0.0; - out_4158952900173024917[102] = 0.0; - out_4158952900173024917[103] = 0.0; - out_4158952900173024917[104] = 0.0; - out_4158952900173024917[105] = 0.0; - out_4158952900173024917[106] = 0.0; - out_4158952900173024917[107] = 0.0; - out_4158952900173024917[108] = 0.0; - out_4158952900173024917[109] = 0.0; - out_4158952900173024917[110] = 0.0; - out_4158952900173024917[111] = 0.0; - out_4158952900173024917[112] = 0.0; - out_4158952900173024917[113] = 0.0; - out_4158952900173024917[114] = 1.0; - out_4158952900173024917[115] = 0.0; - out_4158952900173024917[116] = 0.0; - out_4158952900173024917[117] = 0.0; - out_4158952900173024917[118] = 0.0; - out_4158952900173024917[119] = 0.0; - out_4158952900173024917[120] = 0.0; - out_4158952900173024917[121] = 0.0; - out_4158952900173024917[122] = 0.0; - out_4158952900173024917[123] = 0.0; - out_4158952900173024917[124] = 0.0; - out_4158952900173024917[125] = 0.0; - out_4158952900173024917[126] = 0.0; - out_4158952900173024917[127] = 0.0; - out_4158952900173024917[128] = 0.0; - out_4158952900173024917[129] = 0.0; - out_4158952900173024917[130] = 0.0; - out_4158952900173024917[131] = 0.0; - out_4158952900173024917[132] = 0.0; - out_4158952900173024917[133] = 1.0; - out_4158952900173024917[134] = 0.0; - out_4158952900173024917[135] = 0.0; - out_4158952900173024917[136] = 0.0; - out_4158952900173024917[137] = 0.0; - out_4158952900173024917[138] = 0.0; - out_4158952900173024917[139] = 0.0; - out_4158952900173024917[140] = 0.0; - out_4158952900173024917[141] = 0.0; - out_4158952900173024917[142] = 0.0; - out_4158952900173024917[143] = 0.0; - out_4158952900173024917[144] = 0.0; - out_4158952900173024917[145] = 0.0; - out_4158952900173024917[146] = 0.0; - out_4158952900173024917[147] = 0.0; - out_4158952900173024917[148] = 0.0; - out_4158952900173024917[149] = 0.0; - out_4158952900173024917[150] = 0.0; - out_4158952900173024917[151] = 0.0; - out_4158952900173024917[152] = 1.0; - out_4158952900173024917[153] = 0.0; - out_4158952900173024917[154] = 0.0; - out_4158952900173024917[155] = 0.0; - out_4158952900173024917[156] = 0.0; - out_4158952900173024917[157] = 0.0; - out_4158952900173024917[158] = 0.0; - out_4158952900173024917[159] = 0.0; - out_4158952900173024917[160] = 0.0; - out_4158952900173024917[161] = 0.0; - out_4158952900173024917[162] = 0.0; - out_4158952900173024917[163] = 0.0; - out_4158952900173024917[164] = 0.0; - out_4158952900173024917[165] = 0.0; - out_4158952900173024917[166] = 0.0; - out_4158952900173024917[167] = 0.0; - out_4158952900173024917[168] = 0.0; - out_4158952900173024917[169] = 0.0; - out_4158952900173024917[170] = 0.0; - out_4158952900173024917[171] = 1.0; - out_4158952900173024917[172] = 0.0; - out_4158952900173024917[173] = 0.0; - out_4158952900173024917[174] = 0.0; - out_4158952900173024917[175] = 0.0; - out_4158952900173024917[176] = 0.0; - out_4158952900173024917[177] = 0.0; - out_4158952900173024917[178] = 0.0; - out_4158952900173024917[179] = 0.0; - out_4158952900173024917[180] = 0.0; - out_4158952900173024917[181] = 0.0; - out_4158952900173024917[182] = 0.0; - out_4158952900173024917[183] = 0.0; - out_4158952900173024917[184] = 0.0; - out_4158952900173024917[185] = 0.0; - out_4158952900173024917[186] = 0.0; - out_4158952900173024917[187] = 0.0; - out_4158952900173024917[188] = 0.0; - out_4158952900173024917[189] = 0.0; - out_4158952900173024917[190] = 1.0; - out_4158952900173024917[191] = 0.0; - out_4158952900173024917[192] = 0.0; - out_4158952900173024917[193] = 0.0; - out_4158952900173024917[194] = 0.0; - out_4158952900173024917[195] = 0.0; - out_4158952900173024917[196] = 0.0; - out_4158952900173024917[197] = 0.0; - out_4158952900173024917[198] = 0.0; - out_4158952900173024917[199] = 0.0; - out_4158952900173024917[200] = 0.0; - out_4158952900173024917[201] = 0.0; - out_4158952900173024917[202] = 0.0; - out_4158952900173024917[203] = 0.0; - out_4158952900173024917[204] = 0.0; - out_4158952900173024917[205] = 0.0; - out_4158952900173024917[206] = 0.0; - out_4158952900173024917[207] = 0.0; - out_4158952900173024917[208] = 0.0; - out_4158952900173024917[209] = 1.0; - out_4158952900173024917[210] = 0.0; - out_4158952900173024917[211] = 0.0; - out_4158952900173024917[212] = 0.0; - out_4158952900173024917[213] = 0.0; - out_4158952900173024917[214] = 0.0; - out_4158952900173024917[215] = 0.0; - out_4158952900173024917[216] = 0.0; - out_4158952900173024917[217] = 0.0; - out_4158952900173024917[218] = 0.0; - out_4158952900173024917[219] = 0.0; - out_4158952900173024917[220] = 0.0; - out_4158952900173024917[221] = 0.0; - out_4158952900173024917[222] = 0.0; - out_4158952900173024917[223] = 0.0; - out_4158952900173024917[224] = 0.0; - out_4158952900173024917[225] = 0.0; - out_4158952900173024917[226] = 0.0; - out_4158952900173024917[227] = 0.0; - out_4158952900173024917[228] = 1.0; - out_4158952900173024917[229] = 0.0; - out_4158952900173024917[230] = 0.0; - out_4158952900173024917[231] = 0.0; - out_4158952900173024917[232] = 0.0; - out_4158952900173024917[233] = 0.0; - out_4158952900173024917[234] = 0.0; - out_4158952900173024917[235] = 0.0; - out_4158952900173024917[236] = 0.0; - out_4158952900173024917[237] = 0.0; - out_4158952900173024917[238] = 0.0; - out_4158952900173024917[239] = 0.0; - out_4158952900173024917[240] = 0.0; - out_4158952900173024917[241] = 0.0; - out_4158952900173024917[242] = 0.0; - out_4158952900173024917[243] = 0.0; - out_4158952900173024917[244] = 0.0; - out_4158952900173024917[245] = 0.0; - out_4158952900173024917[246] = 0.0; - out_4158952900173024917[247] = 1.0; - out_4158952900173024917[248] = 0.0; - out_4158952900173024917[249] = 0.0; - out_4158952900173024917[250] = 0.0; - out_4158952900173024917[251] = 0.0; - out_4158952900173024917[252] = 0.0; - out_4158952900173024917[253] = 0.0; - out_4158952900173024917[254] = 0.0; - out_4158952900173024917[255] = 0.0; - out_4158952900173024917[256] = 0.0; - out_4158952900173024917[257] = 0.0; - out_4158952900173024917[258] = 0.0; - out_4158952900173024917[259] = 0.0; - out_4158952900173024917[260] = 0.0; - out_4158952900173024917[261] = 0.0; - out_4158952900173024917[262] = 0.0; - out_4158952900173024917[263] = 0.0; - out_4158952900173024917[264] = 0.0; - out_4158952900173024917[265] = 0.0; - out_4158952900173024917[266] = 1.0; - out_4158952900173024917[267] = 0.0; - out_4158952900173024917[268] = 0.0; - out_4158952900173024917[269] = 0.0; - out_4158952900173024917[270] = 0.0; - out_4158952900173024917[271] = 0.0; - out_4158952900173024917[272] = 0.0; - out_4158952900173024917[273] = 0.0; - out_4158952900173024917[274] = 0.0; - out_4158952900173024917[275] = 0.0; - out_4158952900173024917[276] = 0.0; - out_4158952900173024917[277] = 0.0; - out_4158952900173024917[278] = 0.0; - out_4158952900173024917[279] = 0.0; - out_4158952900173024917[280] = 0.0; - out_4158952900173024917[281] = 0.0; - out_4158952900173024917[282] = 0.0; - out_4158952900173024917[283] = 0.0; - out_4158952900173024917[284] = 0.0; - out_4158952900173024917[285] = 1.0; - out_4158952900173024917[286] = 0.0; - out_4158952900173024917[287] = 0.0; - out_4158952900173024917[288] = 0.0; - out_4158952900173024917[289] = 0.0; - out_4158952900173024917[290] = 0.0; - out_4158952900173024917[291] = 0.0; - out_4158952900173024917[292] = 0.0; - out_4158952900173024917[293] = 0.0; - out_4158952900173024917[294] = 0.0; - out_4158952900173024917[295] = 0.0; - out_4158952900173024917[296] = 0.0; - out_4158952900173024917[297] = 0.0; - out_4158952900173024917[298] = 0.0; - out_4158952900173024917[299] = 0.0; - out_4158952900173024917[300] = 0.0; - out_4158952900173024917[301] = 0.0; - out_4158952900173024917[302] = 0.0; - out_4158952900173024917[303] = 0.0; - out_4158952900173024917[304] = 1.0; - out_4158952900173024917[305] = 0.0; - out_4158952900173024917[306] = 0.0; - out_4158952900173024917[307] = 0.0; - out_4158952900173024917[308] = 0.0; - out_4158952900173024917[309] = 0.0; - out_4158952900173024917[310] = 0.0; - out_4158952900173024917[311] = 0.0; - out_4158952900173024917[312] = 0.0; - out_4158952900173024917[313] = 0.0; - out_4158952900173024917[314] = 0.0; - out_4158952900173024917[315] = 0.0; - out_4158952900173024917[316] = 0.0; - out_4158952900173024917[317] = 0.0; - out_4158952900173024917[318] = 0.0; - out_4158952900173024917[319] = 0.0; - out_4158952900173024917[320] = 0.0; - out_4158952900173024917[321] = 0.0; - out_4158952900173024917[322] = 0.0; - out_4158952900173024917[323] = 1.0; +void H_mod_fun(double *state, double *out_4168561321210011185) { + out_4168561321210011185[0] = 1.0; + out_4168561321210011185[1] = 0.0; + out_4168561321210011185[2] = 0.0; + out_4168561321210011185[3] = 0.0; + out_4168561321210011185[4] = 0.0; + out_4168561321210011185[5] = 0.0; + out_4168561321210011185[6] = 0.0; + out_4168561321210011185[7] = 0.0; + out_4168561321210011185[8] = 0.0; + out_4168561321210011185[9] = 0.0; + out_4168561321210011185[10] = 0.0; + out_4168561321210011185[11] = 0.0; + out_4168561321210011185[12] = 0.0; + out_4168561321210011185[13] = 0.0; + out_4168561321210011185[14] = 0.0; + out_4168561321210011185[15] = 0.0; + out_4168561321210011185[16] = 0.0; + out_4168561321210011185[17] = 0.0; + out_4168561321210011185[18] = 0.0; + out_4168561321210011185[19] = 1.0; + out_4168561321210011185[20] = 0.0; + out_4168561321210011185[21] = 0.0; + out_4168561321210011185[22] = 0.0; + out_4168561321210011185[23] = 0.0; + out_4168561321210011185[24] = 0.0; + out_4168561321210011185[25] = 0.0; + out_4168561321210011185[26] = 0.0; + out_4168561321210011185[27] = 0.0; + out_4168561321210011185[28] = 0.0; + out_4168561321210011185[29] = 0.0; + out_4168561321210011185[30] = 0.0; + out_4168561321210011185[31] = 0.0; + out_4168561321210011185[32] = 0.0; + out_4168561321210011185[33] = 0.0; + out_4168561321210011185[34] = 0.0; + out_4168561321210011185[35] = 0.0; + out_4168561321210011185[36] = 0.0; + out_4168561321210011185[37] = 0.0; + out_4168561321210011185[38] = 1.0; + out_4168561321210011185[39] = 0.0; + out_4168561321210011185[40] = 0.0; + out_4168561321210011185[41] = 0.0; + out_4168561321210011185[42] = 0.0; + out_4168561321210011185[43] = 0.0; + out_4168561321210011185[44] = 0.0; + out_4168561321210011185[45] = 0.0; + out_4168561321210011185[46] = 0.0; + out_4168561321210011185[47] = 0.0; + out_4168561321210011185[48] = 0.0; + out_4168561321210011185[49] = 0.0; + out_4168561321210011185[50] = 0.0; + out_4168561321210011185[51] = 0.0; + out_4168561321210011185[52] = 0.0; + out_4168561321210011185[53] = 0.0; + out_4168561321210011185[54] = 0.0; + out_4168561321210011185[55] = 0.0; + out_4168561321210011185[56] = 0.0; + out_4168561321210011185[57] = 1.0; + out_4168561321210011185[58] = 0.0; + out_4168561321210011185[59] = 0.0; + out_4168561321210011185[60] = 0.0; + out_4168561321210011185[61] = 0.0; + out_4168561321210011185[62] = 0.0; + out_4168561321210011185[63] = 0.0; + out_4168561321210011185[64] = 0.0; + out_4168561321210011185[65] = 0.0; + out_4168561321210011185[66] = 0.0; + out_4168561321210011185[67] = 0.0; + out_4168561321210011185[68] = 0.0; + out_4168561321210011185[69] = 0.0; + out_4168561321210011185[70] = 0.0; + out_4168561321210011185[71] = 0.0; + out_4168561321210011185[72] = 0.0; + out_4168561321210011185[73] = 0.0; + out_4168561321210011185[74] = 0.0; + out_4168561321210011185[75] = 0.0; + out_4168561321210011185[76] = 1.0; + out_4168561321210011185[77] = 0.0; + out_4168561321210011185[78] = 0.0; + out_4168561321210011185[79] = 0.0; + out_4168561321210011185[80] = 0.0; + out_4168561321210011185[81] = 0.0; + out_4168561321210011185[82] = 0.0; + out_4168561321210011185[83] = 0.0; + out_4168561321210011185[84] = 0.0; + out_4168561321210011185[85] = 0.0; + out_4168561321210011185[86] = 0.0; + out_4168561321210011185[87] = 0.0; + out_4168561321210011185[88] = 0.0; + out_4168561321210011185[89] = 0.0; + out_4168561321210011185[90] = 0.0; + out_4168561321210011185[91] = 0.0; + out_4168561321210011185[92] = 0.0; + out_4168561321210011185[93] = 0.0; + out_4168561321210011185[94] = 0.0; + out_4168561321210011185[95] = 1.0; + out_4168561321210011185[96] = 0.0; + out_4168561321210011185[97] = 0.0; + out_4168561321210011185[98] = 0.0; + out_4168561321210011185[99] = 0.0; + out_4168561321210011185[100] = 0.0; + out_4168561321210011185[101] = 0.0; + out_4168561321210011185[102] = 0.0; + out_4168561321210011185[103] = 0.0; + out_4168561321210011185[104] = 0.0; + out_4168561321210011185[105] = 0.0; + out_4168561321210011185[106] = 0.0; + out_4168561321210011185[107] = 0.0; + out_4168561321210011185[108] = 0.0; + out_4168561321210011185[109] = 0.0; + out_4168561321210011185[110] = 0.0; + out_4168561321210011185[111] = 0.0; + out_4168561321210011185[112] = 0.0; + out_4168561321210011185[113] = 0.0; + out_4168561321210011185[114] = 1.0; + out_4168561321210011185[115] = 0.0; + out_4168561321210011185[116] = 0.0; + out_4168561321210011185[117] = 0.0; + out_4168561321210011185[118] = 0.0; + out_4168561321210011185[119] = 0.0; + out_4168561321210011185[120] = 0.0; + out_4168561321210011185[121] = 0.0; + out_4168561321210011185[122] = 0.0; + out_4168561321210011185[123] = 0.0; + out_4168561321210011185[124] = 0.0; + out_4168561321210011185[125] = 0.0; + out_4168561321210011185[126] = 0.0; + out_4168561321210011185[127] = 0.0; + out_4168561321210011185[128] = 0.0; + out_4168561321210011185[129] = 0.0; + out_4168561321210011185[130] = 0.0; + out_4168561321210011185[131] = 0.0; + out_4168561321210011185[132] = 0.0; + out_4168561321210011185[133] = 1.0; + out_4168561321210011185[134] = 0.0; + out_4168561321210011185[135] = 0.0; + out_4168561321210011185[136] = 0.0; + out_4168561321210011185[137] = 0.0; + out_4168561321210011185[138] = 0.0; + out_4168561321210011185[139] = 0.0; + out_4168561321210011185[140] = 0.0; + out_4168561321210011185[141] = 0.0; + out_4168561321210011185[142] = 0.0; + out_4168561321210011185[143] = 0.0; + out_4168561321210011185[144] = 0.0; + out_4168561321210011185[145] = 0.0; + out_4168561321210011185[146] = 0.0; + out_4168561321210011185[147] = 0.0; + out_4168561321210011185[148] = 0.0; + out_4168561321210011185[149] = 0.0; + out_4168561321210011185[150] = 0.0; + out_4168561321210011185[151] = 0.0; + out_4168561321210011185[152] = 1.0; + out_4168561321210011185[153] = 0.0; + out_4168561321210011185[154] = 0.0; + out_4168561321210011185[155] = 0.0; + out_4168561321210011185[156] = 0.0; + out_4168561321210011185[157] = 0.0; + out_4168561321210011185[158] = 0.0; + out_4168561321210011185[159] = 0.0; + out_4168561321210011185[160] = 0.0; + out_4168561321210011185[161] = 0.0; + out_4168561321210011185[162] = 0.0; + out_4168561321210011185[163] = 0.0; + out_4168561321210011185[164] = 0.0; + out_4168561321210011185[165] = 0.0; + out_4168561321210011185[166] = 0.0; + out_4168561321210011185[167] = 0.0; + out_4168561321210011185[168] = 0.0; + out_4168561321210011185[169] = 0.0; + out_4168561321210011185[170] = 0.0; + out_4168561321210011185[171] = 1.0; + out_4168561321210011185[172] = 0.0; + out_4168561321210011185[173] = 0.0; + out_4168561321210011185[174] = 0.0; + out_4168561321210011185[175] = 0.0; + out_4168561321210011185[176] = 0.0; + out_4168561321210011185[177] = 0.0; + out_4168561321210011185[178] = 0.0; + out_4168561321210011185[179] = 0.0; + out_4168561321210011185[180] = 0.0; + out_4168561321210011185[181] = 0.0; + out_4168561321210011185[182] = 0.0; + out_4168561321210011185[183] = 0.0; + out_4168561321210011185[184] = 0.0; + out_4168561321210011185[185] = 0.0; + out_4168561321210011185[186] = 0.0; + out_4168561321210011185[187] = 0.0; + out_4168561321210011185[188] = 0.0; + out_4168561321210011185[189] = 0.0; + out_4168561321210011185[190] = 1.0; + out_4168561321210011185[191] = 0.0; + out_4168561321210011185[192] = 0.0; + out_4168561321210011185[193] = 0.0; + out_4168561321210011185[194] = 0.0; + out_4168561321210011185[195] = 0.0; + out_4168561321210011185[196] = 0.0; + out_4168561321210011185[197] = 0.0; + out_4168561321210011185[198] = 0.0; + out_4168561321210011185[199] = 0.0; + out_4168561321210011185[200] = 0.0; + out_4168561321210011185[201] = 0.0; + out_4168561321210011185[202] = 0.0; + out_4168561321210011185[203] = 0.0; + out_4168561321210011185[204] = 0.0; + out_4168561321210011185[205] = 0.0; + out_4168561321210011185[206] = 0.0; + out_4168561321210011185[207] = 0.0; + out_4168561321210011185[208] = 0.0; + out_4168561321210011185[209] = 1.0; + out_4168561321210011185[210] = 0.0; + out_4168561321210011185[211] = 0.0; + out_4168561321210011185[212] = 0.0; + out_4168561321210011185[213] = 0.0; + out_4168561321210011185[214] = 0.0; + out_4168561321210011185[215] = 0.0; + out_4168561321210011185[216] = 0.0; + out_4168561321210011185[217] = 0.0; + out_4168561321210011185[218] = 0.0; + out_4168561321210011185[219] = 0.0; + out_4168561321210011185[220] = 0.0; + out_4168561321210011185[221] = 0.0; + out_4168561321210011185[222] = 0.0; + out_4168561321210011185[223] = 0.0; + out_4168561321210011185[224] = 0.0; + out_4168561321210011185[225] = 0.0; + out_4168561321210011185[226] = 0.0; + out_4168561321210011185[227] = 0.0; + out_4168561321210011185[228] = 1.0; + out_4168561321210011185[229] = 0.0; + out_4168561321210011185[230] = 0.0; + out_4168561321210011185[231] = 0.0; + out_4168561321210011185[232] = 0.0; + out_4168561321210011185[233] = 0.0; + out_4168561321210011185[234] = 0.0; + out_4168561321210011185[235] = 0.0; + out_4168561321210011185[236] = 0.0; + out_4168561321210011185[237] = 0.0; + out_4168561321210011185[238] = 0.0; + out_4168561321210011185[239] = 0.0; + out_4168561321210011185[240] = 0.0; + out_4168561321210011185[241] = 0.0; + out_4168561321210011185[242] = 0.0; + out_4168561321210011185[243] = 0.0; + out_4168561321210011185[244] = 0.0; + out_4168561321210011185[245] = 0.0; + out_4168561321210011185[246] = 0.0; + out_4168561321210011185[247] = 1.0; + out_4168561321210011185[248] = 0.0; + out_4168561321210011185[249] = 0.0; + out_4168561321210011185[250] = 0.0; + out_4168561321210011185[251] = 0.0; + out_4168561321210011185[252] = 0.0; + out_4168561321210011185[253] = 0.0; + out_4168561321210011185[254] = 0.0; + out_4168561321210011185[255] = 0.0; + out_4168561321210011185[256] = 0.0; + out_4168561321210011185[257] = 0.0; + out_4168561321210011185[258] = 0.0; + out_4168561321210011185[259] = 0.0; + out_4168561321210011185[260] = 0.0; + out_4168561321210011185[261] = 0.0; + out_4168561321210011185[262] = 0.0; + out_4168561321210011185[263] = 0.0; + out_4168561321210011185[264] = 0.0; + out_4168561321210011185[265] = 0.0; + out_4168561321210011185[266] = 1.0; + out_4168561321210011185[267] = 0.0; + out_4168561321210011185[268] = 0.0; + out_4168561321210011185[269] = 0.0; + out_4168561321210011185[270] = 0.0; + out_4168561321210011185[271] = 0.0; + out_4168561321210011185[272] = 0.0; + out_4168561321210011185[273] = 0.0; + out_4168561321210011185[274] = 0.0; + out_4168561321210011185[275] = 0.0; + out_4168561321210011185[276] = 0.0; + out_4168561321210011185[277] = 0.0; + out_4168561321210011185[278] = 0.0; + out_4168561321210011185[279] = 0.0; + out_4168561321210011185[280] = 0.0; + out_4168561321210011185[281] = 0.0; + out_4168561321210011185[282] = 0.0; + out_4168561321210011185[283] = 0.0; + out_4168561321210011185[284] = 0.0; + out_4168561321210011185[285] = 1.0; + out_4168561321210011185[286] = 0.0; + out_4168561321210011185[287] = 0.0; + out_4168561321210011185[288] = 0.0; + out_4168561321210011185[289] = 0.0; + out_4168561321210011185[290] = 0.0; + out_4168561321210011185[291] = 0.0; + out_4168561321210011185[292] = 0.0; + out_4168561321210011185[293] = 0.0; + out_4168561321210011185[294] = 0.0; + out_4168561321210011185[295] = 0.0; + out_4168561321210011185[296] = 0.0; + out_4168561321210011185[297] = 0.0; + out_4168561321210011185[298] = 0.0; + out_4168561321210011185[299] = 0.0; + out_4168561321210011185[300] = 0.0; + out_4168561321210011185[301] = 0.0; + out_4168561321210011185[302] = 0.0; + out_4168561321210011185[303] = 0.0; + out_4168561321210011185[304] = 1.0; + out_4168561321210011185[305] = 0.0; + out_4168561321210011185[306] = 0.0; + out_4168561321210011185[307] = 0.0; + out_4168561321210011185[308] = 0.0; + out_4168561321210011185[309] = 0.0; + out_4168561321210011185[310] = 0.0; + out_4168561321210011185[311] = 0.0; + out_4168561321210011185[312] = 0.0; + out_4168561321210011185[313] = 0.0; + out_4168561321210011185[314] = 0.0; + out_4168561321210011185[315] = 0.0; + out_4168561321210011185[316] = 0.0; + out_4168561321210011185[317] = 0.0; + out_4168561321210011185[318] = 0.0; + out_4168561321210011185[319] = 0.0; + out_4168561321210011185[320] = 0.0; + out_4168561321210011185[321] = 0.0; + out_4168561321210011185[322] = 0.0; + out_4168561321210011185[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4480310224533440205) { - out_4480310224533440205[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_4480310224533440205[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_4480310224533440205[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_4480310224533440205[3] = dt*state[12] + state[3]; - out_4480310224533440205[4] = dt*state[13] + state[4]; - out_4480310224533440205[5] = dt*state[14] + state[5]; - out_4480310224533440205[6] = state[6]; - out_4480310224533440205[7] = state[7]; - out_4480310224533440205[8] = state[8]; - out_4480310224533440205[9] = state[9]; - out_4480310224533440205[10] = state[10]; - out_4480310224533440205[11] = state[11]; - out_4480310224533440205[12] = state[12]; - out_4480310224533440205[13] = state[13]; - out_4480310224533440205[14] = state[14]; - out_4480310224533440205[15] = state[15]; - out_4480310224533440205[16] = state[16]; - out_4480310224533440205[17] = state[17]; +void f_fun(double *state, double dt, double *out_7595162637232814159) { + out_7595162637232814159[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_7595162637232814159[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_7595162637232814159[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_7595162637232814159[3] = dt*state[12] + state[3]; + out_7595162637232814159[4] = dt*state[13] + state[4]; + out_7595162637232814159[5] = dt*state[14] + state[5]; + out_7595162637232814159[6] = state[6]; + out_7595162637232814159[7] = state[7]; + out_7595162637232814159[8] = state[8]; + out_7595162637232814159[9] = state[9]; + out_7595162637232814159[10] = state[10]; + out_7595162637232814159[11] = state[11]; + out_7595162637232814159[12] = state[12]; + out_7595162637232814159[13] = state[13]; + out_7595162637232814159[14] = state[14]; + out_7595162637232814159[15] = state[15]; + out_7595162637232814159[16] = state[16]; + out_7595162637232814159[17] = state[17]; } -void F_fun(double *state, double dt, double *out_2252133042369597057) { - out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[2] = 0; - out_2252133042369597057[3] = 0; - out_2252133042369597057[4] = 0; - out_2252133042369597057[5] = 0; - out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[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_2252133042369597057[9] = 0; - out_2252133042369597057[10] = 0; - out_2252133042369597057[11] = 0; - out_2252133042369597057[12] = 0; - out_2252133042369597057[13] = 0; - out_2252133042369597057[14] = 0; - out_2252133042369597057[15] = 0; - out_2252133042369597057[16] = 0; - out_2252133042369597057[17] = 0; - out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[20] = 0; - out_2252133042369597057[21] = 0; - out_2252133042369597057[22] = 0; - out_2252133042369597057[23] = 0; - out_2252133042369597057[24] = 0; - out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[27] = 0; - out_2252133042369597057[28] = 0; - out_2252133042369597057[29] = 0; - out_2252133042369597057[30] = 0; - out_2252133042369597057[31] = 0; - out_2252133042369597057[32] = 0; - out_2252133042369597057[33] = 0; - out_2252133042369597057[34] = 0; - out_2252133042369597057[35] = 0; - out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[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_2252133042369597057[39] = 0; - out_2252133042369597057[40] = 0; - out_2252133042369597057[41] = 0; - out_2252133042369597057[42] = 0; - out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[45] = 0; - out_2252133042369597057[46] = 0; - out_2252133042369597057[47] = 0; - out_2252133042369597057[48] = 0; - out_2252133042369597057[49] = 0; - out_2252133042369597057[50] = 0; - out_2252133042369597057[51] = 0; - out_2252133042369597057[52] = 0; - out_2252133042369597057[53] = 0; - out_2252133042369597057[54] = 0; - out_2252133042369597057[55] = 0; - out_2252133042369597057[56] = 0; - out_2252133042369597057[57] = 1; - out_2252133042369597057[58] = 0; - out_2252133042369597057[59] = 0; - out_2252133042369597057[60] = 0; - out_2252133042369597057[61] = 0; - out_2252133042369597057[62] = 0; - out_2252133042369597057[63] = 0; - out_2252133042369597057[64] = 0; - out_2252133042369597057[65] = 0; - out_2252133042369597057[66] = dt; - out_2252133042369597057[67] = 0; - out_2252133042369597057[68] = 0; - out_2252133042369597057[69] = 0; - out_2252133042369597057[70] = 0; - out_2252133042369597057[71] = 0; - out_2252133042369597057[72] = 0; - out_2252133042369597057[73] = 0; - out_2252133042369597057[74] = 0; - out_2252133042369597057[75] = 0; - out_2252133042369597057[76] = 1; - out_2252133042369597057[77] = 0; - out_2252133042369597057[78] = 0; - out_2252133042369597057[79] = 0; - out_2252133042369597057[80] = 0; - out_2252133042369597057[81] = 0; - out_2252133042369597057[82] = 0; - out_2252133042369597057[83] = 0; - out_2252133042369597057[84] = 0; - out_2252133042369597057[85] = dt; - out_2252133042369597057[86] = 0; - out_2252133042369597057[87] = 0; - out_2252133042369597057[88] = 0; - out_2252133042369597057[89] = 0; - out_2252133042369597057[90] = 0; - out_2252133042369597057[91] = 0; - out_2252133042369597057[92] = 0; - out_2252133042369597057[93] = 0; - out_2252133042369597057[94] = 0; - out_2252133042369597057[95] = 1; - out_2252133042369597057[96] = 0; - out_2252133042369597057[97] = 0; - out_2252133042369597057[98] = 0; - out_2252133042369597057[99] = 0; - out_2252133042369597057[100] = 0; - out_2252133042369597057[101] = 0; - out_2252133042369597057[102] = 0; - out_2252133042369597057[103] = 0; - out_2252133042369597057[104] = dt; - out_2252133042369597057[105] = 0; - out_2252133042369597057[106] = 0; - out_2252133042369597057[107] = 0; - out_2252133042369597057[108] = 0; - out_2252133042369597057[109] = 0; - out_2252133042369597057[110] = 0; - out_2252133042369597057[111] = 0; - out_2252133042369597057[112] = 0; - out_2252133042369597057[113] = 0; - out_2252133042369597057[114] = 1; - out_2252133042369597057[115] = 0; - out_2252133042369597057[116] = 0; - out_2252133042369597057[117] = 0; - out_2252133042369597057[118] = 0; - out_2252133042369597057[119] = 0; - out_2252133042369597057[120] = 0; - out_2252133042369597057[121] = 0; - out_2252133042369597057[122] = 0; - out_2252133042369597057[123] = 0; - out_2252133042369597057[124] = 0; - out_2252133042369597057[125] = 0; - out_2252133042369597057[126] = 0; - out_2252133042369597057[127] = 0; - out_2252133042369597057[128] = 0; - out_2252133042369597057[129] = 0; - out_2252133042369597057[130] = 0; - out_2252133042369597057[131] = 0; - out_2252133042369597057[132] = 0; - out_2252133042369597057[133] = 1; - out_2252133042369597057[134] = 0; - out_2252133042369597057[135] = 0; - out_2252133042369597057[136] = 0; - out_2252133042369597057[137] = 0; - out_2252133042369597057[138] = 0; - out_2252133042369597057[139] = 0; - out_2252133042369597057[140] = 0; - out_2252133042369597057[141] = 0; - out_2252133042369597057[142] = 0; - out_2252133042369597057[143] = 0; - out_2252133042369597057[144] = 0; - out_2252133042369597057[145] = 0; - out_2252133042369597057[146] = 0; - out_2252133042369597057[147] = 0; - out_2252133042369597057[148] = 0; - out_2252133042369597057[149] = 0; - out_2252133042369597057[150] = 0; - out_2252133042369597057[151] = 0; - out_2252133042369597057[152] = 1; - out_2252133042369597057[153] = 0; - out_2252133042369597057[154] = 0; - out_2252133042369597057[155] = 0; - out_2252133042369597057[156] = 0; - out_2252133042369597057[157] = 0; - out_2252133042369597057[158] = 0; - out_2252133042369597057[159] = 0; - out_2252133042369597057[160] = 0; - out_2252133042369597057[161] = 0; - out_2252133042369597057[162] = 0; - out_2252133042369597057[163] = 0; - out_2252133042369597057[164] = 0; - out_2252133042369597057[165] = 0; - out_2252133042369597057[166] = 0; - out_2252133042369597057[167] = 0; - out_2252133042369597057[168] = 0; - out_2252133042369597057[169] = 0; - out_2252133042369597057[170] = 0; - out_2252133042369597057[171] = 1; - out_2252133042369597057[172] = 0; - out_2252133042369597057[173] = 0; - out_2252133042369597057[174] = 0; - out_2252133042369597057[175] = 0; - out_2252133042369597057[176] = 0; - out_2252133042369597057[177] = 0; - out_2252133042369597057[178] = 0; - out_2252133042369597057[179] = 0; - out_2252133042369597057[180] = 0; - out_2252133042369597057[181] = 0; - out_2252133042369597057[182] = 0; - out_2252133042369597057[183] = 0; - out_2252133042369597057[184] = 0; - out_2252133042369597057[185] = 0; - out_2252133042369597057[186] = 0; - out_2252133042369597057[187] = 0; - out_2252133042369597057[188] = 0; - out_2252133042369597057[189] = 0; - out_2252133042369597057[190] = 1; - out_2252133042369597057[191] = 0; - out_2252133042369597057[192] = 0; - out_2252133042369597057[193] = 0; - out_2252133042369597057[194] = 0; - out_2252133042369597057[195] = 0; - out_2252133042369597057[196] = 0; - out_2252133042369597057[197] = 0; - out_2252133042369597057[198] = 0; - out_2252133042369597057[199] = 0; - out_2252133042369597057[200] = 0; - out_2252133042369597057[201] = 0; - out_2252133042369597057[202] = 0; - out_2252133042369597057[203] = 0; - out_2252133042369597057[204] = 0; - out_2252133042369597057[205] = 0; - out_2252133042369597057[206] = 0; - out_2252133042369597057[207] = 0; - out_2252133042369597057[208] = 0; - out_2252133042369597057[209] = 1; - out_2252133042369597057[210] = 0; - out_2252133042369597057[211] = 0; - out_2252133042369597057[212] = 0; - out_2252133042369597057[213] = 0; - out_2252133042369597057[214] = 0; - out_2252133042369597057[215] = 0; - out_2252133042369597057[216] = 0; - out_2252133042369597057[217] = 0; - out_2252133042369597057[218] = 0; - out_2252133042369597057[219] = 0; - out_2252133042369597057[220] = 0; - out_2252133042369597057[221] = 0; - out_2252133042369597057[222] = 0; - out_2252133042369597057[223] = 0; - out_2252133042369597057[224] = 0; - out_2252133042369597057[225] = 0; - out_2252133042369597057[226] = 0; - out_2252133042369597057[227] = 0; - out_2252133042369597057[228] = 1; - out_2252133042369597057[229] = 0; - out_2252133042369597057[230] = 0; - out_2252133042369597057[231] = 0; - out_2252133042369597057[232] = 0; - out_2252133042369597057[233] = 0; - out_2252133042369597057[234] = 0; - out_2252133042369597057[235] = 0; - out_2252133042369597057[236] = 0; - out_2252133042369597057[237] = 0; - out_2252133042369597057[238] = 0; - out_2252133042369597057[239] = 0; - out_2252133042369597057[240] = 0; - out_2252133042369597057[241] = 0; - out_2252133042369597057[242] = 0; - out_2252133042369597057[243] = 0; - out_2252133042369597057[244] = 0; - out_2252133042369597057[245] = 0; - out_2252133042369597057[246] = 0; - out_2252133042369597057[247] = 1; - out_2252133042369597057[248] = 0; - out_2252133042369597057[249] = 0; - out_2252133042369597057[250] = 0; - out_2252133042369597057[251] = 0; - out_2252133042369597057[252] = 0; - out_2252133042369597057[253] = 0; - out_2252133042369597057[254] = 0; - out_2252133042369597057[255] = 0; - out_2252133042369597057[256] = 0; - out_2252133042369597057[257] = 0; - out_2252133042369597057[258] = 0; - out_2252133042369597057[259] = 0; - out_2252133042369597057[260] = 0; - out_2252133042369597057[261] = 0; - out_2252133042369597057[262] = 0; - out_2252133042369597057[263] = 0; - out_2252133042369597057[264] = 0; - out_2252133042369597057[265] = 0; - out_2252133042369597057[266] = 1; - out_2252133042369597057[267] = 0; - out_2252133042369597057[268] = 0; - out_2252133042369597057[269] = 0; - out_2252133042369597057[270] = 0; - out_2252133042369597057[271] = 0; - out_2252133042369597057[272] = 0; - out_2252133042369597057[273] = 0; - out_2252133042369597057[274] = 0; - out_2252133042369597057[275] = 0; - out_2252133042369597057[276] = 0; - out_2252133042369597057[277] = 0; - out_2252133042369597057[278] = 0; - out_2252133042369597057[279] = 0; - out_2252133042369597057[280] = 0; - out_2252133042369597057[281] = 0; - out_2252133042369597057[282] = 0; - out_2252133042369597057[283] = 0; - out_2252133042369597057[284] = 0; - out_2252133042369597057[285] = 1; - out_2252133042369597057[286] = 0; - out_2252133042369597057[287] = 0; - out_2252133042369597057[288] = 0; - out_2252133042369597057[289] = 0; - out_2252133042369597057[290] = 0; - out_2252133042369597057[291] = 0; - out_2252133042369597057[292] = 0; - out_2252133042369597057[293] = 0; - out_2252133042369597057[294] = 0; - out_2252133042369597057[295] = 0; - out_2252133042369597057[296] = 0; - out_2252133042369597057[297] = 0; - out_2252133042369597057[298] = 0; - out_2252133042369597057[299] = 0; - out_2252133042369597057[300] = 0; - out_2252133042369597057[301] = 0; - out_2252133042369597057[302] = 0; - out_2252133042369597057[303] = 0; - out_2252133042369597057[304] = 1; - out_2252133042369597057[305] = 0; - out_2252133042369597057[306] = 0; - out_2252133042369597057[307] = 0; - out_2252133042369597057[308] = 0; - out_2252133042369597057[309] = 0; - out_2252133042369597057[310] = 0; - out_2252133042369597057[311] = 0; - out_2252133042369597057[312] = 0; - out_2252133042369597057[313] = 0; - out_2252133042369597057[314] = 0; - out_2252133042369597057[315] = 0; - out_2252133042369597057[316] = 0; - out_2252133042369597057[317] = 0; - out_2252133042369597057[318] = 0; - out_2252133042369597057[319] = 0; - out_2252133042369597057[320] = 0; - out_2252133042369597057[321] = 0; - out_2252133042369597057[322] = 0; - out_2252133042369597057[323] = 1; +void F_fun(double *state, double dt, double *out_1091902965475609973) { + out_1091902965475609973[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_1091902965475609973[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_1091902965475609973[2] = 0; + out_1091902965475609973[3] = 0; + out_1091902965475609973[4] = 0; + out_1091902965475609973[5] = 0; + out_1091902965475609973[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_1091902965475609973[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_1091902965475609973[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_1091902965475609973[9] = 0; + out_1091902965475609973[10] = 0; + out_1091902965475609973[11] = 0; + out_1091902965475609973[12] = 0; + out_1091902965475609973[13] = 0; + out_1091902965475609973[14] = 0; + out_1091902965475609973[15] = 0; + out_1091902965475609973[16] = 0; + out_1091902965475609973[17] = 0; + out_1091902965475609973[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_1091902965475609973[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_1091902965475609973[20] = 0; + out_1091902965475609973[21] = 0; + out_1091902965475609973[22] = 0; + out_1091902965475609973[23] = 0; + out_1091902965475609973[24] = 0; + out_1091902965475609973[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_1091902965475609973[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_1091902965475609973[27] = 0; + out_1091902965475609973[28] = 0; + out_1091902965475609973[29] = 0; + out_1091902965475609973[30] = 0; + out_1091902965475609973[31] = 0; + out_1091902965475609973[32] = 0; + out_1091902965475609973[33] = 0; + out_1091902965475609973[34] = 0; + out_1091902965475609973[35] = 0; + out_1091902965475609973[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_1091902965475609973[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_1091902965475609973[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_1091902965475609973[39] = 0; + out_1091902965475609973[40] = 0; + out_1091902965475609973[41] = 0; + out_1091902965475609973[42] = 0; + out_1091902965475609973[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_1091902965475609973[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_1091902965475609973[45] = 0; + out_1091902965475609973[46] = 0; + out_1091902965475609973[47] = 0; + out_1091902965475609973[48] = 0; + out_1091902965475609973[49] = 0; + out_1091902965475609973[50] = 0; + out_1091902965475609973[51] = 0; + out_1091902965475609973[52] = 0; + out_1091902965475609973[53] = 0; + out_1091902965475609973[54] = 0; + out_1091902965475609973[55] = 0; + out_1091902965475609973[56] = 0; + out_1091902965475609973[57] = 1; + out_1091902965475609973[58] = 0; + out_1091902965475609973[59] = 0; + out_1091902965475609973[60] = 0; + out_1091902965475609973[61] = 0; + out_1091902965475609973[62] = 0; + out_1091902965475609973[63] = 0; + out_1091902965475609973[64] = 0; + out_1091902965475609973[65] = 0; + out_1091902965475609973[66] = dt; + out_1091902965475609973[67] = 0; + out_1091902965475609973[68] = 0; + out_1091902965475609973[69] = 0; + out_1091902965475609973[70] = 0; + out_1091902965475609973[71] = 0; + out_1091902965475609973[72] = 0; + out_1091902965475609973[73] = 0; + out_1091902965475609973[74] = 0; + out_1091902965475609973[75] = 0; + out_1091902965475609973[76] = 1; + out_1091902965475609973[77] = 0; + out_1091902965475609973[78] = 0; + out_1091902965475609973[79] = 0; + out_1091902965475609973[80] = 0; + out_1091902965475609973[81] = 0; + out_1091902965475609973[82] = 0; + out_1091902965475609973[83] = 0; + out_1091902965475609973[84] = 0; + out_1091902965475609973[85] = dt; + out_1091902965475609973[86] = 0; + out_1091902965475609973[87] = 0; + out_1091902965475609973[88] = 0; + out_1091902965475609973[89] = 0; + out_1091902965475609973[90] = 0; + out_1091902965475609973[91] = 0; + out_1091902965475609973[92] = 0; + out_1091902965475609973[93] = 0; + out_1091902965475609973[94] = 0; + out_1091902965475609973[95] = 1; + out_1091902965475609973[96] = 0; + out_1091902965475609973[97] = 0; + out_1091902965475609973[98] = 0; + out_1091902965475609973[99] = 0; + out_1091902965475609973[100] = 0; + out_1091902965475609973[101] = 0; + out_1091902965475609973[102] = 0; + out_1091902965475609973[103] = 0; + out_1091902965475609973[104] = dt; + out_1091902965475609973[105] = 0; + out_1091902965475609973[106] = 0; + out_1091902965475609973[107] = 0; + out_1091902965475609973[108] = 0; + out_1091902965475609973[109] = 0; + out_1091902965475609973[110] = 0; + out_1091902965475609973[111] = 0; + out_1091902965475609973[112] = 0; + out_1091902965475609973[113] = 0; + out_1091902965475609973[114] = 1; + out_1091902965475609973[115] = 0; + out_1091902965475609973[116] = 0; + out_1091902965475609973[117] = 0; + out_1091902965475609973[118] = 0; + out_1091902965475609973[119] = 0; + out_1091902965475609973[120] = 0; + out_1091902965475609973[121] = 0; + out_1091902965475609973[122] = 0; + out_1091902965475609973[123] = 0; + out_1091902965475609973[124] = 0; + out_1091902965475609973[125] = 0; + out_1091902965475609973[126] = 0; + out_1091902965475609973[127] = 0; + out_1091902965475609973[128] = 0; + out_1091902965475609973[129] = 0; + out_1091902965475609973[130] = 0; + out_1091902965475609973[131] = 0; + out_1091902965475609973[132] = 0; + out_1091902965475609973[133] = 1; + out_1091902965475609973[134] = 0; + out_1091902965475609973[135] = 0; + out_1091902965475609973[136] = 0; + out_1091902965475609973[137] = 0; + out_1091902965475609973[138] = 0; + out_1091902965475609973[139] = 0; + out_1091902965475609973[140] = 0; + out_1091902965475609973[141] = 0; + out_1091902965475609973[142] = 0; + out_1091902965475609973[143] = 0; + out_1091902965475609973[144] = 0; + out_1091902965475609973[145] = 0; + out_1091902965475609973[146] = 0; + out_1091902965475609973[147] = 0; + out_1091902965475609973[148] = 0; + out_1091902965475609973[149] = 0; + out_1091902965475609973[150] = 0; + out_1091902965475609973[151] = 0; + out_1091902965475609973[152] = 1; + out_1091902965475609973[153] = 0; + out_1091902965475609973[154] = 0; + out_1091902965475609973[155] = 0; + out_1091902965475609973[156] = 0; + out_1091902965475609973[157] = 0; + out_1091902965475609973[158] = 0; + out_1091902965475609973[159] = 0; + out_1091902965475609973[160] = 0; + out_1091902965475609973[161] = 0; + out_1091902965475609973[162] = 0; + out_1091902965475609973[163] = 0; + out_1091902965475609973[164] = 0; + out_1091902965475609973[165] = 0; + out_1091902965475609973[166] = 0; + out_1091902965475609973[167] = 0; + out_1091902965475609973[168] = 0; + out_1091902965475609973[169] = 0; + out_1091902965475609973[170] = 0; + out_1091902965475609973[171] = 1; + out_1091902965475609973[172] = 0; + out_1091902965475609973[173] = 0; + out_1091902965475609973[174] = 0; + out_1091902965475609973[175] = 0; + out_1091902965475609973[176] = 0; + out_1091902965475609973[177] = 0; + out_1091902965475609973[178] = 0; + out_1091902965475609973[179] = 0; + out_1091902965475609973[180] = 0; + out_1091902965475609973[181] = 0; + out_1091902965475609973[182] = 0; + out_1091902965475609973[183] = 0; + out_1091902965475609973[184] = 0; + out_1091902965475609973[185] = 0; + out_1091902965475609973[186] = 0; + out_1091902965475609973[187] = 0; + out_1091902965475609973[188] = 0; + out_1091902965475609973[189] = 0; + out_1091902965475609973[190] = 1; + out_1091902965475609973[191] = 0; + out_1091902965475609973[192] = 0; + out_1091902965475609973[193] = 0; + out_1091902965475609973[194] = 0; + out_1091902965475609973[195] = 0; + out_1091902965475609973[196] = 0; + out_1091902965475609973[197] = 0; + out_1091902965475609973[198] = 0; + out_1091902965475609973[199] = 0; + out_1091902965475609973[200] = 0; + out_1091902965475609973[201] = 0; + out_1091902965475609973[202] = 0; + out_1091902965475609973[203] = 0; + out_1091902965475609973[204] = 0; + out_1091902965475609973[205] = 0; + out_1091902965475609973[206] = 0; + out_1091902965475609973[207] = 0; + out_1091902965475609973[208] = 0; + out_1091902965475609973[209] = 1; + out_1091902965475609973[210] = 0; + out_1091902965475609973[211] = 0; + out_1091902965475609973[212] = 0; + out_1091902965475609973[213] = 0; + out_1091902965475609973[214] = 0; + out_1091902965475609973[215] = 0; + out_1091902965475609973[216] = 0; + out_1091902965475609973[217] = 0; + out_1091902965475609973[218] = 0; + out_1091902965475609973[219] = 0; + out_1091902965475609973[220] = 0; + out_1091902965475609973[221] = 0; + out_1091902965475609973[222] = 0; + out_1091902965475609973[223] = 0; + out_1091902965475609973[224] = 0; + out_1091902965475609973[225] = 0; + out_1091902965475609973[226] = 0; + out_1091902965475609973[227] = 0; + out_1091902965475609973[228] = 1; + out_1091902965475609973[229] = 0; + out_1091902965475609973[230] = 0; + out_1091902965475609973[231] = 0; + out_1091902965475609973[232] = 0; + out_1091902965475609973[233] = 0; + out_1091902965475609973[234] = 0; + out_1091902965475609973[235] = 0; + out_1091902965475609973[236] = 0; + out_1091902965475609973[237] = 0; + out_1091902965475609973[238] = 0; + out_1091902965475609973[239] = 0; + out_1091902965475609973[240] = 0; + out_1091902965475609973[241] = 0; + out_1091902965475609973[242] = 0; + out_1091902965475609973[243] = 0; + out_1091902965475609973[244] = 0; + out_1091902965475609973[245] = 0; + out_1091902965475609973[246] = 0; + out_1091902965475609973[247] = 1; + out_1091902965475609973[248] = 0; + out_1091902965475609973[249] = 0; + out_1091902965475609973[250] = 0; + out_1091902965475609973[251] = 0; + out_1091902965475609973[252] = 0; + out_1091902965475609973[253] = 0; + out_1091902965475609973[254] = 0; + out_1091902965475609973[255] = 0; + out_1091902965475609973[256] = 0; + out_1091902965475609973[257] = 0; + out_1091902965475609973[258] = 0; + out_1091902965475609973[259] = 0; + out_1091902965475609973[260] = 0; + out_1091902965475609973[261] = 0; + out_1091902965475609973[262] = 0; + out_1091902965475609973[263] = 0; + out_1091902965475609973[264] = 0; + out_1091902965475609973[265] = 0; + out_1091902965475609973[266] = 1; + out_1091902965475609973[267] = 0; + out_1091902965475609973[268] = 0; + out_1091902965475609973[269] = 0; + out_1091902965475609973[270] = 0; + out_1091902965475609973[271] = 0; + out_1091902965475609973[272] = 0; + out_1091902965475609973[273] = 0; + out_1091902965475609973[274] = 0; + out_1091902965475609973[275] = 0; + out_1091902965475609973[276] = 0; + out_1091902965475609973[277] = 0; + out_1091902965475609973[278] = 0; + out_1091902965475609973[279] = 0; + out_1091902965475609973[280] = 0; + out_1091902965475609973[281] = 0; + out_1091902965475609973[282] = 0; + out_1091902965475609973[283] = 0; + out_1091902965475609973[284] = 0; + out_1091902965475609973[285] = 1; + out_1091902965475609973[286] = 0; + out_1091902965475609973[287] = 0; + out_1091902965475609973[288] = 0; + out_1091902965475609973[289] = 0; + out_1091902965475609973[290] = 0; + out_1091902965475609973[291] = 0; + out_1091902965475609973[292] = 0; + out_1091902965475609973[293] = 0; + out_1091902965475609973[294] = 0; + out_1091902965475609973[295] = 0; + out_1091902965475609973[296] = 0; + out_1091902965475609973[297] = 0; + out_1091902965475609973[298] = 0; + out_1091902965475609973[299] = 0; + out_1091902965475609973[300] = 0; + out_1091902965475609973[301] = 0; + out_1091902965475609973[302] = 0; + out_1091902965475609973[303] = 0; + out_1091902965475609973[304] = 1; + out_1091902965475609973[305] = 0; + out_1091902965475609973[306] = 0; + out_1091902965475609973[307] = 0; + out_1091902965475609973[308] = 0; + out_1091902965475609973[309] = 0; + out_1091902965475609973[310] = 0; + out_1091902965475609973[311] = 0; + out_1091902965475609973[312] = 0; + out_1091902965475609973[313] = 0; + out_1091902965475609973[314] = 0; + out_1091902965475609973[315] = 0; + out_1091902965475609973[316] = 0; + out_1091902965475609973[317] = 0; + out_1091902965475609973[318] = 0; + out_1091902965475609973[319] = 0; + out_1091902965475609973[320] = 0; + out_1091902965475609973[321] = 0; + out_1091902965475609973[322] = 0; + out_1091902965475609973[323] = 1; } -void h_4(double *state, double *unused, double *out_1225931617160521679) { - out_1225931617160521679[0] = state[6] + state[9]; - out_1225931617160521679[1] = state[7] + state[10]; - out_1225931617160521679[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_4271893182613978797) { + out_4271893182613978797[0] = state[6] + state[9]; + out_4271893182613978797[1] = state[7] + state[10]; + out_4271893182613978797[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_3404791898118312310) { - out_3404791898118312310[0] = 0; - out_3404791898118312310[1] = 0; - out_3404791898118312310[2] = 0; - out_3404791898118312310[3] = 0; - out_3404791898118312310[4] = 0; - out_3404791898118312310[5] = 0; - out_3404791898118312310[6] = 1; - out_3404791898118312310[7] = 0; - out_3404791898118312310[8] = 0; - out_3404791898118312310[9] = 1; - out_3404791898118312310[10] = 0; - out_3404791898118312310[11] = 0; - out_3404791898118312310[12] = 0; - out_3404791898118312310[13] = 0; - out_3404791898118312310[14] = 0; - out_3404791898118312310[15] = 0; - out_3404791898118312310[16] = 0; - out_3404791898118312310[17] = 0; - out_3404791898118312310[18] = 0; - out_3404791898118312310[19] = 0; - out_3404791898118312310[20] = 0; - out_3404791898118312310[21] = 0; - out_3404791898118312310[22] = 0; - out_3404791898118312310[23] = 0; - out_3404791898118312310[24] = 0; - out_3404791898118312310[25] = 1; - out_3404791898118312310[26] = 0; - out_3404791898118312310[27] = 0; - out_3404791898118312310[28] = 1; - out_3404791898118312310[29] = 0; - out_3404791898118312310[30] = 0; - out_3404791898118312310[31] = 0; - out_3404791898118312310[32] = 0; - out_3404791898118312310[33] = 0; - out_3404791898118312310[34] = 0; - out_3404791898118312310[35] = 0; - out_3404791898118312310[36] = 0; - out_3404791898118312310[37] = 0; - out_3404791898118312310[38] = 0; - out_3404791898118312310[39] = 0; - out_3404791898118312310[40] = 0; - out_3404791898118312310[41] = 0; - out_3404791898118312310[42] = 0; - out_3404791898118312310[43] = 0; - out_3404791898118312310[44] = 1; - out_3404791898118312310[45] = 0; - out_3404791898118312310[46] = 0; - out_3404791898118312310[47] = 1; - out_3404791898118312310[48] = 0; - out_3404791898118312310[49] = 0; - out_3404791898118312310[50] = 0; - out_3404791898118312310[51] = 0; - out_3404791898118312310[52] = 0; - out_3404791898118312310[53] = 0; +void H_4(double *state, double *unused, double *out_5265198332585855939) { + out_5265198332585855939[0] = 0; + out_5265198332585855939[1] = 0; + out_5265198332585855939[2] = 0; + out_5265198332585855939[3] = 0; + out_5265198332585855939[4] = 0; + out_5265198332585855939[5] = 0; + out_5265198332585855939[6] = 1; + out_5265198332585855939[7] = 0; + out_5265198332585855939[8] = 0; + out_5265198332585855939[9] = 1; + out_5265198332585855939[10] = 0; + out_5265198332585855939[11] = 0; + out_5265198332585855939[12] = 0; + out_5265198332585855939[13] = 0; + out_5265198332585855939[14] = 0; + out_5265198332585855939[15] = 0; + out_5265198332585855939[16] = 0; + out_5265198332585855939[17] = 0; + out_5265198332585855939[18] = 0; + out_5265198332585855939[19] = 0; + out_5265198332585855939[20] = 0; + out_5265198332585855939[21] = 0; + out_5265198332585855939[22] = 0; + out_5265198332585855939[23] = 0; + out_5265198332585855939[24] = 0; + out_5265198332585855939[25] = 1; + out_5265198332585855939[26] = 0; + out_5265198332585855939[27] = 0; + out_5265198332585855939[28] = 1; + out_5265198332585855939[29] = 0; + out_5265198332585855939[30] = 0; + out_5265198332585855939[31] = 0; + out_5265198332585855939[32] = 0; + out_5265198332585855939[33] = 0; + out_5265198332585855939[34] = 0; + out_5265198332585855939[35] = 0; + out_5265198332585855939[36] = 0; + out_5265198332585855939[37] = 0; + out_5265198332585855939[38] = 0; + out_5265198332585855939[39] = 0; + out_5265198332585855939[40] = 0; + out_5265198332585855939[41] = 0; + out_5265198332585855939[42] = 0; + out_5265198332585855939[43] = 0; + out_5265198332585855939[44] = 1; + out_5265198332585855939[45] = 0; + out_5265198332585855939[46] = 0; + out_5265198332585855939[47] = 1; + out_5265198332585855939[48] = 0; + out_5265198332585855939[49] = 0; + out_5265198332585855939[50] = 0; + out_5265198332585855939[51] = 0; + out_5265198332585855939[52] = 0; + out_5265198332585855939[53] = 0; } -void h_10(double *state, double *unused, double *out_8314558572170779017) { - out_8314558572170779017[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_8314558572170779017[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_8314558572170779017[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_6068675735727225828) { + out_6068675735727225828[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_6068675735727225828[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_6068675735727225828[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_3543553484029179274) { - out_3543553484029179274[0] = 0; - out_3543553484029179274[1] = 9.8100000000000005*cos(state[1]); - out_3543553484029179274[2] = 0; - out_3543553484029179274[3] = 0; - out_3543553484029179274[4] = -state[8]; - out_3543553484029179274[5] = state[7]; - out_3543553484029179274[6] = 0; - out_3543553484029179274[7] = state[5]; - out_3543553484029179274[8] = -state[4]; - out_3543553484029179274[9] = 0; - out_3543553484029179274[10] = 0; - out_3543553484029179274[11] = 0; - out_3543553484029179274[12] = 1; - out_3543553484029179274[13] = 0; - out_3543553484029179274[14] = 0; - out_3543553484029179274[15] = 1; - out_3543553484029179274[16] = 0; - out_3543553484029179274[17] = 0; - out_3543553484029179274[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_3543553484029179274[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_3543553484029179274[20] = 0; - out_3543553484029179274[21] = state[8]; - out_3543553484029179274[22] = 0; - out_3543553484029179274[23] = -state[6]; - out_3543553484029179274[24] = -state[5]; - out_3543553484029179274[25] = 0; - out_3543553484029179274[26] = state[3]; - out_3543553484029179274[27] = 0; - out_3543553484029179274[28] = 0; - out_3543553484029179274[29] = 0; - out_3543553484029179274[30] = 0; - out_3543553484029179274[31] = 1; - out_3543553484029179274[32] = 0; - out_3543553484029179274[33] = 0; - out_3543553484029179274[34] = 1; - out_3543553484029179274[35] = 0; - out_3543553484029179274[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_3543553484029179274[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_3543553484029179274[38] = 0; - out_3543553484029179274[39] = -state[7]; - out_3543553484029179274[40] = state[6]; - out_3543553484029179274[41] = 0; - out_3543553484029179274[42] = state[4]; - out_3543553484029179274[43] = -state[3]; - out_3543553484029179274[44] = 0; - out_3543553484029179274[45] = 0; - out_3543553484029179274[46] = 0; - out_3543553484029179274[47] = 0; - out_3543553484029179274[48] = 0; - out_3543553484029179274[49] = 0; - out_3543553484029179274[50] = 1; - out_3543553484029179274[51] = 0; - out_3543553484029179274[52] = 0; - out_3543553484029179274[53] = 1; +void H_10(double *state, double *unused, double *out_4012474741247108873) { + out_4012474741247108873[0] = 0; + out_4012474741247108873[1] = 9.8100000000000005*cos(state[1]); + out_4012474741247108873[2] = 0; + out_4012474741247108873[3] = 0; + out_4012474741247108873[4] = -state[8]; + out_4012474741247108873[5] = state[7]; + out_4012474741247108873[6] = 0; + out_4012474741247108873[7] = state[5]; + out_4012474741247108873[8] = -state[4]; + out_4012474741247108873[9] = 0; + out_4012474741247108873[10] = 0; + out_4012474741247108873[11] = 0; + out_4012474741247108873[12] = 1; + out_4012474741247108873[13] = 0; + out_4012474741247108873[14] = 0; + out_4012474741247108873[15] = 1; + out_4012474741247108873[16] = 0; + out_4012474741247108873[17] = 0; + out_4012474741247108873[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_4012474741247108873[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_4012474741247108873[20] = 0; + out_4012474741247108873[21] = state[8]; + out_4012474741247108873[22] = 0; + out_4012474741247108873[23] = -state[6]; + out_4012474741247108873[24] = -state[5]; + out_4012474741247108873[25] = 0; + out_4012474741247108873[26] = state[3]; + out_4012474741247108873[27] = 0; + out_4012474741247108873[28] = 0; + out_4012474741247108873[29] = 0; + out_4012474741247108873[30] = 0; + out_4012474741247108873[31] = 1; + out_4012474741247108873[32] = 0; + out_4012474741247108873[33] = 0; + out_4012474741247108873[34] = 1; + out_4012474741247108873[35] = 0; + out_4012474741247108873[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_4012474741247108873[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_4012474741247108873[38] = 0; + out_4012474741247108873[39] = -state[7]; + out_4012474741247108873[40] = state[6]; + out_4012474741247108873[41] = 0; + out_4012474741247108873[42] = state[4]; + out_4012474741247108873[43] = -state[3]; + out_4012474741247108873[44] = 0; + out_4012474741247108873[45] = 0; + out_4012474741247108873[46] = 0; + out_4012474741247108873[47] = 0; + out_4012474741247108873[48] = 0; + out_4012474741247108873[49] = 0; + out_4012474741247108873[50] = 1; + out_4012474741247108873[51] = 0; + out_4012474741247108873[52] = 0; + out_4012474741247108873[53] = 1; } -void h_13(double *state, double *unused, double *out_7492025966781743409) { - out_7492025966781743409[0] = state[3]; - out_7492025966781743409[1] = state[4]; - out_7492025966781743409[2] = state[5]; +void h_13(double *state, double *unused, double *out_860321329376649767) { + out_860321329376649767[0] = state[3]; + out_860321329376649767[1] = state[4]; + out_860321329376649767[2] = state[5]; } -void H_13(double *state, double *unused, double *out_4205839310198388619) { - out_4205839310198388619[0] = 0; - out_4205839310198388619[1] = 0; - out_4205839310198388619[2] = 0; - out_4205839310198388619[3] = 1; - out_4205839310198388619[4] = 0; - out_4205839310198388619[5] = 0; - out_4205839310198388619[6] = 0; - out_4205839310198388619[7] = 0; - out_4205839310198388619[8] = 0; - out_4205839310198388619[9] = 0; - out_4205839310198388619[10] = 0; - out_4205839310198388619[11] = 0; - out_4205839310198388619[12] = 0; - out_4205839310198388619[13] = 0; - out_4205839310198388619[14] = 0; - out_4205839310198388619[15] = 0; - out_4205839310198388619[16] = 0; - out_4205839310198388619[17] = 0; - out_4205839310198388619[18] = 0; - out_4205839310198388619[19] = 0; - out_4205839310198388619[20] = 0; - out_4205839310198388619[21] = 0; - out_4205839310198388619[22] = 1; - out_4205839310198388619[23] = 0; - out_4205839310198388619[24] = 0; - out_4205839310198388619[25] = 0; - out_4205839310198388619[26] = 0; - out_4205839310198388619[27] = 0; - out_4205839310198388619[28] = 0; - out_4205839310198388619[29] = 0; - out_4205839310198388619[30] = 0; - out_4205839310198388619[31] = 0; - out_4205839310198388619[32] = 0; - out_4205839310198388619[33] = 0; - out_4205839310198388619[34] = 0; - out_4205839310198388619[35] = 0; - out_4205839310198388619[36] = 0; - out_4205839310198388619[37] = 0; - out_4205839310198388619[38] = 0; - out_4205839310198388619[39] = 0; - out_4205839310198388619[40] = 0; - out_4205839310198388619[41] = 1; - out_4205839310198388619[42] = 0; - out_4205839310198388619[43] = 0; - out_4205839310198388619[44] = 0; - out_4205839310198388619[45] = 0; - out_4205839310198388619[46] = 0; - out_4205839310198388619[47] = 0; - out_4205839310198388619[48] = 0; - out_4205839310198388619[49] = 0; - out_4205839310198388619[50] = 0; - out_4205839310198388619[51] = 0; - out_4205839310198388619[52] = 0; - out_4205839310198388619[53] = 0; +void H_13(double *state, double *unused, double *out_2052924507253523138) { + out_2052924507253523138[0] = 0; + out_2052924507253523138[1] = 0; + out_2052924507253523138[2] = 0; + out_2052924507253523138[3] = 1; + out_2052924507253523138[4] = 0; + out_2052924507253523138[5] = 0; + out_2052924507253523138[6] = 0; + out_2052924507253523138[7] = 0; + out_2052924507253523138[8] = 0; + out_2052924507253523138[9] = 0; + out_2052924507253523138[10] = 0; + out_2052924507253523138[11] = 0; + out_2052924507253523138[12] = 0; + out_2052924507253523138[13] = 0; + out_2052924507253523138[14] = 0; + out_2052924507253523138[15] = 0; + out_2052924507253523138[16] = 0; + out_2052924507253523138[17] = 0; + out_2052924507253523138[18] = 0; + out_2052924507253523138[19] = 0; + out_2052924507253523138[20] = 0; + out_2052924507253523138[21] = 0; + out_2052924507253523138[22] = 1; + out_2052924507253523138[23] = 0; + out_2052924507253523138[24] = 0; + out_2052924507253523138[25] = 0; + out_2052924507253523138[26] = 0; + out_2052924507253523138[27] = 0; + out_2052924507253523138[28] = 0; + out_2052924507253523138[29] = 0; + out_2052924507253523138[30] = 0; + out_2052924507253523138[31] = 0; + out_2052924507253523138[32] = 0; + out_2052924507253523138[33] = 0; + out_2052924507253523138[34] = 0; + out_2052924507253523138[35] = 0; + out_2052924507253523138[36] = 0; + out_2052924507253523138[37] = 0; + out_2052924507253523138[38] = 0; + out_2052924507253523138[39] = 0; + out_2052924507253523138[40] = 0; + out_2052924507253523138[41] = 1; + out_2052924507253523138[42] = 0; + out_2052924507253523138[43] = 0; + out_2052924507253523138[44] = 0; + out_2052924507253523138[45] = 0; + out_2052924507253523138[46] = 0; + out_2052924507253523138[47] = 0; + out_2052924507253523138[48] = 0; + out_2052924507253523138[49] = 0; + out_2052924507253523138[50] = 0; + out_2052924507253523138[51] = 0; + out_2052924507253523138[52] = 0; + out_2052924507253523138[53] = 0; } -void h_14(double *state, double *unused, double *out_8444019918627598165) { - out_8444019918627598165[0] = state[6]; - out_8444019918627598165[1] = state[7]; - out_8444019918627598165[2] = state[8]; +void h_14(double *state, double *unused, double *out_5677441075365347292) { + out_5677441075365347292[0] = state[6]; + out_5677441075365347292[1] = state[7]; + out_5677441075365347292[2] = state[8]; } -void H_14(double *state, double *unused, double *out_6487580330413684606) { - out_6487580330413684606[0] = 0; - out_6487580330413684606[1] = 0; - out_6487580330413684606[2] = 0; - out_6487580330413684606[3] = 0; - out_6487580330413684606[4] = 0; - out_6487580330413684606[5] = 0; - out_6487580330413684606[6] = 1; - out_6487580330413684606[7] = 0; - out_6487580330413684606[8] = 0; - out_6487580330413684606[9] = 0; - out_6487580330413684606[10] = 0; - out_6487580330413684606[11] = 0; - out_6487580330413684606[12] = 0; - out_6487580330413684606[13] = 0; - out_6487580330413684606[14] = 0; - out_6487580330413684606[15] = 0; - out_6487580330413684606[16] = 0; - out_6487580330413684606[17] = 0; - out_6487580330413684606[18] = 0; - out_6487580330413684606[19] = 0; - out_6487580330413684606[20] = 0; - out_6487580330413684606[21] = 0; - out_6487580330413684606[22] = 0; - out_6487580330413684606[23] = 0; - out_6487580330413684606[24] = 0; - out_6487580330413684606[25] = 1; - out_6487580330413684606[26] = 0; - out_6487580330413684606[27] = 0; - out_6487580330413684606[28] = 0; - out_6487580330413684606[29] = 0; - out_6487580330413684606[30] = 0; - out_6487580330413684606[31] = 0; - out_6487580330413684606[32] = 0; - out_6487580330413684606[33] = 0; - out_6487580330413684606[34] = 0; - out_6487580330413684606[35] = 0; - out_6487580330413684606[36] = 0; - out_6487580330413684606[37] = 0; - out_6487580330413684606[38] = 0; - out_6487580330413684606[39] = 0; - out_6487580330413684606[40] = 0; - out_6487580330413684606[41] = 0; - out_6487580330413684606[42] = 0; - out_6487580330413684606[43] = 0; - out_6487580330413684606[44] = 1; - out_6487580330413684606[45] = 0; - out_6487580330413684606[46] = 0; - out_6487580330413684606[47] = 0; - out_6487580330413684606[48] = 0; - out_6487580330413684606[49] = 0; - out_6487580330413684606[50] = 0; - out_6487580330413684606[51] = 0; - out_6487580330413684606[52] = 0; - out_6487580330413684606[53] = 0; +void H_14(double *state, double *unused, double *out_1301957476246371410) { + out_1301957476246371410[0] = 0; + out_1301957476246371410[1] = 0; + out_1301957476246371410[2] = 0; + out_1301957476246371410[3] = 0; + out_1301957476246371410[4] = 0; + out_1301957476246371410[5] = 0; + out_1301957476246371410[6] = 1; + out_1301957476246371410[7] = 0; + out_1301957476246371410[8] = 0; + out_1301957476246371410[9] = 0; + out_1301957476246371410[10] = 0; + out_1301957476246371410[11] = 0; + out_1301957476246371410[12] = 0; + out_1301957476246371410[13] = 0; + out_1301957476246371410[14] = 0; + out_1301957476246371410[15] = 0; + out_1301957476246371410[16] = 0; + out_1301957476246371410[17] = 0; + out_1301957476246371410[18] = 0; + out_1301957476246371410[19] = 0; + out_1301957476246371410[20] = 0; + out_1301957476246371410[21] = 0; + out_1301957476246371410[22] = 0; + out_1301957476246371410[23] = 0; + out_1301957476246371410[24] = 0; + out_1301957476246371410[25] = 1; + out_1301957476246371410[26] = 0; + out_1301957476246371410[27] = 0; + out_1301957476246371410[28] = 0; + out_1301957476246371410[29] = 0; + out_1301957476246371410[30] = 0; + out_1301957476246371410[31] = 0; + out_1301957476246371410[32] = 0; + out_1301957476246371410[33] = 0; + out_1301957476246371410[34] = 0; + out_1301957476246371410[35] = 0; + out_1301957476246371410[36] = 0; + out_1301957476246371410[37] = 0; + out_1301957476246371410[38] = 0; + out_1301957476246371410[39] = 0; + out_1301957476246371410[40] = 0; + out_1301957476246371410[41] = 0; + out_1301957476246371410[42] = 0; + out_1301957476246371410[43] = 0; + out_1301957476246371410[44] = 1; + out_1301957476246371410[45] = 0; + out_1301957476246371410[46] = 0; + out_1301957476246371410[47] = 0; + out_1301957476246371410[48] = 0; + out_1301957476246371410[49] = 0; + out_1301957476246371410[50] = 0; + out_1301957476246371410[51] = 0; + out_1301957476246371410[52] = 0; + out_1301957476246371410[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_2822233615381903085) { - err_fun(nom_x, delta_x, out_2822233615381903085); +void pose_err_fun(double *nom_x, double *delta_x, double *out_609976965150865312) { + err_fun(nom_x, delta_x, out_609976965150865312); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3556797172859207138) { - inv_err_fun(nom_x, true_x, out_3556797172859207138); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8281732551506992950) { + inv_err_fun(nom_x, true_x, out_8281732551506992950); } -void pose_H_mod_fun(double *state, double *out_4158952900173024917) { - H_mod_fun(state, out_4158952900173024917); +void pose_H_mod_fun(double *state, double *out_4168561321210011185) { + H_mod_fun(state, out_4168561321210011185); } -void pose_f_fun(double *state, double dt, double *out_4480310224533440205) { - f_fun(state, dt, out_4480310224533440205); +void pose_f_fun(double *state, double dt, double *out_7595162637232814159) { + f_fun(state, dt, out_7595162637232814159); } -void pose_F_fun(double *state, double dt, double *out_2252133042369597057) { - F_fun(state, dt, out_2252133042369597057); +void pose_F_fun(double *state, double dt, double *out_1091902965475609973) { + F_fun(state, dt, out_1091902965475609973); } -void pose_h_4(double *state, double *unused, double *out_1225931617160521679) { - h_4(state, unused, out_1225931617160521679); +void pose_h_4(double *state, double *unused, double *out_4271893182613978797) { + h_4(state, unused, out_4271893182613978797); } -void pose_H_4(double *state, double *unused, double *out_3404791898118312310) { - H_4(state, unused, out_3404791898118312310); +void pose_H_4(double *state, double *unused, double *out_5265198332585855939) { + H_4(state, unused, out_5265198332585855939); } -void pose_h_10(double *state, double *unused, double *out_8314558572170779017) { - h_10(state, unused, out_8314558572170779017); +void pose_h_10(double *state, double *unused, double *out_6068675735727225828) { + h_10(state, unused, out_6068675735727225828); } -void pose_H_10(double *state, double *unused, double *out_3543553484029179274) { - H_10(state, unused, out_3543553484029179274); +void pose_H_10(double *state, double *unused, double *out_4012474741247108873) { + H_10(state, unused, out_4012474741247108873); } -void pose_h_13(double *state, double *unused, double *out_7492025966781743409) { - h_13(state, unused, out_7492025966781743409); +void pose_h_13(double *state, double *unused, double *out_860321329376649767) { + h_13(state, unused, out_860321329376649767); } -void pose_H_13(double *state, double *unused, double *out_4205839310198388619) { - H_13(state, unused, out_4205839310198388619); +void pose_H_13(double *state, double *unused, double *out_2052924507253523138) { + H_13(state, unused, out_2052924507253523138); } -void pose_h_14(double *state, double *unused, double *out_8444019918627598165) { - h_14(state, unused, out_8444019918627598165); +void pose_h_14(double *state, double *unused, double *out_5677441075365347292) { + h_14(state, unused, out_5677441075365347292); } -void pose_H_14(double *state, double *unused, double *out_6487580330413684606) { - H_14(state, unused, out_6487580330413684606); +void pose_H_14(double *state, double *unused, double *out_1301957476246371410) { + H_14(state, unused, out_1301957476246371410); } 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 b7e1330e1..2a7524b7c 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_2822233615381903085); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3556797172859207138); -void pose_H_mod_fun(double *state, double *out_4158952900173024917); -void pose_f_fun(double *state, double dt, double *out_4480310224533440205); -void pose_F_fun(double *state, double dt, double *out_2252133042369597057); -void pose_h_4(double *state, double *unused, double *out_1225931617160521679); -void pose_H_4(double *state, double *unused, double *out_3404791898118312310); -void pose_h_10(double *state, double *unused, double *out_8314558572170779017); -void pose_H_10(double *state, double *unused, double *out_3543553484029179274); -void pose_h_13(double *state, double *unused, double *out_7492025966781743409); -void pose_H_13(double *state, double *unused, double *out_4205839310198388619); -void pose_h_14(double *state, double *unused, double *out_8444019918627598165); -void pose_H_14(double *state, double *unused, double *out_6487580330413684606); +void pose_err_fun(double *nom_x, double *delta_x, double *out_609976965150865312); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8281732551506992950); +void pose_H_mod_fun(double *state, double *out_4168561321210011185); +void pose_f_fun(double *state, double dt, double *out_7595162637232814159); +void pose_F_fun(double *state, double dt, double *out_1091902965475609973); +void pose_h_4(double *state, double *unused, double *out_4271893182613978797); +void pose_H_4(double *state, double *unused, double *out_5265198332585855939); +void pose_h_10(double *state, double *unused, double *out_6068675735727225828); +void pose_H_10(double *state, double *unused, double *out_4012474741247108873); +void pose_h_13(double *state, double *unused, double *out_860321329376649767); +void pose_H_13(double *state, double *unused, double *out_2052924507253523138); +void pose_h_14(double *state, double *unused, double *out_5677441075365347292); +void pose_H_14(double *state, double *unused, double *out_1301957476246371410); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 3310e7f0d..ed4b717f3 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 6cae17eb6..0cc5e4ef3 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 54bfdddfc..1ed37e468 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 8a8884ab0..cd2489dfe 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 4f3ff4ef8..2e1aeb0f1 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 9893bb0f2..8e8265c0a 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ