diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index b44caa71b..9c8c92816 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 9d3eb13b2..2dcd3ac3c 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 1b9329228..286299a56 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 4b36f6fbd..7d6977580 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 e8155c53b..412be3a83 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 49e95d8d3..b21b290a2 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 e7dcc26ec..4071f64d5 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 e7dcc26ec..4071f64d5 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 6d92f29e7..fa16e0f4f 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 49e95d8d3..b21b290a2 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 125942ee1..91b5253cc 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-04dbd8d0-DEBUG"; +const uint8_t gitversion[19] = "DEV-0c7d259e-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 9b2192494..7e00a5790 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 ef0cea0a5..d8167d56d 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 ad0344368..ad6dbb719 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 8cbac2855..d04c7c0f7 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 997eea0a1..1669ecd8d 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 c2eba0ef6..599d9d90e 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 c2b4eb37c..155f2681d 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 d30d6472f..2f0258e87 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 fba1c63a4..c08ed13f2 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 f8163687c..b1250ae17 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 d9a2c4211..8126771ce 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 8d6433ecc..8fe65949f 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 936f5876a..23f12969e 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 fd8989ede..48fde4410 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 1fc3714d3..ba572275a 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 07d5d49b9..7b75975b8 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 7aa4c5d1b..cf7f396e5 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 5b2e96b94..1b9528208 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 39c0599c2..dd8ecb12e 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 5b472f170..3fb4f35b6 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 6373d0d8e..806270017 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-04dbd8d0-DEBUG \ No newline at end of file +DEV-0c7d259e-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index c960793ff..66a7f99cf 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_411609516403737386) { - out_411609516403737386[0] = delta_x[0] + nom_x[0]; - out_411609516403737386[1] = delta_x[1] + nom_x[1]; - out_411609516403737386[2] = delta_x[2] + nom_x[2]; - out_411609516403737386[3] = delta_x[3] + nom_x[3]; - out_411609516403737386[4] = delta_x[4] + nom_x[4]; - out_411609516403737386[5] = delta_x[5] + nom_x[5]; - out_411609516403737386[6] = delta_x[6] + nom_x[6]; - out_411609516403737386[7] = delta_x[7] + nom_x[7]; - out_411609516403737386[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_4530123797725818472) { + out_4530123797725818472[0] = delta_x[0] + nom_x[0]; + out_4530123797725818472[1] = delta_x[1] + nom_x[1]; + out_4530123797725818472[2] = delta_x[2] + nom_x[2]; + out_4530123797725818472[3] = delta_x[3] + nom_x[3]; + out_4530123797725818472[4] = delta_x[4] + nom_x[4]; + out_4530123797725818472[5] = delta_x[5] + nom_x[5]; + out_4530123797725818472[6] = delta_x[6] + nom_x[6]; + out_4530123797725818472[7] = delta_x[7] + nom_x[7]; + out_4530123797725818472[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7082078735183717431) { - out_7082078735183717431[0] = -nom_x[0] + true_x[0]; - out_7082078735183717431[1] = -nom_x[1] + true_x[1]; - out_7082078735183717431[2] = -nom_x[2] + true_x[2]; - out_7082078735183717431[3] = -nom_x[3] + true_x[3]; - out_7082078735183717431[4] = -nom_x[4] + true_x[4]; - out_7082078735183717431[5] = -nom_x[5] + true_x[5]; - out_7082078735183717431[6] = -nom_x[6] + true_x[6]; - out_7082078735183717431[7] = -nom_x[7] + true_x[7]; - out_7082078735183717431[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8174571641545541533) { + out_8174571641545541533[0] = -nom_x[0] + true_x[0]; + out_8174571641545541533[1] = -nom_x[1] + true_x[1]; + out_8174571641545541533[2] = -nom_x[2] + true_x[2]; + out_8174571641545541533[3] = -nom_x[3] + true_x[3]; + out_8174571641545541533[4] = -nom_x[4] + true_x[4]; + out_8174571641545541533[5] = -nom_x[5] + true_x[5]; + out_8174571641545541533[6] = -nom_x[6] + true_x[6]; + out_8174571641545541533[7] = -nom_x[7] + true_x[7]; + out_8174571641545541533[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_1390740467130861487) { - out_1390740467130861487[0] = 1.0; - out_1390740467130861487[1] = 0.0; - out_1390740467130861487[2] = 0.0; - out_1390740467130861487[3] = 0.0; - out_1390740467130861487[4] = 0.0; - out_1390740467130861487[5] = 0.0; - out_1390740467130861487[6] = 0.0; - out_1390740467130861487[7] = 0.0; - out_1390740467130861487[8] = 0.0; - out_1390740467130861487[9] = 0.0; - out_1390740467130861487[10] = 1.0; - out_1390740467130861487[11] = 0.0; - out_1390740467130861487[12] = 0.0; - out_1390740467130861487[13] = 0.0; - out_1390740467130861487[14] = 0.0; - out_1390740467130861487[15] = 0.0; - out_1390740467130861487[16] = 0.0; - out_1390740467130861487[17] = 0.0; - out_1390740467130861487[18] = 0.0; - out_1390740467130861487[19] = 0.0; - out_1390740467130861487[20] = 1.0; - out_1390740467130861487[21] = 0.0; - out_1390740467130861487[22] = 0.0; - out_1390740467130861487[23] = 0.0; - out_1390740467130861487[24] = 0.0; - out_1390740467130861487[25] = 0.0; - out_1390740467130861487[26] = 0.0; - out_1390740467130861487[27] = 0.0; - out_1390740467130861487[28] = 0.0; - out_1390740467130861487[29] = 0.0; - out_1390740467130861487[30] = 1.0; - out_1390740467130861487[31] = 0.0; - out_1390740467130861487[32] = 0.0; - out_1390740467130861487[33] = 0.0; - out_1390740467130861487[34] = 0.0; - out_1390740467130861487[35] = 0.0; - out_1390740467130861487[36] = 0.0; - out_1390740467130861487[37] = 0.0; - out_1390740467130861487[38] = 0.0; - out_1390740467130861487[39] = 0.0; - out_1390740467130861487[40] = 1.0; - out_1390740467130861487[41] = 0.0; - out_1390740467130861487[42] = 0.0; - out_1390740467130861487[43] = 0.0; - out_1390740467130861487[44] = 0.0; - out_1390740467130861487[45] = 0.0; - out_1390740467130861487[46] = 0.0; - out_1390740467130861487[47] = 0.0; - out_1390740467130861487[48] = 0.0; - out_1390740467130861487[49] = 0.0; - out_1390740467130861487[50] = 1.0; - out_1390740467130861487[51] = 0.0; - out_1390740467130861487[52] = 0.0; - out_1390740467130861487[53] = 0.0; - out_1390740467130861487[54] = 0.0; - out_1390740467130861487[55] = 0.0; - out_1390740467130861487[56] = 0.0; - out_1390740467130861487[57] = 0.0; - out_1390740467130861487[58] = 0.0; - out_1390740467130861487[59] = 0.0; - out_1390740467130861487[60] = 1.0; - out_1390740467130861487[61] = 0.0; - out_1390740467130861487[62] = 0.0; - out_1390740467130861487[63] = 0.0; - out_1390740467130861487[64] = 0.0; - out_1390740467130861487[65] = 0.0; - out_1390740467130861487[66] = 0.0; - out_1390740467130861487[67] = 0.0; - out_1390740467130861487[68] = 0.0; - out_1390740467130861487[69] = 0.0; - out_1390740467130861487[70] = 1.0; - out_1390740467130861487[71] = 0.0; - out_1390740467130861487[72] = 0.0; - out_1390740467130861487[73] = 0.0; - out_1390740467130861487[74] = 0.0; - out_1390740467130861487[75] = 0.0; - out_1390740467130861487[76] = 0.0; - out_1390740467130861487[77] = 0.0; - out_1390740467130861487[78] = 0.0; - out_1390740467130861487[79] = 0.0; - out_1390740467130861487[80] = 1.0; +void H_mod_fun(double *state, double *out_1010343532093577119) { + out_1010343532093577119[0] = 1.0; + out_1010343532093577119[1] = 0.0; + out_1010343532093577119[2] = 0.0; + out_1010343532093577119[3] = 0.0; + out_1010343532093577119[4] = 0.0; + out_1010343532093577119[5] = 0.0; + out_1010343532093577119[6] = 0.0; + out_1010343532093577119[7] = 0.0; + out_1010343532093577119[8] = 0.0; + out_1010343532093577119[9] = 0.0; + out_1010343532093577119[10] = 1.0; + out_1010343532093577119[11] = 0.0; + out_1010343532093577119[12] = 0.0; + out_1010343532093577119[13] = 0.0; + out_1010343532093577119[14] = 0.0; + out_1010343532093577119[15] = 0.0; + out_1010343532093577119[16] = 0.0; + out_1010343532093577119[17] = 0.0; + out_1010343532093577119[18] = 0.0; + out_1010343532093577119[19] = 0.0; + out_1010343532093577119[20] = 1.0; + out_1010343532093577119[21] = 0.0; + out_1010343532093577119[22] = 0.0; + out_1010343532093577119[23] = 0.0; + out_1010343532093577119[24] = 0.0; + out_1010343532093577119[25] = 0.0; + out_1010343532093577119[26] = 0.0; + out_1010343532093577119[27] = 0.0; + out_1010343532093577119[28] = 0.0; + out_1010343532093577119[29] = 0.0; + out_1010343532093577119[30] = 1.0; + out_1010343532093577119[31] = 0.0; + out_1010343532093577119[32] = 0.0; + out_1010343532093577119[33] = 0.0; + out_1010343532093577119[34] = 0.0; + out_1010343532093577119[35] = 0.0; + out_1010343532093577119[36] = 0.0; + out_1010343532093577119[37] = 0.0; + out_1010343532093577119[38] = 0.0; + out_1010343532093577119[39] = 0.0; + out_1010343532093577119[40] = 1.0; + out_1010343532093577119[41] = 0.0; + out_1010343532093577119[42] = 0.0; + out_1010343532093577119[43] = 0.0; + out_1010343532093577119[44] = 0.0; + out_1010343532093577119[45] = 0.0; + out_1010343532093577119[46] = 0.0; + out_1010343532093577119[47] = 0.0; + out_1010343532093577119[48] = 0.0; + out_1010343532093577119[49] = 0.0; + out_1010343532093577119[50] = 1.0; + out_1010343532093577119[51] = 0.0; + out_1010343532093577119[52] = 0.0; + out_1010343532093577119[53] = 0.0; + out_1010343532093577119[54] = 0.0; + out_1010343532093577119[55] = 0.0; + out_1010343532093577119[56] = 0.0; + out_1010343532093577119[57] = 0.0; + out_1010343532093577119[58] = 0.0; + out_1010343532093577119[59] = 0.0; + out_1010343532093577119[60] = 1.0; + out_1010343532093577119[61] = 0.0; + out_1010343532093577119[62] = 0.0; + out_1010343532093577119[63] = 0.0; + out_1010343532093577119[64] = 0.0; + out_1010343532093577119[65] = 0.0; + out_1010343532093577119[66] = 0.0; + out_1010343532093577119[67] = 0.0; + out_1010343532093577119[68] = 0.0; + out_1010343532093577119[69] = 0.0; + out_1010343532093577119[70] = 1.0; + out_1010343532093577119[71] = 0.0; + out_1010343532093577119[72] = 0.0; + out_1010343532093577119[73] = 0.0; + out_1010343532093577119[74] = 0.0; + out_1010343532093577119[75] = 0.0; + out_1010343532093577119[76] = 0.0; + out_1010343532093577119[77] = 0.0; + out_1010343532093577119[78] = 0.0; + out_1010343532093577119[79] = 0.0; + out_1010343532093577119[80] = 1.0; } -void f_fun(double *state, double dt, double *out_2246396216540142177) { - out_2246396216540142177[0] = state[0]; - out_2246396216540142177[1] = state[1]; - out_2246396216540142177[2] = state[2]; - out_2246396216540142177[3] = state[3]; - out_2246396216540142177[4] = state[4]; - out_2246396216540142177[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_2246396216540142177[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_2246396216540142177[7] = state[7]; - out_2246396216540142177[8] = state[8]; +void f_fun(double *state, double dt, double *out_8102414924978153817) { + out_8102414924978153817[0] = state[0]; + out_8102414924978153817[1] = state[1]; + out_8102414924978153817[2] = state[2]; + out_8102414924978153817[3] = state[3]; + out_8102414924978153817[4] = state[4]; + out_8102414924978153817[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_8102414924978153817[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_8102414924978153817[7] = state[7]; + out_8102414924978153817[8] = state[8]; } -void F_fun(double *state, double dt, double *out_3263326103915417388) { - out_3263326103915417388[0] = 1; - out_3263326103915417388[1] = 0; - out_3263326103915417388[2] = 0; - out_3263326103915417388[3] = 0; - out_3263326103915417388[4] = 0; - out_3263326103915417388[5] = 0; - out_3263326103915417388[6] = 0; - out_3263326103915417388[7] = 0; - out_3263326103915417388[8] = 0; - out_3263326103915417388[9] = 0; - out_3263326103915417388[10] = 1; - out_3263326103915417388[11] = 0; - out_3263326103915417388[12] = 0; - out_3263326103915417388[13] = 0; - out_3263326103915417388[14] = 0; - out_3263326103915417388[15] = 0; - out_3263326103915417388[16] = 0; - out_3263326103915417388[17] = 0; - out_3263326103915417388[18] = 0; - out_3263326103915417388[19] = 0; - out_3263326103915417388[20] = 1; - out_3263326103915417388[21] = 0; - out_3263326103915417388[22] = 0; - out_3263326103915417388[23] = 0; - out_3263326103915417388[24] = 0; - out_3263326103915417388[25] = 0; - out_3263326103915417388[26] = 0; - out_3263326103915417388[27] = 0; - out_3263326103915417388[28] = 0; - out_3263326103915417388[29] = 0; - out_3263326103915417388[30] = 1; - out_3263326103915417388[31] = 0; - out_3263326103915417388[32] = 0; - out_3263326103915417388[33] = 0; - out_3263326103915417388[34] = 0; - out_3263326103915417388[35] = 0; - out_3263326103915417388[36] = 0; - out_3263326103915417388[37] = 0; - out_3263326103915417388[38] = 0; - out_3263326103915417388[39] = 0; - out_3263326103915417388[40] = 1; - out_3263326103915417388[41] = 0; - out_3263326103915417388[42] = 0; - out_3263326103915417388[43] = 0; - out_3263326103915417388[44] = 0; - out_3263326103915417388[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_3263326103915417388[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_3263326103915417388[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3263326103915417388[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3263326103915417388[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_3263326103915417388[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_3263326103915417388[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_3263326103915417388[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_3263326103915417388[53] = -9.8100000000000005*dt; - out_3263326103915417388[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_3263326103915417388[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_3263326103915417388[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3263326103915417388[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3263326103915417388[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_3263326103915417388[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_3263326103915417388[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_3263326103915417388[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3263326103915417388[62] = 0; - out_3263326103915417388[63] = 0; - out_3263326103915417388[64] = 0; - out_3263326103915417388[65] = 0; - out_3263326103915417388[66] = 0; - out_3263326103915417388[67] = 0; - out_3263326103915417388[68] = 0; - out_3263326103915417388[69] = 0; - out_3263326103915417388[70] = 1; - out_3263326103915417388[71] = 0; - out_3263326103915417388[72] = 0; - out_3263326103915417388[73] = 0; - out_3263326103915417388[74] = 0; - out_3263326103915417388[75] = 0; - out_3263326103915417388[76] = 0; - out_3263326103915417388[77] = 0; - out_3263326103915417388[78] = 0; - out_3263326103915417388[79] = 0; - out_3263326103915417388[80] = 1; +void F_fun(double *state, double dt, double *out_1635199146977261472) { + out_1635199146977261472[0] = 1; + out_1635199146977261472[1] = 0; + out_1635199146977261472[2] = 0; + out_1635199146977261472[3] = 0; + out_1635199146977261472[4] = 0; + out_1635199146977261472[5] = 0; + out_1635199146977261472[6] = 0; + out_1635199146977261472[7] = 0; + out_1635199146977261472[8] = 0; + out_1635199146977261472[9] = 0; + out_1635199146977261472[10] = 1; + out_1635199146977261472[11] = 0; + out_1635199146977261472[12] = 0; + out_1635199146977261472[13] = 0; + out_1635199146977261472[14] = 0; + out_1635199146977261472[15] = 0; + out_1635199146977261472[16] = 0; + out_1635199146977261472[17] = 0; + out_1635199146977261472[18] = 0; + out_1635199146977261472[19] = 0; + out_1635199146977261472[20] = 1; + out_1635199146977261472[21] = 0; + out_1635199146977261472[22] = 0; + out_1635199146977261472[23] = 0; + out_1635199146977261472[24] = 0; + out_1635199146977261472[25] = 0; + out_1635199146977261472[26] = 0; + out_1635199146977261472[27] = 0; + out_1635199146977261472[28] = 0; + out_1635199146977261472[29] = 0; + out_1635199146977261472[30] = 1; + out_1635199146977261472[31] = 0; + out_1635199146977261472[32] = 0; + out_1635199146977261472[33] = 0; + out_1635199146977261472[34] = 0; + out_1635199146977261472[35] = 0; + out_1635199146977261472[36] = 0; + out_1635199146977261472[37] = 0; + out_1635199146977261472[38] = 0; + out_1635199146977261472[39] = 0; + out_1635199146977261472[40] = 1; + out_1635199146977261472[41] = 0; + out_1635199146977261472[42] = 0; + out_1635199146977261472[43] = 0; + out_1635199146977261472[44] = 0; + out_1635199146977261472[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_1635199146977261472[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_1635199146977261472[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_1635199146977261472[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_1635199146977261472[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_1635199146977261472[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_1635199146977261472[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_1635199146977261472[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_1635199146977261472[53] = -9.8100000000000005*dt; + out_1635199146977261472[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_1635199146977261472[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_1635199146977261472[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1635199146977261472[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1635199146977261472[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_1635199146977261472[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_1635199146977261472[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_1635199146977261472[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1635199146977261472[62] = 0; + out_1635199146977261472[63] = 0; + out_1635199146977261472[64] = 0; + out_1635199146977261472[65] = 0; + out_1635199146977261472[66] = 0; + out_1635199146977261472[67] = 0; + out_1635199146977261472[68] = 0; + out_1635199146977261472[69] = 0; + out_1635199146977261472[70] = 1; + out_1635199146977261472[71] = 0; + out_1635199146977261472[72] = 0; + out_1635199146977261472[73] = 0; + out_1635199146977261472[74] = 0; + out_1635199146977261472[75] = 0; + out_1635199146977261472[76] = 0; + out_1635199146977261472[77] = 0; + out_1635199146977261472[78] = 0; + out_1635199146977261472[79] = 0; + out_1635199146977261472[80] = 1; } -void h_25(double *state, double *unused, double *out_7774150008233015132) { - out_7774150008233015132[0] = state[6]; +void h_25(double *state, double *unused, double *out_7182033227359772288) { + out_7182033227359772288[0] = state[6]; } -void H_25(double *state, double *unused, double *out_1594416823404739290) { - out_1594416823404739290[0] = 0; - out_1594416823404739290[1] = 0; - out_1594416823404739290[2] = 0; - out_1594416823404739290[3] = 0; - out_1594416823404739290[4] = 0; - out_1594416823404739290[5] = 0; - out_1594416823404739290[6] = 1; - out_1594416823404739290[7] = 0; - out_1594416823404739290[8] = 0; +void H_25(double *state, double *unused, double *out_6910994500880282686) { + out_6910994500880282686[0] = 0; + out_6910994500880282686[1] = 0; + out_6910994500880282686[2] = 0; + out_6910994500880282686[3] = 0; + out_6910994500880282686[4] = 0; + out_6910994500880282686[5] = 0; + out_6910994500880282686[6] = 1; + out_6910994500880282686[7] = 0; + out_6910994500880282686[8] = 0; } -void h_24(double *state, double *unused, double *out_3517564190390228050) { - out_3517564190390228050[0] = state[4]; - out_3517564190390228050[1] = state[5]; +void h_24(double *state, double *unused, double *out_8844123752507495678) { + out_8844123752507495678[0] = state[4]; + out_8844123752507495678[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5823974810272187603) { - out_5823974810272187603[0] = 0; - out_5823974810272187603[1] = 0; - out_5823974810272187603[2] = 0; - out_5823974810272187603[3] = 0; - out_5823974810272187603[4] = 1; - out_5823974810272187603[5] = 0; - out_5823974810272187603[6] = 0; - out_5823974810272187603[7] = 0; - out_5823974810272187603[8] = 0; - out_5823974810272187603[9] = 0; - out_5823974810272187603[10] = 0; - out_5823974810272187603[11] = 0; - out_5823974810272187603[12] = 0; - out_5823974810272187603[13] = 0; - out_5823974810272187603[14] = 1; - out_5823974810272187603[15] = 0; - out_5823974810272187603[16] = 0; - out_5823974810272187603[17] = 0; +void H_24(double *state, double *unused, double *out_9166842576004100315) { + out_9166842576004100315[0] = 0; + out_9166842576004100315[1] = 0; + out_9166842576004100315[2] = 0; + out_9166842576004100315[3] = 0; + out_9166842576004100315[4] = 1; + out_9166842576004100315[5] = 0; + out_9166842576004100315[6] = 0; + out_9166842576004100315[7] = 0; + out_9166842576004100315[8] = 0; + out_9166842576004100315[9] = 0; + out_9166842576004100315[10] = 0; + out_9166842576004100315[11] = 0; + out_9166842576004100315[12] = 0; + out_9166842576004100315[13] = 0; + out_9166842576004100315[14] = 1; + out_9166842576004100315[15] = 0; + out_9166842576004100315[16] = 0; + out_9166842576004100315[17] = 0; } -void h_30(double *state, double *unused, double *out_3901758839126185548) { - out_3901758839126185548[0] = state[4]; +void h_30(double *state, double *unused, double *out_6906839165075266399) { + out_6906839165075266399[0] = state[4]; } -void H_30(double *state, double *unused, double *out_923916135102509337) { - out_923916135102509337[0] = 0; - out_923916135102509337[1] = 0; - out_923916135102509337[2] = 0; - out_923916135102509337[3] = 0; - out_923916135102509337[4] = 1; - out_923916135102509337[5] = 0; - out_923916135102509337[6] = 0; - out_923916135102509337[7] = 0; - out_923916135102509337[8] = 0; +void H_30(double *state, double *unused, double *out_2383298170752674488) { + out_2383298170752674488[0] = 0; + out_2383298170752674488[1] = 0; + out_2383298170752674488[2] = 0; + out_2383298170752674488[3] = 0; + out_2383298170752674488[4] = 1; + out_2383298170752674488[5] = 0; + out_2383298170752674488[6] = 0; + out_2383298170752674488[7] = 0; + out_2383298170752674488[8] = 0; } -void h_26(double *state, double *unused, double *out_4894550625139811592) { - out_4894550625139811592[0] = state[7]; +void h_26(double *state, double *unused, double *out_6538231640973439572) { + out_6538231640973439572[0] = state[7]; } -void H_26(double *state, double *unused, double *out_5335920142278795514) { - out_5335920142278795514[0] = 0; - out_5335920142278795514[1] = 0; - out_5335920142278795514[2] = 0; - out_5335920142278795514[3] = 0; - out_5335920142278795514[4] = 0; - out_5335920142278795514[5] = 0; - out_5335920142278795514[6] = 0; - out_5335920142278795514[7] = 1; - out_5335920142278795514[8] = 0; +void H_26(double *state, double *unused, double *out_3169491182006226462) { + out_3169491182006226462[0] = 0; + out_3169491182006226462[1] = 0; + out_3169491182006226462[2] = 0; + out_3169491182006226462[3] = 0; + out_3169491182006226462[4] = 0; + out_3169491182006226462[5] = 0; + out_3169491182006226462[6] = 0; + out_3169491182006226462[7] = 1; + out_3169491182006226462[8] = 0; } -void h_27(double *state, double *unused, double *out_8717659861134677567) { - out_8717659861134677567[0] = state[3]; +void h_27(double *state, double *unused, double *out_8125543080261434723) { + out_8125543080261434723[0] = state[3]; } -void H_27(double *state, double *unused, double *out_1250847176697915574) { - out_1250847176697915574[0] = 0; - out_1250847176697915574[1] = 0; - out_1250847176697915574[2] = 0; - out_1250847176697915574[3] = 1; - out_1250847176697915574[4] = 0; - out_1250847176697915574[5] = 0; - out_1250847176697915574[6] = 0; - out_1250847176697915574[7] = 0; - out_1250847176697915574[8] = 0; +void H_27(double *state, double *unused, double *out_208534858952249577) { + out_208534858952249577[0] = 0; + out_208534858952249577[1] = 0; + out_208534858952249577[2] = 0; + out_208534858952249577[3] = 1; + out_208534858952249577[4] = 0; + out_208534858952249577[5] = 0; + out_208534858952249577[6] = 0; + out_208534858952249577[7] = 0; + out_208534858952249577[8] = 0; } -void h_29(double *state, double *unused, double *out_1089206735856332165) { - out_1089206735856332165[0] = state[1]; +void h_29(double *state, double *unused, double *out_7850349017976928834) { + out_7850349017976928834[0] = state[1]; } -void H_29(double *state, double *unused, double *out_1434147479416901521) { - out_1434147479416901521[0] = 0; - out_1434147479416901521[1] = 1; - out_1434147479416901521[2] = 0; - out_1434147479416901521[3] = 0; - out_1434147479416901521[4] = 0; - out_1434147479416901521[5] = 0; - out_1434147479416901521[6] = 0; - out_1434147479416901521[7] = 0; - out_1434147479416901521[8] = 0; +void H_29(double *state, double *unused, double *out_2893529515067066672) { + out_2893529515067066672[0] = 0; + out_2893529515067066672[1] = 1; + out_2893529515067066672[2] = 0; + out_2893529515067066672[3] = 0; + out_2893529515067066672[4] = 0; + out_2893529515067066672[5] = 0; + out_2893529515067066672[6] = 0; + out_2893529515067066672[7] = 0; + out_2893529515067066672[8] = 0; } -void h_28(double *state, double *unused, double *out_3715986593473831922) { - out_3715986593473831922[0] = state[0]; +void h_28(double *state, double *unused, double *out_3123869812600589078) { + out_3123869812600589078[0] = state[0]; } -void H_28(double *state, double *unused, double *out_3648251537652629053) { - out_3648251537652629053[0] = 1; - out_3648251537652629053[1] = 0; - out_3648251537652629053[2] = 0; - out_3648251537652629053[3] = 0; - out_3648251537652629053[4] = 0; - out_3648251537652629053[5] = 0; - out_3648251537652629053[6] = 0; - out_3648251537652629053[7] = 0; - out_3648251537652629053[8] = 0; +void H_28(double *state, double *unused, double *out_2188869502002463902) { + out_2188869502002463902[0] = 1; + out_2188869502002463902[1] = 0; + out_2188869502002463902[2] = 0; + out_2188869502002463902[3] = 0; + out_2188869502002463902[4] = 0; + out_2188869502002463902[5] = 0; + out_2188869502002463902[6] = 0; + out_2188869502002463902[7] = 0; + out_2188869502002463902[8] = 0; } -void h_31(double *state, double *unused, double *out_10884751412342691) { - out_10884751412342691[0] = state[8]; +void h_31(double *state, double *unused, double *out_6770159118416154415) { + out_6770159118416154415[0] = state[8]; } -void H_31(double *state, double *unused, double *out_5962128244512146990) { - out_5962128244512146990[0] = 0; - out_5962128244512146990[1] = 0; - out_5962128244512146990[2] = 0; - out_5962128244512146990[3] = 0; - out_5962128244512146990[4] = 0; - out_5962128244512146990[5] = 0; - out_5962128244512146990[6] = 0; - out_5962128244512146990[7] = 0; - out_5962128244512146990[8] = 1; +void H_31(double *state, double *unused, double *out_2543283079772874986) { + out_2543283079772874986[0] = 0; + out_2543283079772874986[1] = 0; + out_2543283079772874986[2] = 0; + out_2543283079772874986[3] = 0; + out_2543283079772874986[4] = 0; + out_2543283079772874986[5] = 0; + out_2543283079772874986[6] = 0; + out_2543283079772874986[7] = 0; + out_2543283079772874986[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_411609516403737386) { - err_fun(nom_x, delta_x, out_411609516403737386); +void car_err_fun(double *nom_x, double *delta_x, double *out_4530123797725818472) { + err_fun(nom_x, delta_x, out_4530123797725818472); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7082078735183717431) { - inv_err_fun(nom_x, true_x, out_7082078735183717431); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8174571641545541533) { + inv_err_fun(nom_x, true_x, out_8174571641545541533); } -void car_H_mod_fun(double *state, double *out_1390740467130861487) { - H_mod_fun(state, out_1390740467130861487); +void car_H_mod_fun(double *state, double *out_1010343532093577119) { + H_mod_fun(state, out_1010343532093577119); } -void car_f_fun(double *state, double dt, double *out_2246396216540142177) { - f_fun(state, dt, out_2246396216540142177); +void car_f_fun(double *state, double dt, double *out_8102414924978153817) { + f_fun(state, dt, out_8102414924978153817); } -void car_F_fun(double *state, double dt, double *out_3263326103915417388) { - F_fun(state, dt, out_3263326103915417388); +void car_F_fun(double *state, double dt, double *out_1635199146977261472) { + F_fun(state, dt, out_1635199146977261472); } -void car_h_25(double *state, double *unused, double *out_7774150008233015132) { - h_25(state, unused, out_7774150008233015132); +void car_h_25(double *state, double *unused, double *out_7182033227359772288) { + h_25(state, unused, out_7182033227359772288); } -void car_H_25(double *state, double *unused, double *out_1594416823404739290) { - H_25(state, unused, out_1594416823404739290); +void car_H_25(double *state, double *unused, double *out_6910994500880282686) { + H_25(state, unused, out_6910994500880282686); } -void car_h_24(double *state, double *unused, double *out_3517564190390228050) { - h_24(state, unused, out_3517564190390228050); +void car_h_24(double *state, double *unused, double *out_8844123752507495678) { + h_24(state, unused, out_8844123752507495678); } -void car_H_24(double *state, double *unused, double *out_5823974810272187603) { - H_24(state, unused, out_5823974810272187603); +void car_H_24(double *state, double *unused, double *out_9166842576004100315) { + H_24(state, unused, out_9166842576004100315); } -void car_h_30(double *state, double *unused, double *out_3901758839126185548) { - h_30(state, unused, out_3901758839126185548); +void car_h_30(double *state, double *unused, double *out_6906839165075266399) { + h_30(state, unused, out_6906839165075266399); } -void car_H_30(double *state, double *unused, double *out_923916135102509337) { - H_30(state, unused, out_923916135102509337); +void car_H_30(double *state, double *unused, double *out_2383298170752674488) { + H_30(state, unused, out_2383298170752674488); } -void car_h_26(double *state, double *unused, double *out_4894550625139811592) { - h_26(state, unused, out_4894550625139811592); +void car_h_26(double *state, double *unused, double *out_6538231640973439572) { + h_26(state, unused, out_6538231640973439572); } -void car_H_26(double *state, double *unused, double *out_5335920142278795514) { - H_26(state, unused, out_5335920142278795514); +void car_H_26(double *state, double *unused, double *out_3169491182006226462) { + H_26(state, unused, out_3169491182006226462); } -void car_h_27(double *state, double *unused, double *out_8717659861134677567) { - h_27(state, unused, out_8717659861134677567); +void car_h_27(double *state, double *unused, double *out_8125543080261434723) { + h_27(state, unused, out_8125543080261434723); } -void car_H_27(double *state, double *unused, double *out_1250847176697915574) { - H_27(state, unused, out_1250847176697915574); +void car_H_27(double *state, double *unused, double *out_208534858952249577) { + H_27(state, unused, out_208534858952249577); } -void car_h_29(double *state, double *unused, double *out_1089206735856332165) { - h_29(state, unused, out_1089206735856332165); +void car_h_29(double *state, double *unused, double *out_7850349017976928834) { + h_29(state, unused, out_7850349017976928834); } -void car_H_29(double *state, double *unused, double *out_1434147479416901521) { - H_29(state, unused, out_1434147479416901521); +void car_H_29(double *state, double *unused, double *out_2893529515067066672) { + H_29(state, unused, out_2893529515067066672); } -void car_h_28(double *state, double *unused, double *out_3715986593473831922) { - h_28(state, unused, out_3715986593473831922); +void car_h_28(double *state, double *unused, double *out_3123869812600589078) { + h_28(state, unused, out_3123869812600589078); } -void car_H_28(double *state, double *unused, double *out_3648251537652629053) { - H_28(state, unused, out_3648251537652629053); +void car_H_28(double *state, double *unused, double *out_2188869502002463902) { + H_28(state, unused, out_2188869502002463902); } -void car_h_31(double *state, double *unused, double *out_10884751412342691) { - h_31(state, unused, out_10884751412342691); +void car_h_31(double *state, double *unused, double *out_6770159118416154415) { + h_31(state, unused, out_6770159118416154415); } -void car_H_31(double *state, double *unused, double *out_5962128244512146990) { - H_31(state, unused, out_5962128244512146990); +void car_H_31(double *state, double *unused, double *out_2543283079772874986) { + H_31(state, unused, out_2543283079772874986); } 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 e67dbc01c..a6f984b3b 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_411609516403737386); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7082078735183717431); -void car_H_mod_fun(double *state, double *out_1390740467130861487); -void car_f_fun(double *state, double dt, double *out_2246396216540142177); -void car_F_fun(double *state, double dt, double *out_3263326103915417388); -void car_h_25(double *state, double *unused, double *out_7774150008233015132); -void car_H_25(double *state, double *unused, double *out_1594416823404739290); -void car_h_24(double *state, double *unused, double *out_3517564190390228050); -void car_H_24(double *state, double *unused, double *out_5823974810272187603); -void car_h_30(double *state, double *unused, double *out_3901758839126185548); -void car_H_30(double *state, double *unused, double *out_923916135102509337); -void car_h_26(double *state, double *unused, double *out_4894550625139811592); -void car_H_26(double *state, double *unused, double *out_5335920142278795514); -void car_h_27(double *state, double *unused, double *out_8717659861134677567); -void car_H_27(double *state, double *unused, double *out_1250847176697915574); -void car_h_29(double *state, double *unused, double *out_1089206735856332165); -void car_H_29(double *state, double *unused, double *out_1434147479416901521); -void car_h_28(double *state, double *unused, double *out_3715986593473831922); -void car_H_28(double *state, double *unused, double *out_3648251537652629053); -void car_h_31(double *state, double *unused, double *out_10884751412342691); -void car_H_31(double *state, double *unused, double *out_5962128244512146990); +void car_err_fun(double *nom_x, double *delta_x, double *out_4530123797725818472); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8174571641545541533); +void car_H_mod_fun(double *state, double *out_1010343532093577119); +void car_f_fun(double *state, double dt, double *out_8102414924978153817); +void car_F_fun(double *state, double dt, double *out_1635199146977261472); +void car_h_25(double *state, double *unused, double *out_7182033227359772288); +void car_H_25(double *state, double *unused, double *out_6910994500880282686); +void car_h_24(double *state, double *unused, double *out_8844123752507495678); +void car_H_24(double *state, double *unused, double *out_9166842576004100315); +void car_h_30(double *state, double *unused, double *out_6906839165075266399); +void car_H_30(double *state, double *unused, double *out_2383298170752674488); +void car_h_26(double *state, double *unused, double *out_6538231640973439572); +void car_H_26(double *state, double *unused, double *out_3169491182006226462); +void car_h_27(double *state, double *unused, double *out_8125543080261434723); +void car_H_27(double *state, double *unused, double *out_208534858952249577); +void car_h_29(double *state, double *unused, double *out_7850349017976928834); +void car_H_29(double *state, double *unused, double *out_2893529515067066672); +void car_h_28(double *state, double *unused, double *out_3123869812600589078); +void car_H_28(double *state, double *unused, double *out_2188869502002463902); +void car_h_31(double *state, double *unused, double *out_6770159118416154415); +void car_H_31(double *state, double *unused, double *out_2543283079772874986); 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 73627aa71..35d8bae49 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_3060982030804230470) { - out_3060982030804230470[0] = delta_x[0] + nom_x[0]; - out_3060982030804230470[1] = delta_x[1] + nom_x[1]; - out_3060982030804230470[2] = delta_x[2] + nom_x[2]; - out_3060982030804230470[3] = delta_x[3] + nom_x[3]; - out_3060982030804230470[4] = delta_x[4] + nom_x[4]; - out_3060982030804230470[5] = delta_x[5] + nom_x[5]; - out_3060982030804230470[6] = delta_x[6] + nom_x[6]; - out_3060982030804230470[7] = delta_x[7] + nom_x[7]; - out_3060982030804230470[8] = delta_x[8] + nom_x[8]; - out_3060982030804230470[9] = delta_x[9] + nom_x[9]; - out_3060982030804230470[10] = delta_x[10] + nom_x[10]; - out_3060982030804230470[11] = delta_x[11] + nom_x[11]; - out_3060982030804230470[12] = delta_x[12] + nom_x[12]; - out_3060982030804230470[13] = delta_x[13] + nom_x[13]; - out_3060982030804230470[14] = delta_x[14] + nom_x[14]; - out_3060982030804230470[15] = delta_x[15] + nom_x[15]; - out_3060982030804230470[16] = delta_x[16] + nom_x[16]; - out_3060982030804230470[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_1903969700519405728) { + out_1903969700519405728[0] = delta_x[0] + nom_x[0]; + out_1903969700519405728[1] = delta_x[1] + nom_x[1]; + out_1903969700519405728[2] = delta_x[2] + nom_x[2]; + out_1903969700519405728[3] = delta_x[3] + nom_x[3]; + out_1903969700519405728[4] = delta_x[4] + nom_x[4]; + out_1903969700519405728[5] = delta_x[5] + nom_x[5]; + out_1903969700519405728[6] = delta_x[6] + nom_x[6]; + out_1903969700519405728[7] = delta_x[7] + nom_x[7]; + out_1903969700519405728[8] = delta_x[8] + nom_x[8]; + out_1903969700519405728[9] = delta_x[9] + nom_x[9]; + out_1903969700519405728[10] = delta_x[10] + nom_x[10]; + out_1903969700519405728[11] = delta_x[11] + nom_x[11]; + out_1903969700519405728[12] = delta_x[12] + nom_x[12]; + out_1903969700519405728[13] = delta_x[13] + nom_x[13]; + out_1903969700519405728[14] = delta_x[14] + nom_x[14]; + out_1903969700519405728[15] = delta_x[15] + nom_x[15]; + out_1903969700519405728[16] = delta_x[16] + nom_x[16]; + out_1903969700519405728[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1893009693429340384) { - out_1893009693429340384[0] = -nom_x[0] + true_x[0]; - out_1893009693429340384[1] = -nom_x[1] + true_x[1]; - out_1893009693429340384[2] = -nom_x[2] + true_x[2]; - out_1893009693429340384[3] = -nom_x[3] + true_x[3]; - out_1893009693429340384[4] = -nom_x[4] + true_x[4]; - out_1893009693429340384[5] = -nom_x[5] + true_x[5]; - out_1893009693429340384[6] = -nom_x[6] + true_x[6]; - out_1893009693429340384[7] = -nom_x[7] + true_x[7]; - out_1893009693429340384[8] = -nom_x[8] + true_x[8]; - out_1893009693429340384[9] = -nom_x[9] + true_x[9]; - out_1893009693429340384[10] = -nom_x[10] + true_x[10]; - out_1893009693429340384[11] = -nom_x[11] + true_x[11]; - out_1893009693429340384[12] = -nom_x[12] + true_x[12]; - out_1893009693429340384[13] = -nom_x[13] + true_x[13]; - out_1893009693429340384[14] = -nom_x[14] + true_x[14]; - out_1893009693429340384[15] = -nom_x[15] + true_x[15]; - out_1893009693429340384[16] = -nom_x[16] + true_x[16]; - out_1893009693429340384[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3362098330798648840) { + out_3362098330798648840[0] = -nom_x[0] + true_x[0]; + out_3362098330798648840[1] = -nom_x[1] + true_x[1]; + out_3362098330798648840[2] = -nom_x[2] + true_x[2]; + out_3362098330798648840[3] = -nom_x[3] + true_x[3]; + out_3362098330798648840[4] = -nom_x[4] + true_x[4]; + out_3362098330798648840[5] = -nom_x[5] + true_x[5]; + out_3362098330798648840[6] = -nom_x[6] + true_x[6]; + out_3362098330798648840[7] = -nom_x[7] + true_x[7]; + out_3362098330798648840[8] = -nom_x[8] + true_x[8]; + out_3362098330798648840[9] = -nom_x[9] + true_x[9]; + out_3362098330798648840[10] = -nom_x[10] + true_x[10]; + out_3362098330798648840[11] = -nom_x[11] + true_x[11]; + out_3362098330798648840[12] = -nom_x[12] + true_x[12]; + out_3362098330798648840[13] = -nom_x[13] + true_x[13]; + out_3362098330798648840[14] = -nom_x[14] + true_x[14]; + out_3362098330798648840[15] = -nom_x[15] + true_x[15]; + out_3362098330798648840[16] = -nom_x[16] + true_x[16]; + out_3362098330798648840[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_8923486826760505000) { - out_8923486826760505000[0] = 1.0; - out_8923486826760505000[1] = 0.0; - out_8923486826760505000[2] = 0.0; - out_8923486826760505000[3] = 0.0; - out_8923486826760505000[4] = 0.0; - out_8923486826760505000[5] = 0.0; - out_8923486826760505000[6] = 0.0; - out_8923486826760505000[7] = 0.0; - out_8923486826760505000[8] = 0.0; - out_8923486826760505000[9] = 0.0; - out_8923486826760505000[10] = 0.0; - out_8923486826760505000[11] = 0.0; - out_8923486826760505000[12] = 0.0; - out_8923486826760505000[13] = 0.0; - out_8923486826760505000[14] = 0.0; - out_8923486826760505000[15] = 0.0; - out_8923486826760505000[16] = 0.0; - out_8923486826760505000[17] = 0.0; - out_8923486826760505000[18] = 0.0; - out_8923486826760505000[19] = 1.0; - out_8923486826760505000[20] = 0.0; - out_8923486826760505000[21] = 0.0; - out_8923486826760505000[22] = 0.0; - out_8923486826760505000[23] = 0.0; - out_8923486826760505000[24] = 0.0; - out_8923486826760505000[25] = 0.0; - out_8923486826760505000[26] = 0.0; - out_8923486826760505000[27] = 0.0; - out_8923486826760505000[28] = 0.0; - out_8923486826760505000[29] = 0.0; - out_8923486826760505000[30] = 0.0; - out_8923486826760505000[31] = 0.0; - out_8923486826760505000[32] = 0.0; - out_8923486826760505000[33] = 0.0; - out_8923486826760505000[34] = 0.0; - out_8923486826760505000[35] = 0.0; - out_8923486826760505000[36] = 0.0; - out_8923486826760505000[37] = 0.0; - out_8923486826760505000[38] = 1.0; - out_8923486826760505000[39] = 0.0; - out_8923486826760505000[40] = 0.0; - out_8923486826760505000[41] = 0.0; - out_8923486826760505000[42] = 0.0; - out_8923486826760505000[43] = 0.0; - out_8923486826760505000[44] = 0.0; - out_8923486826760505000[45] = 0.0; - out_8923486826760505000[46] = 0.0; - out_8923486826760505000[47] = 0.0; - out_8923486826760505000[48] = 0.0; - out_8923486826760505000[49] = 0.0; - out_8923486826760505000[50] = 0.0; - out_8923486826760505000[51] = 0.0; - out_8923486826760505000[52] = 0.0; - out_8923486826760505000[53] = 0.0; - out_8923486826760505000[54] = 0.0; - out_8923486826760505000[55] = 0.0; - out_8923486826760505000[56] = 0.0; - out_8923486826760505000[57] = 1.0; - out_8923486826760505000[58] = 0.0; - out_8923486826760505000[59] = 0.0; - out_8923486826760505000[60] = 0.0; - out_8923486826760505000[61] = 0.0; - out_8923486826760505000[62] = 0.0; - out_8923486826760505000[63] = 0.0; - out_8923486826760505000[64] = 0.0; - out_8923486826760505000[65] = 0.0; - out_8923486826760505000[66] = 0.0; - out_8923486826760505000[67] = 0.0; - out_8923486826760505000[68] = 0.0; - out_8923486826760505000[69] = 0.0; - out_8923486826760505000[70] = 0.0; - out_8923486826760505000[71] = 0.0; - out_8923486826760505000[72] = 0.0; - out_8923486826760505000[73] = 0.0; - out_8923486826760505000[74] = 0.0; - out_8923486826760505000[75] = 0.0; - out_8923486826760505000[76] = 1.0; - out_8923486826760505000[77] = 0.0; - out_8923486826760505000[78] = 0.0; - out_8923486826760505000[79] = 0.0; - out_8923486826760505000[80] = 0.0; - out_8923486826760505000[81] = 0.0; - out_8923486826760505000[82] = 0.0; - out_8923486826760505000[83] = 0.0; - out_8923486826760505000[84] = 0.0; - out_8923486826760505000[85] = 0.0; - out_8923486826760505000[86] = 0.0; - out_8923486826760505000[87] = 0.0; - out_8923486826760505000[88] = 0.0; - out_8923486826760505000[89] = 0.0; - out_8923486826760505000[90] = 0.0; - out_8923486826760505000[91] = 0.0; - out_8923486826760505000[92] = 0.0; - out_8923486826760505000[93] = 0.0; - out_8923486826760505000[94] = 0.0; - out_8923486826760505000[95] = 1.0; - out_8923486826760505000[96] = 0.0; - out_8923486826760505000[97] = 0.0; - out_8923486826760505000[98] = 0.0; - out_8923486826760505000[99] = 0.0; - out_8923486826760505000[100] = 0.0; - out_8923486826760505000[101] = 0.0; - out_8923486826760505000[102] = 0.0; - out_8923486826760505000[103] = 0.0; - out_8923486826760505000[104] = 0.0; - out_8923486826760505000[105] = 0.0; - out_8923486826760505000[106] = 0.0; - out_8923486826760505000[107] = 0.0; - out_8923486826760505000[108] = 0.0; - out_8923486826760505000[109] = 0.0; - out_8923486826760505000[110] = 0.0; - out_8923486826760505000[111] = 0.0; - out_8923486826760505000[112] = 0.0; - out_8923486826760505000[113] = 0.0; - out_8923486826760505000[114] = 1.0; - out_8923486826760505000[115] = 0.0; - out_8923486826760505000[116] = 0.0; - out_8923486826760505000[117] = 0.0; - out_8923486826760505000[118] = 0.0; - out_8923486826760505000[119] = 0.0; - out_8923486826760505000[120] = 0.0; - out_8923486826760505000[121] = 0.0; - out_8923486826760505000[122] = 0.0; - out_8923486826760505000[123] = 0.0; - out_8923486826760505000[124] = 0.0; - out_8923486826760505000[125] = 0.0; - out_8923486826760505000[126] = 0.0; - out_8923486826760505000[127] = 0.0; - out_8923486826760505000[128] = 0.0; - out_8923486826760505000[129] = 0.0; - out_8923486826760505000[130] = 0.0; - out_8923486826760505000[131] = 0.0; - out_8923486826760505000[132] = 0.0; - out_8923486826760505000[133] = 1.0; - out_8923486826760505000[134] = 0.0; - out_8923486826760505000[135] = 0.0; - out_8923486826760505000[136] = 0.0; - out_8923486826760505000[137] = 0.0; - out_8923486826760505000[138] = 0.0; - out_8923486826760505000[139] = 0.0; - out_8923486826760505000[140] = 0.0; - out_8923486826760505000[141] = 0.0; - out_8923486826760505000[142] = 0.0; - out_8923486826760505000[143] = 0.0; - out_8923486826760505000[144] = 0.0; - out_8923486826760505000[145] = 0.0; - out_8923486826760505000[146] = 0.0; - out_8923486826760505000[147] = 0.0; - out_8923486826760505000[148] = 0.0; - out_8923486826760505000[149] = 0.0; - out_8923486826760505000[150] = 0.0; - out_8923486826760505000[151] = 0.0; - out_8923486826760505000[152] = 1.0; - out_8923486826760505000[153] = 0.0; - out_8923486826760505000[154] = 0.0; - out_8923486826760505000[155] = 0.0; - out_8923486826760505000[156] = 0.0; - out_8923486826760505000[157] = 0.0; - out_8923486826760505000[158] = 0.0; - out_8923486826760505000[159] = 0.0; - out_8923486826760505000[160] = 0.0; - out_8923486826760505000[161] = 0.0; - out_8923486826760505000[162] = 0.0; - out_8923486826760505000[163] = 0.0; - out_8923486826760505000[164] = 0.0; - out_8923486826760505000[165] = 0.0; - out_8923486826760505000[166] = 0.0; - out_8923486826760505000[167] = 0.0; - out_8923486826760505000[168] = 0.0; - out_8923486826760505000[169] = 0.0; - out_8923486826760505000[170] = 0.0; - out_8923486826760505000[171] = 1.0; - out_8923486826760505000[172] = 0.0; - out_8923486826760505000[173] = 0.0; - out_8923486826760505000[174] = 0.0; - out_8923486826760505000[175] = 0.0; - out_8923486826760505000[176] = 0.0; - out_8923486826760505000[177] = 0.0; - out_8923486826760505000[178] = 0.0; - out_8923486826760505000[179] = 0.0; - out_8923486826760505000[180] = 0.0; - out_8923486826760505000[181] = 0.0; - out_8923486826760505000[182] = 0.0; - out_8923486826760505000[183] = 0.0; - out_8923486826760505000[184] = 0.0; - out_8923486826760505000[185] = 0.0; - out_8923486826760505000[186] = 0.0; - out_8923486826760505000[187] = 0.0; - out_8923486826760505000[188] = 0.0; - out_8923486826760505000[189] = 0.0; - out_8923486826760505000[190] = 1.0; - out_8923486826760505000[191] = 0.0; - out_8923486826760505000[192] = 0.0; - out_8923486826760505000[193] = 0.0; - out_8923486826760505000[194] = 0.0; - out_8923486826760505000[195] = 0.0; - out_8923486826760505000[196] = 0.0; - out_8923486826760505000[197] = 0.0; - out_8923486826760505000[198] = 0.0; - out_8923486826760505000[199] = 0.0; - out_8923486826760505000[200] = 0.0; - out_8923486826760505000[201] = 0.0; - out_8923486826760505000[202] = 0.0; - out_8923486826760505000[203] = 0.0; - out_8923486826760505000[204] = 0.0; - out_8923486826760505000[205] = 0.0; - out_8923486826760505000[206] = 0.0; - out_8923486826760505000[207] = 0.0; - out_8923486826760505000[208] = 0.0; - out_8923486826760505000[209] = 1.0; - out_8923486826760505000[210] = 0.0; - out_8923486826760505000[211] = 0.0; - out_8923486826760505000[212] = 0.0; - out_8923486826760505000[213] = 0.0; - out_8923486826760505000[214] = 0.0; - out_8923486826760505000[215] = 0.0; - out_8923486826760505000[216] = 0.0; - out_8923486826760505000[217] = 0.0; - out_8923486826760505000[218] = 0.0; - out_8923486826760505000[219] = 0.0; - out_8923486826760505000[220] = 0.0; - out_8923486826760505000[221] = 0.0; - out_8923486826760505000[222] = 0.0; - out_8923486826760505000[223] = 0.0; - out_8923486826760505000[224] = 0.0; - out_8923486826760505000[225] = 0.0; - out_8923486826760505000[226] = 0.0; - out_8923486826760505000[227] = 0.0; - out_8923486826760505000[228] = 1.0; - out_8923486826760505000[229] = 0.0; - out_8923486826760505000[230] = 0.0; - out_8923486826760505000[231] = 0.0; - out_8923486826760505000[232] = 0.0; - out_8923486826760505000[233] = 0.0; - out_8923486826760505000[234] = 0.0; - out_8923486826760505000[235] = 0.0; - out_8923486826760505000[236] = 0.0; - out_8923486826760505000[237] = 0.0; - out_8923486826760505000[238] = 0.0; - out_8923486826760505000[239] = 0.0; - out_8923486826760505000[240] = 0.0; - out_8923486826760505000[241] = 0.0; - out_8923486826760505000[242] = 0.0; - out_8923486826760505000[243] = 0.0; - out_8923486826760505000[244] = 0.0; - out_8923486826760505000[245] = 0.0; - out_8923486826760505000[246] = 0.0; - out_8923486826760505000[247] = 1.0; - out_8923486826760505000[248] = 0.0; - out_8923486826760505000[249] = 0.0; - out_8923486826760505000[250] = 0.0; - out_8923486826760505000[251] = 0.0; - out_8923486826760505000[252] = 0.0; - out_8923486826760505000[253] = 0.0; - out_8923486826760505000[254] = 0.0; - out_8923486826760505000[255] = 0.0; - out_8923486826760505000[256] = 0.0; - out_8923486826760505000[257] = 0.0; - out_8923486826760505000[258] = 0.0; - out_8923486826760505000[259] = 0.0; - out_8923486826760505000[260] = 0.0; - out_8923486826760505000[261] = 0.0; - out_8923486826760505000[262] = 0.0; - out_8923486826760505000[263] = 0.0; - out_8923486826760505000[264] = 0.0; - out_8923486826760505000[265] = 0.0; - out_8923486826760505000[266] = 1.0; - out_8923486826760505000[267] = 0.0; - out_8923486826760505000[268] = 0.0; - out_8923486826760505000[269] = 0.0; - out_8923486826760505000[270] = 0.0; - out_8923486826760505000[271] = 0.0; - out_8923486826760505000[272] = 0.0; - out_8923486826760505000[273] = 0.0; - out_8923486826760505000[274] = 0.0; - out_8923486826760505000[275] = 0.0; - out_8923486826760505000[276] = 0.0; - out_8923486826760505000[277] = 0.0; - out_8923486826760505000[278] = 0.0; - out_8923486826760505000[279] = 0.0; - out_8923486826760505000[280] = 0.0; - out_8923486826760505000[281] = 0.0; - out_8923486826760505000[282] = 0.0; - out_8923486826760505000[283] = 0.0; - out_8923486826760505000[284] = 0.0; - out_8923486826760505000[285] = 1.0; - out_8923486826760505000[286] = 0.0; - out_8923486826760505000[287] = 0.0; - out_8923486826760505000[288] = 0.0; - out_8923486826760505000[289] = 0.0; - out_8923486826760505000[290] = 0.0; - out_8923486826760505000[291] = 0.0; - out_8923486826760505000[292] = 0.0; - out_8923486826760505000[293] = 0.0; - out_8923486826760505000[294] = 0.0; - out_8923486826760505000[295] = 0.0; - out_8923486826760505000[296] = 0.0; - out_8923486826760505000[297] = 0.0; - out_8923486826760505000[298] = 0.0; - out_8923486826760505000[299] = 0.0; - out_8923486826760505000[300] = 0.0; - out_8923486826760505000[301] = 0.0; - out_8923486826760505000[302] = 0.0; - out_8923486826760505000[303] = 0.0; - out_8923486826760505000[304] = 1.0; - out_8923486826760505000[305] = 0.0; - out_8923486826760505000[306] = 0.0; - out_8923486826760505000[307] = 0.0; - out_8923486826760505000[308] = 0.0; - out_8923486826760505000[309] = 0.0; - out_8923486826760505000[310] = 0.0; - out_8923486826760505000[311] = 0.0; - out_8923486826760505000[312] = 0.0; - out_8923486826760505000[313] = 0.0; - out_8923486826760505000[314] = 0.0; - out_8923486826760505000[315] = 0.0; - out_8923486826760505000[316] = 0.0; - out_8923486826760505000[317] = 0.0; - out_8923486826760505000[318] = 0.0; - out_8923486826760505000[319] = 0.0; - out_8923486826760505000[320] = 0.0; - out_8923486826760505000[321] = 0.0; - out_8923486826760505000[322] = 0.0; - out_8923486826760505000[323] = 1.0; +void H_mod_fun(double *state, double *out_3822470159260117769) { + out_3822470159260117769[0] = 1.0; + out_3822470159260117769[1] = 0.0; + out_3822470159260117769[2] = 0.0; + out_3822470159260117769[3] = 0.0; + out_3822470159260117769[4] = 0.0; + out_3822470159260117769[5] = 0.0; + out_3822470159260117769[6] = 0.0; + out_3822470159260117769[7] = 0.0; + out_3822470159260117769[8] = 0.0; + out_3822470159260117769[9] = 0.0; + out_3822470159260117769[10] = 0.0; + out_3822470159260117769[11] = 0.0; + out_3822470159260117769[12] = 0.0; + out_3822470159260117769[13] = 0.0; + out_3822470159260117769[14] = 0.0; + out_3822470159260117769[15] = 0.0; + out_3822470159260117769[16] = 0.0; + out_3822470159260117769[17] = 0.0; + out_3822470159260117769[18] = 0.0; + out_3822470159260117769[19] = 1.0; + out_3822470159260117769[20] = 0.0; + out_3822470159260117769[21] = 0.0; + out_3822470159260117769[22] = 0.0; + out_3822470159260117769[23] = 0.0; + out_3822470159260117769[24] = 0.0; + out_3822470159260117769[25] = 0.0; + out_3822470159260117769[26] = 0.0; + out_3822470159260117769[27] = 0.0; + out_3822470159260117769[28] = 0.0; + out_3822470159260117769[29] = 0.0; + out_3822470159260117769[30] = 0.0; + out_3822470159260117769[31] = 0.0; + out_3822470159260117769[32] = 0.0; + out_3822470159260117769[33] = 0.0; + out_3822470159260117769[34] = 0.0; + out_3822470159260117769[35] = 0.0; + out_3822470159260117769[36] = 0.0; + out_3822470159260117769[37] = 0.0; + out_3822470159260117769[38] = 1.0; + out_3822470159260117769[39] = 0.0; + out_3822470159260117769[40] = 0.0; + out_3822470159260117769[41] = 0.0; + out_3822470159260117769[42] = 0.0; + out_3822470159260117769[43] = 0.0; + out_3822470159260117769[44] = 0.0; + out_3822470159260117769[45] = 0.0; + out_3822470159260117769[46] = 0.0; + out_3822470159260117769[47] = 0.0; + out_3822470159260117769[48] = 0.0; + out_3822470159260117769[49] = 0.0; + out_3822470159260117769[50] = 0.0; + out_3822470159260117769[51] = 0.0; + out_3822470159260117769[52] = 0.0; + out_3822470159260117769[53] = 0.0; + out_3822470159260117769[54] = 0.0; + out_3822470159260117769[55] = 0.0; + out_3822470159260117769[56] = 0.0; + out_3822470159260117769[57] = 1.0; + out_3822470159260117769[58] = 0.0; + out_3822470159260117769[59] = 0.0; + out_3822470159260117769[60] = 0.0; + out_3822470159260117769[61] = 0.0; + out_3822470159260117769[62] = 0.0; + out_3822470159260117769[63] = 0.0; + out_3822470159260117769[64] = 0.0; + out_3822470159260117769[65] = 0.0; + out_3822470159260117769[66] = 0.0; + out_3822470159260117769[67] = 0.0; + out_3822470159260117769[68] = 0.0; + out_3822470159260117769[69] = 0.0; + out_3822470159260117769[70] = 0.0; + out_3822470159260117769[71] = 0.0; + out_3822470159260117769[72] = 0.0; + out_3822470159260117769[73] = 0.0; + out_3822470159260117769[74] = 0.0; + out_3822470159260117769[75] = 0.0; + out_3822470159260117769[76] = 1.0; + out_3822470159260117769[77] = 0.0; + out_3822470159260117769[78] = 0.0; + out_3822470159260117769[79] = 0.0; + out_3822470159260117769[80] = 0.0; + out_3822470159260117769[81] = 0.0; + out_3822470159260117769[82] = 0.0; + out_3822470159260117769[83] = 0.0; + out_3822470159260117769[84] = 0.0; + out_3822470159260117769[85] = 0.0; + out_3822470159260117769[86] = 0.0; + out_3822470159260117769[87] = 0.0; + out_3822470159260117769[88] = 0.0; + out_3822470159260117769[89] = 0.0; + out_3822470159260117769[90] = 0.0; + out_3822470159260117769[91] = 0.0; + out_3822470159260117769[92] = 0.0; + out_3822470159260117769[93] = 0.0; + out_3822470159260117769[94] = 0.0; + out_3822470159260117769[95] = 1.0; + out_3822470159260117769[96] = 0.0; + out_3822470159260117769[97] = 0.0; + out_3822470159260117769[98] = 0.0; + out_3822470159260117769[99] = 0.0; + out_3822470159260117769[100] = 0.0; + out_3822470159260117769[101] = 0.0; + out_3822470159260117769[102] = 0.0; + out_3822470159260117769[103] = 0.0; + out_3822470159260117769[104] = 0.0; + out_3822470159260117769[105] = 0.0; + out_3822470159260117769[106] = 0.0; + out_3822470159260117769[107] = 0.0; + out_3822470159260117769[108] = 0.0; + out_3822470159260117769[109] = 0.0; + out_3822470159260117769[110] = 0.0; + out_3822470159260117769[111] = 0.0; + out_3822470159260117769[112] = 0.0; + out_3822470159260117769[113] = 0.0; + out_3822470159260117769[114] = 1.0; + out_3822470159260117769[115] = 0.0; + out_3822470159260117769[116] = 0.0; + out_3822470159260117769[117] = 0.0; + out_3822470159260117769[118] = 0.0; + out_3822470159260117769[119] = 0.0; + out_3822470159260117769[120] = 0.0; + out_3822470159260117769[121] = 0.0; + out_3822470159260117769[122] = 0.0; + out_3822470159260117769[123] = 0.0; + out_3822470159260117769[124] = 0.0; + out_3822470159260117769[125] = 0.0; + out_3822470159260117769[126] = 0.0; + out_3822470159260117769[127] = 0.0; + out_3822470159260117769[128] = 0.0; + out_3822470159260117769[129] = 0.0; + out_3822470159260117769[130] = 0.0; + out_3822470159260117769[131] = 0.0; + out_3822470159260117769[132] = 0.0; + out_3822470159260117769[133] = 1.0; + out_3822470159260117769[134] = 0.0; + out_3822470159260117769[135] = 0.0; + out_3822470159260117769[136] = 0.0; + out_3822470159260117769[137] = 0.0; + out_3822470159260117769[138] = 0.0; + out_3822470159260117769[139] = 0.0; + out_3822470159260117769[140] = 0.0; + out_3822470159260117769[141] = 0.0; + out_3822470159260117769[142] = 0.0; + out_3822470159260117769[143] = 0.0; + out_3822470159260117769[144] = 0.0; + out_3822470159260117769[145] = 0.0; + out_3822470159260117769[146] = 0.0; + out_3822470159260117769[147] = 0.0; + out_3822470159260117769[148] = 0.0; + out_3822470159260117769[149] = 0.0; + out_3822470159260117769[150] = 0.0; + out_3822470159260117769[151] = 0.0; + out_3822470159260117769[152] = 1.0; + out_3822470159260117769[153] = 0.0; + out_3822470159260117769[154] = 0.0; + out_3822470159260117769[155] = 0.0; + out_3822470159260117769[156] = 0.0; + out_3822470159260117769[157] = 0.0; + out_3822470159260117769[158] = 0.0; + out_3822470159260117769[159] = 0.0; + out_3822470159260117769[160] = 0.0; + out_3822470159260117769[161] = 0.0; + out_3822470159260117769[162] = 0.0; + out_3822470159260117769[163] = 0.0; + out_3822470159260117769[164] = 0.0; + out_3822470159260117769[165] = 0.0; + out_3822470159260117769[166] = 0.0; + out_3822470159260117769[167] = 0.0; + out_3822470159260117769[168] = 0.0; + out_3822470159260117769[169] = 0.0; + out_3822470159260117769[170] = 0.0; + out_3822470159260117769[171] = 1.0; + out_3822470159260117769[172] = 0.0; + out_3822470159260117769[173] = 0.0; + out_3822470159260117769[174] = 0.0; + out_3822470159260117769[175] = 0.0; + out_3822470159260117769[176] = 0.0; + out_3822470159260117769[177] = 0.0; + out_3822470159260117769[178] = 0.0; + out_3822470159260117769[179] = 0.0; + out_3822470159260117769[180] = 0.0; + out_3822470159260117769[181] = 0.0; + out_3822470159260117769[182] = 0.0; + out_3822470159260117769[183] = 0.0; + out_3822470159260117769[184] = 0.0; + out_3822470159260117769[185] = 0.0; + out_3822470159260117769[186] = 0.0; + out_3822470159260117769[187] = 0.0; + out_3822470159260117769[188] = 0.0; + out_3822470159260117769[189] = 0.0; + out_3822470159260117769[190] = 1.0; + out_3822470159260117769[191] = 0.0; + out_3822470159260117769[192] = 0.0; + out_3822470159260117769[193] = 0.0; + out_3822470159260117769[194] = 0.0; + out_3822470159260117769[195] = 0.0; + out_3822470159260117769[196] = 0.0; + out_3822470159260117769[197] = 0.0; + out_3822470159260117769[198] = 0.0; + out_3822470159260117769[199] = 0.0; + out_3822470159260117769[200] = 0.0; + out_3822470159260117769[201] = 0.0; + out_3822470159260117769[202] = 0.0; + out_3822470159260117769[203] = 0.0; + out_3822470159260117769[204] = 0.0; + out_3822470159260117769[205] = 0.0; + out_3822470159260117769[206] = 0.0; + out_3822470159260117769[207] = 0.0; + out_3822470159260117769[208] = 0.0; + out_3822470159260117769[209] = 1.0; + out_3822470159260117769[210] = 0.0; + out_3822470159260117769[211] = 0.0; + out_3822470159260117769[212] = 0.0; + out_3822470159260117769[213] = 0.0; + out_3822470159260117769[214] = 0.0; + out_3822470159260117769[215] = 0.0; + out_3822470159260117769[216] = 0.0; + out_3822470159260117769[217] = 0.0; + out_3822470159260117769[218] = 0.0; + out_3822470159260117769[219] = 0.0; + out_3822470159260117769[220] = 0.0; + out_3822470159260117769[221] = 0.0; + out_3822470159260117769[222] = 0.0; + out_3822470159260117769[223] = 0.0; + out_3822470159260117769[224] = 0.0; + out_3822470159260117769[225] = 0.0; + out_3822470159260117769[226] = 0.0; + out_3822470159260117769[227] = 0.0; + out_3822470159260117769[228] = 1.0; + out_3822470159260117769[229] = 0.0; + out_3822470159260117769[230] = 0.0; + out_3822470159260117769[231] = 0.0; + out_3822470159260117769[232] = 0.0; + out_3822470159260117769[233] = 0.0; + out_3822470159260117769[234] = 0.0; + out_3822470159260117769[235] = 0.0; + out_3822470159260117769[236] = 0.0; + out_3822470159260117769[237] = 0.0; + out_3822470159260117769[238] = 0.0; + out_3822470159260117769[239] = 0.0; + out_3822470159260117769[240] = 0.0; + out_3822470159260117769[241] = 0.0; + out_3822470159260117769[242] = 0.0; + out_3822470159260117769[243] = 0.0; + out_3822470159260117769[244] = 0.0; + out_3822470159260117769[245] = 0.0; + out_3822470159260117769[246] = 0.0; + out_3822470159260117769[247] = 1.0; + out_3822470159260117769[248] = 0.0; + out_3822470159260117769[249] = 0.0; + out_3822470159260117769[250] = 0.0; + out_3822470159260117769[251] = 0.0; + out_3822470159260117769[252] = 0.0; + out_3822470159260117769[253] = 0.0; + out_3822470159260117769[254] = 0.0; + out_3822470159260117769[255] = 0.0; + out_3822470159260117769[256] = 0.0; + out_3822470159260117769[257] = 0.0; + out_3822470159260117769[258] = 0.0; + out_3822470159260117769[259] = 0.0; + out_3822470159260117769[260] = 0.0; + out_3822470159260117769[261] = 0.0; + out_3822470159260117769[262] = 0.0; + out_3822470159260117769[263] = 0.0; + out_3822470159260117769[264] = 0.0; + out_3822470159260117769[265] = 0.0; + out_3822470159260117769[266] = 1.0; + out_3822470159260117769[267] = 0.0; + out_3822470159260117769[268] = 0.0; + out_3822470159260117769[269] = 0.0; + out_3822470159260117769[270] = 0.0; + out_3822470159260117769[271] = 0.0; + out_3822470159260117769[272] = 0.0; + out_3822470159260117769[273] = 0.0; + out_3822470159260117769[274] = 0.0; + out_3822470159260117769[275] = 0.0; + out_3822470159260117769[276] = 0.0; + out_3822470159260117769[277] = 0.0; + out_3822470159260117769[278] = 0.0; + out_3822470159260117769[279] = 0.0; + out_3822470159260117769[280] = 0.0; + out_3822470159260117769[281] = 0.0; + out_3822470159260117769[282] = 0.0; + out_3822470159260117769[283] = 0.0; + out_3822470159260117769[284] = 0.0; + out_3822470159260117769[285] = 1.0; + out_3822470159260117769[286] = 0.0; + out_3822470159260117769[287] = 0.0; + out_3822470159260117769[288] = 0.0; + out_3822470159260117769[289] = 0.0; + out_3822470159260117769[290] = 0.0; + out_3822470159260117769[291] = 0.0; + out_3822470159260117769[292] = 0.0; + out_3822470159260117769[293] = 0.0; + out_3822470159260117769[294] = 0.0; + out_3822470159260117769[295] = 0.0; + out_3822470159260117769[296] = 0.0; + out_3822470159260117769[297] = 0.0; + out_3822470159260117769[298] = 0.0; + out_3822470159260117769[299] = 0.0; + out_3822470159260117769[300] = 0.0; + out_3822470159260117769[301] = 0.0; + out_3822470159260117769[302] = 0.0; + out_3822470159260117769[303] = 0.0; + out_3822470159260117769[304] = 1.0; + out_3822470159260117769[305] = 0.0; + out_3822470159260117769[306] = 0.0; + out_3822470159260117769[307] = 0.0; + out_3822470159260117769[308] = 0.0; + out_3822470159260117769[309] = 0.0; + out_3822470159260117769[310] = 0.0; + out_3822470159260117769[311] = 0.0; + out_3822470159260117769[312] = 0.0; + out_3822470159260117769[313] = 0.0; + out_3822470159260117769[314] = 0.0; + out_3822470159260117769[315] = 0.0; + out_3822470159260117769[316] = 0.0; + out_3822470159260117769[317] = 0.0; + out_3822470159260117769[318] = 0.0; + out_3822470159260117769[319] = 0.0; + out_3822470159260117769[320] = 0.0; + out_3822470159260117769[321] = 0.0; + out_3822470159260117769[322] = 0.0; + out_3822470159260117769[323] = 1.0; } -void f_fun(double *state, double dt, double *out_6932327335752377632) { - out_6932327335752377632[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_6932327335752377632[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_6932327335752377632[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_6932327335752377632[3] = dt*state[12] + state[3]; - out_6932327335752377632[4] = dt*state[13] + state[4]; - out_6932327335752377632[5] = dt*state[14] + state[5]; - out_6932327335752377632[6] = state[6]; - out_6932327335752377632[7] = state[7]; - out_6932327335752377632[8] = state[8]; - out_6932327335752377632[9] = state[9]; - out_6932327335752377632[10] = state[10]; - out_6932327335752377632[11] = state[11]; - out_6932327335752377632[12] = state[12]; - out_6932327335752377632[13] = state[13]; - out_6932327335752377632[14] = state[14]; - out_6932327335752377632[15] = state[15]; - out_6932327335752377632[16] = state[16]; - out_6932327335752377632[17] = state[17]; +void f_fun(double *state, double dt, double *out_4539628665192259779) { + out_4539628665192259779[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_4539628665192259779[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_4539628665192259779[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_4539628665192259779[3] = dt*state[12] + state[3]; + out_4539628665192259779[4] = dt*state[13] + state[4]; + out_4539628665192259779[5] = dt*state[14] + state[5]; + out_4539628665192259779[6] = state[6]; + out_4539628665192259779[7] = state[7]; + out_4539628665192259779[8] = state[8]; + out_4539628665192259779[9] = state[9]; + out_4539628665192259779[10] = state[10]; + out_4539628665192259779[11] = state[11]; + out_4539628665192259779[12] = state[12]; + out_4539628665192259779[13] = state[13]; + out_4539628665192259779[14] = state[14]; + out_4539628665192259779[15] = state[15]; + out_4539628665192259779[16] = state[16]; + out_4539628665192259779[17] = state[17]; } -void F_fun(double *state, double dt, double *out_3364331378734993426) { - out_3364331378734993426[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_3364331378734993426[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_3364331378734993426[2] = 0; - out_3364331378734993426[3] = 0; - out_3364331378734993426[4] = 0; - out_3364331378734993426[5] = 0; - out_3364331378734993426[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_3364331378734993426[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_3364331378734993426[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_3364331378734993426[9] = 0; - out_3364331378734993426[10] = 0; - out_3364331378734993426[11] = 0; - out_3364331378734993426[12] = 0; - out_3364331378734993426[13] = 0; - out_3364331378734993426[14] = 0; - out_3364331378734993426[15] = 0; - out_3364331378734993426[16] = 0; - out_3364331378734993426[17] = 0; - out_3364331378734993426[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_3364331378734993426[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_3364331378734993426[20] = 0; - out_3364331378734993426[21] = 0; - out_3364331378734993426[22] = 0; - out_3364331378734993426[23] = 0; - out_3364331378734993426[24] = 0; - out_3364331378734993426[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_3364331378734993426[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_3364331378734993426[27] = 0; - out_3364331378734993426[28] = 0; - out_3364331378734993426[29] = 0; - out_3364331378734993426[30] = 0; - out_3364331378734993426[31] = 0; - out_3364331378734993426[32] = 0; - out_3364331378734993426[33] = 0; - out_3364331378734993426[34] = 0; - out_3364331378734993426[35] = 0; - out_3364331378734993426[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_3364331378734993426[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_3364331378734993426[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_3364331378734993426[39] = 0; - out_3364331378734993426[40] = 0; - out_3364331378734993426[41] = 0; - out_3364331378734993426[42] = 0; - out_3364331378734993426[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_3364331378734993426[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_3364331378734993426[45] = 0; - out_3364331378734993426[46] = 0; - out_3364331378734993426[47] = 0; - out_3364331378734993426[48] = 0; - out_3364331378734993426[49] = 0; - out_3364331378734993426[50] = 0; - out_3364331378734993426[51] = 0; - out_3364331378734993426[52] = 0; - out_3364331378734993426[53] = 0; - out_3364331378734993426[54] = 0; - out_3364331378734993426[55] = 0; - out_3364331378734993426[56] = 0; - out_3364331378734993426[57] = 1; - out_3364331378734993426[58] = 0; - out_3364331378734993426[59] = 0; - out_3364331378734993426[60] = 0; - out_3364331378734993426[61] = 0; - out_3364331378734993426[62] = 0; - out_3364331378734993426[63] = 0; - out_3364331378734993426[64] = 0; - out_3364331378734993426[65] = 0; - out_3364331378734993426[66] = dt; - out_3364331378734993426[67] = 0; - out_3364331378734993426[68] = 0; - out_3364331378734993426[69] = 0; - out_3364331378734993426[70] = 0; - out_3364331378734993426[71] = 0; - out_3364331378734993426[72] = 0; - out_3364331378734993426[73] = 0; - out_3364331378734993426[74] = 0; - out_3364331378734993426[75] = 0; - out_3364331378734993426[76] = 1; - out_3364331378734993426[77] = 0; - out_3364331378734993426[78] = 0; - out_3364331378734993426[79] = 0; - out_3364331378734993426[80] = 0; - out_3364331378734993426[81] = 0; - out_3364331378734993426[82] = 0; - out_3364331378734993426[83] = 0; - out_3364331378734993426[84] = 0; - out_3364331378734993426[85] = dt; - out_3364331378734993426[86] = 0; - out_3364331378734993426[87] = 0; - out_3364331378734993426[88] = 0; - out_3364331378734993426[89] = 0; - out_3364331378734993426[90] = 0; - out_3364331378734993426[91] = 0; - out_3364331378734993426[92] = 0; - out_3364331378734993426[93] = 0; - out_3364331378734993426[94] = 0; - out_3364331378734993426[95] = 1; - out_3364331378734993426[96] = 0; - out_3364331378734993426[97] = 0; - out_3364331378734993426[98] = 0; - out_3364331378734993426[99] = 0; - out_3364331378734993426[100] = 0; - out_3364331378734993426[101] = 0; - out_3364331378734993426[102] = 0; - out_3364331378734993426[103] = 0; - out_3364331378734993426[104] = dt; - out_3364331378734993426[105] = 0; - out_3364331378734993426[106] = 0; - out_3364331378734993426[107] = 0; - out_3364331378734993426[108] = 0; - out_3364331378734993426[109] = 0; - out_3364331378734993426[110] = 0; - out_3364331378734993426[111] = 0; - out_3364331378734993426[112] = 0; - out_3364331378734993426[113] = 0; - out_3364331378734993426[114] = 1; - out_3364331378734993426[115] = 0; - out_3364331378734993426[116] = 0; - out_3364331378734993426[117] = 0; - out_3364331378734993426[118] = 0; - out_3364331378734993426[119] = 0; - out_3364331378734993426[120] = 0; - out_3364331378734993426[121] = 0; - out_3364331378734993426[122] = 0; - out_3364331378734993426[123] = 0; - out_3364331378734993426[124] = 0; - out_3364331378734993426[125] = 0; - out_3364331378734993426[126] = 0; - out_3364331378734993426[127] = 0; - out_3364331378734993426[128] = 0; - out_3364331378734993426[129] = 0; - out_3364331378734993426[130] = 0; - out_3364331378734993426[131] = 0; - out_3364331378734993426[132] = 0; - out_3364331378734993426[133] = 1; - out_3364331378734993426[134] = 0; - out_3364331378734993426[135] = 0; - out_3364331378734993426[136] = 0; - out_3364331378734993426[137] = 0; - out_3364331378734993426[138] = 0; - out_3364331378734993426[139] = 0; - out_3364331378734993426[140] = 0; - out_3364331378734993426[141] = 0; - out_3364331378734993426[142] = 0; - out_3364331378734993426[143] = 0; - out_3364331378734993426[144] = 0; - out_3364331378734993426[145] = 0; - out_3364331378734993426[146] = 0; - out_3364331378734993426[147] = 0; - out_3364331378734993426[148] = 0; - out_3364331378734993426[149] = 0; - out_3364331378734993426[150] = 0; - out_3364331378734993426[151] = 0; - out_3364331378734993426[152] = 1; - out_3364331378734993426[153] = 0; - out_3364331378734993426[154] = 0; - out_3364331378734993426[155] = 0; - out_3364331378734993426[156] = 0; - out_3364331378734993426[157] = 0; - out_3364331378734993426[158] = 0; - out_3364331378734993426[159] = 0; - out_3364331378734993426[160] = 0; - out_3364331378734993426[161] = 0; - out_3364331378734993426[162] = 0; - out_3364331378734993426[163] = 0; - out_3364331378734993426[164] = 0; - out_3364331378734993426[165] = 0; - out_3364331378734993426[166] = 0; - out_3364331378734993426[167] = 0; - out_3364331378734993426[168] = 0; - out_3364331378734993426[169] = 0; - out_3364331378734993426[170] = 0; - out_3364331378734993426[171] = 1; - out_3364331378734993426[172] = 0; - out_3364331378734993426[173] = 0; - out_3364331378734993426[174] = 0; - out_3364331378734993426[175] = 0; - out_3364331378734993426[176] = 0; - out_3364331378734993426[177] = 0; - out_3364331378734993426[178] = 0; - out_3364331378734993426[179] = 0; - out_3364331378734993426[180] = 0; - out_3364331378734993426[181] = 0; - out_3364331378734993426[182] = 0; - out_3364331378734993426[183] = 0; - out_3364331378734993426[184] = 0; - out_3364331378734993426[185] = 0; - out_3364331378734993426[186] = 0; - out_3364331378734993426[187] = 0; - out_3364331378734993426[188] = 0; - out_3364331378734993426[189] = 0; - out_3364331378734993426[190] = 1; - out_3364331378734993426[191] = 0; - out_3364331378734993426[192] = 0; - out_3364331378734993426[193] = 0; - out_3364331378734993426[194] = 0; - out_3364331378734993426[195] = 0; - out_3364331378734993426[196] = 0; - out_3364331378734993426[197] = 0; - out_3364331378734993426[198] = 0; - out_3364331378734993426[199] = 0; - out_3364331378734993426[200] = 0; - out_3364331378734993426[201] = 0; - out_3364331378734993426[202] = 0; - out_3364331378734993426[203] = 0; - out_3364331378734993426[204] = 0; - out_3364331378734993426[205] = 0; - out_3364331378734993426[206] = 0; - out_3364331378734993426[207] = 0; - out_3364331378734993426[208] = 0; - out_3364331378734993426[209] = 1; - out_3364331378734993426[210] = 0; - out_3364331378734993426[211] = 0; - out_3364331378734993426[212] = 0; - out_3364331378734993426[213] = 0; - out_3364331378734993426[214] = 0; - out_3364331378734993426[215] = 0; - out_3364331378734993426[216] = 0; - out_3364331378734993426[217] = 0; - out_3364331378734993426[218] = 0; - out_3364331378734993426[219] = 0; - out_3364331378734993426[220] = 0; - out_3364331378734993426[221] = 0; - out_3364331378734993426[222] = 0; - out_3364331378734993426[223] = 0; - out_3364331378734993426[224] = 0; - out_3364331378734993426[225] = 0; - out_3364331378734993426[226] = 0; - out_3364331378734993426[227] = 0; - out_3364331378734993426[228] = 1; - out_3364331378734993426[229] = 0; - out_3364331378734993426[230] = 0; - out_3364331378734993426[231] = 0; - out_3364331378734993426[232] = 0; - out_3364331378734993426[233] = 0; - out_3364331378734993426[234] = 0; - out_3364331378734993426[235] = 0; - out_3364331378734993426[236] = 0; - out_3364331378734993426[237] = 0; - out_3364331378734993426[238] = 0; - out_3364331378734993426[239] = 0; - out_3364331378734993426[240] = 0; - out_3364331378734993426[241] = 0; - out_3364331378734993426[242] = 0; - out_3364331378734993426[243] = 0; - out_3364331378734993426[244] = 0; - out_3364331378734993426[245] = 0; - out_3364331378734993426[246] = 0; - out_3364331378734993426[247] = 1; - out_3364331378734993426[248] = 0; - out_3364331378734993426[249] = 0; - out_3364331378734993426[250] = 0; - out_3364331378734993426[251] = 0; - out_3364331378734993426[252] = 0; - out_3364331378734993426[253] = 0; - out_3364331378734993426[254] = 0; - out_3364331378734993426[255] = 0; - out_3364331378734993426[256] = 0; - out_3364331378734993426[257] = 0; - out_3364331378734993426[258] = 0; - out_3364331378734993426[259] = 0; - out_3364331378734993426[260] = 0; - out_3364331378734993426[261] = 0; - out_3364331378734993426[262] = 0; - out_3364331378734993426[263] = 0; - out_3364331378734993426[264] = 0; - out_3364331378734993426[265] = 0; - out_3364331378734993426[266] = 1; - out_3364331378734993426[267] = 0; - out_3364331378734993426[268] = 0; - out_3364331378734993426[269] = 0; - out_3364331378734993426[270] = 0; - out_3364331378734993426[271] = 0; - out_3364331378734993426[272] = 0; - out_3364331378734993426[273] = 0; - out_3364331378734993426[274] = 0; - out_3364331378734993426[275] = 0; - out_3364331378734993426[276] = 0; - out_3364331378734993426[277] = 0; - out_3364331378734993426[278] = 0; - out_3364331378734993426[279] = 0; - out_3364331378734993426[280] = 0; - out_3364331378734993426[281] = 0; - out_3364331378734993426[282] = 0; - out_3364331378734993426[283] = 0; - out_3364331378734993426[284] = 0; - out_3364331378734993426[285] = 1; - out_3364331378734993426[286] = 0; - out_3364331378734993426[287] = 0; - out_3364331378734993426[288] = 0; - out_3364331378734993426[289] = 0; - out_3364331378734993426[290] = 0; - out_3364331378734993426[291] = 0; - out_3364331378734993426[292] = 0; - out_3364331378734993426[293] = 0; - out_3364331378734993426[294] = 0; - out_3364331378734993426[295] = 0; - out_3364331378734993426[296] = 0; - out_3364331378734993426[297] = 0; - out_3364331378734993426[298] = 0; - out_3364331378734993426[299] = 0; - out_3364331378734993426[300] = 0; - out_3364331378734993426[301] = 0; - out_3364331378734993426[302] = 0; - out_3364331378734993426[303] = 0; - out_3364331378734993426[304] = 1; - out_3364331378734993426[305] = 0; - out_3364331378734993426[306] = 0; - out_3364331378734993426[307] = 0; - out_3364331378734993426[308] = 0; - out_3364331378734993426[309] = 0; - out_3364331378734993426[310] = 0; - out_3364331378734993426[311] = 0; - out_3364331378734993426[312] = 0; - out_3364331378734993426[313] = 0; - out_3364331378734993426[314] = 0; - out_3364331378734993426[315] = 0; - out_3364331378734993426[316] = 0; - out_3364331378734993426[317] = 0; - out_3364331378734993426[318] = 0; - out_3364331378734993426[319] = 0; - out_3364331378734993426[320] = 0; - out_3364331378734993426[321] = 0; - out_3364331378734993426[322] = 0; - out_3364331378734993426[323] = 1; +void F_fun(double *state, double dt, double *out_4279109943705499799) { + out_4279109943705499799[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_4279109943705499799[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_4279109943705499799[2] = 0; + out_4279109943705499799[3] = 0; + out_4279109943705499799[4] = 0; + out_4279109943705499799[5] = 0; + out_4279109943705499799[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_4279109943705499799[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_4279109943705499799[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_4279109943705499799[9] = 0; + out_4279109943705499799[10] = 0; + out_4279109943705499799[11] = 0; + out_4279109943705499799[12] = 0; + out_4279109943705499799[13] = 0; + out_4279109943705499799[14] = 0; + out_4279109943705499799[15] = 0; + out_4279109943705499799[16] = 0; + out_4279109943705499799[17] = 0; + out_4279109943705499799[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_4279109943705499799[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_4279109943705499799[20] = 0; + out_4279109943705499799[21] = 0; + out_4279109943705499799[22] = 0; + out_4279109943705499799[23] = 0; + out_4279109943705499799[24] = 0; + out_4279109943705499799[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_4279109943705499799[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_4279109943705499799[27] = 0; + out_4279109943705499799[28] = 0; + out_4279109943705499799[29] = 0; + out_4279109943705499799[30] = 0; + out_4279109943705499799[31] = 0; + out_4279109943705499799[32] = 0; + out_4279109943705499799[33] = 0; + out_4279109943705499799[34] = 0; + out_4279109943705499799[35] = 0; + out_4279109943705499799[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_4279109943705499799[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_4279109943705499799[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_4279109943705499799[39] = 0; + out_4279109943705499799[40] = 0; + out_4279109943705499799[41] = 0; + out_4279109943705499799[42] = 0; + out_4279109943705499799[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_4279109943705499799[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_4279109943705499799[45] = 0; + out_4279109943705499799[46] = 0; + out_4279109943705499799[47] = 0; + out_4279109943705499799[48] = 0; + out_4279109943705499799[49] = 0; + out_4279109943705499799[50] = 0; + out_4279109943705499799[51] = 0; + out_4279109943705499799[52] = 0; + out_4279109943705499799[53] = 0; + out_4279109943705499799[54] = 0; + out_4279109943705499799[55] = 0; + out_4279109943705499799[56] = 0; + out_4279109943705499799[57] = 1; + out_4279109943705499799[58] = 0; + out_4279109943705499799[59] = 0; + out_4279109943705499799[60] = 0; + out_4279109943705499799[61] = 0; + out_4279109943705499799[62] = 0; + out_4279109943705499799[63] = 0; + out_4279109943705499799[64] = 0; + out_4279109943705499799[65] = 0; + out_4279109943705499799[66] = dt; + out_4279109943705499799[67] = 0; + out_4279109943705499799[68] = 0; + out_4279109943705499799[69] = 0; + out_4279109943705499799[70] = 0; + out_4279109943705499799[71] = 0; + out_4279109943705499799[72] = 0; + out_4279109943705499799[73] = 0; + out_4279109943705499799[74] = 0; + out_4279109943705499799[75] = 0; + out_4279109943705499799[76] = 1; + out_4279109943705499799[77] = 0; + out_4279109943705499799[78] = 0; + out_4279109943705499799[79] = 0; + out_4279109943705499799[80] = 0; + out_4279109943705499799[81] = 0; + out_4279109943705499799[82] = 0; + out_4279109943705499799[83] = 0; + out_4279109943705499799[84] = 0; + out_4279109943705499799[85] = dt; + out_4279109943705499799[86] = 0; + out_4279109943705499799[87] = 0; + out_4279109943705499799[88] = 0; + out_4279109943705499799[89] = 0; + out_4279109943705499799[90] = 0; + out_4279109943705499799[91] = 0; + out_4279109943705499799[92] = 0; + out_4279109943705499799[93] = 0; + out_4279109943705499799[94] = 0; + out_4279109943705499799[95] = 1; + out_4279109943705499799[96] = 0; + out_4279109943705499799[97] = 0; + out_4279109943705499799[98] = 0; + out_4279109943705499799[99] = 0; + out_4279109943705499799[100] = 0; + out_4279109943705499799[101] = 0; + out_4279109943705499799[102] = 0; + out_4279109943705499799[103] = 0; + out_4279109943705499799[104] = dt; + out_4279109943705499799[105] = 0; + out_4279109943705499799[106] = 0; + out_4279109943705499799[107] = 0; + out_4279109943705499799[108] = 0; + out_4279109943705499799[109] = 0; + out_4279109943705499799[110] = 0; + out_4279109943705499799[111] = 0; + out_4279109943705499799[112] = 0; + out_4279109943705499799[113] = 0; + out_4279109943705499799[114] = 1; + out_4279109943705499799[115] = 0; + out_4279109943705499799[116] = 0; + out_4279109943705499799[117] = 0; + out_4279109943705499799[118] = 0; + out_4279109943705499799[119] = 0; + out_4279109943705499799[120] = 0; + out_4279109943705499799[121] = 0; + out_4279109943705499799[122] = 0; + out_4279109943705499799[123] = 0; + out_4279109943705499799[124] = 0; + out_4279109943705499799[125] = 0; + out_4279109943705499799[126] = 0; + out_4279109943705499799[127] = 0; + out_4279109943705499799[128] = 0; + out_4279109943705499799[129] = 0; + out_4279109943705499799[130] = 0; + out_4279109943705499799[131] = 0; + out_4279109943705499799[132] = 0; + out_4279109943705499799[133] = 1; + out_4279109943705499799[134] = 0; + out_4279109943705499799[135] = 0; + out_4279109943705499799[136] = 0; + out_4279109943705499799[137] = 0; + out_4279109943705499799[138] = 0; + out_4279109943705499799[139] = 0; + out_4279109943705499799[140] = 0; + out_4279109943705499799[141] = 0; + out_4279109943705499799[142] = 0; + out_4279109943705499799[143] = 0; + out_4279109943705499799[144] = 0; + out_4279109943705499799[145] = 0; + out_4279109943705499799[146] = 0; + out_4279109943705499799[147] = 0; + out_4279109943705499799[148] = 0; + out_4279109943705499799[149] = 0; + out_4279109943705499799[150] = 0; + out_4279109943705499799[151] = 0; + out_4279109943705499799[152] = 1; + out_4279109943705499799[153] = 0; + out_4279109943705499799[154] = 0; + out_4279109943705499799[155] = 0; + out_4279109943705499799[156] = 0; + out_4279109943705499799[157] = 0; + out_4279109943705499799[158] = 0; + out_4279109943705499799[159] = 0; + out_4279109943705499799[160] = 0; + out_4279109943705499799[161] = 0; + out_4279109943705499799[162] = 0; + out_4279109943705499799[163] = 0; + out_4279109943705499799[164] = 0; + out_4279109943705499799[165] = 0; + out_4279109943705499799[166] = 0; + out_4279109943705499799[167] = 0; + out_4279109943705499799[168] = 0; + out_4279109943705499799[169] = 0; + out_4279109943705499799[170] = 0; + out_4279109943705499799[171] = 1; + out_4279109943705499799[172] = 0; + out_4279109943705499799[173] = 0; + out_4279109943705499799[174] = 0; + out_4279109943705499799[175] = 0; + out_4279109943705499799[176] = 0; + out_4279109943705499799[177] = 0; + out_4279109943705499799[178] = 0; + out_4279109943705499799[179] = 0; + out_4279109943705499799[180] = 0; + out_4279109943705499799[181] = 0; + out_4279109943705499799[182] = 0; + out_4279109943705499799[183] = 0; + out_4279109943705499799[184] = 0; + out_4279109943705499799[185] = 0; + out_4279109943705499799[186] = 0; + out_4279109943705499799[187] = 0; + out_4279109943705499799[188] = 0; + out_4279109943705499799[189] = 0; + out_4279109943705499799[190] = 1; + out_4279109943705499799[191] = 0; + out_4279109943705499799[192] = 0; + out_4279109943705499799[193] = 0; + out_4279109943705499799[194] = 0; + out_4279109943705499799[195] = 0; + out_4279109943705499799[196] = 0; + out_4279109943705499799[197] = 0; + out_4279109943705499799[198] = 0; + out_4279109943705499799[199] = 0; + out_4279109943705499799[200] = 0; + out_4279109943705499799[201] = 0; + out_4279109943705499799[202] = 0; + out_4279109943705499799[203] = 0; + out_4279109943705499799[204] = 0; + out_4279109943705499799[205] = 0; + out_4279109943705499799[206] = 0; + out_4279109943705499799[207] = 0; + out_4279109943705499799[208] = 0; + out_4279109943705499799[209] = 1; + out_4279109943705499799[210] = 0; + out_4279109943705499799[211] = 0; + out_4279109943705499799[212] = 0; + out_4279109943705499799[213] = 0; + out_4279109943705499799[214] = 0; + out_4279109943705499799[215] = 0; + out_4279109943705499799[216] = 0; + out_4279109943705499799[217] = 0; + out_4279109943705499799[218] = 0; + out_4279109943705499799[219] = 0; + out_4279109943705499799[220] = 0; + out_4279109943705499799[221] = 0; + out_4279109943705499799[222] = 0; + out_4279109943705499799[223] = 0; + out_4279109943705499799[224] = 0; + out_4279109943705499799[225] = 0; + out_4279109943705499799[226] = 0; + out_4279109943705499799[227] = 0; + out_4279109943705499799[228] = 1; + out_4279109943705499799[229] = 0; + out_4279109943705499799[230] = 0; + out_4279109943705499799[231] = 0; + out_4279109943705499799[232] = 0; + out_4279109943705499799[233] = 0; + out_4279109943705499799[234] = 0; + out_4279109943705499799[235] = 0; + out_4279109943705499799[236] = 0; + out_4279109943705499799[237] = 0; + out_4279109943705499799[238] = 0; + out_4279109943705499799[239] = 0; + out_4279109943705499799[240] = 0; + out_4279109943705499799[241] = 0; + out_4279109943705499799[242] = 0; + out_4279109943705499799[243] = 0; + out_4279109943705499799[244] = 0; + out_4279109943705499799[245] = 0; + out_4279109943705499799[246] = 0; + out_4279109943705499799[247] = 1; + out_4279109943705499799[248] = 0; + out_4279109943705499799[249] = 0; + out_4279109943705499799[250] = 0; + out_4279109943705499799[251] = 0; + out_4279109943705499799[252] = 0; + out_4279109943705499799[253] = 0; + out_4279109943705499799[254] = 0; + out_4279109943705499799[255] = 0; + out_4279109943705499799[256] = 0; + out_4279109943705499799[257] = 0; + out_4279109943705499799[258] = 0; + out_4279109943705499799[259] = 0; + out_4279109943705499799[260] = 0; + out_4279109943705499799[261] = 0; + out_4279109943705499799[262] = 0; + out_4279109943705499799[263] = 0; + out_4279109943705499799[264] = 0; + out_4279109943705499799[265] = 0; + out_4279109943705499799[266] = 1; + out_4279109943705499799[267] = 0; + out_4279109943705499799[268] = 0; + out_4279109943705499799[269] = 0; + out_4279109943705499799[270] = 0; + out_4279109943705499799[271] = 0; + out_4279109943705499799[272] = 0; + out_4279109943705499799[273] = 0; + out_4279109943705499799[274] = 0; + out_4279109943705499799[275] = 0; + out_4279109943705499799[276] = 0; + out_4279109943705499799[277] = 0; + out_4279109943705499799[278] = 0; + out_4279109943705499799[279] = 0; + out_4279109943705499799[280] = 0; + out_4279109943705499799[281] = 0; + out_4279109943705499799[282] = 0; + out_4279109943705499799[283] = 0; + out_4279109943705499799[284] = 0; + out_4279109943705499799[285] = 1; + out_4279109943705499799[286] = 0; + out_4279109943705499799[287] = 0; + out_4279109943705499799[288] = 0; + out_4279109943705499799[289] = 0; + out_4279109943705499799[290] = 0; + out_4279109943705499799[291] = 0; + out_4279109943705499799[292] = 0; + out_4279109943705499799[293] = 0; + out_4279109943705499799[294] = 0; + out_4279109943705499799[295] = 0; + out_4279109943705499799[296] = 0; + out_4279109943705499799[297] = 0; + out_4279109943705499799[298] = 0; + out_4279109943705499799[299] = 0; + out_4279109943705499799[300] = 0; + out_4279109943705499799[301] = 0; + out_4279109943705499799[302] = 0; + out_4279109943705499799[303] = 0; + out_4279109943705499799[304] = 1; + out_4279109943705499799[305] = 0; + out_4279109943705499799[306] = 0; + out_4279109943705499799[307] = 0; + out_4279109943705499799[308] = 0; + out_4279109943705499799[309] = 0; + out_4279109943705499799[310] = 0; + out_4279109943705499799[311] = 0; + out_4279109943705499799[312] = 0; + out_4279109943705499799[313] = 0; + out_4279109943705499799[314] = 0; + out_4279109943705499799[315] = 0; + out_4279109943705499799[316] = 0; + out_4279109943705499799[317] = 0; + out_4279109943705499799[318] = 0; + out_4279109943705499799[319] = 0; + out_4279109943705499799[320] = 0; + out_4279109943705499799[321] = 0; + out_4279109943705499799[322] = 0; + out_4279109943705499799[323] = 1; } -void h_4(double *state, double *unused, double *out_2331867789495253821) { - out_2331867789495253821[0] = state[6] + state[9]; - out_2331867789495253821[1] = state[7] + state[10]; - out_2331867789495253821[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_4468827944582346069) { + out_4468827944582346069[0] = state[6] + state[9]; + out_4468827944582346069[1] = state[7] + state[10]; + out_4468827944582346069[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_7103788100854208139) { - out_7103788100854208139[0] = 0; - out_7103788100854208139[1] = 0; - out_7103788100854208139[2] = 0; - out_7103788100854208139[3] = 0; - out_7103788100854208139[4] = 0; - out_7103788100854208139[5] = 0; - out_7103788100854208139[6] = 1; - out_7103788100854208139[7] = 0; - out_7103788100854208139[8] = 0; - out_7103788100854208139[9] = 1; - out_7103788100854208139[10] = 0; - out_7103788100854208139[11] = 0; - out_7103788100854208139[12] = 0; - out_7103788100854208139[13] = 0; - out_7103788100854208139[14] = 0; - out_7103788100854208139[15] = 0; - out_7103788100854208139[16] = 0; - out_7103788100854208139[17] = 0; - out_7103788100854208139[18] = 0; - out_7103788100854208139[19] = 0; - out_7103788100854208139[20] = 0; - out_7103788100854208139[21] = 0; - out_7103788100854208139[22] = 0; - out_7103788100854208139[23] = 0; - out_7103788100854208139[24] = 0; - out_7103788100854208139[25] = 1; - out_7103788100854208139[26] = 0; - out_7103788100854208139[27] = 0; - out_7103788100854208139[28] = 1; - out_7103788100854208139[29] = 0; - out_7103788100854208139[30] = 0; - out_7103788100854208139[31] = 0; - out_7103788100854208139[32] = 0; - out_7103788100854208139[33] = 0; - out_7103788100854208139[34] = 0; - out_7103788100854208139[35] = 0; - out_7103788100854208139[36] = 0; - out_7103788100854208139[37] = 0; - out_7103788100854208139[38] = 0; - out_7103788100854208139[39] = 0; - out_7103788100854208139[40] = 0; - out_7103788100854208139[41] = 0; - out_7103788100854208139[42] = 0; - out_7103788100854208139[43] = 0; - out_7103788100854208139[44] = 1; - out_7103788100854208139[45] = 0; - out_7103788100854208139[46] = 0; - out_7103788100854208139[47] = 1; - out_7103788100854208139[48] = 0; - out_7103788100854208139[49] = 0; - out_7103788100854208139[50] = 0; - out_7103788100854208139[51] = 0; - out_7103788100854208139[52] = 0; - out_7103788100854208139[53] = 0; +void H_4(double *state, double *unused, double *out_4576631161314830376) { + out_4576631161314830376[0] = 0; + out_4576631161314830376[1] = 0; + out_4576631161314830376[2] = 0; + out_4576631161314830376[3] = 0; + out_4576631161314830376[4] = 0; + out_4576631161314830376[5] = 0; + out_4576631161314830376[6] = 1; + out_4576631161314830376[7] = 0; + out_4576631161314830376[8] = 0; + out_4576631161314830376[9] = 1; + out_4576631161314830376[10] = 0; + out_4576631161314830376[11] = 0; + out_4576631161314830376[12] = 0; + out_4576631161314830376[13] = 0; + out_4576631161314830376[14] = 0; + out_4576631161314830376[15] = 0; + out_4576631161314830376[16] = 0; + out_4576631161314830376[17] = 0; + out_4576631161314830376[18] = 0; + out_4576631161314830376[19] = 0; + out_4576631161314830376[20] = 0; + out_4576631161314830376[21] = 0; + out_4576631161314830376[22] = 0; + out_4576631161314830376[23] = 0; + out_4576631161314830376[24] = 0; + out_4576631161314830376[25] = 1; + out_4576631161314830376[26] = 0; + out_4576631161314830376[27] = 0; + out_4576631161314830376[28] = 1; + out_4576631161314830376[29] = 0; + out_4576631161314830376[30] = 0; + out_4576631161314830376[31] = 0; + out_4576631161314830376[32] = 0; + out_4576631161314830376[33] = 0; + out_4576631161314830376[34] = 0; + out_4576631161314830376[35] = 0; + out_4576631161314830376[36] = 0; + out_4576631161314830376[37] = 0; + out_4576631161314830376[38] = 0; + out_4576631161314830376[39] = 0; + out_4576631161314830376[40] = 0; + out_4576631161314830376[41] = 0; + out_4576631161314830376[42] = 0; + out_4576631161314830376[43] = 0; + out_4576631161314830376[44] = 1; + out_4576631161314830376[45] = 0; + out_4576631161314830376[46] = 0; + out_4576631161314830376[47] = 1; + out_4576631161314830376[48] = 0; + out_4576631161314830376[49] = 0; + out_4576631161314830376[50] = 0; + out_4576631161314830376[51] = 0; + out_4576631161314830376[52] = 0; + out_4576631161314830376[53] = 0; } -void h_10(double *state, double *unused, double *out_6139878679209159095) { - out_6139878679209159095[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_6139878679209159095[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_6139878679209159095[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_7687610123346311094) { + out_7687610123346311094[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_7687610123346311094[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_7687610123346311094[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_1053793302334783175) { - out_1053793302334783175[0] = 0; - out_1053793302334783175[1] = 9.8100000000000005*cos(state[1]); - out_1053793302334783175[2] = 0; - out_1053793302334783175[3] = 0; - out_1053793302334783175[4] = -state[8]; - out_1053793302334783175[5] = state[7]; - out_1053793302334783175[6] = 0; - out_1053793302334783175[7] = state[5]; - out_1053793302334783175[8] = -state[4]; - out_1053793302334783175[9] = 0; - out_1053793302334783175[10] = 0; - out_1053793302334783175[11] = 0; - out_1053793302334783175[12] = 1; - out_1053793302334783175[13] = 0; - out_1053793302334783175[14] = 0; - out_1053793302334783175[15] = 1; - out_1053793302334783175[16] = 0; - out_1053793302334783175[17] = 0; - out_1053793302334783175[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_1053793302334783175[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_1053793302334783175[20] = 0; - out_1053793302334783175[21] = state[8]; - out_1053793302334783175[22] = 0; - out_1053793302334783175[23] = -state[6]; - out_1053793302334783175[24] = -state[5]; - out_1053793302334783175[25] = 0; - out_1053793302334783175[26] = state[3]; - out_1053793302334783175[27] = 0; - out_1053793302334783175[28] = 0; - out_1053793302334783175[29] = 0; - out_1053793302334783175[30] = 0; - out_1053793302334783175[31] = 1; - out_1053793302334783175[32] = 0; - out_1053793302334783175[33] = 0; - out_1053793302334783175[34] = 1; - out_1053793302334783175[35] = 0; - out_1053793302334783175[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_1053793302334783175[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_1053793302334783175[38] = 0; - out_1053793302334783175[39] = -state[7]; - out_1053793302334783175[40] = state[6]; - out_1053793302334783175[41] = 0; - out_1053793302334783175[42] = state[4]; - out_1053793302334783175[43] = -state[3]; - out_1053793302334783175[44] = 0; - out_1053793302334783175[45] = 0; - out_1053793302334783175[46] = 0; - out_1053793302334783175[47] = 0; - out_1053793302334783175[48] = 0; - out_1053793302334783175[49] = 0; - out_1053793302334783175[50] = 1; - out_1053793302334783175[51] = 0; - out_1053793302334783175[52] = 0; - out_1053793302334783175[53] = 1; +void H_10(double *state, double *unused, double *out_1094040887207712728) { + out_1094040887207712728[0] = 0; + out_1094040887207712728[1] = 9.8100000000000005*cos(state[1]); + out_1094040887207712728[2] = 0; + out_1094040887207712728[3] = 0; + out_1094040887207712728[4] = -state[8]; + out_1094040887207712728[5] = state[7]; + out_1094040887207712728[6] = 0; + out_1094040887207712728[7] = state[5]; + out_1094040887207712728[8] = -state[4]; + out_1094040887207712728[9] = 0; + out_1094040887207712728[10] = 0; + out_1094040887207712728[11] = 0; + out_1094040887207712728[12] = 1; + out_1094040887207712728[13] = 0; + out_1094040887207712728[14] = 0; + out_1094040887207712728[15] = 1; + out_1094040887207712728[16] = 0; + out_1094040887207712728[17] = 0; + out_1094040887207712728[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_1094040887207712728[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_1094040887207712728[20] = 0; + out_1094040887207712728[21] = state[8]; + out_1094040887207712728[22] = 0; + out_1094040887207712728[23] = -state[6]; + out_1094040887207712728[24] = -state[5]; + out_1094040887207712728[25] = 0; + out_1094040887207712728[26] = state[3]; + out_1094040887207712728[27] = 0; + out_1094040887207712728[28] = 0; + out_1094040887207712728[29] = 0; + out_1094040887207712728[30] = 0; + out_1094040887207712728[31] = 1; + out_1094040887207712728[32] = 0; + out_1094040887207712728[33] = 0; + out_1094040887207712728[34] = 1; + out_1094040887207712728[35] = 0; + out_1094040887207712728[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_1094040887207712728[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_1094040887207712728[38] = 0; + out_1094040887207712728[39] = -state[7]; + out_1094040887207712728[40] = state[6]; + out_1094040887207712728[41] = 0; + out_1094040887207712728[42] = state[4]; + out_1094040887207712728[43] = -state[3]; + out_1094040887207712728[44] = 0; + out_1094040887207712728[45] = 0; + out_1094040887207712728[46] = 0; + out_1094040887207712728[47] = 0; + out_1094040887207712728[48] = 0; + out_1094040887207712728[49] = 0; + out_1094040887207712728[50] = 1; + out_1094040887207712728[51] = 0; + out_1094040887207712728[52] = 0; + out_1094040887207712728[53] = 1; } -void h_13(double *state, double *unused, double *out_1682911552899262681) { - out_1682911552899262681[0] = state[3]; - out_1682911552899262681[1] = state[4]; - out_1682911552899262681[2] = state[5]; +void h_13(double *state, double *unused, double *out_649951466656749498) { + out_649951466656749498[0] = state[3]; + out_649951466656749498[1] = state[4]; + out_649951466656749498[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3732324764538642548) { - out_3732324764538642548[0] = 0; - out_3732324764538642548[1] = 0; - out_3732324764538642548[2] = 0; - out_3732324764538642548[3] = 1; - out_3732324764538642548[4] = 0; - out_3732324764538642548[5] = 0; - out_3732324764538642548[6] = 0; - out_3732324764538642548[7] = 0; - out_3732324764538642548[8] = 0; - out_3732324764538642548[9] = 0; - out_3732324764538642548[10] = 0; - out_3732324764538642548[11] = 0; - out_3732324764538642548[12] = 0; - out_3732324764538642548[13] = 0; - out_3732324764538642548[14] = 0; - out_3732324764538642548[15] = 0; - out_3732324764538642548[16] = 0; - out_3732324764538642548[17] = 0; - out_3732324764538642548[18] = 0; - out_3732324764538642548[19] = 0; - out_3732324764538642548[20] = 0; - out_3732324764538642548[21] = 0; - out_3732324764538642548[22] = 1; - out_3732324764538642548[23] = 0; - out_3732324764538642548[24] = 0; - out_3732324764538642548[25] = 0; - out_3732324764538642548[26] = 0; - out_3732324764538642548[27] = 0; - out_3732324764538642548[28] = 0; - out_3732324764538642548[29] = 0; - out_3732324764538642548[30] = 0; - out_3732324764538642548[31] = 0; - out_3732324764538642548[32] = 0; - out_3732324764538642548[33] = 0; - out_3732324764538642548[34] = 0; - out_3732324764538642548[35] = 0; - out_3732324764538642548[36] = 0; - out_3732324764538642548[37] = 0; - out_3732324764538642548[38] = 0; - out_3732324764538642548[39] = 0; - out_3732324764538642548[40] = 0; - out_3732324764538642548[41] = 1; - out_3732324764538642548[42] = 0; - out_3732324764538642548[43] = 0; - out_3732324764538642548[44] = 0; - out_3732324764538642548[45] = 0; - out_3732324764538642548[46] = 0; - out_3732324764538642548[47] = 0; - out_3732324764538642548[48] = 0; - out_3732324764538642548[49] = 0; - out_3732324764538642548[50] = 0; - out_3732324764538642548[51] = 0; - out_3732324764538642548[52] = 0; - out_3732324764538642548[53] = 0; +void H_13(double *state, double *unused, double *out_6259481704078020311) { + out_6259481704078020311[0] = 0; + out_6259481704078020311[1] = 0; + out_6259481704078020311[2] = 0; + out_6259481704078020311[3] = 1; + out_6259481704078020311[4] = 0; + out_6259481704078020311[5] = 0; + out_6259481704078020311[6] = 0; + out_6259481704078020311[7] = 0; + out_6259481704078020311[8] = 0; + out_6259481704078020311[9] = 0; + out_6259481704078020311[10] = 0; + out_6259481704078020311[11] = 0; + out_6259481704078020311[12] = 0; + out_6259481704078020311[13] = 0; + out_6259481704078020311[14] = 0; + out_6259481704078020311[15] = 0; + out_6259481704078020311[16] = 0; + out_6259481704078020311[17] = 0; + out_6259481704078020311[18] = 0; + out_6259481704078020311[19] = 0; + out_6259481704078020311[20] = 0; + out_6259481704078020311[21] = 0; + out_6259481704078020311[22] = 1; + out_6259481704078020311[23] = 0; + out_6259481704078020311[24] = 0; + out_6259481704078020311[25] = 0; + out_6259481704078020311[26] = 0; + out_6259481704078020311[27] = 0; + out_6259481704078020311[28] = 0; + out_6259481704078020311[29] = 0; + out_6259481704078020311[30] = 0; + out_6259481704078020311[31] = 0; + out_6259481704078020311[32] = 0; + out_6259481704078020311[33] = 0; + out_6259481704078020311[34] = 0; + out_6259481704078020311[35] = 0; + out_6259481704078020311[36] = 0; + out_6259481704078020311[37] = 0; + out_6259481704078020311[38] = 0; + out_6259481704078020311[39] = 0; + out_6259481704078020311[40] = 0; + out_6259481704078020311[41] = 1; + out_6259481704078020311[42] = 0; + out_6259481704078020311[43] = 0; + out_6259481704078020311[44] = 0; + out_6259481704078020311[45] = 0; + out_6259481704078020311[46] = 0; + out_6259481704078020311[47] = 0; + out_6259481704078020311[48] = 0; + out_6259481704078020311[49] = 0; + out_6259481704078020311[50] = 0; + out_6259481704078020311[51] = 0; + out_6259481704078020311[52] = 0; + out_6259481704078020311[53] = 0; } -void h_14(double *state, double *unused, double *out_7957741524853228127) { - out_7957741524853228127[0] = state[6]; - out_7957741524853228127[1] = state[7]; - out_7957741524853228127[2] = state[8]; +void h_14(double *state, double *unused, double *out_5063684233320489319) { + out_5063684233320489319[0] = state[6]; + out_5063684233320489319[1] = state[7]; + out_5063684233320489319[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4020999668558835843) { - out_4020999668558835843[0] = 0; - out_4020999668558835843[1] = 0; - out_4020999668558835843[2] = 0; - out_4020999668558835843[3] = 0; - out_4020999668558835843[4] = 0; - out_4020999668558835843[5] = 0; - out_4020999668558835843[6] = 1; - out_4020999668558835843[7] = 0; - out_4020999668558835843[8] = 0; - out_4020999668558835843[9] = 0; - out_4020999668558835843[10] = 0; - out_4020999668558835843[11] = 0; - out_4020999668558835843[12] = 0; - out_4020999668558835843[13] = 0; - out_4020999668558835843[14] = 0; - out_4020999668558835843[15] = 0; - out_4020999668558835843[16] = 0; - out_4020999668558835843[17] = 0; - out_4020999668558835843[18] = 0; - out_4020999668558835843[19] = 0; - out_4020999668558835843[20] = 0; - out_4020999668558835843[21] = 0; - out_4020999668558835843[22] = 0; - out_4020999668558835843[23] = 0; - out_4020999668558835843[24] = 0; - out_4020999668558835843[25] = 1; - out_4020999668558835843[26] = 0; - out_4020999668558835843[27] = 0; - out_4020999668558835843[28] = 0; - out_4020999668558835843[29] = 0; - out_4020999668558835843[30] = 0; - out_4020999668558835843[31] = 0; - out_4020999668558835843[32] = 0; - out_4020999668558835843[33] = 0; - out_4020999668558835843[34] = 0; - out_4020999668558835843[35] = 0; - out_4020999668558835843[36] = 0; - out_4020999668558835843[37] = 0; - out_4020999668558835843[38] = 0; - out_4020999668558835843[39] = 0; - out_4020999668558835843[40] = 0; - out_4020999668558835843[41] = 0; - out_4020999668558835843[42] = 0; - out_4020999668558835843[43] = 0; - out_4020999668558835843[44] = 1; - out_4020999668558835843[45] = 0; - out_4020999668558835843[46] = 0; - out_4020999668558835843[47] = 0; - out_4020999668558835843[48] = 0; - out_4020999668558835843[49] = 0; - out_4020999668558835843[50] = 0; - out_4020999668558835843[51] = 0; - out_4020999668558835843[52] = 0; - out_4020999668558835843[53] = 0; +void H_14(double *state, double *unused, double *out_8539872017654314905) { + out_8539872017654314905[0] = 0; + out_8539872017654314905[1] = 0; + out_8539872017654314905[2] = 0; + out_8539872017654314905[3] = 0; + out_8539872017654314905[4] = 0; + out_8539872017654314905[5] = 0; + out_8539872017654314905[6] = 1; + out_8539872017654314905[7] = 0; + out_8539872017654314905[8] = 0; + out_8539872017654314905[9] = 0; + out_8539872017654314905[10] = 0; + out_8539872017654314905[11] = 0; + out_8539872017654314905[12] = 0; + out_8539872017654314905[13] = 0; + out_8539872017654314905[14] = 0; + out_8539872017654314905[15] = 0; + out_8539872017654314905[16] = 0; + out_8539872017654314905[17] = 0; + out_8539872017654314905[18] = 0; + out_8539872017654314905[19] = 0; + out_8539872017654314905[20] = 0; + out_8539872017654314905[21] = 0; + out_8539872017654314905[22] = 0; + out_8539872017654314905[23] = 0; + out_8539872017654314905[24] = 0; + out_8539872017654314905[25] = 1; + out_8539872017654314905[26] = 0; + out_8539872017654314905[27] = 0; + out_8539872017654314905[28] = 0; + out_8539872017654314905[29] = 0; + out_8539872017654314905[30] = 0; + out_8539872017654314905[31] = 0; + out_8539872017654314905[32] = 0; + out_8539872017654314905[33] = 0; + out_8539872017654314905[34] = 0; + out_8539872017654314905[35] = 0; + out_8539872017654314905[36] = 0; + out_8539872017654314905[37] = 0; + out_8539872017654314905[38] = 0; + out_8539872017654314905[39] = 0; + out_8539872017654314905[40] = 0; + out_8539872017654314905[41] = 0; + out_8539872017654314905[42] = 0; + out_8539872017654314905[43] = 0; + out_8539872017654314905[44] = 1; + out_8539872017654314905[45] = 0; + out_8539872017654314905[46] = 0; + out_8539872017654314905[47] = 0; + out_8539872017654314905[48] = 0; + out_8539872017654314905[49] = 0; + out_8539872017654314905[50] = 0; + out_8539872017654314905[51] = 0; + out_8539872017654314905[52] = 0; + out_8539872017654314905[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_3060982030804230470) { - err_fun(nom_x, delta_x, out_3060982030804230470); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1903969700519405728) { + err_fun(nom_x, delta_x, out_1903969700519405728); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1893009693429340384) { - inv_err_fun(nom_x, true_x, out_1893009693429340384); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3362098330798648840) { + inv_err_fun(nom_x, true_x, out_3362098330798648840); } -void pose_H_mod_fun(double *state, double *out_8923486826760505000) { - H_mod_fun(state, out_8923486826760505000); +void pose_H_mod_fun(double *state, double *out_3822470159260117769) { + H_mod_fun(state, out_3822470159260117769); } -void pose_f_fun(double *state, double dt, double *out_6932327335752377632) { - f_fun(state, dt, out_6932327335752377632); +void pose_f_fun(double *state, double dt, double *out_4539628665192259779) { + f_fun(state, dt, out_4539628665192259779); } -void pose_F_fun(double *state, double dt, double *out_3364331378734993426) { - F_fun(state, dt, out_3364331378734993426); +void pose_F_fun(double *state, double dt, double *out_4279109943705499799) { + F_fun(state, dt, out_4279109943705499799); } -void pose_h_4(double *state, double *unused, double *out_2331867789495253821) { - h_4(state, unused, out_2331867789495253821); +void pose_h_4(double *state, double *unused, double *out_4468827944582346069) { + h_4(state, unused, out_4468827944582346069); } -void pose_H_4(double *state, double *unused, double *out_7103788100854208139) { - H_4(state, unused, out_7103788100854208139); +void pose_H_4(double *state, double *unused, double *out_4576631161314830376) { + H_4(state, unused, out_4576631161314830376); } -void pose_h_10(double *state, double *unused, double *out_6139878679209159095) { - h_10(state, unused, out_6139878679209159095); +void pose_h_10(double *state, double *unused, double *out_7687610123346311094) { + h_10(state, unused, out_7687610123346311094); } -void pose_H_10(double *state, double *unused, double *out_1053793302334783175) { - H_10(state, unused, out_1053793302334783175); +void pose_H_10(double *state, double *unused, double *out_1094040887207712728) { + H_10(state, unused, out_1094040887207712728); } -void pose_h_13(double *state, double *unused, double *out_1682911552899262681) { - h_13(state, unused, out_1682911552899262681); +void pose_h_13(double *state, double *unused, double *out_649951466656749498) { + h_13(state, unused, out_649951466656749498); } -void pose_H_13(double *state, double *unused, double *out_3732324764538642548) { - H_13(state, unused, out_3732324764538642548); +void pose_H_13(double *state, double *unused, double *out_6259481704078020311) { + H_13(state, unused, out_6259481704078020311); } -void pose_h_14(double *state, double *unused, double *out_7957741524853228127) { - h_14(state, unused, out_7957741524853228127); +void pose_h_14(double *state, double *unused, double *out_5063684233320489319) { + h_14(state, unused, out_5063684233320489319); } -void pose_H_14(double *state, double *unused, double *out_4020999668558835843) { - H_14(state, unused, out_4020999668558835843); +void pose_H_14(double *state, double *unused, double *out_8539872017654314905) { + H_14(state, unused, out_8539872017654314905); } 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 e80b561ab..331d8e84b 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_3060982030804230470); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1893009693429340384); -void pose_H_mod_fun(double *state, double *out_8923486826760505000); -void pose_f_fun(double *state, double dt, double *out_6932327335752377632); -void pose_F_fun(double *state, double dt, double *out_3364331378734993426); -void pose_h_4(double *state, double *unused, double *out_2331867789495253821); -void pose_H_4(double *state, double *unused, double *out_7103788100854208139); -void pose_h_10(double *state, double *unused, double *out_6139878679209159095); -void pose_H_10(double *state, double *unused, double *out_1053793302334783175); -void pose_h_13(double *state, double *unused, double *out_1682911552899262681); -void pose_H_13(double *state, double *unused, double *out_3732324764538642548); -void pose_h_14(double *state, double *unused, double *out_7957741524853228127); -void pose_H_14(double *state, double *unused, double *out_4020999668558835843); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1903969700519405728); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3362098330798648840); +void pose_H_mod_fun(double *state, double *out_3822470159260117769); +void pose_f_fun(double *state, double dt, double *out_4539628665192259779); +void pose_F_fun(double *state, double dt, double *out_4279109943705499799); +void pose_h_4(double *state, double *unused, double *out_4468827944582346069); +void pose_H_4(double *state, double *unused, double *out_4576631161314830376); +void pose_h_10(double *state, double *unused, double *out_7687610123346311094); +void pose_H_10(double *state, double *unused, double *out_1094040887207712728); +void pose_h_13(double *state, double *unused, double *out_649951466656749498); +void pose_H_13(double *state, double *unused, double *out_6259481704078020311); +void pose_h_14(double *state, double *unused, double *out_5063684233320489319); +void pose_H_14(double *state, double *unused, double *out_8539872017654314905); 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 b0e322ddb..851ed5ecb 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ