diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 6f4ed0175..ee5912930 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 9be223cff..b165e4819 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 b98e5e0b7..fae68cc76 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 63852ad13..fc2c7403a 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 62296bf04..d56104a6a 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 9e646c576..0d03b02bd 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 db60fa4d1..457c2ead4 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 db60fa4d1..457c2ead4 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 921790a1c..68c207be4 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 9e646c576..0d03b02bd 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 556bf8976..d02420ebf 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-76dbf941-DEBUG"; +const uint8_t gitversion[19] = "DEV-b78e4430-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 7ffa63094..a8442fcd2 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 c8796a3e1..4d53327fe 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 4e12bb832..b732e2c8d 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 aa8d53877..a1c153cdb 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 2cd445efa..a4f5cab0e 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 5b553e759..8d01bfb1e 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 d9d44b250..3a570cb73 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 80544392e..3b91f83b2 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 bf059e919..fbfe85b13 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 ec642fdc3..57c694d55 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 d09be3bf3..a694952eb 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 ee75a90a7..dc097ea09 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 4628a1bed..eb6bea684 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 2566299c2..2a954a650 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 1612bfe22..cef8bf9ab 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 6b4200b37..cbbe3264e 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 8c5b03b98..11234b387 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 d365c7fac..91634a093 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 60ff8859d..32234aad3 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 74793b91f..57f101f00 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 991b1c241..245409de6 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-76dbf941-DEBUG \ No newline at end of file +DEV-b78e4430-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index ab0dc3678..43e2970c5 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_8544853402428642963) { - out_8544853402428642963[0] = delta_x[0] + nom_x[0]; - out_8544853402428642963[1] = delta_x[1] + nom_x[1]; - out_8544853402428642963[2] = delta_x[2] + nom_x[2]; - out_8544853402428642963[3] = delta_x[3] + nom_x[3]; - out_8544853402428642963[4] = delta_x[4] + nom_x[4]; - out_8544853402428642963[5] = delta_x[5] + nom_x[5]; - out_8544853402428642963[6] = delta_x[6] + nom_x[6]; - out_8544853402428642963[7] = delta_x[7] + nom_x[7]; - out_8544853402428642963[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_7371356248284686223) { + out_7371356248284686223[0] = delta_x[0] + nom_x[0]; + out_7371356248284686223[1] = delta_x[1] + nom_x[1]; + out_7371356248284686223[2] = delta_x[2] + nom_x[2]; + out_7371356248284686223[3] = delta_x[3] + nom_x[3]; + out_7371356248284686223[4] = delta_x[4] + nom_x[4]; + out_7371356248284686223[5] = delta_x[5] + nom_x[5]; + out_7371356248284686223[6] = delta_x[6] + nom_x[6]; + out_7371356248284686223[7] = delta_x[7] + nom_x[7]; + out_7371356248284686223[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4966147196229178357) { - out_4966147196229178357[0] = -nom_x[0] + true_x[0]; - out_4966147196229178357[1] = -nom_x[1] + true_x[1]; - out_4966147196229178357[2] = -nom_x[2] + true_x[2]; - out_4966147196229178357[3] = -nom_x[3] + true_x[3]; - out_4966147196229178357[4] = -nom_x[4] + true_x[4]; - out_4966147196229178357[5] = -nom_x[5] + true_x[5]; - out_4966147196229178357[6] = -nom_x[6] + true_x[6]; - out_4966147196229178357[7] = -nom_x[7] + true_x[7]; - out_4966147196229178357[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3965605218365756510) { + out_3965605218365756510[0] = -nom_x[0] + true_x[0]; + out_3965605218365756510[1] = -nom_x[1] + true_x[1]; + out_3965605218365756510[2] = -nom_x[2] + true_x[2]; + out_3965605218365756510[3] = -nom_x[3] + true_x[3]; + out_3965605218365756510[4] = -nom_x[4] + true_x[4]; + out_3965605218365756510[5] = -nom_x[5] + true_x[5]; + out_3965605218365756510[6] = -nom_x[6] + true_x[6]; + out_3965605218365756510[7] = -nom_x[7] + true_x[7]; + out_3965605218365756510[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_5259284650049385741) { - out_5259284650049385741[0] = 1.0; - out_5259284650049385741[1] = 0.0; - out_5259284650049385741[2] = 0.0; - out_5259284650049385741[3] = 0.0; - out_5259284650049385741[4] = 0.0; - out_5259284650049385741[5] = 0.0; - out_5259284650049385741[6] = 0.0; - out_5259284650049385741[7] = 0.0; - out_5259284650049385741[8] = 0.0; - out_5259284650049385741[9] = 0.0; - out_5259284650049385741[10] = 1.0; - out_5259284650049385741[11] = 0.0; - out_5259284650049385741[12] = 0.0; - out_5259284650049385741[13] = 0.0; - out_5259284650049385741[14] = 0.0; - out_5259284650049385741[15] = 0.0; - out_5259284650049385741[16] = 0.0; - out_5259284650049385741[17] = 0.0; - out_5259284650049385741[18] = 0.0; - out_5259284650049385741[19] = 0.0; - out_5259284650049385741[20] = 1.0; - out_5259284650049385741[21] = 0.0; - out_5259284650049385741[22] = 0.0; - out_5259284650049385741[23] = 0.0; - out_5259284650049385741[24] = 0.0; - out_5259284650049385741[25] = 0.0; - out_5259284650049385741[26] = 0.0; - out_5259284650049385741[27] = 0.0; - out_5259284650049385741[28] = 0.0; - out_5259284650049385741[29] = 0.0; - out_5259284650049385741[30] = 1.0; - out_5259284650049385741[31] = 0.0; - out_5259284650049385741[32] = 0.0; - out_5259284650049385741[33] = 0.0; - out_5259284650049385741[34] = 0.0; - out_5259284650049385741[35] = 0.0; - out_5259284650049385741[36] = 0.0; - out_5259284650049385741[37] = 0.0; - out_5259284650049385741[38] = 0.0; - out_5259284650049385741[39] = 0.0; - out_5259284650049385741[40] = 1.0; - out_5259284650049385741[41] = 0.0; - out_5259284650049385741[42] = 0.0; - out_5259284650049385741[43] = 0.0; - out_5259284650049385741[44] = 0.0; - out_5259284650049385741[45] = 0.0; - out_5259284650049385741[46] = 0.0; - out_5259284650049385741[47] = 0.0; - out_5259284650049385741[48] = 0.0; - out_5259284650049385741[49] = 0.0; - out_5259284650049385741[50] = 1.0; - out_5259284650049385741[51] = 0.0; - out_5259284650049385741[52] = 0.0; - out_5259284650049385741[53] = 0.0; - out_5259284650049385741[54] = 0.0; - out_5259284650049385741[55] = 0.0; - out_5259284650049385741[56] = 0.0; - out_5259284650049385741[57] = 0.0; - out_5259284650049385741[58] = 0.0; - out_5259284650049385741[59] = 0.0; - out_5259284650049385741[60] = 1.0; - out_5259284650049385741[61] = 0.0; - out_5259284650049385741[62] = 0.0; - out_5259284650049385741[63] = 0.0; - out_5259284650049385741[64] = 0.0; - out_5259284650049385741[65] = 0.0; - out_5259284650049385741[66] = 0.0; - out_5259284650049385741[67] = 0.0; - out_5259284650049385741[68] = 0.0; - out_5259284650049385741[69] = 0.0; - out_5259284650049385741[70] = 1.0; - out_5259284650049385741[71] = 0.0; - out_5259284650049385741[72] = 0.0; - out_5259284650049385741[73] = 0.0; - out_5259284650049385741[74] = 0.0; - out_5259284650049385741[75] = 0.0; - out_5259284650049385741[76] = 0.0; - out_5259284650049385741[77] = 0.0; - out_5259284650049385741[78] = 0.0; - out_5259284650049385741[79] = 0.0; - out_5259284650049385741[80] = 1.0; +void H_mod_fun(double *state, double *out_2474093599384348200) { + out_2474093599384348200[0] = 1.0; + out_2474093599384348200[1] = 0.0; + out_2474093599384348200[2] = 0.0; + out_2474093599384348200[3] = 0.0; + out_2474093599384348200[4] = 0.0; + out_2474093599384348200[5] = 0.0; + out_2474093599384348200[6] = 0.0; + out_2474093599384348200[7] = 0.0; + out_2474093599384348200[8] = 0.0; + out_2474093599384348200[9] = 0.0; + out_2474093599384348200[10] = 1.0; + out_2474093599384348200[11] = 0.0; + out_2474093599384348200[12] = 0.0; + out_2474093599384348200[13] = 0.0; + out_2474093599384348200[14] = 0.0; + out_2474093599384348200[15] = 0.0; + out_2474093599384348200[16] = 0.0; + out_2474093599384348200[17] = 0.0; + out_2474093599384348200[18] = 0.0; + out_2474093599384348200[19] = 0.0; + out_2474093599384348200[20] = 1.0; + out_2474093599384348200[21] = 0.0; + out_2474093599384348200[22] = 0.0; + out_2474093599384348200[23] = 0.0; + out_2474093599384348200[24] = 0.0; + out_2474093599384348200[25] = 0.0; + out_2474093599384348200[26] = 0.0; + out_2474093599384348200[27] = 0.0; + out_2474093599384348200[28] = 0.0; + out_2474093599384348200[29] = 0.0; + out_2474093599384348200[30] = 1.0; + out_2474093599384348200[31] = 0.0; + out_2474093599384348200[32] = 0.0; + out_2474093599384348200[33] = 0.0; + out_2474093599384348200[34] = 0.0; + out_2474093599384348200[35] = 0.0; + out_2474093599384348200[36] = 0.0; + out_2474093599384348200[37] = 0.0; + out_2474093599384348200[38] = 0.0; + out_2474093599384348200[39] = 0.0; + out_2474093599384348200[40] = 1.0; + out_2474093599384348200[41] = 0.0; + out_2474093599384348200[42] = 0.0; + out_2474093599384348200[43] = 0.0; + out_2474093599384348200[44] = 0.0; + out_2474093599384348200[45] = 0.0; + out_2474093599384348200[46] = 0.0; + out_2474093599384348200[47] = 0.0; + out_2474093599384348200[48] = 0.0; + out_2474093599384348200[49] = 0.0; + out_2474093599384348200[50] = 1.0; + out_2474093599384348200[51] = 0.0; + out_2474093599384348200[52] = 0.0; + out_2474093599384348200[53] = 0.0; + out_2474093599384348200[54] = 0.0; + out_2474093599384348200[55] = 0.0; + out_2474093599384348200[56] = 0.0; + out_2474093599384348200[57] = 0.0; + out_2474093599384348200[58] = 0.0; + out_2474093599384348200[59] = 0.0; + out_2474093599384348200[60] = 1.0; + out_2474093599384348200[61] = 0.0; + out_2474093599384348200[62] = 0.0; + out_2474093599384348200[63] = 0.0; + out_2474093599384348200[64] = 0.0; + out_2474093599384348200[65] = 0.0; + out_2474093599384348200[66] = 0.0; + out_2474093599384348200[67] = 0.0; + out_2474093599384348200[68] = 0.0; + out_2474093599384348200[69] = 0.0; + out_2474093599384348200[70] = 1.0; + out_2474093599384348200[71] = 0.0; + out_2474093599384348200[72] = 0.0; + out_2474093599384348200[73] = 0.0; + out_2474093599384348200[74] = 0.0; + out_2474093599384348200[75] = 0.0; + out_2474093599384348200[76] = 0.0; + out_2474093599384348200[77] = 0.0; + out_2474093599384348200[78] = 0.0; + out_2474093599384348200[79] = 0.0; + out_2474093599384348200[80] = 1.0; } -void f_fun(double *state, double dt, double *out_5571654789601065205) { - out_5571654789601065205[0] = state[0]; - out_5571654789601065205[1] = state[1]; - out_5571654789601065205[2] = state[2]; - out_5571654789601065205[3] = state[3]; - out_5571654789601065205[4] = state[4]; - out_5571654789601065205[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_5571654789601065205[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_5571654789601065205[7] = state[7]; - out_5571654789601065205[8] = state[8]; +void f_fun(double *state, double dt, double *out_2271815562821462998) { + out_2271815562821462998[0] = state[0]; + out_2271815562821462998[1] = state[1]; + out_2271815562821462998[2] = state[2]; + out_2271815562821462998[3] = state[3]; + out_2271815562821462998[4] = state[4]; + out_2271815562821462998[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_2271815562821462998[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_2271815562821462998[7] = state[7]; + out_2271815562821462998[8] = state[8]; } -void F_fun(double *state, double dt, double *out_7466465670683569774) { - out_7466465670683569774[0] = 1; - out_7466465670683569774[1] = 0; - out_7466465670683569774[2] = 0; - out_7466465670683569774[3] = 0; - out_7466465670683569774[4] = 0; - out_7466465670683569774[5] = 0; - out_7466465670683569774[6] = 0; - out_7466465670683569774[7] = 0; - out_7466465670683569774[8] = 0; - out_7466465670683569774[9] = 0; - out_7466465670683569774[10] = 1; - out_7466465670683569774[11] = 0; - out_7466465670683569774[12] = 0; - out_7466465670683569774[13] = 0; - out_7466465670683569774[14] = 0; - out_7466465670683569774[15] = 0; - out_7466465670683569774[16] = 0; - out_7466465670683569774[17] = 0; - out_7466465670683569774[18] = 0; - out_7466465670683569774[19] = 0; - out_7466465670683569774[20] = 1; - out_7466465670683569774[21] = 0; - out_7466465670683569774[22] = 0; - out_7466465670683569774[23] = 0; - out_7466465670683569774[24] = 0; - out_7466465670683569774[25] = 0; - out_7466465670683569774[26] = 0; - out_7466465670683569774[27] = 0; - out_7466465670683569774[28] = 0; - out_7466465670683569774[29] = 0; - out_7466465670683569774[30] = 1; - out_7466465670683569774[31] = 0; - out_7466465670683569774[32] = 0; - out_7466465670683569774[33] = 0; - out_7466465670683569774[34] = 0; - out_7466465670683569774[35] = 0; - out_7466465670683569774[36] = 0; - out_7466465670683569774[37] = 0; - out_7466465670683569774[38] = 0; - out_7466465670683569774[39] = 0; - out_7466465670683569774[40] = 1; - out_7466465670683569774[41] = 0; - out_7466465670683569774[42] = 0; - out_7466465670683569774[43] = 0; - out_7466465670683569774[44] = 0; - out_7466465670683569774[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_7466465670683569774[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_7466465670683569774[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_7466465670683569774[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_7466465670683569774[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_7466465670683569774[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_7466465670683569774[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_7466465670683569774[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_7466465670683569774[53] = -9.8100000000000005*dt; - out_7466465670683569774[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_7466465670683569774[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_7466465670683569774[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7466465670683569774[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7466465670683569774[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_7466465670683569774[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_7466465670683569774[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_7466465670683569774[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7466465670683569774[62] = 0; - out_7466465670683569774[63] = 0; - out_7466465670683569774[64] = 0; - out_7466465670683569774[65] = 0; - out_7466465670683569774[66] = 0; - out_7466465670683569774[67] = 0; - out_7466465670683569774[68] = 0; - out_7466465670683569774[69] = 0; - out_7466465670683569774[70] = 1; - out_7466465670683569774[71] = 0; - out_7466465670683569774[72] = 0; - out_7466465670683569774[73] = 0; - out_7466465670683569774[74] = 0; - out_7466465670683569774[75] = 0; - out_7466465670683569774[76] = 0; - out_7466465670683569774[77] = 0; - out_7466465670683569774[78] = 0; - out_7466465670683569774[79] = 0; - out_7466465670683569774[80] = 1; +void F_fun(double *state, double dt, double *out_7600503497871632413) { + out_7600503497871632413[0] = 1; + out_7600503497871632413[1] = 0; + out_7600503497871632413[2] = 0; + out_7600503497871632413[3] = 0; + out_7600503497871632413[4] = 0; + out_7600503497871632413[5] = 0; + out_7600503497871632413[6] = 0; + out_7600503497871632413[7] = 0; + out_7600503497871632413[8] = 0; + out_7600503497871632413[9] = 0; + out_7600503497871632413[10] = 1; + out_7600503497871632413[11] = 0; + out_7600503497871632413[12] = 0; + out_7600503497871632413[13] = 0; + out_7600503497871632413[14] = 0; + out_7600503497871632413[15] = 0; + out_7600503497871632413[16] = 0; + out_7600503497871632413[17] = 0; + out_7600503497871632413[18] = 0; + out_7600503497871632413[19] = 0; + out_7600503497871632413[20] = 1; + out_7600503497871632413[21] = 0; + out_7600503497871632413[22] = 0; + out_7600503497871632413[23] = 0; + out_7600503497871632413[24] = 0; + out_7600503497871632413[25] = 0; + out_7600503497871632413[26] = 0; + out_7600503497871632413[27] = 0; + out_7600503497871632413[28] = 0; + out_7600503497871632413[29] = 0; + out_7600503497871632413[30] = 1; + out_7600503497871632413[31] = 0; + out_7600503497871632413[32] = 0; + out_7600503497871632413[33] = 0; + out_7600503497871632413[34] = 0; + out_7600503497871632413[35] = 0; + out_7600503497871632413[36] = 0; + out_7600503497871632413[37] = 0; + out_7600503497871632413[38] = 0; + out_7600503497871632413[39] = 0; + out_7600503497871632413[40] = 1; + out_7600503497871632413[41] = 0; + out_7600503497871632413[42] = 0; + out_7600503497871632413[43] = 0; + out_7600503497871632413[44] = 0; + out_7600503497871632413[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_7600503497871632413[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_7600503497871632413[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7600503497871632413[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7600503497871632413[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_7600503497871632413[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_7600503497871632413[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_7600503497871632413[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_7600503497871632413[53] = -9.8100000000000005*dt; + out_7600503497871632413[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_7600503497871632413[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_7600503497871632413[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7600503497871632413[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7600503497871632413[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_7600503497871632413[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_7600503497871632413[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_7600503497871632413[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7600503497871632413[62] = 0; + out_7600503497871632413[63] = 0; + out_7600503497871632413[64] = 0; + out_7600503497871632413[65] = 0; + out_7600503497871632413[66] = 0; + out_7600503497871632413[67] = 0; + out_7600503497871632413[68] = 0; + out_7600503497871632413[69] = 0; + out_7600503497871632413[70] = 1; + out_7600503497871632413[71] = 0; + out_7600503497871632413[72] = 0; + out_7600503497871632413[73] = 0; + out_7600503497871632413[74] = 0; + out_7600503497871632413[75] = 0; + out_7600503497871632413[76] = 0; + out_7600503497871632413[77] = 0; + out_7600503497871632413[78] = 0; + out_7600503497871632413[79] = 0; + out_7600503497871632413[80] = 1; } -void h_25(double *state, double *unused, double *out_7657999268865908143) { - out_7657999268865908143[0] = state[6]; +void h_25(double *state, double *unused, double *out_2245281936356175607) { + out_2245281936356175607[0] = state[6]; } -void H_25(double *state, double *unused, double *out_8283767598475501035) { - out_8283767598475501035[0] = 0; - out_8283767598475501035[1] = 0; - out_8283767598475501035[2] = 0; - out_8283767598475501035[3] = 0; - out_8283767598475501035[4] = 0; - out_8283767598475501035[5] = 0; - out_8283767598475501035[6] = 1; - out_8283767598475501035[7] = 0; - out_8283767598475501035[8] = 0; +void H_25(double *state, double *unused, double *out_7377785424569013040) { + out_7377785424569013040[0] = 0; + out_7377785424569013040[1] = 0; + out_7377785424569013040[2] = 0; + out_7377785424569013040[3] = 0; + out_7377785424569013040[4] = 0; + out_7377785424569013040[5] = 0; + out_7377785424569013040[6] = 1; + out_7377785424569013040[7] = 0; + out_7377785424569013040[8] = 0; } -void h_24(double *state, double *unused, double *out_8959567811302299328) { - out_8959567811302299328[0] = state[4]; - out_8959567811302299328[1] = state[5]; +void h_24(double *state, double *unused, double *out_5229861498965331368) { + out_5229861498965331368[0] = state[4]; + out_5229861498965331368[1] = state[5]; } -void H_24(double *state, double *unused, double *out_1708195791883982934) { - out_1708195791883982934[0] = 0; - out_1708195791883982934[1] = 0; - out_1708195791883982934[2] = 0; - out_1708195791883982934[3] = 0; - out_1708195791883982934[4] = 1; - out_1708195791883982934[5] = 0; - out_1708195791883982934[6] = 0; - out_1708195791883982934[7] = 0; - out_1708195791883982934[8] = 0; - out_1708195791883982934[9] = 0; - out_1708195791883982934[10] = 0; - out_1708195791883982934[11] = 0; - out_1708195791883982934[12] = 0; - out_1708195791883982934[13] = 0; - out_1708195791883982934[14] = 1; - out_1708195791883982934[15] = 0; - out_1708195791883982934[16] = 0; - out_1708195791883982934[17] = 0; +void H_24(double *state, double *unused, double *out_4493386842549020475) { + out_4493386842549020475[0] = 0; + out_4493386842549020475[1] = 0; + out_4493386842549020475[2] = 0; + out_4493386842549020475[3] = 0; + out_4493386842549020475[4] = 1; + out_4493386842549020475[5] = 0; + out_4493386842549020475[6] = 0; + out_4493386842549020475[7] = 0; + out_4493386842549020475[8] = 0; + out_4493386842549020475[9] = 0; + out_4493386842549020475[10] = 0; + out_4493386842549020475[11] = 0; + out_4493386842549020475[12] = 0; + out_4493386842549020475[13] = 0; + out_4493386842549020475[14] = 1; + out_4493386842549020475[15] = 0; + out_4493386842549020475[16] = 0; + out_4493386842549020475[17] = 0; } -void h_30(double *state, double *unused, double *out_4661606344031996983) { - out_4661606344031996983[0] = state[4]; +void h_30(double *state, double *unused, double *out_9036311057795238262) { + out_9036311057795238262[0] = state[4]; } -void H_30(double *state, double *unused, double *out_5765434639968252408) { - out_5765434639968252408[0] = 0; - out_5765434639968252408[1] = 0; - out_5765434639968252408[2] = 0; - out_5765434639968252408[3] = 0; - out_5765434639968252408[4] = 1; - out_5765434639968252408[5] = 0; - out_5765434639968252408[6] = 0; - out_5765434639968252408[7] = 0; - out_5765434639968252408[8] = 0; +void H_30(double *state, double *unused, double *out_8550625690633289949) { + out_8550625690633289949[0] = 0; + out_8550625690633289949[1] = 0; + out_8550625690633289949[2] = 0; + out_8550625690633289949[3] = 0; + out_8550625690633289949[4] = 1; + out_8550625690633289949[5] = 0; + out_8550625690633289949[6] = 0; + out_8550625690633289949[7] = 0; + out_8550625690633289949[8] = 0; } -void h_26(double *state, double *unused, double *out_668943426824494179) { - out_668943426824494179[0] = state[7]; +void h_26(double *state, double *unused, double *out_8466923163012650919) { + out_8466923163012650919[0] = state[7]; } -void H_26(double *state, double *unused, double *out_6421473156359994357) { - out_6421473156359994357[0] = 0; - out_6421473156359994357[1] = 0; - out_6421473156359994357[2] = 0; - out_6421473156359994357[3] = 0; - out_6421473156359994357[4] = 0; - out_6421473156359994357[5] = 0; - out_6421473156359994357[6] = 0; - out_6421473156359994357[7] = 1; - out_6421473156359994357[8] = 0; +void H_26(double *state, double *unused, double *out_7764432679379737975) { + out_7764432679379737975[0] = 0; + out_7764432679379737975[1] = 0; + out_7764432679379737975[2] = 0; + out_7764432679379737975[3] = 0; + out_7764432679379737975[4] = 0; + out_7764432679379737975[5] = 0; + out_7764432679379737975[6] = 0; + out_7764432679379737975[7] = 1; + out_7764432679379737975[8] = 0; } -void h_27(double *state, double *unused, double *out_4581587017363022418) { - out_4581587017363022418[0] = state[3]; +void h_27(double *state, double *unused, double *out_2036281904887484901) { + out_2036281904887484901[0] = state[3]; } -void H_27(double *state, double *unused, double *out_3541840568784309191) { - out_3541840568784309191[0] = 0; - out_3541840568784309191[1] = 0; - out_3541840568784309191[2] = 0; - out_3541840568784309191[3] = 1; - out_3541840568784309191[4] = 0; - out_3541840568784309191[5] = 0; - out_3541840568784309191[6] = 0; - out_3541840568784309191[7] = 0; - out_3541840568784309191[8] = 0; +void H_27(double *state, double *unused, double *out_6327031619449346732) { + out_6327031619449346732[0] = 0; + out_6327031619449346732[1] = 0; + out_6327031619449346732[2] = 0; + out_6327031619449346732[3] = 1; + out_6327031619449346732[4] = 0; + out_6327031619449346732[5] = 0; + out_6327031619449346732[6] = 0; + out_6327031619449346732[7] = 0; + out_6327031619449346732[8] = 0; } -void h_29(double *state, double *unused, double *out_3311352477521500941) { - out_3311352477521500941[0] = state[1]; +void h_29(double *state, double *unused, double *out_7562339341438508014) { + out_7562339341438508014[0] = state[1]; } -void H_29(double *state, double *unused, double *out_5255203295653860224) { - out_5255203295653860224[0] = 0; - out_5255203295653860224[1] = 1; - out_5255203295653860224[2] = 0; - out_5255203295653860224[3] = 0; - out_5255203295653860224[4] = 0; - out_5255203295653860224[5] = 0; - out_5255203295653860224[6] = 0; - out_5255203295653860224[7] = 0; - out_5255203295653860224[8] = 0; +void H_29(double *state, double *unused, double *out_8040394346318897765) { + out_8040394346318897765[0] = 0; + out_8040394346318897765[1] = 1; + out_8040394346318897765[2] = 0; + out_8040394346318897765[3] = 0; + out_8040394346318897765[4] = 0; + out_8040394346318897765[5] = 0; + out_8040394346318897765[6] = 0; + out_8040394346318897765[7] = 0; + out_8040394346318897765[8] = 0; } -void h_28(double *state, double *unused, double *out_1769034914093169035) { - out_1769034914093169035[0] = state[0]; +void h_28(double *state, double *unused, double *out_2481951949823838038) { + out_2481951949823838038[0] = state[0]; } -void H_28(double *state, double *unused, double *out_8109141760986160818) { - out_8109141760986160818[0] = 1; - out_8109141760986160818[1] = 0; - out_8109141760986160818[2] = 0; - out_8109141760986160818[3] = 0; - out_8109141760986160818[4] = 0; - out_8109141760986160818[5] = 0; - out_8109141760986160818[6] = 0; - out_8109141760986160818[7] = 0; - out_8109141760986160818[8] = 0; +void H_28(double *state, double *unused, double *out_5323950710321123277) { + out_5323950710321123277[0] = 1; + out_5323950710321123277[1] = 0; + out_5323950710321123277[2] = 0; + out_5323950710321123277[3] = 0; + out_5323950710321123277[4] = 0; + out_5323950710321123277[5] = 0; + out_5323950710321123277[6] = 0; + out_5323950710321123277[7] = 0; + out_5323950710321123277[8] = 0; } -void h_31(double *state, double *unused, double *out_3703077070402039012) { - out_3703077070402039012[0] = state[8]; +void h_31(double *state, double *unused, double *out_5123667467256710023) { + out_5123667467256710023[0] = state[8]; } -void H_31(double *state, double *unused, double *out_8253121636598540607) { - out_8253121636598540607[0] = 0; - out_8253121636598540607[1] = 0; - out_8253121636598540607[2] = 0; - out_8253121636598540607[3] = 0; - out_8253121636598540607[4] = 0; - out_8253121636598540607[5] = 0; - out_8253121636598540607[6] = 0; - out_8253121636598540607[7] = 0; - out_8253121636598540607[8] = 1; +void H_31(double *state, double *unused, double *out_7408431386445973468) { + out_7408431386445973468[0] = 0; + out_7408431386445973468[1] = 0; + out_7408431386445973468[2] = 0; + out_7408431386445973468[3] = 0; + out_7408431386445973468[4] = 0; + out_7408431386445973468[5] = 0; + out_7408431386445973468[6] = 0; + out_7408431386445973468[7] = 0; + out_7408431386445973468[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_8544853402428642963) { - err_fun(nom_x, delta_x, out_8544853402428642963); +void car_err_fun(double *nom_x, double *delta_x, double *out_7371356248284686223) { + err_fun(nom_x, delta_x, out_7371356248284686223); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4966147196229178357) { - inv_err_fun(nom_x, true_x, out_4966147196229178357); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3965605218365756510) { + inv_err_fun(nom_x, true_x, out_3965605218365756510); } -void car_H_mod_fun(double *state, double *out_5259284650049385741) { - H_mod_fun(state, out_5259284650049385741); +void car_H_mod_fun(double *state, double *out_2474093599384348200) { + H_mod_fun(state, out_2474093599384348200); } -void car_f_fun(double *state, double dt, double *out_5571654789601065205) { - f_fun(state, dt, out_5571654789601065205); +void car_f_fun(double *state, double dt, double *out_2271815562821462998) { + f_fun(state, dt, out_2271815562821462998); } -void car_F_fun(double *state, double dt, double *out_7466465670683569774) { - F_fun(state, dt, out_7466465670683569774); +void car_F_fun(double *state, double dt, double *out_7600503497871632413) { + F_fun(state, dt, out_7600503497871632413); } -void car_h_25(double *state, double *unused, double *out_7657999268865908143) { - h_25(state, unused, out_7657999268865908143); +void car_h_25(double *state, double *unused, double *out_2245281936356175607) { + h_25(state, unused, out_2245281936356175607); } -void car_H_25(double *state, double *unused, double *out_8283767598475501035) { - H_25(state, unused, out_8283767598475501035); +void car_H_25(double *state, double *unused, double *out_7377785424569013040) { + H_25(state, unused, out_7377785424569013040); } -void car_h_24(double *state, double *unused, double *out_8959567811302299328) { - h_24(state, unused, out_8959567811302299328); +void car_h_24(double *state, double *unused, double *out_5229861498965331368) { + h_24(state, unused, out_5229861498965331368); } -void car_H_24(double *state, double *unused, double *out_1708195791883982934) { - H_24(state, unused, out_1708195791883982934); +void car_H_24(double *state, double *unused, double *out_4493386842549020475) { + H_24(state, unused, out_4493386842549020475); } -void car_h_30(double *state, double *unused, double *out_4661606344031996983) { - h_30(state, unused, out_4661606344031996983); +void car_h_30(double *state, double *unused, double *out_9036311057795238262) { + h_30(state, unused, out_9036311057795238262); } -void car_H_30(double *state, double *unused, double *out_5765434639968252408) { - H_30(state, unused, out_5765434639968252408); +void car_H_30(double *state, double *unused, double *out_8550625690633289949) { + H_30(state, unused, out_8550625690633289949); } -void car_h_26(double *state, double *unused, double *out_668943426824494179) { - h_26(state, unused, out_668943426824494179); +void car_h_26(double *state, double *unused, double *out_8466923163012650919) { + h_26(state, unused, out_8466923163012650919); } -void car_H_26(double *state, double *unused, double *out_6421473156359994357) { - H_26(state, unused, out_6421473156359994357); +void car_H_26(double *state, double *unused, double *out_7764432679379737975) { + H_26(state, unused, out_7764432679379737975); } -void car_h_27(double *state, double *unused, double *out_4581587017363022418) { - h_27(state, unused, out_4581587017363022418); +void car_h_27(double *state, double *unused, double *out_2036281904887484901) { + h_27(state, unused, out_2036281904887484901); } -void car_H_27(double *state, double *unused, double *out_3541840568784309191) { - H_27(state, unused, out_3541840568784309191); +void car_H_27(double *state, double *unused, double *out_6327031619449346732) { + H_27(state, unused, out_6327031619449346732); } -void car_h_29(double *state, double *unused, double *out_3311352477521500941) { - h_29(state, unused, out_3311352477521500941); +void car_h_29(double *state, double *unused, double *out_7562339341438508014) { + h_29(state, unused, out_7562339341438508014); } -void car_H_29(double *state, double *unused, double *out_5255203295653860224) { - H_29(state, unused, out_5255203295653860224); +void car_H_29(double *state, double *unused, double *out_8040394346318897765) { + H_29(state, unused, out_8040394346318897765); } -void car_h_28(double *state, double *unused, double *out_1769034914093169035) { - h_28(state, unused, out_1769034914093169035); +void car_h_28(double *state, double *unused, double *out_2481951949823838038) { + h_28(state, unused, out_2481951949823838038); } -void car_H_28(double *state, double *unused, double *out_8109141760986160818) { - H_28(state, unused, out_8109141760986160818); +void car_H_28(double *state, double *unused, double *out_5323950710321123277) { + H_28(state, unused, out_5323950710321123277); } -void car_h_31(double *state, double *unused, double *out_3703077070402039012) { - h_31(state, unused, out_3703077070402039012); +void car_h_31(double *state, double *unused, double *out_5123667467256710023) { + h_31(state, unused, out_5123667467256710023); } -void car_H_31(double *state, double *unused, double *out_8253121636598540607) { - H_31(state, unused, out_8253121636598540607); +void car_H_31(double *state, double *unused, double *out_7408431386445973468) { + H_31(state, unused, out_7408431386445973468); } 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 085fc7227..0836bbc23 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_8544853402428642963); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4966147196229178357); -void car_H_mod_fun(double *state, double *out_5259284650049385741); -void car_f_fun(double *state, double dt, double *out_5571654789601065205); -void car_F_fun(double *state, double dt, double *out_7466465670683569774); -void car_h_25(double *state, double *unused, double *out_7657999268865908143); -void car_H_25(double *state, double *unused, double *out_8283767598475501035); -void car_h_24(double *state, double *unused, double *out_8959567811302299328); -void car_H_24(double *state, double *unused, double *out_1708195791883982934); -void car_h_30(double *state, double *unused, double *out_4661606344031996983); -void car_H_30(double *state, double *unused, double *out_5765434639968252408); -void car_h_26(double *state, double *unused, double *out_668943426824494179); -void car_H_26(double *state, double *unused, double *out_6421473156359994357); -void car_h_27(double *state, double *unused, double *out_4581587017363022418); -void car_H_27(double *state, double *unused, double *out_3541840568784309191); -void car_h_29(double *state, double *unused, double *out_3311352477521500941); -void car_H_29(double *state, double *unused, double *out_5255203295653860224); -void car_h_28(double *state, double *unused, double *out_1769034914093169035); -void car_H_28(double *state, double *unused, double *out_8109141760986160818); -void car_h_31(double *state, double *unused, double *out_3703077070402039012); -void car_H_31(double *state, double *unused, double *out_8253121636598540607); +void car_err_fun(double *nom_x, double *delta_x, double *out_7371356248284686223); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3965605218365756510); +void car_H_mod_fun(double *state, double *out_2474093599384348200); +void car_f_fun(double *state, double dt, double *out_2271815562821462998); +void car_F_fun(double *state, double dt, double *out_7600503497871632413); +void car_h_25(double *state, double *unused, double *out_2245281936356175607); +void car_H_25(double *state, double *unused, double *out_7377785424569013040); +void car_h_24(double *state, double *unused, double *out_5229861498965331368); +void car_H_24(double *state, double *unused, double *out_4493386842549020475); +void car_h_30(double *state, double *unused, double *out_9036311057795238262); +void car_H_30(double *state, double *unused, double *out_8550625690633289949); +void car_h_26(double *state, double *unused, double *out_8466923163012650919); +void car_H_26(double *state, double *unused, double *out_7764432679379737975); +void car_h_27(double *state, double *unused, double *out_2036281904887484901); +void car_H_27(double *state, double *unused, double *out_6327031619449346732); +void car_h_29(double *state, double *unused, double *out_7562339341438508014); +void car_H_29(double *state, double *unused, double *out_8040394346318897765); +void car_h_28(double *state, double *unused, double *out_2481951949823838038); +void car_H_28(double *state, double *unused, double *out_5323950710321123277); +void car_h_31(double *state, double *unused, double *out_5123667467256710023); +void car_H_31(double *state, double *unused, double *out_7408431386445973468); 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 78b98cc4a..d771745e5 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_3328634486354213070) { - out_3328634486354213070[0] = delta_x[0] + nom_x[0]; - out_3328634486354213070[1] = delta_x[1] + nom_x[1]; - out_3328634486354213070[2] = delta_x[2] + nom_x[2]; - out_3328634486354213070[3] = delta_x[3] + nom_x[3]; - out_3328634486354213070[4] = delta_x[4] + nom_x[4]; - out_3328634486354213070[5] = delta_x[5] + nom_x[5]; - out_3328634486354213070[6] = delta_x[6] + nom_x[6]; - out_3328634486354213070[7] = delta_x[7] + nom_x[7]; - out_3328634486354213070[8] = delta_x[8] + nom_x[8]; - out_3328634486354213070[9] = delta_x[9] + nom_x[9]; - out_3328634486354213070[10] = delta_x[10] + nom_x[10]; - out_3328634486354213070[11] = delta_x[11] + nom_x[11]; - out_3328634486354213070[12] = delta_x[12] + nom_x[12]; - out_3328634486354213070[13] = delta_x[13] + nom_x[13]; - out_3328634486354213070[14] = delta_x[14] + nom_x[14]; - out_3328634486354213070[15] = delta_x[15] + nom_x[15]; - out_3328634486354213070[16] = delta_x[16] + nom_x[16]; - out_3328634486354213070[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_8940459122146932400) { + out_8940459122146932400[0] = delta_x[0] + nom_x[0]; + out_8940459122146932400[1] = delta_x[1] + nom_x[1]; + out_8940459122146932400[2] = delta_x[2] + nom_x[2]; + out_8940459122146932400[3] = delta_x[3] + nom_x[3]; + out_8940459122146932400[4] = delta_x[4] + nom_x[4]; + out_8940459122146932400[5] = delta_x[5] + nom_x[5]; + out_8940459122146932400[6] = delta_x[6] + nom_x[6]; + out_8940459122146932400[7] = delta_x[7] + nom_x[7]; + out_8940459122146932400[8] = delta_x[8] + nom_x[8]; + out_8940459122146932400[9] = delta_x[9] + nom_x[9]; + out_8940459122146932400[10] = delta_x[10] + nom_x[10]; + out_8940459122146932400[11] = delta_x[11] + nom_x[11]; + out_8940459122146932400[12] = delta_x[12] + nom_x[12]; + out_8940459122146932400[13] = delta_x[13] + nom_x[13]; + out_8940459122146932400[14] = delta_x[14] + nom_x[14]; + out_8940459122146932400[15] = delta_x[15] + nom_x[15]; + out_8940459122146932400[16] = delta_x[16] + nom_x[16]; + out_8940459122146932400[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_2068367058924519839) { - out_2068367058924519839[0] = -nom_x[0] + true_x[0]; - out_2068367058924519839[1] = -nom_x[1] + true_x[1]; - out_2068367058924519839[2] = -nom_x[2] + true_x[2]; - out_2068367058924519839[3] = -nom_x[3] + true_x[3]; - out_2068367058924519839[4] = -nom_x[4] + true_x[4]; - out_2068367058924519839[5] = -nom_x[5] + true_x[5]; - out_2068367058924519839[6] = -nom_x[6] + true_x[6]; - out_2068367058924519839[7] = -nom_x[7] + true_x[7]; - out_2068367058924519839[8] = -nom_x[8] + true_x[8]; - out_2068367058924519839[9] = -nom_x[9] + true_x[9]; - out_2068367058924519839[10] = -nom_x[10] + true_x[10]; - out_2068367058924519839[11] = -nom_x[11] + true_x[11]; - out_2068367058924519839[12] = -nom_x[12] + true_x[12]; - out_2068367058924519839[13] = -nom_x[13] + true_x[13]; - out_2068367058924519839[14] = -nom_x[14] + true_x[14]; - out_2068367058924519839[15] = -nom_x[15] + true_x[15]; - out_2068367058924519839[16] = -nom_x[16] + true_x[16]; - out_2068367058924519839[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3384987028755125218) { + out_3384987028755125218[0] = -nom_x[0] + true_x[0]; + out_3384987028755125218[1] = -nom_x[1] + true_x[1]; + out_3384987028755125218[2] = -nom_x[2] + true_x[2]; + out_3384987028755125218[3] = -nom_x[3] + true_x[3]; + out_3384987028755125218[4] = -nom_x[4] + true_x[4]; + out_3384987028755125218[5] = -nom_x[5] + true_x[5]; + out_3384987028755125218[6] = -nom_x[6] + true_x[6]; + out_3384987028755125218[7] = -nom_x[7] + true_x[7]; + out_3384987028755125218[8] = -nom_x[8] + true_x[8]; + out_3384987028755125218[9] = -nom_x[9] + true_x[9]; + out_3384987028755125218[10] = -nom_x[10] + true_x[10]; + out_3384987028755125218[11] = -nom_x[11] + true_x[11]; + out_3384987028755125218[12] = -nom_x[12] + true_x[12]; + out_3384987028755125218[13] = -nom_x[13] + true_x[13]; + out_3384987028755125218[14] = -nom_x[14] + true_x[14]; + out_3384987028755125218[15] = -nom_x[15] + true_x[15]; + out_3384987028755125218[16] = -nom_x[16] + true_x[16]; + out_3384987028755125218[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_5273281981643743211) { - out_5273281981643743211[0] = 1.0; - out_5273281981643743211[1] = 0.0; - out_5273281981643743211[2] = 0.0; - out_5273281981643743211[3] = 0.0; - out_5273281981643743211[4] = 0.0; - out_5273281981643743211[5] = 0.0; - out_5273281981643743211[6] = 0.0; - out_5273281981643743211[7] = 0.0; - out_5273281981643743211[8] = 0.0; - out_5273281981643743211[9] = 0.0; - out_5273281981643743211[10] = 0.0; - out_5273281981643743211[11] = 0.0; - out_5273281981643743211[12] = 0.0; - out_5273281981643743211[13] = 0.0; - out_5273281981643743211[14] = 0.0; - out_5273281981643743211[15] = 0.0; - out_5273281981643743211[16] = 0.0; - out_5273281981643743211[17] = 0.0; - out_5273281981643743211[18] = 0.0; - out_5273281981643743211[19] = 1.0; - out_5273281981643743211[20] = 0.0; - out_5273281981643743211[21] = 0.0; - out_5273281981643743211[22] = 0.0; - out_5273281981643743211[23] = 0.0; - out_5273281981643743211[24] = 0.0; - out_5273281981643743211[25] = 0.0; - out_5273281981643743211[26] = 0.0; - out_5273281981643743211[27] = 0.0; - out_5273281981643743211[28] = 0.0; - out_5273281981643743211[29] = 0.0; - out_5273281981643743211[30] = 0.0; - out_5273281981643743211[31] = 0.0; - out_5273281981643743211[32] = 0.0; - out_5273281981643743211[33] = 0.0; - out_5273281981643743211[34] = 0.0; - out_5273281981643743211[35] = 0.0; - out_5273281981643743211[36] = 0.0; - out_5273281981643743211[37] = 0.0; - out_5273281981643743211[38] = 1.0; - out_5273281981643743211[39] = 0.0; - out_5273281981643743211[40] = 0.0; - out_5273281981643743211[41] = 0.0; - out_5273281981643743211[42] = 0.0; - out_5273281981643743211[43] = 0.0; - out_5273281981643743211[44] = 0.0; - out_5273281981643743211[45] = 0.0; - out_5273281981643743211[46] = 0.0; - out_5273281981643743211[47] = 0.0; - out_5273281981643743211[48] = 0.0; - out_5273281981643743211[49] = 0.0; - out_5273281981643743211[50] = 0.0; - out_5273281981643743211[51] = 0.0; - out_5273281981643743211[52] = 0.0; - out_5273281981643743211[53] = 0.0; - out_5273281981643743211[54] = 0.0; - out_5273281981643743211[55] = 0.0; - out_5273281981643743211[56] = 0.0; - out_5273281981643743211[57] = 1.0; - out_5273281981643743211[58] = 0.0; - out_5273281981643743211[59] = 0.0; - out_5273281981643743211[60] = 0.0; - out_5273281981643743211[61] = 0.0; - out_5273281981643743211[62] = 0.0; - out_5273281981643743211[63] = 0.0; - out_5273281981643743211[64] = 0.0; - out_5273281981643743211[65] = 0.0; - out_5273281981643743211[66] = 0.0; - out_5273281981643743211[67] = 0.0; - out_5273281981643743211[68] = 0.0; - out_5273281981643743211[69] = 0.0; - out_5273281981643743211[70] = 0.0; - out_5273281981643743211[71] = 0.0; - out_5273281981643743211[72] = 0.0; - out_5273281981643743211[73] = 0.0; - out_5273281981643743211[74] = 0.0; - out_5273281981643743211[75] = 0.0; - out_5273281981643743211[76] = 1.0; - out_5273281981643743211[77] = 0.0; - out_5273281981643743211[78] = 0.0; - out_5273281981643743211[79] = 0.0; - out_5273281981643743211[80] = 0.0; - out_5273281981643743211[81] = 0.0; - out_5273281981643743211[82] = 0.0; - out_5273281981643743211[83] = 0.0; - out_5273281981643743211[84] = 0.0; - out_5273281981643743211[85] = 0.0; - out_5273281981643743211[86] = 0.0; - out_5273281981643743211[87] = 0.0; - out_5273281981643743211[88] = 0.0; - out_5273281981643743211[89] = 0.0; - out_5273281981643743211[90] = 0.0; - out_5273281981643743211[91] = 0.0; - out_5273281981643743211[92] = 0.0; - out_5273281981643743211[93] = 0.0; - out_5273281981643743211[94] = 0.0; - out_5273281981643743211[95] = 1.0; - out_5273281981643743211[96] = 0.0; - out_5273281981643743211[97] = 0.0; - out_5273281981643743211[98] = 0.0; - out_5273281981643743211[99] = 0.0; - out_5273281981643743211[100] = 0.0; - out_5273281981643743211[101] = 0.0; - out_5273281981643743211[102] = 0.0; - out_5273281981643743211[103] = 0.0; - out_5273281981643743211[104] = 0.0; - out_5273281981643743211[105] = 0.0; - out_5273281981643743211[106] = 0.0; - out_5273281981643743211[107] = 0.0; - out_5273281981643743211[108] = 0.0; - out_5273281981643743211[109] = 0.0; - out_5273281981643743211[110] = 0.0; - out_5273281981643743211[111] = 0.0; - out_5273281981643743211[112] = 0.0; - out_5273281981643743211[113] = 0.0; - out_5273281981643743211[114] = 1.0; - out_5273281981643743211[115] = 0.0; - out_5273281981643743211[116] = 0.0; - out_5273281981643743211[117] = 0.0; - out_5273281981643743211[118] = 0.0; - out_5273281981643743211[119] = 0.0; - out_5273281981643743211[120] = 0.0; - out_5273281981643743211[121] = 0.0; - out_5273281981643743211[122] = 0.0; - out_5273281981643743211[123] = 0.0; - out_5273281981643743211[124] = 0.0; - out_5273281981643743211[125] = 0.0; - out_5273281981643743211[126] = 0.0; - out_5273281981643743211[127] = 0.0; - out_5273281981643743211[128] = 0.0; - out_5273281981643743211[129] = 0.0; - out_5273281981643743211[130] = 0.0; - out_5273281981643743211[131] = 0.0; - out_5273281981643743211[132] = 0.0; - out_5273281981643743211[133] = 1.0; - out_5273281981643743211[134] = 0.0; - out_5273281981643743211[135] = 0.0; - out_5273281981643743211[136] = 0.0; - out_5273281981643743211[137] = 0.0; - out_5273281981643743211[138] = 0.0; - out_5273281981643743211[139] = 0.0; - out_5273281981643743211[140] = 0.0; - out_5273281981643743211[141] = 0.0; - out_5273281981643743211[142] = 0.0; - out_5273281981643743211[143] = 0.0; - out_5273281981643743211[144] = 0.0; - out_5273281981643743211[145] = 0.0; - out_5273281981643743211[146] = 0.0; - out_5273281981643743211[147] = 0.0; - out_5273281981643743211[148] = 0.0; - out_5273281981643743211[149] = 0.0; - out_5273281981643743211[150] = 0.0; - out_5273281981643743211[151] = 0.0; - out_5273281981643743211[152] = 1.0; - out_5273281981643743211[153] = 0.0; - out_5273281981643743211[154] = 0.0; - out_5273281981643743211[155] = 0.0; - out_5273281981643743211[156] = 0.0; - out_5273281981643743211[157] = 0.0; - out_5273281981643743211[158] = 0.0; - out_5273281981643743211[159] = 0.0; - out_5273281981643743211[160] = 0.0; - out_5273281981643743211[161] = 0.0; - out_5273281981643743211[162] = 0.0; - out_5273281981643743211[163] = 0.0; - out_5273281981643743211[164] = 0.0; - out_5273281981643743211[165] = 0.0; - out_5273281981643743211[166] = 0.0; - out_5273281981643743211[167] = 0.0; - out_5273281981643743211[168] = 0.0; - out_5273281981643743211[169] = 0.0; - out_5273281981643743211[170] = 0.0; - out_5273281981643743211[171] = 1.0; - out_5273281981643743211[172] = 0.0; - out_5273281981643743211[173] = 0.0; - out_5273281981643743211[174] = 0.0; - out_5273281981643743211[175] = 0.0; - out_5273281981643743211[176] = 0.0; - out_5273281981643743211[177] = 0.0; - out_5273281981643743211[178] = 0.0; - out_5273281981643743211[179] = 0.0; - out_5273281981643743211[180] = 0.0; - out_5273281981643743211[181] = 0.0; - out_5273281981643743211[182] = 0.0; - out_5273281981643743211[183] = 0.0; - out_5273281981643743211[184] = 0.0; - out_5273281981643743211[185] = 0.0; - out_5273281981643743211[186] = 0.0; - out_5273281981643743211[187] = 0.0; - out_5273281981643743211[188] = 0.0; - out_5273281981643743211[189] = 0.0; - out_5273281981643743211[190] = 1.0; - out_5273281981643743211[191] = 0.0; - out_5273281981643743211[192] = 0.0; - out_5273281981643743211[193] = 0.0; - out_5273281981643743211[194] = 0.0; - out_5273281981643743211[195] = 0.0; - out_5273281981643743211[196] = 0.0; - out_5273281981643743211[197] = 0.0; - out_5273281981643743211[198] = 0.0; - out_5273281981643743211[199] = 0.0; - out_5273281981643743211[200] = 0.0; - out_5273281981643743211[201] = 0.0; - out_5273281981643743211[202] = 0.0; - out_5273281981643743211[203] = 0.0; - out_5273281981643743211[204] = 0.0; - out_5273281981643743211[205] = 0.0; - out_5273281981643743211[206] = 0.0; - out_5273281981643743211[207] = 0.0; - out_5273281981643743211[208] = 0.0; - out_5273281981643743211[209] = 1.0; - out_5273281981643743211[210] = 0.0; - out_5273281981643743211[211] = 0.0; - out_5273281981643743211[212] = 0.0; - out_5273281981643743211[213] = 0.0; - out_5273281981643743211[214] = 0.0; - out_5273281981643743211[215] = 0.0; - out_5273281981643743211[216] = 0.0; - out_5273281981643743211[217] = 0.0; - out_5273281981643743211[218] = 0.0; - out_5273281981643743211[219] = 0.0; - out_5273281981643743211[220] = 0.0; - out_5273281981643743211[221] = 0.0; - out_5273281981643743211[222] = 0.0; - out_5273281981643743211[223] = 0.0; - out_5273281981643743211[224] = 0.0; - out_5273281981643743211[225] = 0.0; - out_5273281981643743211[226] = 0.0; - out_5273281981643743211[227] = 0.0; - out_5273281981643743211[228] = 1.0; - out_5273281981643743211[229] = 0.0; - out_5273281981643743211[230] = 0.0; - out_5273281981643743211[231] = 0.0; - out_5273281981643743211[232] = 0.0; - out_5273281981643743211[233] = 0.0; - out_5273281981643743211[234] = 0.0; - out_5273281981643743211[235] = 0.0; - out_5273281981643743211[236] = 0.0; - out_5273281981643743211[237] = 0.0; - out_5273281981643743211[238] = 0.0; - out_5273281981643743211[239] = 0.0; - out_5273281981643743211[240] = 0.0; - out_5273281981643743211[241] = 0.0; - out_5273281981643743211[242] = 0.0; - out_5273281981643743211[243] = 0.0; - out_5273281981643743211[244] = 0.0; - out_5273281981643743211[245] = 0.0; - out_5273281981643743211[246] = 0.0; - out_5273281981643743211[247] = 1.0; - out_5273281981643743211[248] = 0.0; - out_5273281981643743211[249] = 0.0; - out_5273281981643743211[250] = 0.0; - out_5273281981643743211[251] = 0.0; - out_5273281981643743211[252] = 0.0; - out_5273281981643743211[253] = 0.0; - out_5273281981643743211[254] = 0.0; - out_5273281981643743211[255] = 0.0; - out_5273281981643743211[256] = 0.0; - out_5273281981643743211[257] = 0.0; - out_5273281981643743211[258] = 0.0; - out_5273281981643743211[259] = 0.0; - out_5273281981643743211[260] = 0.0; - out_5273281981643743211[261] = 0.0; - out_5273281981643743211[262] = 0.0; - out_5273281981643743211[263] = 0.0; - out_5273281981643743211[264] = 0.0; - out_5273281981643743211[265] = 0.0; - out_5273281981643743211[266] = 1.0; - out_5273281981643743211[267] = 0.0; - out_5273281981643743211[268] = 0.0; - out_5273281981643743211[269] = 0.0; - out_5273281981643743211[270] = 0.0; - out_5273281981643743211[271] = 0.0; - out_5273281981643743211[272] = 0.0; - out_5273281981643743211[273] = 0.0; - out_5273281981643743211[274] = 0.0; - out_5273281981643743211[275] = 0.0; - out_5273281981643743211[276] = 0.0; - out_5273281981643743211[277] = 0.0; - out_5273281981643743211[278] = 0.0; - out_5273281981643743211[279] = 0.0; - out_5273281981643743211[280] = 0.0; - out_5273281981643743211[281] = 0.0; - out_5273281981643743211[282] = 0.0; - out_5273281981643743211[283] = 0.0; - out_5273281981643743211[284] = 0.0; - out_5273281981643743211[285] = 1.0; - out_5273281981643743211[286] = 0.0; - out_5273281981643743211[287] = 0.0; - out_5273281981643743211[288] = 0.0; - out_5273281981643743211[289] = 0.0; - out_5273281981643743211[290] = 0.0; - out_5273281981643743211[291] = 0.0; - out_5273281981643743211[292] = 0.0; - out_5273281981643743211[293] = 0.0; - out_5273281981643743211[294] = 0.0; - out_5273281981643743211[295] = 0.0; - out_5273281981643743211[296] = 0.0; - out_5273281981643743211[297] = 0.0; - out_5273281981643743211[298] = 0.0; - out_5273281981643743211[299] = 0.0; - out_5273281981643743211[300] = 0.0; - out_5273281981643743211[301] = 0.0; - out_5273281981643743211[302] = 0.0; - out_5273281981643743211[303] = 0.0; - out_5273281981643743211[304] = 1.0; - out_5273281981643743211[305] = 0.0; - out_5273281981643743211[306] = 0.0; - out_5273281981643743211[307] = 0.0; - out_5273281981643743211[308] = 0.0; - out_5273281981643743211[309] = 0.0; - out_5273281981643743211[310] = 0.0; - out_5273281981643743211[311] = 0.0; - out_5273281981643743211[312] = 0.0; - out_5273281981643743211[313] = 0.0; - out_5273281981643743211[314] = 0.0; - out_5273281981643743211[315] = 0.0; - out_5273281981643743211[316] = 0.0; - out_5273281981643743211[317] = 0.0; - out_5273281981643743211[318] = 0.0; - out_5273281981643743211[319] = 0.0; - out_5273281981643743211[320] = 0.0; - out_5273281981643743211[321] = 0.0; - out_5273281981643743211[322] = 0.0; - out_5273281981643743211[323] = 1.0; +void H_mod_fun(double *state, double *out_5245124330854285834) { + out_5245124330854285834[0] = 1.0; + out_5245124330854285834[1] = 0.0; + out_5245124330854285834[2] = 0.0; + out_5245124330854285834[3] = 0.0; + out_5245124330854285834[4] = 0.0; + out_5245124330854285834[5] = 0.0; + out_5245124330854285834[6] = 0.0; + out_5245124330854285834[7] = 0.0; + out_5245124330854285834[8] = 0.0; + out_5245124330854285834[9] = 0.0; + out_5245124330854285834[10] = 0.0; + out_5245124330854285834[11] = 0.0; + out_5245124330854285834[12] = 0.0; + out_5245124330854285834[13] = 0.0; + out_5245124330854285834[14] = 0.0; + out_5245124330854285834[15] = 0.0; + out_5245124330854285834[16] = 0.0; + out_5245124330854285834[17] = 0.0; + out_5245124330854285834[18] = 0.0; + out_5245124330854285834[19] = 1.0; + out_5245124330854285834[20] = 0.0; + out_5245124330854285834[21] = 0.0; + out_5245124330854285834[22] = 0.0; + out_5245124330854285834[23] = 0.0; + out_5245124330854285834[24] = 0.0; + out_5245124330854285834[25] = 0.0; + out_5245124330854285834[26] = 0.0; + out_5245124330854285834[27] = 0.0; + out_5245124330854285834[28] = 0.0; + out_5245124330854285834[29] = 0.0; + out_5245124330854285834[30] = 0.0; + out_5245124330854285834[31] = 0.0; + out_5245124330854285834[32] = 0.0; + out_5245124330854285834[33] = 0.0; + out_5245124330854285834[34] = 0.0; + out_5245124330854285834[35] = 0.0; + out_5245124330854285834[36] = 0.0; + out_5245124330854285834[37] = 0.0; + out_5245124330854285834[38] = 1.0; + out_5245124330854285834[39] = 0.0; + out_5245124330854285834[40] = 0.0; + out_5245124330854285834[41] = 0.0; + out_5245124330854285834[42] = 0.0; + out_5245124330854285834[43] = 0.0; + out_5245124330854285834[44] = 0.0; + out_5245124330854285834[45] = 0.0; + out_5245124330854285834[46] = 0.0; + out_5245124330854285834[47] = 0.0; + out_5245124330854285834[48] = 0.0; + out_5245124330854285834[49] = 0.0; + out_5245124330854285834[50] = 0.0; + out_5245124330854285834[51] = 0.0; + out_5245124330854285834[52] = 0.0; + out_5245124330854285834[53] = 0.0; + out_5245124330854285834[54] = 0.0; + out_5245124330854285834[55] = 0.0; + out_5245124330854285834[56] = 0.0; + out_5245124330854285834[57] = 1.0; + out_5245124330854285834[58] = 0.0; + out_5245124330854285834[59] = 0.0; + out_5245124330854285834[60] = 0.0; + out_5245124330854285834[61] = 0.0; + out_5245124330854285834[62] = 0.0; + out_5245124330854285834[63] = 0.0; + out_5245124330854285834[64] = 0.0; + out_5245124330854285834[65] = 0.0; + out_5245124330854285834[66] = 0.0; + out_5245124330854285834[67] = 0.0; + out_5245124330854285834[68] = 0.0; + out_5245124330854285834[69] = 0.0; + out_5245124330854285834[70] = 0.0; + out_5245124330854285834[71] = 0.0; + out_5245124330854285834[72] = 0.0; + out_5245124330854285834[73] = 0.0; + out_5245124330854285834[74] = 0.0; + out_5245124330854285834[75] = 0.0; + out_5245124330854285834[76] = 1.0; + out_5245124330854285834[77] = 0.0; + out_5245124330854285834[78] = 0.0; + out_5245124330854285834[79] = 0.0; + out_5245124330854285834[80] = 0.0; + out_5245124330854285834[81] = 0.0; + out_5245124330854285834[82] = 0.0; + out_5245124330854285834[83] = 0.0; + out_5245124330854285834[84] = 0.0; + out_5245124330854285834[85] = 0.0; + out_5245124330854285834[86] = 0.0; + out_5245124330854285834[87] = 0.0; + out_5245124330854285834[88] = 0.0; + out_5245124330854285834[89] = 0.0; + out_5245124330854285834[90] = 0.0; + out_5245124330854285834[91] = 0.0; + out_5245124330854285834[92] = 0.0; + out_5245124330854285834[93] = 0.0; + out_5245124330854285834[94] = 0.0; + out_5245124330854285834[95] = 1.0; + out_5245124330854285834[96] = 0.0; + out_5245124330854285834[97] = 0.0; + out_5245124330854285834[98] = 0.0; + out_5245124330854285834[99] = 0.0; + out_5245124330854285834[100] = 0.0; + out_5245124330854285834[101] = 0.0; + out_5245124330854285834[102] = 0.0; + out_5245124330854285834[103] = 0.0; + out_5245124330854285834[104] = 0.0; + out_5245124330854285834[105] = 0.0; + out_5245124330854285834[106] = 0.0; + out_5245124330854285834[107] = 0.0; + out_5245124330854285834[108] = 0.0; + out_5245124330854285834[109] = 0.0; + out_5245124330854285834[110] = 0.0; + out_5245124330854285834[111] = 0.0; + out_5245124330854285834[112] = 0.0; + out_5245124330854285834[113] = 0.0; + out_5245124330854285834[114] = 1.0; + out_5245124330854285834[115] = 0.0; + out_5245124330854285834[116] = 0.0; + out_5245124330854285834[117] = 0.0; + out_5245124330854285834[118] = 0.0; + out_5245124330854285834[119] = 0.0; + out_5245124330854285834[120] = 0.0; + out_5245124330854285834[121] = 0.0; + out_5245124330854285834[122] = 0.0; + out_5245124330854285834[123] = 0.0; + out_5245124330854285834[124] = 0.0; + out_5245124330854285834[125] = 0.0; + out_5245124330854285834[126] = 0.0; + out_5245124330854285834[127] = 0.0; + out_5245124330854285834[128] = 0.0; + out_5245124330854285834[129] = 0.0; + out_5245124330854285834[130] = 0.0; + out_5245124330854285834[131] = 0.0; + out_5245124330854285834[132] = 0.0; + out_5245124330854285834[133] = 1.0; + out_5245124330854285834[134] = 0.0; + out_5245124330854285834[135] = 0.0; + out_5245124330854285834[136] = 0.0; + out_5245124330854285834[137] = 0.0; + out_5245124330854285834[138] = 0.0; + out_5245124330854285834[139] = 0.0; + out_5245124330854285834[140] = 0.0; + out_5245124330854285834[141] = 0.0; + out_5245124330854285834[142] = 0.0; + out_5245124330854285834[143] = 0.0; + out_5245124330854285834[144] = 0.0; + out_5245124330854285834[145] = 0.0; + out_5245124330854285834[146] = 0.0; + out_5245124330854285834[147] = 0.0; + out_5245124330854285834[148] = 0.0; + out_5245124330854285834[149] = 0.0; + out_5245124330854285834[150] = 0.0; + out_5245124330854285834[151] = 0.0; + out_5245124330854285834[152] = 1.0; + out_5245124330854285834[153] = 0.0; + out_5245124330854285834[154] = 0.0; + out_5245124330854285834[155] = 0.0; + out_5245124330854285834[156] = 0.0; + out_5245124330854285834[157] = 0.0; + out_5245124330854285834[158] = 0.0; + out_5245124330854285834[159] = 0.0; + out_5245124330854285834[160] = 0.0; + out_5245124330854285834[161] = 0.0; + out_5245124330854285834[162] = 0.0; + out_5245124330854285834[163] = 0.0; + out_5245124330854285834[164] = 0.0; + out_5245124330854285834[165] = 0.0; + out_5245124330854285834[166] = 0.0; + out_5245124330854285834[167] = 0.0; + out_5245124330854285834[168] = 0.0; + out_5245124330854285834[169] = 0.0; + out_5245124330854285834[170] = 0.0; + out_5245124330854285834[171] = 1.0; + out_5245124330854285834[172] = 0.0; + out_5245124330854285834[173] = 0.0; + out_5245124330854285834[174] = 0.0; + out_5245124330854285834[175] = 0.0; + out_5245124330854285834[176] = 0.0; + out_5245124330854285834[177] = 0.0; + out_5245124330854285834[178] = 0.0; + out_5245124330854285834[179] = 0.0; + out_5245124330854285834[180] = 0.0; + out_5245124330854285834[181] = 0.0; + out_5245124330854285834[182] = 0.0; + out_5245124330854285834[183] = 0.0; + out_5245124330854285834[184] = 0.0; + out_5245124330854285834[185] = 0.0; + out_5245124330854285834[186] = 0.0; + out_5245124330854285834[187] = 0.0; + out_5245124330854285834[188] = 0.0; + out_5245124330854285834[189] = 0.0; + out_5245124330854285834[190] = 1.0; + out_5245124330854285834[191] = 0.0; + out_5245124330854285834[192] = 0.0; + out_5245124330854285834[193] = 0.0; + out_5245124330854285834[194] = 0.0; + out_5245124330854285834[195] = 0.0; + out_5245124330854285834[196] = 0.0; + out_5245124330854285834[197] = 0.0; + out_5245124330854285834[198] = 0.0; + out_5245124330854285834[199] = 0.0; + out_5245124330854285834[200] = 0.0; + out_5245124330854285834[201] = 0.0; + out_5245124330854285834[202] = 0.0; + out_5245124330854285834[203] = 0.0; + out_5245124330854285834[204] = 0.0; + out_5245124330854285834[205] = 0.0; + out_5245124330854285834[206] = 0.0; + out_5245124330854285834[207] = 0.0; + out_5245124330854285834[208] = 0.0; + out_5245124330854285834[209] = 1.0; + out_5245124330854285834[210] = 0.0; + out_5245124330854285834[211] = 0.0; + out_5245124330854285834[212] = 0.0; + out_5245124330854285834[213] = 0.0; + out_5245124330854285834[214] = 0.0; + out_5245124330854285834[215] = 0.0; + out_5245124330854285834[216] = 0.0; + out_5245124330854285834[217] = 0.0; + out_5245124330854285834[218] = 0.0; + out_5245124330854285834[219] = 0.0; + out_5245124330854285834[220] = 0.0; + out_5245124330854285834[221] = 0.0; + out_5245124330854285834[222] = 0.0; + out_5245124330854285834[223] = 0.0; + out_5245124330854285834[224] = 0.0; + out_5245124330854285834[225] = 0.0; + out_5245124330854285834[226] = 0.0; + out_5245124330854285834[227] = 0.0; + out_5245124330854285834[228] = 1.0; + out_5245124330854285834[229] = 0.0; + out_5245124330854285834[230] = 0.0; + out_5245124330854285834[231] = 0.0; + out_5245124330854285834[232] = 0.0; + out_5245124330854285834[233] = 0.0; + out_5245124330854285834[234] = 0.0; + out_5245124330854285834[235] = 0.0; + out_5245124330854285834[236] = 0.0; + out_5245124330854285834[237] = 0.0; + out_5245124330854285834[238] = 0.0; + out_5245124330854285834[239] = 0.0; + out_5245124330854285834[240] = 0.0; + out_5245124330854285834[241] = 0.0; + out_5245124330854285834[242] = 0.0; + out_5245124330854285834[243] = 0.0; + out_5245124330854285834[244] = 0.0; + out_5245124330854285834[245] = 0.0; + out_5245124330854285834[246] = 0.0; + out_5245124330854285834[247] = 1.0; + out_5245124330854285834[248] = 0.0; + out_5245124330854285834[249] = 0.0; + out_5245124330854285834[250] = 0.0; + out_5245124330854285834[251] = 0.0; + out_5245124330854285834[252] = 0.0; + out_5245124330854285834[253] = 0.0; + out_5245124330854285834[254] = 0.0; + out_5245124330854285834[255] = 0.0; + out_5245124330854285834[256] = 0.0; + out_5245124330854285834[257] = 0.0; + out_5245124330854285834[258] = 0.0; + out_5245124330854285834[259] = 0.0; + out_5245124330854285834[260] = 0.0; + out_5245124330854285834[261] = 0.0; + out_5245124330854285834[262] = 0.0; + out_5245124330854285834[263] = 0.0; + out_5245124330854285834[264] = 0.0; + out_5245124330854285834[265] = 0.0; + out_5245124330854285834[266] = 1.0; + out_5245124330854285834[267] = 0.0; + out_5245124330854285834[268] = 0.0; + out_5245124330854285834[269] = 0.0; + out_5245124330854285834[270] = 0.0; + out_5245124330854285834[271] = 0.0; + out_5245124330854285834[272] = 0.0; + out_5245124330854285834[273] = 0.0; + out_5245124330854285834[274] = 0.0; + out_5245124330854285834[275] = 0.0; + out_5245124330854285834[276] = 0.0; + out_5245124330854285834[277] = 0.0; + out_5245124330854285834[278] = 0.0; + out_5245124330854285834[279] = 0.0; + out_5245124330854285834[280] = 0.0; + out_5245124330854285834[281] = 0.0; + out_5245124330854285834[282] = 0.0; + out_5245124330854285834[283] = 0.0; + out_5245124330854285834[284] = 0.0; + out_5245124330854285834[285] = 1.0; + out_5245124330854285834[286] = 0.0; + out_5245124330854285834[287] = 0.0; + out_5245124330854285834[288] = 0.0; + out_5245124330854285834[289] = 0.0; + out_5245124330854285834[290] = 0.0; + out_5245124330854285834[291] = 0.0; + out_5245124330854285834[292] = 0.0; + out_5245124330854285834[293] = 0.0; + out_5245124330854285834[294] = 0.0; + out_5245124330854285834[295] = 0.0; + out_5245124330854285834[296] = 0.0; + out_5245124330854285834[297] = 0.0; + out_5245124330854285834[298] = 0.0; + out_5245124330854285834[299] = 0.0; + out_5245124330854285834[300] = 0.0; + out_5245124330854285834[301] = 0.0; + out_5245124330854285834[302] = 0.0; + out_5245124330854285834[303] = 0.0; + out_5245124330854285834[304] = 1.0; + out_5245124330854285834[305] = 0.0; + out_5245124330854285834[306] = 0.0; + out_5245124330854285834[307] = 0.0; + out_5245124330854285834[308] = 0.0; + out_5245124330854285834[309] = 0.0; + out_5245124330854285834[310] = 0.0; + out_5245124330854285834[311] = 0.0; + out_5245124330854285834[312] = 0.0; + out_5245124330854285834[313] = 0.0; + out_5245124330854285834[314] = 0.0; + out_5245124330854285834[315] = 0.0; + out_5245124330854285834[316] = 0.0; + out_5245124330854285834[317] = 0.0; + out_5245124330854285834[318] = 0.0; + out_5245124330854285834[319] = 0.0; + out_5245124330854285834[320] = 0.0; + out_5245124330854285834[321] = 0.0; + out_5245124330854285834[322] = 0.0; + out_5245124330854285834[323] = 1.0; } -void f_fun(double *state, double dt, double *out_3798976948542527989) { - out_3798976948542527989[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_3798976948542527989[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_3798976948542527989[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_3798976948542527989[3] = dt*state[12] + state[3]; - out_3798976948542527989[4] = dt*state[13] + state[4]; - out_3798976948542527989[5] = dt*state[14] + state[5]; - out_3798976948542527989[6] = state[6]; - out_3798976948542527989[7] = state[7]; - out_3798976948542527989[8] = state[8]; - out_3798976948542527989[9] = state[9]; - out_3798976948542527989[10] = state[10]; - out_3798976948542527989[11] = state[11]; - out_3798976948542527989[12] = state[12]; - out_3798976948542527989[13] = state[13]; - out_3798976948542527989[14] = state[14]; - out_3798976948542527989[15] = state[15]; - out_3798976948542527989[16] = state[16]; - out_3798976948542527989[17] = state[17]; +void f_fun(double *state, double dt, double *out_6830900184565569763) { + out_6830900184565569763[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_6830900184565569763[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_6830900184565569763[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_6830900184565569763[3] = dt*state[12] + state[3]; + out_6830900184565569763[4] = dt*state[13] + state[4]; + out_6830900184565569763[5] = dt*state[14] + state[5]; + out_6830900184565569763[6] = state[6]; + out_6830900184565569763[7] = state[7]; + out_6830900184565569763[8] = state[8]; + out_6830900184565569763[9] = state[9]; + out_6830900184565569763[10] = state[10]; + out_6830900184565569763[11] = state[11]; + out_6830900184565569763[12] = state[12]; + out_6830900184565569763[13] = state[13]; + out_6830900184565569763[14] = state[14]; + out_6830900184565569763[15] = state[15]; + out_6830900184565569763[16] = state[16]; + out_6830900184565569763[17] = state[17]; } -void F_fun(double *state, double dt, double *out_5376087696404799721) { - out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[2] = 0; - out_5376087696404799721[3] = 0; - out_5376087696404799721[4] = 0; - out_5376087696404799721[5] = 0; - out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[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_5376087696404799721[9] = 0; - out_5376087696404799721[10] = 0; - out_5376087696404799721[11] = 0; - out_5376087696404799721[12] = 0; - out_5376087696404799721[13] = 0; - out_5376087696404799721[14] = 0; - out_5376087696404799721[15] = 0; - out_5376087696404799721[16] = 0; - out_5376087696404799721[17] = 0; - out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[20] = 0; - out_5376087696404799721[21] = 0; - out_5376087696404799721[22] = 0; - out_5376087696404799721[23] = 0; - out_5376087696404799721[24] = 0; - out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[27] = 0; - out_5376087696404799721[28] = 0; - out_5376087696404799721[29] = 0; - out_5376087696404799721[30] = 0; - out_5376087696404799721[31] = 0; - out_5376087696404799721[32] = 0; - out_5376087696404799721[33] = 0; - out_5376087696404799721[34] = 0; - out_5376087696404799721[35] = 0; - out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[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_5376087696404799721[39] = 0; - out_5376087696404799721[40] = 0; - out_5376087696404799721[41] = 0; - out_5376087696404799721[42] = 0; - out_5376087696404799721[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_5376087696404799721[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_5376087696404799721[45] = 0; - out_5376087696404799721[46] = 0; - out_5376087696404799721[47] = 0; - out_5376087696404799721[48] = 0; - out_5376087696404799721[49] = 0; - out_5376087696404799721[50] = 0; - out_5376087696404799721[51] = 0; - out_5376087696404799721[52] = 0; - out_5376087696404799721[53] = 0; - out_5376087696404799721[54] = 0; - out_5376087696404799721[55] = 0; - out_5376087696404799721[56] = 0; - out_5376087696404799721[57] = 1; - out_5376087696404799721[58] = 0; - out_5376087696404799721[59] = 0; - out_5376087696404799721[60] = 0; - out_5376087696404799721[61] = 0; - out_5376087696404799721[62] = 0; - out_5376087696404799721[63] = 0; - out_5376087696404799721[64] = 0; - out_5376087696404799721[65] = 0; - out_5376087696404799721[66] = dt; - out_5376087696404799721[67] = 0; - out_5376087696404799721[68] = 0; - out_5376087696404799721[69] = 0; - out_5376087696404799721[70] = 0; - out_5376087696404799721[71] = 0; - out_5376087696404799721[72] = 0; - out_5376087696404799721[73] = 0; - out_5376087696404799721[74] = 0; - out_5376087696404799721[75] = 0; - out_5376087696404799721[76] = 1; - out_5376087696404799721[77] = 0; - out_5376087696404799721[78] = 0; - out_5376087696404799721[79] = 0; - out_5376087696404799721[80] = 0; - out_5376087696404799721[81] = 0; - out_5376087696404799721[82] = 0; - out_5376087696404799721[83] = 0; - out_5376087696404799721[84] = 0; - out_5376087696404799721[85] = dt; - out_5376087696404799721[86] = 0; - out_5376087696404799721[87] = 0; - out_5376087696404799721[88] = 0; - out_5376087696404799721[89] = 0; - out_5376087696404799721[90] = 0; - out_5376087696404799721[91] = 0; - out_5376087696404799721[92] = 0; - out_5376087696404799721[93] = 0; - out_5376087696404799721[94] = 0; - out_5376087696404799721[95] = 1; - out_5376087696404799721[96] = 0; - out_5376087696404799721[97] = 0; - out_5376087696404799721[98] = 0; - out_5376087696404799721[99] = 0; - out_5376087696404799721[100] = 0; - out_5376087696404799721[101] = 0; - out_5376087696404799721[102] = 0; - out_5376087696404799721[103] = 0; - out_5376087696404799721[104] = dt; - out_5376087696404799721[105] = 0; - out_5376087696404799721[106] = 0; - out_5376087696404799721[107] = 0; - out_5376087696404799721[108] = 0; - out_5376087696404799721[109] = 0; - out_5376087696404799721[110] = 0; - out_5376087696404799721[111] = 0; - out_5376087696404799721[112] = 0; - out_5376087696404799721[113] = 0; - out_5376087696404799721[114] = 1; - out_5376087696404799721[115] = 0; - out_5376087696404799721[116] = 0; - out_5376087696404799721[117] = 0; - out_5376087696404799721[118] = 0; - out_5376087696404799721[119] = 0; - out_5376087696404799721[120] = 0; - out_5376087696404799721[121] = 0; - out_5376087696404799721[122] = 0; - out_5376087696404799721[123] = 0; - out_5376087696404799721[124] = 0; - out_5376087696404799721[125] = 0; - out_5376087696404799721[126] = 0; - out_5376087696404799721[127] = 0; - out_5376087696404799721[128] = 0; - out_5376087696404799721[129] = 0; - out_5376087696404799721[130] = 0; - out_5376087696404799721[131] = 0; - out_5376087696404799721[132] = 0; - out_5376087696404799721[133] = 1; - out_5376087696404799721[134] = 0; - out_5376087696404799721[135] = 0; - out_5376087696404799721[136] = 0; - out_5376087696404799721[137] = 0; - out_5376087696404799721[138] = 0; - out_5376087696404799721[139] = 0; - out_5376087696404799721[140] = 0; - out_5376087696404799721[141] = 0; - out_5376087696404799721[142] = 0; - out_5376087696404799721[143] = 0; - out_5376087696404799721[144] = 0; - out_5376087696404799721[145] = 0; - out_5376087696404799721[146] = 0; - out_5376087696404799721[147] = 0; - out_5376087696404799721[148] = 0; - out_5376087696404799721[149] = 0; - out_5376087696404799721[150] = 0; - out_5376087696404799721[151] = 0; - out_5376087696404799721[152] = 1; - out_5376087696404799721[153] = 0; - out_5376087696404799721[154] = 0; - out_5376087696404799721[155] = 0; - out_5376087696404799721[156] = 0; - out_5376087696404799721[157] = 0; - out_5376087696404799721[158] = 0; - out_5376087696404799721[159] = 0; - out_5376087696404799721[160] = 0; - out_5376087696404799721[161] = 0; - out_5376087696404799721[162] = 0; - out_5376087696404799721[163] = 0; - out_5376087696404799721[164] = 0; - out_5376087696404799721[165] = 0; - out_5376087696404799721[166] = 0; - out_5376087696404799721[167] = 0; - out_5376087696404799721[168] = 0; - out_5376087696404799721[169] = 0; - out_5376087696404799721[170] = 0; - out_5376087696404799721[171] = 1; - out_5376087696404799721[172] = 0; - out_5376087696404799721[173] = 0; - out_5376087696404799721[174] = 0; - out_5376087696404799721[175] = 0; - out_5376087696404799721[176] = 0; - out_5376087696404799721[177] = 0; - out_5376087696404799721[178] = 0; - out_5376087696404799721[179] = 0; - out_5376087696404799721[180] = 0; - out_5376087696404799721[181] = 0; - out_5376087696404799721[182] = 0; - out_5376087696404799721[183] = 0; - out_5376087696404799721[184] = 0; - out_5376087696404799721[185] = 0; - out_5376087696404799721[186] = 0; - out_5376087696404799721[187] = 0; - out_5376087696404799721[188] = 0; - out_5376087696404799721[189] = 0; - out_5376087696404799721[190] = 1; - out_5376087696404799721[191] = 0; - out_5376087696404799721[192] = 0; - out_5376087696404799721[193] = 0; - out_5376087696404799721[194] = 0; - out_5376087696404799721[195] = 0; - out_5376087696404799721[196] = 0; - out_5376087696404799721[197] = 0; - out_5376087696404799721[198] = 0; - out_5376087696404799721[199] = 0; - out_5376087696404799721[200] = 0; - out_5376087696404799721[201] = 0; - out_5376087696404799721[202] = 0; - out_5376087696404799721[203] = 0; - out_5376087696404799721[204] = 0; - out_5376087696404799721[205] = 0; - out_5376087696404799721[206] = 0; - out_5376087696404799721[207] = 0; - out_5376087696404799721[208] = 0; - out_5376087696404799721[209] = 1; - out_5376087696404799721[210] = 0; - out_5376087696404799721[211] = 0; - out_5376087696404799721[212] = 0; - out_5376087696404799721[213] = 0; - out_5376087696404799721[214] = 0; - out_5376087696404799721[215] = 0; - out_5376087696404799721[216] = 0; - out_5376087696404799721[217] = 0; - out_5376087696404799721[218] = 0; - out_5376087696404799721[219] = 0; - out_5376087696404799721[220] = 0; - out_5376087696404799721[221] = 0; - out_5376087696404799721[222] = 0; - out_5376087696404799721[223] = 0; - out_5376087696404799721[224] = 0; - out_5376087696404799721[225] = 0; - out_5376087696404799721[226] = 0; - out_5376087696404799721[227] = 0; - out_5376087696404799721[228] = 1; - out_5376087696404799721[229] = 0; - out_5376087696404799721[230] = 0; - out_5376087696404799721[231] = 0; - out_5376087696404799721[232] = 0; - out_5376087696404799721[233] = 0; - out_5376087696404799721[234] = 0; - out_5376087696404799721[235] = 0; - out_5376087696404799721[236] = 0; - out_5376087696404799721[237] = 0; - out_5376087696404799721[238] = 0; - out_5376087696404799721[239] = 0; - out_5376087696404799721[240] = 0; - out_5376087696404799721[241] = 0; - out_5376087696404799721[242] = 0; - out_5376087696404799721[243] = 0; - out_5376087696404799721[244] = 0; - out_5376087696404799721[245] = 0; - out_5376087696404799721[246] = 0; - out_5376087696404799721[247] = 1; - out_5376087696404799721[248] = 0; - out_5376087696404799721[249] = 0; - out_5376087696404799721[250] = 0; - out_5376087696404799721[251] = 0; - out_5376087696404799721[252] = 0; - out_5376087696404799721[253] = 0; - out_5376087696404799721[254] = 0; - out_5376087696404799721[255] = 0; - out_5376087696404799721[256] = 0; - out_5376087696404799721[257] = 0; - out_5376087696404799721[258] = 0; - out_5376087696404799721[259] = 0; - out_5376087696404799721[260] = 0; - out_5376087696404799721[261] = 0; - out_5376087696404799721[262] = 0; - out_5376087696404799721[263] = 0; - out_5376087696404799721[264] = 0; - out_5376087696404799721[265] = 0; - out_5376087696404799721[266] = 1; - out_5376087696404799721[267] = 0; - out_5376087696404799721[268] = 0; - out_5376087696404799721[269] = 0; - out_5376087696404799721[270] = 0; - out_5376087696404799721[271] = 0; - out_5376087696404799721[272] = 0; - out_5376087696404799721[273] = 0; - out_5376087696404799721[274] = 0; - out_5376087696404799721[275] = 0; - out_5376087696404799721[276] = 0; - out_5376087696404799721[277] = 0; - out_5376087696404799721[278] = 0; - out_5376087696404799721[279] = 0; - out_5376087696404799721[280] = 0; - out_5376087696404799721[281] = 0; - out_5376087696404799721[282] = 0; - out_5376087696404799721[283] = 0; - out_5376087696404799721[284] = 0; - out_5376087696404799721[285] = 1; - out_5376087696404799721[286] = 0; - out_5376087696404799721[287] = 0; - out_5376087696404799721[288] = 0; - out_5376087696404799721[289] = 0; - out_5376087696404799721[290] = 0; - out_5376087696404799721[291] = 0; - out_5376087696404799721[292] = 0; - out_5376087696404799721[293] = 0; - out_5376087696404799721[294] = 0; - out_5376087696404799721[295] = 0; - out_5376087696404799721[296] = 0; - out_5376087696404799721[297] = 0; - out_5376087696404799721[298] = 0; - out_5376087696404799721[299] = 0; - out_5376087696404799721[300] = 0; - out_5376087696404799721[301] = 0; - out_5376087696404799721[302] = 0; - out_5376087696404799721[303] = 0; - out_5376087696404799721[304] = 1; - out_5376087696404799721[305] = 0; - out_5376087696404799721[306] = 0; - out_5376087696404799721[307] = 0; - out_5376087696404799721[308] = 0; - out_5376087696404799721[309] = 0; - out_5376087696404799721[310] = 0; - out_5376087696404799721[311] = 0; - out_5376087696404799721[312] = 0; - out_5376087696404799721[313] = 0; - out_5376087696404799721[314] = 0; - out_5376087696404799721[315] = 0; - out_5376087696404799721[316] = 0; - out_5376087696404799721[317] = 0; - out_5376087696404799721[318] = 0; - out_5376087696404799721[319] = 0; - out_5376087696404799721[320] = 0; - out_5376087696404799721[321] = 0; - out_5376087696404799721[322] = 0; - out_5376087696404799721[323] = 1; +void F_fun(double *state, double dt, double *out_5118094281500581129) { + out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[2] = 0; + out_5118094281500581129[3] = 0; + out_5118094281500581129[4] = 0; + out_5118094281500581129[5] = 0; + out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[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_5118094281500581129[9] = 0; + out_5118094281500581129[10] = 0; + out_5118094281500581129[11] = 0; + out_5118094281500581129[12] = 0; + out_5118094281500581129[13] = 0; + out_5118094281500581129[14] = 0; + out_5118094281500581129[15] = 0; + out_5118094281500581129[16] = 0; + out_5118094281500581129[17] = 0; + out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[20] = 0; + out_5118094281500581129[21] = 0; + out_5118094281500581129[22] = 0; + out_5118094281500581129[23] = 0; + out_5118094281500581129[24] = 0; + out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[27] = 0; + out_5118094281500581129[28] = 0; + out_5118094281500581129[29] = 0; + out_5118094281500581129[30] = 0; + out_5118094281500581129[31] = 0; + out_5118094281500581129[32] = 0; + out_5118094281500581129[33] = 0; + out_5118094281500581129[34] = 0; + out_5118094281500581129[35] = 0; + out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[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_5118094281500581129[39] = 0; + out_5118094281500581129[40] = 0; + out_5118094281500581129[41] = 0; + out_5118094281500581129[42] = 0; + out_5118094281500581129[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_5118094281500581129[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_5118094281500581129[45] = 0; + out_5118094281500581129[46] = 0; + out_5118094281500581129[47] = 0; + out_5118094281500581129[48] = 0; + out_5118094281500581129[49] = 0; + out_5118094281500581129[50] = 0; + out_5118094281500581129[51] = 0; + out_5118094281500581129[52] = 0; + out_5118094281500581129[53] = 0; + out_5118094281500581129[54] = 0; + out_5118094281500581129[55] = 0; + out_5118094281500581129[56] = 0; + out_5118094281500581129[57] = 1; + out_5118094281500581129[58] = 0; + out_5118094281500581129[59] = 0; + out_5118094281500581129[60] = 0; + out_5118094281500581129[61] = 0; + out_5118094281500581129[62] = 0; + out_5118094281500581129[63] = 0; + out_5118094281500581129[64] = 0; + out_5118094281500581129[65] = 0; + out_5118094281500581129[66] = dt; + out_5118094281500581129[67] = 0; + out_5118094281500581129[68] = 0; + out_5118094281500581129[69] = 0; + out_5118094281500581129[70] = 0; + out_5118094281500581129[71] = 0; + out_5118094281500581129[72] = 0; + out_5118094281500581129[73] = 0; + out_5118094281500581129[74] = 0; + out_5118094281500581129[75] = 0; + out_5118094281500581129[76] = 1; + out_5118094281500581129[77] = 0; + out_5118094281500581129[78] = 0; + out_5118094281500581129[79] = 0; + out_5118094281500581129[80] = 0; + out_5118094281500581129[81] = 0; + out_5118094281500581129[82] = 0; + out_5118094281500581129[83] = 0; + out_5118094281500581129[84] = 0; + out_5118094281500581129[85] = dt; + out_5118094281500581129[86] = 0; + out_5118094281500581129[87] = 0; + out_5118094281500581129[88] = 0; + out_5118094281500581129[89] = 0; + out_5118094281500581129[90] = 0; + out_5118094281500581129[91] = 0; + out_5118094281500581129[92] = 0; + out_5118094281500581129[93] = 0; + out_5118094281500581129[94] = 0; + out_5118094281500581129[95] = 1; + out_5118094281500581129[96] = 0; + out_5118094281500581129[97] = 0; + out_5118094281500581129[98] = 0; + out_5118094281500581129[99] = 0; + out_5118094281500581129[100] = 0; + out_5118094281500581129[101] = 0; + out_5118094281500581129[102] = 0; + out_5118094281500581129[103] = 0; + out_5118094281500581129[104] = dt; + out_5118094281500581129[105] = 0; + out_5118094281500581129[106] = 0; + out_5118094281500581129[107] = 0; + out_5118094281500581129[108] = 0; + out_5118094281500581129[109] = 0; + out_5118094281500581129[110] = 0; + out_5118094281500581129[111] = 0; + out_5118094281500581129[112] = 0; + out_5118094281500581129[113] = 0; + out_5118094281500581129[114] = 1; + out_5118094281500581129[115] = 0; + out_5118094281500581129[116] = 0; + out_5118094281500581129[117] = 0; + out_5118094281500581129[118] = 0; + out_5118094281500581129[119] = 0; + out_5118094281500581129[120] = 0; + out_5118094281500581129[121] = 0; + out_5118094281500581129[122] = 0; + out_5118094281500581129[123] = 0; + out_5118094281500581129[124] = 0; + out_5118094281500581129[125] = 0; + out_5118094281500581129[126] = 0; + out_5118094281500581129[127] = 0; + out_5118094281500581129[128] = 0; + out_5118094281500581129[129] = 0; + out_5118094281500581129[130] = 0; + out_5118094281500581129[131] = 0; + out_5118094281500581129[132] = 0; + out_5118094281500581129[133] = 1; + out_5118094281500581129[134] = 0; + out_5118094281500581129[135] = 0; + out_5118094281500581129[136] = 0; + out_5118094281500581129[137] = 0; + out_5118094281500581129[138] = 0; + out_5118094281500581129[139] = 0; + out_5118094281500581129[140] = 0; + out_5118094281500581129[141] = 0; + out_5118094281500581129[142] = 0; + out_5118094281500581129[143] = 0; + out_5118094281500581129[144] = 0; + out_5118094281500581129[145] = 0; + out_5118094281500581129[146] = 0; + out_5118094281500581129[147] = 0; + out_5118094281500581129[148] = 0; + out_5118094281500581129[149] = 0; + out_5118094281500581129[150] = 0; + out_5118094281500581129[151] = 0; + out_5118094281500581129[152] = 1; + out_5118094281500581129[153] = 0; + out_5118094281500581129[154] = 0; + out_5118094281500581129[155] = 0; + out_5118094281500581129[156] = 0; + out_5118094281500581129[157] = 0; + out_5118094281500581129[158] = 0; + out_5118094281500581129[159] = 0; + out_5118094281500581129[160] = 0; + out_5118094281500581129[161] = 0; + out_5118094281500581129[162] = 0; + out_5118094281500581129[163] = 0; + out_5118094281500581129[164] = 0; + out_5118094281500581129[165] = 0; + out_5118094281500581129[166] = 0; + out_5118094281500581129[167] = 0; + out_5118094281500581129[168] = 0; + out_5118094281500581129[169] = 0; + out_5118094281500581129[170] = 0; + out_5118094281500581129[171] = 1; + out_5118094281500581129[172] = 0; + out_5118094281500581129[173] = 0; + out_5118094281500581129[174] = 0; + out_5118094281500581129[175] = 0; + out_5118094281500581129[176] = 0; + out_5118094281500581129[177] = 0; + out_5118094281500581129[178] = 0; + out_5118094281500581129[179] = 0; + out_5118094281500581129[180] = 0; + out_5118094281500581129[181] = 0; + out_5118094281500581129[182] = 0; + out_5118094281500581129[183] = 0; + out_5118094281500581129[184] = 0; + out_5118094281500581129[185] = 0; + out_5118094281500581129[186] = 0; + out_5118094281500581129[187] = 0; + out_5118094281500581129[188] = 0; + out_5118094281500581129[189] = 0; + out_5118094281500581129[190] = 1; + out_5118094281500581129[191] = 0; + out_5118094281500581129[192] = 0; + out_5118094281500581129[193] = 0; + out_5118094281500581129[194] = 0; + out_5118094281500581129[195] = 0; + out_5118094281500581129[196] = 0; + out_5118094281500581129[197] = 0; + out_5118094281500581129[198] = 0; + out_5118094281500581129[199] = 0; + out_5118094281500581129[200] = 0; + out_5118094281500581129[201] = 0; + out_5118094281500581129[202] = 0; + out_5118094281500581129[203] = 0; + out_5118094281500581129[204] = 0; + out_5118094281500581129[205] = 0; + out_5118094281500581129[206] = 0; + out_5118094281500581129[207] = 0; + out_5118094281500581129[208] = 0; + out_5118094281500581129[209] = 1; + out_5118094281500581129[210] = 0; + out_5118094281500581129[211] = 0; + out_5118094281500581129[212] = 0; + out_5118094281500581129[213] = 0; + out_5118094281500581129[214] = 0; + out_5118094281500581129[215] = 0; + out_5118094281500581129[216] = 0; + out_5118094281500581129[217] = 0; + out_5118094281500581129[218] = 0; + out_5118094281500581129[219] = 0; + out_5118094281500581129[220] = 0; + out_5118094281500581129[221] = 0; + out_5118094281500581129[222] = 0; + out_5118094281500581129[223] = 0; + out_5118094281500581129[224] = 0; + out_5118094281500581129[225] = 0; + out_5118094281500581129[226] = 0; + out_5118094281500581129[227] = 0; + out_5118094281500581129[228] = 1; + out_5118094281500581129[229] = 0; + out_5118094281500581129[230] = 0; + out_5118094281500581129[231] = 0; + out_5118094281500581129[232] = 0; + out_5118094281500581129[233] = 0; + out_5118094281500581129[234] = 0; + out_5118094281500581129[235] = 0; + out_5118094281500581129[236] = 0; + out_5118094281500581129[237] = 0; + out_5118094281500581129[238] = 0; + out_5118094281500581129[239] = 0; + out_5118094281500581129[240] = 0; + out_5118094281500581129[241] = 0; + out_5118094281500581129[242] = 0; + out_5118094281500581129[243] = 0; + out_5118094281500581129[244] = 0; + out_5118094281500581129[245] = 0; + out_5118094281500581129[246] = 0; + out_5118094281500581129[247] = 1; + out_5118094281500581129[248] = 0; + out_5118094281500581129[249] = 0; + out_5118094281500581129[250] = 0; + out_5118094281500581129[251] = 0; + out_5118094281500581129[252] = 0; + out_5118094281500581129[253] = 0; + out_5118094281500581129[254] = 0; + out_5118094281500581129[255] = 0; + out_5118094281500581129[256] = 0; + out_5118094281500581129[257] = 0; + out_5118094281500581129[258] = 0; + out_5118094281500581129[259] = 0; + out_5118094281500581129[260] = 0; + out_5118094281500581129[261] = 0; + out_5118094281500581129[262] = 0; + out_5118094281500581129[263] = 0; + out_5118094281500581129[264] = 0; + out_5118094281500581129[265] = 0; + out_5118094281500581129[266] = 1; + out_5118094281500581129[267] = 0; + out_5118094281500581129[268] = 0; + out_5118094281500581129[269] = 0; + out_5118094281500581129[270] = 0; + out_5118094281500581129[271] = 0; + out_5118094281500581129[272] = 0; + out_5118094281500581129[273] = 0; + out_5118094281500581129[274] = 0; + out_5118094281500581129[275] = 0; + out_5118094281500581129[276] = 0; + out_5118094281500581129[277] = 0; + out_5118094281500581129[278] = 0; + out_5118094281500581129[279] = 0; + out_5118094281500581129[280] = 0; + out_5118094281500581129[281] = 0; + out_5118094281500581129[282] = 0; + out_5118094281500581129[283] = 0; + out_5118094281500581129[284] = 0; + out_5118094281500581129[285] = 1; + out_5118094281500581129[286] = 0; + out_5118094281500581129[287] = 0; + out_5118094281500581129[288] = 0; + out_5118094281500581129[289] = 0; + out_5118094281500581129[290] = 0; + out_5118094281500581129[291] = 0; + out_5118094281500581129[292] = 0; + out_5118094281500581129[293] = 0; + out_5118094281500581129[294] = 0; + out_5118094281500581129[295] = 0; + out_5118094281500581129[296] = 0; + out_5118094281500581129[297] = 0; + out_5118094281500581129[298] = 0; + out_5118094281500581129[299] = 0; + out_5118094281500581129[300] = 0; + out_5118094281500581129[301] = 0; + out_5118094281500581129[302] = 0; + out_5118094281500581129[303] = 0; + out_5118094281500581129[304] = 1; + out_5118094281500581129[305] = 0; + out_5118094281500581129[306] = 0; + out_5118094281500581129[307] = 0; + out_5118094281500581129[308] = 0; + out_5118094281500581129[309] = 0; + out_5118094281500581129[310] = 0; + out_5118094281500581129[311] = 0; + out_5118094281500581129[312] = 0; + out_5118094281500581129[313] = 0; + out_5118094281500581129[314] = 0; + out_5118094281500581129[315] = 0; + out_5118094281500581129[316] = 0; + out_5118094281500581129[317] = 0; + out_5118094281500581129[318] = 0; + out_5118094281500581129[319] = 0; + out_5118094281500581129[320] = 0; + out_5118094281500581129[321] = 0; + out_5118094281500581129[322] = 0; + out_5118094281500581129[323] = 1; } -void h_4(double *state, double *unused, double *out_6100547545538860384) { - out_6100547545538860384[0] = state[6] + state[9]; - out_6100547545538860384[1] = state[7] + state[10]; - out_6100547545538860384[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_4935070827926002458) { + out_4935070827926002458[0] = state[6] + state[9]; + out_4935070827926002458[1] = state[7] + state[10]; + out_4935070827926002458[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_9045089191487650783) { - out_9045089191487650783[0] = 0; - out_9045089191487650783[1] = 0; - out_9045089191487650783[2] = 0; - out_9045089191487650783[3] = 0; - out_9045089191487650783[4] = 0; - out_9045089191487650783[5] = 0; - out_9045089191487650783[6] = 1; - out_9045089191487650783[7] = 0; - out_9045089191487650783[8] = 0; - out_9045089191487650783[9] = 1; - out_9045089191487650783[10] = 0; - out_9045089191487650783[11] = 0; - out_9045089191487650783[12] = 0; - out_9045089191487650783[13] = 0; - out_9045089191487650783[14] = 0; - out_9045089191487650783[15] = 0; - out_9045089191487650783[16] = 0; - out_9045089191487650783[17] = 0; - out_9045089191487650783[18] = 0; - out_9045089191487650783[19] = 0; - out_9045089191487650783[20] = 0; - out_9045089191487650783[21] = 0; - out_9045089191487650783[22] = 0; - out_9045089191487650783[23] = 0; - out_9045089191487650783[24] = 0; - out_9045089191487650783[25] = 1; - out_9045089191487650783[26] = 0; - out_9045089191487650783[27] = 0; - out_9045089191487650783[28] = 1; - out_9045089191487650783[29] = 0; - out_9045089191487650783[30] = 0; - out_9045089191487650783[31] = 0; - out_9045089191487650783[32] = 0; - out_9045089191487650783[33] = 0; - out_9045089191487650783[34] = 0; - out_9045089191487650783[35] = 0; - out_9045089191487650783[36] = 0; - out_9045089191487650783[37] = 0; - out_9045089191487650783[38] = 0; - out_9045089191487650783[39] = 0; - out_9045089191487650783[40] = 0; - out_9045089191487650783[41] = 0; - out_9045089191487650783[42] = 0; - out_9045089191487650783[43] = 0; - out_9045089191487650783[44] = 1; - out_9045089191487650783[45] = 0; - out_9045089191487650783[46] = 0; - out_9045089191487650783[47] = 1; - out_9045089191487650783[48] = 0; - out_9045089191487650783[49] = 0; - out_9045089191487650783[50] = 0; - out_9045089191487650783[51] = 0; - out_9045089191487650783[52] = 0; - out_9045089191487650783[53] = 0; +void H_4(double *state, double *unused, double *out_6443071812737183938) { + out_6443071812737183938[0] = 0; + out_6443071812737183938[1] = 0; + out_6443071812737183938[2] = 0; + out_6443071812737183938[3] = 0; + out_6443071812737183938[4] = 0; + out_6443071812737183938[5] = 0; + out_6443071812737183938[6] = 1; + out_6443071812737183938[7] = 0; + out_6443071812737183938[8] = 0; + out_6443071812737183938[9] = 1; + out_6443071812737183938[10] = 0; + out_6443071812737183938[11] = 0; + out_6443071812737183938[12] = 0; + out_6443071812737183938[13] = 0; + out_6443071812737183938[14] = 0; + out_6443071812737183938[15] = 0; + out_6443071812737183938[16] = 0; + out_6443071812737183938[17] = 0; + out_6443071812737183938[18] = 0; + out_6443071812737183938[19] = 0; + out_6443071812737183938[20] = 0; + out_6443071812737183938[21] = 0; + out_6443071812737183938[22] = 0; + out_6443071812737183938[23] = 0; + out_6443071812737183938[24] = 0; + out_6443071812737183938[25] = 1; + out_6443071812737183938[26] = 0; + out_6443071812737183938[27] = 0; + out_6443071812737183938[28] = 1; + out_6443071812737183938[29] = 0; + out_6443071812737183938[30] = 0; + out_6443071812737183938[31] = 0; + out_6443071812737183938[32] = 0; + out_6443071812737183938[33] = 0; + out_6443071812737183938[34] = 0; + out_6443071812737183938[35] = 0; + out_6443071812737183938[36] = 0; + out_6443071812737183938[37] = 0; + out_6443071812737183938[38] = 0; + out_6443071812737183938[39] = 0; + out_6443071812737183938[40] = 0; + out_6443071812737183938[41] = 0; + out_6443071812737183938[42] = 0; + out_6443071812737183938[43] = 0; + out_6443071812737183938[44] = 1; + out_6443071812737183938[45] = 0; + out_6443071812737183938[46] = 0; + out_6443071812737183938[47] = 1; + out_6443071812737183938[48] = 0; + out_6443071812737183938[49] = 0; + out_6443071812737183938[50] = 0; + out_6443071812737183938[51] = 0; + out_6443071812737183938[52] = 0; + out_6443071812737183938[53] = 0; } -void h_10(double *state, double *unused, double *out_7661539883733460450) { - out_7661539883733460450[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_7661539883733460450[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_7661539883733460450[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_2021668210924663063) { + out_2021668210924663063[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_2021668210924663063[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_2021668210924663063[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_2752907549792722588) { - out_2752907549792722588[0] = 0; - out_2752907549792722588[1] = 9.8100000000000005*cos(state[1]); - out_2752907549792722588[2] = 0; - out_2752907549792722588[3] = 0; - out_2752907549792722588[4] = -state[8]; - out_2752907549792722588[5] = state[7]; - out_2752907549792722588[6] = 0; - out_2752907549792722588[7] = state[5]; - out_2752907549792722588[8] = -state[4]; - out_2752907549792722588[9] = 0; - out_2752907549792722588[10] = 0; - out_2752907549792722588[11] = 0; - out_2752907549792722588[12] = 1; - out_2752907549792722588[13] = 0; - out_2752907549792722588[14] = 0; - out_2752907549792722588[15] = 1; - out_2752907549792722588[16] = 0; - out_2752907549792722588[17] = 0; - out_2752907549792722588[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_2752907549792722588[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_2752907549792722588[20] = 0; - out_2752907549792722588[21] = state[8]; - out_2752907549792722588[22] = 0; - out_2752907549792722588[23] = -state[6]; - out_2752907549792722588[24] = -state[5]; - out_2752907549792722588[25] = 0; - out_2752907549792722588[26] = state[3]; - out_2752907549792722588[27] = 0; - out_2752907549792722588[28] = 0; - out_2752907549792722588[29] = 0; - out_2752907549792722588[30] = 0; - out_2752907549792722588[31] = 1; - out_2752907549792722588[32] = 0; - out_2752907549792722588[33] = 0; - out_2752907549792722588[34] = 1; - out_2752907549792722588[35] = 0; - out_2752907549792722588[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_2752907549792722588[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_2752907549792722588[38] = 0; - out_2752907549792722588[39] = -state[7]; - out_2752907549792722588[40] = state[6]; - out_2752907549792722588[41] = 0; - out_2752907549792722588[42] = state[4]; - out_2752907549792722588[43] = -state[3]; - out_2752907549792722588[44] = 0; - out_2752907549792722588[45] = 0; - out_2752907549792722588[46] = 0; - out_2752907549792722588[47] = 0; - out_2752907549792722588[48] = 0; - out_2752907549792722588[49] = 0; - out_2752907549792722588[50] = 1; - out_2752907549792722588[51] = 0; - out_2752907549792722588[52] = 0; - out_2752907549792722588[53] = 1; +void H_10(double *state, double *unused, double *out_7830124823924821015) { + out_7830124823924821015[0] = 0; + out_7830124823924821015[1] = 9.8100000000000005*cos(state[1]); + out_7830124823924821015[2] = 0; + out_7830124823924821015[3] = 0; + out_7830124823924821015[4] = -state[8]; + out_7830124823924821015[5] = state[7]; + out_7830124823924821015[6] = 0; + out_7830124823924821015[7] = state[5]; + out_7830124823924821015[8] = -state[4]; + out_7830124823924821015[9] = 0; + out_7830124823924821015[10] = 0; + out_7830124823924821015[11] = 0; + out_7830124823924821015[12] = 1; + out_7830124823924821015[13] = 0; + out_7830124823924821015[14] = 0; + out_7830124823924821015[15] = 1; + out_7830124823924821015[16] = 0; + out_7830124823924821015[17] = 0; + out_7830124823924821015[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_7830124823924821015[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_7830124823924821015[20] = 0; + out_7830124823924821015[21] = state[8]; + out_7830124823924821015[22] = 0; + out_7830124823924821015[23] = -state[6]; + out_7830124823924821015[24] = -state[5]; + out_7830124823924821015[25] = 0; + out_7830124823924821015[26] = state[3]; + out_7830124823924821015[27] = 0; + out_7830124823924821015[28] = 0; + out_7830124823924821015[29] = 0; + out_7830124823924821015[30] = 0; + out_7830124823924821015[31] = 1; + out_7830124823924821015[32] = 0; + out_7830124823924821015[33] = 0; + out_7830124823924821015[34] = 1; + out_7830124823924821015[35] = 0; + out_7830124823924821015[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_7830124823924821015[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_7830124823924821015[38] = 0; + out_7830124823924821015[39] = -state[7]; + out_7830124823924821015[40] = state[6]; + out_7830124823924821015[41] = 0; + out_7830124823924821015[42] = state[4]; + out_7830124823924821015[43] = -state[3]; + out_7830124823924821015[44] = 0; + out_7830124823924821015[45] = 0; + out_7830124823924821015[46] = 0; + out_7830124823924821015[47] = 0; + out_7830124823924821015[48] = 0; + out_7830124823924821015[49] = 0; + out_7830124823924821015[50] = 1; + out_7830124823924821015[51] = 0; + out_7830124823924821015[52] = 0; + out_7830124823924821015[53] = 1; } -void h_13(double *state, double *unused, double *out_4247381704481051963) { - out_4247381704481051963[0] = state[3]; - out_4247381704481051963[1] = state[4]; - out_4247381704481051963[2] = state[5]; +void h_13(double *state, double *unused, double *out_7135402855206141384) { + out_7135402855206141384[0] = state[3]; + out_7135402855206141384[1] = state[4]; + out_7135402855206141384[2] = state[5]; } -void H_13(double *state, double *unused, double *out_1791023673905199904) { - out_1791023673905199904[0] = 0; - out_1791023673905199904[1] = 0; - out_1791023673905199904[2] = 0; - out_1791023673905199904[3] = 1; - out_1791023673905199904[4] = 0; - out_1791023673905199904[5] = 0; - out_1791023673905199904[6] = 0; - out_1791023673905199904[7] = 0; - out_1791023673905199904[8] = 0; - out_1791023673905199904[9] = 0; - out_1791023673905199904[10] = 0; - out_1791023673905199904[11] = 0; - out_1791023673905199904[12] = 0; - out_1791023673905199904[13] = 0; - out_1791023673905199904[14] = 0; - out_1791023673905199904[15] = 0; - out_1791023673905199904[16] = 0; - out_1791023673905199904[17] = 0; - out_1791023673905199904[18] = 0; - out_1791023673905199904[19] = 0; - out_1791023673905199904[20] = 0; - out_1791023673905199904[21] = 0; - out_1791023673905199904[22] = 1; - out_1791023673905199904[23] = 0; - out_1791023673905199904[24] = 0; - out_1791023673905199904[25] = 0; - out_1791023673905199904[26] = 0; - out_1791023673905199904[27] = 0; - out_1791023673905199904[28] = 0; - out_1791023673905199904[29] = 0; - out_1791023673905199904[30] = 0; - out_1791023673905199904[31] = 0; - out_1791023673905199904[32] = 0; - out_1791023673905199904[33] = 0; - out_1791023673905199904[34] = 0; - out_1791023673905199904[35] = 0; - out_1791023673905199904[36] = 0; - out_1791023673905199904[37] = 0; - out_1791023673905199904[38] = 0; - out_1791023673905199904[39] = 0; - out_1791023673905199904[40] = 0; - out_1791023673905199904[41] = 1; - out_1791023673905199904[42] = 0; - out_1791023673905199904[43] = 0; - out_1791023673905199904[44] = 0; - out_1791023673905199904[45] = 0; - out_1791023673905199904[46] = 0; - out_1791023673905199904[47] = 0; - out_1791023673905199904[48] = 0; - out_1791023673905199904[49] = 0; - out_1791023673905199904[50] = 0; - out_1791023673905199904[51] = 0; - out_1791023673905199904[52] = 0; - out_1791023673905199904[53] = 0; +void H_13(double *state, double *unused, double *out_8791398435640034877) { + out_8791398435640034877[0] = 0; + out_8791398435640034877[1] = 0; + out_8791398435640034877[2] = 0; + out_8791398435640034877[3] = 1; + out_8791398435640034877[4] = 0; + out_8791398435640034877[5] = 0; + out_8791398435640034877[6] = 0; + out_8791398435640034877[7] = 0; + out_8791398435640034877[8] = 0; + out_8791398435640034877[9] = 0; + out_8791398435640034877[10] = 0; + out_8791398435640034877[11] = 0; + out_8791398435640034877[12] = 0; + out_8791398435640034877[13] = 0; + out_8791398435640034877[14] = 0; + out_8791398435640034877[15] = 0; + out_8791398435640034877[16] = 0; + out_8791398435640034877[17] = 0; + out_8791398435640034877[18] = 0; + out_8791398435640034877[19] = 0; + out_8791398435640034877[20] = 0; + out_8791398435640034877[21] = 0; + out_8791398435640034877[22] = 1; + out_8791398435640034877[23] = 0; + out_8791398435640034877[24] = 0; + out_8791398435640034877[25] = 0; + out_8791398435640034877[26] = 0; + out_8791398435640034877[27] = 0; + out_8791398435640034877[28] = 0; + out_8791398435640034877[29] = 0; + out_8791398435640034877[30] = 0; + out_8791398435640034877[31] = 0; + out_8791398435640034877[32] = 0; + out_8791398435640034877[33] = 0; + out_8791398435640034877[34] = 0; + out_8791398435640034877[35] = 0; + out_8791398435640034877[36] = 0; + out_8791398435640034877[37] = 0; + out_8791398435640034877[38] = 0; + out_8791398435640034877[39] = 0; + out_8791398435640034877[40] = 0; + out_8791398435640034877[41] = 1; + out_8791398435640034877[42] = 0; + out_8791398435640034877[43] = 0; + out_8791398435640034877[44] = 0; + out_8791398435640034877[45] = 0; + out_8791398435640034877[46] = 0; + out_8791398435640034877[47] = 0; + out_8791398435640034877[48] = 0; + out_8791398435640034877[49] = 0; + out_8791398435640034877[50] = 0; + out_8791398435640034877[51] = 0; + out_8791398435640034877[52] = 0; + out_8791398435640034877[53] = 0; } -void h_14(double *state, double *unused, double *out_7786063953392294783) { - out_7786063953392294783[0] = state[6]; - out_7786063953392294783[1] = state[7]; - out_7786063953392294783[2] = state[8]; +void h_14(double *state, double *unused, double *out_7043440464612625298) { + out_7043440464612625298[0] = state[6]; + out_7043440464612625298[1] = state[7]; + out_7043440464612625298[2] = state[8]; } -void H_14(double *state, double *unused, double *out_5438414025882416304) { - out_5438414025882416304[0] = 0; - out_5438414025882416304[1] = 0; - out_5438414025882416304[2] = 0; - out_5438414025882416304[3] = 0; - out_5438414025882416304[4] = 0; - out_5438414025882416304[5] = 0; - out_5438414025882416304[6] = 1; - out_5438414025882416304[7] = 0; - out_5438414025882416304[8] = 0; - out_5438414025882416304[9] = 0; - out_5438414025882416304[10] = 0; - out_5438414025882416304[11] = 0; - out_5438414025882416304[12] = 0; - out_5438414025882416304[13] = 0; - out_5438414025882416304[14] = 0; - out_5438414025882416304[15] = 0; - out_5438414025882416304[16] = 0; - out_5438414025882416304[17] = 0; - out_5438414025882416304[18] = 0; - out_5438414025882416304[19] = 0; - out_5438414025882416304[20] = 0; - out_5438414025882416304[21] = 0; - out_5438414025882416304[22] = 0; - out_5438414025882416304[23] = 0; - out_5438414025882416304[24] = 0; - out_5438414025882416304[25] = 1; - out_5438414025882416304[26] = 0; - out_5438414025882416304[27] = 0; - out_5438414025882416304[28] = 0; - out_5438414025882416304[29] = 0; - out_5438414025882416304[30] = 0; - out_5438414025882416304[31] = 0; - out_5438414025882416304[32] = 0; - out_5438414025882416304[33] = 0; - out_5438414025882416304[34] = 0; - out_5438414025882416304[35] = 0; - out_5438414025882416304[36] = 0; - out_5438414025882416304[37] = 0; - out_5438414025882416304[38] = 0; - out_5438414025882416304[39] = 0; - out_5438414025882416304[40] = 0; - out_5438414025882416304[41] = 0; - out_5438414025882416304[42] = 0; - out_5438414025882416304[43] = 0; - out_5438414025882416304[44] = 1; - out_5438414025882416304[45] = 0; - out_5438414025882416304[46] = 0; - out_5438414025882416304[47] = 0; - out_5438414025882416304[48] = 0; - out_5438414025882416304[49] = 0; - out_5438414025882416304[50] = 0; - out_5438414025882416304[51] = 0; - out_5438414025882416304[52] = 0; - out_5438414025882416304[53] = 0; +void H_14(double *state, double *unused, double *out_3360283380441811642) { + out_3360283380441811642[0] = 0; + out_3360283380441811642[1] = 0; + out_3360283380441811642[2] = 0; + out_3360283380441811642[3] = 0; + out_3360283380441811642[4] = 0; + out_3360283380441811642[5] = 0; + out_3360283380441811642[6] = 1; + out_3360283380441811642[7] = 0; + out_3360283380441811642[8] = 0; + out_3360283380441811642[9] = 0; + out_3360283380441811642[10] = 0; + out_3360283380441811642[11] = 0; + out_3360283380441811642[12] = 0; + out_3360283380441811642[13] = 0; + out_3360283380441811642[14] = 0; + out_3360283380441811642[15] = 0; + out_3360283380441811642[16] = 0; + out_3360283380441811642[17] = 0; + out_3360283380441811642[18] = 0; + out_3360283380441811642[19] = 0; + out_3360283380441811642[20] = 0; + out_3360283380441811642[21] = 0; + out_3360283380441811642[22] = 0; + out_3360283380441811642[23] = 0; + out_3360283380441811642[24] = 0; + out_3360283380441811642[25] = 1; + out_3360283380441811642[26] = 0; + out_3360283380441811642[27] = 0; + out_3360283380441811642[28] = 0; + out_3360283380441811642[29] = 0; + out_3360283380441811642[30] = 0; + out_3360283380441811642[31] = 0; + out_3360283380441811642[32] = 0; + out_3360283380441811642[33] = 0; + out_3360283380441811642[34] = 0; + out_3360283380441811642[35] = 0; + out_3360283380441811642[36] = 0; + out_3360283380441811642[37] = 0; + out_3360283380441811642[38] = 0; + out_3360283380441811642[39] = 0; + out_3360283380441811642[40] = 0; + out_3360283380441811642[41] = 0; + out_3360283380441811642[42] = 0; + out_3360283380441811642[43] = 0; + out_3360283380441811642[44] = 1; + out_3360283380441811642[45] = 0; + out_3360283380441811642[46] = 0; + out_3360283380441811642[47] = 0; + out_3360283380441811642[48] = 0; + out_3360283380441811642[49] = 0; + out_3360283380441811642[50] = 0; + out_3360283380441811642[51] = 0; + out_3360283380441811642[52] = 0; + out_3360283380441811642[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_3328634486354213070) { - err_fun(nom_x, delta_x, out_3328634486354213070); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8940459122146932400) { + err_fun(nom_x, delta_x, out_8940459122146932400); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2068367058924519839) { - inv_err_fun(nom_x, true_x, out_2068367058924519839); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3384987028755125218) { + inv_err_fun(nom_x, true_x, out_3384987028755125218); } -void pose_H_mod_fun(double *state, double *out_5273281981643743211) { - H_mod_fun(state, out_5273281981643743211); +void pose_H_mod_fun(double *state, double *out_5245124330854285834) { + H_mod_fun(state, out_5245124330854285834); } -void pose_f_fun(double *state, double dt, double *out_3798976948542527989) { - f_fun(state, dt, out_3798976948542527989); +void pose_f_fun(double *state, double dt, double *out_6830900184565569763) { + f_fun(state, dt, out_6830900184565569763); } -void pose_F_fun(double *state, double dt, double *out_5376087696404799721) { - F_fun(state, dt, out_5376087696404799721); +void pose_F_fun(double *state, double dt, double *out_5118094281500581129) { + F_fun(state, dt, out_5118094281500581129); } -void pose_h_4(double *state, double *unused, double *out_6100547545538860384) { - h_4(state, unused, out_6100547545538860384); +void pose_h_4(double *state, double *unused, double *out_4935070827926002458) { + h_4(state, unused, out_4935070827926002458); } -void pose_H_4(double *state, double *unused, double *out_9045089191487650783) { - H_4(state, unused, out_9045089191487650783); +void pose_H_4(double *state, double *unused, double *out_6443071812737183938) { + H_4(state, unused, out_6443071812737183938); } -void pose_h_10(double *state, double *unused, double *out_7661539883733460450) { - h_10(state, unused, out_7661539883733460450); +void pose_h_10(double *state, double *unused, double *out_2021668210924663063) { + h_10(state, unused, out_2021668210924663063); } -void pose_H_10(double *state, double *unused, double *out_2752907549792722588) { - H_10(state, unused, out_2752907549792722588); +void pose_H_10(double *state, double *unused, double *out_7830124823924821015) { + H_10(state, unused, out_7830124823924821015); } -void pose_h_13(double *state, double *unused, double *out_4247381704481051963) { - h_13(state, unused, out_4247381704481051963); +void pose_h_13(double *state, double *unused, double *out_7135402855206141384) { + h_13(state, unused, out_7135402855206141384); } -void pose_H_13(double *state, double *unused, double *out_1791023673905199904) { - H_13(state, unused, out_1791023673905199904); +void pose_H_13(double *state, double *unused, double *out_8791398435640034877) { + H_13(state, unused, out_8791398435640034877); } -void pose_h_14(double *state, double *unused, double *out_7786063953392294783) { - h_14(state, unused, out_7786063953392294783); +void pose_h_14(double *state, double *unused, double *out_7043440464612625298) { + h_14(state, unused, out_7043440464612625298); } -void pose_H_14(double *state, double *unused, double *out_5438414025882416304) { - H_14(state, unused, out_5438414025882416304); +void pose_H_14(double *state, double *unused, double *out_3360283380441811642) { + H_14(state, unused, out_3360283380441811642); } 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 071312037..ee9159d12 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_3328634486354213070); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2068367058924519839); -void pose_H_mod_fun(double *state, double *out_5273281981643743211); -void pose_f_fun(double *state, double dt, double *out_3798976948542527989); -void pose_F_fun(double *state, double dt, double *out_5376087696404799721); -void pose_h_4(double *state, double *unused, double *out_6100547545538860384); -void pose_H_4(double *state, double *unused, double *out_9045089191487650783); -void pose_h_10(double *state, double *unused, double *out_7661539883733460450); -void pose_H_10(double *state, double *unused, double *out_2752907549792722588); -void pose_h_13(double *state, double *unused, double *out_4247381704481051963); -void pose_H_13(double *state, double *unused, double *out_1791023673905199904); -void pose_h_14(double *state, double *unused, double *out_7786063953392294783); -void pose_H_14(double *state, double *unused, double *out_5438414025882416304); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8940459122146932400); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3384987028755125218); +void pose_H_mod_fun(double *state, double *out_5245124330854285834); +void pose_f_fun(double *state, double dt, double *out_6830900184565569763); +void pose_F_fun(double *state, double dt, double *out_5118094281500581129); +void pose_h_4(double *state, double *unused, double *out_4935070827926002458); +void pose_H_4(double *state, double *unused, double *out_6443071812737183938); +void pose_h_10(double *state, double *unused, double *out_2021668210924663063); +void pose_H_10(double *state, double *unused, double *out_7830124823924821015); +void pose_h_13(double *state, double *unused, double *out_7135402855206141384); +void pose_H_13(double *state, double *unused, double *out_8791398435640034877); +void pose_h_14(double *state, double *unused, double *out_7043440464612625298); +void pose_H_14(double *state, double *unused, double *out_3360283380441811642); 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 d4416fe65..49e849592 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ