diff --git a/common/libcommon.a b/common/libcommon.a index e96bbf07e..a6dca1234 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index f8206f032..b5e8953ee 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index ba8586098..b505bf53e 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 60704c763..de58de734 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 7b942223f..ea939aae9 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 6786f753f..94e12c3a2 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 95b025624..6197fd071 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 0d70c7829..97bf5b8d5 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 16d2c03d6..ce3f8169f 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 16d2c03d6..ce3f8169f 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 620ff08e3..a8157dace 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 0d70c7829..97bf5b8d5 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 0e678651d..287412014 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-bec246c6-DEBUG"; +const uint8_t gitversion[19] = "DEV-cf1c99cf-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index e753bd02d..8a62c12d8 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 1f9b8029d..912996ee2 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 d4dc20dde..221562edb 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 951965022..9b2bd61be 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 d745af951..ffbf53155 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 38df0060b..c6e209a97 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 765776fc7..f365adca4 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 59d7ab3a8..b191170a9 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 03dacca6d..8a2a01ec2 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 22e68c15d..81254563c 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 f0e7cd7ba..38b80994a 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 1cae10572..6a5f1f31b 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 76b2edd5f..e5426fbc7 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 cb2de0438..10af1b047 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 a30f1386d..935a481db 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 99f852b3c..026ab6bac 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 841d0a482..7346f7356 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 cedacfc0b..9216d7fc1 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 a420dab7f..a37232ec0 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 2a19fc20a..a0d68aa4d 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 3d1b5933c..1b8335a7e 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-bec246c6-DEBUG \ No newline at end of file +DEV-cf1c99cf-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 24e5ef9f8..485df0582 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_3499577995962015470) { - out_3499577995962015470[0] = delta_x[0] + nom_x[0]; - out_3499577995962015470[1] = delta_x[1] + nom_x[1]; - out_3499577995962015470[2] = delta_x[2] + nom_x[2]; - out_3499577995962015470[3] = delta_x[3] + nom_x[3]; - out_3499577995962015470[4] = delta_x[4] + nom_x[4]; - out_3499577995962015470[5] = delta_x[5] + nom_x[5]; - out_3499577995962015470[6] = delta_x[6] + nom_x[6]; - out_3499577995962015470[7] = delta_x[7] + nom_x[7]; - out_3499577995962015470[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_5319131797443970272) { + out_5319131797443970272[0] = delta_x[0] + nom_x[0]; + out_5319131797443970272[1] = delta_x[1] + nom_x[1]; + out_5319131797443970272[2] = delta_x[2] + nom_x[2]; + out_5319131797443970272[3] = delta_x[3] + nom_x[3]; + out_5319131797443970272[4] = delta_x[4] + nom_x[4]; + out_5319131797443970272[5] = delta_x[5] + nom_x[5]; + out_5319131797443970272[6] = delta_x[6] + nom_x[6]; + out_5319131797443970272[7] = delta_x[7] + nom_x[7]; + out_5319131797443970272[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7704661140039100406) { - out_7704661140039100406[0] = -nom_x[0] + true_x[0]; - out_7704661140039100406[1] = -nom_x[1] + true_x[1]; - out_7704661140039100406[2] = -nom_x[2] + true_x[2]; - out_7704661140039100406[3] = -nom_x[3] + true_x[3]; - out_7704661140039100406[4] = -nom_x[4] + true_x[4]; - out_7704661140039100406[5] = -nom_x[5] + true_x[5]; - out_7704661140039100406[6] = -nom_x[6] + true_x[6]; - out_7704661140039100406[7] = -nom_x[7] + true_x[7]; - out_7704661140039100406[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3644993322176215676) { + out_3644993322176215676[0] = -nom_x[0] + true_x[0]; + out_3644993322176215676[1] = -nom_x[1] + true_x[1]; + out_3644993322176215676[2] = -nom_x[2] + true_x[2]; + out_3644993322176215676[3] = -nom_x[3] + true_x[3]; + out_3644993322176215676[4] = -nom_x[4] + true_x[4]; + out_3644993322176215676[5] = -nom_x[5] + true_x[5]; + out_3644993322176215676[6] = -nom_x[6] + true_x[6]; + out_3644993322176215676[7] = -nom_x[7] + true_x[7]; + out_3644993322176215676[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_3593592511346668572) { - out_3593592511346668572[0] = 1.0; - out_3593592511346668572[1] = 0.0; - out_3593592511346668572[2] = 0.0; - out_3593592511346668572[3] = 0.0; - out_3593592511346668572[4] = 0.0; - out_3593592511346668572[5] = 0.0; - out_3593592511346668572[6] = 0.0; - out_3593592511346668572[7] = 0.0; - out_3593592511346668572[8] = 0.0; - out_3593592511346668572[9] = 0.0; - out_3593592511346668572[10] = 1.0; - out_3593592511346668572[11] = 0.0; - out_3593592511346668572[12] = 0.0; - out_3593592511346668572[13] = 0.0; - out_3593592511346668572[14] = 0.0; - out_3593592511346668572[15] = 0.0; - out_3593592511346668572[16] = 0.0; - out_3593592511346668572[17] = 0.0; - out_3593592511346668572[18] = 0.0; - out_3593592511346668572[19] = 0.0; - out_3593592511346668572[20] = 1.0; - out_3593592511346668572[21] = 0.0; - out_3593592511346668572[22] = 0.0; - out_3593592511346668572[23] = 0.0; - out_3593592511346668572[24] = 0.0; - out_3593592511346668572[25] = 0.0; - out_3593592511346668572[26] = 0.0; - out_3593592511346668572[27] = 0.0; - out_3593592511346668572[28] = 0.0; - out_3593592511346668572[29] = 0.0; - out_3593592511346668572[30] = 1.0; - out_3593592511346668572[31] = 0.0; - out_3593592511346668572[32] = 0.0; - out_3593592511346668572[33] = 0.0; - out_3593592511346668572[34] = 0.0; - out_3593592511346668572[35] = 0.0; - out_3593592511346668572[36] = 0.0; - out_3593592511346668572[37] = 0.0; - out_3593592511346668572[38] = 0.0; - out_3593592511346668572[39] = 0.0; - out_3593592511346668572[40] = 1.0; - out_3593592511346668572[41] = 0.0; - out_3593592511346668572[42] = 0.0; - out_3593592511346668572[43] = 0.0; - out_3593592511346668572[44] = 0.0; - out_3593592511346668572[45] = 0.0; - out_3593592511346668572[46] = 0.0; - out_3593592511346668572[47] = 0.0; - out_3593592511346668572[48] = 0.0; - out_3593592511346668572[49] = 0.0; - out_3593592511346668572[50] = 1.0; - out_3593592511346668572[51] = 0.0; - out_3593592511346668572[52] = 0.0; - out_3593592511346668572[53] = 0.0; - out_3593592511346668572[54] = 0.0; - out_3593592511346668572[55] = 0.0; - out_3593592511346668572[56] = 0.0; - out_3593592511346668572[57] = 0.0; - out_3593592511346668572[58] = 0.0; - out_3593592511346668572[59] = 0.0; - out_3593592511346668572[60] = 1.0; - out_3593592511346668572[61] = 0.0; - out_3593592511346668572[62] = 0.0; - out_3593592511346668572[63] = 0.0; - out_3593592511346668572[64] = 0.0; - out_3593592511346668572[65] = 0.0; - out_3593592511346668572[66] = 0.0; - out_3593592511346668572[67] = 0.0; - out_3593592511346668572[68] = 0.0; - out_3593592511346668572[69] = 0.0; - out_3593592511346668572[70] = 1.0; - out_3593592511346668572[71] = 0.0; - out_3593592511346668572[72] = 0.0; - out_3593592511346668572[73] = 0.0; - out_3593592511346668572[74] = 0.0; - out_3593592511346668572[75] = 0.0; - out_3593592511346668572[76] = 0.0; - out_3593592511346668572[77] = 0.0; - out_3593592511346668572[78] = 0.0; - out_3593592511346668572[79] = 0.0; - out_3593592511346668572[80] = 1.0; +void H_mod_fun(double *state, double *out_3163511227028018782) { + out_3163511227028018782[0] = 1.0; + out_3163511227028018782[1] = 0.0; + out_3163511227028018782[2] = 0.0; + out_3163511227028018782[3] = 0.0; + out_3163511227028018782[4] = 0.0; + out_3163511227028018782[5] = 0.0; + out_3163511227028018782[6] = 0.0; + out_3163511227028018782[7] = 0.0; + out_3163511227028018782[8] = 0.0; + out_3163511227028018782[9] = 0.0; + out_3163511227028018782[10] = 1.0; + out_3163511227028018782[11] = 0.0; + out_3163511227028018782[12] = 0.0; + out_3163511227028018782[13] = 0.0; + out_3163511227028018782[14] = 0.0; + out_3163511227028018782[15] = 0.0; + out_3163511227028018782[16] = 0.0; + out_3163511227028018782[17] = 0.0; + out_3163511227028018782[18] = 0.0; + out_3163511227028018782[19] = 0.0; + out_3163511227028018782[20] = 1.0; + out_3163511227028018782[21] = 0.0; + out_3163511227028018782[22] = 0.0; + out_3163511227028018782[23] = 0.0; + out_3163511227028018782[24] = 0.0; + out_3163511227028018782[25] = 0.0; + out_3163511227028018782[26] = 0.0; + out_3163511227028018782[27] = 0.0; + out_3163511227028018782[28] = 0.0; + out_3163511227028018782[29] = 0.0; + out_3163511227028018782[30] = 1.0; + out_3163511227028018782[31] = 0.0; + out_3163511227028018782[32] = 0.0; + out_3163511227028018782[33] = 0.0; + out_3163511227028018782[34] = 0.0; + out_3163511227028018782[35] = 0.0; + out_3163511227028018782[36] = 0.0; + out_3163511227028018782[37] = 0.0; + out_3163511227028018782[38] = 0.0; + out_3163511227028018782[39] = 0.0; + out_3163511227028018782[40] = 1.0; + out_3163511227028018782[41] = 0.0; + out_3163511227028018782[42] = 0.0; + out_3163511227028018782[43] = 0.0; + out_3163511227028018782[44] = 0.0; + out_3163511227028018782[45] = 0.0; + out_3163511227028018782[46] = 0.0; + out_3163511227028018782[47] = 0.0; + out_3163511227028018782[48] = 0.0; + out_3163511227028018782[49] = 0.0; + out_3163511227028018782[50] = 1.0; + out_3163511227028018782[51] = 0.0; + out_3163511227028018782[52] = 0.0; + out_3163511227028018782[53] = 0.0; + out_3163511227028018782[54] = 0.0; + out_3163511227028018782[55] = 0.0; + out_3163511227028018782[56] = 0.0; + out_3163511227028018782[57] = 0.0; + out_3163511227028018782[58] = 0.0; + out_3163511227028018782[59] = 0.0; + out_3163511227028018782[60] = 1.0; + out_3163511227028018782[61] = 0.0; + out_3163511227028018782[62] = 0.0; + out_3163511227028018782[63] = 0.0; + out_3163511227028018782[64] = 0.0; + out_3163511227028018782[65] = 0.0; + out_3163511227028018782[66] = 0.0; + out_3163511227028018782[67] = 0.0; + out_3163511227028018782[68] = 0.0; + out_3163511227028018782[69] = 0.0; + out_3163511227028018782[70] = 1.0; + out_3163511227028018782[71] = 0.0; + out_3163511227028018782[72] = 0.0; + out_3163511227028018782[73] = 0.0; + out_3163511227028018782[74] = 0.0; + out_3163511227028018782[75] = 0.0; + out_3163511227028018782[76] = 0.0; + out_3163511227028018782[77] = 0.0; + out_3163511227028018782[78] = 0.0; + out_3163511227028018782[79] = 0.0; + out_3163511227028018782[80] = 1.0; } -void f_fun(double *state, double dt, double *out_5138495986546656101) { - out_5138495986546656101[0] = state[0]; - out_5138495986546656101[1] = state[1]; - out_5138495986546656101[2] = state[2]; - out_5138495986546656101[3] = state[3]; - out_5138495986546656101[4] = state[4]; - out_5138495986546656101[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_5138495986546656101[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_5138495986546656101[7] = state[7]; - out_5138495986546656101[8] = state[8]; +void f_fun(double *state, double dt, double *out_1546760495327089109) { + out_1546760495327089109[0] = state[0]; + out_1546760495327089109[1] = state[1]; + out_1546760495327089109[2] = state[2]; + out_1546760495327089109[3] = state[3]; + out_1546760495327089109[4] = state[4]; + out_1546760495327089109[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_1546760495327089109[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_1546760495327089109[7] = state[7]; + out_1546760495327089109[8] = state[8]; } -void F_fun(double *state, double dt, double *out_9094696385837044219) { - out_9094696385837044219[0] = 1; - out_9094696385837044219[1] = 0; - out_9094696385837044219[2] = 0; - out_9094696385837044219[3] = 0; - out_9094696385837044219[4] = 0; - out_9094696385837044219[5] = 0; - out_9094696385837044219[6] = 0; - out_9094696385837044219[7] = 0; - out_9094696385837044219[8] = 0; - out_9094696385837044219[9] = 0; - out_9094696385837044219[10] = 1; - out_9094696385837044219[11] = 0; - out_9094696385837044219[12] = 0; - out_9094696385837044219[13] = 0; - out_9094696385837044219[14] = 0; - out_9094696385837044219[15] = 0; - out_9094696385837044219[16] = 0; - out_9094696385837044219[17] = 0; - out_9094696385837044219[18] = 0; - out_9094696385837044219[19] = 0; - out_9094696385837044219[20] = 1; - out_9094696385837044219[21] = 0; - out_9094696385837044219[22] = 0; - out_9094696385837044219[23] = 0; - out_9094696385837044219[24] = 0; - out_9094696385837044219[25] = 0; - out_9094696385837044219[26] = 0; - out_9094696385837044219[27] = 0; - out_9094696385837044219[28] = 0; - out_9094696385837044219[29] = 0; - out_9094696385837044219[30] = 1; - out_9094696385837044219[31] = 0; - out_9094696385837044219[32] = 0; - out_9094696385837044219[33] = 0; - out_9094696385837044219[34] = 0; - out_9094696385837044219[35] = 0; - out_9094696385837044219[36] = 0; - out_9094696385837044219[37] = 0; - out_9094696385837044219[38] = 0; - out_9094696385837044219[39] = 0; - out_9094696385837044219[40] = 1; - out_9094696385837044219[41] = 0; - out_9094696385837044219[42] = 0; - out_9094696385837044219[43] = 0; - out_9094696385837044219[44] = 0; - out_9094696385837044219[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_9094696385837044219[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_9094696385837044219[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_9094696385837044219[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_9094696385837044219[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_9094696385837044219[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_9094696385837044219[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_9094696385837044219[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_9094696385837044219[53] = -9.8100000000000005*dt; - out_9094696385837044219[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_9094696385837044219[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_9094696385837044219[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_9094696385837044219[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_9094696385837044219[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_9094696385837044219[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_9094696385837044219[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_9094696385837044219[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_9094696385837044219[62] = 0; - out_9094696385837044219[63] = 0; - out_9094696385837044219[64] = 0; - out_9094696385837044219[65] = 0; - out_9094696385837044219[66] = 0; - out_9094696385837044219[67] = 0; - out_9094696385837044219[68] = 0; - out_9094696385837044219[69] = 0; - out_9094696385837044219[70] = 1; - out_9094696385837044219[71] = 0; - out_9094696385837044219[72] = 0; - out_9094696385837044219[73] = 0; - out_9094696385837044219[74] = 0; - out_9094696385837044219[75] = 0; - out_9094696385837044219[76] = 0; - out_9094696385837044219[77] = 0; - out_9094696385837044219[78] = 0; - out_9094696385837044219[79] = 0; - out_9094696385837044219[80] = 1; +void F_fun(double *state, double dt, double *out_3348391828080031822) { + out_3348391828080031822[0] = 1; + out_3348391828080031822[1] = 0; + out_3348391828080031822[2] = 0; + out_3348391828080031822[3] = 0; + out_3348391828080031822[4] = 0; + out_3348391828080031822[5] = 0; + out_3348391828080031822[6] = 0; + out_3348391828080031822[7] = 0; + out_3348391828080031822[8] = 0; + out_3348391828080031822[9] = 0; + out_3348391828080031822[10] = 1; + out_3348391828080031822[11] = 0; + out_3348391828080031822[12] = 0; + out_3348391828080031822[13] = 0; + out_3348391828080031822[14] = 0; + out_3348391828080031822[15] = 0; + out_3348391828080031822[16] = 0; + out_3348391828080031822[17] = 0; + out_3348391828080031822[18] = 0; + out_3348391828080031822[19] = 0; + out_3348391828080031822[20] = 1; + out_3348391828080031822[21] = 0; + out_3348391828080031822[22] = 0; + out_3348391828080031822[23] = 0; + out_3348391828080031822[24] = 0; + out_3348391828080031822[25] = 0; + out_3348391828080031822[26] = 0; + out_3348391828080031822[27] = 0; + out_3348391828080031822[28] = 0; + out_3348391828080031822[29] = 0; + out_3348391828080031822[30] = 1; + out_3348391828080031822[31] = 0; + out_3348391828080031822[32] = 0; + out_3348391828080031822[33] = 0; + out_3348391828080031822[34] = 0; + out_3348391828080031822[35] = 0; + out_3348391828080031822[36] = 0; + out_3348391828080031822[37] = 0; + out_3348391828080031822[38] = 0; + out_3348391828080031822[39] = 0; + out_3348391828080031822[40] = 1; + out_3348391828080031822[41] = 0; + out_3348391828080031822[42] = 0; + out_3348391828080031822[43] = 0; + out_3348391828080031822[44] = 0; + out_3348391828080031822[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_3348391828080031822[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_3348391828080031822[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3348391828080031822[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3348391828080031822[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_3348391828080031822[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_3348391828080031822[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_3348391828080031822[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_3348391828080031822[53] = -9.8100000000000005*dt; + out_3348391828080031822[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_3348391828080031822[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_3348391828080031822[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3348391828080031822[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3348391828080031822[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_3348391828080031822[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_3348391828080031822[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_3348391828080031822[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3348391828080031822[62] = 0; + out_3348391828080031822[63] = 0; + out_3348391828080031822[64] = 0; + out_3348391828080031822[65] = 0; + out_3348391828080031822[66] = 0; + out_3348391828080031822[67] = 0; + out_3348391828080031822[68] = 0; + out_3348391828080031822[69] = 0; + out_3348391828080031822[70] = 1; + out_3348391828080031822[71] = 0; + out_3348391828080031822[72] = 0; + out_3348391828080031822[73] = 0; + out_3348391828080031822[74] = 0; + out_3348391828080031822[75] = 0; + out_3348391828080031822[76] = 0; + out_3348391828080031822[77] = 0; + out_3348391828080031822[78] = 0; + out_3348391828080031822[79] = 0; + out_3348391828080031822[80] = 1; } -void h_25(double *state, double *unused, double *out_1209975406708631015) { - out_1209975406708631015[0] = state[6]; +void h_25(double *state, double *unused, double *out_4471759016602712561) { + out_4471759016602712561[0] = state[6]; } -void H_25(double *state, double *unused, double *out_4098926953546965284) { - out_4098926953546965284[0] = 0; - out_4098926953546965284[1] = 0; - out_4098926953546965284[2] = 0; - out_4098926953546965284[3] = 0; - out_4098926953546965284[4] = 0; - out_4098926953546965284[5] = 0; - out_4098926953546965284[6] = 1; - out_4098926953546965284[7] = 0; - out_4098926953546965284[8] = 0; +void H_25(double *state, double *unused, double *out_1740180598156646058) { + out_1740180598156646058[0] = 0; + out_1740180598156646058[1] = 0; + out_1740180598156646058[2] = 0; + out_1740180598156646058[3] = 0; + out_1740180598156646058[4] = 0; + out_1740180598156646058[5] = 0; + out_1740180598156646058[6] = 1; + out_1740180598156646058[7] = 0; + out_1740180598156646058[8] = 0; } -void h_24(double *state, double *unused, double *out_5058461778376183297) { - out_5058461778376183297[0] = state[4]; - out_5058461778376183297[1] = state[5]; +void h_24(double *state, double *unused, double *out_3126254991737006136) { + out_3126254991737006136[0] = state[4]; + out_3126254991737006136[1] = state[5]; } -void H_24(double *state, double *unused, double *out_4943923562330660683) { - out_4943923562330660683[0] = 0; - out_4943923562330660683[1] = 0; - out_4943923562330660683[2] = 0; - out_4943923562330660683[3] = 0; - out_4943923562330660683[4] = 1; - out_4943923562330660683[5] = 0; - out_4943923562330660683[6] = 0; - out_4943923562330660683[7] = 0; - out_4943923562330660683[8] = 0; - out_4943923562330660683[9] = 0; - out_4943923562330660683[10] = 0; - out_4943923562330660683[11] = 0; - out_4943923562330660683[12] = 0; - out_4943923562330660683[13] = 0; - out_4943923562330660683[14] = 1; - out_4943923562330660683[15] = 0; - out_4943923562330660683[16] = 0; - out_4943923562330660683[17] = 0; +void H_24(double *state, double *unused, double *out_1269723116113307334) { + out_1269723116113307334[0] = 0; + out_1269723116113307334[1] = 0; + out_1269723116113307334[2] = 0; + out_1269723116113307334[3] = 0; + out_1269723116113307334[4] = 1; + out_1269723116113307334[5] = 0; + out_1269723116113307334[6] = 0; + out_1269723116113307334[7] = 0; + out_1269723116113307334[8] = 0; + out_1269723116113307334[9] = 0; + out_1269723116113307334[10] = 0; + out_1269723116113307334[11] = 0; + out_1269723116113307334[12] = 0; + out_1269723116113307334[13] = 0; + out_1269723116113307334[14] = 1; + out_1269723116113307334[15] = 0; + out_1269723116113307334[16] = 0; + out_1269723116113307334[17] = 0; } -void h_30(double *state, double *unused, double *out_5560859819641719921) { - out_5560859819641719921[0] = state[4]; +void h_30(double *state, double *unused, double *out_4168118460115829323) { + out_4168118460115829323[0] = state[4]; } -void H_30(double *state, double *unused, double *out_7431126778670969577) { - out_7431126778670969577[0] = 0; - out_7431126778670969577[1] = 0; - out_7431126778670969577[2] = 0; - out_7431126778670969577[3] = 0; - out_7431126778670969577[4] = 1; - out_7431126778670969577[5] = 0; - out_7431126778670969577[6] = 0; - out_7431126778670969577[7] = 0; - out_7431126778670969577[8] = 0; +void H_30(double *state, double *unused, double *out_1610841651013405988) { + out_1610841651013405988[0] = 0; + out_1610841651013405988[1] = 0; + out_1610841651013405988[2] = 0; + out_1610841651013405988[3] = 0; + out_1610841651013405988[4] = 1; + out_1610841651013405988[5] = 0; + out_1610841651013405988[6] = 0; + out_1610841651013405988[7] = 0; + out_1610841651013405988[8] = 0; } -void h_26(double *state, double *unused, double *out_4617349966740057486) { - out_4617349966740057486[0] = state[7]; +void h_26(double *state, double *unused, double *out_2045289894744513329) { + out_2045289894744513329[0] = state[7]; } -void H_26(double *state, double *unused, double *out_4755781017657277188) { - out_4755781017657277188[0] = 0; - out_4755781017657277188[1] = 0; - out_4755781017657277188[2] = 0; - out_4755781017657277188[3] = 0; - out_4755781017657277188[4] = 0; - out_4755781017657277188[5] = 0; - out_4755781017657277188[6] = 0; - out_4755781017657277188[7] = 1; - out_4755781017657277188[8] = 0; +void H_26(double *state, double *unused, double *out_2001322720717410166) { + out_2001322720717410166[0] = 0; + out_2001322720717410166[1] = 0; + out_2001322720717410166[2] = 0; + out_2001322720717410166[3] = 0; + out_2001322720717410166[4] = 0; + out_2001322720717410166[5] = 0; + out_2001322720717410166[6] = 0; + out_2001322720717410166[7] = 1; + out_2001322720717410166[8] = 0; } -void h_27(double *state, double *unused, double *out_6211648674369476660) { - out_6211648674369476660[0] = state[3]; +void h_27(double *state, double *unused, double *out_1927282500811136585) { + out_1927282500811136585[0] = state[3]; } -void H_27(double *state, double *unused, double *out_8840853983238157128) { - out_8840853983238157128[0] = 0; - out_8840853983238157128[1] = 0; - out_8840853983238157128[2] = 0; - out_8840853983238157128[3] = 1; - out_8840853983238157128[4] = 0; - out_8840853983238157128[5] = 0; - out_8840853983238157128[6] = 0; - out_8840853983238157128[7] = 0; - out_8840853983238157128[8] = 0; +void H_27(double *state, double *unused, double *out_563921660787018923) { + out_563921660787018923[0] = 0; + out_563921660787018923[1] = 0; + out_563921660787018923[2] = 0; + out_563921660787018923[3] = 1; + out_563921660787018923[4] = 0; + out_563921660787018923[5] = 0; + out_563921660787018923[6] = 0; + out_563921660787018923[7] = 0; + out_563921660787018923[8] = 0; } -void h_29(double *state, double *unused, double *out_559186551980874276) { - out_559186551980874276[0] = state[1]; +void h_29(double *state, double *unused, double *out_8022800891634675039) { + out_8022800891634675039[0] = state[1]; } -void H_29(double *state, double *unused, double *out_6920895434356577393) { - out_6920895434356577393[0] = 0; - out_6920895434356577393[1] = 1; - out_6920895434356577393[2] = 0; - out_6920895434356577393[3] = 0; - out_6920895434356577393[4] = 0; - out_6920895434356577393[5] = 0; - out_6920895434356577393[6] = 0; - out_6920895434356577393[7] = 0; - out_6920895434356577393[8] = 0; +void H_29(double *state, double *unused, double *out_2277284387656569956) { + out_2277284387656569956[0] = 0; + out_2277284387656569956[1] = 1; + out_2277284387656569956[2] = 0; + out_2277284387656569956[3] = 0; + out_2277284387656569956[4] = 0; + out_2277284387656569956[5] = 0; + out_2277284387656569956[6] = 0; + out_2277284387656569956[7] = 0; + out_2277284387656569956[8] = 0; } -void h_28(double *state, double *unused, double *out_7233422131679229311) { - out_7233422131679229311[0] = state[0]; +void h_28(double *state, double *unused, double *out_4964068552255469270) { + out_4964068552255469270[0] = state[0]; } -void H_28(double *state, double *unused, double *out_6443449622283443649) { - out_6443449622283443649[0] = 1; - out_6443449622283443649[1] = 0; - out_6443449622283443649[2] = 0; - out_6443449622283443649[3] = 0; - out_6443449622283443649[4] = 0; - out_6443449622283443649[5] = 0; - out_6443449622283443649[6] = 0; - out_6443449622283443649[7] = 0; - out_6443449622283443649[8] = 0; +void H_28(double *state, double *unused, double *out_313654116091243705) { + out_313654116091243705[0] = 1; + out_313654116091243705[1] = 0; + out_313654116091243705[2] = 0; + out_313654116091243705[3] = 0; + out_313654116091243705[4] = 0; + out_313654116091243705[5] = 0; + out_313654116091243705[6] = 0; + out_313654116091243705[7] = 0; + out_313654116091243705[8] = 0; } -void h_31(double *state, double *unused, double *out_8973240663529303456) { - out_8973240663529303456[0] = state[8]; +void h_31(double *state, double *unused, double *out_834309488348690211) { + out_834309488348690211[0] = state[8]; } -void H_31(double *state, double *unused, double *out_4129572915423925712) { - out_4129572915423925712[0] = 0; - out_4129572915423925712[1] = 0; - out_4129572915423925712[2] = 0; - out_4129572915423925712[3] = 0; - out_4129572915423925712[4] = 0; - out_4129572915423925712[5] = 0; - out_4129572915423925712[6] = 0; - out_4129572915423925712[7] = 0; - out_4129572915423925712[8] = 1; +void H_31(double *state, double *unused, double *out_1770826560033606486) { + out_1770826560033606486[0] = 0; + out_1770826560033606486[1] = 0; + out_1770826560033606486[2] = 0; + out_1770826560033606486[3] = 0; + out_1770826560033606486[4] = 0; + out_1770826560033606486[5] = 0; + out_1770826560033606486[6] = 0; + out_1770826560033606486[7] = 0; + out_1770826560033606486[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_3499577995962015470) { - err_fun(nom_x, delta_x, out_3499577995962015470); +void car_err_fun(double *nom_x, double *delta_x, double *out_5319131797443970272) { + err_fun(nom_x, delta_x, out_5319131797443970272); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7704661140039100406) { - inv_err_fun(nom_x, true_x, out_7704661140039100406); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3644993322176215676) { + inv_err_fun(nom_x, true_x, out_3644993322176215676); } -void car_H_mod_fun(double *state, double *out_3593592511346668572) { - H_mod_fun(state, out_3593592511346668572); +void car_H_mod_fun(double *state, double *out_3163511227028018782) { + H_mod_fun(state, out_3163511227028018782); } -void car_f_fun(double *state, double dt, double *out_5138495986546656101) { - f_fun(state, dt, out_5138495986546656101); +void car_f_fun(double *state, double dt, double *out_1546760495327089109) { + f_fun(state, dt, out_1546760495327089109); } -void car_F_fun(double *state, double dt, double *out_9094696385837044219) { - F_fun(state, dt, out_9094696385837044219); +void car_F_fun(double *state, double dt, double *out_3348391828080031822) { + F_fun(state, dt, out_3348391828080031822); } -void car_h_25(double *state, double *unused, double *out_1209975406708631015) { - h_25(state, unused, out_1209975406708631015); +void car_h_25(double *state, double *unused, double *out_4471759016602712561) { + h_25(state, unused, out_4471759016602712561); } -void car_H_25(double *state, double *unused, double *out_4098926953546965284) { - H_25(state, unused, out_4098926953546965284); +void car_H_25(double *state, double *unused, double *out_1740180598156646058) { + H_25(state, unused, out_1740180598156646058); } -void car_h_24(double *state, double *unused, double *out_5058461778376183297) { - h_24(state, unused, out_5058461778376183297); +void car_h_24(double *state, double *unused, double *out_3126254991737006136) { + h_24(state, unused, out_3126254991737006136); } -void car_H_24(double *state, double *unused, double *out_4943923562330660683) { - H_24(state, unused, out_4943923562330660683); +void car_H_24(double *state, double *unused, double *out_1269723116113307334) { + H_24(state, unused, out_1269723116113307334); } -void car_h_30(double *state, double *unused, double *out_5560859819641719921) { - h_30(state, unused, out_5560859819641719921); +void car_h_30(double *state, double *unused, double *out_4168118460115829323) { + h_30(state, unused, out_4168118460115829323); } -void car_H_30(double *state, double *unused, double *out_7431126778670969577) { - H_30(state, unused, out_7431126778670969577); +void car_H_30(double *state, double *unused, double *out_1610841651013405988) { + H_30(state, unused, out_1610841651013405988); } -void car_h_26(double *state, double *unused, double *out_4617349966740057486) { - h_26(state, unused, out_4617349966740057486); +void car_h_26(double *state, double *unused, double *out_2045289894744513329) { + h_26(state, unused, out_2045289894744513329); } -void car_H_26(double *state, double *unused, double *out_4755781017657277188) { - H_26(state, unused, out_4755781017657277188); +void car_H_26(double *state, double *unused, double *out_2001322720717410166) { + H_26(state, unused, out_2001322720717410166); } -void car_h_27(double *state, double *unused, double *out_6211648674369476660) { - h_27(state, unused, out_6211648674369476660); +void car_h_27(double *state, double *unused, double *out_1927282500811136585) { + h_27(state, unused, out_1927282500811136585); } -void car_H_27(double *state, double *unused, double *out_8840853983238157128) { - H_27(state, unused, out_8840853983238157128); +void car_H_27(double *state, double *unused, double *out_563921660787018923) { + H_27(state, unused, out_563921660787018923); } -void car_h_29(double *state, double *unused, double *out_559186551980874276) { - h_29(state, unused, out_559186551980874276); +void car_h_29(double *state, double *unused, double *out_8022800891634675039) { + h_29(state, unused, out_8022800891634675039); } -void car_H_29(double *state, double *unused, double *out_6920895434356577393) { - H_29(state, unused, out_6920895434356577393); +void car_H_29(double *state, double *unused, double *out_2277284387656569956) { + H_29(state, unused, out_2277284387656569956); } -void car_h_28(double *state, double *unused, double *out_7233422131679229311) { - h_28(state, unused, out_7233422131679229311); +void car_h_28(double *state, double *unused, double *out_4964068552255469270) { + h_28(state, unused, out_4964068552255469270); } -void car_H_28(double *state, double *unused, double *out_6443449622283443649) { - H_28(state, unused, out_6443449622283443649); +void car_H_28(double *state, double *unused, double *out_313654116091243705) { + H_28(state, unused, out_313654116091243705); } -void car_h_31(double *state, double *unused, double *out_8973240663529303456) { - h_31(state, unused, out_8973240663529303456); +void car_h_31(double *state, double *unused, double *out_834309488348690211) { + h_31(state, unused, out_834309488348690211); } -void car_H_31(double *state, double *unused, double *out_4129572915423925712) { - H_31(state, unused, out_4129572915423925712); +void car_H_31(double *state, double *unused, double *out_1770826560033606486) { + H_31(state, unused, out_1770826560033606486); } 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 4bde03742..7dd345d4f 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_3499577995962015470); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7704661140039100406); -void car_H_mod_fun(double *state, double *out_3593592511346668572); -void car_f_fun(double *state, double dt, double *out_5138495986546656101); -void car_F_fun(double *state, double dt, double *out_9094696385837044219); -void car_h_25(double *state, double *unused, double *out_1209975406708631015); -void car_H_25(double *state, double *unused, double *out_4098926953546965284); -void car_h_24(double *state, double *unused, double *out_5058461778376183297); -void car_H_24(double *state, double *unused, double *out_4943923562330660683); -void car_h_30(double *state, double *unused, double *out_5560859819641719921); -void car_H_30(double *state, double *unused, double *out_7431126778670969577); -void car_h_26(double *state, double *unused, double *out_4617349966740057486); -void car_H_26(double *state, double *unused, double *out_4755781017657277188); -void car_h_27(double *state, double *unused, double *out_6211648674369476660); -void car_H_27(double *state, double *unused, double *out_8840853983238157128); -void car_h_29(double *state, double *unused, double *out_559186551980874276); -void car_H_29(double *state, double *unused, double *out_6920895434356577393); -void car_h_28(double *state, double *unused, double *out_7233422131679229311); -void car_H_28(double *state, double *unused, double *out_6443449622283443649); -void car_h_31(double *state, double *unused, double *out_8973240663529303456); -void car_H_31(double *state, double *unused, double *out_4129572915423925712); +void car_err_fun(double *nom_x, double *delta_x, double *out_5319131797443970272); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3644993322176215676); +void car_H_mod_fun(double *state, double *out_3163511227028018782); +void car_f_fun(double *state, double dt, double *out_1546760495327089109); +void car_F_fun(double *state, double dt, double *out_3348391828080031822); +void car_h_25(double *state, double *unused, double *out_4471759016602712561); +void car_H_25(double *state, double *unused, double *out_1740180598156646058); +void car_h_24(double *state, double *unused, double *out_3126254991737006136); +void car_H_24(double *state, double *unused, double *out_1269723116113307334); +void car_h_30(double *state, double *unused, double *out_4168118460115829323); +void car_H_30(double *state, double *unused, double *out_1610841651013405988); +void car_h_26(double *state, double *unused, double *out_2045289894744513329); +void car_H_26(double *state, double *unused, double *out_2001322720717410166); +void car_h_27(double *state, double *unused, double *out_1927282500811136585); +void car_H_27(double *state, double *unused, double *out_563921660787018923); +void car_h_29(double *state, double *unused, double *out_8022800891634675039); +void car_H_29(double *state, double *unused, double *out_2277284387656569956); +void car_h_28(double *state, double *unused, double *out_4964068552255469270); +void car_H_28(double *state, double *unused, double *out_313654116091243705); +void car_h_31(double *state, double *unused, double *out_834309488348690211); +void car_H_31(double *state, double *unused, double *out_1770826560033606486); 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 19675ff39..26ea1ddf6 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_4711793940499862270) { - out_4711793940499862270[0] = delta_x[0] + nom_x[0]; - out_4711793940499862270[1] = delta_x[1] + nom_x[1]; - out_4711793940499862270[2] = delta_x[2] + nom_x[2]; - out_4711793940499862270[3] = delta_x[3] + nom_x[3]; - out_4711793940499862270[4] = delta_x[4] + nom_x[4]; - out_4711793940499862270[5] = delta_x[5] + nom_x[5]; - out_4711793940499862270[6] = delta_x[6] + nom_x[6]; - out_4711793940499862270[7] = delta_x[7] + nom_x[7]; - out_4711793940499862270[8] = delta_x[8] + nom_x[8]; - out_4711793940499862270[9] = delta_x[9] + nom_x[9]; - out_4711793940499862270[10] = delta_x[10] + nom_x[10]; - out_4711793940499862270[11] = delta_x[11] + nom_x[11]; - out_4711793940499862270[12] = delta_x[12] + nom_x[12]; - out_4711793940499862270[13] = delta_x[13] + nom_x[13]; - out_4711793940499862270[14] = delta_x[14] + nom_x[14]; - out_4711793940499862270[15] = delta_x[15] + nom_x[15]; - out_4711793940499862270[16] = delta_x[16] + nom_x[16]; - out_4711793940499862270[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_9044617002367965712) { + out_9044617002367965712[0] = delta_x[0] + nom_x[0]; + out_9044617002367965712[1] = delta_x[1] + nom_x[1]; + out_9044617002367965712[2] = delta_x[2] + nom_x[2]; + out_9044617002367965712[3] = delta_x[3] + nom_x[3]; + out_9044617002367965712[4] = delta_x[4] + nom_x[4]; + out_9044617002367965712[5] = delta_x[5] + nom_x[5]; + out_9044617002367965712[6] = delta_x[6] + nom_x[6]; + out_9044617002367965712[7] = delta_x[7] + nom_x[7]; + out_9044617002367965712[8] = delta_x[8] + nom_x[8]; + out_9044617002367965712[9] = delta_x[9] + nom_x[9]; + out_9044617002367965712[10] = delta_x[10] + nom_x[10]; + out_9044617002367965712[11] = delta_x[11] + nom_x[11]; + out_9044617002367965712[12] = delta_x[12] + nom_x[12]; + out_9044617002367965712[13] = delta_x[13] + nom_x[13]; + out_9044617002367965712[14] = delta_x[14] + nom_x[14]; + out_9044617002367965712[15] = delta_x[15] + nom_x[15]; + out_9044617002367965712[16] = delta_x[16] + nom_x[16]; + out_9044617002367965712[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8790106481935061031) { - out_8790106481935061031[0] = -nom_x[0] + true_x[0]; - out_8790106481935061031[1] = -nom_x[1] + true_x[1]; - out_8790106481935061031[2] = -nom_x[2] + true_x[2]; - out_8790106481935061031[3] = -nom_x[3] + true_x[3]; - out_8790106481935061031[4] = -nom_x[4] + true_x[4]; - out_8790106481935061031[5] = -nom_x[5] + true_x[5]; - out_8790106481935061031[6] = -nom_x[6] + true_x[6]; - out_8790106481935061031[7] = -nom_x[7] + true_x[7]; - out_8790106481935061031[8] = -nom_x[8] + true_x[8]; - out_8790106481935061031[9] = -nom_x[9] + true_x[9]; - out_8790106481935061031[10] = -nom_x[10] + true_x[10]; - out_8790106481935061031[11] = -nom_x[11] + true_x[11]; - out_8790106481935061031[12] = -nom_x[12] + true_x[12]; - out_8790106481935061031[13] = -nom_x[13] + true_x[13]; - out_8790106481935061031[14] = -nom_x[14] + true_x[14]; - out_8790106481935061031[15] = -nom_x[15] + true_x[15]; - out_8790106481935061031[16] = -nom_x[16] + true_x[16]; - out_8790106481935061031[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2986388781113821274) { + out_2986388781113821274[0] = -nom_x[0] + true_x[0]; + out_2986388781113821274[1] = -nom_x[1] + true_x[1]; + out_2986388781113821274[2] = -nom_x[2] + true_x[2]; + out_2986388781113821274[3] = -nom_x[3] + true_x[3]; + out_2986388781113821274[4] = -nom_x[4] + true_x[4]; + out_2986388781113821274[5] = -nom_x[5] + true_x[5]; + out_2986388781113821274[6] = -nom_x[6] + true_x[6]; + out_2986388781113821274[7] = -nom_x[7] + true_x[7]; + out_2986388781113821274[8] = -nom_x[8] + true_x[8]; + out_2986388781113821274[9] = -nom_x[9] + true_x[9]; + out_2986388781113821274[10] = -nom_x[10] + true_x[10]; + out_2986388781113821274[11] = -nom_x[11] + true_x[11]; + out_2986388781113821274[12] = -nom_x[12] + true_x[12]; + out_2986388781113821274[13] = -nom_x[13] + true_x[13]; + out_2986388781113821274[14] = -nom_x[14] + true_x[14]; + out_2986388781113821274[15] = -nom_x[15] + true_x[15]; + out_2986388781113821274[16] = -nom_x[16] + true_x[16]; + out_2986388781113821274[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_3710500625877642434) { - out_3710500625877642434[0] = 1.0; - out_3710500625877642434[1] = 0.0; - out_3710500625877642434[2] = 0.0; - out_3710500625877642434[3] = 0.0; - out_3710500625877642434[4] = 0.0; - out_3710500625877642434[5] = 0.0; - out_3710500625877642434[6] = 0.0; - out_3710500625877642434[7] = 0.0; - out_3710500625877642434[8] = 0.0; - out_3710500625877642434[9] = 0.0; - out_3710500625877642434[10] = 0.0; - out_3710500625877642434[11] = 0.0; - out_3710500625877642434[12] = 0.0; - out_3710500625877642434[13] = 0.0; - out_3710500625877642434[14] = 0.0; - out_3710500625877642434[15] = 0.0; - out_3710500625877642434[16] = 0.0; - out_3710500625877642434[17] = 0.0; - out_3710500625877642434[18] = 0.0; - out_3710500625877642434[19] = 1.0; - out_3710500625877642434[20] = 0.0; - out_3710500625877642434[21] = 0.0; - out_3710500625877642434[22] = 0.0; - out_3710500625877642434[23] = 0.0; - out_3710500625877642434[24] = 0.0; - out_3710500625877642434[25] = 0.0; - out_3710500625877642434[26] = 0.0; - out_3710500625877642434[27] = 0.0; - out_3710500625877642434[28] = 0.0; - out_3710500625877642434[29] = 0.0; - out_3710500625877642434[30] = 0.0; - out_3710500625877642434[31] = 0.0; - out_3710500625877642434[32] = 0.0; - out_3710500625877642434[33] = 0.0; - out_3710500625877642434[34] = 0.0; - out_3710500625877642434[35] = 0.0; - out_3710500625877642434[36] = 0.0; - out_3710500625877642434[37] = 0.0; - out_3710500625877642434[38] = 1.0; - out_3710500625877642434[39] = 0.0; - out_3710500625877642434[40] = 0.0; - out_3710500625877642434[41] = 0.0; - out_3710500625877642434[42] = 0.0; - out_3710500625877642434[43] = 0.0; - out_3710500625877642434[44] = 0.0; - out_3710500625877642434[45] = 0.0; - out_3710500625877642434[46] = 0.0; - out_3710500625877642434[47] = 0.0; - out_3710500625877642434[48] = 0.0; - out_3710500625877642434[49] = 0.0; - out_3710500625877642434[50] = 0.0; - out_3710500625877642434[51] = 0.0; - out_3710500625877642434[52] = 0.0; - out_3710500625877642434[53] = 0.0; - out_3710500625877642434[54] = 0.0; - out_3710500625877642434[55] = 0.0; - out_3710500625877642434[56] = 0.0; - out_3710500625877642434[57] = 1.0; - out_3710500625877642434[58] = 0.0; - out_3710500625877642434[59] = 0.0; - out_3710500625877642434[60] = 0.0; - out_3710500625877642434[61] = 0.0; - out_3710500625877642434[62] = 0.0; - out_3710500625877642434[63] = 0.0; - out_3710500625877642434[64] = 0.0; - out_3710500625877642434[65] = 0.0; - out_3710500625877642434[66] = 0.0; - out_3710500625877642434[67] = 0.0; - out_3710500625877642434[68] = 0.0; - out_3710500625877642434[69] = 0.0; - out_3710500625877642434[70] = 0.0; - out_3710500625877642434[71] = 0.0; - out_3710500625877642434[72] = 0.0; - out_3710500625877642434[73] = 0.0; - out_3710500625877642434[74] = 0.0; - out_3710500625877642434[75] = 0.0; - out_3710500625877642434[76] = 1.0; - out_3710500625877642434[77] = 0.0; - out_3710500625877642434[78] = 0.0; - out_3710500625877642434[79] = 0.0; - out_3710500625877642434[80] = 0.0; - out_3710500625877642434[81] = 0.0; - out_3710500625877642434[82] = 0.0; - out_3710500625877642434[83] = 0.0; - out_3710500625877642434[84] = 0.0; - out_3710500625877642434[85] = 0.0; - out_3710500625877642434[86] = 0.0; - out_3710500625877642434[87] = 0.0; - out_3710500625877642434[88] = 0.0; - out_3710500625877642434[89] = 0.0; - out_3710500625877642434[90] = 0.0; - out_3710500625877642434[91] = 0.0; - out_3710500625877642434[92] = 0.0; - out_3710500625877642434[93] = 0.0; - out_3710500625877642434[94] = 0.0; - out_3710500625877642434[95] = 1.0; - out_3710500625877642434[96] = 0.0; - out_3710500625877642434[97] = 0.0; - out_3710500625877642434[98] = 0.0; - out_3710500625877642434[99] = 0.0; - out_3710500625877642434[100] = 0.0; - out_3710500625877642434[101] = 0.0; - out_3710500625877642434[102] = 0.0; - out_3710500625877642434[103] = 0.0; - out_3710500625877642434[104] = 0.0; - out_3710500625877642434[105] = 0.0; - out_3710500625877642434[106] = 0.0; - out_3710500625877642434[107] = 0.0; - out_3710500625877642434[108] = 0.0; - out_3710500625877642434[109] = 0.0; - out_3710500625877642434[110] = 0.0; - out_3710500625877642434[111] = 0.0; - out_3710500625877642434[112] = 0.0; - out_3710500625877642434[113] = 0.0; - out_3710500625877642434[114] = 1.0; - out_3710500625877642434[115] = 0.0; - out_3710500625877642434[116] = 0.0; - out_3710500625877642434[117] = 0.0; - out_3710500625877642434[118] = 0.0; - out_3710500625877642434[119] = 0.0; - out_3710500625877642434[120] = 0.0; - out_3710500625877642434[121] = 0.0; - out_3710500625877642434[122] = 0.0; - out_3710500625877642434[123] = 0.0; - out_3710500625877642434[124] = 0.0; - out_3710500625877642434[125] = 0.0; - out_3710500625877642434[126] = 0.0; - out_3710500625877642434[127] = 0.0; - out_3710500625877642434[128] = 0.0; - out_3710500625877642434[129] = 0.0; - out_3710500625877642434[130] = 0.0; - out_3710500625877642434[131] = 0.0; - out_3710500625877642434[132] = 0.0; - out_3710500625877642434[133] = 1.0; - out_3710500625877642434[134] = 0.0; - out_3710500625877642434[135] = 0.0; - out_3710500625877642434[136] = 0.0; - out_3710500625877642434[137] = 0.0; - out_3710500625877642434[138] = 0.0; - out_3710500625877642434[139] = 0.0; - out_3710500625877642434[140] = 0.0; - out_3710500625877642434[141] = 0.0; - out_3710500625877642434[142] = 0.0; - out_3710500625877642434[143] = 0.0; - out_3710500625877642434[144] = 0.0; - out_3710500625877642434[145] = 0.0; - out_3710500625877642434[146] = 0.0; - out_3710500625877642434[147] = 0.0; - out_3710500625877642434[148] = 0.0; - out_3710500625877642434[149] = 0.0; - out_3710500625877642434[150] = 0.0; - out_3710500625877642434[151] = 0.0; - out_3710500625877642434[152] = 1.0; - out_3710500625877642434[153] = 0.0; - out_3710500625877642434[154] = 0.0; - out_3710500625877642434[155] = 0.0; - out_3710500625877642434[156] = 0.0; - out_3710500625877642434[157] = 0.0; - out_3710500625877642434[158] = 0.0; - out_3710500625877642434[159] = 0.0; - out_3710500625877642434[160] = 0.0; - out_3710500625877642434[161] = 0.0; - out_3710500625877642434[162] = 0.0; - out_3710500625877642434[163] = 0.0; - out_3710500625877642434[164] = 0.0; - out_3710500625877642434[165] = 0.0; - out_3710500625877642434[166] = 0.0; - out_3710500625877642434[167] = 0.0; - out_3710500625877642434[168] = 0.0; - out_3710500625877642434[169] = 0.0; - out_3710500625877642434[170] = 0.0; - out_3710500625877642434[171] = 1.0; - out_3710500625877642434[172] = 0.0; - out_3710500625877642434[173] = 0.0; - out_3710500625877642434[174] = 0.0; - out_3710500625877642434[175] = 0.0; - out_3710500625877642434[176] = 0.0; - out_3710500625877642434[177] = 0.0; - out_3710500625877642434[178] = 0.0; - out_3710500625877642434[179] = 0.0; - out_3710500625877642434[180] = 0.0; - out_3710500625877642434[181] = 0.0; - out_3710500625877642434[182] = 0.0; - out_3710500625877642434[183] = 0.0; - out_3710500625877642434[184] = 0.0; - out_3710500625877642434[185] = 0.0; - out_3710500625877642434[186] = 0.0; - out_3710500625877642434[187] = 0.0; - out_3710500625877642434[188] = 0.0; - out_3710500625877642434[189] = 0.0; - out_3710500625877642434[190] = 1.0; - out_3710500625877642434[191] = 0.0; - out_3710500625877642434[192] = 0.0; - out_3710500625877642434[193] = 0.0; - out_3710500625877642434[194] = 0.0; - out_3710500625877642434[195] = 0.0; - out_3710500625877642434[196] = 0.0; - out_3710500625877642434[197] = 0.0; - out_3710500625877642434[198] = 0.0; - out_3710500625877642434[199] = 0.0; - out_3710500625877642434[200] = 0.0; - out_3710500625877642434[201] = 0.0; - out_3710500625877642434[202] = 0.0; - out_3710500625877642434[203] = 0.0; - out_3710500625877642434[204] = 0.0; - out_3710500625877642434[205] = 0.0; - out_3710500625877642434[206] = 0.0; - out_3710500625877642434[207] = 0.0; - out_3710500625877642434[208] = 0.0; - out_3710500625877642434[209] = 1.0; - out_3710500625877642434[210] = 0.0; - out_3710500625877642434[211] = 0.0; - out_3710500625877642434[212] = 0.0; - out_3710500625877642434[213] = 0.0; - out_3710500625877642434[214] = 0.0; - out_3710500625877642434[215] = 0.0; - out_3710500625877642434[216] = 0.0; - out_3710500625877642434[217] = 0.0; - out_3710500625877642434[218] = 0.0; - out_3710500625877642434[219] = 0.0; - out_3710500625877642434[220] = 0.0; - out_3710500625877642434[221] = 0.0; - out_3710500625877642434[222] = 0.0; - out_3710500625877642434[223] = 0.0; - out_3710500625877642434[224] = 0.0; - out_3710500625877642434[225] = 0.0; - out_3710500625877642434[226] = 0.0; - out_3710500625877642434[227] = 0.0; - out_3710500625877642434[228] = 1.0; - out_3710500625877642434[229] = 0.0; - out_3710500625877642434[230] = 0.0; - out_3710500625877642434[231] = 0.0; - out_3710500625877642434[232] = 0.0; - out_3710500625877642434[233] = 0.0; - out_3710500625877642434[234] = 0.0; - out_3710500625877642434[235] = 0.0; - out_3710500625877642434[236] = 0.0; - out_3710500625877642434[237] = 0.0; - out_3710500625877642434[238] = 0.0; - out_3710500625877642434[239] = 0.0; - out_3710500625877642434[240] = 0.0; - out_3710500625877642434[241] = 0.0; - out_3710500625877642434[242] = 0.0; - out_3710500625877642434[243] = 0.0; - out_3710500625877642434[244] = 0.0; - out_3710500625877642434[245] = 0.0; - out_3710500625877642434[246] = 0.0; - out_3710500625877642434[247] = 1.0; - out_3710500625877642434[248] = 0.0; - out_3710500625877642434[249] = 0.0; - out_3710500625877642434[250] = 0.0; - out_3710500625877642434[251] = 0.0; - out_3710500625877642434[252] = 0.0; - out_3710500625877642434[253] = 0.0; - out_3710500625877642434[254] = 0.0; - out_3710500625877642434[255] = 0.0; - out_3710500625877642434[256] = 0.0; - out_3710500625877642434[257] = 0.0; - out_3710500625877642434[258] = 0.0; - out_3710500625877642434[259] = 0.0; - out_3710500625877642434[260] = 0.0; - out_3710500625877642434[261] = 0.0; - out_3710500625877642434[262] = 0.0; - out_3710500625877642434[263] = 0.0; - out_3710500625877642434[264] = 0.0; - out_3710500625877642434[265] = 0.0; - out_3710500625877642434[266] = 1.0; - out_3710500625877642434[267] = 0.0; - out_3710500625877642434[268] = 0.0; - out_3710500625877642434[269] = 0.0; - out_3710500625877642434[270] = 0.0; - out_3710500625877642434[271] = 0.0; - out_3710500625877642434[272] = 0.0; - out_3710500625877642434[273] = 0.0; - out_3710500625877642434[274] = 0.0; - out_3710500625877642434[275] = 0.0; - out_3710500625877642434[276] = 0.0; - out_3710500625877642434[277] = 0.0; - out_3710500625877642434[278] = 0.0; - out_3710500625877642434[279] = 0.0; - out_3710500625877642434[280] = 0.0; - out_3710500625877642434[281] = 0.0; - out_3710500625877642434[282] = 0.0; - out_3710500625877642434[283] = 0.0; - out_3710500625877642434[284] = 0.0; - out_3710500625877642434[285] = 1.0; - out_3710500625877642434[286] = 0.0; - out_3710500625877642434[287] = 0.0; - out_3710500625877642434[288] = 0.0; - out_3710500625877642434[289] = 0.0; - out_3710500625877642434[290] = 0.0; - out_3710500625877642434[291] = 0.0; - out_3710500625877642434[292] = 0.0; - out_3710500625877642434[293] = 0.0; - out_3710500625877642434[294] = 0.0; - out_3710500625877642434[295] = 0.0; - out_3710500625877642434[296] = 0.0; - out_3710500625877642434[297] = 0.0; - out_3710500625877642434[298] = 0.0; - out_3710500625877642434[299] = 0.0; - out_3710500625877642434[300] = 0.0; - out_3710500625877642434[301] = 0.0; - out_3710500625877642434[302] = 0.0; - out_3710500625877642434[303] = 0.0; - out_3710500625877642434[304] = 1.0; - out_3710500625877642434[305] = 0.0; - out_3710500625877642434[306] = 0.0; - out_3710500625877642434[307] = 0.0; - out_3710500625877642434[308] = 0.0; - out_3710500625877642434[309] = 0.0; - out_3710500625877642434[310] = 0.0; - out_3710500625877642434[311] = 0.0; - out_3710500625877642434[312] = 0.0; - out_3710500625877642434[313] = 0.0; - out_3710500625877642434[314] = 0.0; - out_3710500625877642434[315] = 0.0; - out_3710500625877642434[316] = 0.0; - out_3710500625877642434[317] = 0.0; - out_3710500625877642434[318] = 0.0; - out_3710500625877642434[319] = 0.0; - out_3710500625877642434[320] = 0.0; - out_3710500625877642434[321] = 0.0; - out_3710500625877642434[322] = 0.0; - out_3710500625877642434[323] = 1.0; +void H_mod_fun(double *state, double *out_5449976102936300367) { + out_5449976102936300367[0] = 1.0; + out_5449976102936300367[1] = 0.0; + out_5449976102936300367[2] = 0.0; + out_5449976102936300367[3] = 0.0; + out_5449976102936300367[4] = 0.0; + out_5449976102936300367[5] = 0.0; + out_5449976102936300367[6] = 0.0; + out_5449976102936300367[7] = 0.0; + out_5449976102936300367[8] = 0.0; + out_5449976102936300367[9] = 0.0; + out_5449976102936300367[10] = 0.0; + out_5449976102936300367[11] = 0.0; + out_5449976102936300367[12] = 0.0; + out_5449976102936300367[13] = 0.0; + out_5449976102936300367[14] = 0.0; + out_5449976102936300367[15] = 0.0; + out_5449976102936300367[16] = 0.0; + out_5449976102936300367[17] = 0.0; + out_5449976102936300367[18] = 0.0; + out_5449976102936300367[19] = 1.0; + out_5449976102936300367[20] = 0.0; + out_5449976102936300367[21] = 0.0; + out_5449976102936300367[22] = 0.0; + out_5449976102936300367[23] = 0.0; + out_5449976102936300367[24] = 0.0; + out_5449976102936300367[25] = 0.0; + out_5449976102936300367[26] = 0.0; + out_5449976102936300367[27] = 0.0; + out_5449976102936300367[28] = 0.0; + out_5449976102936300367[29] = 0.0; + out_5449976102936300367[30] = 0.0; + out_5449976102936300367[31] = 0.0; + out_5449976102936300367[32] = 0.0; + out_5449976102936300367[33] = 0.0; + out_5449976102936300367[34] = 0.0; + out_5449976102936300367[35] = 0.0; + out_5449976102936300367[36] = 0.0; + out_5449976102936300367[37] = 0.0; + out_5449976102936300367[38] = 1.0; + out_5449976102936300367[39] = 0.0; + out_5449976102936300367[40] = 0.0; + out_5449976102936300367[41] = 0.0; + out_5449976102936300367[42] = 0.0; + out_5449976102936300367[43] = 0.0; + out_5449976102936300367[44] = 0.0; + out_5449976102936300367[45] = 0.0; + out_5449976102936300367[46] = 0.0; + out_5449976102936300367[47] = 0.0; + out_5449976102936300367[48] = 0.0; + out_5449976102936300367[49] = 0.0; + out_5449976102936300367[50] = 0.0; + out_5449976102936300367[51] = 0.0; + out_5449976102936300367[52] = 0.0; + out_5449976102936300367[53] = 0.0; + out_5449976102936300367[54] = 0.0; + out_5449976102936300367[55] = 0.0; + out_5449976102936300367[56] = 0.0; + out_5449976102936300367[57] = 1.0; + out_5449976102936300367[58] = 0.0; + out_5449976102936300367[59] = 0.0; + out_5449976102936300367[60] = 0.0; + out_5449976102936300367[61] = 0.0; + out_5449976102936300367[62] = 0.0; + out_5449976102936300367[63] = 0.0; + out_5449976102936300367[64] = 0.0; + out_5449976102936300367[65] = 0.0; + out_5449976102936300367[66] = 0.0; + out_5449976102936300367[67] = 0.0; + out_5449976102936300367[68] = 0.0; + out_5449976102936300367[69] = 0.0; + out_5449976102936300367[70] = 0.0; + out_5449976102936300367[71] = 0.0; + out_5449976102936300367[72] = 0.0; + out_5449976102936300367[73] = 0.0; + out_5449976102936300367[74] = 0.0; + out_5449976102936300367[75] = 0.0; + out_5449976102936300367[76] = 1.0; + out_5449976102936300367[77] = 0.0; + out_5449976102936300367[78] = 0.0; + out_5449976102936300367[79] = 0.0; + out_5449976102936300367[80] = 0.0; + out_5449976102936300367[81] = 0.0; + out_5449976102936300367[82] = 0.0; + out_5449976102936300367[83] = 0.0; + out_5449976102936300367[84] = 0.0; + out_5449976102936300367[85] = 0.0; + out_5449976102936300367[86] = 0.0; + out_5449976102936300367[87] = 0.0; + out_5449976102936300367[88] = 0.0; + out_5449976102936300367[89] = 0.0; + out_5449976102936300367[90] = 0.0; + out_5449976102936300367[91] = 0.0; + out_5449976102936300367[92] = 0.0; + out_5449976102936300367[93] = 0.0; + out_5449976102936300367[94] = 0.0; + out_5449976102936300367[95] = 1.0; + out_5449976102936300367[96] = 0.0; + out_5449976102936300367[97] = 0.0; + out_5449976102936300367[98] = 0.0; + out_5449976102936300367[99] = 0.0; + out_5449976102936300367[100] = 0.0; + out_5449976102936300367[101] = 0.0; + out_5449976102936300367[102] = 0.0; + out_5449976102936300367[103] = 0.0; + out_5449976102936300367[104] = 0.0; + out_5449976102936300367[105] = 0.0; + out_5449976102936300367[106] = 0.0; + out_5449976102936300367[107] = 0.0; + out_5449976102936300367[108] = 0.0; + out_5449976102936300367[109] = 0.0; + out_5449976102936300367[110] = 0.0; + out_5449976102936300367[111] = 0.0; + out_5449976102936300367[112] = 0.0; + out_5449976102936300367[113] = 0.0; + out_5449976102936300367[114] = 1.0; + out_5449976102936300367[115] = 0.0; + out_5449976102936300367[116] = 0.0; + out_5449976102936300367[117] = 0.0; + out_5449976102936300367[118] = 0.0; + out_5449976102936300367[119] = 0.0; + out_5449976102936300367[120] = 0.0; + out_5449976102936300367[121] = 0.0; + out_5449976102936300367[122] = 0.0; + out_5449976102936300367[123] = 0.0; + out_5449976102936300367[124] = 0.0; + out_5449976102936300367[125] = 0.0; + out_5449976102936300367[126] = 0.0; + out_5449976102936300367[127] = 0.0; + out_5449976102936300367[128] = 0.0; + out_5449976102936300367[129] = 0.0; + out_5449976102936300367[130] = 0.0; + out_5449976102936300367[131] = 0.0; + out_5449976102936300367[132] = 0.0; + out_5449976102936300367[133] = 1.0; + out_5449976102936300367[134] = 0.0; + out_5449976102936300367[135] = 0.0; + out_5449976102936300367[136] = 0.0; + out_5449976102936300367[137] = 0.0; + out_5449976102936300367[138] = 0.0; + out_5449976102936300367[139] = 0.0; + out_5449976102936300367[140] = 0.0; + out_5449976102936300367[141] = 0.0; + out_5449976102936300367[142] = 0.0; + out_5449976102936300367[143] = 0.0; + out_5449976102936300367[144] = 0.0; + out_5449976102936300367[145] = 0.0; + out_5449976102936300367[146] = 0.0; + out_5449976102936300367[147] = 0.0; + out_5449976102936300367[148] = 0.0; + out_5449976102936300367[149] = 0.0; + out_5449976102936300367[150] = 0.0; + out_5449976102936300367[151] = 0.0; + out_5449976102936300367[152] = 1.0; + out_5449976102936300367[153] = 0.0; + out_5449976102936300367[154] = 0.0; + out_5449976102936300367[155] = 0.0; + out_5449976102936300367[156] = 0.0; + out_5449976102936300367[157] = 0.0; + out_5449976102936300367[158] = 0.0; + out_5449976102936300367[159] = 0.0; + out_5449976102936300367[160] = 0.0; + out_5449976102936300367[161] = 0.0; + out_5449976102936300367[162] = 0.0; + out_5449976102936300367[163] = 0.0; + out_5449976102936300367[164] = 0.0; + out_5449976102936300367[165] = 0.0; + out_5449976102936300367[166] = 0.0; + out_5449976102936300367[167] = 0.0; + out_5449976102936300367[168] = 0.0; + out_5449976102936300367[169] = 0.0; + out_5449976102936300367[170] = 0.0; + out_5449976102936300367[171] = 1.0; + out_5449976102936300367[172] = 0.0; + out_5449976102936300367[173] = 0.0; + out_5449976102936300367[174] = 0.0; + out_5449976102936300367[175] = 0.0; + out_5449976102936300367[176] = 0.0; + out_5449976102936300367[177] = 0.0; + out_5449976102936300367[178] = 0.0; + out_5449976102936300367[179] = 0.0; + out_5449976102936300367[180] = 0.0; + out_5449976102936300367[181] = 0.0; + out_5449976102936300367[182] = 0.0; + out_5449976102936300367[183] = 0.0; + out_5449976102936300367[184] = 0.0; + out_5449976102936300367[185] = 0.0; + out_5449976102936300367[186] = 0.0; + out_5449976102936300367[187] = 0.0; + out_5449976102936300367[188] = 0.0; + out_5449976102936300367[189] = 0.0; + out_5449976102936300367[190] = 1.0; + out_5449976102936300367[191] = 0.0; + out_5449976102936300367[192] = 0.0; + out_5449976102936300367[193] = 0.0; + out_5449976102936300367[194] = 0.0; + out_5449976102936300367[195] = 0.0; + out_5449976102936300367[196] = 0.0; + out_5449976102936300367[197] = 0.0; + out_5449976102936300367[198] = 0.0; + out_5449976102936300367[199] = 0.0; + out_5449976102936300367[200] = 0.0; + out_5449976102936300367[201] = 0.0; + out_5449976102936300367[202] = 0.0; + out_5449976102936300367[203] = 0.0; + out_5449976102936300367[204] = 0.0; + out_5449976102936300367[205] = 0.0; + out_5449976102936300367[206] = 0.0; + out_5449976102936300367[207] = 0.0; + out_5449976102936300367[208] = 0.0; + out_5449976102936300367[209] = 1.0; + out_5449976102936300367[210] = 0.0; + out_5449976102936300367[211] = 0.0; + out_5449976102936300367[212] = 0.0; + out_5449976102936300367[213] = 0.0; + out_5449976102936300367[214] = 0.0; + out_5449976102936300367[215] = 0.0; + out_5449976102936300367[216] = 0.0; + out_5449976102936300367[217] = 0.0; + out_5449976102936300367[218] = 0.0; + out_5449976102936300367[219] = 0.0; + out_5449976102936300367[220] = 0.0; + out_5449976102936300367[221] = 0.0; + out_5449976102936300367[222] = 0.0; + out_5449976102936300367[223] = 0.0; + out_5449976102936300367[224] = 0.0; + out_5449976102936300367[225] = 0.0; + out_5449976102936300367[226] = 0.0; + out_5449976102936300367[227] = 0.0; + out_5449976102936300367[228] = 1.0; + out_5449976102936300367[229] = 0.0; + out_5449976102936300367[230] = 0.0; + out_5449976102936300367[231] = 0.0; + out_5449976102936300367[232] = 0.0; + out_5449976102936300367[233] = 0.0; + out_5449976102936300367[234] = 0.0; + out_5449976102936300367[235] = 0.0; + out_5449976102936300367[236] = 0.0; + out_5449976102936300367[237] = 0.0; + out_5449976102936300367[238] = 0.0; + out_5449976102936300367[239] = 0.0; + out_5449976102936300367[240] = 0.0; + out_5449976102936300367[241] = 0.0; + out_5449976102936300367[242] = 0.0; + out_5449976102936300367[243] = 0.0; + out_5449976102936300367[244] = 0.0; + out_5449976102936300367[245] = 0.0; + out_5449976102936300367[246] = 0.0; + out_5449976102936300367[247] = 1.0; + out_5449976102936300367[248] = 0.0; + out_5449976102936300367[249] = 0.0; + out_5449976102936300367[250] = 0.0; + out_5449976102936300367[251] = 0.0; + out_5449976102936300367[252] = 0.0; + out_5449976102936300367[253] = 0.0; + out_5449976102936300367[254] = 0.0; + out_5449976102936300367[255] = 0.0; + out_5449976102936300367[256] = 0.0; + out_5449976102936300367[257] = 0.0; + out_5449976102936300367[258] = 0.0; + out_5449976102936300367[259] = 0.0; + out_5449976102936300367[260] = 0.0; + out_5449976102936300367[261] = 0.0; + out_5449976102936300367[262] = 0.0; + out_5449976102936300367[263] = 0.0; + out_5449976102936300367[264] = 0.0; + out_5449976102936300367[265] = 0.0; + out_5449976102936300367[266] = 1.0; + out_5449976102936300367[267] = 0.0; + out_5449976102936300367[268] = 0.0; + out_5449976102936300367[269] = 0.0; + out_5449976102936300367[270] = 0.0; + out_5449976102936300367[271] = 0.0; + out_5449976102936300367[272] = 0.0; + out_5449976102936300367[273] = 0.0; + out_5449976102936300367[274] = 0.0; + out_5449976102936300367[275] = 0.0; + out_5449976102936300367[276] = 0.0; + out_5449976102936300367[277] = 0.0; + out_5449976102936300367[278] = 0.0; + out_5449976102936300367[279] = 0.0; + out_5449976102936300367[280] = 0.0; + out_5449976102936300367[281] = 0.0; + out_5449976102936300367[282] = 0.0; + out_5449976102936300367[283] = 0.0; + out_5449976102936300367[284] = 0.0; + out_5449976102936300367[285] = 1.0; + out_5449976102936300367[286] = 0.0; + out_5449976102936300367[287] = 0.0; + out_5449976102936300367[288] = 0.0; + out_5449976102936300367[289] = 0.0; + out_5449976102936300367[290] = 0.0; + out_5449976102936300367[291] = 0.0; + out_5449976102936300367[292] = 0.0; + out_5449976102936300367[293] = 0.0; + out_5449976102936300367[294] = 0.0; + out_5449976102936300367[295] = 0.0; + out_5449976102936300367[296] = 0.0; + out_5449976102936300367[297] = 0.0; + out_5449976102936300367[298] = 0.0; + out_5449976102936300367[299] = 0.0; + out_5449976102936300367[300] = 0.0; + out_5449976102936300367[301] = 0.0; + out_5449976102936300367[302] = 0.0; + out_5449976102936300367[303] = 0.0; + out_5449976102936300367[304] = 1.0; + out_5449976102936300367[305] = 0.0; + out_5449976102936300367[306] = 0.0; + out_5449976102936300367[307] = 0.0; + out_5449976102936300367[308] = 0.0; + out_5449976102936300367[309] = 0.0; + out_5449976102936300367[310] = 0.0; + out_5449976102936300367[311] = 0.0; + out_5449976102936300367[312] = 0.0; + out_5449976102936300367[313] = 0.0; + out_5449976102936300367[314] = 0.0; + out_5449976102936300367[315] = 0.0; + out_5449976102936300367[316] = 0.0; + out_5449976102936300367[317] = 0.0; + out_5449976102936300367[318] = 0.0; + out_5449976102936300367[319] = 0.0; + out_5449976102936300367[320] = 0.0; + out_5449976102936300367[321] = 0.0; + out_5449976102936300367[322] = 0.0; + out_5449976102936300367[323] = 1.0; } -void f_fun(double *state, double dt, double *out_5725984385602899057) { - out_5725984385602899057[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_5725984385602899057[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_5725984385602899057[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_5725984385602899057[3] = dt*state[12] + state[3]; - out_5725984385602899057[4] = dt*state[13] + state[4]; - out_5725984385602899057[5] = dt*state[14] + state[5]; - out_5725984385602899057[6] = state[6]; - out_5725984385602899057[7] = state[7]; - out_5725984385602899057[8] = state[8]; - out_5725984385602899057[9] = state[9]; - out_5725984385602899057[10] = state[10]; - out_5725984385602899057[11] = state[11]; - out_5725984385602899057[12] = state[12]; - out_5725984385602899057[13] = state[13]; - out_5725984385602899057[14] = state[14]; - out_5725984385602899057[15] = state[15]; - out_5725984385602899057[16] = state[16]; - out_5725984385602899057[17] = state[17]; +void f_fun(double *state, double dt, double *out_2001314774583974236) { + out_2001314774583974236[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_2001314774583974236[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_2001314774583974236[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_2001314774583974236[3] = dt*state[12] + state[3]; + out_2001314774583974236[4] = dt*state[13] + state[4]; + out_2001314774583974236[5] = dt*state[14] + state[5]; + out_2001314774583974236[6] = state[6]; + out_2001314774583974236[7] = state[7]; + out_2001314774583974236[8] = state[8]; + out_2001314774583974236[9] = state[9]; + out_2001314774583974236[10] = state[10]; + out_2001314774583974236[11] = state[11]; + out_2001314774583974236[12] = state[12]; + out_2001314774583974236[13] = state[13]; + out_2001314774583974236[14] = state[14]; + out_2001314774583974236[15] = state[15]; + out_2001314774583974236[16] = state[16]; + out_2001314774583974236[17] = state[17]; } -void F_fun(double *state, double dt, double *out_3503616768446574438) { - out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[2] = 0; - out_3503616768446574438[3] = 0; - out_3503616768446574438[4] = 0; - out_3503616768446574438[5] = 0; - out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[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_3503616768446574438[9] = 0; - out_3503616768446574438[10] = 0; - out_3503616768446574438[11] = 0; - out_3503616768446574438[12] = 0; - out_3503616768446574438[13] = 0; - out_3503616768446574438[14] = 0; - out_3503616768446574438[15] = 0; - out_3503616768446574438[16] = 0; - out_3503616768446574438[17] = 0; - out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[20] = 0; - out_3503616768446574438[21] = 0; - out_3503616768446574438[22] = 0; - out_3503616768446574438[23] = 0; - out_3503616768446574438[24] = 0; - out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[27] = 0; - out_3503616768446574438[28] = 0; - out_3503616768446574438[29] = 0; - out_3503616768446574438[30] = 0; - out_3503616768446574438[31] = 0; - out_3503616768446574438[32] = 0; - out_3503616768446574438[33] = 0; - out_3503616768446574438[34] = 0; - out_3503616768446574438[35] = 0; - out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[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_3503616768446574438[39] = 0; - out_3503616768446574438[40] = 0; - out_3503616768446574438[41] = 0; - out_3503616768446574438[42] = 0; - out_3503616768446574438[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_3503616768446574438[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_3503616768446574438[45] = 0; - out_3503616768446574438[46] = 0; - out_3503616768446574438[47] = 0; - out_3503616768446574438[48] = 0; - out_3503616768446574438[49] = 0; - out_3503616768446574438[50] = 0; - out_3503616768446574438[51] = 0; - out_3503616768446574438[52] = 0; - out_3503616768446574438[53] = 0; - out_3503616768446574438[54] = 0; - out_3503616768446574438[55] = 0; - out_3503616768446574438[56] = 0; - out_3503616768446574438[57] = 1; - out_3503616768446574438[58] = 0; - out_3503616768446574438[59] = 0; - out_3503616768446574438[60] = 0; - out_3503616768446574438[61] = 0; - out_3503616768446574438[62] = 0; - out_3503616768446574438[63] = 0; - out_3503616768446574438[64] = 0; - out_3503616768446574438[65] = 0; - out_3503616768446574438[66] = dt; - out_3503616768446574438[67] = 0; - out_3503616768446574438[68] = 0; - out_3503616768446574438[69] = 0; - out_3503616768446574438[70] = 0; - out_3503616768446574438[71] = 0; - out_3503616768446574438[72] = 0; - out_3503616768446574438[73] = 0; - out_3503616768446574438[74] = 0; - out_3503616768446574438[75] = 0; - out_3503616768446574438[76] = 1; - out_3503616768446574438[77] = 0; - out_3503616768446574438[78] = 0; - out_3503616768446574438[79] = 0; - out_3503616768446574438[80] = 0; - out_3503616768446574438[81] = 0; - out_3503616768446574438[82] = 0; - out_3503616768446574438[83] = 0; - out_3503616768446574438[84] = 0; - out_3503616768446574438[85] = dt; - out_3503616768446574438[86] = 0; - out_3503616768446574438[87] = 0; - out_3503616768446574438[88] = 0; - out_3503616768446574438[89] = 0; - out_3503616768446574438[90] = 0; - out_3503616768446574438[91] = 0; - out_3503616768446574438[92] = 0; - out_3503616768446574438[93] = 0; - out_3503616768446574438[94] = 0; - out_3503616768446574438[95] = 1; - out_3503616768446574438[96] = 0; - out_3503616768446574438[97] = 0; - out_3503616768446574438[98] = 0; - out_3503616768446574438[99] = 0; - out_3503616768446574438[100] = 0; - out_3503616768446574438[101] = 0; - out_3503616768446574438[102] = 0; - out_3503616768446574438[103] = 0; - out_3503616768446574438[104] = dt; - out_3503616768446574438[105] = 0; - out_3503616768446574438[106] = 0; - out_3503616768446574438[107] = 0; - out_3503616768446574438[108] = 0; - out_3503616768446574438[109] = 0; - out_3503616768446574438[110] = 0; - out_3503616768446574438[111] = 0; - out_3503616768446574438[112] = 0; - out_3503616768446574438[113] = 0; - out_3503616768446574438[114] = 1; - out_3503616768446574438[115] = 0; - out_3503616768446574438[116] = 0; - out_3503616768446574438[117] = 0; - out_3503616768446574438[118] = 0; - out_3503616768446574438[119] = 0; - out_3503616768446574438[120] = 0; - out_3503616768446574438[121] = 0; - out_3503616768446574438[122] = 0; - out_3503616768446574438[123] = 0; - out_3503616768446574438[124] = 0; - out_3503616768446574438[125] = 0; - out_3503616768446574438[126] = 0; - out_3503616768446574438[127] = 0; - out_3503616768446574438[128] = 0; - out_3503616768446574438[129] = 0; - out_3503616768446574438[130] = 0; - out_3503616768446574438[131] = 0; - out_3503616768446574438[132] = 0; - out_3503616768446574438[133] = 1; - out_3503616768446574438[134] = 0; - out_3503616768446574438[135] = 0; - out_3503616768446574438[136] = 0; - out_3503616768446574438[137] = 0; - out_3503616768446574438[138] = 0; - out_3503616768446574438[139] = 0; - out_3503616768446574438[140] = 0; - out_3503616768446574438[141] = 0; - out_3503616768446574438[142] = 0; - out_3503616768446574438[143] = 0; - out_3503616768446574438[144] = 0; - out_3503616768446574438[145] = 0; - out_3503616768446574438[146] = 0; - out_3503616768446574438[147] = 0; - out_3503616768446574438[148] = 0; - out_3503616768446574438[149] = 0; - out_3503616768446574438[150] = 0; - out_3503616768446574438[151] = 0; - out_3503616768446574438[152] = 1; - out_3503616768446574438[153] = 0; - out_3503616768446574438[154] = 0; - out_3503616768446574438[155] = 0; - out_3503616768446574438[156] = 0; - out_3503616768446574438[157] = 0; - out_3503616768446574438[158] = 0; - out_3503616768446574438[159] = 0; - out_3503616768446574438[160] = 0; - out_3503616768446574438[161] = 0; - out_3503616768446574438[162] = 0; - out_3503616768446574438[163] = 0; - out_3503616768446574438[164] = 0; - out_3503616768446574438[165] = 0; - out_3503616768446574438[166] = 0; - out_3503616768446574438[167] = 0; - out_3503616768446574438[168] = 0; - out_3503616768446574438[169] = 0; - out_3503616768446574438[170] = 0; - out_3503616768446574438[171] = 1; - out_3503616768446574438[172] = 0; - out_3503616768446574438[173] = 0; - out_3503616768446574438[174] = 0; - out_3503616768446574438[175] = 0; - out_3503616768446574438[176] = 0; - out_3503616768446574438[177] = 0; - out_3503616768446574438[178] = 0; - out_3503616768446574438[179] = 0; - out_3503616768446574438[180] = 0; - out_3503616768446574438[181] = 0; - out_3503616768446574438[182] = 0; - out_3503616768446574438[183] = 0; - out_3503616768446574438[184] = 0; - out_3503616768446574438[185] = 0; - out_3503616768446574438[186] = 0; - out_3503616768446574438[187] = 0; - out_3503616768446574438[188] = 0; - out_3503616768446574438[189] = 0; - out_3503616768446574438[190] = 1; - out_3503616768446574438[191] = 0; - out_3503616768446574438[192] = 0; - out_3503616768446574438[193] = 0; - out_3503616768446574438[194] = 0; - out_3503616768446574438[195] = 0; - out_3503616768446574438[196] = 0; - out_3503616768446574438[197] = 0; - out_3503616768446574438[198] = 0; - out_3503616768446574438[199] = 0; - out_3503616768446574438[200] = 0; - out_3503616768446574438[201] = 0; - out_3503616768446574438[202] = 0; - out_3503616768446574438[203] = 0; - out_3503616768446574438[204] = 0; - out_3503616768446574438[205] = 0; - out_3503616768446574438[206] = 0; - out_3503616768446574438[207] = 0; - out_3503616768446574438[208] = 0; - out_3503616768446574438[209] = 1; - out_3503616768446574438[210] = 0; - out_3503616768446574438[211] = 0; - out_3503616768446574438[212] = 0; - out_3503616768446574438[213] = 0; - out_3503616768446574438[214] = 0; - out_3503616768446574438[215] = 0; - out_3503616768446574438[216] = 0; - out_3503616768446574438[217] = 0; - out_3503616768446574438[218] = 0; - out_3503616768446574438[219] = 0; - out_3503616768446574438[220] = 0; - out_3503616768446574438[221] = 0; - out_3503616768446574438[222] = 0; - out_3503616768446574438[223] = 0; - out_3503616768446574438[224] = 0; - out_3503616768446574438[225] = 0; - out_3503616768446574438[226] = 0; - out_3503616768446574438[227] = 0; - out_3503616768446574438[228] = 1; - out_3503616768446574438[229] = 0; - out_3503616768446574438[230] = 0; - out_3503616768446574438[231] = 0; - out_3503616768446574438[232] = 0; - out_3503616768446574438[233] = 0; - out_3503616768446574438[234] = 0; - out_3503616768446574438[235] = 0; - out_3503616768446574438[236] = 0; - out_3503616768446574438[237] = 0; - out_3503616768446574438[238] = 0; - out_3503616768446574438[239] = 0; - out_3503616768446574438[240] = 0; - out_3503616768446574438[241] = 0; - out_3503616768446574438[242] = 0; - out_3503616768446574438[243] = 0; - out_3503616768446574438[244] = 0; - out_3503616768446574438[245] = 0; - out_3503616768446574438[246] = 0; - out_3503616768446574438[247] = 1; - out_3503616768446574438[248] = 0; - out_3503616768446574438[249] = 0; - out_3503616768446574438[250] = 0; - out_3503616768446574438[251] = 0; - out_3503616768446574438[252] = 0; - out_3503616768446574438[253] = 0; - out_3503616768446574438[254] = 0; - out_3503616768446574438[255] = 0; - out_3503616768446574438[256] = 0; - out_3503616768446574438[257] = 0; - out_3503616768446574438[258] = 0; - out_3503616768446574438[259] = 0; - out_3503616768446574438[260] = 0; - out_3503616768446574438[261] = 0; - out_3503616768446574438[262] = 0; - out_3503616768446574438[263] = 0; - out_3503616768446574438[264] = 0; - out_3503616768446574438[265] = 0; - out_3503616768446574438[266] = 1; - out_3503616768446574438[267] = 0; - out_3503616768446574438[268] = 0; - out_3503616768446574438[269] = 0; - out_3503616768446574438[270] = 0; - out_3503616768446574438[271] = 0; - out_3503616768446574438[272] = 0; - out_3503616768446574438[273] = 0; - out_3503616768446574438[274] = 0; - out_3503616768446574438[275] = 0; - out_3503616768446574438[276] = 0; - out_3503616768446574438[277] = 0; - out_3503616768446574438[278] = 0; - out_3503616768446574438[279] = 0; - out_3503616768446574438[280] = 0; - out_3503616768446574438[281] = 0; - out_3503616768446574438[282] = 0; - out_3503616768446574438[283] = 0; - out_3503616768446574438[284] = 0; - out_3503616768446574438[285] = 1; - out_3503616768446574438[286] = 0; - out_3503616768446574438[287] = 0; - out_3503616768446574438[288] = 0; - out_3503616768446574438[289] = 0; - out_3503616768446574438[290] = 0; - out_3503616768446574438[291] = 0; - out_3503616768446574438[292] = 0; - out_3503616768446574438[293] = 0; - out_3503616768446574438[294] = 0; - out_3503616768446574438[295] = 0; - out_3503616768446574438[296] = 0; - out_3503616768446574438[297] = 0; - out_3503616768446574438[298] = 0; - out_3503616768446574438[299] = 0; - out_3503616768446574438[300] = 0; - out_3503616768446574438[301] = 0; - out_3503616768446574438[302] = 0; - out_3503616768446574438[303] = 0; - out_3503616768446574438[304] = 1; - out_3503616768446574438[305] = 0; - out_3503616768446574438[306] = 0; - out_3503616768446574438[307] = 0; - out_3503616768446574438[308] = 0; - out_3503616768446574438[309] = 0; - out_3503616768446574438[310] = 0; - out_3503616768446574438[311] = 0; - out_3503616768446574438[312] = 0; - out_3503616768446574438[313] = 0; - out_3503616768446574438[314] = 0; - out_3503616768446574438[315] = 0; - out_3503616768446574438[316] = 0; - out_3503616768446574438[317] = 0; - out_3503616768446574438[318] = 0; - out_3503616768446574438[319] = 0; - out_3503616768446574438[320] = 0; - out_3503616768446574438[321] = 0; - out_3503616768446574438[322] = 0; - out_3503616768446574438[323] = 1; +void F_fun(double *state, double dt, double *out_173210708380746968) { + out_173210708380746968[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_173210708380746968[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_173210708380746968[2] = 0; + out_173210708380746968[3] = 0; + out_173210708380746968[4] = 0; + out_173210708380746968[5] = 0; + out_173210708380746968[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_173210708380746968[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_173210708380746968[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_173210708380746968[9] = 0; + out_173210708380746968[10] = 0; + out_173210708380746968[11] = 0; + out_173210708380746968[12] = 0; + out_173210708380746968[13] = 0; + out_173210708380746968[14] = 0; + out_173210708380746968[15] = 0; + out_173210708380746968[16] = 0; + out_173210708380746968[17] = 0; + out_173210708380746968[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_173210708380746968[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_173210708380746968[20] = 0; + out_173210708380746968[21] = 0; + out_173210708380746968[22] = 0; + out_173210708380746968[23] = 0; + out_173210708380746968[24] = 0; + out_173210708380746968[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_173210708380746968[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_173210708380746968[27] = 0; + out_173210708380746968[28] = 0; + out_173210708380746968[29] = 0; + out_173210708380746968[30] = 0; + out_173210708380746968[31] = 0; + out_173210708380746968[32] = 0; + out_173210708380746968[33] = 0; + out_173210708380746968[34] = 0; + out_173210708380746968[35] = 0; + out_173210708380746968[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_173210708380746968[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_173210708380746968[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_173210708380746968[39] = 0; + out_173210708380746968[40] = 0; + out_173210708380746968[41] = 0; + out_173210708380746968[42] = 0; + out_173210708380746968[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_173210708380746968[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_173210708380746968[45] = 0; + out_173210708380746968[46] = 0; + out_173210708380746968[47] = 0; + out_173210708380746968[48] = 0; + out_173210708380746968[49] = 0; + out_173210708380746968[50] = 0; + out_173210708380746968[51] = 0; + out_173210708380746968[52] = 0; + out_173210708380746968[53] = 0; + out_173210708380746968[54] = 0; + out_173210708380746968[55] = 0; + out_173210708380746968[56] = 0; + out_173210708380746968[57] = 1; + out_173210708380746968[58] = 0; + out_173210708380746968[59] = 0; + out_173210708380746968[60] = 0; + out_173210708380746968[61] = 0; + out_173210708380746968[62] = 0; + out_173210708380746968[63] = 0; + out_173210708380746968[64] = 0; + out_173210708380746968[65] = 0; + out_173210708380746968[66] = dt; + out_173210708380746968[67] = 0; + out_173210708380746968[68] = 0; + out_173210708380746968[69] = 0; + out_173210708380746968[70] = 0; + out_173210708380746968[71] = 0; + out_173210708380746968[72] = 0; + out_173210708380746968[73] = 0; + out_173210708380746968[74] = 0; + out_173210708380746968[75] = 0; + out_173210708380746968[76] = 1; + out_173210708380746968[77] = 0; + out_173210708380746968[78] = 0; + out_173210708380746968[79] = 0; + out_173210708380746968[80] = 0; + out_173210708380746968[81] = 0; + out_173210708380746968[82] = 0; + out_173210708380746968[83] = 0; + out_173210708380746968[84] = 0; + out_173210708380746968[85] = dt; + out_173210708380746968[86] = 0; + out_173210708380746968[87] = 0; + out_173210708380746968[88] = 0; + out_173210708380746968[89] = 0; + out_173210708380746968[90] = 0; + out_173210708380746968[91] = 0; + out_173210708380746968[92] = 0; + out_173210708380746968[93] = 0; + out_173210708380746968[94] = 0; + out_173210708380746968[95] = 1; + out_173210708380746968[96] = 0; + out_173210708380746968[97] = 0; + out_173210708380746968[98] = 0; + out_173210708380746968[99] = 0; + out_173210708380746968[100] = 0; + out_173210708380746968[101] = 0; + out_173210708380746968[102] = 0; + out_173210708380746968[103] = 0; + out_173210708380746968[104] = dt; + out_173210708380746968[105] = 0; + out_173210708380746968[106] = 0; + out_173210708380746968[107] = 0; + out_173210708380746968[108] = 0; + out_173210708380746968[109] = 0; + out_173210708380746968[110] = 0; + out_173210708380746968[111] = 0; + out_173210708380746968[112] = 0; + out_173210708380746968[113] = 0; + out_173210708380746968[114] = 1; + out_173210708380746968[115] = 0; + out_173210708380746968[116] = 0; + out_173210708380746968[117] = 0; + out_173210708380746968[118] = 0; + out_173210708380746968[119] = 0; + out_173210708380746968[120] = 0; + out_173210708380746968[121] = 0; + out_173210708380746968[122] = 0; + out_173210708380746968[123] = 0; + out_173210708380746968[124] = 0; + out_173210708380746968[125] = 0; + out_173210708380746968[126] = 0; + out_173210708380746968[127] = 0; + out_173210708380746968[128] = 0; + out_173210708380746968[129] = 0; + out_173210708380746968[130] = 0; + out_173210708380746968[131] = 0; + out_173210708380746968[132] = 0; + out_173210708380746968[133] = 1; + out_173210708380746968[134] = 0; + out_173210708380746968[135] = 0; + out_173210708380746968[136] = 0; + out_173210708380746968[137] = 0; + out_173210708380746968[138] = 0; + out_173210708380746968[139] = 0; + out_173210708380746968[140] = 0; + out_173210708380746968[141] = 0; + out_173210708380746968[142] = 0; + out_173210708380746968[143] = 0; + out_173210708380746968[144] = 0; + out_173210708380746968[145] = 0; + out_173210708380746968[146] = 0; + out_173210708380746968[147] = 0; + out_173210708380746968[148] = 0; + out_173210708380746968[149] = 0; + out_173210708380746968[150] = 0; + out_173210708380746968[151] = 0; + out_173210708380746968[152] = 1; + out_173210708380746968[153] = 0; + out_173210708380746968[154] = 0; + out_173210708380746968[155] = 0; + out_173210708380746968[156] = 0; + out_173210708380746968[157] = 0; + out_173210708380746968[158] = 0; + out_173210708380746968[159] = 0; + out_173210708380746968[160] = 0; + out_173210708380746968[161] = 0; + out_173210708380746968[162] = 0; + out_173210708380746968[163] = 0; + out_173210708380746968[164] = 0; + out_173210708380746968[165] = 0; + out_173210708380746968[166] = 0; + out_173210708380746968[167] = 0; + out_173210708380746968[168] = 0; + out_173210708380746968[169] = 0; + out_173210708380746968[170] = 0; + out_173210708380746968[171] = 1; + out_173210708380746968[172] = 0; + out_173210708380746968[173] = 0; + out_173210708380746968[174] = 0; + out_173210708380746968[175] = 0; + out_173210708380746968[176] = 0; + out_173210708380746968[177] = 0; + out_173210708380746968[178] = 0; + out_173210708380746968[179] = 0; + out_173210708380746968[180] = 0; + out_173210708380746968[181] = 0; + out_173210708380746968[182] = 0; + out_173210708380746968[183] = 0; + out_173210708380746968[184] = 0; + out_173210708380746968[185] = 0; + out_173210708380746968[186] = 0; + out_173210708380746968[187] = 0; + out_173210708380746968[188] = 0; + out_173210708380746968[189] = 0; + out_173210708380746968[190] = 1; + out_173210708380746968[191] = 0; + out_173210708380746968[192] = 0; + out_173210708380746968[193] = 0; + out_173210708380746968[194] = 0; + out_173210708380746968[195] = 0; + out_173210708380746968[196] = 0; + out_173210708380746968[197] = 0; + out_173210708380746968[198] = 0; + out_173210708380746968[199] = 0; + out_173210708380746968[200] = 0; + out_173210708380746968[201] = 0; + out_173210708380746968[202] = 0; + out_173210708380746968[203] = 0; + out_173210708380746968[204] = 0; + out_173210708380746968[205] = 0; + out_173210708380746968[206] = 0; + out_173210708380746968[207] = 0; + out_173210708380746968[208] = 0; + out_173210708380746968[209] = 1; + out_173210708380746968[210] = 0; + out_173210708380746968[211] = 0; + out_173210708380746968[212] = 0; + out_173210708380746968[213] = 0; + out_173210708380746968[214] = 0; + out_173210708380746968[215] = 0; + out_173210708380746968[216] = 0; + out_173210708380746968[217] = 0; + out_173210708380746968[218] = 0; + out_173210708380746968[219] = 0; + out_173210708380746968[220] = 0; + out_173210708380746968[221] = 0; + out_173210708380746968[222] = 0; + out_173210708380746968[223] = 0; + out_173210708380746968[224] = 0; + out_173210708380746968[225] = 0; + out_173210708380746968[226] = 0; + out_173210708380746968[227] = 0; + out_173210708380746968[228] = 1; + out_173210708380746968[229] = 0; + out_173210708380746968[230] = 0; + out_173210708380746968[231] = 0; + out_173210708380746968[232] = 0; + out_173210708380746968[233] = 0; + out_173210708380746968[234] = 0; + out_173210708380746968[235] = 0; + out_173210708380746968[236] = 0; + out_173210708380746968[237] = 0; + out_173210708380746968[238] = 0; + out_173210708380746968[239] = 0; + out_173210708380746968[240] = 0; + out_173210708380746968[241] = 0; + out_173210708380746968[242] = 0; + out_173210708380746968[243] = 0; + out_173210708380746968[244] = 0; + out_173210708380746968[245] = 0; + out_173210708380746968[246] = 0; + out_173210708380746968[247] = 1; + out_173210708380746968[248] = 0; + out_173210708380746968[249] = 0; + out_173210708380746968[250] = 0; + out_173210708380746968[251] = 0; + out_173210708380746968[252] = 0; + out_173210708380746968[253] = 0; + out_173210708380746968[254] = 0; + out_173210708380746968[255] = 0; + out_173210708380746968[256] = 0; + out_173210708380746968[257] = 0; + out_173210708380746968[258] = 0; + out_173210708380746968[259] = 0; + out_173210708380746968[260] = 0; + out_173210708380746968[261] = 0; + out_173210708380746968[262] = 0; + out_173210708380746968[263] = 0; + out_173210708380746968[264] = 0; + out_173210708380746968[265] = 0; + out_173210708380746968[266] = 1; + out_173210708380746968[267] = 0; + out_173210708380746968[268] = 0; + out_173210708380746968[269] = 0; + out_173210708380746968[270] = 0; + out_173210708380746968[271] = 0; + out_173210708380746968[272] = 0; + out_173210708380746968[273] = 0; + out_173210708380746968[274] = 0; + out_173210708380746968[275] = 0; + out_173210708380746968[276] = 0; + out_173210708380746968[277] = 0; + out_173210708380746968[278] = 0; + out_173210708380746968[279] = 0; + out_173210708380746968[280] = 0; + out_173210708380746968[281] = 0; + out_173210708380746968[282] = 0; + out_173210708380746968[283] = 0; + out_173210708380746968[284] = 0; + out_173210708380746968[285] = 1; + out_173210708380746968[286] = 0; + out_173210708380746968[287] = 0; + out_173210708380746968[288] = 0; + out_173210708380746968[289] = 0; + out_173210708380746968[290] = 0; + out_173210708380746968[291] = 0; + out_173210708380746968[292] = 0; + out_173210708380746968[293] = 0; + out_173210708380746968[294] = 0; + out_173210708380746968[295] = 0; + out_173210708380746968[296] = 0; + out_173210708380746968[297] = 0; + out_173210708380746968[298] = 0; + out_173210708380746968[299] = 0; + out_173210708380746968[300] = 0; + out_173210708380746968[301] = 0; + out_173210708380746968[302] = 0; + out_173210708380746968[303] = 0; + out_173210708380746968[304] = 1; + out_173210708380746968[305] = 0; + out_173210708380746968[306] = 0; + out_173210708380746968[307] = 0; + out_173210708380746968[308] = 0; + out_173210708380746968[309] = 0; + out_173210708380746968[310] = 0; + out_173210708380746968[311] = 0; + out_173210708380746968[312] = 0; + out_173210708380746968[313] = 0; + out_173210708380746968[314] = 0; + out_173210708380746968[315] = 0; + out_173210708380746968[316] = 0; + out_173210708380746968[317] = 0; + out_173210708380746968[318] = 0; + out_173210708380746968[319] = 0; + out_173210708380746968[320] = 0; + out_173210708380746968[321] = 0; + out_173210708380746968[322] = 0; + out_173210708380746968[323] = 1; } -void h_4(double *state, double *unused, double *out_3803432647309987328) { - out_3803432647309987328[0] = state[6] + state[9]; - out_3803432647309987328[1] = state[7] + state[10]; - out_3803432647309987328[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_4290451809527946304) { + out_4290451809527946304[0] = state[6] + state[9]; + out_4290451809527946304[1] = state[7] + state[10]; + out_4290451809527946304[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_7950155699686530755) { - out_7950155699686530755[0] = 0; - out_7950155699686530755[1] = 0; - out_7950155699686530755[2] = 0; - out_7950155699686530755[3] = 0; - out_7950155699686530755[4] = 0; - out_7950155699686530755[5] = 0; - out_7950155699686530755[6] = 1; - out_7950155699686530755[7] = 0; - out_7950155699686530755[8] = 0; - out_7950155699686530755[9] = 1; - out_7950155699686530755[10] = 0; - out_7950155699686530755[11] = 0; - out_7950155699686530755[12] = 0; - out_7950155699686530755[13] = 0; - out_7950155699686530755[14] = 0; - out_7950155699686530755[15] = 0; - out_7950155699686530755[16] = 0; - out_7950155699686530755[17] = 0; - out_7950155699686530755[18] = 0; - out_7950155699686530755[19] = 0; - out_7950155699686530755[20] = 0; - out_7950155699686530755[21] = 0; - out_7950155699686530755[22] = 0; - out_7950155699686530755[23] = 0; - out_7950155699686530755[24] = 0; - out_7950155699686530755[25] = 1; - out_7950155699686530755[26] = 0; - out_7950155699686530755[27] = 0; - out_7950155699686530755[28] = 1; - out_7950155699686530755[29] = 0; - out_7950155699686530755[30] = 0; - out_7950155699686530755[31] = 0; - out_7950155699686530755[32] = 0; - out_7950155699686530755[33] = 0; - out_7950155699686530755[34] = 0; - out_7950155699686530755[35] = 0; - out_7950155699686530755[36] = 0; - out_7950155699686530755[37] = 0; - out_7950155699686530755[38] = 0; - out_7950155699686530755[39] = 0; - out_7950155699686530755[40] = 0; - out_7950155699686530755[41] = 0; - out_7950155699686530755[42] = 0; - out_7950155699686530755[43] = 0; - out_7950155699686530755[44] = 1; - out_7950155699686530755[45] = 0; - out_7950155699686530755[46] = 0; - out_7950155699686530755[47] = 1; - out_7950155699686530755[48] = 0; - out_7950155699686530755[49] = 0; - out_7950155699686530755[50] = 0; - out_7950155699686530755[51] = 0; - out_7950155699686530755[52] = 0; - out_7950155699686530755[53] = 0; +void H_4(double *state, double *unused, double *out_4695815100881587760) { + out_4695815100881587760[0] = 0; + out_4695815100881587760[1] = 0; + out_4695815100881587760[2] = 0; + out_4695815100881587760[3] = 0; + out_4695815100881587760[4] = 0; + out_4695815100881587760[5] = 0; + out_4695815100881587760[6] = 1; + out_4695815100881587760[7] = 0; + out_4695815100881587760[8] = 0; + out_4695815100881587760[9] = 1; + out_4695815100881587760[10] = 0; + out_4695815100881587760[11] = 0; + out_4695815100881587760[12] = 0; + out_4695815100881587760[13] = 0; + out_4695815100881587760[14] = 0; + out_4695815100881587760[15] = 0; + out_4695815100881587760[16] = 0; + out_4695815100881587760[17] = 0; + out_4695815100881587760[18] = 0; + out_4695815100881587760[19] = 0; + out_4695815100881587760[20] = 0; + out_4695815100881587760[21] = 0; + out_4695815100881587760[22] = 0; + out_4695815100881587760[23] = 0; + out_4695815100881587760[24] = 0; + out_4695815100881587760[25] = 1; + out_4695815100881587760[26] = 0; + out_4695815100881587760[27] = 0; + out_4695815100881587760[28] = 1; + out_4695815100881587760[29] = 0; + out_4695815100881587760[30] = 0; + out_4695815100881587760[31] = 0; + out_4695815100881587760[32] = 0; + out_4695815100881587760[33] = 0; + out_4695815100881587760[34] = 0; + out_4695815100881587760[35] = 0; + out_4695815100881587760[36] = 0; + out_4695815100881587760[37] = 0; + out_4695815100881587760[38] = 0; + out_4695815100881587760[39] = 0; + out_4695815100881587760[40] = 0; + out_4695815100881587760[41] = 0; + out_4695815100881587760[42] = 0; + out_4695815100881587760[43] = 0; + out_4695815100881587760[44] = 1; + out_4695815100881587760[45] = 0; + out_4695815100881587760[46] = 0; + out_4695815100881587760[47] = 1; + out_4695815100881587760[48] = 0; + out_4695815100881587760[49] = 0; + out_4695815100881587760[50] = 0; + out_4695815100881587760[51] = 0; + out_4695815100881587760[52] = 0; + out_4695815100881587760[53] = 0; } -void h_10(double *state, double *unused, double *out_477421004614984104) { - out_477421004614984104[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_477421004614984104[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_477421004614984104[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_6723857678965398132) { + out_6723857678965398132[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_6723857678965398132[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_6723857678965398132[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_8536057142137197060) { - out_8536057142137197060[0] = 0; - out_8536057142137197060[1] = 9.8100000000000005*cos(state[1]); - out_8536057142137197060[2] = 0; - out_8536057142137197060[3] = 0; - out_8536057142137197060[4] = -state[8]; - out_8536057142137197060[5] = state[7]; - out_8536057142137197060[6] = 0; - out_8536057142137197060[7] = state[5]; - out_8536057142137197060[8] = -state[4]; - out_8536057142137197060[9] = 0; - out_8536057142137197060[10] = 0; - out_8536057142137197060[11] = 0; - out_8536057142137197060[12] = 1; - out_8536057142137197060[13] = 0; - out_8536057142137197060[14] = 0; - out_8536057142137197060[15] = 1; - out_8536057142137197060[16] = 0; - out_8536057142137197060[17] = 0; - out_8536057142137197060[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_8536057142137197060[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_8536057142137197060[20] = 0; - out_8536057142137197060[21] = state[8]; - out_8536057142137197060[22] = 0; - out_8536057142137197060[23] = -state[6]; - out_8536057142137197060[24] = -state[5]; - out_8536057142137197060[25] = 0; - out_8536057142137197060[26] = state[3]; - out_8536057142137197060[27] = 0; - out_8536057142137197060[28] = 0; - out_8536057142137197060[29] = 0; - out_8536057142137197060[30] = 0; - out_8536057142137197060[31] = 1; - out_8536057142137197060[32] = 0; - out_8536057142137197060[33] = 0; - out_8536057142137197060[34] = 1; - out_8536057142137197060[35] = 0; - out_8536057142137197060[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_8536057142137197060[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_8536057142137197060[38] = 0; - out_8536057142137197060[39] = -state[7]; - out_8536057142137197060[40] = state[6]; - out_8536057142137197060[41] = 0; - out_8536057142137197060[42] = state[4]; - out_8536057142137197060[43] = -state[3]; - out_8536057142137197060[44] = 0; - out_8536057142137197060[45] = 0; - out_8536057142137197060[46] = 0; - out_8536057142137197060[47] = 0; - out_8536057142137197060[48] = 0; - out_8536057142137197060[49] = 0; - out_8536057142137197060[50] = 1; - out_8536057142137197060[51] = 0; - out_8536057142137197060[52] = 0; - out_8536057142137197060[53] = 1; +void H_10(double *state, double *unused, double *out_984080189925557249) { + out_984080189925557249[0] = 0; + out_984080189925557249[1] = 9.8100000000000005*cos(state[1]); + out_984080189925557249[2] = 0; + out_984080189925557249[3] = 0; + out_984080189925557249[4] = -state[8]; + out_984080189925557249[5] = state[7]; + out_984080189925557249[6] = 0; + out_984080189925557249[7] = state[5]; + out_984080189925557249[8] = -state[4]; + out_984080189925557249[9] = 0; + out_984080189925557249[10] = 0; + out_984080189925557249[11] = 0; + out_984080189925557249[12] = 1; + out_984080189925557249[13] = 0; + out_984080189925557249[14] = 0; + out_984080189925557249[15] = 1; + out_984080189925557249[16] = 0; + out_984080189925557249[17] = 0; + out_984080189925557249[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_984080189925557249[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_984080189925557249[20] = 0; + out_984080189925557249[21] = state[8]; + out_984080189925557249[22] = 0; + out_984080189925557249[23] = -state[6]; + out_984080189925557249[24] = -state[5]; + out_984080189925557249[25] = 0; + out_984080189925557249[26] = state[3]; + out_984080189925557249[27] = 0; + out_984080189925557249[28] = 0; + out_984080189925557249[29] = 0; + out_984080189925557249[30] = 0; + out_984080189925557249[31] = 1; + out_984080189925557249[32] = 0; + out_984080189925557249[33] = 0; + out_984080189925557249[34] = 1; + out_984080189925557249[35] = 0; + out_984080189925557249[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_984080189925557249[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_984080189925557249[38] = 0; + out_984080189925557249[39] = -state[7]; + out_984080189925557249[40] = state[6]; + out_984080189925557249[41] = 0; + out_984080189925557249[42] = state[4]; + out_984080189925557249[43] = -state[3]; + out_984080189925557249[44] = 0; + out_984080189925557249[45] = 0; + out_984080189925557249[46] = 0; + out_984080189925557249[47] = 0; + out_984080189925557249[48] = 0; + out_984080189925557249[49] = 0; + out_984080189925557249[50] = 1; + out_984080189925557249[51] = 0; + out_984080189925557249[52] = 0; + out_984080189925557249[53] = 1; } -void h_13(double *state, double *unused, double *out_1243321013145270066) { - out_1243321013145270066[0] = state[3]; - out_1243321013145270066[1] = state[4]; - out_1243321013145270066[2] = state[5]; +void h_13(double *state, double *unused, double *out_1838091537859337419) { + out_1838091537859337419[0] = state[3]; + out_1838091537859337419[1] = state[4]; + out_1838091537859337419[2] = state[5]; } -void H_13(double *state, double *unused, double *out_8514757619368374859) { - out_8514757619368374859[0] = 0; - out_8514757619368374859[1] = 0; - out_8514757619368374859[2] = 0; - out_8514757619368374859[3] = 1; - out_8514757619368374859[4] = 0; - out_8514757619368374859[5] = 0; - out_8514757619368374859[6] = 0; - out_8514757619368374859[7] = 0; - out_8514757619368374859[8] = 0; - out_8514757619368374859[9] = 0; - out_8514757619368374859[10] = 0; - out_8514757619368374859[11] = 0; - out_8514757619368374859[12] = 0; - out_8514757619368374859[13] = 0; - out_8514757619368374859[14] = 0; - out_8514757619368374859[15] = 0; - out_8514757619368374859[16] = 0; - out_8514757619368374859[17] = 0; - out_8514757619368374859[18] = 0; - out_8514757619368374859[19] = 0; - out_8514757619368374859[20] = 0; - out_8514757619368374859[21] = 0; - out_8514757619368374859[22] = 1; - out_8514757619368374859[23] = 0; - out_8514757619368374859[24] = 0; - out_8514757619368374859[25] = 0; - out_8514757619368374859[26] = 0; - out_8514757619368374859[27] = 0; - out_8514757619368374859[28] = 0; - out_8514757619368374859[29] = 0; - out_8514757619368374859[30] = 0; - out_8514757619368374859[31] = 0; - out_8514757619368374859[32] = 0; - out_8514757619368374859[33] = 0; - out_8514757619368374859[34] = 0; - out_8514757619368374859[35] = 0; - out_8514757619368374859[36] = 0; - out_8514757619368374859[37] = 0; - out_8514757619368374859[38] = 0; - out_8514757619368374859[39] = 0; - out_8514757619368374859[40] = 0; - out_8514757619368374859[41] = 1; - out_8514757619368374859[42] = 0; - out_8514757619368374859[43] = 0; - out_8514757619368374859[44] = 0; - out_8514757619368374859[45] = 0; - out_8514757619368374859[46] = 0; - out_8514757619368374859[47] = 0; - out_8514757619368374859[48] = 0; - out_8514757619368374859[49] = 0; - out_8514757619368374859[50] = 0; - out_8514757619368374859[51] = 0; - out_8514757619368374859[52] = 0; - out_8514757619368374859[53] = 0; +void H_13(double *state, double *unused, double *out_1483541275549254959) { + out_1483541275549254959[0] = 0; + out_1483541275549254959[1] = 0; + out_1483541275549254959[2] = 0; + out_1483541275549254959[3] = 1; + out_1483541275549254959[4] = 0; + out_1483541275549254959[5] = 0; + out_1483541275549254959[6] = 0; + out_1483541275549254959[7] = 0; + out_1483541275549254959[8] = 0; + out_1483541275549254959[9] = 0; + out_1483541275549254959[10] = 0; + out_1483541275549254959[11] = 0; + out_1483541275549254959[12] = 0; + out_1483541275549254959[13] = 0; + out_1483541275549254959[14] = 0; + out_1483541275549254959[15] = 0; + out_1483541275549254959[16] = 0; + out_1483541275549254959[17] = 0; + out_1483541275549254959[18] = 0; + out_1483541275549254959[19] = 0; + out_1483541275549254959[20] = 0; + out_1483541275549254959[21] = 0; + out_1483541275549254959[22] = 1; + out_1483541275549254959[23] = 0; + out_1483541275549254959[24] = 0; + out_1483541275549254959[25] = 0; + out_1483541275549254959[26] = 0; + out_1483541275549254959[27] = 0; + out_1483541275549254959[28] = 0; + out_1483541275549254959[29] = 0; + out_1483541275549254959[30] = 0; + out_1483541275549254959[31] = 0; + out_1483541275549254959[32] = 0; + out_1483541275549254959[33] = 0; + out_1483541275549254959[34] = 0; + out_1483541275549254959[35] = 0; + out_1483541275549254959[36] = 0; + out_1483541275549254959[37] = 0; + out_1483541275549254959[38] = 0; + out_1483541275549254959[39] = 0; + out_1483541275549254959[40] = 0; + out_1483541275549254959[41] = 1; + out_1483541275549254959[42] = 0; + out_1483541275549254959[43] = 0; + out_1483541275549254959[44] = 0; + out_1483541275549254959[45] = 0; + out_1483541275549254959[46] = 0; + out_1483541275549254959[47] = 0; + out_1483541275549254959[48] = 0; + out_1483541275549254959[49] = 0; + out_1483541275549254959[50] = 0; + out_1483541275549254959[51] = 0; + out_1483541275549254959[52] = 0; + out_1483541275549254959[53] = 0; } -void h_14(double *state, double *unused, double *out_7694485530011040401) { - out_7694485530011040401[0] = state[6]; - out_7694485530011040401[1] = state[7]; - out_7694485530011040401[2] = state[8]; +void h_14(double *state, double *unused, double *out_849503921054780572) { + out_849503921054780572[0] = state[6]; + out_849503921054780572[1] = state[7]; + out_849503921054780572[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4867367267391158459) { - out_4867367267391158459[0] = 0; - out_4867367267391158459[1] = 0; - out_4867367267391158459[2] = 0; - out_4867367267391158459[3] = 0; - out_4867367267391158459[4] = 0; - out_4867367267391158459[5] = 0; - out_4867367267391158459[6] = 1; - out_4867367267391158459[7] = 0; - out_4867367267391158459[8] = 0; - out_4867367267391158459[9] = 0; - out_4867367267391158459[10] = 0; - out_4867367267391158459[11] = 0; - out_4867367267391158459[12] = 0; - out_4867367267391158459[13] = 0; - out_4867367267391158459[14] = 0; - out_4867367267391158459[15] = 0; - out_4867367267391158459[16] = 0; - out_4867367267391158459[17] = 0; - out_4867367267391158459[18] = 0; - out_4867367267391158459[19] = 0; - out_4867367267391158459[20] = 0; - out_4867367267391158459[21] = 0; - out_4867367267391158459[22] = 0; - out_4867367267391158459[23] = 0; - out_4867367267391158459[24] = 0; - out_4867367267391158459[25] = 1; - out_4867367267391158459[26] = 0; - out_4867367267391158459[27] = 0; - out_4867367267391158459[28] = 0; - out_4867367267391158459[29] = 0; - out_4867367267391158459[30] = 0; - out_4867367267391158459[31] = 0; - out_4867367267391158459[32] = 0; - out_4867367267391158459[33] = 0; - out_4867367267391158459[34] = 0; - out_4867367267391158459[35] = 0; - out_4867367267391158459[36] = 0; - out_4867367267391158459[37] = 0; - out_4867367267391158459[38] = 0; - out_4867367267391158459[39] = 0; - out_4867367267391158459[40] = 0; - out_4867367267391158459[41] = 0; - out_4867367267391158459[42] = 0; - out_4867367267391158459[43] = 0; - out_4867367267391158459[44] = 1; - out_4867367267391158459[45] = 0; - out_4867367267391158459[46] = 0; - out_4867367267391158459[47] = 0; - out_4867367267391158459[48] = 0; - out_4867367267391158459[49] = 0; - out_4867367267391158459[50] = 0; - out_4867367267391158459[51] = 0; - out_4867367267391158459[52] = 0; - out_4867367267391158459[53] = 0; +void H_14(double *state, double *unused, double *out_732574244542103231) { + out_732574244542103231[0] = 0; + out_732574244542103231[1] = 0; + out_732574244542103231[2] = 0; + out_732574244542103231[3] = 0; + out_732574244542103231[4] = 0; + out_732574244542103231[5] = 0; + out_732574244542103231[6] = 1; + out_732574244542103231[7] = 0; + out_732574244542103231[8] = 0; + out_732574244542103231[9] = 0; + out_732574244542103231[10] = 0; + out_732574244542103231[11] = 0; + out_732574244542103231[12] = 0; + out_732574244542103231[13] = 0; + out_732574244542103231[14] = 0; + out_732574244542103231[15] = 0; + out_732574244542103231[16] = 0; + out_732574244542103231[17] = 0; + out_732574244542103231[18] = 0; + out_732574244542103231[19] = 0; + out_732574244542103231[20] = 0; + out_732574244542103231[21] = 0; + out_732574244542103231[22] = 0; + out_732574244542103231[23] = 0; + out_732574244542103231[24] = 0; + out_732574244542103231[25] = 1; + out_732574244542103231[26] = 0; + out_732574244542103231[27] = 0; + out_732574244542103231[28] = 0; + out_732574244542103231[29] = 0; + out_732574244542103231[30] = 0; + out_732574244542103231[31] = 0; + out_732574244542103231[32] = 0; + out_732574244542103231[33] = 0; + out_732574244542103231[34] = 0; + out_732574244542103231[35] = 0; + out_732574244542103231[36] = 0; + out_732574244542103231[37] = 0; + out_732574244542103231[38] = 0; + out_732574244542103231[39] = 0; + out_732574244542103231[40] = 0; + out_732574244542103231[41] = 0; + out_732574244542103231[42] = 0; + out_732574244542103231[43] = 0; + out_732574244542103231[44] = 1; + out_732574244542103231[45] = 0; + out_732574244542103231[46] = 0; + out_732574244542103231[47] = 0; + out_732574244542103231[48] = 0; + out_732574244542103231[49] = 0; + out_732574244542103231[50] = 0; + out_732574244542103231[51] = 0; + out_732574244542103231[52] = 0; + out_732574244542103231[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_4711793940499862270) { - err_fun(nom_x, delta_x, out_4711793940499862270); +void pose_err_fun(double *nom_x, double *delta_x, double *out_9044617002367965712) { + err_fun(nom_x, delta_x, out_9044617002367965712); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8790106481935061031) { - inv_err_fun(nom_x, true_x, out_8790106481935061031); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2986388781113821274) { + inv_err_fun(nom_x, true_x, out_2986388781113821274); } -void pose_H_mod_fun(double *state, double *out_3710500625877642434) { - H_mod_fun(state, out_3710500625877642434); +void pose_H_mod_fun(double *state, double *out_5449976102936300367) { + H_mod_fun(state, out_5449976102936300367); } -void pose_f_fun(double *state, double dt, double *out_5725984385602899057) { - f_fun(state, dt, out_5725984385602899057); +void pose_f_fun(double *state, double dt, double *out_2001314774583974236) { + f_fun(state, dt, out_2001314774583974236); } -void pose_F_fun(double *state, double dt, double *out_3503616768446574438) { - F_fun(state, dt, out_3503616768446574438); +void pose_F_fun(double *state, double dt, double *out_173210708380746968) { + F_fun(state, dt, out_173210708380746968); } -void pose_h_4(double *state, double *unused, double *out_3803432647309987328) { - h_4(state, unused, out_3803432647309987328); +void pose_h_4(double *state, double *unused, double *out_4290451809527946304) { + h_4(state, unused, out_4290451809527946304); } -void pose_H_4(double *state, double *unused, double *out_7950155699686530755) { - H_4(state, unused, out_7950155699686530755); +void pose_H_4(double *state, double *unused, double *out_4695815100881587760) { + H_4(state, unused, out_4695815100881587760); } -void pose_h_10(double *state, double *unused, double *out_477421004614984104) { - h_10(state, unused, out_477421004614984104); +void pose_h_10(double *state, double *unused, double *out_6723857678965398132) { + h_10(state, unused, out_6723857678965398132); } -void pose_H_10(double *state, double *unused, double *out_8536057142137197060) { - H_10(state, unused, out_8536057142137197060); +void pose_H_10(double *state, double *unused, double *out_984080189925557249) { + H_10(state, unused, out_984080189925557249); } -void pose_h_13(double *state, double *unused, double *out_1243321013145270066) { - h_13(state, unused, out_1243321013145270066); +void pose_h_13(double *state, double *unused, double *out_1838091537859337419) { + h_13(state, unused, out_1838091537859337419); } -void pose_H_13(double *state, double *unused, double *out_8514757619368374859) { - H_13(state, unused, out_8514757619368374859); +void pose_H_13(double *state, double *unused, double *out_1483541275549254959) { + H_13(state, unused, out_1483541275549254959); } -void pose_h_14(double *state, double *unused, double *out_7694485530011040401) { - h_14(state, unused, out_7694485530011040401); +void pose_h_14(double *state, double *unused, double *out_849503921054780572) { + h_14(state, unused, out_849503921054780572); } -void pose_H_14(double *state, double *unused, double *out_4867367267391158459) { - H_14(state, unused, out_4867367267391158459); +void pose_H_14(double *state, double *unused, double *out_732574244542103231) { + H_14(state, unused, out_732574244542103231); } 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 030a926d9..77d9b509e 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_4711793940499862270); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8790106481935061031); -void pose_H_mod_fun(double *state, double *out_3710500625877642434); -void pose_f_fun(double *state, double dt, double *out_5725984385602899057); -void pose_F_fun(double *state, double dt, double *out_3503616768446574438); -void pose_h_4(double *state, double *unused, double *out_3803432647309987328); -void pose_H_4(double *state, double *unused, double *out_7950155699686530755); -void pose_h_10(double *state, double *unused, double *out_477421004614984104); -void pose_H_10(double *state, double *unused, double *out_8536057142137197060); -void pose_h_13(double *state, double *unused, double *out_1243321013145270066); -void pose_H_13(double *state, double *unused, double *out_8514757619368374859); -void pose_h_14(double *state, double *unused, double *out_7694485530011040401); -void pose_H_14(double *state, double *unused, double *out_4867367267391158459); +void pose_err_fun(double *nom_x, double *delta_x, double *out_9044617002367965712); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2986388781113821274); +void pose_H_mod_fun(double *state, double *out_5449976102936300367); +void pose_f_fun(double *state, double dt, double *out_2001314774583974236); +void pose_F_fun(double *state, double dt, double *out_173210708380746968); +void pose_h_4(double *state, double *unused, double *out_4290451809527946304); +void pose_H_4(double *state, double *unused, double *out_4695815100881587760); +void pose_h_10(double *state, double *unused, double *out_6723857678965398132); +void pose_H_10(double *state, double *unused, double *out_984080189925557249); +void pose_h_13(double *state, double *unused, double *out_1838091537859337419); +void pose_H_13(double *state, double *unused, double *out_1483541275549254959); +void pose_h_14(double *state, double *unused, double *out_849503921054780572); +void pose_H_14(double *state, double *unused, double *out_732574244542103231); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 039f9a996..329b71766 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 8940bc2f5..85774c2e7 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 1885d55f1..f73b32266 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 2464eb7c2..6026381b0 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index b7f024f70..039b0c728 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index cd78b5605..9dab54c83 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ