diff --git a/cereal/libcereal.a b/cereal/libcereal.a index 269d7c0a0..5e74684bc 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index d6cdc1062..c6de4bd09 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 a6207c31a..f80df7adf 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 4cb2f807a..fcb68a53f 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 46b9d1d44..766f90e07 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 7a5c7ac36..98d647e3b 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 76e3729ea..e7b8cd9da 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 695957834..10b9d4c3e 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 695957834..10b9d4c3e 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 ed5bbe3a5..08fa96fc5 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 76e3729ea..e7b8cd9da 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 41c2bde33..16fd8be40 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-6ee00d3f-DEBUG"; +const uint8_t gitversion[19] = "DEV-028ee7b3-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index b3bd217cf..cc7af0bc8 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 461407aca..e531c0a71 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 5321b4c6f..44f5eab5d 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 145b6c2ff..53b31e7ff 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 b0a488b9a..5bc59643a 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 4b988f68b..5a7dc4537 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 4cfc88ca8..ac6a902bb 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 cee69bb26..7201448be 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 882889cd2..3d8e6171b 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 fea89a823..51722a4e7 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 a7a30f5cc..934721f42 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 d441e8bc9..6b0c9155a 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 b6034f158..24eac1a38 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 a60faaec9..51cc03ef7 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 8173cc10f..bf6bd5169 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 f44fd5993..a153aefd1 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 13c350da7..40daf9a34 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 31a6380fe..30e2ec11f 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 7cedbf9a1..f3c33645f 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 91a7a2ea8..e45078f16 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 da62c943d..2b6d75b05 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-6ee00d3f-DEBUG \ No newline at end of file +DEV-028ee7b3-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 187abd1e4..a9ae05958 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_1099503832950344155) { - out_1099503832950344155[0] = delta_x[0] + nom_x[0]; - out_1099503832950344155[1] = delta_x[1] + nom_x[1]; - out_1099503832950344155[2] = delta_x[2] + nom_x[2]; - out_1099503832950344155[3] = delta_x[3] + nom_x[3]; - out_1099503832950344155[4] = delta_x[4] + nom_x[4]; - out_1099503832950344155[5] = delta_x[5] + nom_x[5]; - out_1099503832950344155[6] = delta_x[6] + nom_x[6]; - out_1099503832950344155[7] = delta_x[7] + nom_x[7]; - out_1099503832950344155[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_8227521165613923208) { + out_8227521165613923208[0] = delta_x[0] + nom_x[0]; + out_8227521165613923208[1] = delta_x[1] + nom_x[1]; + out_8227521165613923208[2] = delta_x[2] + nom_x[2]; + out_8227521165613923208[3] = delta_x[3] + nom_x[3]; + out_8227521165613923208[4] = delta_x[4] + nom_x[4]; + out_8227521165613923208[5] = delta_x[5] + nom_x[5]; + out_8227521165613923208[6] = delta_x[6] + nom_x[6]; + out_8227521165613923208[7] = delta_x[7] + nom_x[7]; + out_8227521165613923208[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1857055699068035744) { - out_1857055699068035744[0] = -nom_x[0] + true_x[0]; - out_1857055699068035744[1] = -nom_x[1] + true_x[1]; - out_1857055699068035744[2] = -nom_x[2] + true_x[2]; - out_1857055699068035744[3] = -nom_x[3] + true_x[3]; - out_1857055699068035744[4] = -nom_x[4] + true_x[4]; - out_1857055699068035744[5] = -nom_x[5] + true_x[5]; - out_1857055699068035744[6] = -nom_x[6] + true_x[6]; - out_1857055699068035744[7] = -nom_x[7] + true_x[7]; - out_1857055699068035744[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5130787649833322574) { + out_5130787649833322574[0] = -nom_x[0] + true_x[0]; + out_5130787649833322574[1] = -nom_x[1] + true_x[1]; + out_5130787649833322574[2] = -nom_x[2] + true_x[2]; + out_5130787649833322574[3] = -nom_x[3] + true_x[3]; + out_5130787649833322574[4] = -nom_x[4] + true_x[4]; + out_5130787649833322574[5] = -nom_x[5] + true_x[5]; + out_5130787649833322574[6] = -nom_x[6] + true_x[6]; + out_5130787649833322574[7] = -nom_x[7] + true_x[7]; + out_5130787649833322574[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_6364761287714699355) { - out_6364761287714699355[0] = 1.0; - out_6364761287714699355[1] = 0.0; - out_6364761287714699355[2] = 0.0; - out_6364761287714699355[3] = 0.0; - out_6364761287714699355[4] = 0.0; - out_6364761287714699355[5] = 0.0; - out_6364761287714699355[6] = 0.0; - out_6364761287714699355[7] = 0.0; - out_6364761287714699355[8] = 0.0; - out_6364761287714699355[9] = 0.0; - out_6364761287714699355[10] = 1.0; - out_6364761287714699355[11] = 0.0; - out_6364761287714699355[12] = 0.0; - out_6364761287714699355[13] = 0.0; - out_6364761287714699355[14] = 0.0; - out_6364761287714699355[15] = 0.0; - out_6364761287714699355[16] = 0.0; - out_6364761287714699355[17] = 0.0; - out_6364761287714699355[18] = 0.0; - out_6364761287714699355[19] = 0.0; - out_6364761287714699355[20] = 1.0; - out_6364761287714699355[21] = 0.0; - out_6364761287714699355[22] = 0.0; - out_6364761287714699355[23] = 0.0; - out_6364761287714699355[24] = 0.0; - out_6364761287714699355[25] = 0.0; - out_6364761287714699355[26] = 0.0; - out_6364761287714699355[27] = 0.0; - out_6364761287714699355[28] = 0.0; - out_6364761287714699355[29] = 0.0; - out_6364761287714699355[30] = 1.0; - out_6364761287714699355[31] = 0.0; - out_6364761287714699355[32] = 0.0; - out_6364761287714699355[33] = 0.0; - out_6364761287714699355[34] = 0.0; - out_6364761287714699355[35] = 0.0; - out_6364761287714699355[36] = 0.0; - out_6364761287714699355[37] = 0.0; - out_6364761287714699355[38] = 0.0; - out_6364761287714699355[39] = 0.0; - out_6364761287714699355[40] = 1.0; - out_6364761287714699355[41] = 0.0; - out_6364761287714699355[42] = 0.0; - out_6364761287714699355[43] = 0.0; - out_6364761287714699355[44] = 0.0; - out_6364761287714699355[45] = 0.0; - out_6364761287714699355[46] = 0.0; - out_6364761287714699355[47] = 0.0; - out_6364761287714699355[48] = 0.0; - out_6364761287714699355[49] = 0.0; - out_6364761287714699355[50] = 1.0; - out_6364761287714699355[51] = 0.0; - out_6364761287714699355[52] = 0.0; - out_6364761287714699355[53] = 0.0; - out_6364761287714699355[54] = 0.0; - out_6364761287714699355[55] = 0.0; - out_6364761287714699355[56] = 0.0; - out_6364761287714699355[57] = 0.0; - out_6364761287714699355[58] = 0.0; - out_6364761287714699355[59] = 0.0; - out_6364761287714699355[60] = 1.0; - out_6364761287714699355[61] = 0.0; - out_6364761287714699355[62] = 0.0; - out_6364761287714699355[63] = 0.0; - out_6364761287714699355[64] = 0.0; - out_6364761287714699355[65] = 0.0; - out_6364761287714699355[66] = 0.0; - out_6364761287714699355[67] = 0.0; - out_6364761287714699355[68] = 0.0; - out_6364761287714699355[69] = 0.0; - out_6364761287714699355[70] = 1.0; - out_6364761287714699355[71] = 0.0; - out_6364761287714699355[72] = 0.0; - out_6364761287714699355[73] = 0.0; - out_6364761287714699355[74] = 0.0; - out_6364761287714699355[75] = 0.0; - out_6364761287714699355[76] = 0.0; - out_6364761287714699355[77] = 0.0; - out_6364761287714699355[78] = 0.0; - out_6364761287714699355[79] = 0.0; - out_6364761287714699355[80] = 1.0; +void H_mod_fun(double *state, double *out_7022051272719162928) { + out_7022051272719162928[0] = 1.0; + out_7022051272719162928[1] = 0.0; + out_7022051272719162928[2] = 0.0; + out_7022051272719162928[3] = 0.0; + out_7022051272719162928[4] = 0.0; + out_7022051272719162928[5] = 0.0; + out_7022051272719162928[6] = 0.0; + out_7022051272719162928[7] = 0.0; + out_7022051272719162928[8] = 0.0; + out_7022051272719162928[9] = 0.0; + out_7022051272719162928[10] = 1.0; + out_7022051272719162928[11] = 0.0; + out_7022051272719162928[12] = 0.0; + out_7022051272719162928[13] = 0.0; + out_7022051272719162928[14] = 0.0; + out_7022051272719162928[15] = 0.0; + out_7022051272719162928[16] = 0.0; + out_7022051272719162928[17] = 0.0; + out_7022051272719162928[18] = 0.0; + out_7022051272719162928[19] = 0.0; + out_7022051272719162928[20] = 1.0; + out_7022051272719162928[21] = 0.0; + out_7022051272719162928[22] = 0.0; + out_7022051272719162928[23] = 0.0; + out_7022051272719162928[24] = 0.0; + out_7022051272719162928[25] = 0.0; + out_7022051272719162928[26] = 0.0; + out_7022051272719162928[27] = 0.0; + out_7022051272719162928[28] = 0.0; + out_7022051272719162928[29] = 0.0; + out_7022051272719162928[30] = 1.0; + out_7022051272719162928[31] = 0.0; + out_7022051272719162928[32] = 0.0; + out_7022051272719162928[33] = 0.0; + out_7022051272719162928[34] = 0.0; + out_7022051272719162928[35] = 0.0; + out_7022051272719162928[36] = 0.0; + out_7022051272719162928[37] = 0.0; + out_7022051272719162928[38] = 0.0; + out_7022051272719162928[39] = 0.0; + out_7022051272719162928[40] = 1.0; + out_7022051272719162928[41] = 0.0; + out_7022051272719162928[42] = 0.0; + out_7022051272719162928[43] = 0.0; + out_7022051272719162928[44] = 0.0; + out_7022051272719162928[45] = 0.0; + out_7022051272719162928[46] = 0.0; + out_7022051272719162928[47] = 0.0; + out_7022051272719162928[48] = 0.0; + out_7022051272719162928[49] = 0.0; + out_7022051272719162928[50] = 1.0; + out_7022051272719162928[51] = 0.0; + out_7022051272719162928[52] = 0.0; + out_7022051272719162928[53] = 0.0; + out_7022051272719162928[54] = 0.0; + out_7022051272719162928[55] = 0.0; + out_7022051272719162928[56] = 0.0; + out_7022051272719162928[57] = 0.0; + out_7022051272719162928[58] = 0.0; + out_7022051272719162928[59] = 0.0; + out_7022051272719162928[60] = 1.0; + out_7022051272719162928[61] = 0.0; + out_7022051272719162928[62] = 0.0; + out_7022051272719162928[63] = 0.0; + out_7022051272719162928[64] = 0.0; + out_7022051272719162928[65] = 0.0; + out_7022051272719162928[66] = 0.0; + out_7022051272719162928[67] = 0.0; + out_7022051272719162928[68] = 0.0; + out_7022051272719162928[69] = 0.0; + out_7022051272719162928[70] = 1.0; + out_7022051272719162928[71] = 0.0; + out_7022051272719162928[72] = 0.0; + out_7022051272719162928[73] = 0.0; + out_7022051272719162928[74] = 0.0; + out_7022051272719162928[75] = 0.0; + out_7022051272719162928[76] = 0.0; + out_7022051272719162928[77] = 0.0; + out_7022051272719162928[78] = 0.0; + out_7022051272719162928[79] = 0.0; + out_7022051272719162928[80] = 1.0; } -void f_fun(double *state, double dt, double *out_8704640073878126620) { - out_8704640073878126620[0] = state[0]; - out_8704640073878126620[1] = state[1]; - out_8704640073878126620[2] = state[2]; - out_8704640073878126620[3] = state[3]; - out_8704640073878126620[4] = state[4]; - out_8704640073878126620[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_8704640073878126620[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_8704640073878126620[7] = state[7]; - out_8704640073878126620[8] = state[8]; +void f_fun(double *state, double dt, double *out_2984704176532588452) { + out_2984704176532588452[0] = state[0]; + out_2984704176532588452[1] = state[1]; + out_2984704176532588452[2] = state[2]; + out_2984704176532588452[3] = state[3]; + out_2984704176532588452[4] = state[4]; + out_2984704176532588452[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_2984704176532588452[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_2984704176532588452[7] = state[7]; + out_2984704176532588452[8] = state[8]; } -void F_fun(double *state, double dt, double *out_2231700017024740017) { - out_2231700017024740017[0] = 1; - out_2231700017024740017[1] = 0; - out_2231700017024740017[2] = 0; - out_2231700017024740017[3] = 0; - out_2231700017024740017[4] = 0; - out_2231700017024740017[5] = 0; - out_2231700017024740017[6] = 0; - out_2231700017024740017[7] = 0; - out_2231700017024740017[8] = 0; - out_2231700017024740017[9] = 0; - out_2231700017024740017[10] = 1; - out_2231700017024740017[11] = 0; - out_2231700017024740017[12] = 0; - out_2231700017024740017[13] = 0; - out_2231700017024740017[14] = 0; - out_2231700017024740017[15] = 0; - out_2231700017024740017[16] = 0; - out_2231700017024740017[17] = 0; - out_2231700017024740017[18] = 0; - out_2231700017024740017[19] = 0; - out_2231700017024740017[20] = 1; - out_2231700017024740017[21] = 0; - out_2231700017024740017[22] = 0; - out_2231700017024740017[23] = 0; - out_2231700017024740017[24] = 0; - out_2231700017024740017[25] = 0; - out_2231700017024740017[26] = 0; - out_2231700017024740017[27] = 0; - out_2231700017024740017[28] = 0; - out_2231700017024740017[29] = 0; - out_2231700017024740017[30] = 1; - out_2231700017024740017[31] = 0; - out_2231700017024740017[32] = 0; - out_2231700017024740017[33] = 0; - out_2231700017024740017[34] = 0; - out_2231700017024740017[35] = 0; - out_2231700017024740017[36] = 0; - out_2231700017024740017[37] = 0; - out_2231700017024740017[38] = 0; - out_2231700017024740017[39] = 0; - out_2231700017024740017[40] = 1; - out_2231700017024740017[41] = 0; - out_2231700017024740017[42] = 0; - out_2231700017024740017[43] = 0; - out_2231700017024740017[44] = 0; - out_2231700017024740017[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_2231700017024740017[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_2231700017024740017[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2231700017024740017[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2231700017024740017[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_2231700017024740017[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_2231700017024740017[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_2231700017024740017[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_2231700017024740017[53] = -9.8100000000000005*dt; - out_2231700017024740017[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_2231700017024740017[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_2231700017024740017[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2231700017024740017[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2231700017024740017[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_2231700017024740017[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_2231700017024740017[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_2231700017024740017[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2231700017024740017[62] = 0; - out_2231700017024740017[63] = 0; - out_2231700017024740017[64] = 0; - out_2231700017024740017[65] = 0; - out_2231700017024740017[66] = 0; - out_2231700017024740017[67] = 0; - out_2231700017024740017[68] = 0; - out_2231700017024740017[69] = 0; - out_2231700017024740017[70] = 1; - out_2231700017024740017[71] = 0; - out_2231700017024740017[72] = 0; - out_2231700017024740017[73] = 0; - out_2231700017024740017[74] = 0; - out_2231700017024740017[75] = 0; - out_2231700017024740017[76] = 0; - out_2231700017024740017[77] = 0; - out_2231700017024740017[78] = 0; - out_2231700017024740017[79] = 0; - out_2231700017024740017[80] = 1; +void F_fun(double *state, double dt, double *out_314948888837440074) { + out_314948888837440074[0] = 1; + out_314948888837440074[1] = 0; + out_314948888837440074[2] = 0; + out_314948888837440074[3] = 0; + out_314948888837440074[4] = 0; + out_314948888837440074[5] = 0; + out_314948888837440074[6] = 0; + out_314948888837440074[7] = 0; + out_314948888837440074[8] = 0; + out_314948888837440074[9] = 0; + out_314948888837440074[10] = 1; + out_314948888837440074[11] = 0; + out_314948888837440074[12] = 0; + out_314948888837440074[13] = 0; + out_314948888837440074[14] = 0; + out_314948888837440074[15] = 0; + out_314948888837440074[16] = 0; + out_314948888837440074[17] = 0; + out_314948888837440074[18] = 0; + out_314948888837440074[19] = 0; + out_314948888837440074[20] = 1; + out_314948888837440074[21] = 0; + out_314948888837440074[22] = 0; + out_314948888837440074[23] = 0; + out_314948888837440074[24] = 0; + out_314948888837440074[25] = 0; + out_314948888837440074[26] = 0; + out_314948888837440074[27] = 0; + out_314948888837440074[28] = 0; + out_314948888837440074[29] = 0; + out_314948888837440074[30] = 1; + out_314948888837440074[31] = 0; + out_314948888837440074[32] = 0; + out_314948888837440074[33] = 0; + out_314948888837440074[34] = 0; + out_314948888837440074[35] = 0; + out_314948888837440074[36] = 0; + out_314948888837440074[37] = 0; + out_314948888837440074[38] = 0; + out_314948888837440074[39] = 0; + out_314948888837440074[40] = 1; + out_314948888837440074[41] = 0; + out_314948888837440074[42] = 0; + out_314948888837440074[43] = 0; + out_314948888837440074[44] = 0; + out_314948888837440074[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_314948888837440074[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_314948888837440074[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_314948888837440074[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_314948888837440074[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_314948888837440074[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_314948888837440074[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_314948888837440074[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_314948888837440074[53] = -9.8100000000000005*dt; + out_314948888837440074[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_314948888837440074[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_314948888837440074[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_314948888837440074[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_314948888837440074[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_314948888837440074[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_314948888837440074[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_314948888837440074[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_314948888837440074[62] = 0; + out_314948888837440074[63] = 0; + out_314948888837440074[64] = 0; + out_314948888837440074[65] = 0; + out_314948888837440074[66] = 0; + out_314948888837440074[67] = 0; + out_314948888837440074[68] = 0; + out_314948888837440074[69] = 0; + out_314948888837440074[70] = 1; + out_314948888837440074[71] = 0; + out_314948888837440074[72] = 0; + out_314948888837440074[73] = 0; + out_314948888837440074[74] = 0; + out_314948888837440074[75] = 0; + out_314948888837440074[76] = 0; + out_314948888837440074[77] = 0; + out_314948888837440074[78] = 0; + out_314948888837440074[79] = 0; + out_314948888837440074[80] = 1; } -void h_25(double *state, double *unused, double *out_4358124578925276600) { - out_4358124578925276600[0] = state[6]; +void h_25(double *state, double *unused, double *out_26482314378299030) { + out_26482314378299030[0] = state[6]; } -void H_25(double *state, double *unused, double *out_2841780637725207678) { - out_2841780637725207678[0] = 0; - out_2841780637725207678[1] = 0; - out_2841780637725207678[2] = 0; - out_2841780637725207678[3] = 0; - out_2841780637725207678[4] = 0; - out_2841780637725207678[5] = 0; - out_2841780637725207678[6] = 1; - out_2841780637725207678[7] = 0; - out_2841780637725207678[8] = 0; +void H_25(double *state, double *unused, double *out_3499070622729671251) { + out_3499070622729671251[0] = 0; + out_3499070622729671251[1] = 0; + out_3499070622729671251[2] = 0; + out_3499070622729671251[3] = 0; + out_3499070622729671251[4] = 0; + out_3499070622729671251[5] = 0; + out_3499070622729671251[6] = 1; + out_3499070622729671251[7] = 0; + out_3499070622729671251[8] = 0; } -void h_24(double *state, double *unused, double *out_8493679030338339311) { - out_8493679030338339311[0] = state[4]; - out_8493679030338339311[1] = state[5]; +void h_24(double *state, double *unused, double *out_7613998408709405751) { + out_7613998408709405751[0] = state[4]; + out_7613998408709405751[1] = state[5]; } -void H_24(double *state, double *unused, double *out_716144961077115458) { - out_716144961077115458[0] = 0; - out_716144961077115458[1] = 0; - out_716144961077115458[2] = 0; - out_716144961077115458[3] = 0; - out_716144961077115458[4] = 1; - out_716144961077115458[5] = 0; - out_716144961077115458[6] = 0; - out_716144961077115458[7] = 0; - out_716144961077115458[8] = 0; - out_716144961077115458[9] = 0; - out_716144961077115458[10] = 0; - out_716144961077115458[11] = 0; - out_716144961077115458[12] = 0; - out_716144961077115458[13] = 0; - out_716144961077115458[14] = 1; - out_716144961077115458[15] = 0; - out_716144961077115458[16] = 0; - out_716144961077115458[17] = 0; +void H_24(double *state, double *unused, double *out_1273362838750802689) { + out_1273362838750802689[0] = 0; + out_1273362838750802689[1] = 0; + out_1273362838750802689[2] = 0; + out_1273362838750802689[3] = 0; + out_1273362838750802689[4] = 1; + out_1273362838750802689[5] = 0; + out_1273362838750802689[6] = 0; + out_1273362838750802689[7] = 0; + out_1273362838750802689[8] = 0; + out_1273362838750802689[9] = 0; + out_1273362838750802689[10] = 0; + out_1273362838750802689[11] = 0; + out_1273362838750802689[12] = 0; + out_1273362838750802689[13] = 0; + out_1273362838750802689[14] = 1; + out_1273362838750802689[15] = 0; + out_1273362838750802689[16] = 0; + out_1273362838750802689[17] = 0; } -void h_30(double *state, double *unused, double *out_3523148071828772585) { - out_3523148071828772585[0] = state[4]; +void h_30(double *state, double *unused, double *out_7907754965132348215) { + out_7907754965132348215[0] = state[4]; } -void H_30(double *state, double *unused, double *out_4074909703766409077) { - out_4074909703766409077[0] = 0; - out_4074909703766409077[1] = 0; - out_4074909703766409077[2] = 0; - out_4074909703766409077[3] = 0; - out_4074909703766409077[4] = 1; - out_4074909703766409077[5] = 0; - out_4074909703766409077[6] = 0; - out_4074909703766409077[7] = 0; - out_4074909703766409077[8] = 0; +void H_30(double *state, double *unused, double *out_3417619718761945504) { + out_3417619718761945504[0] = 0; + out_3417619718761945504[1] = 0; + out_3417619718761945504[2] = 0; + out_3417619718761945504[3] = 0; + out_3417619718761945504[4] = 1; + out_3417619718761945504[5] = 0; + out_3417619718761945504[6] = 0; + out_3417619718761945504[7] = 0; + out_3417619718761945504[8] = 0; } -void h_26(double *state, double *unused, double *out_1478525195832073060) { - out_1478525195832073060[0] = state[7]; +void h_26(double *state, double *unused, double *out_7304439080455870698) { + out_7304439080455870698[0] = state[7]; } -void H_26(double *state, double *unused, double *out_6583283956599263902) { - out_6583283956599263902[0] = 0; - out_6583283956599263902[1] = 0; - out_6583283956599263902[2] = 0; - out_6583283956599263902[3] = 0; - out_6583283956599263902[4] = 0; - out_6583283956599263902[5] = 0; - out_6583283956599263902[6] = 0; - out_6583283956599263902[7] = 1; - out_6583283956599263902[8] = 0; +void H_26(double *state, double *unused, double *out_2842216558619359347) { + out_2842216558619359347[0] = 0; + out_2842216558619359347[1] = 0; + out_2842216558619359347[2] = 0; + out_2842216558619359347[3] = 0; + out_2842216558619359347[4] = 0; + out_2842216558619359347[5] = 0; + out_2842216558619359347[6] = 0; + out_2842216558619359347[7] = 1; + out_2842216558619359347[8] = 0; } -void h_27(double *state, double *unused, double *out_643548688735569045) { - out_643548688735569045[0] = state[3]; +void h_27(double *state, double *unused, double *out_8106273622613022122) { + out_8106273622613022122[0] = state[3]; } -void H_27(double *state, double *unused, double *out_1900146391965984166) { - out_1900146391965984166[0] = 0; - out_1900146391965984166[1] = 0; - out_1900146391965984166[2] = 0; - out_1900146391965984166[3] = 1; - out_1900146391965984166[4] = 0; - out_1900146391965984166[5] = 0; - out_1900146391965984166[6] = 0; - out_1900146391965984166[7] = 0; - out_1900146391965984166[8] = 0; +void H_27(double *state, double *unused, double *out_1242856406961520593) { + out_1242856406961520593[0] = 0; + out_1242856406961520593[1] = 0; + out_1242856406961520593[2] = 0; + out_1242856406961520593[3] = 1; + out_1242856406961520593[4] = 0; + out_1242856406961520593[5] = 0; + out_1242856406961520593[6] = 0; + out_1242856406961520593[7] = 0; + out_1242856406961520593[8] = 0; } -void h_29(double *state, double *unused, double *out_4280998216989591395) { - out_4280998216989591395[0] = state[1]; +void h_29(double *state, double *unused, double *out_1742679644311206261) { + out_1742679644311206261[0] = state[1]; } -void H_29(double *state, double *unused, double *out_4585141048080801261) { - out_4585141048080801261[0] = 0; - out_4585141048080801261[1] = 1; - out_4585141048080801261[2] = 0; - out_4585141048080801261[3] = 0; - out_4585141048080801261[4] = 0; - out_4585141048080801261[5] = 0; - out_4585141048080801261[6] = 0; - out_4585141048080801261[7] = 0; - out_4585141048080801261[8] = 0; +void H_29(double *state, double *unused, double *out_3927851063076337688) { + out_3927851063076337688[0] = 0; + out_3927851063076337688[1] = 1; + out_3927851063076337688[2] = 0; + out_3927851063076337688[3] = 0; + out_3927851063076337688[4] = 0; + out_3927851063076337688[5] = 0; + out_3927851063076337688[6] = 0; + out_3927851063076337688[7] = 0; + out_3927851063076337688[8] = 0; } -void h_28(double *state, double *unused, double *out_8338420483271271720) { - out_8338420483271271720[0] = state[0]; +void h_28(double *state, double *unused, double *out_6626345518038675162) { + out_6626345518038675162[0] = state[0]; } -void H_28(double *state, double *unused, double *out_497257968988729313) { - out_497257968988729313[0] = 1; - out_497257968988729313[1] = 0; - out_497257968988729313[2] = 0; - out_497257968988729313[3] = 0; - out_497257968988729313[4] = 0; - out_497257968988729313[5] = 0; - out_497257968988729313[6] = 0; - out_497257968988729313[7] = 0; - out_497257968988729313[8] = 0; +void H_28(double *state, double *unused, double *out_1154547953993192886) { + out_1154547953993192886[0] = 1; + out_1154547953993192886[1] = 0; + out_1154547953993192886[2] = 0; + out_1154547953993192886[3] = 0; + out_1154547953993192886[4] = 0; + out_1154547953993192886[5] = 0; + out_1154547953993192886[6] = 0; + out_1154547953993192886[7] = 0; + out_1154547953993192886[8] = 0; } -void h_31(double *state, double *unused, double *out_7305850569280761561) { - out_7305850569280761561[0] = state[8]; +void h_31(double *state, double *unused, double *out_1477113707007182197) { + out_1477113707007182197[0] = state[8]; } -void H_31(double *state, double *unused, double *out_2811134675848247250) { - out_2811134675848247250[0] = 0; - out_2811134675848247250[1] = 0; - out_2811134675848247250[2] = 0; - out_2811134675848247250[3] = 0; - out_2811134675848247250[4] = 0; - out_2811134675848247250[5] = 0; - out_2811134675848247250[6] = 0; - out_2811134675848247250[7] = 0; - out_2811134675848247250[8] = 1; +void H_31(double *state, double *unused, double *out_3468424660852710823) { + out_3468424660852710823[0] = 0; + out_3468424660852710823[1] = 0; + out_3468424660852710823[2] = 0; + out_3468424660852710823[3] = 0; + out_3468424660852710823[4] = 0; + out_3468424660852710823[5] = 0; + out_3468424660852710823[6] = 0; + out_3468424660852710823[7] = 0; + out_3468424660852710823[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_1099503832950344155) { - err_fun(nom_x, delta_x, out_1099503832950344155); +void car_err_fun(double *nom_x, double *delta_x, double *out_8227521165613923208) { + err_fun(nom_x, delta_x, out_8227521165613923208); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1857055699068035744) { - inv_err_fun(nom_x, true_x, out_1857055699068035744); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5130787649833322574) { + inv_err_fun(nom_x, true_x, out_5130787649833322574); } -void car_H_mod_fun(double *state, double *out_6364761287714699355) { - H_mod_fun(state, out_6364761287714699355); +void car_H_mod_fun(double *state, double *out_7022051272719162928) { + H_mod_fun(state, out_7022051272719162928); } -void car_f_fun(double *state, double dt, double *out_8704640073878126620) { - f_fun(state, dt, out_8704640073878126620); +void car_f_fun(double *state, double dt, double *out_2984704176532588452) { + f_fun(state, dt, out_2984704176532588452); } -void car_F_fun(double *state, double dt, double *out_2231700017024740017) { - F_fun(state, dt, out_2231700017024740017); +void car_F_fun(double *state, double dt, double *out_314948888837440074) { + F_fun(state, dt, out_314948888837440074); } -void car_h_25(double *state, double *unused, double *out_4358124578925276600) { - h_25(state, unused, out_4358124578925276600); +void car_h_25(double *state, double *unused, double *out_26482314378299030) { + h_25(state, unused, out_26482314378299030); } -void car_H_25(double *state, double *unused, double *out_2841780637725207678) { - H_25(state, unused, out_2841780637725207678); +void car_H_25(double *state, double *unused, double *out_3499070622729671251) { + H_25(state, unused, out_3499070622729671251); } -void car_h_24(double *state, double *unused, double *out_8493679030338339311) { - h_24(state, unused, out_8493679030338339311); +void car_h_24(double *state, double *unused, double *out_7613998408709405751) { + h_24(state, unused, out_7613998408709405751); } -void car_H_24(double *state, double *unused, double *out_716144961077115458) { - H_24(state, unused, out_716144961077115458); +void car_H_24(double *state, double *unused, double *out_1273362838750802689) { + H_24(state, unused, out_1273362838750802689); } -void car_h_30(double *state, double *unused, double *out_3523148071828772585) { - h_30(state, unused, out_3523148071828772585); +void car_h_30(double *state, double *unused, double *out_7907754965132348215) { + h_30(state, unused, out_7907754965132348215); } -void car_H_30(double *state, double *unused, double *out_4074909703766409077) { - H_30(state, unused, out_4074909703766409077); +void car_H_30(double *state, double *unused, double *out_3417619718761945504) { + H_30(state, unused, out_3417619718761945504); } -void car_h_26(double *state, double *unused, double *out_1478525195832073060) { - h_26(state, unused, out_1478525195832073060); +void car_h_26(double *state, double *unused, double *out_7304439080455870698) { + h_26(state, unused, out_7304439080455870698); } -void car_H_26(double *state, double *unused, double *out_6583283956599263902) { - H_26(state, unused, out_6583283956599263902); +void car_H_26(double *state, double *unused, double *out_2842216558619359347) { + H_26(state, unused, out_2842216558619359347); } -void car_h_27(double *state, double *unused, double *out_643548688735569045) { - h_27(state, unused, out_643548688735569045); +void car_h_27(double *state, double *unused, double *out_8106273622613022122) { + h_27(state, unused, out_8106273622613022122); } -void car_H_27(double *state, double *unused, double *out_1900146391965984166) { - H_27(state, unused, out_1900146391965984166); +void car_H_27(double *state, double *unused, double *out_1242856406961520593) { + H_27(state, unused, out_1242856406961520593); } -void car_h_29(double *state, double *unused, double *out_4280998216989591395) { - h_29(state, unused, out_4280998216989591395); +void car_h_29(double *state, double *unused, double *out_1742679644311206261) { + h_29(state, unused, out_1742679644311206261); } -void car_H_29(double *state, double *unused, double *out_4585141048080801261) { - H_29(state, unused, out_4585141048080801261); +void car_H_29(double *state, double *unused, double *out_3927851063076337688) { + H_29(state, unused, out_3927851063076337688); } -void car_h_28(double *state, double *unused, double *out_8338420483271271720) { - h_28(state, unused, out_8338420483271271720); +void car_h_28(double *state, double *unused, double *out_6626345518038675162) { + h_28(state, unused, out_6626345518038675162); } -void car_H_28(double *state, double *unused, double *out_497257968988729313) { - H_28(state, unused, out_497257968988729313); +void car_H_28(double *state, double *unused, double *out_1154547953993192886) { + H_28(state, unused, out_1154547953993192886); } -void car_h_31(double *state, double *unused, double *out_7305850569280761561) { - h_31(state, unused, out_7305850569280761561); +void car_h_31(double *state, double *unused, double *out_1477113707007182197) { + h_31(state, unused, out_1477113707007182197); } -void car_H_31(double *state, double *unused, double *out_2811134675848247250) { - H_31(state, unused, out_2811134675848247250); +void car_H_31(double *state, double *unused, double *out_3468424660852710823) { + H_31(state, unused, out_3468424660852710823); } 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 15ddc71ae..db87bbb94 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_1099503832950344155); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1857055699068035744); -void car_H_mod_fun(double *state, double *out_6364761287714699355); -void car_f_fun(double *state, double dt, double *out_8704640073878126620); -void car_F_fun(double *state, double dt, double *out_2231700017024740017); -void car_h_25(double *state, double *unused, double *out_4358124578925276600); -void car_H_25(double *state, double *unused, double *out_2841780637725207678); -void car_h_24(double *state, double *unused, double *out_8493679030338339311); -void car_H_24(double *state, double *unused, double *out_716144961077115458); -void car_h_30(double *state, double *unused, double *out_3523148071828772585); -void car_H_30(double *state, double *unused, double *out_4074909703766409077); -void car_h_26(double *state, double *unused, double *out_1478525195832073060); -void car_H_26(double *state, double *unused, double *out_6583283956599263902); -void car_h_27(double *state, double *unused, double *out_643548688735569045); -void car_H_27(double *state, double *unused, double *out_1900146391965984166); -void car_h_29(double *state, double *unused, double *out_4280998216989591395); -void car_H_29(double *state, double *unused, double *out_4585141048080801261); -void car_h_28(double *state, double *unused, double *out_8338420483271271720); -void car_H_28(double *state, double *unused, double *out_497257968988729313); -void car_h_31(double *state, double *unused, double *out_7305850569280761561); -void car_H_31(double *state, double *unused, double *out_2811134675848247250); +void car_err_fun(double *nom_x, double *delta_x, double *out_8227521165613923208); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5130787649833322574); +void car_H_mod_fun(double *state, double *out_7022051272719162928); +void car_f_fun(double *state, double dt, double *out_2984704176532588452); +void car_F_fun(double *state, double dt, double *out_314948888837440074); +void car_h_25(double *state, double *unused, double *out_26482314378299030); +void car_H_25(double *state, double *unused, double *out_3499070622729671251); +void car_h_24(double *state, double *unused, double *out_7613998408709405751); +void car_H_24(double *state, double *unused, double *out_1273362838750802689); +void car_h_30(double *state, double *unused, double *out_7907754965132348215); +void car_H_30(double *state, double *unused, double *out_3417619718761945504); +void car_h_26(double *state, double *unused, double *out_7304439080455870698); +void car_H_26(double *state, double *unused, double *out_2842216558619359347); +void car_h_27(double *state, double *unused, double *out_8106273622613022122); +void car_H_27(double *state, double *unused, double *out_1242856406961520593); +void car_h_29(double *state, double *unused, double *out_1742679644311206261); +void car_H_29(double *state, double *unused, double *out_3927851063076337688); +void car_h_28(double *state, double *unused, double *out_6626345518038675162); +void car_H_28(double *state, double *unused, double *out_1154547953993192886); +void car_h_31(double *state, double *unused, double *out_1477113707007182197); +void car_H_31(double *state, double *unused, double *out_3468424660852710823); 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 99dbcf1e5..27b5d3139 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_7527833659731555578) { - out_7527833659731555578[0] = delta_x[0] + nom_x[0]; - out_7527833659731555578[1] = delta_x[1] + nom_x[1]; - out_7527833659731555578[2] = delta_x[2] + nom_x[2]; - out_7527833659731555578[3] = delta_x[3] + nom_x[3]; - out_7527833659731555578[4] = delta_x[4] + nom_x[4]; - out_7527833659731555578[5] = delta_x[5] + nom_x[5]; - out_7527833659731555578[6] = delta_x[6] + nom_x[6]; - out_7527833659731555578[7] = delta_x[7] + nom_x[7]; - out_7527833659731555578[8] = delta_x[8] + nom_x[8]; - out_7527833659731555578[9] = delta_x[9] + nom_x[9]; - out_7527833659731555578[10] = delta_x[10] + nom_x[10]; - out_7527833659731555578[11] = delta_x[11] + nom_x[11]; - out_7527833659731555578[12] = delta_x[12] + nom_x[12]; - out_7527833659731555578[13] = delta_x[13] + nom_x[13]; - out_7527833659731555578[14] = delta_x[14] + nom_x[14]; - out_7527833659731555578[15] = delta_x[15] + nom_x[15]; - out_7527833659731555578[16] = delta_x[16] + nom_x[16]; - out_7527833659731555578[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_2889016141520795461) { + out_2889016141520795461[0] = delta_x[0] + nom_x[0]; + out_2889016141520795461[1] = delta_x[1] + nom_x[1]; + out_2889016141520795461[2] = delta_x[2] + nom_x[2]; + out_2889016141520795461[3] = delta_x[3] + nom_x[3]; + out_2889016141520795461[4] = delta_x[4] + nom_x[4]; + out_2889016141520795461[5] = delta_x[5] + nom_x[5]; + out_2889016141520795461[6] = delta_x[6] + nom_x[6]; + out_2889016141520795461[7] = delta_x[7] + nom_x[7]; + out_2889016141520795461[8] = delta_x[8] + nom_x[8]; + out_2889016141520795461[9] = delta_x[9] + nom_x[9]; + out_2889016141520795461[10] = delta_x[10] + nom_x[10]; + out_2889016141520795461[11] = delta_x[11] + nom_x[11]; + out_2889016141520795461[12] = delta_x[12] + nom_x[12]; + out_2889016141520795461[13] = delta_x[13] + nom_x[13]; + out_2889016141520795461[14] = delta_x[14] + nom_x[14]; + out_2889016141520795461[15] = delta_x[15] + nom_x[15]; + out_2889016141520795461[16] = delta_x[16] + nom_x[16]; + out_2889016141520795461[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8121371430920178446) { - out_8121371430920178446[0] = -nom_x[0] + true_x[0]; - out_8121371430920178446[1] = -nom_x[1] + true_x[1]; - out_8121371430920178446[2] = -nom_x[2] + true_x[2]; - out_8121371430920178446[3] = -nom_x[3] + true_x[3]; - out_8121371430920178446[4] = -nom_x[4] + true_x[4]; - out_8121371430920178446[5] = -nom_x[5] + true_x[5]; - out_8121371430920178446[6] = -nom_x[6] + true_x[6]; - out_8121371430920178446[7] = -nom_x[7] + true_x[7]; - out_8121371430920178446[8] = -nom_x[8] + true_x[8]; - out_8121371430920178446[9] = -nom_x[9] + true_x[9]; - out_8121371430920178446[10] = -nom_x[10] + true_x[10]; - out_8121371430920178446[11] = -nom_x[11] + true_x[11]; - out_8121371430920178446[12] = -nom_x[12] + true_x[12]; - out_8121371430920178446[13] = -nom_x[13] + true_x[13]; - out_8121371430920178446[14] = -nom_x[14] + true_x[14]; - out_8121371430920178446[15] = -nom_x[15] + true_x[15]; - out_8121371430920178446[16] = -nom_x[16] + true_x[16]; - out_8121371430920178446[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6586333085018065986) { + out_6586333085018065986[0] = -nom_x[0] + true_x[0]; + out_6586333085018065986[1] = -nom_x[1] + true_x[1]; + out_6586333085018065986[2] = -nom_x[2] + true_x[2]; + out_6586333085018065986[3] = -nom_x[3] + true_x[3]; + out_6586333085018065986[4] = -nom_x[4] + true_x[4]; + out_6586333085018065986[5] = -nom_x[5] + true_x[5]; + out_6586333085018065986[6] = -nom_x[6] + true_x[6]; + out_6586333085018065986[7] = -nom_x[7] + true_x[7]; + out_6586333085018065986[8] = -nom_x[8] + true_x[8]; + out_6586333085018065986[9] = -nom_x[9] + true_x[9]; + out_6586333085018065986[10] = -nom_x[10] + true_x[10]; + out_6586333085018065986[11] = -nom_x[11] + true_x[11]; + out_6586333085018065986[12] = -nom_x[12] + true_x[12]; + out_6586333085018065986[13] = -nom_x[13] + true_x[13]; + out_6586333085018065986[14] = -nom_x[14] + true_x[14]; + out_6586333085018065986[15] = -nom_x[15] + true_x[15]; + out_6586333085018065986[16] = -nom_x[16] + true_x[16]; + out_6586333085018065986[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_2787357462559363540) { - out_2787357462559363540[0] = 1.0; - out_2787357462559363540[1] = 0.0; - out_2787357462559363540[2] = 0.0; - out_2787357462559363540[3] = 0.0; - out_2787357462559363540[4] = 0.0; - out_2787357462559363540[5] = 0.0; - out_2787357462559363540[6] = 0.0; - out_2787357462559363540[7] = 0.0; - out_2787357462559363540[8] = 0.0; - out_2787357462559363540[9] = 0.0; - out_2787357462559363540[10] = 0.0; - out_2787357462559363540[11] = 0.0; - out_2787357462559363540[12] = 0.0; - out_2787357462559363540[13] = 0.0; - out_2787357462559363540[14] = 0.0; - out_2787357462559363540[15] = 0.0; - out_2787357462559363540[16] = 0.0; - out_2787357462559363540[17] = 0.0; - out_2787357462559363540[18] = 0.0; - out_2787357462559363540[19] = 1.0; - out_2787357462559363540[20] = 0.0; - out_2787357462559363540[21] = 0.0; - out_2787357462559363540[22] = 0.0; - out_2787357462559363540[23] = 0.0; - out_2787357462559363540[24] = 0.0; - out_2787357462559363540[25] = 0.0; - out_2787357462559363540[26] = 0.0; - out_2787357462559363540[27] = 0.0; - out_2787357462559363540[28] = 0.0; - out_2787357462559363540[29] = 0.0; - out_2787357462559363540[30] = 0.0; - out_2787357462559363540[31] = 0.0; - out_2787357462559363540[32] = 0.0; - out_2787357462559363540[33] = 0.0; - out_2787357462559363540[34] = 0.0; - out_2787357462559363540[35] = 0.0; - out_2787357462559363540[36] = 0.0; - out_2787357462559363540[37] = 0.0; - out_2787357462559363540[38] = 1.0; - out_2787357462559363540[39] = 0.0; - out_2787357462559363540[40] = 0.0; - out_2787357462559363540[41] = 0.0; - out_2787357462559363540[42] = 0.0; - out_2787357462559363540[43] = 0.0; - out_2787357462559363540[44] = 0.0; - out_2787357462559363540[45] = 0.0; - out_2787357462559363540[46] = 0.0; - out_2787357462559363540[47] = 0.0; - out_2787357462559363540[48] = 0.0; - out_2787357462559363540[49] = 0.0; - out_2787357462559363540[50] = 0.0; - out_2787357462559363540[51] = 0.0; - out_2787357462559363540[52] = 0.0; - out_2787357462559363540[53] = 0.0; - out_2787357462559363540[54] = 0.0; - out_2787357462559363540[55] = 0.0; - out_2787357462559363540[56] = 0.0; - out_2787357462559363540[57] = 1.0; - out_2787357462559363540[58] = 0.0; - out_2787357462559363540[59] = 0.0; - out_2787357462559363540[60] = 0.0; - out_2787357462559363540[61] = 0.0; - out_2787357462559363540[62] = 0.0; - out_2787357462559363540[63] = 0.0; - out_2787357462559363540[64] = 0.0; - out_2787357462559363540[65] = 0.0; - out_2787357462559363540[66] = 0.0; - out_2787357462559363540[67] = 0.0; - out_2787357462559363540[68] = 0.0; - out_2787357462559363540[69] = 0.0; - out_2787357462559363540[70] = 0.0; - out_2787357462559363540[71] = 0.0; - out_2787357462559363540[72] = 0.0; - out_2787357462559363540[73] = 0.0; - out_2787357462559363540[74] = 0.0; - out_2787357462559363540[75] = 0.0; - out_2787357462559363540[76] = 1.0; - out_2787357462559363540[77] = 0.0; - out_2787357462559363540[78] = 0.0; - out_2787357462559363540[79] = 0.0; - out_2787357462559363540[80] = 0.0; - out_2787357462559363540[81] = 0.0; - out_2787357462559363540[82] = 0.0; - out_2787357462559363540[83] = 0.0; - out_2787357462559363540[84] = 0.0; - out_2787357462559363540[85] = 0.0; - out_2787357462559363540[86] = 0.0; - out_2787357462559363540[87] = 0.0; - out_2787357462559363540[88] = 0.0; - out_2787357462559363540[89] = 0.0; - out_2787357462559363540[90] = 0.0; - out_2787357462559363540[91] = 0.0; - out_2787357462559363540[92] = 0.0; - out_2787357462559363540[93] = 0.0; - out_2787357462559363540[94] = 0.0; - out_2787357462559363540[95] = 1.0; - out_2787357462559363540[96] = 0.0; - out_2787357462559363540[97] = 0.0; - out_2787357462559363540[98] = 0.0; - out_2787357462559363540[99] = 0.0; - out_2787357462559363540[100] = 0.0; - out_2787357462559363540[101] = 0.0; - out_2787357462559363540[102] = 0.0; - out_2787357462559363540[103] = 0.0; - out_2787357462559363540[104] = 0.0; - out_2787357462559363540[105] = 0.0; - out_2787357462559363540[106] = 0.0; - out_2787357462559363540[107] = 0.0; - out_2787357462559363540[108] = 0.0; - out_2787357462559363540[109] = 0.0; - out_2787357462559363540[110] = 0.0; - out_2787357462559363540[111] = 0.0; - out_2787357462559363540[112] = 0.0; - out_2787357462559363540[113] = 0.0; - out_2787357462559363540[114] = 1.0; - out_2787357462559363540[115] = 0.0; - out_2787357462559363540[116] = 0.0; - out_2787357462559363540[117] = 0.0; - out_2787357462559363540[118] = 0.0; - out_2787357462559363540[119] = 0.0; - out_2787357462559363540[120] = 0.0; - out_2787357462559363540[121] = 0.0; - out_2787357462559363540[122] = 0.0; - out_2787357462559363540[123] = 0.0; - out_2787357462559363540[124] = 0.0; - out_2787357462559363540[125] = 0.0; - out_2787357462559363540[126] = 0.0; - out_2787357462559363540[127] = 0.0; - out_2787357462559363540[128] = 0.0; - out_2787357462559363540[129] = 0.0; - out_2787357462559363540[130] = 0.0; - out_2787357462559363540[131] = 0.0; - out_2787357462559363540[132] = 0.0; - out_2787357462559363540[133] = 1.0; - out_2787357462559363540[134] = 0.0; - out_2787357462559363540[135] = 0.0; - out_2787357462559363540[136] = 0.0; - out_2787357462559363540[137] = 0.0; - out_2787357462559363540[138] = 0.0; - out_2787357462559363540[139] = 0.0; - out_2787357462559363540[140] = 0.0; - out_2787357462559363540[141] = 0.0; - out_2787357462559363540[142] = 0.0; - out_2787357462559363540[143] = 0.0; - out_2787357462559363540[144] = 0.0; - out_2787357462559363540[145] = 0.0; - out_2787357462559363540[146] = 0.0; - out_2787357462559363540[147] = 0.0; - out_2787357462559363540[148] = 0.0; - out_2787357462559363540[149] = 0.0; - out_2787357462559363540[150] = 0.0; - out_2787357462559363540[151] = 0.0; - out_2787357462559363540[152] = 1.0; - out_2787357462559363540[153] = 0.0; - out_2787357462559363540[154] = 0.0; - out_2787357462559363540[155] = 0.0; - out_2787357462559363540[156] = 0.0; - out_2787357462559363540[157] = 0.0; - out_2787357462559363540[158] = 0.0; - out_2787357462559363540[159] = 0.0; - out_2787357462559363540[160] = 0.0; - out_2787357462559363540[161] = 0.0; - out_2787357462559363540[162] = 0.0; - out_2787357462559363540[163] = 0.0; - out_2787357462559363540[164] = 0.0; - out_2787357462559363540[165] = 0.0; - out_2787357462559363540[166] = 0.0; - out_2787357462559363540[167] = 0.0; - out_2787357462559363540[168] = 0.0; - out_2787357462559363540[169] = 0.0; - out_2787357462559363540[170] = 0.0; - out_2787357462559363540[171] = 1.0; - out_2787357462559363540[172] = 0.0; - out_2787357462559363540[173] = 0.0; - out_2787357462559363540[174] = 0.0; - out_2787357462559363540[175] = 0.0; - out_2787357462559363540[176] = 0.0; - out_2787357462559363540[177] = 0.0; - out_2787357462559363540[178] = 0.0; - out_2787357462559363540[179] = 0.0; - out_2787357462559363540[180] = 0.0; - out_2787357462559363540[181] = 0.0; - out_2787357462559363540[182] = 0.0; - out_2787357462559363540[183] = 0.0; - out_2787357462559363540[184] = 0.0; - out_2787357462559363540[185] = 0.0; - out_2787357462559363540[186] = 0.0; - out_2787357462559363540[187] = 0.0; - out_2787357462559363540[188] = 0.0; - out_2787357462559363540[189] = 0.0; - out_2787357462559363540[190] = 1.0; - out_2787357462559363540[191] = 0.0; - out_2787357462559363540[192] = 0.0; - out_2787357462559363540[193] = 0.0; - out_2787357462559363540[194] = 0.0; - out_2787357462559363540[195] = 0.0; - out_2787357462559363540[196] = 0.0; - out_2787357462559363540[197] = 0.0; - out_2787357462559363540[198] = 0.0; - out_2787357462559363540[199] = 0.0; - out_2787357462559363540[200] = 0.0; - out_2787357462559363540[201] = 0.0; - out_2787357462559363540[202] = 0.0; - out_2787357462559363540[203] = 0.0; - out_2787357462559363540[204] = 0.0; - out_2787357462559363540[205] = 0.0; - out_2787357462559363540[206] = 0.0; - out_2787357462559363540[207] = 0.0; - out_2787357462559363540[208] = 0.0; - out_2787357462559363540[209] = 1.0; - out_2787357462559363540[210] = 0.0; - out_2787357462559363540[211] = 0.0; - out_2787357462559363540[212] = 0.0; - out_2787357462559363540[213] = 0.0; - out_2787357462559363540[214] = 0.0; - out_2787357462559363540[215] = 0.0; - out_2787357462559363540[216] = 0.0; - out_2787357462559363540[217] = 0.0; - out_2787357462559363540[218] = 0.0; - out_2787357462559363540[219] = 0.0; - out_2787357462559363540[220] = 0.0; - out_2787357462559363540[221] = 0.0; - out_2787357462559363540[222] = 0.0; - out_2787357462559363540[223] = 0.0; - out_2787357462559363540[224] = 0.0; - out_2787357462559363540[225] = 0.0; - out_2787357462559363540[226] = 0.0; - out_2787357462559363540[227] = 0.0; - out_2787357462559363540[228] = 1.0; - out_2787357462559363540[229] = 0.0; - out_2787357462559363540[230] = 0.0; - out_2787357462559363540[231] = 0.0; - out_2787357462559363540[232] = 0.0; - out_2787357462559363540[233] = 0.0; - out_2787357462559363540[234] = 0.0; - out_2787357462559363540[235] = 0.0; - out_2787357462559363540[236] = 0.0; - out_2787357462559363540[237] = 0.0; - out_2787357462559363540[238] = 0.0; - out_2787357462559363540[239] = 0.0; - out_2787357462559363540[240] = 0.0; - out_2787357462559363540[241] = 0.0; - out_2787357462559363540[242] = 0.0; - out_2787357462559363540[243] = 0.0; - out_2787357462559363540[244] = 0.0; - out_2787357462559363540[245] = 0.0; - out_2787357462559363540[246] = 0.0; - out_2787357462559363540[247] = 1.0; - out_2787357462559363540[248] = 0.0; - out_2787357462559363540[249] = 0.0; - out_2787357462559363540[250] = 0.0; - out_2787357462559363540[251] = 0.0; - out_2787357462559363540[252] = 0.0; - out_2787357462559363540[253] = 0.0; - out_2787357462559363540[254] = 0.0; - out_2787357462559363540[255] = 0.0; - out_2787357462559363540[256] = 0.0; - out_2787357462559363540[257] = 0.0; - out_2787357462559363540[258] = 0.0; - out_2787357462559363540[259] = 0.0; - out_2787357462559363540[260] = 0.0; - out_2787357462559363540[261] = 0.0; - out_2787357462559363540[262] = 0.0; - out_2787357462559363540[263] = 0.0; - out_2787357462559363540[264] = 0.0; - out_2787357462559363540[265] = 0.0; - out_2787357462559363540[266] = 1.0; - out_2787357462559363540[267] = 0.0; - out_2787357462559363540[268] = 0.0; - out_2787357462559363540[269] = 0.0; - out_2787357462559363540[270] = 0.0; - out_2787357462559363540[271] = 0.0; - out_2787357462559363540[272] = 0.0; - out_2787357462559363540[273] = 0.0; - out_2787357462559363540[274] = 0.0; - out_2787357462559363540[275] = 0.0; - out_2787357462559363540[276] = 0.0; - out_2787357462559363540[277] = 0.0; - out_2787357462559363540[278] = 0.0; - out_2787357462559363540[279] = 0.0; - out_2787357462559363540[280] = 0.0; - out_2787357462559363540[281] = 0.0; - out_2787357462559363540[282] = 0.0; - out_2787357462559363540[283] = 0.0; - out_2787357462559363540[284] = 0.0; - out_2787357462559363540[285] = 1.0; - out_2787357462559363540[286] = 0.0; - out_2787357462559363540[287] = 0.0; - out_2787357462559363540[288] = 0.0; - out_2787357462559363540[289] = 0.0; - out_2787357462559363540[290] = 0.0; - out_2787357462559363540[291] = 0.0; - out_2787357462559363540[292] = 0.0; - out_2787357462559363540[293] = 0.0; - out_2787357462559363540[294] = 0.0; - out_2787357462559363540[295] = 0.0; - out_2787357462559363540[296] = 0.0; - out_2787357462559363540[297] = 0.0; - out_2787357462559363540[298] = 0.0; - out_2787357462559363540[299] = 0.0; - out_2787357462559363540[300] = 0.0; - out_2787357462559363540[301] = 0.0; - out_2787357462559363540[302] = 0.0; - out_2787357462559363540[303] = 0.0; - out_2787357462559363540[304] = 1.0; - out_2787357462559363540[305] = 0.0; - out_2787357462559363540[306] = 0.0; - out_2787357462559363540[307] = 0.0; - out_2787357462559363540[308] = 0.0; - out_2787357462559363540[309] = 0.0; - out_2787357462559363540[310] = 0.0; - out_2787357462559363540[311] = 0.0; - out_2787357462559363540[312] = 0.0; - out_2787357462559363540[313] = 0.0; - out_2787357462559363540[314] = 0.0; - out_2787357462559363540[315] = 0.0; - out_2787357462559363540[316] = 0.0; - out_2787357462559363540[317] = 0.0; - out_2787357462559363540[318] = 0.0; - out_2787357462559363540[319] = 0.0; - out_2787357462559363540[320] = 0.0; - out_2787357462559363540[321] = 0.0; - out_2787357462559363540[322] = 0.0; - out_2787357462559363540[323] = 1.0; +void H_mod_fun(double *state, double *out_6984305438091048706) { + out_6984305438091048706[0] = 1.0; + out_6984305438091048706[1] = 0.0; + out_6984305438091048706[2] = 0.0; + out_6984305438091048706[3] = 0.0; + out_6984305438091048706[4] = 0.0; + out_6984305438091048706[5] = 0.0; + out_6984305438091048706[6] = 0.0; + out_6984305438091048706[7] = 0.0; + out_6984305438091048706[8] = 0.0; + out_6984305438091048706[9] = 0.0; + out_6984305438091048706[10] = 0.0; + out_6984305438091048706[11] = 0.0; + out_6984305438091048706[12] = 0.0; + out_6984305438091048706[13] = 0.0; + out_6984305438091048706[14] = 0.0; + out_6984305438091048706[15] = 0.0; + out_6984305438091048706[16] = 0.0; + out_6984305438091048706[17] = 0.0; + out_6984305438091048706[18] = 0.0; + out_6984305438091048706[19] = 1.0; + out_6984305438091048706[20] = 0.0; + out_6984305438091048706[21] = 0.0; + out_6984305438091048706[22] = 0.0; + out_6984305438091048706[23] = 0.0; + out_6984305438091048706[24] = 0.0; + out_6984305438091048706[25] = 0.0; + out_6984305438091048706[26] = 0.0; + out_6984305438091048706[27] = 0.0; + out_6984305438091048706[28] = 0.0; + out_6984305438091048706[29] = 0.0; + out_6984305438091048706[30] = 0.0; + out_6984305438091048706[31] = 0.0; + out_6984305438091048706[32] = 0.0; + out_6984305438091048706[33] = 0.0; + out_6984305438091048706[34] = 0.0; + out_6984305438091048706[35] = 0.0; + out_6984305438091048706[36] = 0.0; + out_6984305438091048706[37] = 0.0; + out_6984305438091048706[38] = 1.0; + out_6984305438091048706[39] = 0.0; + out_6984305438091048706[40] = 0.0; + out_6984305438091048706[41] = 0.0; + out_6984305438091048706[42] = 0.0; + out_6984305438091048706[43] = 0.0; + out_6984305438091048706[44] = 0.0; + out_6984305438091048706[45] = 0.0; + out_6984305438091048706[46] = 0.0; + out_6984305438091048706[47] = 0.0; + out_6984305438091048706[48] = 0.0; + out_6984305438091048706[49] = 0.0; + out_6984305438091048706[50] = 0.0; + out_6984305438091048706[51] = 0.0; + out_6984305438091048706[52] = 0.0; + out_6984305438091048706[53] = 0.0; + out_6984305438091048706[54] = 0.0; + out_6984305438091048706[55] = 0.0; + out_6984305438091048706[56] = 0.0; + out_6984305438091048706[57] = 1.0; + out_6984305438091048706[58] = 0.0; + out_6984305438091048706[59] = 0.0; + out_6984305438091048706[60] = 0.0; + out_6984305438091048706[61] = 0.0; + out_6984305438091048706[62] = 0.0; + out_6984305438091048706[63] = 0.0; + out_6984305438091048706[64] = 0.0; + out_6984305438091048706[65] = 0.0; + out_6984305438091048706[66] = 0.0; + out_6984305438091048706[67] = 0.0; + out_6984305438091048706[68] = 0.0; + out_6984305438091048706[69] = 0.0; + out_6984305438091048706[70] = 0.0; + out_6984305438091048706[71] = 0.0; + out_6984305438091048706[72] = 0.0; + out_6984305438091048706[73] = 0.0; + out_6984305438091048706[74] = 0.0; + out_6984305438091048706[75] = 0.0; + out_6984305438091048706[76] = 1.0; + out_6984305438091048706[77] = 0.0; + out_6984305438091048706[78] = 0.0; + out_6984305438091048706[79] = 0.0; + out_6984305438091048706[80] = 0.0; + out_6984305438091048706[81] = 0.0; + out_6984305438091048706[82] = 0.0; + out_6984305438091048706[83] = 0.0; + out_6984305438091048706[84] = 0.0; + out_6984305438091048706[85] = 0.0; + out_6984305438091048706[86] = 0.0; + out_6984305438091048706[87] = 0.0; + out_6984305438091048706[88] = 0.0; + out_6984305438091048706[89] = 0.0; + out_6984305438091048706[90] = 0.0; + out_6984305438091048706[91] = 0.0; + out_6984305438091048706[92] = 0.0; + out_6984305438091048706[93] = 0.0; + out_6984305438091048706[94] = 0.0; + out_6984305438091048706[95] = 1.0; + out_6984305438091048706[96] = 0.0; + out_6984305438091048706[97] = 0.0; + out_6984305438091048706[98] = 0.0; + out_6984305438091048706[99] = 0.0; + out_6984305438091048706[100] = 0.0; + out_6984305438091048706[101] = 0.0; + out_6984305438091048706[102] = 0.0; + out_6984305438091048706[103] = 0.0; + out_6984305438091048706[104] = 0.0; + out_6984305438091048706[105] = 0.0; + out_6984305438091048706[106] = 0.0; + out_6984305438091048706[107] = 0.0; + out_6984305438091048706[108] = 0.0; + out_6984305438091048706[109] = 0.0; + out_6984305438091048706[110] = 0.0; + out_6984305438091048706[111] = 0.0; + out_6984305438091048706[112] = 0.0; + out_6984305438091048706[113] = 0.0; + out_6984305438091048706[114] = 1.0; + out_6984305438091048706[115] = 0.0; + out_6984305438091048706[116] = 0.0; + out_6984305438091048706[117] = 0.0; + out_6984305438091048706[118] = 0.0; + out_6984305438091048706[119] = 0.0; + out_6984305438091048706[120] = 0.0; + out_6984305438091048706[121] = 0.0; + out_6984305438091048706[122] = 0.0; + out_6984305438091048706[123] = 0.0; + out_6984305438091048706[124] = 0.0; + out_6984305438091048706[125] = 0.0; + out_6984305438091048706[126] = 0.0; + out_6984305438091048706[127] = 0.0; + out_6984305438091048706[128] = 0.0; + out_6984305438091048706[129] = 0.0; + out_6984305438091048706[130] = 0.0; + out_6984305438091048706[131] = 0.0; + out_6984305438091048706[132] = 0.0; + out_6984305438091048706[133] = 1.0; + out_6984305438091048706[134] = 0.0; + out_6984305438091048706[135] = 0.0; + out_6984305438091048706[136] = 0.0; + out_6984305438091048706[137] = 0.0; + out_6984305438091048706[138] = 0.0; + out_6984305438091048706[139] = 0.0; + out_6984305438091048706[140] = 0.0; + out_6984305438091048706[141] = 0.0; + out_6984305438091048706[142] = 0.0; + out_6984305438091048706[143] = 0.0; + out_6984305438091048706[144] = 0.0; + out_6984305438091048706[145] = 0.0; + out_6984305438091048706[146] = 0.0; + out_6984305438091048706[147] = 0.0; + out_6984305438091048706[148] = 0.0; + out_6984305438091048706[149] = 0.0; + out_6984305438091048706[150] = 0.0; + out_6984305438091048706[151] = 0.0; + out_6984305438091048706[152] = 1.0; + out_6984305438091048706[153] = 0.0; + out_6984305438091048706[154] = 0.0; + out_6984305438091048706[155] = 0.0; + out_6984305438091048706[156] = 0.0; + out_6984305438091048706[157] = 0.0; + out_6984305438091048706[158] = 0.0; + out_6984305438091048706[159] = 0.0; + out_6984305438091048706[160] = 0.0; + out_6984305438091048706[161] = 0.0; + out_6984305438091048706[162] = 0.0; + out_6984305438091048706[163] = 0.0; + out_6984305438091048706[164] = 0.0; + out_6984305438091048706[165] = 0.0; + out_6984305438091048706[166] = 0.0; + out_6984305438091048706[167] = 0.0; + out_6984305438091048706[168] = 0.0; + out_6984305438091048706[169] = 0.0; + out_6984305438091048706[170] = 0.0; + out_6984305438091048706[171] = 1.0; + out_6984305438091048706[172] = 0.0; + out_6984305438091048706[173] = 0.0; + out_6984305438091048706[174] = 0.0; + out_6984305438091048706[175] = 0.0; + out_6984305438091048706[176] = 0.0; + out_6984305438091048706[177] = 0.0; + out_6984305438091048706[178] = 0.0; + out_6984305438091048706[179] = 0.0; + out_6984305438091048706[180] = 0.0; + out_6984305438091048706[181] = 0.0; + out_6984305438091048706[182] = 0.0; + out_6984305438091048706[183] = 0.0; + out_6984305438091048706[184] = 0.0; + out_6984305438091048706[185] = 0.0; + out_6984305438091048706[186] = 0.0; + out_6984305438091048706[187] = 0.0; + out_6984305438091048706[188] = 0.0; + out_6984305438091048706[189] = 0.0; + out_6984305438091048706[190] = 1.0; + out_6984305438091048706[191] = 0.0; + out_6984305438091048706[192] = 0.0; + out_6984305438091048706[193] = 0.0; + out_6984305438091048706[194] = 0.0; + out_6984305438091048706[195] = 0.0; + out_6984305438091048706[196] = 0.0; + out_6984305438091048706[197] = 0.0; + out_6984305438091048706[198] = 0.0; + out_6984305438091048706[199] = 0.0; + out_6984305438091048706[200] = 0.0; + out_6984305438091048706[201] = 0.0; + out_6984305438091048706[202] = 0.0; + out_6984305438091048706[203] = 0.0; + out_6984305438091048706[204] = 0.0; + out_6984305438091048706[205] = 0.0; + out_6984305438091048706[206] = 0.0; + out_6984305438091048706[207] = 0.0; + out_6984305438091048706[208] = 0.0; + out_6984305438091048706[209] = 1.0; + out_6984305438091048706[210] = 0.0; + out_6984305438091048706[211] = 0.0; + out_6984305438091048706[212] = 0.0; + out_6984305438091048706[213] = 0.0; + out_6984305438091048706[214] = 0.0; + out_6984305438091048706[215] = 0.0; + out_6984305438091048706[216] = 0.0; + out_6984305438091048706[217] = 0.0; + out_6984305438091048706[218] = 0.0; + out_6984305438091048706[219] = 0.0; + out_6984305438091048706[220] = 0.0; + out_6984305438091048706[221] = 0.0; + out_6984305438091048706[222] = 0.0; + out_6984305438091048706[223] = 0.0; + out_6984305438091048706[224] = 0.0; + out_6984305438091048706[225] = 0.0; + out_6984305438091048706[226] = 0.0; + out_6984305438091048706[227] = 0.0; + out_6984305438091048706[228] = 1.0; + out_6984305438091048706[229] = 0.0; + out_6984305438091048706[230] = 0.0; + out_6984305438091048706[231] = 0.0; + out_6984305438091048706[232] = 0.0; + out_6984305438091048706[233] = 0.0; + out_6984305438091048706[234] = 0.0; + out_6984305438091048706[235] = 0.0; + out_6984305438091048706[236] = 0.0; + out_6984305438091048706[237] = 0.0; + out_6984305438091048706[238] = 0.0; + out_6984305438091048706[239] = 0.0; + out_6984305438091048706[240] = 0.0; + out_6984305438091048706[241] = 0.0; + out_6984305438091048706[242] = 0.0; + out_6984305438091048706[243] = 0.0; + out_6984305438091048706[244] = 0.0; + out_6984305438091048706[245] = 0.0; + out_6984305438091048706[246] = 0.0; + out_6984305438091048706[247] = 1.0; + out_6984305438091048706[248] = 0.0; + out_6984305438091048706[249] = 0.0; + out_6984305438091048706[250] = 0.0; + out_6984305438091048706[251] = 0.0; + out_6984305438091048706[252] = 0.0; + out_6984305438091048706[253] = 0.0; + out_6984305438091048706[254] = 0.0; + out_6984305438091048706[255] = 0.0; + out_6984305438091048706[256] = 0.0; + out_6984305438091048706[257] = 0.0; + out_6984305438091048706[258] = 0.0; + out_6984305438091048706[259] = 0.0; + out_6984305438091048706[260] = 0.0; + out_6984305438091048706[261] = 0.0; + out_6984305438091048706[262] = 0.0; + out_6984305438091048706[263] = 0.0; + out_6984305438091048706[264] = 0.0; + out_6984305438091048706[265] = 0.0; + out_6984305438091048706[266] = 1.0; + out_6984305438091048706[267] = 0.0; + out_6984305438091048706[268] = 0.0; + out_6984305438091048706[269] = 0.0; + out_6984305438091048706[270] = 0.0; + out_6984305438091048706[271] = 0.0; + out_6984305438091048706[272] = 0.0; + out_6984305438091048706[273] = 0.0; + out_6984305438091048706[274] = 0.0; + out_6984305438091048706[275] = 0.0; + out_6984305438091048706[276] = 0.0; + out_6984305438091048706[277] = 0.0; + out_6984305438091048706[278] = 0.0; + out_6984305438091048706[279] = 0.0; + out_6984305438091048706[280] = 0.0; + out_6984305438091048706[281] = 0.0; + out_6984305438091048706[282] = 0.0; + out_6984305438091048706[283] = 0.0; + out_6984305438091048706[284] = 0.0; + out_6984305438091048706[285] = 1.0; + out_6984305438091048706[286] = 0.0; + out_6984305438091048706[287] = 0.0; + out_6984305438091048706[288] = 0.0; + out_6984305438091048706[289] = 0.0; + out_6984305438091048706[290] = 0.0; + out_6984305438091048706[291] = 0.0; + out_6984305438091048706[292] = 0.0; + out_6984305438091048706[293] = 0.0; + out_6984305438091048706[294] = 0.0; + out_6984305438091048706[295] = 0.0; + out_6984305438091048706[296] = 0.0; + out_6984305438091048706[297] = 0.0; + out_6984305438091048706[298] = 0.0; + out_6984305438091048706[299] = 0.0; + out_6984305438091048706[300] = 0.0; + out_6984305438091048706[301] = 0.0; + out_6984305438091048706[302] = 0.0; + out_6984305438091048706[303] = 0.0; + out_6984305438091048706[304] = 1.0; + out_6984305438091048706[305] = 0.0; + out_6984305438091048706[306] = 0.0; + out_6984305438091048706[307] = 0.0; + out_6984305438091048706[308] = 0.0; + out_6984305438091048706[309] = 0.0; + out_6984305438091048706[310] = 0.0; + out_6984305438091048706[311] = 0.0; + out_6984305438091048706[312] = 0.0; + out_6984305438091048706[313] = 0.0; + out_6984305438091048706[314] = 0.0; + out_6984305438091048706[315] = 0.0; + out_6984305438091048706[316] = 0.0; + out_6984305438091048706[317] = 0.0; + out_6984305438091048706[318] = 0.0; + out_6984305438091048706[319] = 0.0; + out_6984305438091048706[320] = 0.0; + out_6984305438091048706[321] = 0.0; + out_6984305438091048706[322] = 0.0; + out_6984305438091048706[323] = 1.0; } -void f_fun(double *state, double dt, double *out_2997254779517652533) { - out_2997254779517652533[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_2997254779517652533[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_2997254779517652533[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_2997254779517652533[3] = dt*state[12] + state[3]; - out_2997254779517652533[4] = dt*state[13] + state[4]; - out_2997254779517652533[5] = dt*state[14] + state[5]; - out_2997254779517652533[6] = state[6]; - out_2997254779517652533[7] = state[7]; - out_2997254779517652533[8] = state[8]; - out_2997254779517652533[9] = state[9]; - out_2997254779517652533[10] = state[10]; - out_2997254779517652533[11] = state[11]; - out_2997254779517652533[12] = state[12]; - out_2997254779517652533[13] = state[13]; - out_2997254779517652533[14] = state[14]; - out_2997254779517652533[15] = state[15]; - out_2997254779517652533[16] = state[16]; - out_2997254779517652533[17] = state[17]; +void f_fun(double *state, double dt, double *out_4187862153732757636) { + out_4187862153732757636[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_4187862153732757636[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_4187862153732757636[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_4187862153732757636[3] = dt*state[12] + state[3]; + out_4187862153732757636[4] = dt*state[13] + state[4]; + out_4187862153732757636[5] = dt*state[14] + state[5]; + out_4187862153732757636[6] = state[6]; + out_4187862153732757636[7] = state[7]; + out_4187862153732757636[8] = state[8]; + out_4187862153732757636[9] = state[9]; + out_4187862153732757636[10] = state[10]; + out_4187862153732757636[11] = state[11]; + out_4187862153732757636[12] = state[12]; + out_4187862153732757636[13] = state[13]; + out_4187862153732757636[14] = state[14]; + out_4187862153732757636[15] = state[15]; + out_4187862153732757636[16] = state[16]; + out_4187862153732757636[17] = state[17]; } -void F_fun(double *state, double dt, double *out_3476500674504442735) { - out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[2] = 0; - out_3476500674504442735[3] = 0; - out_3476500674504442735[4] = 0; - out_3476500674504442735[5] = 0; - out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[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_3476500674504442735[9] = 0; - out_3476500674504442735[10] = 0; - out_3476500674504442735[11] = 0; - out_3476500674504442735[12] = 0; - out_3476500674504442735[13] = 0; - out_3476500674504442735[14] = 0; - out_3476500674504442735[15] = 0; - out_3476500674504442735[16] = 0; - out_3476500674504442735[17] = 0; - out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[20] = 0; - out_3476500674504442735[21] = 0; - out_3476500674504442735[22] = 0; - out_3476500674504442735[23] = 0; - out_3476500674504442735[24] = 0; - out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[27] = 0; - out_3476500674504442735[28] = 0; - out_3476500674504442735[29] = 0; - out_3476500674504442735[30] = 0; - out_3476500674504442735[31] = 0; - out_3476500674504442735[32] = 0; - out_3476500674504442735[33] = 0; - out_3476500674504442735[34] = 0; - out_3476500674504442735[35] = 0; - out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[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_3476500674504442735[39] = 0; - out_3476500674504442735[40] = 0; - out_3476500674504442735[41] = 0; - out_3476500674504442735[42] = 0; - out_3476500674504442735[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_3476500674504442735[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_3476500674504442735[45] = 0; - out_3476500674504442735[46] = 0; - out_3476500674504442735[47] = 0; - out_3476500674504442735[48] = 0; - out_3476500674504442735[49] = 0; - out_3476500674504442735[50] = 0; - out_3476500674504442735[51] = 0; - out_3476500674504442735[52] = 0; - out_3476500674504442735[53] = 0; - out_3476500674504442735[54] = 0; - out_3476500674504442735[55] = 0; - out_3476500674504442735[56] = 0; - out_3476500674504442735[57] = 1; - out_3476500674504442735[58] = 0; - out_3476500674504442735[59] = 0; - out_3476500674504442735[60] = 0; - out_3476500674504442735[61] = 0; - out_3476500674504442735[62] = 0; - out_3476500674504442735[63] = 0; - out_3476500674504442735[64] = 0; - out_3476500674504442735[65] = 0; - out_3476500674504442735[66] = dt; - out_3476500674504442735[67] = 0; - out_3476500674504442735[68] = 0; - out_3476500674504442735[69] = 0; - out_3476500674504442735[70] = 0; - out_3476500674504442735[71] = 0; - out_3476500674504442735[72] = 0; - out_3476500674504442735[73] = 0; - out_3476500674504442735[74] = 0; - out_3476500674504442735[75] = 0; - out_3476500674504442735[76] = 1; - out_3476500674504442735[77] = 0; - out_3476500674504442735[78] = 0; - out_3476500674504442735[79] = 0; - out_3476500674504442735[80] = 0; - out_3476500674504442735[81] = 0; - out_3476500674504442735[82] = 0; - out_3476500674504442735[83] = 0; - out_3476500674504442735[84] = 0; - out_3476500674504442735[85] = dt; - out_3476500674504442735[86] = 0; - out_3476500674504442735[87] = 0; - out_3476500674504442735[88] = 0; - out_3476500674504442735[89] = 0; - out_3476500674504442735[90] = 0; - out_3476500674504442735[91] = 0; - out_3476500674504442735[92] = 0; - out_3476500674504442735[93] = 0; - out_3476500674504442735[94] = 0; - out_3476500674504442735[95] = 1; - out_3476500674504442735[96] = 0; - out_3476500674504442735[97] = 0; - out_3476500674504442735[98] = 0; - out_3476500674504442735[99] = 0; - out_3476500674504442735[100] = 0; - out_3476500674504442735[101] = 0; - out_3476500674504442735[102] = 0; - out_3476500674504442735[103] = 0; - out_3476500674504442735[104] = dt; - out_3476500674504442735[105] = 0; - out_3476500674504442735[106] = 0; - out_3476500674504442735[107] = 0; - out_3476500674504442735[108] = 0; - out_3476500674504442735[109] = 0; - out_3476500674504442735[110] = 0; - out_3476500674504442735[111] = 0; - out_3476500674504442735[112] = 0; - out_3476500674504442735[113] = 0; - out_3476500674504442735[114] = 1; - out_3476500674504442735[115] = 0; - out_3476500674504442735[116] = 0; - out_3476500674504442735[117] = 0; - out_3476500674504442735[118] = 0; - out_3476500674504442735[119] = 0; - out_3476500674504442735[120] = 0; - out_3476500674504442735[121] = 0; - out_3476500674504442735[122] = 0; - out_3476500674504442735[123] = 0; - out_3476500674504442735[124] = 0; - out_3476500674504442735[125] = 0; - out_3476500674504442735[126] = 0; - out_3476500674504442735[127] = 0; - out_3476500674504442735[128] = 0; - out_3476500674504442735[129] = 0; - out_3476500674504442735[130] = 0; - out_3476500674504442735[131] = 0; - out_3476500674504442735[132] = 0; - out_3476500674504442735[133] = 1; - out_3476500674504442735[134] = 0; - out_3476500674504442735[135] = 0; - out_3476500674504442735[136] = 0; - out_3476500674504442735[137] = 0; - out_3476500674504442735[138] = 0; - out_3476500674504442735[139] = 0; - out_3476500674504442735[140] = 0; - out_3476500674504442735[141] = 0; - out_3476500674504442735[142] = 0; - out_3476500674504442735[143] = 0; - out_3476500674504442735[144] = 0; - out_3476500674504442735[145] = 0; - out_3476500674504442735[146] = 0; - out_3476500674504442735[147] = 0; - out_3476500674504442735[148] = 0; - out_3476500674504442735[149] = 0; - out_3476500674504442735[150] = 0; - out_3476500674504442735[151] = 0; - out_3476500674504442735[152] = 1; - out_3476500674504442735[153] = 0; - out_3476500674504442735[154] = 0; - out_3476500674504442735[155] = 0; - out_3476500674504442735[156] = 0; - out_3476500674504442735[157] = 0; - out_3476500674504442735[158] = 0; - out_3476500674504442735[159] = 0; - out_3476500674504442735[160] = 0; - out_3476500674504442735[161] = 0; - out_3476500674504442735[162] = 0; - out_3476500674504442735[163] = 0; - out_3476500674504442735[164] = 0; - out_3476500674504442735[165] = 0; - out_3476500674504442735[166] = 0; - out_3476500674504442735[167] = 0; - out_3476500674504442735[168] = 0; - out_3476500674504442735[169] = 0; - out_3476500674504442735[170] = 0; - out_3476500674504442735[171] = 1; - out_3476500674504442735[172] = 0; - out_3476500674504442735[173] = 0; - out_3476500674504442735[174] = 0; - out_3476500674504442735[175] = 0; - out_3476500674504442735[176] = 0; - out_3476500674504442735[177] = 0; - out_3476500674504442735[178] = 0; - out_3476500674504442735[179] = 0; - out_3476500674504442735[180] = 0; - out_3476500674504442735[181] = 0; - out_3476500674504442735[182] = 0; - out_3476500674504442735[183] = 0; - out_3476500674504442735[184] = 0; - out_3476500674504442735[185] = 0; - out_3476500674504442735[186] = 0; - out_3476500674504442735[187] = 0; - out_3476500674504442735[188] = 0; - out_3476500674504442735[189] = 0; - out_3476500674504442735[190] = 1; - out_3476500674504442735[191] = 0; - out_3476500674504442735[192] = 0; - out_3476500674504442735[193] = 0; - out_3476500674504442735[194] = 0; - out_3476500674504442735[195] = 0; - out_3476500674504442735[196] = 0; - out_3476500674504442735[197] = 0; - out_3476500674504442735[198] = 0; - out_3476500674504442735[199] = 0; - out_3476500674504442735[200] = 0; - out_3476500674504442735[201] = 0; - out_3476500674504442735[202] = 0; - out_3476500674504442735[203] = 0; - out_3476500674504442735[204] = 0; - out_3476500674504442735[205] = 0; - out_3476500674504442735[206] = 0; - out_3476500674504442735[207] = 0; - out_3476500674504442735[208] = 0; - out_3476500674504442735[209] = 1; - out_3476500674504442735[210] = 0; - out_3476500674504442735[211] = 0; - out_3476500674504442735[212] = 0; - out_3476500674504442735[213] = 0; - out_3476500674504442735[214] = 0; - out_3476500674504442735[215] = 0; - out_3476500674504442735[216] = 0; - out_3476500674504442735[217] = 0; - out_3476500674504442735[218] = 0; - out_3476500674504442735[219] = 0; - out_3476500674504442735[220] = 0; - out_3476500674504442735[221] = 0; - out_3476500674504442735[222] = 0; - out_3476500674504442735[223] = 0; - out_3476500674504442735[224] = 0; - out_3476500674504442735[225] = 0; - out_3476500674504442735[226] = 0; - out_3476500674504442735[227] = 0; - out_3476500674504442735[228] = 1; - out_3476500674504442735[229] = 0; - out_3476500674504442735[230] = 0; - out_3476500674504442735[231] = 0; - out_3476500674504442735[232] = 0; - out_3476500674504442735[233] = 0; - out_3476500674504442735[234] = 0; - out_3476500674504442735[235] = 0; - out_3476500674504442735[236] = 0; - out_3476500674504442735[237] = 0; - out_3476500674504442735[238] = 0; - out_3476500674504442735[239] = 0; - out_3476500674504442735[240] = 0; - out_3476500674504442735[241] = 0; - out_3476500674504442735[242] = 0; - out_3476500674504442735[243] = 0; - out_3476500674504442735[244] = 0; - out_3476500674504442735[245] = 0; - out_3476500674504442735[246] = 0; - out_3476500674504442735[247] = 1; - out_3476500674504442735[248] = 0; - out_3476500674504442735[249] = 0; - out_3476500674504442735[250] = 0; - out_3476500674504442735[251] = 0; - out_3476500674504442735[252] = 0; - out_3476500674504442735[253] = 0; - out_3476500674504442735[254] = 0; - out_3476500674504442735[255] = 0; - out_3476500674504442735[256] = 0; - out_3476500674504442735[257] = 0; - out_3476500674504442735[258] = 0; - out_3476500674504442735[259] = 0; - out_3476500674504442735[260] = 0; - out_3476500674504442735[261] = 0; - out_3476500674504442735[262] = 0; - out_3476500674504442735[263] = 0; - out_3476500674504442735[264] = 0; - out_3476500674504442735[265] = 0; - out_3476500674504442735[266] = 1; - out_3476500674504442735[267] = 0; - out_3476500674504442735[268] = 0; - out_3476500674504442735[269] = 0; - out_3476500674504442735[270] = 0; - out_3476500674504442735[271] = 0; - out_3476500674504442735[272] = 0; - out_3476500674504442735[273] = 0; - out_3476500674504442735[274] = 0; - out_3476500674504442735[275] = 0; - out_3476500674504442735[276] = 0; - out_3476500674504442735[277] = 0; - out_3476500674504442735[278] = 0; - out_3476500674504442735[279] = 0; - out_3476500674504442735[280] = 0; - out_3476500674504442735[281] = 0; - out_3476500674504442735[282] = 0; - out_3476500674504442735[283] = 0; - out_3476500674504442735[284] = 0; - out_3476500674504442735[285] = 1; - out_3476500674504442735[286] = 0; - out_3476500674504442735[287] = 0; - out_3476500674504442735[288] = 0; - out_3476500674504442735[289] = 0; - out_3476500674504442735[290] = 0; - out_3476500674504442735[291] = 0; - out_3476500674504442735[292] = 0; - out_3476500674504442735[293] = 0; - out_3476500674504442735[294] = 0; - out_3476500674504442735[295] = 0; - out_3476500674504442735[296] = 0; - out_3476500674504442735[297] = 0; - out_3476500674504442735[298] = 0; - out_3476500674504442735[299] = 0; - out_3476500674504442735[300] = 0; - out_3476500674504442735[301] = 0; - out_3476500674504442735[302] = 0; - out_3476500674504442735[303] = 0; - out_3476500674504442735[304] = 1; - out_3476500674504442735[305] = 0; - out_3476500674504442735[306] = 0; - out_3476500674504442735[307] = 0; - out_3476500674504442735[308] = 0; - out_3476500674504442735[309] = 0; - out_3476500674504442735[310] = 0; - out_3476500674504442735[311] = 0; - out_3476500674504442735[312] = 0; - out_3476500674504442735[313] = 0; - out_3476500674504442735[314] = 0; - out_3476500674504442735[315] = 0; - out_3476500674504442735[316] = 0; - out_3476500674504442735[317] = 0; - out_3476500674504442735[318] = 0; - out_3476500674504442735[319] = 0; - out_3476500674504442735[320] = 0; - out_3476500674504442735[321] = 0; - out_3476500674504442735[322] = 0; - out_3476500674504442735[323] = 1; +void F_fun(double *state, double dt, double *out_7339590787986428927) { + out_7339590787986428927[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_7339590787986428927[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_7339590787986428927[2] = 0; + out_7339590787986428927[3] = 0; + out_7339590787986428927[4] = 0; + out_7339590787986428927[5] = 0; + out_7339590787986428927[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_7339590787986428927[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_7339590787986428927[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_7339590787986428927[9] = 0; + out_7339590787986428927[10] = 0; + out_7339590787986428927[11] = 0; + out_7339590787986428927[12] = 0; + out_7339590787986428927[13] = 0; + out_7339590787986428927[14] = 0; + out_7339590787986428927[15] = 0; + out_7339590787986428927[16] = 0; + out_7339590787986428927[17] = 0; + out_7339590787986428927[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_7339590787986428927[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_7339590787986428927[20] = 0; + out_7339590787986428927[21] = 0; + out_7339590787986428927[22] = 0; + out_7339590787986428927[23] = 0; + out_7339590787986428927[24] = 0; + out_7339590787986428927[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_7339590787986428927[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_7339590787986428927[27] = 0; + out_7339590787986428927[28] = 0; + out_7339590787986428927[29] = 0; + out_7339590787986428927[30] = 0; + out_7339590787986428927[31] = 0; + out_7339590787986428927[32] = 0; + out_7339590787986428927[33] = 0; + out_7339590787986428927[34] = 0; + out_7339590787986428927[35] = 0; + out_7339590787986428927[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_7339590787986428927[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_7339590787986428927[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_7339590787986428927[39] = 0; + out_7339590787986428927[40] = 0; + out_7339590787986428927[41] = 0; + out_7339590787986428927[42] = 0; + out_7339590787986428927[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_7339590787986428927[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_7339590787986428927[45] = 0; + out_7339590787986428927[46] = 0; + out_7339590787986428927[47] = 0; + out_7339590787986428927[48] = 0; + out_7339590787986428927[49] = 0; + out_7339590787986428927[50] = 0; + out_7339590787986428927[51] = 0; + out_7339590787986428927[52] = 0; + out_7339590787986428927[53] = 0; + out_7339590787986428927[54] = 0; + out_7339590787986428927[55] = 0; + out_7339590787986428927[56] = 0; + out_7339590787986428927[57] = 1; + out_7339590787986428927[58] = 0; + out_7339590787986428927[59] = 0; + out_7339590787986428927[60] = 0; + out_7339590787986428927[61] = 0; + out_7339590787986428927[62] = 0; + out_7339590787986428927[63] = 0; + out_7339590787986428927[64] = 0; + out_7339590787986428927[65] = 0; + out_7339590787986428927[66] = dt; + out_7339590787986428927[67] = 0; + out_7339590787986428927[68] = 0; + out_7339590787986428927[69] = 0; + out_7339590787986428927[70] = 0; + out_7339590787986428927[71] = 0; + out_7339590787986428927[72] = 0; + out_7339590787986428927[73] = 0; + out_7339590787986428927[74] = 0; + out_7339590787986428927[75] = 0; + out_7339590787986428927[76] = 1; + out_7339590787986428927[77] = 0; + out_7339590787986428927[78] = 0; + out_7339590787986428927[79] = 0; + out_7339590787986428927[80] = 0; + out_7339590787986428927[81] = 0; + out_7339590787986428927[82] = 0; + out_7339590787986428927[83] = 0; + out_7339590787986428927[84] = 0; + out_7339590787986428927[85] = dt; + out_7339590787986428927[86] = 0; + out_7339590787986428927[87] = 0; + out_7339590787986428927[88] = 0; + out_7339590787986428927[89] = 0; + out_7339590787986428927[90] = 0; + out_7339590787986428927[91] = 0; + out_7339590787986428927[92] = 0; + out_7339590787986428927[93] = 0; + out_7339590787986428927[94] = 0; + out_7339590787986428927[95] = 1; + out_7339590787986428927[96] = 0; + out_7339590787986428927[97] = 0; + out_7339590787986428927[98] = 0; + out_7339590787986428927[99] = 0; + out_7339590787986428927[100] = 0; + out_7339590787986428927[101] = 0; + out_7339590787986428927[102] = 0; + out_7339590787986428927[103] = 0; + out_7339590787986428927[104] = dt; + out_7339590787986428927[105] = 0; + out_7339590787986428927[106] = 0; + out_7339590787986428927[107] = 0; + out_7339590787986428927[108] = 0; + out_7339590787986428927[109] = 0; + out_7339590787986428927[110] = 0; + out_7339590787986428927[111] = 0; + out_7339590787986428927[112] = 0; + out_7339590787986428927[113] = 0; + out_7339590787986428927[114] = 1; + out_7339590787986428927[115] = 0; + out_7339590787986428927[116] = 0; + out_7339590787986428927[117] = 0; + out_7339590787986428927[118] = 0; + out_7339590787986428927[119] = 0; + out_7339590787986428927[120] = 0; + out_7339590787986428927[121] = 0; + out_7339590787986428927[122] = 0; + out_7339590787986428927[123] = 0; + out_7339590787986428927[124] = 0; + out_7339590787986428927[125] = 0; + out_7339590787986428927[126] = 0; + out_7339590787986428927[127] = 0; + out_7339590787986428927[128] = 0; + out_7339590787986428927[129] = 0; + out_7339590787986428927[130] = 0; + out_7339590787986428927[131] = 0; + out_7339590787986428927[132] = 0; + out_7339590787986428927[133] = 1; + out_7339590787986428927[134] = 0; + out_7339590787986428927[135] = 0; + out_7339590787986428927[136] = 0; + out_7339590787986428927[137] = 0; + out_7339590787986428927[138] = 0; + out_7339590787986428927[139] = 0; + out_7339590787986428927[140] = 0; + out_7339590787986428927[141] = 0; + out_7339590787986428927[142] = 0; + out_7339590787986428927[143] = 0; + out_7339590787986428927[144] = 0; + out_7339590787986428927[145] = 0; + out_7339590787986428927[146] = 0; + out_7339590787986428927[147] = 0; + out_7339590787986428927[148] = 0; + out_7339590787986428927[149] = 0; + out_7339590787986428927[150] = 0; + out_7339590787986428927[151] = 0; + out_7339590787986428927[152] = 1; + out_7339590787986428927[153] = 0; + out_7339590787986428927[154] = 0; + out_7339590787986428927[155] = 0; + out_7339590787986428927[156] = 0; + out_7339590787986428927[157] = 0; + out_7339590787986428927[158] = 0; + out_7339590787986428927[159] = 0; + out_7339590787986428927[160] = 0; + out_7339590787986428927[161] = 0; + out_7339590787986428927[162] = 0; + out_7339590787986428927[163] = 0; + out_7339590787986428927[164] = 0; + out_7339590787986428927[165] = 0; + out_7339590787986428927[166] = 0; + out_7339590787986428927[167] = 0; + out_7339590787986428927[168] = 0; + out_7339590787986428927[169] = 0; + out_7339590787986428927[170] = 0; + out_7339590787986428927[171] = 1; + out_7339590787986428927[172] = 0; + out_7339590787986428927[173] = 0; + out_7339590787986428927[174] = 0; + out_7339590787986428927[175] = 0; + out_7339590787986428927[176] = 0; + out_7339590787986428927[177] = 0; + out_7339590787986428927[178] = 0; + out_7339590787986428927[179] = 0; + out_7339590787986428927[180] = 0; + out_7339590787986428927[181] = 0; + out_7339590787986428927[182] = 0; + out_7339590787986428927[183] = 0; + out_7339590787986428927[184] = 0; + out_7339590787986428927[185] = 0; + out_7339590787986428927[186] = 0; + out_7339590787986428927[187] = 0; + out_7339590787986428927[188] = 0; + out_7339590787986428927[189] = 0; + out_7339590787986428927[190] = 1; + out_7339590787986428927[191] = 0; + out_7339590787986428927[192] = 0; + out_7339590787986428927[193] = 0; + out_7339590787986428927[194] = 0; + out_7339590787986428927[195] = 0; + out_7339590787986428927[196] = 0; + out_7339590787986428927[197] = 0; + out_7339590787986428927[198] = 0; + out_7339590787986428927[199] = 0; + out_7339590787986428927[200] = 0; + out_7339590787986428927[201] = 0; + out_7339590787986428927[202] = 0; + out_7339590787986428927[203] = 0; + out_7339590787986428927[204] = 0; + out_7339590787986428927[205] = 0; + out_7339590787986428927[206] = 0; + out_7339590787986428927[207] = 0; + out_7339590787986428927[208] = 0; + out_7339590787986428927[209] = 1; + out_7339590787986428927[210] = 0; + out_7339590787986428927[211] = 0; + out_7339590787986428927[212] = 0; + out_7339590787986428927[213] = 0; + out_7339590787986428927[214] = 0; + out_7339590787986428927[215] = 0; + out_7339590787986428927[216] = 0; + out_7339590787986428927[217] = 0; + out_7339590787986428927[218] = 0; + out_7339590787986428927[219] = 0; + out_7339590787986428927[220] = 0; + out_7339590787986428927[221] = 0; + out_7339590787986428927[222] = 0; + out_7339590787986428927[223] = 0; + out_7339590787986428927[224] = 0; + out_7339590787986428927[225] = 0; + out_7339590787986428927[226] = 0; + out_7339590787986428927[227] = 0; + out_7339590787986428927[228] = 1; + out_7339590787986428927[229] = 0; + out_7339590787986428927[230] = 0; + out_7339590787986428927[231] = 0; + out_7339590787986428927[232] = 0; + out_7339590787986428927[233] = 0; + out_7339590787986428927[234] = 0; + out_7339590787986428927[235] = 0; + out_7339590787986428927[236] = 0; + out_7339590787986428927[237] = 0; + out_7339590787986428927[238] = 0; + out_7339590787986428927[239] = 0; + out_7339590787986428927[240] = 0; + out_7339590787986428927[241] = 0; + out_7339590787986428927[242] = 0; + out_7339590787986428927[243] = 0; + out_7339590787986428927[244] = 0; + out_7339590787986428927[245] = 0; + out_7339590787986428927[246] = 0; + out_7339590787986428927[247] = 1; + out_7339590787986428927[248] = 0; + out_7339590787986428927[249] = 0; + out_7339590787986428927[250] = 0; + out_7339590787986428927[251] = 0; + out_7339590787986428927[252] = 0; + out_7339590787986428927[253] = 0; + out_7339590787986428927[254] = 0; + out_7339590787986428927[255] = 0; + out_7339590787986428927[256] = 0; + out_7339590787986428927[257] = 0; + out_7339590787986428927[258] = 0; + out_7339590787986428927[259] = 0; + out_7339590787986428927[260] = 0; + out_7339590787986428927[261] = 0; + out_7339590787986428927[262] = 0; + out_7339590787986428927[263] = 0; + out_7339590787986428927[264] = 0; + out_7339590787986428927[265] = 0; + out_7339590787986428927[266] = 1; + out_7339590787986428927[267] = 0; + out_7339590787986428927[268] = 0; + out_7339590787986428927[269] = 0; + out_7339590787986428927[270] = 0; + out_7339590787986428927[271] = 0; + out_7339590787986428927[272] = 0; + out_7339590787986428927[273] = 0; + out_7339590787986428927[274] = 0; + out_7339590787986428927[275] = 0; + out_7339590787986428927[276] = 0; + out_7339590787986428927[277] = 0; + out_7339590787986428927[278] = 0; + out_7339590787986428927[279] = 0; + out_7339590787986428927[280] = 0; + out_7339590787986428927[281] = 0; + out_7339590787986428927[282] = 0; + out_7339590787986428927[283] = 0; + out_7339590787986428927[284] = 0; + out_7339590787986428927[285] = 1; + out_7339590787986428927[286] = 0; + out_7339590787986428927[287] = 0; + out_7339590787986428927[288] = 0; + out_7339590787986428927[289] = 0; + out_7339590787986428927[290] = 0; + out_7339590787986428927[291] = 0; + out_7339590787986428927[292] = 0; + out_7339590787986428927[293] = 0; + out_7339590787986428927[294] = 0; + out_7339590787986428927[295] = 0; + out_7339590787986428927[296] = 0; + out_7339590787986428927[297] = 0; + out_7339590787986428927[298] = 0; + out_7339590787986428927[299] = 0; + out_7339590787986428927[300] = 0; + out_7339590787986428927[301] = 0; + out_7339590787986428927[302] = 0; + out_7339590787986428927[303] = 0; + out_7339590787986428927[304] = 1; + out_7339590787986428927[305] = 0; + out_7339590787986428927[306] = 0; + out_7339590787986428927[307] = 0; + out_7339590787986428927[308] = 0; + out_7339590787986428927[309] = 0; + out_7339590787986428927[310] = 0; + out_7339590787986428927[311] = 0; + out_7339590787986428927[312] = 0; + out_7339590787986428927[313] = 0; + out_7339590787986428927[314] = 0; + out_7339590787986428927[315] = 0; + out_7339590787986428927[316] = 0; + out_7339590787986428927[317] = 0; + out_7339590787986428927[318] = 0; + out_7339590787986428927[319] = 0; + out_7339590787986428927[320] = 0; + out_7339590787986428927[321] = 0; + out_7339590787986428927[322] = 0; + out_7339590787986428927[323] = 1; } -void h_4(double *state, double *unused, double *out_2259436079279725972) { - out_2259436079279725972[0] = state[6] + state[9]; - out_2259436079279725972[1] = state[7] + state[10]; - out_2259436079279725972[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_8608741357917418213) { + out_8608741357917418213[0] = state[6] + state[9]; + out_8608741357917418213[1] = state[7] + state[10]; + out_8608741357917418213[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_6715024945092366734) { - out_6715024945092366734[0] = 0; - out_6715024945092366734[1] = 0; - out_6715024945092366734[2] = 0; - out_6715024945092366734[3] = 0; - out_6715024945092366734[4] = 0; - out_6715024945092366734[5] = 0; - out_6715024945092366734[6] = 1; - out_6715024945092366734[7] = 0; - out_6715024945092366734[8] = 0; - out_6715024945092366734[9] = 1; - out_6715024945092366734[10] = 0; - out_6715024945092366734[11] = 0; - out_6715024945092366734[12] = 0; - out_6715024945092366734[13] = 0; - out_6715024945092366734[14] = 0; - out_6715024945092366734[15] = 0; - out_6715024945092366734[16] = 0; - out_6715024945092366734[17] = 0; - out_6715024945092366734[18] = 0; - out_6715024945092366734[19] = 0; - out_6715024945092366734[20] = 0; - out_6715024945092366734[21] = 0; - out_6715024945092366734[22] = 0; - out_6715024945092366734[23] = 0; - out_6715024945092366734[24] = 0; - out_6715024945092366734[25] = 1; - out_6715024945092366734[26] = 0; - out_6715024945092366734[27] = 0; - out_6715024945092366734[28] = 1; - out_6715024945092366734[29] = 0; - out_6715024945092366734[30] = 0; - out_6715024945092366734[31] = 0; - out_6715024945092366734[32] = 0; - out_6715024945092366734[33] = 0; - out_6715024945092366734[34] = 0; - out_6715024945092366734[35] = 0; - out_6715024945092366734[36] = 0; - out_6715024945092366734[37] = 0; - out_6715024945092366734[38] = 0; - out_6715024945092366734[39] = 0; - out_6715024945092366734[40] = 0; - out_6715024945092366734[41] = 0; - out_6715024945092366734[42] = 0; - out_6715024945092366734[43] = 0; - out_6715024945092366734[44] = 1; - out_6715024945092366734[45] = 0; - out_6715024945092366734[46] = 0; - out_6715024945092366734[47] = 1; - out_6715024945092366734[48] = 0; - out_6715024945092366734[49] = 0; - out_6715024945092366734[50] = 0; - out_6715024945092366734[51] = 0; - out_6715024945092366734[52] = 0; - out_6715024945092366734[53] = 0; +void H_4(double *state, double *unused, double *out_8804004163997345567) { + out_8804004163997345567[0] = 0; + out_8804004163997345567[1] = 0; + out_8804004163997345567[2] = 0; + out_8804004163997345567[3] = 0; + out_8804004163997345567[4] = 0; + out_8804004163997345567[5] = 0; + out_8804004163997345567[6] = 1; + out_8804004163997345567[7] = 0; + out_8804004163997345567[8] = 0; + out_8804004163997345567[9] = 1; + out_8804004163997345567[10] = 0; + out_8804004163997345567[11] = 0; + out_8804004163997345567[12] = 0; + out_8804004163997345567[13] = 0; + out_8804004163997345567[14] = 0; + out_8804004163997345567[15] = 0; + out_8804004163997345567[16] = 0; + out_8804004163997345567[17] = 0; + out_8804004163997345567[18] = 0; + out_8804004163997345567[19] = 0; + out_8804004163997345567[20] = 0; + out_8804004163997345567[21] = 0; + out_8804004163997345567[22] = 0; + out_8804004163997345567[23] = 0; + out_8804004163997345567[24] = 0; + out_8804004163997345567[25] = 1; + out_8804004163997345567[26] = 0; + out_8804004163997345567[27] = 0; + out_8804004163997345567[28] = 1; + out_8804004163997345567[29] = 0; + out_8804004163997345567[30] = 0; + out_8804004163997345567[31] = 0; + out_8804004163997345567[32] = 0; + out_8804004163997345567[33] = 0; + out_8804004163997345567[34] = 0; + out_8804004163997345567[35] = 0; + out_8804004163997345567[36] = 0; + out_8804004163997345567[37] = 0; + out_8804004163997345567[38] = 0; + out_8804004163997345567[39] = 0; + out_8804004163997345567[40] = 0; + out_8804004163997345567[41] = 0; + out_8804004163997345567[42] = 0; + out_8804004163997345567[43] = 0; + out_8804004163997345567[44] = 1; + out_8804004163997345567[45] = 0; + out_8804004163997345567[46] = 0; + out_8804004163997345567[47] = 1; + out_8804004163997345567[48] = 0; + out_8804004163997345567[49] = 0; + out_8804004163997345567[50] = 0; + out_8804004163997345567[51] = 0; + out_8804004163997345567[52] = 0; + out_8804004163997345567[53] = 0; } -void h_10(double *state, double *unused, double *out_4495031819694209583) { - out_4495031819694209583[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_4495031819694209583[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_4495031819694209583[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_4067868698993544544) { + out_4067868698993544544[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_4067868698993544544[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_4067868698993544544[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_8084729141614988184) { - out_8084729141614988184[0] = 0; - out_8084729141614988184[1] = 9.8100000000000005*cos(state[1]); - out_8084729141614988184[2] = 0; - out_8084729141614988184[3] = 0; - out_8084729141614988184[4] = -state[8]; - out_8084729141614988184[5] = state[7]; - out_8084729141614988184[6] = 0; - out_8084729141614988184[7] = state[5]; - out_8084729141614988184[8] = -state[4]; - out_8084729141614988184[9] = 0; - out_8084729141614988184[10] = 0; - out_8084729141614988184[11] = 0; - out_8084729141614988184[12] = 1; - out_8084729141614988184[13] = 0; - out_8084729141614988184[14] = 0; - out_8084729141614988184[15] = 1; - out_8084729141614988184[16] = 0; - out_8084729141614988184[17] = 0; - out_8084729141614988184[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_8084729141614988184[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_8084729141614988184[20] = 0; - out_8084729141614988184[21] = state[8]; - out_8084729141614988184[22] = 0; - out_8084729141614988184[23] = -state[6]; - out_8084729141614988184[24] = -state[5]; - out_8084729141614988184[25] = 0; - out_8084729141614988184[26] = state[3]; - out_8084729141614988184[27] = 0; - out_8084729141614988184[28] = 0; - out_8084729141614988184[29] = 0; - out_8084729141614988184[30] = 0; - out_8084729141614988184[31] = 1; - out_8084729141614988184[32] = 0; - out_8084729141614988184[33] = 0; - out_8084729141614988184[34] = 1; - out_8084729141614988184[35] = 0; - out_8084729141614988184[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_8084729141614988184[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_8084729141614988184[38] = 0; - out_8084729141614988184[39] = -state[7]; - out_8084729141614988184[40] = state[6]; - out_8084729141614988184[41] = 0; - out_8084729141614988184[42] = state[4]; - out_8084729141614988184[43] = -state[3]; - out_8084729141614988184[44] = 0; - out_8084729141614988184[45] = 0; - out_8084729141614988184[46] = 0; - out_8084729141614988184[47] = 0; - out_8084729141614988184[48] = 0; - out_8084729141614988184[49] = 0; - out_8084729141614988184[50] = 1; - out_8084729141614988184[51] = 0; - out_8084729141614988184[52] = 0; - out_8084729141614988184[53] = 1; +void H_10(double *state, double *unused, double *out_6538171315605639203) { + out_6538171315605639203[0] = 0; + out_6538171315605639203[1] = 9.8100000000000005*cos(state[1]); + out_6538171315605639203[2] = 0; + out_6538171315605639203[3] = 0; + out_6538171315605639203[4] = -state[8]; + out_6538171315605639203[5] = state[7]; + out_6538171315605639203[6] = 0; + out_6538171315605639203[7] = state[5]; + out_6538171315605639203[8] = -state[4]; + out_6538171315605639203[9] = 0; + out_6538171315605639203[10] = 0; + out_6538171315605639203[11] = 0; + out_6538171315605639203[12] = 1; + out_6538171315605639203[13] = 0; + out_6538171315605639203[14] = 0; + out_6538171315605639203[15] = 1; + out_6538171315605639203[16] = 0; + out_6538171315605639203[17] = 0; + out_6538171315605639203[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_6538171315605639203[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_6538171315605639203[20] = 0; + out_6538171315605639203[21] = state[8]; + out_6538171315605639203[22] = 0; + out_6538171315605639203[23] = -state[6]; + out_6538171315605639203[24] = -state[5]; + out_6538171315605639203[25] = 0; + out_6538171315605639203[26] = state[3]; + out_6538171315605639203[27] = 0; + out_6538171315605639203[28] = 0; + out_6538171315605639203[29] = 0; + out_6538171315605639203[30] = 0; + out_6538171315605639203[31] = 1; + out_6538171315605639203[32] = 0; + out_6538171315605639203[33] = 0; + out_6538171315605639203[34] = 1; + out_6538171315605639203[35] = 0; + out_6538171315605639203[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_6538171315605639203[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_6538171315605639203[38] = 0; + out_6538171315605639203[39] = -state[7]; + out_6538171315605639203[40] = state[6]; + out_6538171315605639203[41] = 0; + out_6538171315605639203[42] = state[4]; + out_6538171315605639203[43] = -state[3]; + out_6538171315605639203[44] = 0; + out_6538171315605639203[45] = 0; + out_6538171315605639203[46] = 0; + out_6538171315605639203[47] = 0; + out_6538171315605639203[48] = 0; + out_6538171315605639203[49] = 0; + out_6538171315605639203[50] = 1; + out_6538171315605639203[51] = 0; + out_6538171315605639203[52] = 0; + out_6538171315605639203[53] = 1; } -void h_13(double *state, double *unused, double *out_7265342654384269353) { - out_7265342654384269353[0] = state[3]; - out_7265342654384269353[1] = state[4]; - out_7265342654384269353[2] = state[5]; +void h_13(double *state, double *unused, double *out_1704356913043206317) { + out_1704356913043206317[0] = state[3]; + out_1704356913043206317[1] = state[4]; + out_1704356913043206317[2] = state[5]; } -void H_13(double *state, double *unused, double *out_8519445303284852081) { - out_8519445303284852081[0] = 0; - out_8519445303284852081[1] = 0; - out_8519445303284852081[2] = 0; - out_8519445303284852081[3] = 1; - out_8519445303284852081[4] = 0; - out_8519445303284852081[5] = 0; - out_8519445303284852081[6] = 0; - out_8519445303284852081[7] = 0; - out_8519445303284852081[8] = 0; - out_8519445303284852081[9] = 0; - out_8519445303284852081[10] = 0; - out_8519445303284852081[11] = 0; - out_8519445303284852081[12] = 0; - out_8519445303284852081[13] = 0; - out_8519445303284852081[14] = 0; - out_8519445303284852081[15] = 0; - out_8519445303284852081[16] = 0; - out_8519445303284852081[17] = 0; - out_8519445303284852081[18] = 0; - out_8519445303284852081[19] = 0; - out_8519445303284852081[20] = 0; - out_8519445303284852081[21] = 0; - out_8519445303284852081[22] = 1; - out_8519445303284852081[23] = 0; - out_8519445303284852081[24] = 0; - out_8519445303284852081[25] = 0; - out_8519445303284852081[26] = 0; - out_8519445303284852081[27] = 0; - out_8519445303284852081[28] = 0; - out_8519445303284852081[29] = 0; - out_8519445303284852081[30] = 0; - out_8519445303284852081[31] = 0; - out_8519445303284852081[32] = 0; - out_8519445303284852081[33] = 0; - out_8519445303284852081[34] = 0; - out_8519445303284852081[35] = 0; - out_8519445303284852081[36] = 0; - out_8519445303284852081[37] = 0; - out_8519445303284852081[38] = 0; - out_8519445303284852081[39] = 0; - out_8519445303284852081[40] = 0; - out_8519445303284852081[41] = 1; - out_8519445303284852081[42] = 0; - out_8519445303284852081[43] = 0; - out_8519445303284852081[44] = 0; - out_8519445303284852081[45] = 0; - out_8519445303284852081[46] = 0; - out_8519445303284852081[47] = 0; - out_8519445303284852081[48] = 0; - out_8519445303284852081[49] = 0; - out_8519445303284852081[50] = 0; - out_8519445303284852081[51] = 0; - out_8519445303284852081[52] = 0; - out_8519445303284852081[53] = 0; +void H_13(double *state, double *unused, double *out_5591730338665012766) { + out_5591730338665012766[0] = 0; + out_5591730338665012766[1] = 0; + out_5591730338665012766[2] = 0; + out_5591730338665012766[3] = 1; + out_5591730338665012766[4] = 0; + out_5591730338665012766[5] = 0; + out_5591730338665012766[6] = 0; + out_5591730338665012766[7] = 0; + out_5591730338665012766[8] = 0; + out_5591730338665012766[9] = 0; + out_5591730338665012766[10] = 0; + out_5591730338665012766[11] = 0; + out_5591730338665012766[12] = 0; + out_5591730338665012766[13] = 0; + out_5591730338665012766[14] = 0; + out_5591730338665012766[15] = 0; + out_5591730338665012766[16] = 0; + out_5591730338665012766[17] = 0; + out_5591730338665012766[18] = 0; + out_5591730338665012766[19] = 0; + out_5591730338665012766[20] = 0; + out_5591730338665012766[21] = 0; + out_5591730338665012766[22] = 1; + out_5591730338665012766[23] = 0; + out_5591730338665012766[24] = 0; + out_5591730338665012766[25] = 0; + out_5591730338665012766[26] = 0; + out_5591730338665012766[27] = 0; + out_5591730338665012766[28] = 0; + out_5591730338665012766[29] = 0; + out_5591730338665012766[30] = 0; + out_5591730338665012766[31] = 0; + out_5591730338665012766[32] = 0; + out_5591730338665012766[33] = 0; + out_5591730338665012766[34] = 0; + out_5591730338665012766[35] = 0; + out_5591730338665012766[36] = 0; + out_5591730338665012766[37] = 0; + out_5591730338665012766[38] = 0; + out_5591730338665012766[39] = 0; + out_5591730338665012766[40] = 0; + out_5591730338665012766[41] = 1; + out_5591730338665012766[42] = 0; + out_5591730338665012766[43] = 0; + out_5591730338665012766[44] = 0; + out_5591730338665012766[45] = 0; + out_5591730338665012766[46] = 0; + out_5591730338665012766[47] = 0; + out_5591730338665012766[48] = 0; + out_5591730338665012766[49] = 0; + out_5591730338665012766[50] = 0; + out_5591730338665012766[51] = 0; + out_5591730338665012766[52] = 0; + out_5591730338665012766[53] = 0; } -void h_14(double *state, double *unused, double *out_2527703015463387955) { - out_2527703015463387955[0] = state[6]; - out_2527703015463387955[1] = state[7]; - out_2527703015463387955[2] = state[8]; +void h_14(double *state, double *unused, double *out_5410919187051701678) { + out_5410919187051701678[0] = state[6]; + out_5410919187051701678[1] = state[7]; + out_5410919187051701678[2] = state[8]; } -void H_14(double *state, double *unused, double *out_7768478272277700353) { - out_7768478272277700353[0] = 0; - out_7768478272277700353[1] = 0; - out_7768478272277700353[2] = 0; - out_7768478272277700353[3] = 0; - out_7768478272277700353[4] = 0; - out_7768478272277700353[5] = 0; - out_7768478272277700353[6] = 1; - out_7768478272277700353[7] = 0; - out_7768478272277700353[8] = 0; - out_7768478272277700353[9] = 0; - out_7768478272277700353[10] = 0; - out_7768478272277700353[11] = 0; - out_7768478272277700353[12] = 0; - out_7768478272277700353[13] = 0; - out_7768478272277700353[14] = 0; - out_7768478272277700353[15] = 0; - out_7768478272277700353[16] = 0; - out_7768478272277700353[17] = 0; - out_7768478272277700353[18] = 0; - out_7768478272277700353[19] = 0; - out_7768478272277700353[20] = 0; - out_7768478272277700353[21] = 0; - out_7768478272277700353[22] = 0; - out_7768478272277700353[23] = 0; - out_7768478272277700353[24] = 0; - out_7768478272277700353[25] = 1; - out_7768478272277700353[26] = 0; - out_7768478272277700353[27] = 0; - out_7768478272277700353[28] = 0; - out_7768478272277700353[29] = 0; - out_7768478272277700353[30] = 0; - out_7768478272277700353[31] = 0; - out_7768478272277700353[32] = 0; - out_7768478272277700353[33] = 0; - out_7768478272277700353[34] = 0; - out_7768478272277700353[35] = 0; - out_7768478272277700353[36] = 0; - out_7768478272277700353[37] = 0; - out_7768478272277700353[38] = 0; - out_7768478272277700353[39] = 0; - out_7768478272277700353[40] = 0; - out_7768478272277700353[41] = 0; - out_7768478272277700353[42] = 0; - out_7768478272277700353[43] = 0; - out_7768478272277700353[44] = 1; - out_7768478272277700353[45] = 0; - out_7768478272277700353[46] = 0; - out_7768478272277700353[47] = 0; - out_7768478272277700353[48] = 0; - out_7768478272277700353[49] = 0; - out_7768478272277700353[50] = 0; - out_7768478272277700353[51] = 0; - out_7768478272277700353[52] = 0; - out_7768478272277700353[53] = 0; +void H_14(double *state, double *unused, double *out_4840763307657861038) { + out_4840763307657861038[0] = 0; + out_4840763307657861038[1] = 0; + out_4840763307657861038[2] = 0; + out_4840763307657861038[3] = 0; + out_4840763307657861038[4] = 0; + out_4840763307657861038[5] = 0; + out_4840763307657861038[6] = 1; + out_4840763307657861038[7] = 0; + out_4840763307657861038[8] = 0; + out_4840763307657861038[9] = 0; + out_4840763307657861038[10] = 0; + out_4840763307657861038[11] = 0; + out_4840763307657861038[12] = 0; + out_4840763307657861038[13] = 0; + out_4840763307657861038[14] = 0; + out_4840763307657861038[15] = 0; + out_4840763307657861038[16] = 0; + out_4840763307657861038[17] = 0; + out_4840763307657861038[18] = 0; + out_4840763307657861038[19] = 0; + out_4840763307657861038[20] = 0; + out_4840763307657861038[21] = 0; + out_4840763307657861038[22] = 0; + out_4840763307657861038[23] = 0; + out_4840763307657861038[24] = 0; + out_4840763307657861038[25] = 1; + out_4840763307657861038[26] = 0; + out_4840763307657861038[27] = 0; + out_4840763307657861038[28] = 0; + out_4840763307657861038[29] = 0; + out_4840763307657861038[30] = 0; + out_4840763307657861038[31] = 0; + out_4840763307657861038[32] = 0; + out_4840763307657861038[33] = 0; + out_4840763307657861038[34] = 0; + out_4840763307657861038[35] = 0; + out_4840763307657861038[36] = 0; + out_4840763307657861038[37] = 0; + out_4840763307657861038[38] = 0; + out_4840763307657861038[39] = 0; + out_4840763307657861038[40] = 0; + out_4840763307657861038[41] = 0; + out_4840763307657861038[42] = 0; + out_4840763307657861038[43] = 0; + out_4840763307657861038[44] = 1; + out_4840763307657861038[45] = 0; + out_4840763307657861038[46] = 0; + out_4840763307657861038[47] = 0; + out_4840763307657861038[48] = 0; + out_4840763307657861038[49] = 0; + out_4840763307657861038[50] = 0; + out_4840763307657861038[51] = 0; + out_4840763307657861038[52] = 0; + out_4840763307657861038[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_7527833659731555578) { - err_fun(nom_x, delta_x, out_7527833659731555578); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2889016141520795461) { + err_fun(nom_x, delta_x, out_2889016141520795461); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8121371430920178446) { - inv_err_fun(nom_x, true_x, out_8121371430920178446); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6586333085018065986) { + inv_err_fun(nom_x, true_x, out_6586333085018065986); } -void pose_H_mod_fun(double *state, double *out_2787357462559363540) { - H_mod_fun(state, out_2787357462559363540); +void pose_H_mod_fun(double *state, double *out_6984305438091048706) { + H_mod_fun(state, out_6984305438091048706); } -void pose_f_fun(double *state, double dt, double *out_2997254779517652533) { - f_fun(state, dt, out_2997254779517652533); +void pose_f_fun(double *state, double dt, double *out_4187862153732757636) { + f_fun(state, dt, out_4187862153732757636); } -void pose_F_fun(double *state, double dt, double *out_3476500674504442735) { - F_fun(state, dt, out_3476500674504442735); +void pose_F_fun(double *state, double dt, double *out_7339590787986428927) { + F_fun(state, dt, out_7339590787986428927); } -void pose_h_4(double *state, double *unused, double *out_2259436079279725972) { - h_4(state, unused, out_2259436079279725972); +void pose_h_4(double *state, double *unused, double *out_8608741357917418213) { + h_4(state, unused, out_8608741357917418213); } -void pose_H_4(double *state, double *unused, double *out_6715024945092366734) { - H_4(state, unused, out_6715024945092366734); +void pose_H_4(double *state, double *unused, double *out_8804004163997345567) { + H_4(state, unused, out_8804004163997345567); } -void pose_h_10(double *state, double *unused, double *out_4495031819694209583) { - h_10(state, unused, out_4495031819694209583); +void pose_h_10(double *state, double *unused, double *out_4067868698993544544) { + h_10(state, unused, out_4067868698993544544); } -void pose_H_10(double *state, double *unused, double *out_8084729141614988184) { - H_10(state, unused, out_8084729141614988184); +void pose_H_10(double *state, double *unused, double *out_6538171315605639203) { + H_10(state, unused, out_6538171315605639203); } -void pose_h_13(double *state, double *unused, double *out_7265342654384269353) { - h_13(state, unused, out_7265342654384269353); +void pose_h_13(double *state, double *unused, double *out_1704356913043206317) { + h_13(state, unused, out_1704356913043206317); } -void pose_H_13(double *state, double *unused, double *out_8519445303284852081) { - H_13(state, unused, out_8519445303284852081); +void pose_H_13(double *state, double *unused, double *out_5591730338665012766) { + H_13(state, unused, out_5591730338665012766); } -void pose_h_14(double *state, double *unused, double *out_2527703015463387955) { - h_14(state, unused, out_2527703015463387955); +void pose_h_14(double *state, double *unused, double *out_5410919187051701678) { + h_14(state, unused, out_5410919187051701678); } -void pose_H_14(double *state, double *unused, double *out_7768478272277700353) { - H_14(state, unused, out_7768478272277700353); +void pose_H_14(double *state, double *unused, double *out_4840763307657861038) { + H_14(state, unused, out_4840763307657861038); } 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 88ab50026..3b6f4c788 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_7527833659731555578); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8121371430920178446); -void pose_H_mod_fun(double *state, double *out_2787357462559363540); -void pose_f_fun(double *state, double dt, double *out_2997254779517652533); -void pose_F_fun(double *state, double dt, double *out_3476500674504442735); -void pose_h_4(double *state, double *unused, double *out_2259436079279725972); -void pose_H_4(double *state, double *unused, double *out_6715024945092366734); -void pose_h_10(double *state, double *unused, double *out_4495031819694209583); -void pose_H_10(double *state, double *unused, double *out_8084729141614988184); -void pose_h_13(double *state, double *unused, double *out_7265342654384269353); -void pose_H_13(double *state, double *unused, double *out_8519445303284852081); -void pose_h_14(double *state, double *unused, double *out_2527703015463387955); -void pose_H_14(double *state, double *unused, double *out_7768478272277700353); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2889016141520795461); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6586333085018065986); +void pose_H_mod_fun(double *state, double *out_6984305438091048706); +void pose_f_fun(double *state, double dt, double *out_4187862153732757636); +void pose_F_fun(double *state, double dt, double *out_7339590787986428927); +void pose_h_4(double *state, double *unused, double *out_8608741357917418213); +void pose_H_4(double *state, double *unused, double *out_8804004163997345567); +void pose_h_10(double *state, double *unused, double *out_4067868698993544544); +void pose_H_10(double *state, double *unused, double *out_6538171315605639203); +void pose_h_13(double *state, double *unused, double *out_1704356913043206317); +void pose_H_13(double *state, double *unused, double *out_5591730338665012766); +void pose_h_14(double *state, double *unused, double *out_5410919187051701678); +void pose_H_14(double *state, double *unused, double *out_4840763307657861038); 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 a895ddcdb..ae83d3dab 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ