diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 42c8caee6d..0b35476dbe 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 f20430986a..c9b78fdc24 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 3e08fe0615..7881a3aadb 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 c8beba9bf3..906710f45b 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 3ed1745dee..54bdd29e78 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 2a53e96106..b1c903545c 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 cf8ed51fef..e4eddfbbf6 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 cf8ed51fef..e4eddfbbf6 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 094bd4cb88..e7346af5ed 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 2a53e96106..b1c903545c 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 dd69b664d2..b94aee6010 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-6cdacd86-DEBUG"; +const uint8_t gitversion[19] = "DEV-3d0c3a8a-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 8393407e16..35e5bd11c8 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 fbbc33d630..c524942cdb 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 d2f557a425..1dc4e82de8 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 f22407199c..24ed4f8a15 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 d7c25f2920..ad7157e3b5 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 bd94efff88..d8167a3b88 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 9d16129d51..c03587224a 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 c770f39f28..196adcb98f 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 5b1ff389ee..d7f3151fba 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 bf903ec415..b81ff2bb3a 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 3f0c5870ef..9974818d95 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 70870df5cd..3fabe44324 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 516b888d42..d832aa82fd 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 e03035f6c5..573b8b51d3 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 de6932f2de..a60f6bbe68 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 c63deb82e9..dcb55faa0b 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 9fe274216e..019e04fb38 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 83abd45470..f747706071 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 a15d40ec8b..d2f6b96c0c 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 716fc6a33d..776be5d2be 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 860c3e87ef..33c7014ef0 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-6cdacd86-DEBUG \ No newline at end of file +DEV-3d0c3a8a-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 6e3c68b44c..074b99bce5 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_2249508041139738373) { - out_2249508041139738373[0] = delta_x[0] + nom_x[0]; - out_2249508041139738373[1] = delta_x[1] + nom_x[1]; - out_2249508041139738373[2] = delta_x[2] + nom_x[2]; - out_2249508041139738373[3] = delta_x[3] + nom_x[3]; - out_2249508041139738373[4] = delta_x[4] + nom_x[4]; - out_2249508041139738373[5] = delta_x[5] + nom_x[5]; - out_2249508041139738373[6] = delta_x[6] + nom_x[6]; - out_2249508041139738373[7] = delta_x[7] + nom_x[7]; - out_2249508041139738373[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1263767819732456823) { + out_1263767819732456823[0] = delta_x[0] + nom_x[0]; + out_1263767819732456823[1] = delta_x[1] + nom_x[1]; + out_1263767819732456823[2] = delta_x[2] + nom_x[2]; + out_1263767819732456823[3] = delta_x[3] + nom_x[3]; + out_1263767819732456823[4] = delta_x[4] + nom_x[4]; + out_1263767819732456823[5] = delta_x[5] + nom_x[5]; + out_1263767819732456823[6] = delta_x[6] + nom_x[6]; + out_1263767819732456823[7] = delta_x[7] + nom_x[7]; + out_1263767819732456823[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4788524825292355547) { - out_4788524825292355547[0] = -nom_x[0] + true_x[0]; - out_4788524825292355547[1] = -nom_x[1] + true_x[1]; - out_4788524825292355547[2] = -nom_x[2] + true_x[2]; - out_4788524825292355547[3] = -nom_x[3] + true_x[3]; - out_4788524825292355547[4] = -nom_x[4] + true_x[4]; - out_4788524825292355547[5] = -nom_x[5] + true_x[5]; - out_4788524825292355547[6] = -nom_x[6] + true_x[6]; - out_4788524825292355547[7] = -nom_x[7] + true_x[7]; - out_4788524825292355547[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3529295667634364975) { + out_3529295667634364975[0] = -nom_x[0] + true_x[0]; + out_3529295667634364975[1] = -nom_x[1] + true_x[1]; + out_3529295667634364975[2] = -nom_x[2] + true_x[2]; + out_3529295667634364975[3] = -nom_x[3] + true_x[3]; + out_3529295667634364975[4] = -nom_x[4] + true_x[4]; + out_3529295667634364975[5] = -nom_x[5] + true_x[5]; + out_3529295667634364975[6] = -nom_x[6] + true_x[6]; + out_3529295667634364975[7] = -nom_x[7] + true_x[7]; + out_3529295667634364975[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_5143375091155045702) { - out_5143375091155045702[0] = 1.0; - out_5143375091155045702[1] = 0.0; - out_5143375091155045702[2] = 0.0; - out_5143375091155045702[3] = 0.0; - out_5143375091155045702[4] = 0.0; - out_5143375091155045702[5] = 0.0; - out_5143375091155045702[6] = 0.0; - out_5143375091155045702[7] = 0.0; - out_5143375091155045702[8] = 0.0; - out_5143375091155045702[9] = 0.0; - out_5143375091155045702[10] = 1.0; - out_5143375091155045702[11] = 0.0; - out_5143375091155045702[12] = 0.0; - out_5143375091155045702[13] = 0.0; - out_5143375091155045702[14] = 0.0; - out_5143375091155045702[15] = 0.0; - out_5143375091155045702[16] = 0.0; - out_5143375091155045702[17] = 0.0; - out_5143375091155045702[18] = 0.0; - out_5143375091155045702[19] = 0.0; - out_5143375091155045702[20] = 1.0; - out_5143375091155045702[21] = 0.0; - out_5143375091155045702[22] = 0.0; - out_5143375091155045702[23] = 0.0; - out_5143375091155045702[24] = 0.0; - out_5143375091155045702[25] = 0.0; - out_5143375091155045702[26] = 0.0; - out_5143375091155045702[27] = 0.0; - out_5143375091155045702[28] = 0.0; - out_5143375091155045702[29] = 0.0; - out_5143375091155045702[30] = 1.0; - out_5143375091155045702[31] = 0.0; - out_5143375091155045702[32] = 0.0; - out_5143375091155045702[33] = 0.0; - out_5143375091155045702[34] = 0.0; - out_5143375091155045702[35] = 0.0; - out_5143375091155045702[36] = 0.0; - out_5143375091155045702[37] = 0.0; - out_5143375091155045702[38] = 0.0; - out_5143375091155045702[39] = 0.0; - out_5143375091155045702[40] = 1.0; - out_5143375091155045702[41] = 0.0; - out_5143375091155045702[42] = 0.0; - out_5143375091155045702[43] = 0.0; - out_5143375091155045702[44] = 0.0; - out_5143375091155045702[45] = 0.0; - out_5143375091155045702[46] = 0.0; - out_5143375091155045702[47] = 0.0; - out_5143375091155045702[48] = 0.0; - out_5143375091155045702[49] = 0.0; - out_5143375091155045702[50] = 1.0; - out_5143375091155045702[51] = 0.0; - out_5143375091155045702[52] = 0.0; - out_5143375091155045702[53] = 0.0; - out_5143375091155045702[54] = 0.0; - out_5143375091155045702[55] = 0.0; - out_5143375091155045702[56] = 0.0; - out_5143375091155045702[57] = 0.0; - out_5143375091155045702[58] = 0.0; - out_5143375091155045702[59] = 0.0; - out_5143375091155045702[60] = 1.0; - out_5143375091155045702[61] = 0.0; - out_5143375091155045702[62] = 0.0; - out_5143375091155045702[63] = 0.0; - out_5143375091155045702[64] = 0.0; - out_5143375091155045702[65] = 0.0; - out_5143375091155045702[66] = 0.0; - out_5143375091155045702[67] = 0.0; - out_5143375091155045702[68] = 0.0; - out_5143375091155045702[69] = 0.0; - out_5143375091155045702[70] = 1.0; - out_5143375091155045702[71] = 0.0; - out_5143375091155045702[72] = 0.0; - out_5143375091155045702[73] = 0.0; - out_5143375091155045702[74] = 0.0; - out_5143375091155045702[75] = 0.0; - out_5143375091155045702[76] = 0.0; - out_5143375091155045702[77] = 0.0; - out_5143375091155045702[78] = 0.0; - out_5143375091155045702[79] = 0.0; - out_5143375091155045702[80] = 1.0; +void H_mod_fun(double *state, double *out_5031874340838454569) { + out_5031874340838454569[0] = 1.0; + out_5031874340838454569[1] = 0.0; + out_5031874340838454569[2] = 0.0; + out_5031874340838454569[3] = 0.0; + out_5031874340838454569[4] = 0.0; + out_5031874340838454569[5] = 0.0; + out_5031874340838454569[6] = 0.0; + out_5031874340838454569[7] = 0.0; + out_5031874340838454569[8] = 0.0; + out_5031874340838454569[9] = 0.0; + out_5031874340838454569[10] = 1.0; + out_5031874340838454569[11] = 0.0; + out_5031874340838454569[12] = 0.0; + out_5031874340838454569[13] = 0.0; + out_5031874340838454569[14] = 0.0; + out_5031874340838454569[15] = 0.0; + out_5031874340838454569[16] = 0.0; + out_5031874340838454569[17] = 0.0; + out_5031874340838454569[18] = 0.0; + out_5031874340838454569[19] = 0.0; + out_5031874340838454569[20] = 1.0; + out_5031874340838454569[21] = 0.0; + out_5031874340838454569[22] = 0.0; + out_5031874340838454569[23] = 0.0; + out_5031874340838454569[24] = 0.0; + out_5031874340838454569[25] = 0.0; + out_5031874340838454569[26] = 0.0; + out_5031874340838454569[27] = 0.0; + out_5031874340838454569[28] = 0.0; + out_5031874340838454569[29] = 0.0; + out_5031874340838454569[30] = 1.0; + out_5031874340838454569[31] = 0.0; + out_5031874340838454569[32] = 0.0; + out_5031874340838454569[33] = 0.0; + out_5031874340838454569[34] = 0.0; + out_5031874340838454569[35] = 0.0; + out_5031874340838454569[36] = 0.0; + out_5031874340838454569[37] = 0.0; + out_5031874340838454569[38] = 0.0; + out_5031874340838454569[39] = 0.0; + out_5031874340838454569[40] = 1.0; + out_5031874340838454569[41] = 0.0; + out_5031874340838454569[42] = 0.0; + out_5031874340838454569[43] = 0.0; + out_5031874340838454569[44] = 0.0; + out_5031874340838454569[45] = 0.0; + out_5031874340838454569[46] = 0.0; + out_5031874340838454569[47] = 0.0; + out_5031874340838454569[48] = 0.0; + out_5031874340838454569[49] = 0.0; + out_5031874340838454569[50] = 1.0; + out_5031874340838454569[51] = 0.0; + out_5031874340838454569[52] = 0.0; + out_5031874340838454569[53] = 0.0; + out_5031874340838454569[54] = 0.0; + out_5031874340838454569[55] = 0.0; + out_5031874340838454569[56] = 0.0; + out_5031874340838454569[57] = 0.0; + out_5031874340838454569[58] = 0.0; + out_5031874340838454569[59] = 0.0; + out_5031874340838454569[60] = 1.0; + out_5031874340838454569[61] = 0.0; + out_5031874340838454569[62] = 0.0; + out_5031874340838454569[63] = 0.0; + out_5031874340838454569[64] = 0.0; + out_5031874340838454569[65] = 0.0; + out_5031874340838454569[66] = 0.0; + out_5031874340838454569[67] = 0.0; + out_5031874340838454569[68] = 0.0; + out_5031874340838454569[69] = 0.0; + out_5031874340838454569[70] = 1.0; + out_5031874340838454569[71] = 0.0; + out_5031874340838454569[72] = 0.0; + out_5031874340838454569[73] = 0.0; + out_5031874340838454569[74] = 0.0; + out_5031874340838454569[75] = 0.0; + out_5031874340838454569[76] = 0.0; + out_5031874340838454569[77] = 0.0; + out_5031874340838454569[78] = 0.0; + out_5031874340838454569[79] = 0.0; + out_5031874340838454569[80] = 1.0; } -void f_fun(double *state, double dt, double *out_223721292699517100) { - out_223721292699517100[0] = state[0]; - out_223721292699517100[1] = state[1]; - out_223721292699517100[2] = state[2]; - out_223721292699517100[3] = state[3]; - out_223721292699517100[4] = state[4]; - out_223721292699517100[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_223721292699517100[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_223721292699517100[7] = state[7]; - out_223721292699517100[8] = state[8]; +void f_fun(double *state, double dt, double *out_1529561136835295758) { + out_1529561136835295758[0] = state[0]; + out_1529561136835295758[1] = state[1]; + out_1529561136835295758[2] = state[2]; + out_1529561136835295758[3] = state[3]; + out_1529561136835295758[4] = state[4]; + out_1529561136835295758[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_1529561136835295758[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_1529561136835295758[7] = state[7]; + out_1529561136835295758[8] = state[8]; } -void F_fun(double *state, double dt, double *out_6998880572782646555) { - out_6998880572782646555[0] = 1; - out_6998880572782646555[1] = 0; - out_6998880572782646555[2] = 0; - out_6998880572782646555[3] = 0; - out_6998880572782646555[4] = 0; - out_6998880572782646555[5] = 0; - out_6998880572782646555[6] = 0; - out_6998880572782646555[7] = 0; - out_6998880572782646555[8] = 0; - out_6998880572782646555[9] = 0; - out_6998880572782646555[10] = 1; - out_6998880572782646555[11] = 0; - out_6998880572782646555[12] = 0; - out_6998880572782646555[13] = 0; - out_6998880572782646555[14] = 0; - out_6998880572782646555[15] = 0; - out_6998880572782646555[16] = 0; - out_6998880572782646555[17] = 0; - out_6998880572782646555[18] = 0; - out_6998880572782646555[19] = 0; - out_6998880572782646555[20] = 1; - out_6998880572782646555[21] = 0; - out_6998880572782646555[22] = 0; - out_6998880572782646555[23] = 0; - out_6998880572782646555[24] = 0; - out_6998880572782646555[25] = 0; - out_6998880572782646555[26] = 0; - out_6998880572782646555[27] = 0; - out_6998880572782646555[28] = 0; - out_6998880572782646555[29] = 0; - out_6998880572782646555[30] = 1; - out_6998880572782646555[31] = 0; - out_6998880572782646555[32] = 0; - out_6998880572782646555[33] = 0; - out_6998880572782646555[34] = 0; - out_6998880572782646555[35] = 0; - out_6998880572782646555[36] = 0; - out_6998880572782646555[37] = 0; - out_6998880572782646555[38] = 0; - out_6998880572782646555[39] = 0; - out_6998880572782646555[40] = 1; - out_6998880572782646555[41] = 0; - out_6998880572782646555[42] = 0; - out_6998880572782646555[43] = 0; - out_6998880572782646555[44] = 0; - out_6998880572782646555[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_6998880572782646555[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_6998880572782646555[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6998880572782646555[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6998880572782646555[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_6998880572782646555[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_6998880572782646555[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_6998880572782646555[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_6998880572782646555[53] = -9.8100000000000005*dt; - out_6998880572782646555[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_6998880572782646555[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_6998880572782646555[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6998880572782646555[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6998880572782646555[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_6998880572782646555[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_6998880572782646555[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_6998880572782646555[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6998880572782646555[62] = 0; - out_6998880572782646555[63] = 0; - out_6998880572782646555[64] = 0; - out_6998880572782646555[65] = 0; - out_6998880572782646555[66] = 0; - out_6998880572782646555[67] = 0; - out_6998880572782646555[68] = 0; - out_6998880572782646555[69] = 0; - out_6998880572782646555[70] = 1; - out_6998880572782646555[71] = 0; - out_6998880572782646555[72] = 0; - out_6998880572782646555[73] = 0; - out_6998880572782646555[74] = 0; - out_6998880572782646555[75] = 0; - out_6998880572782646555[76] = 0; - out_6998880572782646555[77] = 0; - out_6998880572782646555[78] = 0; - out_6998880572782646555[79] = 0; - out_6998880572782646555[80] = 1; +void F_fun(double *state, double dt, double *out_665605705060988729) { + out_665605705060988729[0] = 1; + out_665605705060988729[1] = 0; + out_665605705060988729[2] = 0; + out_665605705060988729[3] = 0; + out_665605705060988729[4] = 0; + out_665605705060988729[5] = 0; + out_665605705060988729[6] = 0; + out_665605705060988729[7] = 0; + out_665605705060988729[8] = 0; + out_665605705060988729[9] = 0; + out_665605705060988729[10] = 1; + out_665605705060988729[11] = 0; + out_665605705060988729[12] = 0; + out_665605705060988729[13] = 0; + out_665605705060988729[14] = 0; + out_665605705060988729[15] = 0; + out_665605705060988729[16] = 0; + out_665605705060988729[17] = 0; + out_665605705060988729[18] = 0; + out_665605705060988729[19] = 0; + out_665605705060988729[20] = 1; + out_665605705060988729[21] = 0; + out_665605705060988729[22] = 0; + out_665605705060988729[23] = 0; + out_665605705060988729[24] = 0; + out_665605705060988729[25] = 0; + out_665605705060988729[26] = 0; + out_665605705060988729[27] = 0; + out_665605705060988729[28] = 0; + out_665605705060988729[29] = 0; + out_665605705060988729[30] = 1; + out_665605705060988729[31] = 0; + out_665605705060988729[32] = 0; + out_665605705060988729[33] = 0; + out_665605705060988729[34] = 0; + out_665605705060988729[35] = 0; + out_665605705060988729[36] = 0; + out_665605705060988729[37] = 0; + out_665605705060988729[38] = 0; + out_665605705060988729[39] = 0; + out_665605705060988729[40] = 1; + out_665605705060988729[41] = 0; + out_665605705060988729[42] = 0; + out_665605705060988729[43] = 0; + out_665605705060988729[44] = 0; + out_665605705060988729[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_665605705060988729[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_665605705060988729[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_665605705060988729[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_665605705060988729[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_665605705060988729[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_665605705060988729[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_665605705060988729[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_665605705060988729[53] = -9.8100000000000005*dt; + out_665605705060988729[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_665605705060988729[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_665605705060988729[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_665605705060988729[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_665605705060988729[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_665605705060988729[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_665605705060988729[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_665605705060988729[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_665605705060988729[62] = 0; + out_665605705060988729[63] = 0; + out_665605705060988729[64] = 0; + out_665605705060988729[65] = 0; + out_665605705060988729[66] = 0; + out_665605705060988729[67] = 0; + out_665605705060988729[68] = 0; + out_665605705060988729[69] = 0; + out_665605705060988729[70] = 1; + out_665605705060988729[71] = 0; + out_665605705060988729[72] = 0; + out_665605705060988729[73] = 0; + out_665605705060988729[74] = 0; + out_665605705060988729[75] = 0; + out_665605705060988729[76] = 0; + out_665605705060988729[77] = 0; + out_665605705060988729[78] = 0; + out_665605705060988729[79] = 0; + out_665605705060988729[80] = 1; } -void h_25(double *state, double *unused, double *out_6606540369410235194) { - out_6606540369410235194[0] = state[6]; +void h_25(double *state, double *unused, double *out_2720961962516974745) { + out_2720961962516974745[0] = state[6]; } -void H_25(double *state, double *unused, double *out_8666423729800410850) { - out_8666423729800410850[0] = 0; - out_8666423729800410850[1] = 0; - out_8666423729800410850[2] = 0; - out_8666423729800410850[3] = 0; - out_8666423729800410850[4] = 0; - out_8666423729800410850[5] = 0; - out_8666423729800410850[6] = 1; - out_8666423729800410850[7] = 0; - out_8666423729800410850[8] = 0; +void H_25(double *state, double *unused, double *out_5863506002035943510) { + out_5863506002035943510[0] = 0; + out_5863506002035943510[1] = 0; + out_5863506002035943510[2] = 0; + out_5863506002035943510[3] = 0; + out_5863506002035943510[4] = 0; + out_5863506002035943510[5] = 0; + out_5863506002035943510[6] = 1; + out_5863506002035943510[7] = 0; + out_5863506002035943510[8] = 0; } -void h_24(double *state, double *unused, double *out_4049880123285190269) { - out_4049880123285190269[0] = state[4]; - out_4049880123285190269[1] = state[5]; +void h_24(double *state, double *unused, double *out_1485787421513289501) { + out_1485787421513289501[0] = state[4]; + out_1485787421513289501[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5108498130998087714) { - out_5108498130998087714[0] = 0; - out_5108498130998087714[1] = 0; - out_5108498130998087714[2] = 0; - out_5108498130998087714[3] = 0; - out_5108498130998087714[4] = 1; - out_5108498130998087714[5] = 0; - out_5108498130998087714[6] = 0; - out_5108498130998087714[7] = 0; - out_5108498130998087714[8] = 0; - out_5108498130998087714[9] = 0; - out_5108498130998087714[10] = 0; - out_5108498130998087714[11] = 0; - out_5108498130998087714[12] = 0; - out_5108498130998087714[13] = 0; - out_5108498130998087714[14] = 1; - out_5108498130998087714[15] = 0; - out_5108498130998087714[16] = 0; - out_5108498130998087714[17] = 0; +void H_24(double *state, double *unused, double *out_1554885631640983383) { + out_1554885631640983383[0] = 0; + out_1554885631640983383[1] = 0; + out_1554885631640983383[2] = 0; + out_1554885631640983383[3] = 0; + out_1554885631640983383[4] = 1; + out_1554885631640983383[5] = 0; + out_1554885631640983383[6] = 0; + out_1554885631640983383[7] = 0; + out_1554885631640983383[8] = 0; + out_1554885631640983383[9] = 0; + out_1554885631640983383[10] = 0; + out_1554885631640983383[11] = 0; + out_1554885631640983383[12] = 0; + out_1554885631640983383[13] = 0; + out_1554885631640983383[14] = 1; + out_1554885631640983383[15] = 0; + out_1554885631640983383[16] = 0; + out_1554885631640983383[17] = 0; } -void h_30(double *state, double *unused, double *out_164294856940115742) { - out_164294856940115742[0] = state[4]; +void h_30(double *state, double *unused, double *out_2878148630868103890) { + out_2878148630868103890[0] = state[4]; } -void H_30(double *state, double *unused, double *out_1749733388308794095) { - out_1749733388308794095[0] = 0; - out_1749733388308794095[1] = 0; - out_1749733388308794095[2] = 0; - out_1749733388308794095[3] = 0; - out_1749733388308794095[4] = 1; - out_1749733388308794095[5] = 0; - out_1749733388308794095[6] = 0; - out_1749733388308794095[7] = 0; - out_1749733388308794095[8] = 0; +void H_30(double *state, double *unused, double *out_5992844949179183580) { + out_5992844949179183580[0] = 0; + out_5992844949179183580[1] = 0; + out_5992844949179183580[2] = 0; + out_5992844949179183580[3] = 0; + out_5992844949179183580[4] = 1; + out_5992844949179183580[5] = 0; + out_5992844949179183580[6] = 0; + out_5992844949179183580[7] = 0; + out_5992844949179183580[8] = 0; } -void h_26(double *state, double *unused, double *out_767610741616593259) { - out_767610741616593259[0] = state[7]; +void h_26(double *state, double *unused, double *out_158637420576228795) { + out_158637420576228795[0] = state[7]; } -void H_26(double *state, double *unused, double *out_6038817025035084542) { - out_6038817025035084542[0] = 0; - out_6038817025035084542[1] = 0; - out_6038817025035084542[2] = 0; - out_6038817025035084542[3] = 0; - out_6038817025035084542[4] = 0; - out_6038817025035084542[5] = 0; - out_6038817025035084542[6] = 0; - out_6038817025035084542[7] = 1; - out_6038817025035084542[8] = 0; +void H_26(double *state, double *unused, double *out_8841734752799551882) { + out_8841734752799551882[0] = 0; + out_8841734752799551882[1] = 0; + out_8841734752799551882[2] = 0; + out_8841734752799551882[3] = 0; + out_8841734752799551882[4] = 0; + out_8841734752799551882[5] = 0; + out_8841734752799551882[6] = 0; + out_8841734752799551882[7] = 1; + out_8841734752799551882[8] = 0; } -void h_27(double *state, double *unused, double *out_9132294156050629254) { - out_9132294156050629254[0] = state[3]; +void h_27(double *state, double *unused, double *out_2280711305143870900) { + out_2280711305143870900[0] = state[3]; } -void H_27(double *state, double *unused, double *out_3924496700109219006) { - out_3924496700109219006[0] = 0; - out_3924496700109219006[1] = 0; - out_3924496700109219006[2] = 0; - out_3924496700109219006[3] = 1; - out_3924496700109219006[4] = 0; - out_3924496700109219006[5] = 0; - out_3924496700109219006[6] = 0; - out_3924496700109219006[7] = 0; - out_3924496700109219006[8] = 0; +void H_27(double *state, double *unused, double *out_8167608260979608491) { + out_8167608260979608491[0] = 0; + out_8167608260979608491[1] = 0; + out_8167608260979608491[2] = 0; + out_8167608260979608491[3] = 1; + out_8167608260979608491[4] = 0; + out_8167608260979608491[5] = 0; + out_8167608260979608491[6] = 0; + out_8167608260979608491[7] = 0; + out_8167608260979608491[8] = 0; } -void h_29(double *state, double *unused, double *out_2543614691308571426) { - out_2543614691308571426[0] = state[1]; +void h_29(double *state, double *unused, double *out_6142394781572372552) { + out_6142394781572372552[0] = state[1]; } -void H_29(double *state, double *unused, double *out_5637859426978770039) { - out_5637859426978770039[0] = 0; - out_5637859426978770039[1] = 1; - out_5637859426978770039[2] = 0; - out_5637859426978770039[3] = 0; - out_5637859426978770039[4] = 0; - out_5637859426978770039[5] = 0; - out_5637859426978770039[6] = 0; - out_5637859426978770039[7] = 0; - out_5637859426978770039[8] = 0; +void H_29(double *state, double *unused, double *out_5482613604864791396) { + out_5482613604864791396[0] = 0; + out_5482613604864791396[1] = 1; + out_5482613604864791396[2] = 0; + out_5482613604864791396[3] = 0; + out_5482613604864791396[4] = 0; + out_5482613604864791396[5] = 0; + out_5482613604864791396[6] = 0; + out_5482613604864791396[7] = 0; + out_5482613604864791396[8] = 0; } -void h_28(double *state, double *unused, double *out_3987404092934981717) { - out_3987404092934981717[0] = state[0]; +void h_28(double *state, double *unused, double *out_6701257866862969865) { + out_6701257866862969865[0] = state[0]; } -void H_28(double *state, double *unused, double *out_7726485629661251003) { - out_7726485629661251003[0] = 1; - out_7726485629661251003[1] = 0; - out_7726485629661251003[2] = 0; - out_7726485629661251003[3] = 0; - out_7726485629661251003[4] = 0; - out_7726485629661251003[5] = 0; - out_7726485629661251003[6] = 0; - out_7726485629661251003[7] = 0; - out_7726485629661251003[8] = 0; +void H_28(double *state, double *unused, double *out_7881731451775229646) { + out_7881731451775229646[0] = 1; + out_7881731451775229646[1] = 0; + out_7881731451775229646[2] = 0; + out_7881731451775229646[3] = 0; + out_7881731451775229646[4] = 0; + out_7881731451775229646[5] = 0; + out_7881731451775229646[6] = 0; + out_7881731451775229646[7] = 0; + out_7881731451775229646[8] = 0; } -void h_31(double *state, double *unused, double *out_6594936115065281760) { - out_6594936115065281760[0] = state[8]; +void h_31(double *state, double *unused, double *out_4049873263833376191) { + out_4049873263833376191[0] = state[8]; } -void H_31(double *state, double *unused, double *out_8635777767923450422) { - out_8635777767923450422[0] = 0; - out_8635777767923450422[1] = 0; - out_8635777767923450422[2] = 0; - out_8635777767923450422[3] = 0; - out_8635777767923450422[4] = 0; - out_8635777767923450422[5] = 0; - out_8635777767923450422[6] = 0; - out_8635777767923450422[7] = 0; - out_8635777767923450422[8] = 1; +void H_31(double *state, double *unused, double *out_5832860040158983082) { + out_5832860040158983082[0] = 0; + out_5832860040158983082[1] = 0; + out_5832860040158983082[2] = 0; + out_5832860040158983082[3] = 0; + out_5832860040158983082[4] = 0; + out_5832860040158983082[5] = 0; + out_5832860040158983082[6] = 0; + out_5832860040158983082[7] = 0; + out_5832860040158983082[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_2249508041139738373) { - err_fun(nom_x, delta_x, out_2249508041139738373); +void car_err_fun(double *nom_x, double *delta_x, double *out_1263767819732456823) { + err_fun(nom_x, delta_x, out_1263767819732456823); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4788524825292355547) { - inv_err_fun(nom_x, true_x, out_4788524825292355547); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3529295667634364975) { + inv_err_fun(nom_x, true_x, out_3529295667634364975); } -void car_H_mod_fun(double *state, double *out_5143375091155045702) { - H_mod_fun(state, out_5143375091155045702); +void car_H_mod_fun(double *state, double *out_5031874340838454569) { + H_mod_fun(state, out_5031874340838454569); } -void car_f_fun(double *state, double dt, double *out_223721292699517100) { - f_fun(state, dt, out_223721292699517100); +void car_f_fun(double *state, double dt, double *out_1529561136835295758) { + f_fun(state, dt, out_1529561136835295758); } -void car_F_fun(double *state, double dt, double *out_6998880572782646555) { - F_fun(state, dt, out_6998880572782646555); +void car_F_fun(double *state, double dt, double *out_665605705060988729) { + F_fun(state, dt, out_665605705060988729); } -void car_h_25(double *state, double *unused, double *out_6606540369410235194) { - h_25(state, unused, out_6606540369410235194); +void car_h_25(double *state, double *unused, double *out_2720961962516974745) { + h_25(state, unused, out_2720961962516974745); } -void car_H_25(double *state, double *unused, double *out_8666423729800410850) { - H_25(state, unused, out_8666423729800410850); +void car_H_25(double *state, double *unused, double *out_5863506002035943510) { + H_25(state, unused, out_5863506002035943510); } -void car_h_24(double *state, double *unused, double *out_4049880123285190269) { - h_24(state, unused, out_4049880123285190269); +void car_h_24(double *state, double *unused, double *out_1485787421513289501) { + h_24(state, unused, out_1485787421513289501); } -void car_H_24(double *state, double *unused, double *out_5108498130998087714) { - H_24(state, unused, out_5108498130998087714); +void car_H_24(double *state, double *unused, double *out_1554885631640983383) { + H_24(state, unused, out_1554885631640983383); } -void car_h_30(double *state, double *unused, double *out_164294856940115742) { - h_30(state, unused, out_164294856940115742); +void car_h_30(double *state, double *unused, double *out_2878148630868103890) { + h_30(state, unused, out_2878148630868103890); } -void car_H_30(double *state, double *unused, double *out_1749733388308794095) { - H_30(state, unused, out_1749733388308794095); +void car_H_30(double *state, double *unused, double *out_5992844949179183580) { + H_30(state, unused, out_5992844949179183580); } -void car_h_26(double *state, double *unused, double *out_767610741616593259) { - h_26(state, unused, out_767610741616593259); +void car_h_26(double *state, double *unused, double *out_158637420576228795) { + h_26(state, unused, out_158637420576228795); } -void car_H_26(double *state, double *unused, double *out_6038817025035084542) { - H_26(state, unused, out_6038817025035084542); +void car_H_26(double *state, double *unused, double *out_8841734752799551882) { + H_26(state, unused, out_8841734752799551882); } -void car_h_27(double *state, double *unused, double *out_9132294156050629254) { - h_27(state, unused, out_9132294156050629254); +void car_h_27(double *state, double *unused, double *out_2280711305143870900) { + h_27(state, unused, out_2280711305143870900); } -void car_H_27(double *state, double *unused, double *out_3924496700109219006) { - H_27(state, unused, out_3924496700109219006); +void car_H_27(double *state, double *unused, double *out_8167608260979608491) { + H_27(state, unused, out_8167608260979608491); } -void car_h_29(double *state, double *unused, double *out_2543614691308571426) { - h_29(state, unused, out_2543614691308571426); +void car_h_29(double *state, double *unused, double *out_6142394781572372552) { + h_29(state, unused, out_6142394781572372552); } -void car_H_29(double *state, double *unused, double *out_5637859426978770039) { - H_29(state, unused, out_5637859426978770039); +void car_H_29(double *state, double *unused, double *out_5482613604864791396) { + H_29(state, unused, out_5482613604864791396); } -void car_h_28(double *state, double *unused, double *out_3987404092934981717) { - h_28(state, unused, out_3987404092934981717); +void car_h_28(double *state, double *unused, double *out_6701257866862969865) { + h_28(state, unused, out_6701257866862969865); } -void car_H_28(double *state, double *unused, double *out_7726485629661251003) { - H_28(state, unused, out_7726485629661251003); +void car_H_28(double *state, double *unused, double *out_7881731451775229646) { + H_28(state, unused, out_7881731451775229646); } -void car_h_31(double *state, double *unused, double *out_6594936115065281760) { - h_31(state, unused, out_6594936115065281760); +void car_h_31(double *state, double *unused, double *out_4049873263833376191) { + h_31(state, unused, out_4049873263833376191); } -void car_H_31(double *state, double *unused, double *out_8635777767923450422) { - H_31(state, unused, out_8635777767923450422); +void car_H_31(double *state, double *unused, double *out_5832860040158983082) { + H_31(state, unused, out_5832860040158983082); } 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 1164d5f5db..7afaee74a3 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_2249508041139738373); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4788524825292355547); -void car_H_mod_fun(double *state, double *out_5143375091155045702); -void car_f_fun(double *state, double dt, double *out_223721292699517100); -void car_F_fun(double *state, double dt, double *out_6998880572782646555); -void car_h_25(double *state, double *unused, double *out_6606540369410235194); -void car_H_25(double *state, double *unused, double *out_8666423729800410850); -void car_h_24(double *state, double *unused, double *out_4049880123285190269); -void car_H_24(double *state, double *unused, double *out_5108498130998087714); -void car_h_30(double *state, double *unused, double *out_164294856940115742); -void car_H_30(double *state, double *unused, double *out_1749733388308794095); -void car_h_26(double *state, double *unused, double *out_767610741616593259); -void car_H_26(double *state, double *unused, double *out_6038817025035084542); -void car_h_27(double *state, double *unused, double *out_9132294156050629254); -void car_H_27(double *state, double *unused, double *out_3924496700109219006); -void car_h_29(double *state, double *unused, double *out_2543614691308571426); -void car_H_29(double *state, double *unused, double *out_5637859426978770039); -void car_h_28(double *state, double *unused, double *out_3987404092934981717); -void car_H_28(double *state, double *unused, double *out_7726485629661251003); -void car_h_31(double *state, double *unused, double *out_6594936115065281760); -void car_H_31(double *state, double *unused, double *out_8635777767923450422); +void car_err_fun(double *nom_x, double *delta_x, double *out_1263767819732456823); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3529295667634364975); +void car_H_mod_fun(double *state, double *out_5031874340838454569); +void car_f_fun(double *state, double dt, double *out_1529561136835295758); +void car_F_fun(double *state, double dt, double *out_665605705060988729); +void car_h_25(double *state, double *unused, double *out_2720961962516974745); +void car_H_25(double *state, double *unused, double *out_5863506002035943510); +void car_h_24(double *state, double *unused, double *out_1485787421513289501); +void car_H_24(double *state, double *unused, double *out_1554885631640983383); +void car_h_30(double *state, double *unused, double *out_2878148630868103890); +void car_H_30(double *state, double *unused, double *out_5992844949179183580); +void car_h_26(double *state, double *unused, double *out_158637420576228795); +void car_H_26(double *state, double *unused, double *out_8841734752799551882); +void car_h_27(double *state, double *unused, double *out_2280711305143870900); +void car_H_27(double *state, double *unused, double *out_8167608260979608491); +void car_h_29(double *state, double *unused, double *out_6142394781572372552); +void car_H_29(double *state, double *unused, double *out_5482613604864791396); +void car_h_28(double *state, double *unused, double *out_6701257866862969865); +void car_H_28(double *state, double *unused, double *out_7881731451775229646); +void car_h_31(double *state, double *unused, double *out_4049873263833376191); +void car_H_31(double *state, double *unused, double *out_5832860040158983082); 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 2236edcb11..08116cb2e5 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_6079659893886918220) { - out_6079659893886918220[0] = delta_x[0] + nom_x[0]; - out_6079659893886918220[1] = delta_x[1] + nom_x[1]; - out_6079659893886918220[2] = delta_x[2] + nom_x[2]; - out_6079659893886918220[3] = delta_x[3] + nom_x[3]; - out_6079659893886918220[4] = delta_x[4] + nom_x[4]; - out_6079659893886918220[5] = delta_x[5] + nom_x[5]; - out_6079659893886918220[6] = delta_x[6] + nom_x[6]; - out_6079659893886918220[7] = delta_x[7] + nom_x[7]; - out_6079659893886918220[8] = delta_x[8] + nom_x[8]; - out_6079659893886918220[9] = delta_x[9] + nom_x[9]; - out_6079659893886918220[10] = delta_x[10] + nom_x[10]; - out_6079659893886918220[11] = delta_x[11] + nom_x[11]; - out_6079659893886918220[12] = delta_x[12] + nom_x[12]; - out_6079659893886918220[13] = delta_x[13] + nom_x[13]; - out_6079659893886918220[14] = delta_x[14] + nom_x[14]; - out_6079659893886918220[15] = delta_x[15] + nom_x[15]; - out_6079659893886918220[16] = delta_x[16] + nom_x[16]; - out_6079659893886918220[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_8550489212526755506) { + out_8550489212526755506[0] = delta_x[0] + nom_x[0]; + out_8550489212526755506[1] = delta_x[1] + nom_x[1]; + out_8550489212526755506[2] = delta_x[2] + nom_x[2]; + out_8550489212526755506[3] = delta_x[3] + nom_x[3]; + out_8550489212526755506[4] = delta_x[4] + nom_x[4]; + out_8550489212526755506[5] = delta_x[5] + nom_x[5]; + out_8550489212526755506[6] = delta_x[6] + nom_x[6]; + out_8550489212526755506[7] = delta_x[7] + nom_x[7]; + out_8550489212526755506[8] = delta_x[8] + nom_x[8]; + out_8550489212526755506[9] = delta_x[9] + nom_x[9]; + out_8550489212526755506[10] = delta_x[10] + nom_x[10]; + out_8550489212526755506[11] = delta_x[11] + nom_x[11]; + out_8550489212526755506[12] = delta_x[12] + nom_x[12]; + out_8550489212526755506[13] = delta_x[13] + nom_x[13]; + out_8550489212526755506[14] = delta_x[14] + nom_x[14]; + out_8550489212526755506[15] = delta_x[15] + nom_x[15]; + out_8550489212526755506[16] = delta_x[16] + nom_x[16]; + out_8550489212526755506[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5200837164005369107) { - out_5200837164005369107[0] = -nom_x[0] + true_x[0]; - out_5200837164005369107[1] = -nom_x[1] + true_x[1]; - out_5200837164005369107[2] = -nom_x[2] + true_x[2]; - out_5200837164005369107[3] = -nom_x[3] + true_x[3]; - out_5200837164005369107[4] = -nom_x[4] + true_x[4]; - out_5200837164005369107[5] = -nom_x[5] + true_x[5]; - out_5200837164005369107[6] = -nom_x[6] + true_x[6]; - out_5200837164005369107[7] = -nom_x[7] + true_x[7]; - out_5200837164005369107[8] = -nom_x[8] + true_x[8]; - out_5200837164005369107[9] = -nom_x[9] + true_x[9]; - out_5200837164005369107[10] = -nom_x[10] + true_x[10]; - out_5200837164005369107[11] = -nom_x[11] + true_x[11]; - out_5200837164005369107[12] = -nom_x[12] + true_x[12]; - out_5200837164005369107[13] = -nom_x[13] + true_x[13]; - out_5200837164005369107[14] = -nom_x[14] + true_x[14]; - out_5200837164005369107[15] = -nom_x[15] + true_x[15]; - out_5200837164005369107[16] = -nom_x[16] + true_x[16]; - out_5200837164005369107[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2197167056715094306) { + out_2197167056715094306[0] = -nom_x[0] + true_x[0]; + out_2197167056715094306[1] = -nom_x[1] + true_x[1]; + out_2197167056715094306[2] = -nom_x[2] + true_x[2]; + out_2197167056715094306[3] = -nom_x[3] + true_x[3]; + out_2197167056715094306[4] = -nom_x[4] + true_x[4]; + out_2197167056715094306[5] = -nom_x[5] + true_x[5]; + out_2197167056715094306[6] = -nom_x[6] + true_x[6]; + out_2197167056715094306[7] = -nom_x[7] + true_x[7]; + out_2197167056715094306[8] = -nom_x[8] + true_x[8]; + out_2197167056715094306[9] = -nom_x[9] + true_x[9]; + out_2197167056715094306[10] = -nom_x[10] + true_x[10]; + out_2197167056715094306[11] = -nom_x[11] + true_x[11]; + out_2197167056715094306[12] = -nom_x[12] + true_x[12]; + out_2197167056715094306[13] = -nom_x[13] + true_x[13]; + out_2197167056715094306[14] = -nom_x[14] + true_x[14]; + out_2197167056715094306[15] = -nom_x[15] + true_x[15]; + out_2197167056715094306[16] = -nom_x[16] + true_x[16]; + out_2197167056715094306[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_1320875044412644660) { - out_1320875044412644660[0] = 1.0; - out_1320875044412644660[1] = 0.0; - out_1320875044412644660[2] = 0.0; - out_1320875044412644660[3] = 0.0; - out_1320875044412644660[4] = 0.0; - out_1320875044412644660[5] = 0.0; - out_1320875044412644660[6] = 0.0; - out_1320875044412644660[7] = 0.0; - out_1320875044412644660[8] = 0.0; - out_1320875044412644660[9] = 0.0; - out_1320875044412644660[10] = 0.0; - out_1320875044412644660[11] = 0.0; - out_1320875044412644660[12] = 0.0; - out_1320875044412644660[13] = 0.0; - out_1320875044412644660[14] = 0.0; - out_1320875044412644660[15] = 0.0; - out_1320875044412644660[16] = 0.0; - out_1320875044412644660[17] = 0.0; - out_1320875044412644660[18] = 0.0; - out_1320875044412644660[19] = 1.0; - out_1320875044412644660[20] = 0.0; - out_1320875044412644660[21] = 0.0; - out_1320875044412644660[22] = 0.0; - out_1320875044412644660[23] = 0.0; - out_1320875044412644660[24] = 0.0; - out_1320875044412644660[25] = 0.0; - out_1320875044412644660[26] = 0.0; - out_1320875044412644660[27] = 0.0; - out_1320875044412644660[28] = 0.0; - out_1320875044412644660[29] = 0.0; - out_1320875044412644660[30] = 0.0; - out_1320875044412644660[31] = 0.0; - out_1320875044412644660[32] = 0.0; - out_1320875044412644660[33] = 0.0; - out_1320875044412644660[34] = 0.0; - out_1320875044412644660[35] = 0.0; - out_1320875044412644660[36] = 0.0; - out_1320875044412644660[37] = 0.0; - out_1320875044412644660[38] = 1.0; - out_1320875044412644660[39] = 0.0; - out_1320875044412644660[40] = 0.0; - out_1320875044412644660[41] = 0.0; - out_1320875044412644660[42] = 0.0; - out_1320875044412644660[43] = 0.0; - out_1320875044412644660[44] = 0.0; - out_1320875044412644660[45] = 0.0; - out_1320875044412644660[46] = 0.0; - out_1320875044412644660[47] = 0.0; - out_1320875044412644660[48] = 0.0; - out_1320875044412644660[49] = 0.0; - out_1320875044412644660[50] = 0.0; - out_1320875044412644660[51] = 0.0; - out_1320875044412644660[52] = 0.0; - out_1320875044412644660[53] = 0.0; - out_1320875044412644660[54] = 0.0; - out_1320875044412644660[55] = 0.0; - out_1320875044412644660[56] = 0.0; - out_1320875044412644660[57] = 1.0; - out_1320875044412644660[58] = 0.0; - out_1320875044412644660[59] = 0.0; - out_1320875044412644660[60] = 0.0; - out_1320875044412644660[61] = 0.0; - out_1320875044412644660[62] = 0.0; - out_1320875044412644660[63] = 0.0; - out_1320875044412644660[64] = 0.0; - out_1320875044412644660[65] = 0.0; - out_1320875044412644660[66] = 0.0; - out_1320875044412644660[67] = 0.0; - out_1320875044412644660[68] = 0.0; - out_1320875044412644660[69] = 0.0; - out_1320875044412644660[70] = 0.0; - out_1320875044412644660[71] = 0.0; - out_1320875044412644660[72] = 0.0; - out_1320875044412644660[73] = 0.0; - out_1320875044412644660[74] = 0.0; - out_1320875044412644660[75] = 0.0; - out_1320875044412644660[76] = 1.0; - out_1320875044412644660[77] = 0.0; - out_1320875044412644660[78] = 0.0; - out_1320875044412644660[79] = 0.0; - out_1320875044412644660[80] = 0.0; - out_1320875044412644660[81] = 0.0; - out_1320875044412644660[82] = 0.0; - out_1320875044412644660[83] = 0.0; - out_1320875044412644660[84] = 0.0; - out_1320875044412644660[85] = 0.0; - out_1320875044412644660[86] = 0.0; - out_1320875044412644660[87] = 0.0; - out_1320875044412644660[88] = 0.0; - out_1320875044412644660[89] = 0.0; - out_1320875044412644660[90] = 0.0; - out_1320875044412644660[91] = 0.0; - out_1320875044412644660[92] = 0.0; - out_1320875044412644660[93] = 0.0; - out_1320875044412644660[94] = 0.0; - out_1320875044412644660[95] = 1.0; - out_1320875044412644660[96] = 0.0; - out_1320875044412644660[97] = 0.0; - out_1320875044412644660[98] = 0.0; - out_1320875044412644660[99] = 0.0; - out_1320875044412644660[100] = 0.0; - out_1320875044412644660[101] = 0.0; - out_1320875044412644660[102] = 0.0; - out_1320875044412644660[103] = 0.0; - out_1320875044412644660[104] = 0.0; - out_1320875044412644660[105] = 0.0; - out_1320875044412644660[106] = 0.0; - out_1320875044412644660[107] = 0.0; - out_1320875044412644660[108] = 0.0; - out_1320875044412644660[109] = 0.0; - out_1320875044412644660[110] = 0.0; - out_1320875044412644660[111] = 0.0; - out_1320875044412644660[112] = 0.0; - out_1320875044412644660[113] = 0.0; - out_1320875044412644660[114] = 1.0; - out_1320875044412644660[115] = 0.0; - out_1320875044412644660[116] = 0.0; - out_1320875044412644660[117] = 0.0; - out_1320875044412644660[118] = 0.0; - out_1320875044412644660[119] = 0.0; - out_1320875044412644660[120] = 0.0; - out_1320875044412644660[121] = 0.0; - out_1320875044412644660[122] = 0.0; - out_1320875044412644660[123] = 0.0; - out_1320875044412644660[124] = 0.0; - out_1320875044412644660[125] = 0.0; - out_1320875044412644660[126] = 0.0; - out_1320875044412644660[127] = 0.0; - out_1320875044412644660[128] = 0.0; - out_1320875044412644660[129] = 0.0; - out_1320875044412644660[130] = 0.0; - out_1320875044412644660[131] = 0.0; - out_1320875044412644660[132] = 0.0; - out_1320875044412644660[133] = 1.0; - out_1320875044412644660[134] = 0.0; - out_1320875044412644660[135] = 0.0; - out_1320875044412644660[136] = 0.0; - out_1320875044412644660[137] = 0.0; - out_1320875044412644660[138] = 0.0; - out_1320875044412644660[139] = 0.0; - out_1320875044412644660[140] = 0.0; - out_1320875044412644660[141] = 0.0; - out_1320875044412644660[142] = 0.0; - out_1320875044412644660[143] = 0.0; - out_1320875044412644660[144] = 0.0; - out_1320875044412644660[145] = 0.0; - out_1320875044412644660[146] = 0.0; - out_1320875044412644660[147] = 0.0; - out_1320875044412644660[148] = 0.0; - out_1320875044412644660[149] = 0.0; - out_1320875044412644660[150] = 0.0; - out_1320875044412644660[151] = 0.0; - out_1320875044412644660[152] = 1.0; - out_1320875044412644660[153] = 0.0; - out_1320875044412644660[154] = 0.0; - out_1320875044412644660[155] = 0.0; - out_1320875044412644660[156] = 0.0; - out_1320875044412644660[157] = 0.0; - out_1320875044412644660[158] = 0.0; - out_1320875044412644660[159] = 0.0; - out_1320875044412644660[160] = 0.0; - out_1320875044412644660[161] = 0.0; - out_1320875044412644660[162] = 0.0; - out_1320875044412644660[163] = 0.0; - out_1320875044412644660[164] = 0.0; - out_1320875044412644660[165] = 0.0; - out_1320875044412644660[166] = 0.0; - out_1320875044412644660[167] = 0.0; - out_1320875044412644660[168] = 0.0; - out_1320875044412644660[169] = 0.0; - out_1320875044412644660[170] = 0.0; - out_1320875044412644660[171] = 1.0; - out_1320875044412644660[172] = 0.0; - out_1320875044412644660[173] = 0.0; - out_1320875044412644660[174] = 0.0; - out_1320875044412644660[175] = 0.0; - out_1320875044412644660[176] = 0.0; - out_1320875044412644660[177] = 0.0; - out_1320875044412644660[178] = 0.0; - out_1320875044412644660[179] = 0.0; - out_1320875044412644660[180] = 0.0; - out_1320875044412644660[181] = 0.0; - out_1320875044412644660[182] = 0.0; - out_1320875044412644660[183] = 0.0; - out_1320875044412644660[184] = 0.0; - out_1320875044412644660[185] = 0.0; - out_1320875044412644660[186] = 0.0; - out_1320875044412644660[187] = 0.0; - out_1320875044412644660[188] = 0.0; - out_1320875044412644660[189] = 0.0; - out_1320875044412644660[190] = 1.0; - out_1320875044412644660[191] = 0.0; - out_1320875044412644660[192] = 0.0; - out_1320875044412644660[193] = 0.0; - out_1320875044412644660[194] = 0.0; - out_1320875044412644660[195] = 0.0; - out_1320875044412644660[196] = 0.0; - out_1320875044412644660[197] = 0.0; - out_1320875044412644660[198] = 0.0; - out_1320875044412644660[199] = 0.0; - out_1320875044412644660[200] = 0.0; - out_1320875044412644660[201] = 0.0; - out_1320875044412644660[202] = 0.0; - out_1320875044412644660[203] = 0.0; - out_1320875044412644660[204] = 0.0; - out_1320875044412644660[205] = 0.0; - out_1320875044412644660[206] = 0.0; - out_1320875044412644660[207] = 0.0; - out_1320875044412644660[208] = 0.0; - out_1320875044412644660[209] = 1.0; - out_1320875044412644660[210] = 0.0; - out_1320875044412644660[211] = 0.0; - out_1320875044412644660[212] = 0.0; - out_1320875044412644660[213] = 0.0; - out_1320875044412644660[214] = 0.0; - out_1320875044412644660[215] = 0.0; - out_1320875044412644660[216] = 0.0; - out_1320875044412644660[217] = 0.0; - out_1320875044412644660[218] = 0.0; - out_1320875044412644660[219] = 0.0; - out_1320875044412644660[220] = 0.0; - out_1320875044412644660[221] = 0.0; - out_1320875044412644660[222] = 0.0; - out_1320875044412644660[223] = 0.0; - out_1320875044412644660[224] = 0.0; - out_1320875044412644660[225] = 0.0; - out_1320875044412644660[226] = 0.0; - out_1320875044412644660[227] = 0.0; - out_1320875044412644660[228] = 1.0; - out_1320875044412644660[229] = 0.0; - out_1320875044412644660[230] = 0.0; - out_1320875044412644660[231] = 0.0; - out_1320875044412644660[232] = 0.0; - out_1320875044412644660[233] = 0.0; - out_1320875044412644660[234] = 0.0; - out_1320875044412644660[235] = 0.0; - out_1320875044412644660[236] = 0.0; - out_1320875044412644660[237] = 0.0; - out_1320875044412644660[238] = 0.0; - out_1320875044412644660[239] = 0.0; - out_1320875044412644660[240] = 0.0; - out_1320875044412644660[241] = 0.0; - out_1320875044412644660[242] = 0.0; - out_1320875044412644660[243] = 0.0; - out_1320875044412644660[244] = 0.0; - out_1320875044412644660[245] = 0.0; - out_1320875044412644660[246] = 0.0; - out_1320875044412644660[247] = 1.0; - out_1320875044412644660[248] = 0.0; - out_1320875044412644660[249] = 0.0; - out_1320875044412644660[250] = 0.0; - out_1320875044412644660[251] = 0.0; - out_1320875044412644660[252] = 0.0; - out_1320875044412644660[253] = 0.0; - out_1320875044412644660[254] = 0.0; - out_1320875044412644660[255] = 0.0; - out_1320875044412644660[256] = 0.0; - out_1320875044412644660[257] = 0.0; - out_1320875044412644660[258] = 0.0; - out_1320875044412644660[259] = 0.0; - out_1320875044412644660[260] = 0.0; - out_1320875044412644660[261] = 0.0; - out_1320875044412644660[262] = 0.0; - out_1320875044412644660[263] = 0.0; - out_1320875044412644660[264] = 0.0; - out_1320875044412644660[265] = 0.0; - out_1320875044412644660[266] = 1.0; - out_1320875044412644660[267] = 0.0; - out_1320875044412644660[268] = 0.0; - out_1320875044412644660[269] = 0.0; - out_1320875044412644660[270] = 0.0; - out_1320875044412644660[271] = 0.0; - out_1320875044412644660[272] = 0.0; - out_1320875044412644660[273] = 0.0; - out_1320875044412644660[274] = 0.0; - out_1320875044412644660[275] = 0.0; - out_1320875044412644660[276] = 0.0; - out_1320875044412644660[277] = 0.0; - out_1320875044412644660[278] = 0.0; - out_1320875044412644660[279] = 0.0; - out_1320875044412644660[280] = 0.0; - out_1320875044412644660[281] = 0.0; - out_1320875044412644660[282] = 0.0; - out_1320875044412644660[283] = 0.0; - out_1320875044412644660[284] = 0.0; - out_1320875044412644660[285] = 1.0; - out_1320875044412644660[286] = 0.0; - out_1320875044412644660[287] = 0.0; - out_1320875044412644660[288] = 0.0; - out_1320875044412644660[289] = 0.0; - out_1320875044412644660[290] = 0.0; - out_1320875044412644660[291] = 0.0; - out_1320875044412644660[292] = 0.0; - out_1320875044412644660[293] = 0.0; - out_1320875044412644660[294] = 0.0; - out_1320875044412644660[295] = 0.0; - out_1320875044412644660[296] = 0.0; - out_1320875044412644660[297] = 0.0; - out_1320875044412644660[298] = 0.0; - out_1320875044412644660[299] = 0.0; - out_1320875044412644660[300] = 0.0; - out_1320875044412644660[301] = 0.0; - out_1320875044412644660[302] = 0.0; - out_1320875044412644660[303] = 0.0; - out_1320875044412644660[304] = 1.0; - out_1320875044412644660[305] = 0.0; - out_1320875044412644660[306] = 0.0; - out_1320875044412644660[307] = 0.0; - out_1320875044412644660[308] = 0.0; - out_1320875044412644660[309] = 0.0; - out_1320875044412644660[310] = 0.0; - out_1320875044412644660[311] = 0.0; - out_1320875044412644660[312] = 0.0; - out_1320875044412644660[313] = 0.0; - out_1320875044412644660[314] = 0.0; - out_1320875044412644660[315] = 0.0; - out_1320875044412644660[316] = 0.0; - out_1320875044412644660[317] = 0.0; - out_1320875044412644660[318] = 0.0; - out_1320875044412644660[319] = 0.0; - out_1320875044412644660[320] = 0.0; - out_1320875044412644660[321] = 0.0; - out_1320875044412644660[322] = 0.0; - out_1320875044412644660[323] = 1.0; +void H_mod_fun(double *state, double *out_1273572215837011735) { + out_1273572215837011735[0] = 1.0; + out_1273572215837011735[1] = 0.0; + out_1273572215837011735[2] = 0.0; + out_1273572215837011735[3] = 0.0; + out_1273572215837011735[4] = 0.0; + out_1273572215837011735[5] = 0.0; + out_1273572215837011735[6] = 0.0; + out_1273572215837011735[7] = 0.0; + out_1273572215837011735[8] = 0.0; + out_1273572215837011735[9] = 0.0; + out_1273572215837011735[10] = 0.0; + out_1273572215837011735[11] = 0.0; + out_1273572215837011735[12] = 0.0; + out_1273572215837011735[13] = 0.0; + out_1273572215837011735[14] = 0.0; + out_1273572215837011735[15] = 0.0; + out_1273572215837011735[16] = 0.0; + out_1273572215837011735[17] = 0.0; + out_1273572215837011735[18] = 0.0; + out_1273572215837011735[19] = 1.0; + out_1273572215837011735[20] = 0.0; + out_1273572215837011735[21] = 0.0; + out_1273572215837011735[22] = 0.0; + out_1273572215837011735[23] = 0.0; + out_1273572215837011735[24] = 0.0; + out_1273572215837011735[25] = 0.0; + out_1273572215837011735[26] = 0.0; + out_1273572215837011735[27] = 0.0; + out_1273572215837011735[28] = 0.0; + out_1273572215837011735[29] = 0.0; + out_1273572215837011735[30] = 0.0; + out_1273572215837011735[31] = 0.0; + out_1273572215837011735[32] = 0.0; + out_1273572215837011735[33] = 0.0; + out_1273572215837011735[34] = 0.0; + out_1273572215837011735[35] = 0.0; + out_1273572215837011735[36] = 0.0; + out_1273572215837011735[37] = 0.0; + out_1273572215837011735[38] = 1.0; + out_1273572215837011735[39] = 0.0; + out_1273572215837011735[40] = 0.0; + out_1273572215837011735[41] = 0.0; + out_1273572215837011735[42] = 0.0; + out_1273572215837011735[43] = 0.0; + out_1273572215837011735[44] = 0.0; + out_1273572215837011735[45] = 0.0; + out_1273572215837011735[46] = 0.0; + out_1273572215837011735[47] = 0.0; + out_1273572215837011735[48] = 0.0; + out_1273572215837011735[49] = 0.0; + out_1273572215837011735[50] = 0.0; + out_1273572215837011735[51] = 0.0; + out_1273572215837011735[52] = 0.0; + out_1273572215837011735[53] = 0.0; + out_1273572215837011735[54] = 0.0; + out_1273572215837011735[55] = 0.0; + out_1273572215837011735[56] = 0.0; + out_1273572215837011735[57] = 1.0; + out_1273572215837011735[58] = 0.0; + out_1273572215837011735[59] = 0.0; + out_1273572215837011735[60] = 0.0; + out_1273572215837011735[61] = 0.0; + out_1273572215837011735[62] = 0.0; + out_1273572215837011735[63] = 0.0; + out_1273572215837011735[64] = 0.0; + out_1273572215837011735[65] = 0.0; + out_1273572215837011735[66] = 0.0; + out_1273572215837011735[67] = 0.0; + out_1273572215837011735[68] = 0.0; + out_1273572215837011735[69] = 0.0; + out_1273572215837011735[70] = 0.0; + out_1273572215837011735[71] = 0.0; + out_1273572215837011735[72] = 0.0; + out_1273572215837011735[73] = 0.0; + out_1273572215837011735[74] = 0.0; + out_1273572215837011735[75] = 0.0; + out_1273572215837011735[76] = 1.0; + out_1273572215837011735[77] = 0.0; + out_1273572215837011735[78] = 0.0; + out_1273572215837011735[79] = 0.0; + out_1273572215837011735[80] = 0.0; + out_1273572215837011735[81] = 0.0; + out_1273572215837011735[82] = 0.0; + out_1273572215837011735[83] = 0.0; + out_1273572215837011735[84] = 0.0; + out_1273572215837011735[85] = 0.0; + out_1273572215837011735[86] = 0.0; + out_1273572215837011735[87] = 0.0; + out_1273572215837011735[88] = 0.0; + out_1273572215837011735[89] = 0.0; + out_1273572215837011735[90] = 0.0; + out_1273572215837011735[91] = 0.0; + out_1273572215837011735[92] = 0.0; + out_1273572215837011735[93] = 0.0; + out_1273572215837011735[94] = 0.0; + out_1273572215837011735[95] = 1.0; + out_1273572215837011735[96] = 0.0; + out_1273572215837011735[97] = 0.0; + out_1273572215837011735[98] = 0.0; + out_1273572215837011735[99] = 0.0; + out_1273572215837011735[100] = 0.0; + out_1273572215837011735[101] = 0.0; + out_1273572215837011735[102] = 0.0; + out_1273572215837011735[103] = 0.0; + out_1273572215837011735[104] = 0.0; + out_1273572215837011735[105] = 0.0; + out_1273572215837011735[106] = 0.0; + out_1273572215837011735[107] = 0.0; + out_1273572215837011735[108] = 0.0; + out_1273572215837011735[109] = 0.0; + out_1273572215837011735[110] = 0.0; + out_1273572215837011735[111] = 0.0; + out_1273572215837011735[112] = 0.0; + out_1273572215837011735[113] = 0.0; + out_1273572215837011735[114] = 1.0; + out_1273572215837011735[115] = 0.0; + out_1273572215837011735[116] = 0.0; + out_1273572215837011735[117] = 0.0; + out_1273572215837011735[118] = 0.0; + out_1273572215837011735[119] = 0.0; + out_1273572215837011735[120] = 0.0; + out_1273572215837011735[121] = 0.0; + out_1273572215837011735[122] = 0.0; + out_1273572215837011735[123] = 0.0; + out_1273572215837011735[124] = 0.0; + out_1273572215837011735[125] = 0.0; + out_1273572215837011735[126] = 0.0; + out_1273572215837011735[127] = 0.0; + out_1273572215837011735[128] = 0.0; + out_1273572215837011735[129] = 0.0; + out_1273572215837011735[130] = 0.0; + out_1273572215837011735[131] = 0.0; + out_1273572215837011735[132] = 0.0; + out_1273572215837011735[133] = 1.0; + out_1273572215837011735[134] = 0.0; + out_1273572215837011735[135] = 0.0; + out_1273572215837011735[136] = 0.0; + out_1273572215837011735[137] = 0.0; + out_1273572215837011735[138] = 0.0; + out_1273572215837011735[139] = 0.0; + out_1273572215837011735[140] = 0.0; + out_1273572215837011735[141] = 0.0; + out_1273572215837011735[142] = 0.0; + out_1273572215837011735[143] = 0.0; + out_1273572215837011735[144] = 0.0; + out_1273572215837011735[145] = 0.0; + out_1273572215837011735[146] = 0.0; + out_1273572215837011735[147] = 0.0; + out_1273572215837011735[148] = 0.0; + out_1273572215837011735[149] = 0.0; + out_1273572215837011735[150] = 0.0; + out_1273572215837011735[151] = 0.0; + out_1273572215837011735[152] = 1.0; + out_1273572215837011735[153] = 0.0; + out_1273572215837011735[154] = 0.0; + out_1273572215837011735[155] = 0.0; + out_1273572215837011735[156] = 0.0; + out_1273572215837011735[157] = 0.0; + out_1273572215837011735[158] = 0.0; + out_1273572215837011735[159] = 0.0; + out_1273572215837011735[160] = 0.0; + out_1273572215837011735[161] = 0.0; + out_1273572215837011735[162] = 0.0; + out_1273572215837011735[163] = 0.0; + out_1273572215837011735[164] = 0.0; + out_1273572215837011735[165] = 0.0; + out_1273572215837011735[166] = 0.0; + out_1273572215837011735[167] = 0.0; + out_1273572215837011735[168] = 0.0; + out_1273572215837011735[169] = 0.0; + out_1273572215837011735[170] = 0.0; + out_1273572215837011735[171] = 1.0; + out_1273572215837011735[172] = 0.0; + out_1273572215837011735[173] = 0.0; + out_1273572215837011735[174] = 0.0; + out_1273572215837011735[175] = 0.0; + out_1273572215837011735[176] = 0.0; + out_1273572215837011735[177] = 0.0; + out_1273572215837011735[178] = 0.0; + out_1273572215837011735[179] = 0.0; + out_1273572215837011735[180] = 0.0; + out_1273572215837011735[181] = 0.0; + out_1273572215837011735[182] = 0.0; + out_1273572215837011735[183] = 0.0; + out_1273572215837011735[184] = 0.0; + out_1273572215837011735[185] = 0.0; + out_1273572215837011735[186] = 0.0; + out_1273572215837011735[187] = 0.0; + out_1273572215837011735[188] = 0.0; + out_1273572215837011735[189] = 0.0; + out_1273572215837011735[190] = 1.0; + out_1273572215837011735[191] = 0.0; + out_1273572215837011735[192] = 0.0; + out_1273572215837011735[193] = 0.0; + out_1273572215837011735[194] = 0.0; + out_1273572215837011735[195] = 0.0; + out_1273572215837011735[196] = 0.0; + out_1273572215837011735[197] = 0.0; + out_1273572215837011735[198] = 0.0; + out_1273572215837011735[199] = 0.0; + out_1273572215837011735[200] = 0.0; + out_1273572215837011735[201] = 0.0; + out_1273572215837011735[202] = 0.0; + out_1273572215837011735[203] = 0.0; + out_1273572215837011735[204] = 0.0; + out_1273572215837011735[205] = 0.0; + out_1273572215837011735[206] = 0.0; + out_1273572215837011735[207] = 0.0; + out_1273572215837011735[208] = 0.0; + out_1273572215837011735[209] = 1.0; + out_1273572215837011735[210] = 0.0; + out_1273572215837011735[211] = 0.0; + out_1273572215837011735[212] = 0.0; + out_1273572215837011735[213] = 0.0; + out_1273572215837011735[214] = 0.0; + out_1273572215837011735[215] = 0.0; + out_1273572215837011735[216] = 0.0; + out_1273572215837011735[217] = 0.0; + out_1273572215837011735[218] = 0.0; + out_1273572215837011735[219] = 0.0; + out_1273572215837011735[220] = 0.0; + out_1273572215837011735[221] = 0.0; + out_1273572215837011735[222] = 0.0; + out_1273572215837011735[223] = 0.0; + out_1273572215837011735[224] = 0.0; + out_1273572215837011735[225] = 0.0; + out_1273572215837011735[226] = 0.0; + out_1273572215837011735[227] = 0.0; + out_1273572215837011735[228] = 1.0; + out_1273572215837011735[229] = 0.0; + out_1273572215837011735[230] = 0.0; + out_1273572215837011735[231] = 0.0; + out_1273572215837011735[232] = 0.0; + out_1273572215837011735[233] = 0.0; + out_1273572215837011735[234] = 0.0; + out_1273572215837011735[235] = 0.0; + out_1273572215837011735[236] = 0.0; + out_1273572215837011735[237] = 0.0; + out_1273572215837011735[238] = 0.0; + out_1273572215837011735[239] = 0.0; + out_1273572215837011735[240] = 0.0; + out_1273572215837011735[241] = 0.0; + out_1273572215837011735[242] = 0.0; + out_1273572215837011735[243] = 0.0; + out_1273572215837011735[244] = 0.0; + out_1273572215837011735[245] = 0.0; + out_1273572215837011735[246] = 0.0; + out_1273572215837011735[247] = 1.0; + out_1273572215837011735[248] = 0.0; + out_1273572215837011735[249] = 0.0; + out_1273572215837011735[250] = 0.0; + out_1273572215837011735[251] = 0.0; + out_1273572215837011735[252] = 0.0; + out_1273572215837011735[253] = 0.0; + out_1273572215837011735[254] = 0.0; + out_1273572215837011735[255] = 0.0; + out_1273572215837011735[256] = 0.0; + out_1273572215837011735[257] = 0.0; + out_1273572215837011735[258] = 0.0; + out_1273572215837011735[259] = 0.0; + out_1273572215837011735[260] = 0.0; + out_1273572215837011735[261] = 0.0; + out_1273572215837011735[262] = 0.0; + out_1273572215837011735[263] = 0.0; + out_1273572215837011735[264] = 0.0; + out_1273572215837011735[265] = 0.0; + out_1273572215837011735[266] = 1.0; + out_1273572215837011735[267] = 0.0; + out_1273572215837011735[268] = 0.0; + out_1273572215837011735[269] = 0.0; + out_1273572215837011735[270] = 0.0; + out_1273572215837011735[271] = 0.0; + out_1273572215837011735[272] = 0.0; + out_1273572215837011735[273] = 0.0; + out_1273572215837011735[274] = 0.0; + out_1273572215837011735[275] = 0.0; + out_1273572215837011735[276] = 0.0; + out_1273572215837011735[277] = 0.0; + out_1273572215837011735[278] = 0.0; + out_1273572215837011735[279] = 0.0; + out_1273572215837011735[280] = 0.0; + out_1273572215837011735[281] = 0.0; + out_1273572215837011735[282] = 0.0; + out_1273572215837011735[283] = 0.0; + out_1273572215837011735[284] = 0.0; + out_1273572215837011735[285] = 1.0; + out_1273572215837011735[286] = 0.0; + out_1273572215837011735[287] = 0.0; + out_1273572215837011735[288] = 0.0; + out_1273572215837011735[289] = 0.0; + out_1273572215837011735[290] = 0.0; + out_1273572215837011735[291] = 0.0; + out_1273572215837011735[292] = 0.0; + out_1273572215837011735[293] = 0.0; + out_1273572215837011735[294] = 0.0; + out_1273572215837011735[295] = 0.0; + out_1273572215837011735[296] = 0.0; + out_1273572215837011735[297] = 0.0; + out_1273572215837011735[298] = 0.0; + out_1273572215837011735[299] = 0.0; + out_1273572215837011735[300] = 0.0; + out_1273572215837011735[301] = 0.0; + out_1273572215837011735[302] = 0.0; + out_1273572215837011735[303] = 0.0; + out_1273572215837011735[304] = 1.0; + out_1273572215837011735[305] = 0.0; + out_1273572215837011735[306] = 0.0; + out_1273572215837011735[307] = 0.0; + out_1273572215837011735[308] = 0.0; + out_1273572215837011735[309] = 0.0; + out_1273572215837011735[310] = 0.0; + out_1273572215837011735[311] = 0.0; + out_1273572215837011735[312] = 0.0; + out_1273572215837011735[313] = 0.0; + out_1273572215837011735[314] = 0.0; + out_1273572215837011735[315] = 0.0; + out_1273572215837011735[316] = 0.0; + out_1273572215837011735[317] = 0.0; + out_1273572215837011735[318] = 0.0; + out_1273572215837011735[319] = 0.0; + out_1273572215837011735[320] = 0.0; + out_1273572215837011735[321] = 0.0; + out_1273572215837011735[322] = 0.0; + out_1273572215837011735[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4957392939814811366) { - out_4957392939814811366[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_4957392939814811366[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_4957392939814811366[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_4957392939814811366[3] = dt*state[12] + state[3]; - out_4957392939814811366[4] = dt*state[13] + state[4]; - out_4957392939814811366[5] = dt*state[14] + state[5]; - out_4957392939814811366[6] = state[6]; - out_4957392939814811366[7] = state[7]; - out_4957392939814811366[8] = state[8]; - out_4957392939814811366[9] = state[9]; - out_4957392939814811366[10] = state[10]; - out_4957392939814811366[11] = state[11]; - out_4957392939814811366[12] = state[12]; - out_4957392939814811366[13] = state[13]; - out_4957392939814811366[14] = state[14]; - out_4957392939814811366[15] = state[15]; - out_4957392939814811366[16] = state[16]; - out_4957392939814811366[17] = state[17]; +void f_fun(double *state, double dt, double *out_58843541046312828) { + out_58843541046312828[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_58843541046312828[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_58843541046312828[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_58843541046312828[3] = dt*state[12] + state[3]; + out_58843541046312828[4] = dt*state[13] + state[4]; + out_58843541046312828[5] = dt*state[14] + state[5]; + out_58843541046312828[6] = state[6]; + out_58843541046312828[7] = state[7]; + out_58843541046312828[8] = state[8]; + out_58843541046312828[9] = state[9]; + out_58843541046312828[10] = state[10]; + out_58843541046312828[11] = state[11]; + out_58843541046312828[12] = state[12]; + out_58843541046312828[13] = state[13]; + out_58843541046312828[14] = state[14]; + out_58843541046312828[15] = state[15]; + out_58843541046312828[16] = state[16]; + out_58843541046312828[17] = state[17]; } -void F_fun(double *state, double dt, double *out_3660031818732653459) { - out_3660031818732653459[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_3660031818732653459[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_3660031818732653459[2] = 0; - out_3660031818732653459[3] = 0; - out_3660031818732653459[4] = 0; - out_3660031818732653459[5] = 0; - out_3660031818732653459[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_3660031818732653459[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_3660031818732653459[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_3660031818732653459[9] = 0; - out_3660031818732653459[10] = 0; - out_3660031818732653459[11] = 0; - out_3660031818732653459[12] = 0; - out_3660031818732653459[13] = 0; - out_3660031818732653459[14] = 0; - out_3660031818732653459[15] = 0; - out_3660031818732653459[16] = 0; - out_3660031818732653459[17] = 0; - out_3660031818732653459[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_3660031818732653459[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_3660031818732653459[20] = 0; - out_3660031818732653459[21] = 0; - out_3660031818732653459[22] = 0; - out_3660031818732653459[23] = 0; - out_3660031818732653459[24] = 0; - out_3660031818732653459[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_3660031818732653459[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_3660031818732653459[27] = 0; - out_3660031818732653459[28] = 0; - out_3660031818732653459[29] = 0; - out_3660031818732653459[30] = 0; - out_3660031818732653459[31] = 0; - out_3660031818732653459[32] = 0; - out_3660031818732653459[33] = 0; - out_3660031818732653459[34] = 0; - out_3660031818732653459[35] = 0; - out_3660031818732653459[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_3660031818732653459[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_3660031818732653459[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_3660031818732653459[39] = 0; - out_3660031818732653459[40] = 0; - out_3660031818732653459[41] = 0; - out_3660031818732653459[42] = 0; - out_3660031818732653459[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_3660031818732653459[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_3660031818732653459[45] = 0; - out_3660031818732653459[46] = 0; - out_3660031818732653459[47] = 0; - out_3660031818732653459[48] = 0; - out_3660031818732653459[49] = 0; - out_3660031818732653459[50] = 0; - out_3660031818732653459[51] = 0; - out_3660031818732653459[52] = 0; - out_3660031818732653459[53] = 0; - out_3660031818732653459[54] = 0; - out_3660031818732653459[55] = 0; - out_3660031818732653459[56] = 0; - out_3660031818732653459[57] = 1; - out_3660031818732653459[58] = 0; - out_3660031818732653459[59] = 0; - out_3660031818732653459[60] = 0; - out_3660031818732653459[61] = 0; - out_3660031818732653459[62] = 0; - out_3660031818732653459[63] = 0; - out_3660031818732653459[64] = 0; - out_3660031818732653459[65] = 0; - out_3660031818732653459[66] = dt; - out_3660031818732653459[67] = 0; - out_3660031818732653459[68] = 0; - out_3660031818732653459[69] = 0; - out_3660031818732653459[70] = 0; - out_3660031818732653459[71] = 0; - out_3660031818732653459[72] = 0; - out_3660031818732653459[73] = 0; - out_3660031818732653459[74] = 0; - out_3660031818732653459[75] = 0; - out_3660031818732653459[76] = 1; - out_3660031818732653459[77] = 0; - out_3660031818732653459[78] = 0; - out_3660031818732653459[79] = 0; - out_3660031818732653459[80] = 0; - out_3660031818732653459[81] = 0; - out_3660031818732653459[82] = 0; - out_3660031818732653459[83] = 0; - out_3660031818732653459[84] = 0; - out_3660031818732653459[85] = dt; - out_3660031818732653459[86] = 0; - out_3660031818732653459[87] = 0; - out_3660031818732653459[88] = 0; - out_3660031818732653459[89] = 0; - out_3660031818732653459[90] = 0; - out_3660031818732653459[91] = 0; - out_3660031818732653459[92] = 0; - out_3660031818732653459[93] = 0; - out_3660031818732653459[94] = 0; - out_3660031818732653459[95] = 1; - out_3660031818732653459[96] = 0; - out_3660031818732653459[97] = 0; - out_3660031818732653459[98] = 0; - out_3660031818732653459[99] = 0; - out_3660031818732653459[100] = 0; - out_3660031818732653459[101] = 0; - out_3660031818732653459[102] = 0; - out_3660031818732653459[103] = 0; - out_3660031818732653459[104] = dt; - out_3660031818732653459[105] = 0; - out_3660031818732653459[106] = 0; - out_3660031818732653459[107] = 0; - out_3660031818732653459[108] = 0; - out_3660031818732653459[109] = 0; - out_3660031818732653459[110] = 0; - out_3660031818732653459[111] = 0; - out_3660031818732653459[112] = 0; - out_3660031818732653459[113] = 0; - out_3660031818732653459[114] = 1; - out_3660031818732653459[115] = 0; - out_3660031818732653459[116] = 0; - out_3660031818732653459[117] = 0; - out_3660031818732653459[118] = 0; - out_3660031818732653459[119] = 0; - out_3660031818732653459[120] = 0; - out_3660031818732653459[121] = 0; - out_3660031818732653459[122] = 0; - out_3660031818732653459[123] = 0; - out_3660031818732653459[124] = 0; - out_3660031818732653459[125] = 0; - out_3660031818732653459[126] = 0; - out_3660031818732653459[127] = 0; - out_3660031818732653459[128] = 0; - out_3660031818732653459[129] = 0; - out_3660031818732653459[130] = 0; - out_3660031818732653459[131] = 0; - out_3660031818732653459[132] = 0; - out_3660031818732653459[133] = 1; - out_3660031818732653459[134] = 0; - out_3660031818732653459[135] = 0; - out_3660031818732653459[136] = 0; - out_3660031818732653459[137] = 0; - out_3660031818732653459[138] = 0; - out_3660031818732653459[139] = 0; - out_3660031818732653459[140] = 0; - out_3660031818732653459[141] = 0; - out_3660031818732653459[142] = 0; - out_3660031818732653459[143] = 0; - out_3660031818732653459[144] = 0; - out_3660031818732653459[145] = 0; - out_3660031818732653459[146] = 0; - out_3660031818732653459[147] = 0; - out_3660031818732653459[148] = 0; - out_3660031818732653459[149] = 0; - out_3660031818732653459[150] = 0; - out_3660031818732653459[151] = 0; - out_3660031818732653459[152] = 1; - out_3660031818732653459[153] = 0; - out_3660031818732653459[154] = 0; - out_3660031818732653459[155] = 0; - out_3660031818732653459[156] = 0; - out_3660031818732653459[157] = 0; - out_3660031818732653459[158] = 0; - out_3660031818732653459[159] = 0; - out_3660031818732653459[160] = 0; - out_3660031818732653459[161] = 0; - out_3660031818732653459[162] = 0; - out_3660031818732653459[163] = 0; - out_3660031818732653459[164] = 0; - out_3660031818732653459[165] = 0; - out_3660031818732653459[166] = 0; - out_3660031818732653459[167] = 0; - out_3660031818732653459[168] = 0; - out_3660031818732653459[169] = 0; - out_3660031818732653459[170] = 0; - out_3660031818732653459[171] = 1; - out_3660031818732653459[172] = 0; - out_3660031818732653459[173] = 0; - out_3660031818732653459[174] = 0; - out_3660031818732653459[175] = 0; - out_3660031818732653459[176] = 0; - out_3660031818732653459[177] = 0; - out_3660031818732653459[178] = 0; - out_3660031818732653459[179] = 0; - out_3660031818732653459[180] = 0; - out_3660031818732653459[181] = 0; - out_3660031818732653459[182] = 0; - out_3660031818732653459[183] = 0; - out_3660031818732653459[184] = 0; - out_3660031818732653459[185] = 0; - out_3660031818732653459[186] = 0; - out_3660031818732653459[187] = 0; - out_3660031818732653459[188] = 0; - out_3660031818732653459[189] = 0; - out_3660031818732653459[190] = 1; - out_3660031818732653459[191] = 0; - out_3660031818732653459[192] = 0; - out_3660031818732653459[193] = 0; - out_3660031818732653459[194] = 0; - out_3660031818732653459[195] = 0; - out_3660031818732653459[196] = 0; - out_3660031818732653459[197] = 0; - out_3660031818732653459[198] = 0; - out_3660031818732653459[199] = 0; - out_3660031818732653459[200] = 0; - out_3660031818732653459[201] = 0; - out_3660031818732653459[202] = 0; - out_3660031818732653459[203] = 0; - out_3660031818732653459[204] = 0; - out_3660031818732653459[205] = 0; - out_3660031818732653459[206] = 0; - out_3660031818732653459[207] = 0; - out_3660031818732653459[208] = 0; - out_3660031818732653459[209] = 1; - out_3660031818732653459[210] = 0; - out_3660031818732653459[211] = 0; - out_3660031818732653459[212] = 0; - out_3660031818732653459[213] = 0; - out_3660031818732653459[214] = 0; - out_3660031818732653459[215] = 0; - out_3660031818732653459[216] = 0; - out_3660031818732653459[217] = 0; - out_3660031818732653459[218] = 0; - out_3660031818732653459[219] = 0; - out_3660031818732653459[220] = 0; - out_3660031818732653459[221] = 0; - out_3660031818732653459[222] = 0; - out_3660031818732653459[223] = 0; - out_3660031818732653459[224] = 0; - out_3660031818732653459[225] = 0; - out_3660031818732653459[226] = 0; - out_3660031818732653459[227] = 0; - out_3660031818732653459[228] = 1; - out_3660031818732653459[229] = 0; - out_3660031818732653459[230] = 0; - out_3660031818732653459[231] = 0; - out_3660031818732653459[232] = 0; - out_3660031818732653459[233] = 0; - out_3660031818732653459[234] = 0; - out_3660031818732653459[235] = 0; - out_3660031818732653459[236] = 0; - out_3660031818732653459[237] = 0; - out_3660031818732653459[238] = 0; - out_3660031818732653459[239] = 0; - out_3660031818732653459[240] = 0; - out_3660031818732653459[241] = 0; - out_3660031818732653459[242] = 0; - out_3660031818732653459[243] = 0; - out_3660031818732653459[244] = 0; - out_3660031818732653459[245] = 0; - out_3660031818732653459[246] = 0; - out_3660031818732653459[247] = 1; - out_3660031818732653459[248] = 0; - out_3660031818732653459[249] = 0; - out_3660031818732653459[250] = 0; - out_3660031818732653459[251] = 0; - out_3660031818732653459[252] = 0; - out_3660031818732653459[253] = 0; - out_3660031818732653459[254] = 0; - out_3660031818732653459[255] = 0; - out_3660031818732653459[256] = 0; - out_3660031818732653459[257] = 0; - out_3660031818732653459[258] = 0; - out_3660031818732653459[259] = 0; - out_3660031818732653459[260] = 0; - out_3660031818732653459[261] = 0; - out_3660031818732653459[262] = 0; - out_3660031818732653459[263] = 0; - out_3660031818732653459[264] = 0; - out_3660031818732653459[265] = 0; - out_3660031818732653459[266] = 1; - out_3660031818732653459[267] = 0; - out_3660031818732653459[268] = 0; - out_3660031818732653459[269] = 0; - out_3660031818732653459[270] = 0; - out_3660031818732653459[271] = 0; - out_3660031818732653459[272] = 0; - out_3660031818732653459[273] = 0; - out_3660031818732653459[274] = 0; - out_3660031818732653459[275] = 0; - out_3660031818732653459[276] = 0; - out_3660031818732653459[277] = 0; - out_3660031818732653459[278] = 0; - out_3660031818732653459[279] = 0; - out_3660031818732653459[280] = 0; - out_3660031818732653459[281] = 0; - out_3660031818732653459[282] = 0; - out_3660031818732653459[283] = 0; - out_3660031818732653459[284] = 0; - out_3660031818732653459[285] = 1; - out_3660031818732653459[286] = 0; - out_3660031818732653459[287] = 0; - out_3660031818732653459[288] = 0; - out_3660031818732653459[289] = 0; - out_3660031818732653459[290] = 0; - out_3660031818732653459[291] = 0; - out_3660031818732653459[292] = 0; - out_3660031818732653459[293] = 0; - out_3660031818732653459[294] = 0; - out_3660031818732653459[295] = 0; - out_3660031818732653459[296] = 0; - out_3660031818732653459[297] = 0; - out_3660031818732653459[298] = 0; - out_3660031818732653459[299] = 0; - out_3660031818732653459[300] = 0; - out_3660031818732653459[301] = 0; - out_3660031818732653459[302] = 0; - out_3660031818732653459[303] = 0; - out_3660031818732653459[304] = 1; - out_3660031818732653459[305] = 0; - out_3660031818732653459[306] = 0; - out_3660031818732653459[307] = 0; - out_3660031818732653459[308] = 0; - out_3660031818732653459[309] = 0; - out_3660031818732653459[310] = 0; - out_3660031818732653459[311] = 0; - out_3660031818732653459[312] = 0; - out_3660031818732653459[313] = 0; - out_3660031818732653459[314] = 0; - out_3660031818732653459[315] = 0; - out_3660031818732653459[316] = 0; - out_3660031818732653459[317] = 0; - out_3660031818732653459[318] = 0; - out_3660031818732653459[319] = 0; - out_3660031818732653459[320] = 0; - out_3660031818732653459[321] = 0; - out_3660031818732653459[322] = 0; - out_3660031818732653459[323] = 1; +void F_fun(double *state, double dt, double *out_4952539587807327652) { + out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[2] = 0; + out_4952539587807327652[3] = 0; + out_4952539587807327652[4] = 0; + out_4952539587807327652[5] = 0; + out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[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_4952539587807327652[9] = 0; + out_4952539587807327652[10] = 0; + out_4952539587807327652[11] = 0; + out_4952539587807327652[12] = 0; + out_4952539587807327652[13] = 0; + out_4952539587807327652[14] = 0; + out_4952539587807327652[15] = 0; + out_4952539587807327652[16] = 0; + out_4952539587807327652[17] = 0; + out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[20] = 0; + out_4952539587807327652[21] = 0; + out_4952539587807327652[22] = 0; + out_4952539587807327652[23] = 0; + out_4952539587807327652[24] = 0; + out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[27] = 0; + out_4952539587807327652[28] = 0; + out_4952539587807327652[29] = 0; + out_4952539587807327652[30] = 0; + out_4952539587807327652[31] = 0; + out_4952539587807327652[32] = 0; + out_4952539587807327652[33] = 0; + out_4952539587807327652[34] = 0; + out_4952539587807327652[35] = 0; + out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[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_4952539587807327652[39] = 0; + out_4952539587807327652[40] = 0; + out_4952539587807327652[41] = 0; + out_4952539587807327652[42] = 0; + out_4952539587807327652[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_4952539587807327652[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_4952539587807327652[45] = 0; + out_4952539587807327652[46] = 0; + out_4952539587807327652[47] = 0; + out_4952539587807327652[48] = 0; + out_4952539587807327652[49] = 0; + out_4952539587807327652[50] = 0; + out_4952539587807327652[51] = 0; + out_4952539587807327652[52] = 0; + out_4952539587807327652[53] = 0; + out_4952539587807327652[54] = 0; + out_4952539587807327652[55] = 0; + out_4952539587807327652[56] = 0; + out_4952539587807327652[57] = 1; + out_4952539587807327652[58] = 0; + out_4952539587807327652[59] = 0; + out_4952539587807327652[60] = 0; + out_4952539587807327652[61] = 0; + out_4952539587807327652[62] = 0; + out_4952539587807327652[63] = 0; + out_4952539587807327652[64] = 0; + out_4952539587807327652[65] = 0; + out_4952539587807327652[66] = dt; + out_4952539587807327652[67] = 0; + out_4952539587807327652[68] = 0; + out_4952539587807327652[69] = 0; + out_4952539587807327652[70] = 0; + out_4952539587807327652[71] = 0; + out_4952539587807327652[72] = 0; + out_4952539587807327652[73] = 0; + out_4952539587807327652[74] = 0; + out_4952539587807327652[75] = 0; + out_4952539587807327652[76] = 1; + out_4952539587807327652[77] = 0; + out_4952539587807327652[78] = 0; + out_4952539587807327652[79] = 0; + out_4952539587807327652[80] = 0; + out_4952539587807327652[81] = 0; + out_4952539587807327652[82] = 0; + out_4952539587807327652[83] = 0; + out_4952539587807327652[84] = 0; + out_4952539587807327652[85] = dt; + out_4952539587807327652[86] = 0; + out_4952539587807327652[87] = 0; + out_4952539587807327652[88] = 0; + out_4952539587807327652[89] = 0; + out_4952539587807327652[90] = 0; + out_4952539587807327652[91] = 0; + out_4952539587807327652[92] = 0; + out_4952539587807327652[93] = 0; + out_4952539587807327652[94] = 0; + out_4952539587807327652[95] = 1; + out_4952539587807327652[96] = 0; + out_4952539587807327652[97] = 0; + out_4952539587807327652[98] = 0; + out_4952539587807327652[99] = 0; + out_4952539587807327652[100] = 0; + out_4952539587807327652[101] = 0; + out_4952539587807327652[102] = 0; + out_4952539587807327652[103] = 0; + out_4952539587807327652[104] = dt; + out_4952539587807327652[105] = 0; + out_4952539587807327652[106] = 0; + out_4952539587807327652[107] = 0; + out_4952539587807327652[108] = 0; + out_4952539587807327652[109] = 0; + out_4952539587807327652[110] = 0; + out_4952539587807327652[111] = 0; + out_4952539587807327652[112] = 0; + out_4952539587807327652[113] = 0; + out_4952539587807327652[114] = 1; + out_4952539587807327652[115] = 0; + out_4952539587807327652[116] = 0; + out_4952539587807327652[117] = 0; + out_4952539587807327652[118] = 0; + out_4952539587807327652[119] = 0; + out_4952539587807327652[120] = 0; + out_4952539587807327652[121] = 0; + out_4952539587807327652[122] = 0; + out_4952539587807327652[123] = 0; + out_4952539587807327652[124] = 0; + out_4952539587807327652[125] = 0; + out_4952539587807327652[126] = 0; + out_4952539587807327652[127] = 0; + out_4952539587807327652[128] = 0; + out_4952539587807327652[129] = 0; + out_4952539587807327652[130] = 0; + out_4952539587807327652[131] = 0; + out_4952539587807327652[132] = 0; + out_4952539587807327652[133] = 1; + out_4952539587807327652[134] = 0; + out_4952539587807327652[135] = 0; + out_4952539587807327652[136] = 0; + out_4952539587807327652[137] = 0; + out_4952539587807327652[138] = 0; + out_4952539587807327652[139] = 0; + out_4952539587807327652[140] = 0; + out_4952539587807327652[141] = 0; + out_4952539587807327652[142] = 0; + out_4952539587807327652[143] = 0; + out_4952539587807327652[144] = 0; + out_4952539587807327652[145] = 0; + out_4952539587807327652[146] = 0; + out_4952539587807327652[147] = 0; + out_4952539587807327652[148] = 0; + out_4952539587807327652[149] = 0; + out_4952539587807327652[150] = 0; + out_4952539587807327652[151] = 0; + out_4952539587807327652[152] = 1; + out_4952539587807327652[153] = 0; + out_4952539587807327652[154] = 0; + out_4952539587807327652[155] = 0; + out_4952539587807327652[156] = 0; + out_4952539587807327652[157] = 0; + out_4952539587807327652[158] = 0; + out_4952539587807327652[159] = 0; + out_4952539587807327652[160] = 0; + out_4952539587807327652[161] = 0; + out_4952539587807327652[162] = 0; + out_4952539587807327652[163] = 0; + out_4952539587807327652[164] = 0; + out_4952539587807327652[165] = 0; + out_4952539587807327652[166] = 0; + out_4952539587807327652[167] = 0; + out_4952539587807327652[168] = 0; + out_4952539587807327652[169] = 0; + out_4952539587807327652[170] = 0; + out_4952539587807327652[171] = 1; + out_4952539587807327652[172] = 0; + out_4952539587807327652[173] = 0; + out_4952539587807327652[174] = 0; + out_4952539587807327652[175] = 0; + out_4952539587807327652[176] = 0; + out_4952539587807327652[177] = 0; + out_4952539587807327652[178] = 0; + out_4952539587807327652[179] = 0; + out_4952539587807327652[180] = 0; + out_4952539587807327652[181] = 0; + out_4952539587807327652[182] = 0; + out_4952539587807327652[183] = 0; + out_4952539587807327652[184] = 0; + out_4952539587807327652[185] = 0; + out_4952539587807327652[186] = 0; + out_4952539587807327652[187] = 0; + out_4952539587807327652[188] = 0; + out_4952539587807327652[189] = 0; + out_4952539587807327652[190] = 1; + out_4952539587807327652[191] = 0; + out_4952539587807327652[192] = 0; + out_4952539587807327652[193] = 0; + out_4952539587807327652[194] = 0; + out_4952539587807327652[195] = 0; + out_4952539587807327652[196] = 0; + out_4952539587807327652[197] = 0; + out_4952539587807327652[198] = 0; + out_4952539587807327652[199] = 0; + out_4952539587807327652[200] = 0; + out_4952539587807327652[201] = 0; + out_4952539587807327652[202] = 0; + out_4952539587807327652[203] = 0; + out_4952539587807327652[204] = 0; + out_4952539587807327652[205] = 0; + out_4952539587807327652[206] = 0; + out_4952539587807327652[207] = 0; + out_4952539587807327652[208] = 0; + out_4952539587807327652[209] = 1; + out_4952539587807327652[210] = 0; + out_4952539587807327652[211] = 0; + out_4952539587807327652[212] = 0; + out_4952539587807327652[213] = 0; + out_4952539587807327652[214] = 0; + out_4952539587807327652[215] = 0; + out_4952539587807327652[216] = 0; + out_4952539587807327652[217] = 0; + out_4952539587807327652[218] = 0; + out_4952539587807327652[219] = 0; + out_4952539587807327652[220] = 0; + out_4952539587807327652[221] = 0; + out_4952539587807327652[222] = 0; + out_4952539587807327652[223] = 0; + out_4952539587807327652[224] = 0; + out_4952539587807327652[225] = 0; + out_4952539587807327652[226] = 0; + out_4952539587807327652[227] = 0; + out_4952539587807327652[228] = 1; + out_4952539587807327652[229] = 0; + out_4952539587807327652[230] = 0; + out_4952539587807327652[231] = 0; + out_4952539587807327652[232] = 0; + out_4952539587807327652[233] = 0; + out_4952539587807327652[234] = 0; + out_4952539587807327652[235] = 0; + out_4952539587807327652[236] = 0; + out_4952539587807327652[237] = 0; + out_4952539587807327652[238] = 0; + out_4952539587807327652[239] = 0; + out_4952539587807327652[240] = 0; + out_4952539587807327652[241] = 0; + out_4952539587807327652[242] = 0; + out_4952539587807327652[243] = 0; + out_4952539587807327652[244] = 0; + out_4952539587807327652[245] = 0; + out_4952539587807327652[246] = 0; + out_4952539587807327652[247] = 1; + out_4952539587807327652[248] = 0; + out_4952539587807327652[249] = 0; + out_4952539587807327652[250] = 0; + out_4952539587807327652[251] = 0; + out_4952539587807327652[252] = 0; + out_4952539587807327652[253] = 0; + out_4952539587807327652[254] = 0; + out_4952539587807327652[255] = 0; + out_4952539587807327652[256] = 0; + out_4952539587807327652[257] = 0; + out_4952539587807327652[258] = 0; + out_4952539587807327652[259] = 0; + out_4952539587807327652[260] = 0; + out_4952539587807327652[261] = 0; + out_4952539587807327652[262] = 0; + out_4952539587807327652[263] = 0; + out_4952539587807327652[264] = 0; + out_4952539587807327652[265] = 0; + out_4952539587807327652[266] = 1; + out_4952539587807327652[267] = 0; + out_4952539587807327652[268] = 0; + out_4952539587807327652[269] = 0; + out_4952539587807327652[270] = 0; + out_4952539587807327652[271] = 0; + out_4952539587807327652[272] = 0; + out_4952539587807327652[273] = 0; + out_4952539587807327652[274] = 0; + out_4952539587807327652[275] = 0; + out_4952539587807327652[276] = 0; + out_4952539587807327652[277] = 0; + out_4952539587807327652[278] = 0; + out_4952539587807327652[279] = 0; + out_4952539587807327652[280] = 0; + out_4952539587807327652[281] = 0; + out_4952539587807327652[282] = 0; + out_4952539587807327652[283] = 0; + out_4952539587807327652[284] = 0; + out_4952539587807327652[285] = 1; + out_4952539587807327652[286] = 0; + out_4952539587807327652[287] = 0; + out_4952539587807327652[288] = 0; + out_4952539587807327652[289] = 0; + out_4952539587807327652[290] = 0; + out_4952539587807327652[291] = 0; + out_4952539587807327652[292] = 0; + out_4952539587807327652[293] = 0; + out_4952539587807327652[294] = 0; + out_4952539587807327652[295] = 0; + out_4952539587807327652[296] = 0; + out_4952539587807327652[297] = 0; + out_4952539587807327652[298] = 0; + out_4952539587807327652[299] = 0; + out_4952539587807327652[300] = 0; + out_4952539587807327652[301] = 0; + out_4952539587807327652[302] = 0; + out_4952539587807327652[303] = 0; + out_4952539587807327652[304] = 1; + out_4952539587807327652[305] = 0; + out_4952539587807327652[306] = 0; + out_4952539587807327652[307] = 0; + out_4952539587807327652[308] = 0; + out_4952539587807327652[309] = 0; + out_4952539587807327652[310] = 0; + out_4952539587807327652[311] = 0; + out_4952539587807327652[312] = 0; + out_4952539587807327652[313] = 0; + out_4952539587807327652[314] = 0; + out_4952539587807327652[315] = 0; + out_4952539587807327652[316] = 0; + out_4952539587807327652[317] = 0; + out_4952539587807327652[318] = 0; + out_4952539587807327652[319] = 0; + out_4952539587807327652[320] = 0; + out_4952539587807327652[321] = 0; + out_4952539587807327652[322] = 0; + out_4952539587807327652[323] = 1; } -void h_4(double *state, double *unused, double *out_4354770932396622955) { - out_4354770932396622955[0] = state[6] + state[9]; - out_4354770932396622955[1] = state[7] + state[10]; - out_4354770932396622955[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_105029621528743882) { + out_105029621528743882[0] = state[6] + state[9]; + out_105029621528743882[1] = state[7] + state[10]; + out_105029621528743882[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_3140573770318941521) { - out_3140573770318941521[0] = 0; - out_3140573770318941521[1] = 0; - out_3140573770318941521[2] = 0; - out_3140573770318941521[3] = 0; - out_3140573770318941521[4] = 0; - out_3140573770318941521[5] = 0; - out_3140573770318941521[6] = 1; - out_3140573770318941521[7] = 0; - out_3140573770318941521[8] = 0; - out_3140573770318941521[9] = 1; - out_3140573770318941521[10] = 0; - out_3140573770318941521[11] = 0; - out_3140573770318941521[12] = 0; - out_3140573770318941521[13] = 0; - out_3140573770318941521[14] = 0; - out_3140573770318941521[15] = 0; - out_3140573770318941521[16] = 0; - out_3140573770318941521[17] = 0; - out_3140573770318941521[18] = 0; - out_3140573770318941521[19] = 0; - out_3140573770318941521[20] = 0; - out_3140573770318941521[21] = 0; - out_3140573770318941521[22] = 0; - out_3140573770318941521[23] = 0; - out_3140573770318941521[24] = 0; - out_3140573770318941521[25] = 1; - out_3140573770318941521[26] = 0; - out_3140573770318941521[27] = 0; - out_3140573770318941521[28] = 1; - out_3140573770318941521[29] = 0; - out_3140573770318941521[30] = 0; - out_3140573770318941521[31] = 0; - out_3140573770318941521[32] = 0; - out_3140573770318941521[33] = 0; - out_3140573770318941521[34] = 0; - out_3140573770318941521[35] = 0; - out_3140573770318941521[36] = 0; - out_3140573770318941521[37] = 0; - out_3140573770318941521[38] = 0; - out_3140573770318941521[39] = 0; - out_3140573770318941521[40] = 0; - out_3140573770318941521[41] = 0; - out_3140573770318941521[42] = 0; - out_3140573770318941521[43] = 0; - out_3140573770318941521[44] = 1; - out_3140573770318941521[45] = 0; - out_3140573770318941521[46] = 0; - out_3140573770318941521[47] = 1; - out_3140573770318941521[48] = 0; - out_3140573770318941521[49] = 0; - out_3140573770318941521[50] = 0; - out_3140573770318941521[51] = 0; - out_3140573770318941521[52] = 0; - out_3140573770318941521[53] = 0; +void H_4(double *state, double *unused, double *out_7121654022588970456) { + out_7121654022588970456[0] = 0; + out_7121654022588970456[1] = 0; + out_7121654022588970456[2] = 0; + out_7121654022588970456[3] = 0; + out_7121654022588970456[4] = 0; + out_7121654022588970456[5] = 0; + out_7121654022588970456[6] = 1; + out_7121654022588970456[7] = 0; + out_7121654022588970456[8] = 0; + out_7121654022588970456[9] = 1; + out_7121654022588970456[10] = 0; + out_7121654022588970456[11] = 0; + out_7121654022588970456[12] = 0; + out_7121654022588970456[13] = 0; + out_7121654022588970456[14] = 0; + out_7121654022588970456[15] = 0; + out_7121654022588970456[16] = 0; + out_7121654022588970456[17] = 0; + out_7121654022588970456[18] = 0; + out_7121654022588970456[19] = 0; + out_7121654022588970456[20] = 0; + out_7121654022588970456[21] = 0; + out_7121654022588970456[22] = 0; + out_7121654022588970456[23] = 0; + out_7121654022588970456[24] = 0; + out_7121654022588970456[25] = 1; + out_7121654022588970456[26] = 0; + out_7121654022588970456[27] = 0; + out_7121654022588970456[28] = 1; + out_7121654022588970456[29] = 0; + out_7121654022588970456[30] = 0; + out_7121654022588970456[31] = 0; + out_7121654022588970456[32] = 0; + out_7121654022588970456[33] = 0; + out_7121654022588970456[34] = 0; + out_7121654022588970456[35] = 0; + out_7121654022588970456[36] = 0; + out_7121654022588970456[37] = 0; + out_7121654022588970456[38] = 0; + out_7121654022588970456[39] = 0; + out_7121654022588970456[40] = 0; + out_7121654022588970456[41] = 0; + out_7121654022588970456[42] = 0; + out_7121654022588970456[43] = 0; + out_7121654022588970456[44] = 1; + out_7121654022588970456[45] = 0; + out_7121654022588970456[46] = 0; + out_7121654022588970456[47] = 1; + out_7121654022588970456[48] = 0; + out_7121654022588970456[49] = 0; + out_7121654022588970456[50] = 0; + out_7121654022588970456[51] = 0; + out_7121654022588970456[52] = 0; + out_7121654022588970456[53] = 0; } -void h_10(double *state, double *unused, double *out_8942219874713773227) { - out_8942219874713773227[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_8942219874713773227[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_8942219874713773227[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_7355867801776776353) { + out_7355867801776776353[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_7355867801776776353[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_7355867801776776353[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_5705253479912587073) { - out_5705253479912587073[0] = 0; - out_5705253479912587073[1] = 9.8100000000000005*cos(state[1]); - out_5705253479912587073[2] = 0; - out_5705253479912587073[3] = 0; - out_5705253479912587073[4] = -state[8]; - out_5705253479912587073[5] = state[7]; - out_5705253479912587073[6] = 0; - out_5705253479912587073[7] = state[5]; - out_5705253479912587073[8] = -state[4]; - out_5705253479912587073[9] = 0; - out_5705253479912587073[10] = 0; - out_5705253479912587073[11] = 0; - out_5705253479912587073[12] = 1; - out_5705253479912587073[13] = 0; - out_5705253479912587073[14] = 0; - out_5705253479912587073[15] = 1; - out_5705253479912587073[16] = 0; - out_5705253479912587073[17] = 0; - out_5705253479912587073[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_5705253479912587073[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_5705253479912587073[20] = 0; - out_5705253479912587073[21] = state[8]; - out_5705253479912587073[22] = 0; - out_5705253479912587073[23] = -state[6]; - out_5705253479912587073[24] = -state[5]; - out_5705253479912587073[25] = 0; - out_5705253479912587073[26] = state[3]; - out_5705253479912587073[27] = 0; - out_5705253479912587073[28] = 0; - out_5705253479912587073[29] = 0; - out_5705253479912587073[30] = 0; - out_5705253479912587073[31] = 1; - out_5705253479912587073[32] = 0; - out_5705253479912587073[33] = 0; - out_5705253479912587073[34] = 1; - out_5705253479912587073[35] = 0; - out_5705253479912587073[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_5705253479912587073[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_5705253479912587073[38] = 0; - out_5705253479912587073[39] = -state[7]; - out_5705253479912587073[40] = state[6]; - out_5705253479912587073[41] = 0; - out_5705253479912587073[42] = state[4]; - out_5705253479912587073[43] = -state[3]; - out_5705253479912587073[44] = 0; - out_5705253479912587073[45] = 0; - out_5705253479912587073[46] = 0; - out_5705253479912587073[47] = 0; - out_5705253479912587073[48] = 0; - out_5705253479912587073[49] = 0; - out_5705253479912587073[50] = 1; - out_5705253479912587073[51] = 0; - out_5705253479912587073[52] = 0; - out_5705253479912587073[53] = 1; +void H_10(double *state, double *unused, double *out_6146846088544597929) { + out_6146846088544597929[0] = 0; + out_6146846088544597929[1] = 9.8100000000000005*cos(state[1]); + out_6146846088544597929[2] = 0; + out_6146846088544597929[3] = 0; + out_6146846088544597929[4] = -state[8]; + out_6146846088544597929[5] = state[7]; + out_6146846088544597929[6] = 0; + out_6146846088544597929[7] = state[5]; + out_6146846088544597929[8] = -state[4]; + out_6146846088544597929[9] = 0; + out_6146846088544597929[10] = 0; + out_6146846088544597929[11] = 0; + out_6146846088544597929[12] = 1; + out_6146846088544597929[13] = 0; + out_6146846088544597929[14] = 0; + out_6146846088544597929[15] = 1; + out_6146846088544597929[16] = 0; + out_6146846088544597929[17] = 0; + out_6146846088544597929[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_6146846088544597929[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_6146846088544597929[20] = 0; + out_6146846088544597929[21] = state[8]; + out_6146846088544597929[22] = 0; + out_6146846088544597929[23] = -state[6]; + out_6146846088544597929[24] = -state[5]; + out_6146846088544597929[25] = 0; + out_6146846088544597929[26] = state[3]; + out_6146846088544597929[27] = 0; + out_6146846088544597929[28] = 0; + out_6146846088544597929[29] = 0; + out_6146846088544597929[30] = 0; + out_6146846088544597929[31] = 1; + out_6146846088544597929[32] = 0; + out_6146846088544597929[33] = 0; + out_6146846088544597929[34] = 1; + out_6146846088544597929[35] = 0; + out_6146846088544597929[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_6146846088544597929[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_6146846088544597929[38] = 0; + out_6146846088544597929[39] = -state[7]; + out_6146846088544597929[40] = state[6]; + out_6146846088544597929[41] = 0; + out_6146846088544597929[42] = state[4]; + out_6146846088544597929[43] = -state[3]; + out_6146846088544597929[44] = 0; + out_6146846088544597929[45] = 0; + out_6146846088544597929[46] = 0; + out_6146846088544597929[47] = 0; + out_6146846088544597929[48] = 0; + out_6146846088544597929[49] = 0; + out_6146846088544597929[50] = 1; + out_6146846088544597929[51] = 0; + out_6146846088544597929[52] = 0; + out_6146846088544597929[53] = 1; } -void h_13(double *state, double *unused, double *out_5867183224538439243) { - out_5867183224538439243[0] = state[3]; - out_5867183224538439243[1] = state[4]; - out_5867183224538439243[2] = state[5]; +void h_13(double *state, double *unused, double *out_2344100091734301077) { + out_2344100091734301077[0] = state[3]; + out_2344100091734301077[1] = state[4]; + out_2344100091734301077[2] = state[5]; } -void H_13(double *state, double *unused, double *out_71700055013391280) { - out_71700055013391280[0] = 0; - out_71700055013391280[1] = 0; - out_71700055013391280[2] = 0; - out_71700055013391280[3] = 1; - out_71700055013391280[4] = 0; - out_71700055013391280[5] = 0; - out_71700055013391280[6] = 0; - out_71700055013391280[7] = 0; - out_71700055013391280[8] = 0; - out_71700055013391280[9] = 0; - out_71700055013391280[10] = 0; - out_71700055013391280[11] = 0; - out_71700055013391280[12] = 0; - out_71700055013391280[13] = 0; - out_71700055013391280[14] = 0; - out_71700055013391280[15] = 0; - out_71700055013391280[16] = 0; - out_71700055013391280[17] = 0; - out_71700055013391280[18] = 0; - out_71700055013391280[19] = 0; - out_71700055013391280[20] = 0; - out_71700055013391280[21] = 0; - out_71700055013391280[22] = 1; - out_71700055013391280[23] = 0; - out_71700055013391280[24] = 0; - out_71700055013391280[25] = 0; - out_71700055013391280[26] = 0; - out_71700055013391280[27] = 0; - out_71700055013391280[28] = 0; - out_71700055013391280[29] = 0; - out_71700055013391280[30] = 0; - out_71700055013391280[31] = 0; - out_71700055013391280[32] = 0; - out_71700055013391280[33] = 0; - out_71700055013391280[34] = 0; - out_71700055013391280[35] = 0; - out_71700055013391280[36] = 0; - out_71700055013391280[37] = 0; - out_71700055013391280[38] = 0; - out_71700055013391280[39] = 0; - out_71700055013391280[40] = 0; - out_71700055013391280[41] = 1; - out_71700055013391280[42] = 0; - out_71700055013391280[43] = 0; - out_71700055013391280[44] = 0; - out_71700055013391280[45] = 0; - out_71700055013391280[46] = 0; - out_71700055013391280[47] = 0; - out_71700055013391280[48] = 0; - out_71700055013391280[49] = 0; - out_71700055013391280[50] = 0; - out_71700055013391280[51] = 0; - out_71700055013391280[52] = 0; - out_71700055013391280[53] = 0; +void H_13(double *state, double *unused, double *out_3909380197256637655) { + out_3909380197256637655[0] = 0; + out_3909380197256637655[1] = 0; + out_3909380197256637655[2] = 0; + out_3909380197256637655[3] = 1; + out_3909380197256637655[4] = 0; + out_3909380197256637655[5] = 0; + out_3909380197256637655[6] = 0; + out_3909380197256637655[7] = 0; + out_3909380197256637655[8] = 0; + out_3909380197256637655[9] = 0; + out_3909380197256637655[10] = 0; + out_3909380197256637655[11] = 0; + out_3909380197256637655[12] = 0; + out_3909380197256637655[13] = 0; + out_3909380197256637655[14] = 0; + out_3909380197256637655[15] = 0; + out_3909380197256637655[16] = 0; + out_3909380197256637655[17] = 0; + out_3909380197256637655[18] = 0; + out_3909380197256637655[19] = 0; + out_3909380197256637655[20] = 0; + out_3909380197256637655[21] = 0; + out_3909380197256637655[22] = 1; + out_3909380197256637655[23] = 0; + out_3909380197256637655[24] = 0; + out_3909380197256637655[25] = 0; + out_3909380197256637655[26] = 0; + out_3909380197256637655[27] = 0; + out_3909380197256637655[28] = 0; + out_3909380197256637655[29] = 0; + out_3909380197256637655[30] = 0; + out_3909380197256637655[31] = 0; + out_3909380197256637655[32] = 0; + out_3909380197256637655[33] = 0; + out_3909380197256637655[34] = 0; + out_3909380197256637655[35] = 0; + out_3909380197256637655[36] = 0; + out_3909380197256637655[37] = 0; + out_3909380197256637655[38] = 0; + out_3909380197256637655[39] = 0; + out_3909380197256637655[40] = 0; + out_3909380197256637655[41] = 1; + out_3909380197256637655[42] = 0; + out_3909380197256637655[43] = 0; + out_3909380197256637655[44] = 0; + out_3909380197256637655[45] = 0; + out_3909380197256637655[46] = 0; + out_3909380197256637655[47] = 0; + out_3909380197256637655[48] = 0; + out_3909380197256637655[49] = 0; + out_3909380197256637655[50] = 0; + out_3909380197256637655[51] = 0; + out_3909380197256637655[52] = 0; + out_3909380197256637655[53] = 0; } -void h_14(double *state, double *unused, double *out_4155160495787210910) { - out_4155160495787210910[0] = state[6]; - out_4155160495787210910[1] = state[7]; - out_4155160495787210910[2] = state[8]; +void h_14(double *state, double *unused, double *out_589344124113930337) { + out_589344124113930337[0] = state[6]; + out_589344124113930337[1] = state[7]; + out_589344124113930337[2] = state[8]; } -void H_14(double *state, double *unused, double *out_822667086020543008) { - out_822667086020543008[0] = 0; - out_822667086020543008[1] = 0; - out_822667086020543008[2] = 0; - out_822667086020543008[3] = 0; - out_822667086020543008[4] = 0; - out_822667086020543008[5] = 0; - out_822667086020543008[6] = 1; - out_822667086020543008[7] = 0; - out_822667086020543008[8] = 0; - out_822667086020543008[9] = 0; - out_822667086020543008[10] = 0; - out_822667086020543008[11] = 0; - out_822667086020543008[12] = 0; - out_822667086020543008[13] = 0; - out_822667086020543008[14] = 0; - out_822667086020543008[15] = 0; - out_822667086020543008[16] = 0; - out_822667086020543008[17] = 0; - out_822667086020543008[18] = 0; - out_822667086020543008[19] = 0; - out_822667086020543008[20] = 0; - out_822667086020543008[21] = 0; - out_822667086020543008[22] = 0; - out_822667086020543008[23] = 0; - out_822667086020543008[24] = 0; - out_822667086020543008[25] = 1; - out_822667086020543008[26] = 0; - out_822667086020543008[27] = 0; - out_822667086020543008[28] = 0; - out_822667086020543008[29] = 0; - out_822667086020543008[30] = 0; - out_822667086020543008[31] = 0; - out_822667086020543008[32] = 0; - out_822667086020543008[33] = 0; - out_822667086020543008[34] = 0; - out_822667086020543008[35] = 0; - out_822667086020543008[36] = 0; - out_822667086020543008[37] = 0; - out_822667086020543008[38] = 0; - out_822667086020543008[39] = 0; - out_822667086020543008[40] = 0; - out_822667086020543008[41] = 0; - out_822667086020543008[42] = 0; - out_822667086020543008[43] = 0; - out_822667086020543008[44] = 1; - out_822667086020543008[45] = 0; - out_822667086020543008[46] = 0; - out_822667086020543008[47] = 0; - out_822667086020543008[48] = 0; - out_822667086020543008[49] = 0; - out_822667086020543008[50] = 0; - out_822667086020543008[51] = 0; - out_822667086020543008[52] = 0; - out_822667086020543008[53] = 0; +void H_14(double *state, double *unused, double *out_3158413166249485927) { + out_3158413166249485927[0] = 0; + out_3158413166249485927[1] = 0; + out_3158413166249485927[2] = 0; + out_3158413166249485927[3] = 0; + out_3158413166249485927[4] = 0; + out_3158413166249485927[5] = 0; + out_3158413166249485927[6] = 1; + out_3158413166249485927[7] = 0; + out_3158413166249485927[8] = 0; + out_3158413166249485927[9] = 0; + out_3158413166249485927[10] = 0; + out_3158413166249485927[11] = 0; + out_3158413166249485927[12] = 0; + out_3158413166249485927[13] = 0; + out_3158413166249485927[14] = 0; + out_3158413166249485927[15] = 0; + out_3158413166249485927[16] = 0; + out_3158413166249485927[17] = 0; + out_3158413166249485927[18] = 0; + out_3158413166249485927[19] = 0; + out_3158413166249485927[20] = 0; + out_3158413166249485927[21] = 0; + out_3158413166249485927[22] = 0; + out_3158413166249485927[23] = 0; + out_3158413166249485927[24] = 0; + out_3158413166249485927[25] = 1; + out_3158413166249485927[26] = 0; + out_3158413166249485927[27] = 0; + out_3158413166249485927[28] = 0; + out_3158413166249485927[29] = 0; + out_3158413166249485927[30] = 0; + out_3158413166249485927[31] = 0; + out_3158413166249485927[32] = 0; + out_3158413166249485927[33] = 0; + out_3158413166249485927[34] = 0; + out_3158413166249485927[35] = 0; + out_3158413166249485927[36] = 0; + out_3158413166249485927[37] = 0; + out_3158413166249485927[38] = 0; + out_3158413166249485927[39] = 0; + out_3158413166249485927[40] = 0; + out_3158413166249485927[41] = 0; + out_3158413166249485927[42] = 0; + out_3158413166249485927[43] = 0; + out_3158413166249485927[44] = 1; + out_3158413166249485927[45] = 0; + out_3158413166249485927[46] = 0; + out_3158413166249485927[47] = 0; + out_3158413166249485927[48] = 0; + out_3158413166249485927[49] = 0; + out_3158413166249485927[50] = 0; + out_3158413166249485927[51] = 0; + out_3158413166249485927[52] = 0; + out_3158413166249485927[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_6079659893886918220) { - err_fun(nom_x, delta_x, out_6079659893886918220); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8550489212526755506) { + err_fun(nom_x, delta_x, out_8550489212526755506); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5200837164005369107) { - inv_err_fun(nom_x, true_x, out_5200837164005369107); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2197167056715094306) { + inv_err_fun(nom_x, true_x, out_2197167056715094306); } -void pose_H_mod_fun(double *state, double *out_1320875044412644660) { - H_mod_fun(state, out_1320875044412644660); +void pose_H_mod_fun(double *state, double *out_1273572215837011735) { + H_mod_fun(state, out_1273572215837011735); } -void pose_f_fun(double *state, double dt, double *out_4957392939814811366) { - f_fun(state, dt, out_4957392939814811366); +void pose_f_fun(double *state, double dt, double *out_58843541046312828) { + f_fun(state, dt, out_58843541046312828); } -void pose_F_fun(double *state, double dt, double *out_3660031818732653459) { - F_fun(state, dt, out_3660031818732653459); +void pose_F_fun(double *state, double dt, double *out_4952539587807327652) { + F_fun(state, dt, out_4952539587807327652); } -void pose_h_4(double *state, double *unused, double *out_4354770932396622955) { - h_4(state, unused, out_4354770932396622955); +void pose_h_4(double *state, double *unused, double *out_105029621528743882) { + h_4(state, unused, out_105029621528743882); } -void pose_H_4(double *state, double *unused, double *out_3140573770318941521) { - H_4(state, unused, out_3140573770318941521); +void pose_H_4(double *state, double *unused, double *out_7121654022588970456) { + H_4(state, unused, out_7121654022588970456); } -void pose_h_10(double *state, double *unused, double *out_8942219874713773227) { - h_10(state, unused, out_8942219874713773227); +void pose_h_10(double *state, double *unused, double *out_7355867801776776353) { + h_10(state, unused, out_7355867801776776353); } -void pose_H_10(double *state, double *unused, double *out_5705253479912587073) { - H_10(state, unused, out_5705253479912587073); +void pose_H_10(double *state, double *unused, double *out_6146846088544597929) { + H_10(state, unused, out_6146846088544597929); } -void pose_h_13(double *state, double *unused, double *out_5867183224538439243) { - h_13(state, unused, out_5867183224538439243); +void pose_h_13(double *state, double *unused, double *out_2344100091734301077) { + h_13(state, unused, out_2344100091734301077); } -void pose_H_13(double *state, double *unused, double *out_71700055013391280) { - H_13(state, unused, out_71700055013391280); +void pose_H_13(double *state, double *unused, double *out_3909380197256637655) { + H_13(state, unused, out_3909380197256637655); } -void pose_h_14(double *state, double *unused, double *out_4155160495787210910) { - h_14(state, unused, out_4155160495787210910); +void pose_h_14(double *state, double *unused, double *out_589344124113930337) { + h_14(state, unused, out_589344124113930337); } -void pose_H_14(double *state, double *unused, double *out_822667086020543008) { - H_14(state, unused, out_822667086020543008); +void pose_H_14(double *state, double *unused, double *out_3158413166249485927) { + H_14(state, unused, out_3158413166249485927); } 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 c2c4205834..2ec0f34a54 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_6079659893886918220); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5200837164005369107); -void pose_H_mod_fun(double *state, double *out_1320875044412644660); -void pose_f_fun(double *state, double dt, double *out_4957392939814811366); -void pose_F_fun(double *state, double dt, double *out_3660031818732653459); -void pose_h_4(double *state, double *unused, double *out_4354770932396622955); -void pose_H_4(double *state, double *unused, double *out_3140573770318941521); -void pose_h_10(double *state, double *unused, double *out_8942219874713773227); -void pose_H_10(double *state, double *unused, double *out_5705253479912587073); -void pose_h_13(double *state, double *unused, double *out_5867183224538439243); -void pose_H_13(double *state, double *unused, double *out_71700055013391280); -void pose_h_14(double *state, double *unused, double *out_4155160495787210910); -void pose_H_14(double *state, double *unused, double *out_822667086020543008); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8550489212526755506); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2197167056715094306); +void pose_H_mod_fun(double *state, double *out_1273572215837011735); +void pose_f_fun(double *state, double dt, double *out_58843541046312828); +void pose_F_fun(double *state, double dt, double *out_4952539587807327652); +void pose_h_4(double *state, double *unused, double *out_105029621528743882); +void pose_H_4(double *state, double *unused, double *out_7121654022588970456); +void pose_h_10(double *state, double *unused, double *out_7355867801776776353); +void pose_H_10(double *state, double *unused, double *out_6146846088544597929); +void pose_h_13(double *state, double *unused, double *out_2344100091734301077); +void pose_H_13(double *state, double *unused, double *out_3909380197256637655); +void pose_h_14(double *state, double *unused, double *out_589344124113930337); +void pose_H_14(double *state, double *unused, double *out_3158413166249485927); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 724413381c..4e383def0d 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ