diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 690223353..ca256aa8b 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 8ebbebec4..4cd58a9ea 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 2668d0791..f77be2e55 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 c0886cc5f..d4afc3c40 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 96fc4d83d..0d40d665b 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 7d8499cc8..809124e2a 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 f1f55a045..82315a460 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 f1f55a045..82315a460 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 6c83d4dc9..797acc5a0 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 7d8499cc8..809124e2a 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 d5c9da117..74a554785 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-eebfd509-DEBUG"; +const uint8_t gitversion[19] = "DEV-e372ae84-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index a5b07711c..952edc5c9 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 8909e0b40..1c171b619 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 4d6a78181..9648cf6b9 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 6e1dc22fe..6d81737b5 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 a3dced3c1..c8c8d7f2b 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 9e8228d62..55f9184fb 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 82fd16434..c35c5734b 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 d716ae7a3..d740b2ee3 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 d37012f7c..3000fdbd7 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 a9cf88384..547ce49ca 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 f7112ef43..51615f0d3 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 c9e24f4c7..989168f23 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 cc5f988b6..a3c8f90f4 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 e616bed1c..72728a192 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 2b6ed6f2c..8c047162f 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 016c4507b..89326e9dd 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 ae86262ba..d7f95b071 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 b91e452aa..e66e01fcc 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 bd7984536..81c09eb93 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 aee49dac5..4078a6766 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 b2b42de34..869abb531 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-eebfd509-DEBUG \ No newline at end of file +DEV-e372ae84-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index fbc5cad33..645b5ee5f 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_7149517118868685515) { - out_7149517118868685515[0] = delta_x[0] + nom_x[0]; - out_7149517118868685515[1] = delta_x[1] + nom_x[1]; - out_7149517118868685515[2] = delta_x[2] + nom_x[2]; - out_7149517118868685515[3] = delta_x[3] + nom_x[3]; - out_7149517118868685515[4] = delta_x[4] + nom_x[4]; - out_7149517118868685515[5] = delta_x[5] + nom_x[5]; - out_7149517118868685515[6] = delta_x[6] + nom_x[6]; - out_7149517118868685515[7] = delta_x[7] + nom_x[7]; - out_7149517118868685515[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_8318180392745555938) { + out_8318180392745555938[0] = delta_x[0] + nom_x[0]; + out_8318180392745555938[1] = delta_x[1] + nom_x[1]; + out_8318180392745555938[2] = delta_x[2] + nom_x[2]; + out_8318180392745555938[3] = delta_x[3] + nom_x[3]; + out_8318180392745555938[4] = delta_x[4] + nom_x[4]; + out_8318180392745555938[5] = delta_x[5] + nom_x[5]; + out_8318180392745555938[6] = delta_x[6] + nom_x[6]; + out_8318180392745555938[7] = delta_x[7] + nom_x[7]; + out_8318180392745555938[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1206282320038144058) { - out_1206282320038144058[0] = -nom_x[0] + true_x[0]; - out_1206282320038144058[1] = -nom_x[1] + true_x[1]; - out_1206282320038144058[2] = -nom_x[2] + true_x[2]; - out_1206282320038144058[3] = -nom_x[3] + true_x[3]; - out_1206282320038144058[4] = -nom_x[4] + true_x[4]; - out_1206282320038144058[5] = -nom_x[5] + true_x[5]; - out_1206282320038144058[6] = -nom_x[6] + true_x[6]; - out_1206282320038144058[7] = -nom_x[7] + true_x[7]; - out_1206282320038144058[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5934219001129015779) { + out_5934219001129015779[0] = -nom_x[0] + true_x[0]; + out_5934219001129015779[1] = -nom_x[1] + true_x[1]; + out_5934219001129015779[2] = -nom_x[2] + true_x[2]; + out_5934219001129015779[3] = -nom_x[3] + true_x[3]; + out_5934219001129015779[4] = -nom_x[4] + true_x[4]; + out_5934219001129015779[5] = -nom_x[5] + true_x[5]; + out_5934219001129015779[6] = -nom_x[6] + true_x[6]; + out_5934219001129015779[7] = -nom_x[7] + true_x[7]; + out_5934219001129015779[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_2536050837023062909) { - out_2536050837023062909[0] = 1.0; - out_2536050837023062909[1] = 0.0; - out_2536050837023062909[2] = 0.0; - out_2536050837023062909[3] = 0.0; - out_2536050837023062909[4] = 0.0; - out_2536050837023062909[5] = 0.0; - out_2536050837023062909[6] = 0.0; - out_2536050837023062909[7] = 0.0; - out_2536050837023062909[8] = 0.0; - out_2536050837023062909[9] = 0.0; - out_2536050837023062909[10] = 1.0; - out_2536050837023062909[11] = 0.0; - out_2536050837023062909[12] = 0.0; - out_2536050837023062909[13] = 0.0; - out_2536050837023062909[14] = 0.0; - out_2536050837023062909[15] = 0.0; - out_2536050837023062909[16] = 0.0; - out_2536050837023062909[17] = 0.0; - out_2536050837023062909[18] = 0.0; - out_2536050837023062909[19] = 0.0; - out_2536050837023062909[20] = 1.0; - out_2536050837023062909[21] = 0.0; - out_2536050837023062909[22] = 0.0; - out_2536050837023062909[23] = 0.0; - out_2536050837023062909[24] = 0.0; - out_2536050837023062909[25] = 0.0; - out_2536050837023062909[26] = 0.0; - out_2536050837023062909[27] = 0.0; - out_2536050837023062909[28] = 0.0; - out_2536050837023062909[29] = 0.0; - out_2536050837023062909[30] = 1.0; - out_2536050837023062909[31] = 0.0; - out_2536050837023062909[32] = 0.0; - out_2536050837023062909[33] = 0.0; - out_2536050837023062909[34] = 0.0; - out_2536050837023062909[35] = 0.0; - out_2536050837023062909[36] = 0.0; - out_2536050837023062909[37] = 0.0; - out_2536050837023062909[38] = 0.0; - out_2536050837023062909[39] = 0.0; - out_2536050837023062909[40] = 1.0; - out_2536050837023062909[41] = 0.0; - out_2536050837023062909[42] = 0.0; - out_2536050837023062909[43] = 0.0; - out_2536050837023062909[44] = 0.0; - out_2536050837023062909[45] = 0.0; - out_2536050837023062909[46] = 0.0; - out_2536050837023062909[47] = 0.0; - out_2536050837023062909[48] = 0.0; - out_2536050837023062909[49] = 0.0; - out_2536050837023062909[50] = 1.0; - out_2536050837023062909[51] = 0.0; - out_2536050837023062909[52] = 0.0; - out_2536050837023062909[53] = 0.0; - out_2536050837023062909[54] = 0.0; - out_2536050837023062909[55] = 0.0; - out_2536050837023062909[56] = 0.0; - out_2536050837023062909[57] = 0.0; - out_2536050837023062909[58] = 0.0; - out_2536050837023062909[59] = 0.0; - out_2536050837023062909[60] = 1.0; - out_2536050837023062909[61] = 0.0; - out_2536050837023062909[62] = 0.0; - out_2536050837023062909[63] = 0.0; - out_2536050837023062909[64] = 0.0; - out_2536050837023062909[65] = 0.0; - out_2536050837023062909[66] = 0.0; - out_2536050837023062909[67] = 0.0; - out_2536050837023062909[68] = 0.0; - out_2536050837023062909[69] = 0.0; - out_2536050837023062909[70] = 1.0; - out_2536050837023062909[71] = 0.0; - out_2536050837023062909[72] = 0.0; - out_2536050837023062909[73] = 0.0; - out_2536050837023062909[74] = 0.0; - out_2536050837023062909[75] = 0.0; - out_2536050837023062909[76] = 0.0; - out_2536050837023062909[77] = 0.0; - out_2536050837023062909[78] = 0.0; - out_2536050837023062909[79] = 0.0; - out_2536050837023062909[80] = 1.0; +void H_mod_fun(double *state, double *out_1926641469114901966) { + out_1926641469114901966[0] = 1.0; + out_1926641469114901966[1] = 0.0; + out_1926641469114901966[2] = 0.0; + out_1926641469114901966[3] = 0.0; + out_1926641469114901966[4] = 0.0; + out_1926641469114901966[5] = 0.0; + out_1926641469114901966[6] = 0.0; + out_1926641469114901966[7] = 0.0; + out_1926641469114901966[8] = 0.0; + out_1926641469114901966[9] = 0.0; + out_1926641469114901966[10] = 1.0; + out_1926641469114901966[11] = 0.0; + out_1926641469114901966[12] = 0.0; + out_1926641469114901966[13] = 0.0; + out_1926641469114901966[14] = 0.0; + out_1926641469114901966[15] = 0.0; + out_1926641469114901966[16] = 0.0; + out_1926641469114901966[17] = 0.0; + out_1926641469114901966[18] = 0.0; + out_1926641469114901966[19] = 0.0; + out_1926641469114901966[20] = 1.0; + out_1926641469114901966[21] = 0.0; + out_1926641469114901966[22] = 0.0; + out_1926641469114901966[23] = 0.0; + out_1926641469114901966[24] = 0.0; + out_1926641469114901966[25] = 0.0; + out_1926641469114901966[26] = 0.0; + out_1926641469114901966[27] = 0.0; + out_1926641469114901966[28] = 0.0; + out_1926641469114901966[29] = 0.0; + out_1926641469114901966[30] = 1.0; + out_1926641469114901966[31] = 0.0; + out_1926641469114901966[32] = 0.0; + out_1926641469114901966[33] = 0.0; + out_1926641469114901966[34] = 0.0; + out_1926641469114901966[35] = 0.0; + out_1926641469114901966[36] = 0.0; + out_1926641469114901966[37] = 0.0; + out_1926641469114901966[38] = 0.0; + out_1926641469114901966[39] = 0.0; + out_1926641469114901966[40] = 1.0; + out_1926641469114901966[41] = 0.0; + out_1926641469114901966[42] = 0.0; + out_1926641469114901966[43] = 0.0; + out_1926641469114901966[44] = 0.0; + out_1926641469114901966[45] = 0.0; + out_1926641469114901966[46] = 0.0; + out_1926641469114901966[47] = 0.0; + out_1926641469114901966[48] = 0.0; + out_1926641469114901966[49] = 0.0; + out_1926641469114901966[50] = 1.0; + out_1926641469114901966[51] = 0.0; + out_1926641469114901966[52] = 0.0; + out_1926641469114901966[53] = 0.0; + out_1926641469114901966[54] = 0.0; + out_1926641469114901966[55] = 0.0; + out_1926641469114901966[56] = 0.0; + out_1926641469114901966[57] = 0.0; + out_1926641469114901966[58] = 0.0; + out_1926641469114901966[59] = 0.0; + out_1926641469114901966[60] = 1.0; + out_1926641469114901966[61] = 0.0; + out_1926641469114901966[62] = 0.0; + out_1926641469114901966[63] = 0.0; + out_1926641469114901966[64] = 0.0; + out_1926641469114901966[65] = 0.0; + out_1926641469114901966[66] = 0.0; + out_1926641469114901966[67] = 0.0; + out_1926641469114901966[68] = 0.0; + out_1926641469114901966[69] = 0.0; + out_1926641469114901966[70] = 1.0; + out_1926641469114901966[71] = 0.0; + out_1926641469114901966[72] = 0.0; + out_1926641469114901966[73] = 0.0; + out_1926641469114901966[74] = 0.0; + out_1926641469114901966[75] = 0.0; + out_1926641469114901966[76] = 0.0; + out_1926641469114901966[77] = 0.0; + out_1926641469114901966[78] = 0.0; + out_1926641469114901966[79] = 0.0; + out_1926641469114901966[80] = 1.0; } -void f_fun(double *state, double dt, double *out_4666135608333935516) { - out_4666135608333935516[0] = state[0]; - out_4666135608333935516[1] = state[1]; - out_4666135608333935516[2] = state[2]; - out_4666135608333935516[3] = state[3]; - out_4666135608333935516[4] = state[4]; - out_4666135608333935516[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_4666135608333935516[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_4666135608333935516[7] = state[7]; - out_4666135608333935516[8] = state[8]; +void f_fun(double *state, double dt, double *out_5221835679523029107) { + out_5221835679523029107[0] = state[0]; + out_5221835679523029107[1] = state[1]; + out_5221835679523029107[2] = state[2]; + out_5221835679523029107[3] = state[3]; + out_5221835679523029107[4] = state[4]; + out_5221835679523029107[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_5221835679523029107[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_5221835679523029107[7] = state[7]; + out_5221835679523029107[8] = state[8]; } -void F_fun(double *state, double dt, double *out_2752692491938964849) { - out_2752692491938964849[0] = 1; - out_2752692491938964849[1] = 0; - out_2752692491938964849[2] = 0; - out_2752692491938964849[3] = 0; - out_2752692491938964849[4] = 0; - out_2752692491938964849[5] = 0; - out_2752692491938964849[6] = 0; - out_2752692491938964849[7] = 0; - out_2752692491938964849[8] = 0; - out_2752692491938964849[9] = 0; - out_2752692491938964849[10] = 1; - out_2752692491938964849[11] = 0; - out_2752692491938964849[12] = 0; - out_2752692491938964849[13] = 0; - out_2752692491938964849[14] = 0; - out_2752692491938964849[15] = 0; - out_2752692491938964849[16] = 0; - out_2752692491938964849[17] = 0; - out_2752692491938964849[18] = 0; - out_2752692491938964849[19] = 0; - out_2752692491938964849[20] = 1; - out_2752692491938964849[21] = 0; - out_2752692491938964849[22] = 0; - out_2752692491938964849[23] = 0; - out_2752692491938964849[24] = 0; - out_2752692491938964849[25] = 0; - out_2752692491938964849[26] = 0; - out_2752692491938964849[27] = 0; - out_2752692491938964849[28] = 0; - out_2752692491938964849[29] = 0; - out_2752692491938964849[30] = 1; - out_2752692491938964849[31] = 0; - out_2752692491938964849[32] = 0; - out_2752692491938964849[33] = 0; - out_2752692491938964849[34] = 0; - out_2752692491938964849[35] = 0; - out_2752692491938964849[36] = 0; - out_2752692491938964849[37] = 0; - out_2752692491938964849[38] = 0; - out_2752692491938964849[39] = 0; - out_2752692491938964849[40] = 1; - out_2752692491938964849[41] = 0; - out_2752692491938964849[42] = 0; - out_2752692491938964849[43] = 0; - out_2752692491938964849[44] = 0; - out_2752692491938964849[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_2752692491938964849[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_2752692491938964849[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2752692491938964849[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2752692491938964849[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_2752692491938964849[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_2752692491938964849[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_2752692491938964849[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_2752692491938964849[53] = -9.8100000000000005*dt; - out_2752692491938964849[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_2752692491938964849[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_2752692491938964849[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2752692491938964849[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2752692491938964849[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_2752692491938964849[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_2752692491938964849[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_2752692491938964849[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2752692491938964849[62] = 0; - out_2752692491938964849[63] = 0; - out_2752692491938964849[64] = 0; - out_2752692491938964849[65] = 0; - out_2752692491938964849[66] = 0; - out_2752692491938964849[67] = 0; - out_2752692491938964849[68] = 0; - out_2752692491938964849[69] = 0; - out_2752692491938964849[70] = 1; - out_2752692491938964849[71] = 0; - out_2752692491938964849[72] = 0; - out_2752692491938964849[73] = 0; - out_2752692491938964849[74] = 0; - out_2752692491938964849[75] = 0; - out_2752692491938964849[76] = 0; - out_2752692491938964849[77] = 0; - out_2752692491938964849[78] = 0; - out_2752692491938964849[79] = 0; - out_2752692491938964849[80] = 1; +void F_fun(double *state, double dt, double *out_1809598832794027362) { + out_1809598832794027362[0] = 1; + out_1809598832794027362[1] = 0; + out_1809598832794027362[2] = 0; + out_1809598832794027362[3] = 0; + out_1809598832794027362[4] = 0; + out_1809598832794027362[5] = 0; + out_1809598832794027362[6] = 0; + out_1809598832794027362[7] = 0; + out_1809598832794027362[8] = 0; + out_1809598832794027362[9] = 0; + out_1809598832794027362[10] = 1; + out_1809598832794027362[11] = 0; + out_1809598832794027362[12] = 0; + out_1809598832794027362[13] = 0; + out_1809598832794027362[14] = 0; + out_1809598832794027362[15] = 0; + out_1809598832794027362[16] = 0; + out_1809598832794027362[17] = 0; + out_1809598832794027362[18] = 0; + out_1809598832794027362[19] = 0; + out_1809598832794027362[20] = 1; + out_1809598832794027362[21] = 0; + out_1809598832794027362[22] = 0; + out_1809598832794027362[23] = 0; + out_1809598832794027362[24] = 0; + out_1809598832794027362[25] = 0; + out_1809598832794027362[26] = 0; + out_1809598832794027362[27] = 0; + out_1809598832794027362[28] = 0; + out_1809598832794027362[29] = 0; + out_1809598832794027362[30] = 1; + out_1809598832794027362[31] = 0; + out_1809598832794027362[32] = 0; + out_1809598832794027362[33] = 0; + out_1809598832794027362[34] = 0; + out_1809598832794027362[35] = 0; + out_1809598832794027362[36] = 0; + out_1809598832794027362[37] = 0; + out_1809598832794027362[38] = 0; + out_1809598832794027362[39] = 0; + out_1809598832794027362[40] = 1; + out_1809598832794027362[41] = 0; + out_1809598832794027362[42] = 0; + out_1809598832794027362[43] = 0; + out_1809598832794027362[44] = 0; + out_1809598832794027362[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_1809598832794027362[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_1809598832794027362[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_1809598832794027362[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_1809598832794027362[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_1809598832794027362[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_1809598832794027362[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_1809598832794027362[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_1809598832794027362[53] = -9.8100000000000005*dt; + out_1809598832794027362[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_1809598832794027362[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_1809598832794027362[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1809598832794027362[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1809598832794027362[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_1809598832794027362[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_1809598832794027362[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_1809598832794027362[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1809598832794027362[62] = 0; + out_1809598832794027362[63] = 0; + out_1809598832794027362[64] = 0; + out_1809598832794027362[65] = 0; + out_1809598832794027362[66] = 0; + out_1809598832794027362[67] = 0; + out_1809598832794027362[68] = 0; + out_1809598832794027362[69] = 0; + out_1809598832794027362[70] = 1; + out_1809598832794027362[71] = 0; + out_1809598832794027362[72] = 0; + out_1809598832794027362[73] = 0; + out_1809598832794027362[74] = 0; + out_1809598832794027362[75] = 0; + out_1809598832794027362[76] = 0; + out_1809598832794027362[77] = 0; + out_1809598832794027362[78] = 0; + out_1809598832794027362[79] = 0; + out_1809598832794027362[80] = 1; } -void h_25(double *state, double *unused, double *out_3787568519496882893) { - out_3787568519496882893[0] = state[6]; +void h_25(double *state, double *unused, double *out_1465140640781784100) { + out_1465140640781784100[0] = state[6]; } -void H_25(double *state, double *unused, double *out_3699858802985056505) { - out_3699858802985056505[0] = 0; - out_3699858802985056505[1] = 0; - out_3699858802985056505[2] = 0; - out_3699858802985056505[3] = 0; - out_3699858802985056505[4] = 0; - out_3699858802985056505[5] = 0; - out_3699858802985056505[6] = 1; - out_3699858802985056505[7] = 0; - out_3699858802985056505[8] = 0; +void H_25(double *state, double *unused, double *out_4563198394130328160) { + out_4563198394130328160[0] = 0; + out_4563198394130328160[1] = 0; + out_4563198394130328160[2] = 0; + out_4563198394130328160[3] = 0; + out_4563198394130328160[4] = 0; + out_4563198394130328160[5] = 0; + out_4563198394130328160[6] = 1; + out_4563198394130328160[7] = 0; + out_4563198394130328160[8] = 0; } -void h_24(double *state, double *unused, double *out_1528856480665249325) { - out_1528856480665249325[0] = state[4]; - out_1528856480665249325[1] = state[5]; +void h_24(double *state, double *unused, double *out_1585421017382478864) { + out_1585421017382478864[0] = state[4]; + out_1585421017382478864[1] = state[5]; } -void H_24(double *state, double *unused, double *out_1970995683807742436) { - out_1970995683807742436[0] = 0; - out_1970995683807742436[1] = 0; - out_1970995683807742436[2] = 0; - out_1970995683807742436[3] = 0; - out_1970995683807742436[4] = 1; - out_1970995683807742436[5] = 0; - out_1970995683807742436[6] = 0; - out_1970995683807742436[7] = 0; - out_1970995683807742436[8] = 0; - out_1970995683807742436[9] = 0; - out_1970995683807742436[10] = 0; - out_1970995683807742436[11] = 0; - out_1970995683807742436[12] = 0; - out_1970995683807742436[13] = 0; - out_1970995683807742436[14] = 1; - out_1970995683807742436[15] = 0; - out_1970995683807742436[16] = 0; - out_1970995683807742436[17] = 0; +void H_24(double *state, double *unused, double *out_6440871086590507136) { + out_6440871086590507136[0] = 0; + out_6440871086590507136[1] = 0; + out_6440871086590507136[2] = 0; + out_6440871086590507136[3] = 0; + out_6440871086590507136[4] = 1; + out_6440871086590507136[5] = 0; + out_6440871086590507136[6] = 0; + out_6440871086590507136[7] = 0; + out_6440871086590507136[8] = 0; + out_6440871086590507136[9] = 0; + out_6440871086590507136[10] = 0; + out_6440871086590507136[11] = 0; + out_6440871086590507136[12] = 0; + out_6440871086590507136[13] = 0; + out_6440871086590507136[14] = 1; + out_6440871086590507136[15] = 0; + out_6440871086590507136[16] = 0; + out_6440871086590507136[17] = 0; } -void h_30(double *state, double *unused, double *out_4709092161766857099) { - out_4709092161766857099[0] = state[4]; +void h_30(double *state, double *unused, double *out_1189946578497278211) { + out_1189946578497278211[0] = state[4]; } -void H_30(double *state, double *unused, double *out_7830194929232878356) { - out_7830194929232878356[0] = 0; - out_7830194929232878356[1] = 0; - out_7830194929232878356[2] = 0; - out_7830194929232878356[3] = 0; - out_7830194929232878356[4] = 1; - out_7830194929232878356[5] = 0; - out_7830194929232878356[6] = 0; - out_7830194929232878356[7] = 0; - out_7830194929232878356[8] = 0; +void H_30(double *state, double *unused, double *out_4692537341273568230) { + out_4692537341273568230[0] = 0; + out_4692537341273568230[1] = 0; + out_4692537341273568230[2] = 0; + out_4692537341273568230[3] = 0; + out_4692537341273568230[4] = 1; + out_4692537341273568230[5] = 0; + out_4692537341273568230[6] = 0; + out_4692537341273568230[7] = 0; + out_4692537341273568230[8] = 0; } -void h_26(double *state, double *unused, double *out_1815522905777326306) { - out_1815522905777326306[0] = state[7]; +void h_26(double *state, double *unused, double *out_1793262463173755728) { + out_1793262463173755728[0] = state[7]; } -void H_26(double *state, double *unused, double *out_41644515888999719) { - out_41644515888999719[0] = 0; - out_41644515888999719[1] = 0; - out_41644515888999719[2] = 0; - out_41644515888999719[3] = 0; - out_41644515888999719[4] = 0; - out_41644515888999719[5] = 0; - out_41644515888999719[6] = 0; - out_41644515888999719[7] = 1; - out_41644515888999719[8] = 0; +void H_26(double *state, double *unused, double *out_8304701713004384384) { + out_8304701713004384384[0] = 0; + out_8304701713004384384[1] = 0; + out_8304701713004384384[2] = 0; + out_8304701713004384384[3] = 0; + out_8304701713004384384[4] = 0; + out_8304701713004384384[5] = 0; + out_8304701713004384384[6] = 0; + out_8304701713004384384[7] = 1; + out_8304701713004384384[8] = 0; } -void h_27(double *state, double *unused, double *out_8789241787157728538) { - out_8789241787157728538[0] = state[3]; +void h_27(double *state, double *unused, double *out_7864182158195633246) { + out_7864182158195633246[0] = state[3]; } -void H_27(double *state, double *unused, double *out_8441785832676248349) { - out_8441785832676248349[0] = 0; - out_8441785832676248349[1] = 0; - out_8441785832676248349[2] = 0; - out_8441785832676248349[3] = 1; - out_8441785832676248349[4] = 0; - out_8441785832676248349[5] = 0; - out_8441785832676248349[6] = 0; - out_8441785832676248349[7] = 0; - out_8441785832676248349[8] = 0; +void H_27(double *state, double *unused, double *out_6867300653073993141) { + out_6867300653073993141[0] = 0; + out_6867300653073993141[1] = 0; + out_6867300653073993141[2] = 0; + out_6867300653073993141[3] = 1; + out_6867300653073993141[4] = 0; + out_6867300653073993141[5] = 0; + out_6867300653073993141[6] = 0; + out_6867300653073993141[7] = 0; + out_6867300653073993141[8] = 0; } -void h_29(double *state, double *unused, double *out_8252567880601862062) { - out_8252567880601862062[0] = state[1]; +void h_29(double *state, double *unused, double *out_4002498681767131594) { + out_4002498681767131594[0] = state[1]; } -void H_29(double *state, double *unused, double *out_7319963584918486172) { - out_7319963584918486172[0] = 0; - out_7319963584918486172[1] = 1; - out_7319963584918486172[2] = 0; - out_7319963584918486172[3] = 0; - out_7319963584918486172[4] = 0; - out_7319963584918486172[5] = 0; - out_7319963584918486172[6] = 0; - out_7319963584918486172[7] = 0; - out_7319963584918486172[8] = 0; +void H_29(double *state, double *unused, double *out_8580663379943544174) { + out_8580663379943544174[0] = 0; + out_8580663379943544174[1] = 1; + out_8580663379943544174[2] = 0; + out_8580663379943544174[3] = 0; + out_8580663379943544174[4] = 0; + out_8580663379943544174[5] = 0; + out_8580663379943544174[6] = 0; + out_8580663379943544174[7] = 0; + out_8580663379943544174[8] = 0; } -void h_28(double *state, double *unused, double *out_5372968497508658522) { - out_5372968497508658522[0] = state[0]; +void h_28(double *state, double *unused, double *out_8807692011097295681) { + out_8807692011097295681[0] = state[0]; } -void H_28(double *state, double *unused, double *out_6044381471721534870) { - out_6044381471721534870[0] = 1; - out_6044381471721534870[1] = 0; - out_6044381471721534870[2] = 0; - out_6044381471721534870[3] = 0; - out_6044381471721534870[4] = 0; - out_6044381471721534870[5] = 0; - out_6044381471721534870[6] = 0; - out_6044381471721534870[7] = 0; - out_6044381471721534870[8] = 0; +void H_28(double *state, double *unused, double *out_4783681676696476868) { + out_4783681676696476868[0] = 1; + out_4783681676696476868[1] = 0; + out_4783681676696476868[2] = 0; + out_4783681676696476868[3] = 0; + out_4783681676696476868[4] = 0; + out_4783681676696476868[5] = 0; + out_4783681676696476868[6] = 0; + out_4783681676696476868[7] = 0; + out_4783681676696476868[8] = 0; } -void h_31(double *state, double *unused, double *out_6895910297391996282) { - out_6895910297391996282[0] = state[8]; +void h_31(double *state, double *unused, double *out_7031520040481955892) { + out_7031520040481955892[0] = state[8]; } -void H_31(double *state, double *unused, double *out_3730504764862016933) { - out_3730504764862016933[0] = 0; - out_3730504764862016933[1] = 0; - out_3730504764862016933[2] = 0; - out_3730504764862016933[3] = 0; - out_3730504764862016933[4] = 0; - out_3730504764862016933[5] = 0; - out_3730504764862016933[6] = 0; - out_3730504764862016933[7] = 0; - out_3730504764862016933[8] = 1; +void H_31(double *state, double *unused, double *out_4532552432253367732) { + out_4532552432253367732[0] = 0; + out_4532552432253367732[1] = 0; + out_4532552432253367732[2] = 0; + out_4532552432253367732[3] = 0; + out_4532552432253367732[4] = 0; + out_4532552432253367732[5] = 0; + out_4532552432253367732[6] = 0; + out_4532552432253367732[7] = 0; + out_4532552432253367732[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_7149517118868685515) { - err_fun(nom_x, delta_x, out_7149517118868685515); +void car_err_fun(double *nom_x, double *delta_x, double *out_8318180392745555938) { + err_fun(nom_x, delta_x, out_8318180392745555938); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1206282320038144058) { - inv_err_fun(nom_x, true_x, out_1206282320038144058); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5934219001129015779) { + inv_err_fun(nom_x, true_x, out_5934219001129015779); } -void car_H_mod_fun(double *state, double *out_2536050837023062909) { - H_mod_fun(state, out_2536050837023062909); +void car_H_mod_fun(double *state, double *out_1926641469114901966) { + H_mod_fun(state, out_1926641469114901966); } -void car_f_fun(double *state, double dt, double *out_4666135608333935516) { - f_fun(state, dt, out_4666135608333935516); +void car_f_fun(double *state, double dt, double *out_5221835679523029107) { + f_fun(state, dt, out_5221835679523029107); } -void car_F_fun(double *state, double dt, double *out_2752692491938964849) { - F_fun(state, dt, out_2752692491938964849); +void car_F_fun(double *state, double dt, double *out_1809598832794027362) { + F_fun(state, dt, out_1809598832794027362); } -void car_h_25(double *state, double *unused, double *out_3787568519496882893) { - h_25(state, unused, out_3787568519496882893); +void car_h_25(double *state, double *unused, double *out_1465140640781784100) { + h_25(state, unused, out_1465140640781784100); } -void car_H_25(double *state, double *unused, double *out_3699858802985056505) { - H_25(state, unused, out_3699858802985056505); +void car_H_25(double *state, double *unused, double *out_4563198394130328160) { + H_25(state, unused, out_4563198394130328160); } -void car_h_24(double *state, double *unused, double *out_1528856480665249325) { - h_24(state, unused, out_1528856480665249325); +void car_h_24(double *state, double *unused, double *out_1585421017382478864) { + h_24(state, unused, out_1585421017382478864); } -void car_H_24(double *state, double *unused, double *out_1970995683807742436) { - H_24(state, unused, out_1970995683807742436); +void car_H_24(double *state, double *unused, double *out_6440871086590507136) { + H_24(state, unused, out_6440871086590507136); } -void car_h_30(double *state, double *unused, double *out_4709092161766857099) { - h_30(state, unused, out_4709092161766857099); +void car_h_30(double *state, double *unused, double *out_1189946578497278211) { + h_30(state, unused, out_1189946578497278211); } -void car_H_30(double *state, double *unused, double *out_7830194929232878356) { - H_30(state, unused, out_7830194929232878356); +void car_H_30(double *state, double *unused, double *out_4692537341273568230) { + H_30(state, unused, out_4692537341273568230); } -void car_h_26(double *state, double *unused, double *out_1815522905777326306) { - h_26(state, unused, out_1815522905777326306); +void car_h_26(double *state, double *unused, double *out_1793262463173755728) { + h_26(state, unused, out_1793262463173755728); } -void car_H_26(double *state, double *unused, double *out_41644515888999719) { - H_26(state, unused, out_41644515888999719); +void car_H_26(double *state, double *unused, double *out_8304701713004384384) { + H_26(state, unused, out_8304701713004384384); } -void car_h_27(double *state, double *unused, double *out_8789241787157728538) { - h_27(state, unused, out_8789241787157728538); +void car_h_27(double *state, double *unused, double *out_7864182158195633246) { + h_27(state, unused, out_7864182158195633246); } -void car_H_27(double *state, double *unused, double *out_8441785832676248349) { - H_27(state, unused, out_8441785832676248349); +void car_H_27(double *state, double *unused, double *out_6867300653073993141) { + H_27(state, unused, out_6867300653073993141); } -void car_h_29(double *state, double *unused, double *out_8252567880601862062) { - h_29(state, unused, out_8252567880601862062); +void car_h_29(double *state, double *unused, double *out_4002498681767131594) { + h_29(state, unused, out_4002498681767131594); } -void car_H_29(double *state, double *unused, double *out_7319963584918486172) { - H_29(state, unused, out_7319963584918486172); +void car_H_29(double *state, double *unused, double *out_8580663379943544174) { + H_29(state, unused, out_8580663379943544174); } -void car_h_28(double *state, double *unused, double *out_5372968497508658522) { - h_28(state, unused, out_5372968497508658522); +void car_h_28(double *state, double *unused, double *out_8807692011097295681) { + h_28(state, unused, out_8807692011097295681); } -void car_H_28(double *state, double *unused, double *out_6044381471721534870) { - H_28(state, unused, out_6044381471721534870); +void car_H_28(double *state, double *unused, double *out_4783681676696476868) { + H_28(state, unused, out_4783681676696476868); } -void car_h_31(double *state, double *unused, double *out_6895910297391996282) { - h_31(state, unused, out_6895910297391996282); +void car_h_31(double *state, double *unused, double *out_7031520040481955892) { + h_31(state, unused, out_7031520040481955892); } -void car_H_31(double *state, double *unused, double *out_3730504764862016933) { - H_31(state, unused, out_3730504764862016933); +void car_H_31(double *state, double *unused, double *out_4532552432253367732) { + H_31(state, unused, out_4532552432253367732); } 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 6b5119a70..a5bfcbd79 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_7149517118868685515); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1206282320038144058); -void car_H_mod_fun(double *state, double *out_2536050837023062909); -void car_f_fun(double *state, double dt, double *out_4666135608333935516); -void car_F_fun(double *state, double dt, double *out_2752692491938964849); -void car_h_25(double *state, double *unused, double *out_3787568519496882893); -void car_H_25(double *state, double *unused, double *out_3699858802985056505); -void car_h_24(double *state, double *unused, double *out_1528856480665249325); -void car_H_24(double *state, double *unused, double *out_1970995683807742436); -void car_h_30(double *state, double *unused, double *out_4709092161766857099); -void car_H_30(double *state, double *unused, double *out_7830194929232878356); -void car_h_26(double *state, double *unused, double *out_1815522905777326306); -void car_H_26(double *state, double *unused, double *out_41644515888999719); -void car_h_27(double *state, double *unused, double *out_8789241787157728538); -void car_H_27(double *state, double *unused, double *out_8441785832676248349); -void car_h_29(double *state, double *unused, double *out_8252567880601862062); -void car_H_29(double *state, double *unused, double *out_7319963584918486172); -void car_h_28(double *state, double *unused, double *out_5372968497508658522); -void car_H_28(double *state, double *unused, double *out_6044381471721534870); -void car_h_31(double *state, double *unused, double *out_6895910297391996282); -void car_H_31(double *state, double *unused, double *out_3730504764862016933); +void car_err_fun(double *nom_x, double *delta_x, double *out_8318180392745555938); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5934219001129015779); +void car_H_mod_fun(double *state, double *out_1926641469114901966); +void car_f_fun(double *state, double dt, double *out_5221835679523029107); +void car_F_fun(double *state, double dt, double *out_1809598832794027362); +void car_h_25(double *state, double *unused, double *out_1465140640781784100); +void car_H_25(double *state, double *unused, double *out_4563198394130328160); +void car_h_24(double *state, double *unused, double *out_1585421017382478864); +void car_H_24(double *state, double *unused, double *out_6440871086590507136); +void car_h_30(double *state, double *unused, double *out_1189946578497278211); +void car_H_30(double *state, double *unused, double *out_4692537341273568230); +void car_h_26(double *state, double *unused, double *out_1793262463173755728); +void car_H_26(double *state, double *unused, double *out_8304701713004384384); +void car_h_27(double *state, double *unused, double *out_7864182158195633246); +void car_H_27(double *state, double *unused, double *out_6867300653073993141); +void car_h_29(double *state, double *unused, double *out_4002498681767131594); +void car_H_29(double *state, double *unused, double *out_8580663379943544174); +void car_h_28(double *state, double *unused, double *out_8807692011097295681); +void car_H_28(double *state, double *unused, double *out_4783681676696476868); +void car_h_31(double *state, double *unused, double *out_7031520040481955892); +void car_H_31(double *state, double *unused, double *out_4532552432253367732); 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 67ac28975..863baa776 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_8259662850093450970) { - out_8259662850093450970[0] = delta_x[0] + nom_x[0]; - out_8259662850093450970[1] = delta_x[1] + nom_x[1]; - out_8259662850093450970[2] = delta_x[2] + nom_x[2]; - out_8259662850093450970[3] = delta_x[3] + nom_x[3]; - out_8259662850093450970[4] = delta_x[4] + nom_x[4]; - out_8259662850093450970[5] = delta_x[5] + nom_x[5]; - out_8259662850093450970[6] = delta_x[6] + nom_x[6]; - out_8259662850093450970[7] = delta_x[7] + nom_x[7]; - out_8259662850093450970[8] = delta_x[8] + nom_x[8]; - out_8259662850093450970[9] = delta_x[9] + nom_x[9]; - out_8259662850093450970[10] = delta_x[10] + nom_x[10]; - out_8259662850093450970[11] = delta_x[11] + nom_x[11]; - out_8259662850093450970[12] = delta_x[12] + nom_x[12]; - out_8259662850093450970[13] = delta_x[13] + nom_x[13]; - out_8259662850093450970[14] = delta_x[14] + nom_x[14]; - out_8259662850093450970[15] = delta_x[15] + nom_x[15]; - out_8259662850093450970[16] = delta_x[16] + nom_x[16]; - out_8259662850093450970[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7993353667610184364) { + out_7993353667610184364[0] = delta_x[0] + nom_x[0]; + out_7993353667610184364[1] = delta_x[1] + nom_x[1]; + out_7993353667610184364[2] = delta_x[2] + nom_x[2]; + out_7993353667610184364[3] = delta_x[3] + nom_x[3]; + out_7993353667610184364[4] = delta_x[4] + nom_x[4]; + out_7993353667610184364[5] = delta_x[5] + nom_x[5]; + out_7993353667610184364[6] = delta_x[6] + nom_x[6]; + out_7993353667610184364[7] = delta_x[7] + nom_x[7]; + out_7993353667610184364[8] = delta_x[8] + nom_x[8]; + out_7993353667610184364[9] = delta_x[9] + nom_x[9]; + out_7993353667610184364[10] = delta_x[10] + nom_x[10]; + out_7993353667610184364[11] = delta_x[11] + nom_x[11]; + out_7993353667610184364[12] = delta_x[12] + nom_x[12]; + out_7993353667610184364[13] = delta_x[13] + nom_x[13]; + out_7993353667610184364[14] = delta_x[14] + nom_x[14]; + out_7993353667610184364[15] = delta_x[15] + nom_x[15]; + out_7993353667610184364[16] = delta_x[16] + nom_x[16]; + out_7993353667610184364[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7477554158578246383) { - out_7477554158578246383[0] = -nom_x[0] + true_x[0]; - out_7477554158578246383[1] = -nom_x[1] + true_x[1]; - out_7477554158578246383[2] = -nom_x[2] + true_x[2]; - out_7477554158578246383[3] = -nom_x[3] + true_x[3]; - out_7477554158578246383[4] = -nom_x[4] + true_x[4]; - out_7477554158578246383[5] = -nom_x[5] + true_x[5]; - out_7477554158578246383[6] = -nom_x[6] + true_x[6]; - out_7477554158578246383[7] = -nom_x[7] + true_x[7]; - out_7477554158578246383[8] = -nom_x[8] + true_x[8]; - out_7477554158578246383[9] = -nom_x[9] + true_x[9]; - out_7477554158578246383[10] = -nom_x[10] + true_x[10]; - out_7477554158578246383[11] = -nom_x[11] + true_x[11]; - out_7477554158578246383[12] = -nom_x[12] + true_x[12]; - out_7477554158578246383[13] = -nom_x[13] + true_x[13]; - out_7477554158578246383[14] = -nom_x[14] + true_x[14]; - out_7477554158578246383[15] = -nom_x[15] + true_x[15]; - out_7477554158578246383[16] = -nom_x[16] + true_x[16]; - out_7477554158578246383[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_533608207331381553) { + out_533608207331381553[0] = -nom_x[0] + true_x[0]; + out_533608207331381553[1] = -nom_x[1] + true_x[1]; + out_533608207331381553[2] = -nom_x[2] + true_x[2]; + out_533608207331381553[3] = -nom_x[3] + true_x[3]; + out_533608207331381553[4] = -nom_x[4] + true_x[4]; + out_533608207331381553[5] = -nom_x[5] + true_x[5]; + out_533608207331381553[6] = -nom_x[6] + true_x[6]; + out_533608207331381553[7] = -nom_x[7] + true_x[7]; + out_533608207331381553[8] = -nom_x[8] + true_x[8]; + out_533608207331381553[9] = -nom_x[9] + true_x[9]; + out_533608207331381553[10] = -nom_x[10] + true_x[10]; + out_533608207331381553[11] = -nom_x[11] + true_x[11]; + out_533608207331381553[12] = -nom_x[12] + true_x[12]; + out_533608207331381553[13] = -nom_x[13] + true_x[13]; + out_533608207331381553[14] = -nom_x[14] + true_x[14]; + out_533608207331381553[15] = -nom_x[15] + true_x[15]; + out_533608207331381553[16] = -nom_x[16] + true_x[16]; + out_533608207331381553[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_6622639630704307229) { - out_6622639630704307229[0] = 1.0; - out_6622639630704307229[1] = 0.0; - out_6622639630704307229[2] = 0.0; - out_6622639630704307229[3] = 0.0; - out_6622639630704307229[4] = 0.0; - out_6622639630704307229[5] = 0.0; - out_6622639630704307229[6] = 0.0; - out_6622639630704307229[7] = 0.0; - out_6622639630704307229[8] = 0.0; - out_6622639630704307229[9] = 0.0; - out_6622639630704307229[10] = 0.0; - out_6622639630704307229[11] = 0.0; - out_6622639630704307229[12] = 0.0; - out_6622639630704307229[13] = 0.0; - out_6622639630704307229[14] = 0.0; - out_6622639630704307229[15] = 0.0; - out_6622639630704307229[16] = 0.0; - out_6622639630704307229[17] = 0.0; - out_6622639630704307229[18] = 0.0; - out_6622639630704307229[19] = 1.0; - out_6622639630704307229[20] = 0.0; - out_6622639630704307229[21] = 0.0; - out_6622639630704307229[22] = 0.0; - out_6622639630704307229[23] = 0.0; - out_6622639630704307229[24] = 0.0; - out_6622639630704307229[25] = 0.0; - out_6622639630704307229[26] = 0.0; - out_6622639630704307229[27] = 0.0; - out_6622639630704307229[28] = 0.0; - out_6622639630704307229[29] = 0.0; - out_6622639630704307229[30] = 0.0; - out_6622639630704307229[31] = 0.0; - out_6622639630704307229[32] = 0.0; - out_6622639630704307229[33] = 0.0; - out_6622639630704307229[34] = 0.0; - out_6622639630704307229[35] = 0.0; - out_6622639630704307229[36] = 0.0; - out_6622639630704307229[37] = 0.0; - out_6622639630704307229[38] = 1.0; - out_6622639630704307229[39] = 0.0; - out_6622639630704307229[40] = 0.0; - out_6622639630704307229[41] = 0.0; - out_6622639630704307229[42] = 0.0; - out_6622639630704307229[43] = 0.0; - out_6622639630704307229[44] = 0.0; - out_6622639630704307229[45] = 0.0; - out_6622639630704307229[46] = 0.0; - out_6622639630704307229[47] = 0.0; - out_6622639630704307229[48] = 0.0; - out_6622639630704307229[49] = 0.0; - out_6622639630704307229[50] = 0.0; - out_6622639630704307229[51] = 0.0; - out_6622639630704307229[52] = 0.0; - out_6622639630704307229[53] = 0.0; - out_6622639630704307229[54] = 0.0; - out_6622639630704307229[55] = 0.0; - out_6622639630704307229[56] = 0.0; - out_6622639630704307229[57] = 1.0; - out_6622639630704307229[58] = 0.0; - out_6622639630704307229[59] = 0.0; - out_6622639630704307229[60] = 0.0; - out_6622639630704307229[61] = 0.0; - out_6622639630704307229[62] = 0.0; - out_6622639630704307229[63] = 0.0; - out_6622639630704307229[64] = 0.0; - out_6622639630704307229[65] = 0.0; - out_6622639630704307229[66] = 0.0; - out_6622639630704307229[67] = 0.0; - out_6622639630704307229[68] = 0.0; - out_6622639630704307229[69] = 0.0; - out_6622639630704307229[70] = 0.0; - out_6622639630704307229[71] = 0.0; - out_6622639630704307229[72] = 0.0; - out_6622639630704307229[73] = 0.0; - out_6622639630704307229[74] = 0.0; - out_6622639630704307229[75] = 0.0; - out_6622639630704307229[76] = 1.0; - out_6622639630704307229[77] = 0.0; - out_6622639630704307229[78] = 0.0; - out_6622639630704307229[79] = 0.0; - out_6622639630704307229[80] = 0.0; - out_6622639630704307229[81] = 0.0; - out_6622639630704307229[82] = 0.0; - out_6622639630704307229[83] = 0.0; - out_6622639630704307229[84] = 0.0; - out_6622639630704307229[85] = 0.0; - out_6622639630704307229[86] = 0.0; - out_6622639630704307229[87] = 0.0; - out_6622639630704307229[88] = 0.0; - out_6622639630704307229[89] = 0.0; - out_6622639630704307229[90] = 0.0; - out_6622639630704307229[91] = 0.0; - out_6622639630704307229[92] = 0.0; - out_6622639630704307229[93] = 0.0; - out_6622639630704307229[94] = 0.0; - out_6622639630704307229[95] = 1.0; - out_6622639630704307229[96] = 0.0; - out_6622639630704307229[97] = 0.0; - out_6622639630704307229[98] = 0.0; - out_6622639630704307229[99] = 0.0; - out_6622639630704307229[100] = 0.0; - out_6622639630704307229[101] = 0.0; - out_6622639630704307229[102] = 0.0; - out_6622639630704307229[103] = 0.0; - out_6622639630704307229[104] = 0.0; - out_6622639630704307229[105] = 0.0; - out_6622639630704307229[106] = 0.0; - out_6622639630704307229[107] = 0.0; - out_6622639630704307229[108] = 0.0; - out_6622639630704307229[109] = 0.0; - out_6622639630704307229[110] = 0.0; - out_6622639630704307229[111] = 0.0; - out_6622639630704307229[112] = 0.0; - out_6622639630704307229[113] = 0.0; - out_6622639630704307229[114] = 1.0; - out_6622639630704307229[115] = 0.0; - out_6622639630704307229[116] = 0.0; - out_6622639630704307229[117] = 0.0; - out_6622639630704307229[118] = 0.0; - out_6622639630704307229[119] = 0.0; - out_6622639630704307229[120] = 0.0; - out_6622639630704307229[121] = 0.0; - out_6622639630704307229[122] = 0.0; - out_6622639630704307229[123] = 0.0; - out_6622639630704307229[124] = 0.0; - out_6622639630704307229[125] = 0.0; - out_6622639630704307229[126] = 0.0; - out_6622639630704307229[127] = 0.0; - out_6622639630704307229[128] = 0.0; - out_6622639630704307229[129] = 0.0; - out_6622639630704307229[130] = 0.0; - out_6622639630704307229[131] = 0.0; - out_6622639630704307229[132] = 0.0; - out_6622639630704307229[133] = 1.0; - out_6622639630704307229[134] = 0.0; - out_6622639630704307229[135] = 0.0; - out_6622639630704307229[136] = 0.0; - out_6622639630704307229[137] = 0.0; - out_6622639630704307229[138] = 0.0; - out_6622639630704307229[139] = 0.0; - out_6622639630704307229[140] = 0.0; - out_6622639630704307229[141] = 0.0; - out_6622639630704307229[142] = 0.0; - out_6622639630704307229[143] = 0.0; - out_6622639630704307229[144] = 0.0; - out_6622639630704307229[145] = 0.0; - out_6622639630704307229[146] = 0.0; - out_6622639630704307229[147] = 0.0; - out_6622639630704307229[148] = 0.0; - out_6622639630704307229[149] = 0.0; - out_6622639630704307229[150] = 0.0; - out_6622639630704307229[151] = 0.0; - out_6622639630704307229[152] = 1.0; - out_6622639630704307229[153] = 0.0; - out_6622639630704307229[154] = 0.0; - out_6622639630704307229[155] = 0.0; - out_6622639630704307229[156] = 0.0; - out_6622639630704307229[157] = 0.0; - out_6622639630704307229[158] = 0.0; - out_6622639630704307229[159] = 0.0; - out_6622639630704307229[160] = 0.0; - out_6622639630704307229[161] = 0.0; - out_6622639630704307229[162] = 0.0; - out_6622639630704307229[163] = 0.0; - out_6622639630704307229[164] = 0.0; - out_6622639630704307229[165] = 0.0; - out_6622639630704307229[166] = 0.0; - out_6622639630704307229[167] = 0.0; - out_6622639630704307229[168] = 0.0; - out_6622639630704307229[169] = 0.0; - out_6622639630704307229[170] = 0.0; - out_6622639630704307229[171] = 1.0; - out_6622639630704307229[172] = 0.0; - out_6622639630704307229[173] = 0.0; - out_6622639630704307229[174] = 0.0; - out_6622639630704307229[175] = 0.0; - out_6622639630704307229[176] = 0.0; - out_6622639630704307229[177] = 0.0; - out_6622639630704307229[178] = 0.0; - out_6622639630704307229[179] = 0.0; - out_6622639630704307229[180] = 0.0; - out_6622639630704307229[181] = 0.0; - out_6622639630704307229[182] = 0.0; - out_6622639630704307229[183] = 0.0; - out_6622639630704307229[184] = 0.0; - out_6622639630704307229[185] = 0.0; - out_6622639630704307229[186] = 0.0; - out_6622639630704307229[187] = 0.0; - out_6622639630704307229[188] = 0.0; - out_6622639630704307229[189] = 0.0; - out_6622639630704307229[190] = 1.0; - out_6622639630704307229[191] = 0.0; - out_6622639630704307229[192] = 0.0; - out_6622639630704307229[193] = 0.0; - out_6622639630704307229[194] = 0.0; - out_6622639630704307229[195] = 0.0; - out_6622639630704307229[196] = 0.0; - out_6622639630704307229[197] = 0.0; - out_6622639630704307229[198] = 0.0; - out_6622639630704307229[199] = 0.0; - out_6622639630704307229[200] = 0.0; - out_6622639630704307229[201] = 0.0; - out_6622639630704307229[202] = 0.0; - out_6622639630704307229[203] = 0.0; - out_6622639630704307229[204] = 0.0; - out_6622639630704307229[205] = 0.0; - out_6622639630704307229[206] = 0.0; - out_6622639630704307229[207] = 0.0; - out_6622639630704307229[208] = 0.0; - out_6622639630704307229[209] = 1.0; - out_6622639630704307229[210] = 0.0; - out_6622639630704307229[211] = 0.0; - out_6622639630704307229[212] = 0.0; - out_6622639630704307229[213] = 0.0; - out_6622639630704307229[214] = 0.0; - out_6622639630704307229[215] = 0.0; - out_6622639630704307229[216] = 0.0; - out_6622639630704307229[217] = 0.0; - out_6622639630704307229[218] = 0.0; - out_6622639630704307229[219] = 0.0; - out_6622639630704307229[220] = 0.0; - out_6622639630704307229[221] = 0.0; - out_6622639630704307229[222] = 0.0; - out_6622639630704307229[223] = 0.0; - out_6622639630704307229[224] = 0.0; - out_6622639630704307229[225] = 0.0; - out_6622639630704307229[226] = 0.0; - out_6622639630704307229[227] = 0.0; - out_6622639630704307229[228] = 1.0; - out_6622639630704307229[229] = 0.0; - out_6622639630704307229[230] = 0.0; - out_6622639630704307229[231] = 0.0; - out_6622639630704307229[232] = 0.0; - out_6622639630704307229[233] = 0.0; - out_6622639630704307229[234] = 0.0; - out_6622639630704307229[235] = 0.0; - out_6622639630704307229[236] = 0.0; - out_6622639630704307229[237] = 0.0; - out_6622639630704307229[238] = 0.0; - out_6622639630704307229[239] = 0.0; - out_6622639630704307229[240] = 0.0; - out_6622639630704307229[241] = 0.0; - out_6622639630704307229[242] = 0.0; - out_6622639630704307229[243] = 0.0; - out_6622639630704307229[244] = 0.0; - out_6622639630704307229[245] = 0.0; - out_6622639630704307229[246] = 0.0; - out_6622639630704307229[247] = 1.0; - out_6622639630704307229[248] = 0.0; - out_6622639630704307229[249] = 0.0; - out_6622639630704307229[250] = 0.0; - out_6622639630704307229[251] = 0.0; - out_6622639630704307229[252] = 0.0; - out_6622639630704307229[253] = 0.0; - out_6622639630704307229[254] = 0.0; - out_6622639630704307229[255] = 0.0; - out_6622639630704307229[256] = 0.0; - out_6622639630704307229[257] = 0.0; - out_6622639630704307229[258] = 0.0; - out_6622639630704307229[259] = 0.0; - out_6622639630704307229[260] = 0.0; - out_6622639630704307229[261] = 0.0; - out_6622639630704307229[262] = 0.0; - out_6622639630704307229[263] = 0.0; - out_6622639630704307229[264] = 0.0; - out_6622639630704307229[265] = 0.0; - out_6622639630704307229[266] = 1.0; - out_6622639630704307229[267] = 0.0; - out_6622639630704307229[268] = 0.0; - out_6622639630704307229[269] = 0.0; - out_6622639630704307229[270] = 0.0; - out_6622639630704307229[271] = 0.0; - out_6622639630704307229[272] = 0.0; - out_6622639630704307229[273] = 0.0; - out_6622639630704307229[274] = 0.0; - out_6622639630704307229[275] = 0.0; - out_6622639630704307229[276] = 0.0; - out_6622639630704307229[277] = 0.0; - out_6622639630704307229[278] = 0.0; - out_6622639630704307229[279] = 0.0; - out_6622639630704307229[280] = 0.0; - out_6622639630704307229[281] = 0.0; - out_6622639630704307229[282] = 0.0; - out_6622639630704307229[283] = 0.0; - out_6622639630704307229[284] = 0.0; - out_6622639630704307229[285] = 1.0; - out_6622639630704307229[286] = 0.0; - out_6622639630704307229[287] = 0.0; - out_6622639630704307229[288] = 0.0; - out_6622639630704307229[289] = 0.0; - out_6622639630704307229[290] = 0.0; - out_6622639630704307229[291] = 0.0; - out_6622639630704307229[292] = 0.0; - out_6622639630704307229[293] = 0.0; - out_6622639630704307229[294] = 0.0; - out_6622639630704307229[295] = 0.0; - out_6622639630704307229[296] = 0.0; - out_6622639630704307229[297] = 0.0; - out_6622639630704307229[298] = 0.0; - out_6622639630704307229[299] = 0.0; - out_6622639630704307229[300] = 0.0; - out_6622639630704307229[301] = 0.0; - out_6622639630704307229[302] = 0.0; - out_6622639630704307229[303] = 0.0; - out_6622639630704307229[304] = 1.0; - out_6622639630704307229[305] = 0.0; - out_6622639630704307229[306] = 0.0; - out_6622639630704307229[307] = 0.0; - out_6622639630704307229[308] = 0.0; - out_6622639630704307229[309] = 0.0; - out_6622639630704307229[310] = 0.0; - out_6622639630704307229[311] = 0.0; - out_6622639630704307229[312] = 0.0; - out_6622639630704307229[313] = 0.0; - out_6622639630704307229[314] = 0.0; - out_6622639630704307229[315] = 0.0; - out_6622639630704307229[316] = 0.0; - out_6622639630704307229[317] = 0.0; - out_6622639630704307229[318] = 0.0; - out_6622639630704307229[319] = 0.0; - out_6622639630704307229[320] = 0.0; - out_6622639630704307229[321] = 0.0; - out_6622639630704307229[322] = 0.0; - out_6622639630704307229[323] = 1.0; +void H_mod_fun(double *state, double *out_5230913290931847385) { + out_5230913290931847385[0] = 1.0; + out_5230913290931847385[1] = 0.0; + out_5230913290931847385[2] = 0.0; + out_5230913290931847385[3] = 0.0; + out_5230913290931847385[4] = 0.0; + out_5230913290931847385[5] = 0.0; + out_5230913290931847385[6] = 0.0; + out_5230913290931847385[7] = 0.0; + out_5230913290931847385[8] = 0.0; + out_5230913290931847385[9] = 0.0; + out_5230913290931847385[10] = 0.0; + out_5230913290931847385[11] = 0.0; + out_5230913290931847385[12] = 0.0; + out_5230913290931847385[13] = 0.0; + out_5230913290931847385[14] = 0.0; + out_5230913290931847385[15] = 0.0; + out_5230913290931847385[16] = 0.0; + out_5230913290931847385[17] = 0.0; + out_5230913290931847385[18] = 0.0; + out_5230913290931847385[19] = 1.0; + out_5230913290931847385[20] = 0.0; + out_5230913290931847385[21] = 0.0; + out_5230913290931847385[22] = 0.0; + out_5230913290931847385[23] = 0.0; + out_5230913290931847385[24] = 0.0; + out_5230913290931847385[25] = 0.0; + out_5230913290931847385[26] = 0.0; + out_5230913290931847385[27] = 0.0; + out_5230913290931847385[28] = 0.0; + out_5230913290931847385[29] = 0.0; + out_5230913290931847385[30] = 0.0; + out_5230913290931847385[31] = 0.0; + out_5230913290931847385[32] = 0.0; + out_5230913290931847385[33] = 0.0; + out_5230913290931847385[34] = 0.0; + out_5230913290931847385[35] = 0.0; + out_5230913290931847385[36] = 0.0; + out_5230913290931847385[37] = 0.0; + out_5230913290931847385[38] = 1.0; + out_5230913290931847385[39] = 0.0; + out_5230913290931847385[40] = 0.0; + out_5230913290931847385[41] = 0.0; + out_5230913290931847385[42] = 0.0; + out_5230913290931847385[43] = 0.0; + out_5230913290931847385[44] = 0.0; + out_5230913290931847385[45] = 0.0; + out_5230913290931847385[46] = 0.0; + out_5230913290931847385[47] = 0.0; + out_5230913290931847385[48] = 0.0; + out_5230913290931847385[49] = 0.0; + out_5230913290931847385[50] = 0.0; + out_5230913290931847385[51] = 0.0; + out_5230913290931847385[52] = 0.0; + out_5230913290931847385[53] = 0.0; + out_5230913290931847385[54] = 0.0; + out_5230913290931847385[55] = 0.0; + out_5230913290931847385[56] = 0.0; + out_5230913290931847385[57] = 1.0; + out_5230913290931847385[58] = 0.0; + out_5230913290931847385[59] = 0.0; + out_5230913290931847385[60] = 0.0; + out_5230913290931847385[61] = 0.0; + out_5230913290931847385[62] = 0.0; + out_5230913290931847385[63] = 0.0; + out_5230913290931847385[64] = 0.0; + out_5230913290931847385[65] = 0.0; + out_5230913290931847385[66] = 0.0; + out_5230913290931847385[67] = 0.0; + out_5230913290931847385[68] = 0.0; + out_5230913290931847385[69] = 0.0; + out_5230913290931847385[70] = 0.0; + out_5230913290931847385[71] = 0.0; + out_5230913290931847385[72] = 0.0; + out_5230913290931847385[73] = 0.0; + out_5230913290931847385[74] = 0.0; + out_5230913290931847385[75] = 0.0; + out_5230913290931847385[76] = 1.0; + out_5230913290931847385[77] = 0.0; + out_5230913290931847385[78] = 0.0; + out_5230913290931847385[79] = 0.0; + out_5230913290931847385[80] = 0.0; + out_5230913290931847385[81] = 0.0; + out_5230913290931847385[82] = 0.0; + out_5230913290931847385[83] = 0.0; + out_5230913290931847385[84] = 0.0; + out_5230913290931847385[85] = 0.0; + out_5230913290931847385[86] = 0.0; + out_5230913290931847385[87] = 0.0; + out_5230913290931847385[88] = 0.0; + out_5230913290931847385[89] = 0.0; + out_5230913290931847385[90] = 0.0; + out_5230913290931847385[91] = 0.0; + out_5230913290931847385[92] = 0.0; + out_5230913290931847385[93] = 0.0; + out_5230913290931847385[94] = 0.0; + out_5230913290931847385[95] = 1.0; + out_5230913290931847385[96] = 0.0; + out_5230913290931847385[97] = 0.0; + out_5230913290931847385[98] = 0.0; + out_5230913290931847385[99] = 0.0; + out_5230913290931847385[100] = 0.0; + out_5230913290931847385[101] = 0.0; + out_5230913290931847385[102] = 0.0; + out_5230913290931847385[103] = 0.0; + out_5230913290931847385[104] = 0.0; + out_5230913290931847385[105] = 0.0; + out_5230913290931847385[106] = 0.0; + out_5230913290931847385[107] = 0.0; + out_5230913290931847385[108] = 0.0; + out_5230913290931847385[109] = 0.0; + out_5230913290931847385[110] = 0.0; + out_5230913290931847385[111] = 0.0; + out_5230913290931847385[112] = 0.0; + out_5230913290931847385[113] = 0.0; + out_5230913290931847385[114] = 1.0; + out_5230913290931847385[115] = 0.0; + out_5230913290931847385[116] = 0.0; + out_5230913290931847385[117] = 0.0; + out_5230913290931847385[118] = 0.0; + out_5230913290931847385[119] = 0.0; + out_5230913290931847385[120] = 0.0; + out_5230913290931847385[121] = 0.0; + out_5230913290931847385[122] = 0.0; + out_5230913290931847385[123] = 0.0; + out_5230913290931847385[124] = 0.0; + out_5230913290931847385[125] = 0.0; + out_5230913290931847385[126] = 0.0; + out_5230913290931847385[127] = 0.0; + out_5230913290931847385[128] = 0.0; + out_5230913290931847385[129] = 0.0; + out_5230913290931847385[130] = 0.0; + out_5230913290931847385[131] = 0.0; + out_5230913290931847385[132] = 0.0; + out_5230913290931847385[133] = 1.0; + out_5230913290931847385[134] = 0.0; + out_5230913290931847385[135] = 0.0; + out_5230913290931847385[136] = 0.0; + out_5230913290931847385[137] = 0.0; + out_5230913290931847385[138] = 0.0; + out_5230913290931847385[139] = 0.0; + out_5230913290931847385[140] = 0.0; + out_5230913290931847385[141] = 0.0; + out_5230913290931847385[142] = 0.0; + out_5230913290931847385[143] = 0.0; + out_5230913290931847385[144] = 0.0; + out_5230913290931847385[145] = 0.0; + out_5230913290931847385[146] = 0.0; + out_5230913290931847385[147] = 0.0; + out_5230913290931847385[148] = 0.0; + out_5230913290931847385[149] = 0.0; + out_5230913290931847385[150] = 0.0; + out_5230913290931847385[151] = 0.0; + out_5230913290931847385[152] = 1.0; + out_5230913290931847385[153] = 0.0; + out_5230913290931847385[154] = 0.0; + out_5230913290931847385[155] = 0.0; + out_5230913290931847385[156] = 0.0; + out_5230913290931847385[157] = 0.0; + out_5230913290931847385[158] = 0.0; + out_5230913290931847385[159] = 0.0; + out_5230913290931847385[160] = 0.0; + out_5230913290931847385[161] = 0.0; + out_5230913290931847385[162] = 0.0; + out_5230913290931847385[163] = 0.0; + out_5230913290931847385[164] = 0.0; + out_5230913290931847385[165] = 0.0; + out_5230913290931847385[166] = 0.0; + out_5230913290931847385[167] = 0.0; + out_5230913290931847385[168] = 0.0; + out_5230913290931847385[169] = 0.0; + out_5230913290931847385[170] = 0.0; + out_5230913290931847385[171] = 1.0; + out_5230913290931847385[172] = 0.0; + out_5230913290931847385[173] = 0.0; + out_5230913290931847385[174] = 0.0; + out_5230913290931847385[175] = 0.0; + out_5230913290931847385[176] = 0.0; + out_5230913290931847385[177] = 0.0; + out_5230913290931847385[178] = 0.0; + out_5230913290931847385[179] = 0.0; + out_5230913290931847385[180] = 0.0; + out_5230913290931847385[181] = 0.0; + out_5230913290931847385[182] = 0.0; + out_5230913290931847385[183] = 0.0; + out_5230913290931847385[184] = 0.0; + out_5230913290931847385[185] = 0.0; + out_5230913290931847385[186] = 0.0; + out_5230913290931847385[187] = 0.0; + out_5230913290931847385[188] = 0.0; + out_5230913290931847385[189] = 0.0; + out_5230913290931847385[190] = 1.0; + out_5230913290931847385[191] = 0.0; + out_5230913290931847385[192] = 0.0; + out_5230913290931847385[193] = 0.0; + out_5230913290931847385[194] = 0.0; + out_5230913290931847385[195] = 0.0; + out_5230913290931847385[196] = 0.0; + out_5230913290931847385[197] = 0.0; + out_5230913290931847385[198] = 0.0; + out_5230913290931847385[199] = 0.0; + out_5230913290931847385[200] = 0.0; + out_5230913290931847385[201] = 0.0; + out_5230913290931847385[202] = 0.0; + out_5230913290931847385[203] = 0.0; + out_5230913290931847385[204] = 0.0; + out_5230913290931847385[205] = 0.0; + out_5230913290931847385[206] = 0.0; + out_5230913290931847385[207] = 0.0; + out_5230913290931847385[208] = 0.0; + out_5230913290931847385[209] = 1.0; + out_5230913290931847385[210] = 0.0; + out_5230913290931847385[211] = 0.0; + out_5230913290931847385[212] = 0.0; + out_5230913290931847385[213] = 0.0; + out_5230913290931847385[214] = 0.0; + out_5230913290931847385[215] = 0.0; + out_5230913290931847385[216] = 0.0; + out_5230913290931847385[217] = 0.0; + out_5230913290931847385[218] = 0.0; + out_5230913290931847385[219] = 0.0; + out_5230913290931847385[220] = 0.0; + out_5230913290931847385[221] = 0.0; + out_5230913290931847385[222] = 0.0; + out_5230913290931847385[223] = 0.0; + out_5230913290931847385[224] = 0.0; + out_5230913290931847385[225] = 0.0; + out_5230913290931847385[226] = 0.0; + out_5230913290931847385[227] = 0.0; + out_5230913290931847385[228] = 1.0; + out_5230913290931847385[229] = 0.0; + out_5230913290931847385[230] = 0.0; + out_5230913290931847385[231] = 0.0; + out_5230913290931847385[232] = 0.0; + out_5230913290931847385[233] = 0.0; + out_5230913290931847385[234] = 0.0; + out_5230913290931847385[235] = 0.0; + out_5230913290931847385[236] = 0.0; + out_5230913290931847385[237] = 0.0; + out_5230913290931847385[238] = 0.0; + out_5230913290931847385[239] = 0.0; + out_5230913290931847385[240] = 0.0; + out_5230913290931847385[241] = 0.0; + out_5230913290931847385[242] = 0.0; + out_5230913290931847385[243] = 0.0; + out_5230913290931847385[244] = 0.0; + out_5230913290931847385[245] = 0.0; + out_5230913290931847385[246] = 0.0; + out_5230913290931847385[247] = 1.0; + out_5230913290931847385[248] = 0.0; + out_5230913290931847385[249] = 0.0; + out_5230913290931847385[250] = 0.0; + out_5230913290931847385[251] = 0.0; + out_5230913290931847385[252] = 0.0; + out_5230913290931847385[253] = 0.0; + out_5230913290931847385[254] = 0.0; + out_5230913290931847385[255] = 0.0; + out_5230913290931847385[256] = 0.0; + out_5230913290931847385[257] = 0.0; + out_5230913290931847385[258] = 0.0; + out_5230913290931847385[259] = 0.0; + out_5230913290931847385[260] = 0.0; + out_5230913290931847385[261] = 0.0; + out_5230913290931847385[262] = 0.0; + out_5230913290931847385[263] = 0.0; + out_5230913290931847385[264] = 0.0; + out_5230913290931847385[265] = 0.0; + out_5230913290931847385[266] = 1.0; + out_5230913290931847385[267] = 0.0; + out_5230913290931847385[268] = 0.0; + out_5230913290931847385[269] = 0.0; + out_5230913290931847385[270] = 0.0; + out_5230913290931847385[271] = 0.0; + out_5230913290931847385[272] = 0.0; + out_5230913290931847385[273] = 0.0; + out_5230913290931847385[274] = 0.0; + out_5230913290931847385[275] = 0.0; + out_5230913290931847385[276] = 0.0; + out_5230913290931847385[277] = 0.0; + out_5230913290931847385[278] = 0.0; + out_5230913290931847385[279] = 0.0; + out_5230913290931847385[280] = 0.0; + out_5230913290931847385[281] = 0.0; + out_5230913290931847385[282] = 0.0; + out_5230913290931847385[283] = 0.0; + out_5230913290931847385[284] = 0.0; + out_5230913290931847385[285] = 1.0; + out_5230913290931847385[286] = 0.0; + out_5230913290931847385[287] = 0.0; + out_5230913290931847385[288] = 0.0; + out_5230913290931847385[289] = 0.0; + out_5230913290931847385[290] = 0.0; + out_5230913290931847385[291] = 0.0; + out_5230913290931847385[292] = 0.0; + out_5230913290931847385[293] = 0.0; + out_5230913290931847385[294] = 0.0; + out_5230913290931847385[295] = 0.0; + out_5230913290931847385[296] = 0.0; + out_5230913290931847385[297] = 0.0; + out_5230913290931847385[298] = 0.0; + out_5230913290931847385[299] = 0.0; + out_5230913290931847385[300] = 0.0; + out_5230913290931847385[301] = 0.0; + out_5230913290931847385[302] = 0.0; + out_5230913290931847385[303] = 0.0; + out_5230913290931847385[304] = 1.0; + out_5230913290931847385[305] = 0.0; + out_5230913290931847385[306] = 0.0; + out_5230913290931847385[307] = 0.0; + out_5230913290931847385[308] = 0.0; + out_5230913290931847385[309] = 0.0; + out_5230913290931847385[310] = 0.0; + out_5230913290931847385[311] = 0.0; + out_5230913290931847385[312] = 0.0; + out_5230913290931847385[313] = 0.0; + out_5230913290931847385[314] = 0.0; + out_5230913290931847385[315] = 0.0; + out_5230913290931847385[316] = 0.0; + out_5230913290931847385[317] = 0.0; + out_5230913290931847385[318] = 0.0; + out_5230913290931847385[319] = 0.0; + out_5230913290931847385[320] = 0.0; + out_5230913290931847385[321] = 0.0; + out_5230913290931847385[322] = 0.0; + out_5230913290931847385[323] = 1.0; } -void f_fun(double *state, double dt, double *out_2747407686181114080) { - out_2747407686181114080[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_2747407686181114080[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_2747407686181114080[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_2747407686181114080[3] = dt*state[12] + state[3]; - out_2747407686181114080[4] = dt*state[13] + state[4]; - out_2747407686181114080[5] = dt*state[14] + state[5]; - out_2747407686181114080[6] = state[6]; - out_2747407686181114080[7] = state[7]; - out_2747407686181114080[8] = state[8]; - out_2747407686181114080[9] = state[9]; - out_2747407686181114080[10] = state[10]; - out_2747407686181114080[11] = state[11]; - out_2747407686181114080[12] = state[12]; - out_2747407686181114080[13] = state[13]; - out_2747407686181114080[14] = state[14]; - out_2747407686181114080[15] = state[15]; - out_2747407686181114080[16] = state[16]; - out_2747407686181114080[17] = state[17]; +void f_fun(double *state, double dt, double *out_60436790132025778) { + out_60436790132025778[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_60436790132025778[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_60436790132025778[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_60436790132025778[3] = dt*state[12] + state[3]; + out_60436790132025778[4] = dt*state[13] + state[4]; + out_60436790132025778[5] = dt*state[14] + state[5]; + out_60436790132025778[6] = state[6]; + out_60436790132025778[7] = state[7]; + out_60436790132025778[8] = state[8]; + out_60436790132025778[9] = state[9]; + out_60436790132025778[10] = state[10]; + out_60436790132025778[11] = state[11]; + out_60436790132025778[12] = state[12]; + out_60436790132025778[13] = state[13]; + out_60436790132025778[14] = state[14]; + out_60436790132025778[15] = state[15]; + out_60436790132025778[16] = state[16]; + out_60436790132025778[17] = state[17]; } -void F_fun(double *state, double dt, double *out_5168326300746653192) { - out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[2] = 0; - out_5168326300746653192[3] = 0; - out_5168326300746653192[4] = 0; - out_5168326300746653192[5] = 0; - out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[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_5168326300746653192[9] = 0; - out_5168326300746653192[10] = 0; - out_5168326300746653192[11] = 0; - out_5168326300746653192[12] = 0; - out_5168326300746653192[13] = 0; - out_5168326300746653192[14] = 0; - out_5168326300746653192[15] = 0; - out_5168326300746653192[16] = 0; - out_5168326300746653192[17] = 0; - out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[20] = 0; - out_5168326300746653192[21] = 0; - out_5168326300746653192[22] = 0; - out_5168326300746653192[23] = 0; - out_5168326300746653192[24] = 0; - out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[27] = 0; - out_5168326300746653192[28] = 0; - out_5168326300746653192[29] = 0; - out_5168326300746653192[30] = 0; - out_5168326300746653192[31] = 0; - out_5168326300746653192[32] = 0; - out_5168326300746653192[33] = 0; - out_5168326300746653192[34] = 0; - out_5168326300746653192[35] = 0; - out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[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_5168326300746653192[39] = 0; - out_5168326300746653192[40] = 0; - out_5168326300746653192[41] = 0; - out_5168326300746653192[42] = 0; - out_5168326300746653192[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_5168326300746653192[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_5168326300746653192[45] = 0; - out_5168326300746653192[46] = 0; - out_5168326300746653192[47] = 0; - out_5168326300746653192[48] = 0; - out_5168326300746653192[49] = 0; - out_5168326300746653192[50] = 0; - out_5168326300746653192[51] = 0; - out_5168326300746653192[52] = 0; - out_5168326300746653192[53] = 0; - out_5168326300746653192[54] = 0; - out_5168326300746653192[55] = 0; - out_5168326300746653192[56] = 0; - out_5168326300746653192[57] = 1; - out_5168326300746653192[58] = 0; - out_5168326300746653192[59] = 0; - out_5168326300746653192[60] = 0; - out_5168326300746653192[61] = 0; - out_5168326300746653192[62] = 0; - out_5168326300746653192[63] = 0; - out_5168326300746653192[64] = 0; - out_5168326300746653192[65] = 0; - out_5168326300746653192[66] = dt; - out_5168326300746653192[67] = 0; - out_5168326300746653192[68] = 0; - out_5168326300746653192[69] = 0; - out_5168326300746653192[70] = 0; - out_5168326300746653192[71] = 0; - out_5168326300746653192[72] = 0; - out_5168326300746653192[73] = 0; - out_5168326300746653192[74] = 0; - out_5168326300746653192[75] = 0; - out_5168326300746653192[76] = 1; - out_5168326300746653192[77] = 0; - out_5168326300746653192[78] = 0; - out_5168326300746653192[79] = 0; - out_5168326300746653192[80] = 0; - out_5168326300746653192[81] = 0; - out_5168326300746653192[82] = 0; - out_5168326300746653192[83] = 0; - out_5168326300746653192[84] = 0; - out_5168326300746653192[85] = dt; - out_5168326300746653192[86] = 0; - out_5168326300746653192[87] = 0; - out_5168326300746653192[88] = 0; - out_5168326300746653192[89] = 0; - out_5168326300746653192[90] = 0; - out_5168326300746653192[91] = 0; - out_5168326300746653192[92] = 0; - out_5168326300746653192[93] = 0; - out_5168326300746653192[94] = 0; - out_5168326300746653192[95] = 1; - out_5168326300746653192[96] = 0; - out_5168326300746653192[97] = 0; - out_5168326300746653192[98] = 0; - out_5168326300746653192[99] = 0; - out_5168326300746653192[100] = 0; - out_5168326300746653192[101] = 0; - out_5168326300746653192[102] = 0; - out_5168326300746653192[103] = 0; - out_5168326300746653192[104] = dt; - out_5168326300746653192[105] = 0; - out_5168326300746653192[106] = 0; - out_5168326300746653192[107] = 0; - out_5168326300746653192[108] = 0; - out_5168326300746653192[109] = 0; - out_5168326300746653192[110] = 0; - out_5168326300746653192[111] = 0; - out_5168326300746653192[112] = 0; - out_5168326300746653192[113] = 0; - out_5168326300746653192[114] = 1; - out_5168326300746653192[115] = 0; - out_5168326300746653192[116] = 0; - out_5168326300746653192[117] = 0; - out_5168326300746653192[118] = 0; - out_5168326300746653192[119] = 0; - out_5168326300746653192[120] = 0; - out_5168326300746653192[121] = 0; - out_5168326300746653192[122] = 0; - out_5168326300746653192[123] = 0; - out_5168326300746653192[124] = 0; - out_5168326300746653192[125] = 0; - out_5168326300746653192[126] = 0; - out_5168326300746653192[127] = 0; - out_5168326300746653192[128] = 0; - out_5168326300746653192[129] = 0; - out_5168326300746653192[130] = 0; - out_5168326300746653192[131] = 0; - out_5168326300746653192[132] = 0; - out_5168326300746653192[133] = 1; - out_5168326300746653192[134] = 0; - out_5168326300746653192[135] = 0; - out_5168326300746653192[136] = 0; - out_5168326300746653192[137] = 0; - out_5168326300746653192[138] = 0; - out_5168326300746653192[139] = 0; - out_5168326300746653192[140] = 0; - out_5168326300746653192[141] = 0; - out_5168326300746653192[142] = 0; - out_5168326300746653192[143] = 0; - out_5168326300746653192[144] = 0; - out_5168326300746653192[145] = 0; - out_5168326300746653192[146] = 0; - out_5168326300746653192[147] = 0; - out_5168326300746653192[148] = 0; - out_5168326300746653192[149] = 0; - out_5168326300746653192[150] = 0; - out_5168326300746653192[151] = 0; - out_5168326300746653192[152] = 1; - out_5168326300746653192[153] = 0; - out_5168326300746653192[154] = 0; - out_5168326300746653192[155] = 0; - out_5168326300746653192[156] = 0; - out_5168326300746653192[157] = 0; - out_5168326300746653192[158] = 0; - out_5168326300746653192[159] = 0; - out_5168326300746653192[160] = 0; - out_5168326300746653192[161] = 0; - out_5168326300746653192[162] = 0; - out_5168326300746653192[163] = 0; - out_5168326300746653192[164] = 0; - out_5168326300746653192[165] = 0; - out_5168326300746653192[166] = 0; - out_5168326300746653192[167] = 0; - out_5168326300746653192[168] = 0; - out_5168326300746653192[169] = 0; - out_5168326300746653192[170] = 0; - out_5168326300746653192[171] = 1; - out_5168326300746653192[172] = 0; - out_5168326300746653192[173] = 0; - out_5168326300746653192[174] = 0; - out_5168326300746653192[175] = 0; - out_5168326300746653192[176] = 0; - out_5168326300746653192[177] = 0; - out_5168326300746653192[178] = 0; - out_5168326300746653192[179] = 0; - out_5168326300746653192[180] = 0; - out_5168326300746653192[181] = 0; - out_5168326300746653192[182] = 0; - out_5168326300746653192[183] = 0; - out_5168326300746653192[184] = 0; - out_5168326300746653192[185] = 0; - out_5168326300746653192[186] = 0; - out_5168326300746653192[187] = 0; - out_5168326300746653192[188] = 0; - out_5168326300746653192[189] = 0; - out_5168326300746653192[190] = 1; - out_5168326300746653192[191] = 0; - out_5168326300746653192[192] = 0; - out_5168326300746653192[193] = 0; - out_5168326300746653192[194] = 0; - out_5168326300746653192[195] = 0; - out_5168326300746653192[196] = 0; - out_5168326300746653192[197] = 0; - out_5168326300746653192[198] = 0; - out_5168326300746653192[199] = 0; - out_5168326300746653192[200] = 0; - out_5168326300746653192[201] = 0; - out_5168326300746653192[202] = 0; - out_5168326300746653192[203] = 0; - out_5168326300746653192[204] = 0; - out_5168326300746653192[205] = 0; - out_5168326300746653192[206] = 0; - out_5168326300746653192[207] = 0; - out_5168326300746653192[208] = 0; - out_5168326300746653192[209] = 1; - out_5168326300746653192[210] = 0; - out_5168326300746653192[211] = 0; - out_5168326300746653192[212] = 0; - out_5168326300746653192[213] = 0; - out_5168326300746653192[214] = 0; - out_5168326300746653192[215] = 0; - out_5168326300746653192[216] = 0; - out_5168326300746653192[217] = 0; - out_5168326300746653192[218] = 0; - out_5168326300746653192[219] = 0; - out_5168326300746653192[220] = 0; - out_5168326300746653192[221] = 0; - out_5168326300746653192[222] = 0; - out_5168326300746653192[223] = 0; - out_5168326300746653192[224] = 0; - out_5168326300746653192[225] = 0; - out_5168326300746653192[226] = 0; - out_5168326300746653192[227] = 0; - out_5168326300746653192[228] = 1; - out_5168326300746653192[229] = 0; - out_5168326300746653192[230] = 0; - out_5168326300746653192[231] = 0; - out_5168326300746653192[232] = 0; - out_5168326300746653192[233] = 0; - out_5168326300746653192[234] = 0; - out_5168326300746653192[235] = 0; - out_5168326300746653192[236] = 0; - out_5168326300746653192[237] = 0; - out_5168326300746653192[238] = 0; - out_5168326300746653192[239] = 0; - out_5168326300746653192[240] = 0; - out_5168326300746653192[241] = 0; - out_5168326300746653192[242] = 0; - out_5168326300746653192[243] = 0; - out_5168326300746653192[244] = 0; - out_5168326300746653192[245] = 0; - out_5168326300746653192[246] = 0; - out_5168326300746653192[247] = 1; - out_5168326300746653192[248] = 0; - out_5168326300746653192[249] = 0; - out_5168326300746653192[250] = 0; - out_5168326300746653192[251] = 0; - out_5168326300746653192[252] = 0; - out_5168326300746653192[253] = 0; - out_5168326300746653192[254] = 0; - out_5168326300746653192[255] = 0; - out_5168326300746653192[256] = 0; - out_5168326300746653192[257] = 0; - out_5168326300746653192[258] = 0; - out_5168326300746653192[259] = 0; - out_5168326300746653192[260] = 0; - out_5168326300746653192[261] = 0; - out_5168326300746653192[262] = 0; - out_5168326300746653192[263] = 0; - out_5168326300746653192[264] = 0; - out_5168326300746653192[265] = 0; - out_5168326300746653192[266] = 1; - out_5168326300746653192[267] = 0; - out_5168326300746653192[268] = 0; - out_5168326300746653192[269] = 0; - out_5168326300746653192[270] = 0; - out_5168326300746653192[271] = 0; - out_5168326300746653192[272] = 0; - out_5168326300746653192[273] = 0; - out_5168326300746653192[274] = 0; - out_5168326300746653192[275] = 0; - out_5168326300746653192[276] = 0; - out_5168326300746653192[277] = 0; - out_5168326300746653192[278] = 0; - out_5168326300746653192[279] = 0; - out_5168326300746653192[280] = 0; - out_5168326300746653192[281] = 0; - out_5168326300746653192[282] = 0; - out_5168326300746653192[283] = 0; - out_5168326300746653192[284] = 0; - out_5168326300746653192[285] = 1; - out_5168326300746653192[286] = 0; - out_5168326300746653192[287] = 0; - out_5168326300746653192[288] = 0; - out_5168326300746653192[289] = 0; - out_5168326300746653192[290] = 0; - out_5168326300746653192[291] = 0; - out_5168326300746653192[292] = 0; - out_5168326300746653192[293] = 0; - out_5168326300746653192[294] = 0; - out_5168326300746653192[295] = 0; - out_5168326300746653192[296] = 0; - out_5168326300746653192[297] = 0; - out_5168326300746653192[298] = 0; - out_5168326300746653192[299] = 0; - out_5168326300746653192[300] = 0; - out_5168326300746653192[301] = 0; - out_5168326300746653192[302] = 0; - out_5168326300746653192[303] = 0; - out_5168326300746653192[304] = 1; - out_5168326300746653192[305] = 0; - out_5168326300746653192[306] = 0; - out_5168326300746653192[307] = 0; - out_5168326300746653192[308] = 0; - out_5168326300746653192[309] = 0; - out_5168326300746653192[310] = 0; - out_5168326300746653192[311] = 0; - out_5168326300746653192[312] = 0; - out_5168326300746653192[313] = 0; - out_5168326300746653192[314] = 0; - out_5168326300746653192[315] = 0; - out_5168326300746653192[316] = 0; - out_5168326300746653192[317] = 0; - out_5168326300746653192[318] = 0; - out_5168326300746653192[319] = 0; - out_5168326300746653192[320] = 0; - out_5168326300746653192[321] = 0; - out_5168326300746653192[322] = 0; - out_5168326300746653192[323] = 1; +void F_fun(double *state, double dt, double *out_8562706762949720732) { + out_8562706762949720732[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_8562706762949720732[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_8562706762949720732[2] = 0; + out_8562706762949720732[3] = 0; + out_8562706762949720732[4] = 0; + out_8562706762949720732[5] = 0; + out_8562706762949720732[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_8562706762949720732[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_8562706762949720732[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_8562706762949720732[9] = 0; + out_8562706762949720732[10] = 0; + out_8562706762949720732[11] = 0; + out_8562706762949720732[12] = 0; + out_8562706762949720732[13] = 0; + out_8562706762949720732[14] = 0; + out_8562706762949720732[15] = 0; + out_8562706762949720732[16] = 0; + out_8562706762949720732[17] = 0; + out_8562706762949720732[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_8562706762949720732[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_8562706762949720732[20] = 0; + out_8562706762949720732[21] = 0; + out_8562706762949720732[22] = 0; + out_8562706762949720732[23] = 0; + out_8562706762949720732[24] = 0; + out_8562706762949720732[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_8562706762949720732[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_8562706762949720732[27] = 0; + out_8562706762949720732[28] = 0; + out_8562706762949720732[29] = 0; + out_8562706762949720732[30] = 0; + out_8562706762949720732[31] = 0; + out_8562706762949720732[32] = 0; + out_8562706762949720732[33] = 0; + out_8562706762949720732[34] = 0; + out_8562706762949720732[35] = 0; + out_8562706762949720732[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_8562706762949720732[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_8562706762949720732[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_8562706762949720732[39] = 0; + out_8562706762949720732[40] = 0; + out_8562706762949720732[41] = 0; + out_8562706762949720732[42] = 0; + out_8562706762949720732[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_8562706762949720732[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_8562706762949720732[45] = 0; + out_8562706762949720732[46] = 0; + out_8562706762949720732[47] = 0; + out_8562706762949720732[48] = 0; + out_8562706762949720732[49] = 0; + out_8562706762949720732[50] = 0; + out_8562706762949720732[51] = 0; + out_8562706762949720732[52] = 0; + out_8562706762949720732[53] = 0; + out_8562706762949720732[54] = 0; + out_8562706762949720732[55] = 0; + out_8562706762949720732[56] = 0; + out_8562706762949720732[57] = 1; + out_8562706762949720732[58] = 0; + out_8562706762949720732[59] = 0; + out_8562706762949720732[60] = 0; + out_8562706762949720732[61] = 0; + out_8562706762949720732[62] = 0; + out_8562706762949720732[63] = 0; + out_8562706762949720732[64] = 0; + out_8562706762949720732[65] = 0; + out_8562706762949720732[66] = dt; + out_8562706762949720732[67] = 0; + out_8562706762949720732[68] = 0; + out_8562706762949720732[69] = 0; + out_8562706762949720732[70] = 0; + out_8562706762949720732[71] = 0; + out_8562706762949720732[72] = 0; + out_8562706762949720732[73] = 0; + out_8562706762949720732[74] = 0; + out_8562706762949720732[75] = 0; + out_8562706762949720732[76] = 1; + out_8562706762949720732[77] = 0; + out_8562706762949720732[78] = 0; + out_8562706762949720732[79] = 0; + out_8562706762949720732[80] = 0; + out_8562706762949720732[81] = 0; + out_8562706762949720732[82] = 0; + out_8562706762949720732[83] = 0; + out_8562706762949720732[84] = 0; + out_8562706762949720732[85] = dt; + out_8562706762949720732[86] = 0; + out_8562706762949720732[87] = 0; + out_8562706762949720732[88] = 0; + out_8562706762949720732[89] = 0; + out_8562706762949720732[90] = 0; + out_8562706762949720732[91] = 0; + out_8562706762949720732[92] = 0; + out_8562706762949720732[93] = 0; + out_8562706762949720732[94] = 0; + out_8562706762949720732[95] = 1; + out_8562706762949720732[96] = 0; + out_8562706762949720732[97] = 0; + out_8562706762949720732[98] = 0; + out_8562706762949720732[99] = 0; + out_8562706762949720732[100] = 0; + out_8562706762949720732[101] = 0; + out_8562706762949720732[102] = 0; + out_8562706762949720732[103] = 0; + out_8562706762949720732[104] = dt; + out_8562706762949720732[105] = 0; + out_8562706762949720732[106] = 0; + out_8562706762949720732[107] = 0; + out_8562706762949720732[108] = 0; + out_8562706762949720732[109] = 0; + out_8562706762949720732[110] = 0; + out_8562706762949720732[111] = 0; + out_8562706762949720732[112] = 0; + out_8562706762949720732[113] = 0; + out_8562706762949720732[114] = 1; + out_8562706762949720732[115] = 0; + out_8562706762949720732[116] = 0; + out_8562706762949720732[117] = 0; + out_8562706762949720732[118] = 0; + out_8562706762949720732[119] = 0; + out_8562706762949720732[120] = 0; + out_8562706762949720732[121] = 0; + out_8562706762949720732[122] = 0; + out_8562706762949720732[123] = 0; + out_8562706762949720732[124] = 0; + out_8562706762949720732[125] = 0; + out_8562706762949720732[126] = 0; + out_8562706762949720732[127] = 0; + out_8562706762949720732[128] = 0; + out_8562706762949720732[129] = 0; + out_8562706762949720732[130] = 0; + out_8562706762949720732[131] = 0; + out_8562706762949720732[132] = 0; + out_8562706762949720732[133] = 1; + out_8562706762949720732[134] = 0; + out_8562706762949720732[135] = 0; + out_8562706762949720732[136] = 0; + out_8562706762949720732[137] = 0; + out_8562706762949720732[138] = 0; + out_8562706762949720732[139] = 0; + out_8562706762949720732[140] = 0; + out_8562706762949720732[141] = 0; + out_8562706762949720732[142] = 0; + out_8562706762949720732[143] = 0; + out_8562706762949720732[144] = 0; + out_8562706762949720732[145] = 0; + out_8562706762949720732[146] = 0; + out_8562706762949720732[147] = 0; + out_8562706762949720732[148] = 0; + out_8562706762949720732[149] = 0; + out_8562706762949720732[150] = 0; + out_8562706762949720732[151] = 0; + out_8562706762949720732[152] = 1; + out_8562706762949720732[153] = 0; + out_8562706762949720732[154] = 0; + out_8562706762949720732[155] = 0; + out_8562706762949720732[156] = 0; + out_8562706762949720732[157] = 0; + out_8562706762949720732[158] = 0; + out_8562706762949720732[159] = 0; + out_8562706762949720732[160] = 0; + out_8562706762949720732[161] = 0; + out_8562706762949720732[162] = 0; + out_8562706762949720732[163] = 0; + out_8562706762949720732[164] = 0; + out_8562706762949720732[165] = 0; + out_8562706762949720732[166] = 0; + out_8562706762949720732[167] = 0; + out_8562706762949720732[168] = 0; + out_8562706762949720732[169] = 0; + out_8562706762949720732[170] = 0; + out_8562706762949720732[171] = 1; + out_8562706762949720732[172] = 0; + out_8562706762949720732[173] = 0; + out_8562706762949720732[174] = 0; + out_8562706762949720732[175] = 0; + out_8562706762949720732[176] = 0; + out_8562706762949720732[177] = 0; + out_8562706762949720732[178] = 0; + out_8562706762949720732[179] = 0; + out_8562706762949720732[180] = 0; + out_8562706762949720732[181] = 0; + out_8562706762949720732[182] = 0; + out_8562706762949720732[183] = 0; + out_8562706762949720732[184] = 0; + out_8562706762949720732[185] = 0; + out_8562706762949720732[186] = 0; + out_8562706762949720732[187] = 0; + out_8562706762949720732[188] = 0; + out_8562706762949720732[189] = 0; + out_8562706762949720732[190] = 1; + out_8562706762949720732[191] = 0; + out_8562706762949720732[192] = 0; + out_8562706762949720732[193] = 0; + out_8562706762949720732[194] = 0; + out_8562706762949720732[195] = 0; + out_8562706762949720732[196] = 0; + out_8562706762949720732[197] = 0; + out_8562706762949720732[198] = 0; + out_8562706762949720732[199] = 0; + out_8562706762949720732[200] = 0; + out_8562706762949720732[201] = 0; + out_8562706762949720732[202] = 0; + out_8562706762949720732[203] = 0; + out_8562706762949720732[204] = 0; + out_8562706762949720732[205] = 0; + out_8562706762949720732[206] = 0; + out_8562706762949720732[207] = 0; + out_8562706762949720732[208] = 0; + out_8562706762949720732[209] = 1; + out_8562706762949720732[210] = 0; + out_8562706762949720732[211] = 0; + out_8562706762949720732[212] = 0; + out_8562706762949720732[213] = 0; + out_8562706762949720732[214] = 0; + out_8562706762949720732[215] = 0; + out_8562706762949720732[216] = 0; + out_8562706762949720732[217] = 0; + out_8562706762949720732[218] = 0; + out_8562706762949720732[219] = 0; + out_8562706762949720732[220] = 0; + out_8562706762949720732[221] = 0; + out_8562706762949720732[222] = 0; + out_8562706762949720732[223] = 0; + out_8562706762949720732[224] = 0; + out_8562706762949720732[225] = 0; + out_8562706762949720732[226] = 0; + out_8562706762949720732[227] = 0; + out_8562706762949720732[228] = 1; + out_8562706762949720732[229] = 0; + out_8562706762949720732[230] = 0; + out_8562706762949720732[231] = 0; + out_8562706762949720732[232] = 0; + out_8562706762949720732[233] = 0; + out_8562706762949720732[234] = 0; + out_8562706762949720732[235] = 0; + out_8562706762949720732[236] = 0; + out_8562706762949720732[237] = 0; + out_8562706762949720732[238] = 0; + out_8562706762949720732[239] = 0; + out_8562706762949720732[240] = 0; + out_8562706762949720732[241] = 0; + out_8562706762949720732[242] = 0; + out_8562706762949720732[243] = 0; + out_8562706762949720732[244] = 0; + out_8562706762949720732[245] = 0; + out_8562706762949720732[246] = 0; + out_8562706762949720732[247] = 1; + out_8562706762949720732[248] = 0; + out_8562706762949720732[249] = 0; + out_8562706762949720732[250] = 0; + out_8562706762949720732[251] = 0; + out_8562706762949720732[252] = 0; + out_8562706762949720732[253] = 0; + out_8562706762949720732[254] = 0; + out_8562706762949720732[255] = 0; + out_8562706762949720732[256] = 0; + out_8562706762949720732[257] = 0; + out_8562706762949720732[258] = 0; + out_8562706762949720732[259] = 0; + out_8562706762949720732[260] = 0; + out_8562706762949720732[261] = 0; + out_8562706762949720732[262] = 0; + out_8562706762949720732[263] = 0; + out_8562706762949720732[264] = 0; + out_8562706762949720732[265] = 0; + out_8562706762949720732[266] = 1; + out_8562706762949720732[267] = 0; + out_8562706762949720732[268] = 0; + out_8562706762949720732[269] = 0; + out_8562706762949720732[270] = 0; + out_8562706762949720732[271] = 0; + out_8562706762949720732[272] = 0; + out_8562706762949720732[273] = 0; + out_8562706762949720732[274] = 0; + out_8562706762949720732[275] = 0; + out_8562706762949720732[276] = 0; + out_8562706762949720732[277] = 0; + out_8562706762949720732[278] = 0; + out_8562706762949720732[279] = 0; + out_8562706762949720732[280] = 0; + out_8562706762949720732[281] = 0; + out_8562706762949720732[282] = 0; + out_8562706762949720732[283] = 0; + out_8562706762949720732[284] = 0; + out_8562706762949720732[285] = 1; + out_8562706762949720732[286] = 0; + out_8562706762949720732[287] = 0; + out_8562706762949720732[288] = 0; + out_8562706762949720732[289] = 0; + out_8562706762949720732[290] = 0; + out_8562706762949720732[291] = 0; + out_8562706762949720732[292] = 0; + out_8562706762949720732[293] = 0; + out_8562706762949720732[294] = 0; + out_8562706762949720732[295] = 0; + out_8562706762949720732[296] = 0; + out_8562706762949720732[297] = 0; + out_8562706762949720732[298] = 0; + out_8562706762949720732[299] = 0; + out_8562706762949720732[300] = 0; + out_8562706762949720732[301] = 0; + out_8562706762949720732[302] = 0; + out_8562706762949720732[303] = 0; + out_8562706762949720732[304] = 1; + out_8562706762949720732[305] = 0; + out_8562706762949720732[306] = 0; + out_8562706762949720732[307] = 0; + out_8562706762949720732[308] = 0; + out_8562706762949720732[309] = 0; + out_8562706762949720732[310] = 0; + out_8562706762949720732[311] = 0; + out_8562706762949720732[312] = 0; + out_8562706762949720732[313] = 0; + out_8562706762949720732[314] = 0; + out_8562706762949720732[315] = 0; + out_8562706762949720732[316] = 0; + out_8562706762949720732[317] = 0; + out_8562706762949720732[318] = 0; + out_8562706762949720732[319] = 0; + out_8562706762949720732[320] = 0; + out_8562706762949720732[321] = 0; + out_8562706762949720732[322] = 0; + out_8562706762949720732[323] = 1; } -void h_4(double *state, double *unused, double *out_8670716112105694718) { - out_8670716112105694718[0] = state[6] + state[9]; - out_8670716112105694718[1] = state[7] + state[10]; - out_8670716112105694718[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_493571647291527942) { + out_493571647291527942[0] = state[6] + state[9]; + out_493571647291527942[1] = state[7] + state[10]; + out_493571647291527942[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_1470121245665226494) { - out_1470121245665226494[0] = 0; - out_1470121245665226494[1] = 0; - out_1470121245665226494[2] = 0; - out_1470121245665226494[3] = 0; - out_1470121245665226494[4] = 0; - out_1470121245665226494[5] = 0; - out_1470121245665226494[6] = 1; - out_1470121245665226494[7] = 0; - out_1470121245665226494[8] = 0; - out_1470121245665226494[9] = 1; - out_1470121245665226494[10] = 0; - out_1470121245665226494[11] = 0; - out_1470121245665226494[12] = 0; - out_1470121245665226494[13] = 0; - out_1470121245665226494[14] = 0; - out_1470121245665226494[15] = 0; - out_1470121245665226494[16] = 0; - out_1470121245665226494[17] = 0; - out_1470121245665226494[18] = 0; - out_1470121245665226494[19] = 0; - out_1470121245665226494[20] = 0; - out_1470121245665226494[21] = 0; - out_1470121245665226494[22] = 0; - out_1470121245665226494[23] = 0; - out_1470121245665226494[24] = 0; - out_1470121245665226494[25] = 1; - out_1470121245665226494[26] = 0; - out_1470121245665226494[27] = 0; - out_1470121245665226494[28] = 1; - out_1470121245665226494[29] = 0; - out_1470121245665226494[30] = 0; - out_1470121245665226494[31] = 0; - out_1470121245665226494[32] = 0; - out_1470121245665226494[33] = 0; - out_1470121245665226494[34] = 0; - out_1470121245665226494[35] = 0; - out_1470121245665226494[36] = 0; - out_1470121245665226494[37] = 0; - out_1470121245665226494[38] = 0; - out_1470121245665226494[39] = 0; - out_1470121245665226494[40] = 0; - out_1470121245665226494[41] = 0; - out_1470121245665226494[42] = 0; - out_1470121245665226494[43] = 0; - out_1470121245665226494[44] = 1; - out_1470121245665226494[45] = 0; - out_1470121245665226494[46] = 0; - out_1470121245665226494[47] = 1; - out_1470121245665226494[48] = 0; - out_1470121245665226494[49] = 0; - out_1470121245665226494[50] = 0; - out_1470121245665226494[51] = 0; - out_1470121245665226494[52] = 0; - out_1470121245665226494[53] = 0; +void H_4(double *state, double *unused, double *out_6428860772814745489) { + out_6428860772814745489[0] = 0; + out_6428860772814745489[1] = 0; + out_6428860772814745489[2] = 0; + out_6428860772814745489[3] = 0; + out_6428860772814745489[4] = 0; + out_6428860772814745489[5] = 0; + out_6428860772814745489[6] = 1; + out_6428860772814745489[7] = 0; + out_6428860772814745489[8] = 0; + out_6428860772814745489[9] = 1; + out_6428860772814745489[10] = 0; + out_6428860772814745489[11] = 0; + out_6428860772814745489[12] = 0; + out_6428860772814745489[13] = 0; + out_6428860772814745489[14] = 0; + out_6428860772814745489[15] = 0; + out_6428860772814745489[16] = 0; + out_6428860772814745489[17] = 0; + out_6428860772814745489[18] = 0; + out_6428860772814745489[19] = 0; + out_6428860772814745489[20] = 0; + out_6428860772814745489[21] = 0; + out_6428860772814745489[22] = 0; + out_6428860772814745489[23] = 0; + out_6428860772814745489[24] = 0; + out_6428860772814745489[25] = 1; + out_6428860772814745489[26] = 0; + out_6428860772814745489[27] = 0; + out_6428860772814745489[28] = 1; + out_6428860772814745489[29] = 0; + out_6428860772814745489[30] = 0; + out_6428860772814745489[31] = 0; + out_6428860772814745489[32] = 0; + out_6428860772814745489[33] = 0; + out_6428860772814745489[34] = 0; + out_6428860772814745489[35] = 0; + out_6428860772814745489[36] = 0; + out_6428860772814745489[37] = 0; + out_6428860772814745489[38] = 0; + out_6428860772814745489[39] = 0; + out_6428860772814745489[40] = 0; + out_6428860772814745489[41] = 0; + out_6428860772814745489[42] = 0; + out_6428860772814745489[43] = 0; + out_6428860772814745489[44] = 1; + out_6428860772814745489[45] = 0; + out_6428860772814745489[46] = 0; + out_6428860772814745489[47] = 1; + out_6428860772814745489[48] = 0; + out_6428860772814745489[49] = 0; + out_6428860772814745489[50] = 0; + out_6428860772814745489[51] = 0; + out_6428860772814745489[52] = 0; + out_6428860772814745489[53] = 0; } -void h_10(double *state, double *unused, double *out_6106389642402371296) { - out_6106389642402371296[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_6106389642402371296[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_6106389642402371296[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_7477536671061017990) { + out_7477536671061017990[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_7477536671061017990[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_7477536671061017990[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_8155893514424523240) { - out_8155893514424523240[0] = 0; - out_8155893514424523240[1] = 9.8100000000000005*cos(state[1]); - out_8155893514424523240[2] = 0; - out_8155893514424523240[3] = 0; - out_8155893514424523240[4] = -state[8]; - out_8155893514424523240[5] = state[7]; - out_8155893514424523240[6] = 0; - out_8155893514424523240[7] = state[5]; - out_8155893514424523240[8] = -state[4]; - out_8155893514424523240[9] = 0; - out_8155893514424523240[10] = 0; - out_8155893514424523240[11] = 0; - out_8155893514424523240[12] = 1; - out_8155893514424523240[13] = 0; - out_8155893514424523240[14] = 0; - out_8155893514424523240[15] = 1; - out_8155893514424523240[16] = 0; - out_8155893514424523240[17] = 0; - out_8155893514424523240[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_8155893514424523240[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_8155893514424523240[20] = 0; - out_8155893514424523240[21] = state[8]; - out_8155893514424523240[22] = 0; - out_8155893514424523240[23] = -state[6]; - out_8155893514424523240[24] = -state[5]; - out_8155893514424523240[25] = 0; - out_8155893514424523240[26] = state[3]; - out_8155893514424523240[27] = 0; - out_8155893514424523240[28] = 0; - out_8155893514424523240[29] = 0; - out_8155893514424523240[30] = 0; - out_8155893514424523240[31] = 1; - out_8155893514424523240[32] = 0; - out_8155893514424523240[33] = 0; - out_8155893514424523240[34] = 1; - out_8155893514424523240[35] = 0; - out_8155893514424523240[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_8155893514424523240[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_8155893514424523240[38] = 0; - out_8155893514424523240[39] = -state[7]; - out_8155893514424523240[40] = state[6]; - out_8155893514424523240[41] = 0; - out_8155893514424523240[42] = state[4]; - out_8155893514424523240[43] = -state[3]; - out_8155893514424523240[44] = 0; - out_8155893514424523240[45] = 0; - out_8155893514424523240[46] = 0; - out_8155893514424523240[47] = 0; - out_8155893514424523240[48] = 0; - out_8155893514424523240[49] = 0; - out_8155893514424523240[50] = 1; - out_8155893514424523240[51] = 0; - out_8155893514424523240[52] = 0; - out_8155893514424523240[53] = 1; +void H_10(double *state, double *unused, double *out_3483601252945400253) { + out_3483601252945400253[0] = 0; + out_3483601252945400253[1] = 9.8100000000000005*cos(state[1]); + out_3483601252945400253[2] = 0; + out_3483601252945400253[3] = 0; + out_3483601252945400253[4] = -state[8]; + out_3483601252945400253[5] = state[7]; + out_3483601252945400253[6] = 0; + out_3483601252945400253[7] = state[5]; + out_3483601252945400253[8] = -state[4]; + out_3483601252945400253[9] = 0; + out_3483601252945400253[10] = 0; + out_3483601252945400253[11] = 0; + out_3483601252945400253[12] = 1; + out_3483601252945400253[13] = 0; + out_3483601252945400253[14] = 0; + out_3483601252945400253[15] = 1; + out_3483601252945400253[16] = 0; + out_3483601252945400253[17] = 0; + out_3483601252945400253[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_3483601252945400253[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_3483601252945400253[20] = 0; + out_3483601252945400253[21] = state[8]; + out_3483601252945400253[22] = 0; + out_3483601252945400253[23] = -state[6]; + out_3483601252945400253[24] = -state[5]; + out_3483601252945400253[25] = 0; + out_3483601252945400253[26] = state[3]; + out_3483601252945400253[27] = 0; + out_3483601252945400253[28] = 0; + out_3483601252945400253[29] = 0; + out_3483601252945400253[30] = 0; + out_3483601252945400253[31] = 1; + out_3483601252945400253[32] = 0; + out_3483601252945400253[33] = 0; + out_3483601252945400253[34] = 1; + out_3483601252945400253[35] = 0; + out_3483601252945400253[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_3483601252945400253[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_3483601252945400253[38] = 0; + out_3483601252945400253[39] = -state[7]; + out_3483601252945400253[40] = state[6]; + out_3483601252945400253[41] = 0; + out_3483601252945400253[42] = state[4]; + out_3483601252945400253[43] = -state[3]; + out_3483601252945400253[44] = 0; + out_3483601252945400253[45] = 0; + out_3483601252945400253[46] = 0; + out_3483601252945400253[47] = 0; + out_3483601252945400253[48] = 0; + out_3483601252945400253[49] = 0; + out_3483601252945400253[50] = 1; + out_3483601252945400253[51] = 0; + out_3483601252945400253[52] = 0; + out_3483601252945400253[53] = 1; } -void h_13(double *state, double *unused, double *out_8157156466131107868) { - out_8157156466131107868[0] = state[3]; - out_8157156466131107868[1] = state[4]; - out_8157156466131107868[2] = state[5]; +void h_13(double *state, double *unused, double *out_2889929992082413223) { + out_2889929992082413223[0] = state[3]; + out_2889929992082413223[1] = state[4]; + out_2889929992082413223[2] = state[5]; } -void H_13(double *state, double *unused, double *out_1742152579667106307) { - out_1742152579667106307[0] = 0; - out_1742152579667106307[1] = 0; - out_1742152579667106307[2] = 0; - out_1742152579667106307[3] = 1; - out_1742152579667106307[4] = 0; - out_1742152579667106307[5] = 0; - out_1742152579667106307[6] = 0; - out_1742152579667106307[7] = 0; - out_1742152579667106307[8] = 0; - out_1742152579667106307[9] = 0; - out_1742152579667106307[10] = 0; - out_1742152579667106307[11] = 0; - out_1742152579667106307[12] = 0; - out_1742152579667106307[13] = 0; - out_1742152579667106307[14] = 0; - out_1742152579667106307[15] = 0; - out_1742152579667106307[16] = 0; - out_1742152579667106307[17] = 0; - out_1742152579667106307[18] = 0; - out_1742152579667106307[19] = 0; - out_1742152579667106307[20] = 0; - out_1742152579667106307[21] = 0; - out_1742152579667106307[22] = 1; - out_1742152579667106307[23] = 0; - out_1742152579667106307[24] = 0; - out_1742152579667106307[25] = 0; - out_1742152579667106307[26] = 0; - out_1742152579667106307[27] = 0; - out_1742152579667106307[28] = 0; - out_1742152579667106307[29] = 0; - out_1742152579667106307[30] = 0; - out_1742152579667106307[31] = 0; - out_1742152579667106307[32] = 0; - out_1742152579667106307[33] = 0; - out_1742152579667106307[34] = 0; - out_1742152579667106307[35] = 0; - out_1742152579667106307[36] = 0; - out_1742152579667106307[37] = 0; - out_1742152579667106307[38] = 0; - out_1742152579667106307[39] = 0; - out_1742152579667106307[40] = 0; - out_1742152579667106307[41] = 1; - out_1742152579667106307[42] = 0; - out_1742152579667106307[43] = 0; - out_1742152579667106307[44] = 0; - out_1742152579667106307[45] = 0; - out_1742152579667106307[46] = 0; - out_1742152579667106307[47] = 0; - out_1742152579667106307[48] = 0; - out_1742152579667106307[49] = 0; - out_1742152579667106307[50] = 0; - out_1742152579667106307[51] = 0; - out_1742152579667106307[52] = 0; - out_1742152579667106307[53] = 0; +void H_13(double *state, double *unused, double *out_8805609475562473326) { + out_8805609475562473326[0] = 0; + out_8805609475562473326[1] = 0; + out_8805609475562473326[2] = 0; + out_8805609475562473326[3] = 1; + out_8805609475562473326[4] = 0; + out_8805609475562473326[5] = 0; + out_8805609475562473326[6] = 0; + out_8805609475562473326[7] = 0; + out_8805609475562473326[8] = 0; + out_8805609475562473326[9] = 0; + out_8805609475562473326[10] = 0; + out_8805609475562473326[11] = 0; + out_8805609475562473326[12] = 0; + out_8805609475562473326[13] = 0; + out_8805609475562473326[14] = 0; + out_8805609475562473326[15] = 0; + out_8805609475562473326[16] = 0; + out_8805609475562473326[17] = 0; + out_8805609475562473326[18] = 0; + out_8805609475562473326[19] = 0; + out_8805609475562473326[20] = 0; + out_8805609475562473326[21] = 0; + out_8805609475562473326[22] = 1; + out_8805609475562473326[23] = 0; + out_8805609475562473326[24] = 0; + out_8805609475562473326[25] = 0; + out_8805609475562473326[26] = 0; + out_8805609475562473326[27] = 0; + out_8805609475562473326[28] = 0; + out_8805609475562473326[29] = 0; + out_8805609475562473326[30] = 0; + out_8805609475562473326[31] = 0; + out_8805609475562473326[32] = 0; + out_8805609475562473326[33] = 0; + out_8805609475562473326[34] = 0; + out_8805609475562473326[35] = 0; + out_8805609475562473326[36] = 0; + out_8805609475562473326[37] = 0; + out_8805609475562473326[38] = 0; + out_8805609475562473326[39] = 0; + out_8805609475562473326[40] = 0; + out_8805609475562473326[41] = 1; + out_8805609475562473326[42] = 0; + out_8805609475562473326[43] = 0; + out_8805609475562473326[44] = 0; + out_8805609475562473326[45] = 0; + out_8805609475562473326[46] = 0; + out_8805609475562473326[47] = 0; + out_8805609475562473326[48] = 0; + out_8805609475562473326[49] = 0; + out_8805609475562473326[50] = 0; + out_8805609475562473326[51] = 0; + out_8805609475562473326[52] = 0; + out_8805609475562473326[53] = 0; } -void h_14(double *state, double *unused, double *out_568036792274749064) { - out_568036792274749064[0] = state[6]; - out_568036792274749064[1] = state[7]; - out_568036792274749064[2] = state[8]; +void h_14(double *state, double *unused, double *out_207203314381018955) { + out_207203314381018955[0] = state[6]; + out_207203314381018955[1] = state[7]; + out_207203314381018955[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4552909677960598790) { - out_4552909677960598790[0] = 0; - out_4552909677960598790[1] = 0; - out_4552909677960598790[2] = 0; - out_4552909677960598790[3] = 0; - out_4552909677960598790[4] = 0; - out_4552909677960598790[5] = 0; - out_4552909677960598790[6] = 1; - out_4552909677960598790[7] = 0; - out_4552909677960598790[8] = 0; - out_4552909677960598790[9] = 0; - out_4552909677960598790[10] = 0; - out_4552909677960598790[11] = 0; - out_4552909677960598790[12] = 0; - out_4552909677960598790[13] = 0; - out_4552909677960598790[14] = 0; - out_4552909677960598790[15] = 0; - out_4552909677960598790[16] = 0; - out_4552909677960598790[17] = 0; - out_4552909677960598790[18] = 0; - out_4552909677960598790[19] = 0; - out_4552909677960598790[20] = 0; - out_4552909677960598790[21] = 0; - out_4552909677960598790[22] = 0; - out_4552909677960598790[23] = 0; - out_4552909677960598790[24] = 0; - out_4552909677960598790[25] = 1; - out_4552909677960598790[26] = 0; - out_4552909677960598790[27] = 0; - out_4552909677960598790[28] = 0; - out_4552909677960598790[29] = 0; - out_4552909677960598790[30] = 0; - out_4552909677960598790[31] = 0; - out_4552909677960598790[32] = 0; - out_4552909677960598790[33] = 0; - out_4552909677960598790[34] = 0; - out_4552909677960598790[35] = 0; - out_4552909677960598790[36] = 0; - out_4552909677960598790[37] = 0; - out_4552909677960598790[38] = 0; - out_4552909677960598790[39] = 0; - out_4552909677960598790[40] = 0; - out_4552909677960598790[41] = 0; - out_4552909677960598790[42] = 0; - out_4552909677960598790[43] = 0; - out_4552909677960598790[44] = 1; - out_4552909677960598790[45] = 0; - out_4552909677960598790[46] = 0; - out_4552909677960598790[47] = 0; - out_4552909677960598790[48] = 0; - out_4552909677960598790[49] = 0; - out_4552909677960598790[50] = 0; - out_4552909677960598790[51] = 0; - out_4552909677960598790[52] = 0; - out_4552909677960598790[53] = 0; +void H_14(double *state, double *unused, double *out_3346072340519373193) { + out_3346072340519373193[0] = 0; + out_3346072340519373193[1] = 0; + out_3346072340519373193[2] = 0; + out_3346072340519373193[3] = 0; + out_3346072340519373193[4] = 0; + out_3346072340519373193[5] = 0; + out_3346072340519373193[6] = 1; + out_3346072340519373193[7] = 0; + out_3346072340519373193[8] = 0; + out_3346072340519373193[9] = 0; + out_3346072340519373193[10] = 0; + out_3346072340519373193[11] = 0; + out_3346072340519373193[12] = 0; + out_3346072340519373193[13] = 0; + out_3346072340519373193[14] = 0; + out_3346072340519373193[15] = 0; + out_3346072340519373193[16] = 0; + out_3346072340519373193[17] = 0; + out_3346072340519373193[18] = 0; + out_3346072340519373193[19] = 0; + out_3346072340519373193[20] = 0; + out_3346072340519373193[21] = 0; + out_3346072340519373193[22] = 0; + out_3346072340519373193[23] = 0; + out_3346072340519373193[24] = 0; + out_3346072340519373193[25] = 1; + out_3346072340519373193[26] = 0; + out_3346072340519373193[27] = 0; + out_3346072340519373193[28] = 0; + out_3346072340519373193[29] = 0; + out_3346072340519373193[30] = 0; + out_3346072340519373193[31] = 0; + out_3346072340519373193[32] = 0; + out_3346072340519373193[33] = 0; + out_3346072340519373193[34] = 0; + out_3346072340519373193[35] = 0; + out_3346072340519373193[36] = 0; + out_3346072340519373193[37] = 0; + out_3346072340519373193[38] = 0; + out_3346072340519373193[39] = 0; + out_3346072340519373193[40] = 0; + out_3346072340519373193[41] = 0; + out_3346072340519373193[42] = 0; + out_3346072340519373193[43] = 0; + out_3346072340519373193[44] = 1; + out_3346072340519373193[45] = 0; + out_3346072340519373193[46] = 0; + out_3346072340519373193[47] = 0; + out_3346072340519373193[48] = 0; + out_3346072340519373193[49] = 0; + out_3346072340519373193[50] = 0; + out_3346072340519373193[51] = 0; + out_3346072340519373193[52] = 0; + out_3346072340519373193[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_8259662850093450970) { - err_fun(nom_x, delta_x, out_8259662850093450970); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7993353667610184364) { + err_fun(nom_x, delta_x, out_7993353667610184364); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7477554158578246383) { - inv_err_fun(nom_x, true_x, out_7477554158578246383); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_533608207331381553) { + inv_err_fun(nom_x, true_x, out_533608207331381553); } -void pose_H_mod_fun(double *state, double *out_6622639630704307229) { - H_mod_fun(state, out_6622639630704307229); +void pose_H_mod_fun(double *state, double *out_5230913290931847385) { + H_mod_fun(state, out_5230913290931847385); } -void pose_f_fun(double *state, double dt, double *out_2747407686181114080) { - f_fun(state, dt, out_2747407686181114080); +void pose_f_fun(double *state, double dt, double *out_60436790132025778) { + f_fun(state, dt, out_60436790132025778); } -void pose_F_fun(double *state, double dt, double *out_5168326300746653192) { - F_fun(state, dt, out_5168326300746653192); +void pose_F_fun(double *state, double dt, double *out_8562706762949720732) { + F_fun(state, dt, out_8562706762949720732); } -void pose_h_4(double *state, double *unused, double *out_8670716112105694718) { - h_4(state, unused, out_8670716112105694718); +void pose_h_4(double *state, double *unused, double *out_493571647291527942) { + h_4(state, unused, out_493571647291527942); } -void pose_H_4(double *state, double *unused, double *out_1470121245665226494) { - H_4(state, unused, out_1470121245665226494); +void pose_H_4(double *state, double *unused, double *out_6428860772814745489) { + H_4(state, unused, out_6428860772814745489); } -void pose_h_10(double *state, double *unused, double *out_6106389642402371296) { - h_10(state, unused, out_6106389642402371296); +void pose_h_10(double *state, double *unused, double *out_7477536671061017990) { + h_10(state, unused, out_7477536671061017990); } -void pose_H_10(double *state, double *unused, double *out_8155893514424523240) { - H_10(state, unused, out_8155893514424523240); +void pose_H_10(double *state, double *unused, double *out_3483601252945400253) { + H_10(state, unused, out_3483601252945400253); } -void pose_h_13(double *state, double *unused, double *out_8157156466131107868) { - h_13(state, unused, out_8157156466131107868); +void pose_h_13(double *state, double *unused, double *out_2889929992082413223) { + h_13(state, unused, out_2889929992082413223); } -void pose_H_13(double *state, double *unused, double *out_1742152579667106307) { - H_13(state, unused, out_1742152579667106307); +void pose_H_13(double *state, double *unused, double *out_8805609475562473326) { + H_13(state, unused, out_8805609475562473326); } -void pose_h_14(double *state, double *unused, double *out_568036792274749064) { - h_14(state, unused, out_568036792274749064); +void pose_h_14(double *state, double *unused, double *out_207203314381018955) { + h_14(state, unused, out_207203314381018955); } -void pose_H_14(double *state, double *unused, double *out_4552909677960598790) { - H_14(state, unused, out_4552909677960598790); +void pose_H_14(double *state, double *unused, double *out_3346072340519373193) { + H_14(state, unused, out_3346072340519373193); } 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 1af60b6f8..0952276be 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_8259662850093450970); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7477554158578246383); -void pose_H_mod_fun(double *state, double *out_6622639630704307229); -void pose_f_fun(double *state, double dt, double *out_2747407686181114080); -void pose_F_fun(double *state, double dt, double *out_5168326300746653192); -void pose_h_4(double *state, double *unused, double *out_8670716112105694718); -void pose_H_4(double *state, double *unused, double *out_1470121245665226494); -void pose_h_10(double *state, double *unused, double *out_6106389642402371296); -void pose_H_10(double *state, double *unused, double *out_8155893514424523240); -void pose_h_13(double *state, double *unused, double *out_8157156466131107868); -void pose_H_13(double *state, double *unused, double *out_1742152579667106307); -void pose_h_14(double *state, double *unused, double *out_568036792274749064); -void pose_H_14(double *state, double *unused, double *out_4552909677960598790); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7993353667610184364); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_533608207331381553); +void pose_H_mod_fun(double *state, double *out_5230913290931847385); +void pose_f_fun(double *state, double dt, double *out_60436790132025778); +void pose_F_fun(double *state, double dt, double *out_8562706762949720732); +void pose_h_4(double *state, double *unused, double *out_493571647291527942); +void pose_H_4(double *state, double *unused, double *out_6428860772814745489); +void pose_h_10(double *state, double *unused, double *out_7477536671061017990); +void pose_H_10(double *state, double *unused, double *out_3483601252945400253); +void pose_h_13(double *state, double *unused, double *out_2889929992082413223); +void pose_H_13(double *state, double *unused, double *out_8805609475562473326); +void pose_h_14(double *state, double *unused, double *out_207203314381018955); +void pose_H_14(double *state, double *unused, double *out_3346072340519373193); 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 e8686ab72..8e23521da 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ