diff --git a/common/libcommon.a b/common/libcommon.a index 8654ee9b0..f70de9b17 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 d5ba12b04..8b03acf06 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 c0fa04a60..7d3b0db07 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 16d821938..065db4bd0 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 e640ccac2..77b723c75 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 d82258848..1c8455266 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 c91e94f83..1af84fbf9 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 5f6f92f12..a7b06c6a2 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 8a1342fed..f2f0dd61d 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 8a1342fed..f2f0dd61d 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 6b9f85095..343d4bc07 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 5f6f92f12..a7b06c6a2 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 af52420b6..031f5cce6 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-6f569061-DEBUG"; +const uint8_t gitversion[19] = "DEV-5d86b7ac-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 9dfb418c4..da493d2c0 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 f4d9df9a0..f5c5b8fb3 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 52d8061f1..4612e65bd 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 bee2ab7b2..d4cf273d4 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 3b65bb8b1..076bee071 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 c0627a145..5db61ec0e 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 8f15f4e2e..fa7209c93 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 db592b565..08b3658ef 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 10fbe3226..bb9abcf50 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 0ba8b6c59..2c6a253e4 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 bbfae5f58..b92664832 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 9126faf51..beade5791 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 dbf26ecd3..0e1d7cc4d 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 fbd1f2024..4681809cf 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 2678c31f1..833870e61 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 78bf75f75..da7ffe939 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 831760408..ba132820c 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 f92591c14..ca230f308 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 d553ce7fb..4661088bd 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 85626654b..762799d5a 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 c49b51a27..4eef293e1 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-6f569061-DEBUG \ No newline at end of file +DEV-5d86b7ac-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index b1e19043b..43fd583b2 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_6119760954545072171) { - out_6119760954545072171[0] = delta_x[0] + nom_x[0]; - out_6119760954545072171[1] = delta_x[1] + nom_x[1]; - out_6119760954545072171[2] = delta_x[2] + nom_x[2]; - out_6119760954545072171[3] = delta_x[3] + nom_x[3]; - out_6119760954545072171[4] = delta_x[4] + nom_x[4]; - out_6119760954545072171[5] = delta_x[5] + nom_x[5]; - out_6119760954545072171[6] = delta_x[6] + nom_x[6]; - out_6119760954545072171[7] = delta_x[7] + nom_x[7]; - out_6119760954545072171[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1626624007171975298) { + out_1626624007171975298[0] = delta_x[0] + nom_x[0]; + out_1626624007171975298[1] = delta_x[1] + nom_x[1]; + out_1626624007171975298[2] = delta_x[2] + nom_x[2]; + out_1626624007171975298[3] = delta_x[3] + nom_x[3]; + out_1626624007171975298[4] = delta_x[4] + nom_x[4]; + out_1626624007171975298[5] = delta_x[5] + nom_x[5]; + out_1626624007171975298[6] = delta_x[6] + nom_x[6]; + out_1626624007171975298[7] = delta_x[7] + nom_x[7]; + out_1626624007171975298[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3032957791392119605) { - out_3032957791392119605[0] = -nom_x[0] + true_x[0]; - out_3032957791392119605[1] = -nom_x[1] + true_x[1]; - out_3032957791392119605[2] = -nom_x[2] + true_x[2]; - out_3032957791392119605[3] = -nom_x[3] + true_x[3]; - out_3032957791392119605[4] = -nom_x[4] + true_x[4]; - out_3032957791392119605[5] = -nom_x[5] + true_x[5]; - out_3032957791392119605[6] = -nom_x[6] + true_x[6]; - out_3032957791392119605[7] = -nom_x[7] + true_x[7]; - out_3032957791392119605[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3841721694315715550) { + out_3841721694315715550[0] = -nom_x[0] + true_x[0]; + out_3841721694315715550[1] = -nom_x[1] + true_x[1]; + out_3841721694315715550[2] = -nom_x[2] + true_x[2]; + out_3841721694315715550[3] = -nom_x[3] + true_x[3]; + out_3841721694315715550[4] = -nom_x[4] + true_x[4]; + out_3841721694315715550[5] = -nom_x[5] + true_x[5]; + out_3841721694315715550[6] = -nom_x[6] + true_x[6]; + out_3841721694315715550[7] = -nom_x[7] + true_x[7]; + out_3841721694315715550[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_6225772227410316778) { - out_6225772227410316778[0] = 1.0; - out_6225772227410316778[1] = 0.0; - out_6225772227410316778[2] = 0.0; - out_6225772227410316778[3] = 0.0; - out_6225772227410316778[4] = 0.0; - out_6225772227410316778[5] = 0.0; - out_6225772227410316778[6] = 0.0; - out_6225772227410316778[7] = 0.0; - out_6225772227410316778[8] = 0.0; - out_6225772227410316778[9] = 0.0; - out_6225772227410316778[10] = 1.0; - out_6225772227410316778[11] = 0.0; - out_6225772227410316778[12] = 0.0; - out_6225772227410316778[13] = 0.0; - out_6225772227410316778[14] = 0.0; - out_6225772227410316778[15] = 0.0; - out_6225772227410316778[16] = 0.0; - out_6225772227410316778[17] = 0.0; - out_6225772227410316778[18] = 0.0; - out_6225772227410316778[19] = 0.0; - out_6225772227410316778[20] = 1.0; - out_6225772227410316778[21] = 0.0; - out_6225772227410316778[22] = 0.0; - out_6225772227410316778[23] = 0.0; - out_6225772227410316778[24] = 0.0; - out_6225772227410316778[25] = 0.0; - out_6225772227410316778[26] = 0.0; - out_6225772227410316778[27] = 0.0; - out_6225772227410316778[28] = 0.0; - out_6225772227410316778[29] = 0.0; - out_6225772227410316778[30] = 1.0; - out_6225772227410316778[31] = 0.0; - out_6225772227410316778[32] = 0.0; - out_6225772227410316778[33] = 0.0; - out_6225772227410316778[34] = 0.0; - out_6225772227410316778[35] = 0.0; - out_6225772227410316778[36] = 0.0; - out_6225772227410316778[37] = 0.0; - out_6225772227410316778[38] = 0.0; - out_6225772227410316778[39] = 0.0; - out_6225772227410316778[40] = 1.0; - out_6225772227410316778[41] = 0.0; - out_6225772227410316778[42] = 0.0; - out_6225772227410316778[43] = 0.0; - out_6225772227410316778[44] = 0.0; - out_6225772227410316778[45] = 0.0; - out_6225772227410316778[46] = 0.0; - out_6225772227410316778[47] = 0.0; - out_6225772227410316778[48] = 0.0; - out_6225772227410316778[49] = 0.0; - out_6225772227410316778[50] = 1.0; - out_6225772227410316778[51] = 0.0; - out_6225772227410316778[52] = 0.0; - out_6225772227410316778[53] = 0.0; - out_6225772227410316778[54] = 0.0; - out_6225772227410316778[55] = 0.0; - out_6225772227410316778[56] = 0.0; - out_6225772227410316778[57] = 0.0; - out_6225772227410316778[58] = 0.0; - out_6225772227410316778[59] = 0.0; - out_6225772227410316778[60] = 1.0; - out_6225772227410316778[61] = 0.0; - out_6225772227410316778[62] = 0.0; - out_6225772227410316778[63] = 0.0; - out_6225772227410316778[64] = 0.0; - out_6225772227410316778[65] = 0.0; - out_6225772227410316778[66] = 0.0; - out_6225772227410316778[67] = 0.0; - out_6225772227410316778[68] = 0.0; - out_6225772227410316778[69] = 0.0; - out_6225772227410316778[70] = 1.0; - out_6225772227410316778[71] = 0.0; - out_6225772227410316778[72] = 0.0; - out_6225772227410316778[73] = 0.0; - out_6225772227410316778[74] = 0.0; - out_6225772227410316778[75] = 0.0; - out_6225772227410316778[76] = 0.0; - out_6225772227410316778[77] = 0.0; - out_6225772227410316778[78] = 0.0; - out_6225772227410316778[79] = 0.0; - out_6225772227410316778[80] = 1.0; +void H_mod_fun(double *state, double *out_7646044064038577504) { + out_7646044064038577504[0] = 1.0; + out_7646044064038577504[1] = 0.0; + out_7646044064038577504[2] = 0.0; + out_7646044064038577504[3] = 0.0; + out_7646044064038577504[4] = 0.0; + out_7646044064038577504[5] = 0.0; + out_7646044064038577504[6] = 0.0; + out_7646044064038577504[7] = 0.0; + out_7646044064038577504[8] = 0.0; + out_7646044064038577504[9] = 0.0; + out_7646044064038577504[10] = 1.0; + out_7646044064038577504[11] = 0.0; + out_7646044064038577504[12] = 0.0; + out_7646044064038577504[13] = 0.0; + out_7646044064038577504[14] = 0.0; + out_7646044064038577504[15] = 0.0; + out_7646044064038577504[16] = 0.0; + out_7646044064038577504[17] = 0.0; + out_7646044064038577504[18] = 0.0; + out_7646044064038577504[19] = 0.0; + out_7646044064038577504[20] = 1.0; + out_7646044064038577504[21] = 0.0; + out_7646044064038577504[22] = 0.0; + out_7646044064038577504[23] = 0.0; + out_7646044064038577504[24] = 0.0; + out_7646044064038577504[25] = 0.0; + out_7646044064038577504[26] = 0.0; + out_7646044064038577504[27] = 0.0; + out_7646044064038577504[28] = 0.0; + out_7646044064038577504[29] = 0.0; + out_7646044064038577504[30] = 1.0; + out_7646044064038577504[31] = 0.0; + out_7646044064038577504[32] = 0.0; + out_7646044064038577504[33] = 0.0; + out_7646044064038577504[34] = 0.0; + out_7646044064038577504[35] = 0.0; + out_7646044064038577504[36] = 0.0; + out_7646044064038577504[37] = 0.0; + out_7646044064038577504[38] = 0.0; + out_7646044064038577504[39] = 0.0; + out_7646044064038577504[40] = 1.0; + out_7646044064038577504[41] = 0.0; + out_7646044064038577504[42] = 0.0; + out_7646044064038577504[43] = 0.0; + out_7646044064038577504[44] = 0.0; + out_7646044064038577504[45] = 0.0; + out_7646044064038577504[46] = 0.0; + out_7646044064038577504[47] = 0.0; + out_7646044064038577504[48] = 0.0; + out_7646044064038577504[49] = 0.0; + out_7646044064038577504[50] = 1.0; + out_7646044064038577504[51] = 0.0; + out_7646044064038577504[52] = 0.0; + out_7646044064038577504[53] = 0.0; + out_7646044064038577504[54] = 0.0; + out_7646044064038577504[55] = 0.0; + out_7646044064038577504[56] = 0.0; + out_7646044064038577504[57] = 0.0; + out_7646044064038577504[58] = 0.0; + out_7646044064038577504[59] = 0.0; + out_7646044064038577504[60] = 1.0; + out_7646044064038577504[61] = 0.0; + out_7646044064038577504[62] = 0.0; + out_7646044064038577504[63] = 0.0; + out_7646044064038577504[64] = 0.0; + out_7646044064038577504[65] = 0.0; + out_7646044064038577504[66] = 0.0; + out_7646044064038577504[67] = 0.0; + out_7646044064038577504[68] = 0.0; + out_7646044064038577504[69] = 0.0; + out_7646044064038577504[70] = 1.0; + out_7646044064038577504[71] = 0.0; + out_7646044064038577504[72] = 0.0; + out_7646044064038577504[73] = 0.0; + out_7646044064038577504[74] = 0.0; + out_7646044064038577504[75] = 0.0; + out_7646044064038577504[76] = 0.0; + out_7646044064038577504[77] = 0.0; + out_7646044064038577504[78] = 0.0; + out_7646044064038577504[79] = 0.0; + out_7646044064038577504[80] = 1.0; } -void f_fun(double *state, double dt, double *out_1791723405675881227) { - out_1791723405675881227[0] = state[0]; - out_1791723405675881227[1] = state[1]; - out_1791723405675881227[2] = state[2]; - out_1791723405675881227[3] = state[3]; - out_1791723405675881227[4] = state[4]; - out_1791723405675881227[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_1791723405675881227[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_1791723405675881227[7] = state[7]; - out_1791723405675881227[8] = state[8]; +void f_fun(double *state, double dt, double *out_3692802059814366209) { + out_3692802059814366209[0] = state[0]; + out_3692802059814366209[1] = state[1]; + out_3692802059814366209[2] = state[2]; + out_3692802059814366209[3] = state[3]; + out_3692802059814366209[4] = state[4]; + out_3692802059814366209[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_3692802059814366209[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_3692802059814366209[7] = state[7]; + out_3692802059814366209[8] = state[8]; } -void F_fun(double *state, double dt, double *out_4540850954218753986) { - out_4540850954218753986[0] = 1; - out_4540850954218753986[1] = 0; - out_4540850954218753986[2] = 0; - out_4540850954218753986[3] = 0; - out_4540850954218753986[4] = 0; - out_4540850954218753986[5] = 0; - out_4540850954218753986[6] = 0; - out_4540850954218753986[7] = 0; - out_4540850954218753986[8] = 0; - out_4540850954218753986[9] = 0; - out_4540850954218753986[10] = 1; - out_4540850954218753986[11] = 0; - out_4540850954218753986[12] = 0; - out_4540850954218753986[13] = 0; - out_4540850954218753986[14] = 0; - out_4540850954218753986[15] = 0; - out_4540850954218753986[16] = 0; - out_4540850954218753986[17] = 0; - out_4540850954218753986[18] = 0; - out_4540850954218753986[19] = 0; - out_4540850954218753986[20] = 1; - out_4540850954218753986[21] = 0; - out_4540850954218753986[22] = 0; - out_4540850954218753986[23] = 0; - out_4540850954218753986[24] = 0; - out_4540850954218753986[25] = 0; - out_4540850954218753986[26] = 0; - out_4540850954218753986[27] = 0; - out_4540850954218753986[28] = 0; - out_4540850954218753986[29] = 0; - out_4540850954218753986[30] = 1; - out_4540850954218753986[31] = 0; - out_4540850954218753986[32] = 0; - out_4540850954218753986[33] = 0; - out_4540850954218753986[34] = 0; - out_4540850954218753986[35] = 0; - out_4540850954218753986[36] = 0; - out_4540850954218753986[37] = 0; - out_4540850954218753986[38] = 0; - out_4540850954218753986[39] = 0; - out_4540850954218753986[40] = 1; - out_4540850954218753986[41] = 0; - out_4540850954218753986[42] = 0; - out_4540850954218753986[43] = 0; - out_4540850954218753986[44] = 0; - out_4540850954218753986[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_4540850954218753986[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_4540850954218753986[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4540850954218753986[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4540850954218753986[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_4540850954218753986[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_4540850954218753986[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_4540850954218753986[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_4540850954218753986[53] = -9.8100000000000005*dt; - out_4540850954218753986[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_4540850954218753986[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_4540850954218753986[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4540850954218753986[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4540850954218753986[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_4540850954218753986[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_4540850954218753986[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_4540850954218753986[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4540850954218753986[62] = 0; - out_4540850954218753986[63] = 0; - out_4540850954218753986[64] = 0; - out_4540850954218753986[65] = 0; - out_4540850954218753986[66] = 0; - out_4540850954218753986[67] = 0; - out_4540850954218753986[68] = 0; - out_4540850954218753986[69] = 0; - out_4540850954218753986[70] = 1; - out_4540850954218753986[71] = 0; - out_4540850954218753986[72] = 0; - out_4540850954218753986[73] = 0; - out_4540850954218753986[74] = 0; - out_4540850954218753986[75] = 0; - out_4540850954218753986[76] = 0; - out_4540850954218753986[77] = 0; - out_4540850954218753986[78] = 0; - out_4540850954218753986[79] = 0; - out_4540850954218753986[80] = 1; +void F_fun(double *state, double dt, double *out_6278869713612932388) { + out_6278869713612932388[0] = 1; + out_6278869713612932388[1] = 0; + out_6278869713612932388[2] = 0; + out_6278869713612932388[3] = 0; + out_6278869713612932388[4] = 0; + out_6278869713612932388[5] = 0; + out_6278869713612932388[6] = 0; + out_6278869713612932388[7] = 0; + out_6278869713612932388[8] = 0; + out_6278869713612932388[9] = 0; + out_6278869713612932388[10] = 1; + out_6278869713612932388[11] = 0; + out_6278869713612932388[12] = 0; + out_6278869713612932388[13] = 0; + out_6278869713612932388[14] = 0; + out_6278869713612932388[15] = 0; + out_6278869713612932388[16] = 0; + out_6278869713612932388[17] = 0; + out_6278869713612932388[18] = 0; + out_6278869713612932388[19] = 0; + out_6278869713612932388[20] = 1; + out_6278869713612932388[21] = 0; + out_6278869713612932388[22] = 0; + out_6278869713612932388[23] = 0; + out_6278869713612932388[24] = 0; + out_6278869713612932388[25] = 0; + out_6278869713612932388[26] = 0; + out_6278869713612932388[27] = 0; + out_6278869713612932388[28] = 0; + out_6278869713612932388[29] = 0; + out_6278869713612932388[30] = 1; + out_6278869713612932388[31] = 0; + out_6278869713612932388[32] = 0; + out_6278869713612932388[33] = 0; + out_6278869713612932388[34] = 0; + out_6278869713612932388[35] = 0; + out_6278869713612932388[36] = 0; + out_6278869713612932388[37] = 0; + out_6278869713612932388[38] = 0; + out_6278869713612932388[39] = 0; + out_6278869713612932388[40] = 1; + out_6278869713612932388[41] = 0; + out_6278869713612932388[42] = 0; + out_6278869713612932388[43] = 0; + out_6278869713612932388[44] = 0; + out_6278869713612932388[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_6278869713612932388[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_6278869713612932388[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_6278869713612932388[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_6278869713612932388[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_6278869713612932388[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_6278869713612932388[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_6278869713612932388[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_6278869713612932388[53] = -9.8100000000000005*dt; + out_6278869713612932388[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_6278869713612932388[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_6278869713612932388[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_6278869713612932388[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_6278869713612932388[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_6278869713612932388[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_6278869713612932388[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_6278869713612932388[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_6278869713612932388[62] = 0; + out_6278869713612932388[63] = 0; + out_6278869713612932388[64] = 0; + out_6278869713612932388[65] = 0; + out_6278869713612932388[66] = 0; + out_6278869713612932388[67] = 0; + out_6278869713612932388[68] = 0; + out_6278869713612932388[69] = 0; + out_6278869713612932388[70] = 1; + out_6278869713612932388[71] = 0; + out_6278869713612932388[72] = 0; + out_6278869713612932388[73] = 0; + out_6278869713612932388[74] = 0; + out_6278869713612932388[75] = 0; + out_6278869713612932388[76] = 0; + out_6278869713612932388[77] = 0; + out_6278869713612932388[78] = 0; + out_6278869713612932388[79] = 0; + out_6278869713612932388[80] = 1; } -void h_25(double *state, double *unused, double *out_6713287060643499630) { - out_6713287060643499630[0] = state[6]; +void h_25(double *state, double *unused, double *out_1903293770744417105) { + out_1903293770744417105[0] = state[6]; } -void H_25(double *state, double *unused, double *out_2967415998501920459) { - out_2967415998501920459[0] = 0; - out_2967415998501920459[1] = 0; - out_2967415998501920459[2] = 0; - out_2967415998501920459[3] = 0; - out_2967415998501920459[4] = 0; - out_2967415998501920459[5] = 0; - out_2967415998501920459[6] = 1; - out_2967415998501920459[7] = 0; - out_2967415998501920459[8] = 0; +void H_25(double *state, double *unused, double *out_1607511783758736875) { + out_1607511783758736875[0] = 0; + out_1607511783758736875[1] = 0; + out_1607511783758736875[2] = 0; + out_1607511783758736875[3] = 0; + out_1607511783758736875[4] = 0; + out_1607511783758736875[5] = 0; + out_1607511783758736875[6] = 1; + out_1607511783758736875[7] = 0; + out_1607511783758736875[8] = 0; } -void h_24(double *state, double *unused, double *out_7006841003408788110) { - out_7006841003408788110[0] = state[4]; - out_7006841003408788110[1] = state[5]; +void h_24(double *state, double *unused, double *out_3019943109680816786) { + out_3019943109680816786[0] = state[4]; + out_3019943109680816786[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5140065597507420025) { - out_5140065597507420025[0] = 0; - out_5140065597507420025[1] = 0; - out_5140065597507420025[2] = 0; - out_5140065597507420025[3] = 0; - out_5140065597507420025[4] = 1; - out_5140065597507420025[5] = 0; - out_5140065597507420025[6] = 0; - out_5140065597507420025[7] = 0; - out_5140065597507420025[8] = 0; - out_5140065597507420025[9] = 0; - out_5140065597507420025[10] = 0; - out_5140065597507420025[11] = 0; - out_5140065597507420025[12] = 0; - out_5140065597507420025[13] = 0; - out_5140065597507420025[14] = 1; - out_5140065597507420025[15] = 0; - out_5140065597507420025[16] = 0; - out_5140065597507420025[17] = 0; +void H_24(double *state, double *unused, double *out_565137815246762691) { + out_565137815246762691[0] = 0; + out_565137815246762691[1] = 0; + out_565137815246762691[2] = 0; + out_565137815246762691[3] = 0; + out_565137815246762691[4] = 1; + out_565137815246762691[5] = 0; + out_565137815246762691[6] = 0; + out_565137815246762691[7] = 0; + out_565137815246762691[8] = 0; + out_565137815246762691[9] = 0; + out_565137815246762691[10] = 0; + out_565137815246762691[11] = 0; + out_565137815246762691[12] = 0; + out_565137815246762691[13] = 0; + out_565137815246762691[14] = 1; + out_565137815246762691[15] = 0; + out_565137815246762691[16] = 0; + out_565137815246762691[17] = 0; } -void h_30(double *state, double *unused, double *out_57548165706851306) { - out_57548165706851306[0] = state[4]; +void h_30(double *state, double *unused, double *out_5540743298998439455) { + out_5540743298998439455[0] = state[4]; } -void H_30(double *state, double *unused, double *out_449083039994671832) { - out_449083039994671832[0] = 0; - out_449083039994671832[1] = 0; - out_449083039994671832[2] = 0; - out_449083039994671832[3] = 0; - out_449083039994671832[4] = 1; - out_449083039994671832[5] = 0; - out_449083039994671832[6] = 0; - out_449083039994671832[7] = 0; - out_449083039994671832[8] = 0; +void H_30(double *state, double *unused, double *out_4125844742265985502) { + out_4125844742265985502[0] = 0; + out_4125844742265985502[1] = 0; + out_4125844742265985502[2] = 0; + out_4125844742265985502[3] = 0; + out_4125844742265985502[4] = 1; + out_4125844742265985502[5] = 0; + out_4125844742265985502[6] = 0; + out_4125844742265985502[7] = 0; + out_4125844742265985502[8] = 0; } -void h_26(double *state, double *unused, double *out_5284319070179179257) { - out_5284319070179179257[0] = state[7]; +void h_26(double *state, double *unused, double *out_539070031337593810) { + out_539070031337593810[0] = state[7]; } -void H_26(double *state, double *unused, double *out_6708919317375976683) { - out_6708919317375976683[0] = 0; - out_6708919317375976683[1] = 0; - out_6708919317375976683[2] = 0; - out_6708919317375976683[3] = 0; - out_6708919317375976683[4] = 0; - out_6708919317375976683[5] = 0; - out_6708919317375976683[6] = 0; - out_6708919317375976683[7] = 1; - out_6708919317375976683[8] = 0; +void H_26(double *state, double *unused, double *out_2133991535115319349) { + out_2133991535115319349[0] = 0; + out_2133991535115319349[1] = 0; + out_2133991535115319349[2] = 0; + out_2133991535115319349[3] = 0; + out_2133991535115319349[4] = 0; + out_2133991535115319349[5] = 0; + out_2133991535115319349[6] = 0; + out_2133991535115319349[7] = 1; + out_2133991535115319349[8] = 0; } -void h_27(double *state, double *unused, double *out_2937147548800054846) { - out_2937147548800054846[0] = state[3]; +void h_27(double *state, double *unused, double *out_7078675401262423660) { + out_7078675401262423660[0] = state[3]; } -void H_27(double *state, double *unused, double *out_2623846351795096743) { - out_2623846351795096743[0] = 0; - out_2623846351795096743[1] = 0; - out_2623846351795096743[2] = 0; - out_2623846351795096743[3] = 1; - out_2623846351795096743[4] = 0; - out_2623846351795096743[5] = 0; - out_2623846351795096743[6] = 0; - out_2623846351795096743[7] = 0; - out_2623846351795096743[8] = 0; +void H_27(double *state, double *unused, double *out_1951081430465560591) { + out_1951081430465560591[0] = 0; + out_1951081430465560591[1] = 0; + out_1951081430465560591[2] = 0; + out_1951081430465560591[3] = 1; + out_1951081430465560591[4] = 0; + out_1951081430465560591[5] = 0; + out_1951081430465560591[6] = 0; + out_1951081430465560591[7] = 0; + out_1951081430465560591[8] = 0; } -void h_29(double *state, double *unused, double *out_3094334217151183991) { - out_3094334217151183991[0] = state[1]; +void h_29(double *state, double *unused, double *out_4597233446096777020) { + out_4597233446096777020[0] = state[1]; } -void H_29(double *state, double *unused, double *out_61148304319720352) { - out_61148304319720352[0] = 0; - out_61148304319720352[1] = 1; - out_61148304319720352[2] = 0; - out_61148304319720352[3] = 0; - out_61148304319720352[4] = 0; - out_61148304319720352[5] = 0; - out_61148304319720352[6] = 0; - out_61148304319720352[7] = 0; - out_61148304319720352[8] = 0; +void H_29(double *state, double *unused, double *out_4636076086580377686) { + out_4636076086580377686[0] = 0; + out_4636076086580377686[1] = 1; + out_4636076086580377686[2] = 0; + out_4636076086580377686[3] = 0; + out_4636076086580377686[4] = 0; + out_4636076086580377686[5] = 0; + out_4636076086580377686[6] = 0; + out_4636076086580377686[7] = 0; + out_4636076086580377686[8] = 0; } -void h_28(double *state, double *unused, double *out_2064525718860790799) { - out_2064525718860790799[0] = state[0]; +void h_28(double *state, double *unused, double *out_2077002133601578015) { + out_2077002133601578015[0] = state[0]; } -void H_28(double *state, double *unused, double *out_5021250712749810222) { - out_5021250712749810222[0] = 1; - out_5021250712749810222[1] = 0; - out_5021250712749810222[2] = 0; - out_5021250712749810222[3] = 0; - out_5021250712749810222[4] = 0; - out_5021250712749810222[5] = 0; - out_5021250712749810222[6] = 0; - out_5021250712749810222[7] = 0; - out_5021250712749810222[8] = 0; +void H_28(double *state, double *unused, double *out_446322930489152888) { + out_446322930489152888[0] = 1; + out_446322930489152888[1] = 0; + out_446322930489152888[2] = 0; + out_446322930489152888[3] = 0; + out_446322930489152888[4] = 0; + out_446322930489152888[5] = 0; + out_446322930489152888[6] = 0; + out_446322930489152888[7] = 0; + out_446322930489152888[8] = 0; } -void h_31(double *state, double *unused, double *out_6870473728994628775) { - out_6870473728994628775[0] = state[8]; +void h_31(double *state, double *unused, double *out_7817026797415165478) { + out_7817026797415165478[0] = state[8]; } -void H_31(double *state, double *unused, double *out_7335127419609328159) { - out_7335127419609328159[0] = 0; - out_7335127419609328159[1] = 0; - out_7335127419609328159[2] = 0; - out_7335127419609328159[3] = 0; - out_7335127419609328159[4] = 0; - out_7335127419609328159[5] = 0; - out_7335127419609328159[6] = 0; - out_7335127419609328159[7] = 0; - out_7335127419609328159[8] = 1; +void H_31(double *state, double *unused, double *out_2760199637348670825) { + out_2760199637348670825[0] = 0; + out_2760199637348670825[1] = 0; + out_2760199637348670825[2] = 0; + out_2760199637348670825[3] = 0; + out_2760199637348670825[4] = 0; + out_2760199637348670825[5] = 0; + out_2760199637348670825[6] = 0; + out_2760199637348670825[7] = 0; + out_2760199637348670825[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_6119760954545072171) { - err_fun(nom_x, delta_x, out_6119760954545072171); +void car_err_fun(double *nom_x, double *delta_x, double *out_1626624007171975298) { + err_fun(nom_x, delta_x, out_1626624007171975298); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3032957791392119605) { - inv_err_fun(nom_x, true_x, out_3032957791392119605); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3841721694315715550) { + inv_err_fun(nom_x, true_x, out_3841721694315715550); } -void car_H_mod_fun(double *state, double *out_6225772227410316778) { - H_mod_fun(state, out_6225772227410316778); +void car_H_mod_fun(double *state, double *out_7646044064038577504) { + H_mod_fun(state, out_7646044064038577504); } -void car_f_fun(double *state, double dt, double *out_1791723405675881227) { - f_fun(state, dt, out_1791723405675881227); +void car_f_fun(double *state, double dt, double *out_3692802059814366209) { + f_fun(state, dt, out_3692802059814366209); } -void car_F_fun(double *state, double dt, double *out_4540850954218753986) { - F_fun(state, dt, out_4540850954218753986); +void car_F_fun(double *state, double dt, double *out_6278869713612932388) { + F_fun(state, dt, out_6278869713612932388); } -void car_h_25(double *state, double *unused, double *out_6713287060643499630) { - h_25(state, unused, out_6713287060643499630); +void car_h_25(double *state, double *unused, double *out_1903293770744417105) { + h_25(state, unused, out_1903293770744417105); } -void car_H_25(double *state, double *unused, double *out_2967415998501920459) { - H_25(state, unused, out_2967415998501920459); +void car_H_25(double *state, double *unused, double *out_1607511783758736875) { + H_25(state, unused, out_1607511783758736875); } -void car_h_24(double *state, double *unused, double *out_7006841003408788110) { - h_24(state, unused, out_7006841003408788110); +void car_h_24(double *state, double *unused, double *out_3019943109680816786) { + h_24(state, unused, out_3019943109680816786); } -void car_H_24(double *state, double *unused, double *out_5140065597507420025) { - H_24(state, unused, out_5140065597507420025); +void car_H_24(double *state, double *unused, double *out_565137815246762691) { + H_24(state, unused, out_565137815246762691); } -void car_h_30(double *state, double *unused, double *out_57548165706851306) { - h_30(state, unused, out_57548165706851306); +void car_h_30(double *state, double *unused, double *out_5540743298998439455) { + h_30(state, unused, out_5540743298998439455); } -void car_H_30(double *state, double *unused, double *out_449083039994671832) { - H_30(state, unused, out_449083039994671832); +void car_H_30(double *state, double *unused, double *out_4125844742265985502) { + H_30(state, unused, out_4125844742265985502); } -void car_h_26(double *state, double *unused, double *out_5284319070179179257) { - h_26(state, unused, out_5284319070179179257); +void car_h_26(double *state, double *unused, double *out_539070031337593810) { + h_26(state, unused, out_539070031337593810); } -void car_H_26(double *state, double *unused, double *out_6708919317375976683) { - H_26(state, unused, out_6708919317375976683); +void car_H_26(double *state, double *unused, double *out_2133991535115319349) { + H_26(state, unused, out_2133991535115319349); } -void car_h_27(double *state, double *unused, double *out_2937147548800054846) { - h_27(state, unused, out_2937147548800054846); +void car_h_27(double *state, double *unused, double *out_7078675401262423660) { + h_27(state, unused, out_7078675401262423660); } -void car_H_27(double *state, double *unused, double *out_2623846351795096743) { - H_27(state, unused, out_2623846351795096743); +void car_H_27(double *state, double *unused, double *out_1951081430465560591) { + H_27(state, unused, out_1951081430465560591); } -void car_h_29(double *state, double *unused, double *out_3094334217151183991) { - h_29(state, unused, out_3094334217151183991); +void car_h_29(double *state, double *unused, double *out_4597233446096777020) { + h_29(state, unused, out_4597233446096777020); } -void car_H_29(double *state, double *unused, double *out_61148304319720352) { - H_29(state, unused, out_61148304319720352); +void car_H_29(double *state, double *unused, double *out_4636076086580377686) { + H_29(state, unused, out_4636076086580377686); } -void car_h_28(double *state, double *unused, double *out_2064525718860790799) { - h_28(state, unused, out_2064525718860790799); +void car_h_28(double *state, double *unused, double *out_2077002133601578015) { + h_28(state, unused, out_2077002133601578015); } -void car_H_28(double *state, double *unused, double *out_5021250712749810222) { - H_28(state, unused, out_5021250712749810222); +void car_H_28(double *state, double *unused, double *out_446322930489152888) { + H_28(state, unused, out_446322930489152888); } -void car_h_31(double *state, double *unused, double *out_6870473728994628775) { - h_31(state, unused, out_6870473728994628775); +void car_h_31(double *state, double *unused, double *out_7817026797415165478) { + h_31(state, unused, out_7817026797415165478); } -void car_H_31(double *state, double *unused, double *out_7335127419609328159) { - H_31(state, unused, out_7335127419609328159); +void car_H_31(double *state, double *unused, double *out_2760199637348670825) { + H_31(state, unused, out_2760199637348670825); } 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 0c8bfa0c4..f3916ddaf 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_6119760954545072171); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3032957791392119605); -void car_H_mod_fun(double *state, double *out_6225772227410316778); -void car_f_fun(double *state, double dt, double *out_1791723405675881227); -void car_F_fun(double *state, double dt, double *out_4540850954218753986); -void car_h_25(double *state, double *unused, double *out_6713287060643499630); -void car_H_25(double *state, double *unused, double *out_2967415998501920459); -void car_h_24(double *state, double *unused, double *out_7006841003408788110); -void car_H_24(double *state, double *unused, double *out_5140065597507420025); -void car_h_30(double *state, double *unused, double *out_57548165706851306); -void car_H_30(double *state, double *unused, double *out_449083039994671832); -void car_h_26(double *state, double *unused, double *out_5284319070179179257); -void car_H_26(double *state, double *unused, double *out_6708919317375976683); -void car_h_27(double *state, double *unused, double *out_2937147548800054846); -void car_H_27(double *state, double *unused, double *out_2623846351795096743); -void car_h_29(double *state, double *unused, double *out_3094334217151183991); -void car_H_29(double *state, double *unused, double *out_61148304319720352); -void car_h_28(double *state, double *unused, double *out_2064525718860790799); -void car_H_28(double *state, double *unused, double *out_5021250712749810222); -void car_h_31(double *state, double *unused, double *out_6870473728994628775); -void car_H_31(double *state, double *unused, double *out_7335127419609328159); +void car_err_fun(double *nom_x, double *delta_x, double *out_1626624007171975298); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3841721694315715550); +void car_H_mod_fun(double *state, double *out_7646044064038577504); +void car_f_fun(double *state, double dt, double *out_3692802059814366209); +void car_F_fun(double *state, double dt, double *out_6278869713612932388); +void car_h_25(double *state, double *unused, double *out_1903293770744417105); +void car_H_25(double *state, double *unused, double *out_1607511783758736875); +void car_h_24(double *state, double *unused, double *out_3019943109680816786); +void car_H_24(double *state, double *unused, double *out_565137815246762691); +void car_h_30(double *state, double *unused, double *out_5540743298998439455); +void car_H_30(double *state, double *unused, double *out_4125844742265985502); +void car_h_26(double *state, double *unused, double *out_539070031337593810); +void car_H_26(double *state, double *unused, double *out_2133991535115319349); +void car_h_27(double *state, double *unused, double *out_7078675401262423660); +void car_H_27(double *state, double *unused, double *out_1951081430465560591); +void car_h_29(double *state, double *unused, double *out_4597233446096777020); +void car_H_29(double *state, double *unused, double *out_4636076086580377686); +void car_h_28(double *state, double *unused, double *out_2077002133601578015); +void car_H_28(double *state, double *unused, double *out_446322930489152888); +void car_h_31(double *state, double *unused, double *out_7817026797415165478); +void car_H_31(double *state, double *unused, double *out_2760199637348670825); 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 da7e03d72..7e8794158 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_4567767818172860295) { - out_4567767818172860295[0] = delta_x[0] + nom_x[0]; - out_4567767818172860295[1] = delta_x[1] + nom_x[1]; - out_4567767818172860295[2] = delta_x[2] + nom_x[2]; - out_4567767818172860295[3] = delta_x[3] + nom_x[3]; - out_4567767818172860295[4] = delta_x[4] + nom_x[4]; - out_4567767818172860295[5] = delta_x[5] + nom_x[5]; - out_4567767818172860295[6] = delta_x[6] + nom_x[6]; - out_4567767818172860295[7] = delta_x[7] + nom_x[7]; - out_4567767818172860295[8] = delta_x[8] + nom_x[8]; - out_4567767818172860295[9] = delta_x[9] + nom_x[9]; - out_4567767818172860295[10] = delta_x[10] + nom_x[10]; - out_4567767818172860295[11] = delta_x[11] + nom_x[11]; - out_4567767818172860295[12] = delta_x[12] + nom_x[12]; - out_4567767818172860295[13] = delta_x[13] + nom_x[13]; - out_4567767818172860295[14] = delta_x[14] + nom_x[14]; - out_4567767818172860295[15] = delta_x[15] + nom_x[15]; - out_4567767818172860295[16] = delta_x[16] + nom_x[16]; - out_4567767818172860295[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_1277882425838003162) { + out_1277882425838003162[0] = delta_x[0] + nom_x[0]; + out_1277882425838003162[1] = delta_x[1] + nom_x[1]; + out_1277882425838003162[2] = delta_x[2] + nom_x[2]; + out_1277882425838003162[3] = delta_x[3] + nom_x[3]; + out_1277882425838003162[4] = delta_x[4] + nom_x[4]; + out_1277882425838003162[5] = delta_x[5] + nom_x[5]; + out_1277882425838003162[6] = delta_x[6] + nom_x[6]; + out_1277882425838003162[7] = delta_x[7] + nom_x[7]; + out_1277882425838003162[8] = delta_x[8] + nom_x[8]; + out_1277882425838003162[9] = delta_x[9] + nom_x[9]; + out_1277882425838003162[10] = delta_x[10] + nom_x[10]; + out_1277882425838003162[11] = delta_x[11] + nom_x[11]; + out_1277882425838003162[12] = delta_x[12] + nom_x[12]; + out_1277882425838003162[13] = delta_x[13] + nom_x[13]; + out_1277882425838003162[14] = delta_x[14] + nom_x[14]; + out_1277882425838003162[15] = delta_x[15] + nom_x[15]; + out_1277882425838003162[16] = delta_x[16] + nom_x[16]; + out_1277882425838003162[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4613807108104960206) { - out_4613807108104960206[0] = -nom_x[0] + true_x[0]; - out_4613807108104960206[1] = -nom_x[1] + true_x[1]; - out_4613807108104960206[2] = -nom_x[2] + true_x[2]; - out_4613807108104960206[3] = -nom_x[3] + true_x[3]; - out_4613807108104960206[4] = -nom_x[4] + true_x[4]; - out_4613807108104960206[5] = -nom_x[5] + true_x[5]; - out_4613807108104960206[6] = -nom_x[6] + true_x[6]; - out_4613807108104960206[7] = -nom_x[7] + true_x[7]; - out_4613807108104960206[8] = -nom_x[8] + true_x[8]; - out_4613807108104960206[9] = -nom_x[9] + true_x[9]; - out_4613807108104960206[10] = -nom_x[10] + true_x[10]; - out_4613807108104960206[11] = -nom_x[11] + true_x[11]; - out_4613807108104960206[12] = -nom_x[12] + true_x[12]; - out_4613807108104960206[13] = -nom_x[13] + true_x[13]; - out_4613807108104960206[14] = -nom_x[14] + true_x[14]; - out_4613807108104960206[15] = -nom_x[15] + true_x[15]; - out_4613807108104960206[16] = -nom_x[16] + true_x[16]; - out_4613807108104960206[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5395183656796766) { + out_5395183656796766[0] = -nom_x[0] + true_x[0]; + out_5395183656796766[1] = -nom_x[1] + true_x[1]; + out_5395183656796766[2] = -nom_x[2] + true_x[2]; + out_5395183656796766[3] = -nom_x[3] + true_x[3]; + out_5395183656796766[4] = -nom_x[4] + true_x[4]; + out_5395183656796766[5] = -nom_x[5] + true_x[5]; + out_5395183656796766[6] = -nom_x[6] + true_x[6]; + out_5395183656796766[7] = -nom_x[7] + true_x[7]; + out_5395183656796766[8] = -nom_x[8] + true_x[8]; + out_5395183656796766[9] = -nom_x[9] + true_x[9]; + out_5395183656796766[10] = -nom_x[10] + true_x[10]; + out_5395183656796766[11] = -nom_x[11] + true_x[11]; + out_5395183656796766[12] = -nom_x[12] + true_x[12]; + out_5395183656796766[13] = -nom_x[13] + true_x[13]; + out_5395183656796766[14] = -nom_x[14] + true_x[14]; + out_5395183656796766[15] = -nom_x[15] + true_x[15]; + out_5395183656796766[16] = -nom_x[16] + true_x[16]; + out_5395183656796766[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_6572770099193808423) { - out_6572770099193808423[0] = 1.0; - out_6572770099193808423[1] = 0.0; - out_6572770099193808423[2] = 0.0; - out_6572770099193808423[3] = 0.0; - out_6572770099193808423[4] = 0.0; - out_6572770099193808423[5] = 0.0; - out_6572770099193808423[6] = 0.0; - out_6572770099193808423[7] = 0.0; - out_6572770099193808423[8] = 0.0; - out_6572770099193808423[9] = 0.0; - out_6572770099193808423[10] = 0.0; - out_6572770099193808423[11] = 0.0; - out_6572770099193808423[12] = 0.0; - out_6572770099193808423[13] = 0.0; - out_6572770099193808423[14] = 0.0; - out_6572770099193808423[15] = 0.0; - out_6572770099193808423[16] = 0.0; - out_6572770099193808423[17] = 0.0; - out_6572770099193808423[18] = 0.0; - out_6572770099193808423[19] = 1.0; - out_6572770099193808423[20] = 0.0; - out_6572770099193808423[21] = 0.0; - out_6572770099193808423[22] = 0.0; - out_6572770099193808423[23] = 0.0; - out_6572770099193808423[24] = 0.0; - out_6572770099193808423[25] = 0.0; - out_6572770099193808423[26] = 0.0; - out_6572770099193808423[27] = 0.0; - out_6572770099193808423[28] = 0.0; - out_6572770099193808423[29] = 0.0; - out_6572770099193808423[30] = 0.0; - out_6572770099193808423[31] = 0.0; - out_6572770099193808423[32] = 0.0; - out_6572770099193808423[33] = 0.0; - out_6572770099193808423[34] = 0.0; - out_6572770099193808423[35] = 0.0; - out_6572770099193808423[36] = 0.0; - out_6572770099193808423[37] = 0.0; - out_6572770099193808423[38] = 1.0; - out_6572770099193808423[39] = 0.0; - out_6572770099193808423[40] = 0.0; - out_6572770099193808423[41] = 0.0; - out_6572770099193808423[42] = 0.0; - out_6572770099193808423[43] = 0.0; - out_6572770099193808423[44] = 0.0; - out_6572770099193808423[45] = 0.0; - out_6572770099193808423[46] = 0.0; - out_6572770099193808423[47] = 0.0; - out_6572770099193808423[48] = 0.0; - out_6572770099193808423[49] = 0.0; - out_6572770099193808423[50] = 0.0; - out_6572770099193808423[51] = 0.0; - out_6572770099193808423[52] = 0.0; - out_6572770099193808423[53] = 0.0; - out_6572770099193808423[54] = 0.0; - out_6572770099193808423[55] = 0.0; - out_6572770099193808423[56] = 0.0; - out_6572770099193808423[57] = 1.0; - out_6572770099193808423[58] = 0.0; - out_6572770099193808423[59] = 0.0; - out_6572770099193808423[60] = 0.0; - out_6572770099193808423[61] = 0.0; - out_6572770099193808423[62] = 0.0; - out_6572770099193808423[63] = 0.0; - out_6572770099193808423[64] = 0.0; - out_6572770099193808423[65] = 0.0; - out_6572770099193808423[66] = 0.0; - out_6572770099193808423[67] = 0.0; - out_6572770099193808423[68] = 0.0; - out_6572770099193808423[69] = 0.0; - out_6572770099193808423[70] = 0.0; - out_6572770099193808423[71] = 0.0; - out_6572770099193808423[72] = 0.0; - out_6572770099193808423[73] = 0.0; - out_6572770099193808423[74] = 0.0; - out_6572770099193808423[75] = 0.0; - out_6572770099193808423[76] = 1.0; - out_6572770099193808423[77] = 0.0; - out_6572770099193808423[78] = 0.0; - out_6572770099193808423[79] = 0.0; - out_6572770099193808423[80] = 0.0; - out_6572770099193808423[81] = 0.0; - out_6572770099193808423[82] = 0.0; - out_6572770099193808423[83] = 0.0; - out_6572770099193808423[84] = 0.0; - out_6572770099193808423[85] = 0.0; - out_6572770099193808423[86] = 0.0; - out_6572770099193808423[87] = 0.0; - out_6572770099193808423[88] = 0.0; - out_6572770099193808423[89] = 0.0; - out_6572770099193808423[90] = 0.0; - out_6572770099193808423[91] = 0.0; - out_6572770099193808423[92] = 0.0; - out_6572770099193808423[93] = 0.0; - out_6572770099193808423[94] = 0.0; - out_6572770099193808423[95] = 1.0; - out_6572770099193808423[96] = 0.0; - out_6572770099193808423[97] = 0.0; - out_6572770099193808423[98] = 0.0; - out_6572770099193808423[99] = 0.0; - out_6572770099193808423[100] = 0.0; - out_6572770099193808423[101] = 0.0; - out_6572770099193808423[102] = 0.0; - out_6572770099193808423[103] = 0.0; - out_6572770099193808423[104] = 0.0; - out_6572770099193808423[105] = 0.0; - out_6572770099193808423[106] = 0.0; - out_6572770099193808423[107] = 0.0; - out_6572770099193808423[108] = 0.0; - out_6572770099193808423[109] = 0.0; - out_6572770099193808423[110] = 0.0; - out_6572770099193808423[111] = 0.0; - out_6572770099193808423[112] = 0.0; - out_6572770099193808423[113] = 0.0; - out_6572770099193808423[114] = 1.0; - out_6572770099193808423[115] = 0.0; - out_6572770099193808423[116] = 0.0; - out_6572770099193808423[117] = 0.0; - out_6572770099193808423[118] = 0.0; - out_6572770099193808423[119] = 0.0; - out_6572770099193808423[120] = 0.0; - out_6572770099193808423[121] = 0.0; - out_6572770099193808423[122] = 0.0; - out_6572770099193808423[123] = 0.0; - out_6572770099193808423[124] = 0.0; - out_6572770099193808423[125] = 0.0; - out_6572770099193808423[126] = 0.0; - out_6572770099193808423[127] = 0.0; - out_6572770099193808423[128] = 0.0; - out_6572770099193808423[129] = 0.0; - out_6572770099193808423[130] = 0.0; - out_6572770099193808423[131] = 0.0; - out_6572770099193808423[132] = 0.0; - out_6572770099193808423[133] = 1.0; - out_6572770099193808423[134] = 0.0; - out_6572770099193808423[135] = 0.0; - out_6572770099193808423[136] = 0.0; - out_6572770099193808423[137] = 0.0; - out_6572770099193808423[138] = 0.0; - out_6572770099193808423[139] = 0.0; - out_6572770099193808423[140] = 0.0; - out_6572770099193808423[141] = 0.0; - out_6572770099193808423[142] = 0.0; - out_6572770099193808423[143] = 0.0; - out_6572770099193808423[144] = 0.0; - out_6572770099193808423[145] = 0.0; - out_6572770099193808423[146] = 0.0; - out_6572770099193808423[147] = 0.0; - out_6572770099193808423[148] = 0.0; - out_6572770099193808423[149] = 0.0; - out_6572770099193808423[150] = 0.0; - out_6572770099193808423[151] = 0.0; - out_6572770099193808423[152] = 1.0; - out_6572770099193808423[153] = 0.0; - out_6572770099193808423[154] = 0.0; - out_6572770099193808423[155] = 0.0; - out_6572770099193808423[156] = 0.0; - out_6572770099193808423[157] = 0.0; - out_6572770099193808423[158] = 0.0; - out_6572770099193808423[159] = 0.0; - out_6572770099193808423[160] = 0.0; - out_6572770099193808423[161] = 0.0; - out_6572770099193808423[162] = 0.0; - out_6572770099193808423[163] = 0.0; - out_6572770099193808423[164] = 0.0; - out_6572770099193808423[165] = 0.0; - out_6572770099193808423[166] = 0.0; - out_6572770099193808423[167] = 0.0; - out_6572770099193808423[168] = 0.0; - out_6572770099193808423[169] = 0.0; - out_6572770099193808423[170] = 0.0; - out_6572770099193808423[171] = 1.0; - out_6572770099193808423[172] = 0.0; - out_6572770099193808423[173] = 0.0; - out_6572770099193808423[174] = 0.0; - out_6572770099193808423[175] = 0.0; - out_6572770099193808423[176] = 0.0; - out_6572770099193808423[177] = 0.0; - out_6572770099193808423[178] = 0.0; - out_6572770099193808423[179] = 0.0; - out_6572770099193808423[180] = 0.0; - out_6572770099193808423[181] = 0.0; - out_6572770099193808423[182] = 0.0; - out_6572770099193808423[183] = 0.0; - out_6572770099193808423[184] = 0.0; - out_6572770099193808423[185] = 0.0; - out_6572770099193808423[186] = 0.0; - out_6572770099193808423[187] = 0.0; - out_6572770099193808423[188] = 0.0; - out_6572770099193808423[189] = 0.0; - out_6572770099193808423[190] = 1.0; - out_6572770099193808423[191] = 0.0; - out_6572770099193808423[192] = 0.0; - out_6572770099193808423[193] = 0.0; - out_6572770099193808423[194] = 0.0; - out_6572770099193808423[195] = 0.0; - out_6572770099193808423[196] = 0.0; - out_6572770099193808423[197] = 0.0; - out_6572770099193808423[198] = 0.0; - out_6572770099193808423[199] = 0.0; - out_6572770099193808423[200] = 0.0; - out_6572770099193808423[201] = 0.0; - out_6572770099193808423[202] = 0.0; - out_6572770099193808423[203] = 0.0; - out_6572770099193808423[204] = 0.0; - out_6572770099193808423[205] = 0.0; - out_6572770099193808423[206] = 0.0; - out_6572770099193808423[207] = 0.0; - out_6572770099193808423[208] = 0.0; - out_6572770099193808423[209] = 1.0; - out_6572770099193808423[210] = 0.0; - out_6572770099193808423[211] = 0.0; - out_6572770099193808423[212] = 0.0; - out_6572770099193808423[213] = 0.0; - out_6572770099193808423[214] = 0.0; - out_6572770099193808423[215] = 0.0; - out_6572770099193808423[216] = 0.0; - out_6572770099193808423[217] = 0.0; - out_6572770099193808423[218] = 0.0; - out_6572770099193808423[219] = 0.0; - out_6572770099193808423[220] = 0.0; - out_6572770099193808423[221] = 0.0; - out_6572770099193808423[222] = 0.0; - out_6572770099193808423[223] = 0.0; - out_6572770099193808423[224] = 0.0; - out_6572770099193808423[225] = 0.0; - out_6572770099193808423[226] = 0.0; - out_6572770099193808423[227] = 0.0; - out_6572770099193808423[228] = 1.0; - out_6572770099193808423[229] = 0.0; - out_6572770099193808423[230] = 0.0; - out_6572770099193808423[231] = 0.0; - out_6572770099193808423[232] = 0.0; - out_6572770099193808423[233] = 0.0; - out_6572770099193808423[234] = 0.0; - out_6572770099193808423[235] = 0.0; - out_6572770099193808423[236] = 0.0; - out_6572770099193808423[237] = 0.0; - out_6572770099193808423[238] = 0.0; - out_6572770099193808423[239] = 0.0; - out_6572770099193808423[240] = 0.0; - out_6572770099193808423[241] = 0.0; - out_6572770099193808423[242] = 0.0; - out_6572770099193808423[243] = 0.0; - out_6572770099193808423[244] = 0.0; - out_6572770099193808423[245] = 0.0; - out_6572770099193808423[246] = 0.0; - out_6572770099193808423[247] = 1.0; - out_6572770099193808423[248] = 0.0; - out_6572770099193808423[249] = 0.0; - out_6572770099193808423[250] = 0.0; - out_6572770099193808423[251] = 0.0; - out_6572770099193808423[252] = 0.0; - out_6572770099193808423[253] = 0.0; - out_6572770099193808423[254] = 0.0; - out_6572770099193808423[255] = 0.0; - out_6572770099193808423[256] = 0.0; - out_6572770099193808423[257] = 0.0; - out_6572770099193808423[258] = 0.0; - out_6572770099193808423[259] = 0.0; - out_6572770099193808423[260] = 0.0; - out_6572770099193808423[261] = 0.0; - out_6572770099193808423[262] = 0.0; - out_6572770099193808423[263] = 0.0; - out_6572770099193808423[264] = 0.0; - out_6572770099193808423[265] = 0.0; - out_6572770099193808423[266] = 1.0; - out_6572770099193808423[267] = 0.0; - out_6572770099193808423[268] = 0.0; - out_6572770099193808423[269] = 0.0; - out_6572770099193808423[270] = 0.0; - out_6572770099193808423[271] = 0.0; - out_6572770099193808423[272] = 0.0; - out_6572770099193808423[273] = 0.0; - out_6572770099193808423[274] = 0.0; - out_6572770099193808423[275] = 0.0; - out_6572770099193808423[276] = 0.0; - out_6572770099193808423[277] = 0.0; - out_6572770099193808423[278] = 0.0; - out_6572770099193808423[279] = 0.0; - out_6572770099193808423[280] = 0.0; - out_6572770099193808423[281] = 0.0; - out_6572770099193808423[282] = 0.0; - out_6572770099193808423[283] = 0.0; - out_6572770099193808423[284] = 0.0; - out_6572770099193808423[285] = 1.0; - out_6572770099193808423[286] = 0.0; - out_6572770099193808423[287] = 0.0; - out_6572770099193808423[288] = 0.0; - out_6572770099193808423[289] = 0.0; - out_6572770099193808423[290] = 0.0; - out_6572770099193808423[291] = 0.0; - out_6572770099193808423[292] = 0.0; - out_6572770099193808423[293] = 0.0; - out_6572770099193808423[294] = 0.0; - out_6572770099193808423[295] = 0.0; - out_6572770099193808423[296] = 0.0; - out_6572770099193808423[297] = 0.0; - out_6572770099193808423[298] = 0.0; - out_6572770099193808423[299] = 0.0; - out_6572770099193808423[300] = 0.0; - out_6572770099193808423[301] = 0.0; - out_6572770099193808423[302] = 0.0; - out_6572770099193808423[303] = 0.0; - out_6572770099193808423[304] = 1.0; - out_6572770099193808423[305] = 0.0; - out_6572770099193808423[306] = 0.0; - out_6572770099193808423[307] = 0.0; - out_6572770099193808423[308] = 0.0; - out_6572770099193808423[309] = 0.0; - out_6572770099193808423[310] = 0.0; - out_6572770099193808423[311] = 0.0; - out_6572770099193808423[312] = 0.0; - out_6572770099193808423[313] = 0.0; - out_6572770099193808423[314] = 0.0; - out_6572770099193808423[315] = 0.0; - out_6572770099193808423[316] = 0.0; - out_6572770099193808423[317] = 0.0; - out_6572770099193808423[318] = 0.0; - out_6572770099193808423[319] = 0.0; - out_6572770099193808423[320] = 0.0; - out_6572770099193808423[321] = 0.0; - out_6572770099193808423[322] = 0.0; - out_6572770099193808423[323] = 1.0; +void H_mod_fun(double *state, double *out_4641299731180659481) { + out_4641299731180659481[0] = 1.0; + out_4641299731180659481[1] = 0.0; + out_4641299731180659481[2] = 0.0; + out_4641299731180659481[3] = 0.0; + out_4641299731180659481[4] = 0.0; + out_4641299731180659481[5] = 0.0; + out_4641299731180659481[6] = 0.0; + out_4641299731180659481[7] = 0.0; + out_4641299731180659481[8] = 0.0; + out_4641299731180659481[9] = 0.0; + out_4641299731180659481[10] = 0.0; + out_4641299731180659481[11] = 0.0; + out_4641299731180659481[12] = 0.0; + out_4641299731180659481[13] = 0.0; + out_4641299731180659481[14] = 0.0; + out_4641299731180659481[15] = 0.0; + out_4641299731180659481[16] = 0.0; + out_4641299731180659481[17] = 0.0; + out_4641299731180659481[18] = 0.0; + out_4641299731180659481[19] = 1.0; + out_4641299731180659481[20] = 0.0; + out_4641299731180659481[21] = 0.0; + out_4641299731180659481[22] = 0.0; + out_4641299731180659481[23] = 0.0; + out_4641299731180659481[24] = 0.0; + out_4641299731180659481[25] = 0.0; + out_4641299731180659481[26] = 0.0; + out_4641299731180659481[27] = 0.0; + out_4641299731180659481[28] = 0.0; + out_4641299731180659481[29] = 0.0; + out_4641299731180659481[30] = 0.0; + out_4641299731180659481[31] = 0.0; + out_4641299731180659481[32] = 0.0; + out_4641299731180659481[33] = 0.0; + out_4641299731180659481[34] = 0.0; + out_4641299731180659481[35] = 0.0; + out_4641299731180659481[36] = 0.0; + out_4641299731180659481[37] = 0.0; + out_4641299731180659481[38] = 1.0; + out_4641299731180659481[39] = 0.0; + out_4641299731180659481[40] = 0.0; + out_4641299731180659481[41] = 0.0; + out_4641299731180659481[42] = 0.0; + out_4641299731180659481[43] = 0.0; + out_4641299731180659481[44] = 0.0; + out_4641299731180659481[45] = 0.0; + out_4641299731180659481[46] = 0.0; + out_4641299731180659481[47] = 0.0; + out_4641299731180659481[48] = 0.0; + out_4641299731180659481[49] = 0.0; + out_4641299731180659481[50] = 0.0; + out_4641299731180659481[51] = 0.0; + out_4641299731180659481[52] = 0.0; + out_4641299731180659481[53] = 0.0; + out_4641299731180659481[54] = 0.0; + out_4641299731180659481[55] = 0.0; + out_4641299731180659481[56] = 0.0; + out_4641299731180659481[57] = 1.0; + out_4641299731180659481[58] = 0.0; + out_4641299731180659481[59] = 0.0; + out_4641299731180659481[60] = 0.0; + out_4641299731180659481[61] = 0.0; + out_4641299731180659481[62] = 0.0; + out_4641299731180659481[63] = 0.0; + out_4641299731180659481[64] = 0.0; + out_4641299731180659481[65] = 0.0; + out_4641299731180659481[66] = 0.0; + out_4641299731180659481[67] = 0.0; + out_4641299731180659481[68] = 0.0; + out_4641299731180659481[69] = 0.0; + out_4641299731180659481[70] = 0.0; + out_4641299731180659481[71] = 0.0; + out_4641299731180659481[72] = 0.0; + out_4641299731180659481[73] = 0.0; + out_4641299731180659481[74] = 0.0; + out_4641299731180659481[75] = 0.0; + out_4641299731180659481[76] = 1.0; + out_4641299731180659481[77] = 0.0; + out_4641299731180659481[78] = 0.0; + out_4641299731180659481[79] = 0.0; + out_4641299731180659481[80] = 0.0; + out_4641299731180659481[81] = 0.0; + out_4641299731180659481[82] = 0.0; + out_4641299731180659481[83] = 0.0; + out_4641299731180659481[84] = 0.0; + out_4641299731180659481[85] = 0.0; + out_4641299731180659481[86] = 0.0; + out_4641299731180659481[87] = 0.0; + out_4641299731180659481[88] = 0.0; + out_4641299731180659481[89] = 0.0; + out_4641299731180659481[90] = 0.0; + out_4641299731180659481[91] = 0.0; + out_4641299731180659481[92] = 0.0; + out_4641299731180659481[93] = 0.0; + out_4641299731180659481[94] = 0.0; + out_4641299731180659481[95] = 1.0; + out_4641299731180659481[96] = 0.0; + out_4641299731180659481[97] = 0.0; + out_4641299731180659481[98] = 0.0; + out_4641299731180659481[99] = 0.0; + out_4641299731180659481[100] = 0.0; + out_4641299731180659481[101] = 0.0; + out_4641299731180659481[102] = 0.0; + out_4641299731180659481[103] = 0.0; + out_4641299731180659481[104] = 0.0; + out_4641299731180659481[105] = 0.0; + out_4641299731180659481[106] = 0.0; + out_4641299731180659481[107] = 0.0; + out_4641299731180659481[108] = 0.0; + out_4641299731180659481[109] = 0.0; + out_4641299731180659481[110] = 0.0; + out_4641299731180659481[111] = 0.0; + out_4641299731180659481[112] = 0.0; + out_4641299731180659481[113] = 0.0; + out_4641299731180659481[114] = 1.0; + out_4641299731180659481[115] = 0.0; + out_4641299731180659481[116] = 0.0; + out_4641299731180659481[117] = 0.0; + out_4641299731180659481[118] = 0.0; + out_4641299731180659481[119] = 0.0; + out_4641299731180659481[120] = 0.0; + out_4641299731180659481[121] = 0.0; + out_4641299731180659481[122] = 0.0; + out_4641299731180659481[123] = 0.0; + out_4641299731180659481[124] = 0.0; + out_4641299731180659481[125] = 0.0; + out_4641299731180659481[126] = 0.0; + out_4641299731180659481[127] = 0.0; + out_4641299731180659481[128] = 0.0; + out_4641299731180659481[129] = 0.0; + out_4641299731180659481[130] = 0.0; + out_4641299731180659481[131] = 0.0; + out_4641299731180659481[132] = 0.0; + out_4641299731180659481[133] = 1.0; + out_4641299731180659481[134] = 0.0; + out_4641299731180659481[135] = 0.0; + out_4641299731180659481[136] = 0.0; + out_4641299731180659481[137] = 0.0; + out_4641299731180659481[138] = 0.0; + out_4641299731180659481[139] = 0.0; + out_4641299731180659481[140] = 0.0; + out_4641299731180659481[141] = 0.0; + out_4641299731180659481[142] = 0.0; + out_4641299731180659481[143] = 0.0; + out_4641299731180659481[144] = 0.0; + out_4641299731180659481[145] = 0.0; + out_4641299731180659481[146] = 0.0; + out_4641299731180659481[147] = 0.0; + out_4641299731180659481[148] = 0.0; + out_4641299731180659481[149] = 0.0; + out_4641299731180659481[150] = 0.0; + out_4641299731180659481[151] = 0.0; + out_4641299731180659481[152] = 1.0; + out_4641299731180659481[153] = 0.0; + out_4641299731180659481[154] = 0.0; + out_4641299731180659481[155] = 0.0; + out_4641299731180659481[156] = 0.0; + out_4641299731180659481[157] = 0.0; + out_4641299731180659481[158] = 0.0; + out_4641299731180659481[159] = 0.0; + out_4641299731180659481[160] = 0.0; + out_4641299731180659481[161] = 0.0; + out_4641299731180659481[162] = 0.0; + out_4641299731180659481[163] = 0.0; + out_4641299731180659481[164] = 0.0; + out_4641299731180659481[165] = 0.0; + out_4641299731180659481[166] = 0.0; + out_4641299731180659481[167] = 0.0; + out_4641299731180659481[168] = 0.0; + out_4641299731180659481[169] = 0.0; + out_4641299731180659481[170] = 0.0; + out_4641299731180659481[171] = 1.0; + out_4641299731180659481[172] = 0.0; + out_4641299731180659481[173] = 0.0; + out_4641299731180659481[174] = 0.0; + out_4641299731180659481[175] = 0.0; + out_4641299731180659481[176] = 0.0; + out_4641299731180659481[177] = 0.0; + out_4641299731180659481[178] = 0.0; + out_4641299731180659481[179] = 0.0; + out_4641299731180659481[180] = 0.0; + out_4641299731180659481[181] = 0.0; + out_4641299731180659481[182] = 0.0; + out_4641299731180659481[183] = 0.0; + out_4641299731180659481[184] = 0.0; + out_4641299731180659481[185] = 0.0; + out_4641299731180659481[186] = 0.0; + out_4641299731180659481[187] = 0.0; + out_4641299731180659481[188] = 0.0; + out_4641299731180659481[189] = 0.0; + out_4641299731180659481[190] = 1.0; + out_4641299731180659481[191] = 0.0; + out_4641299731180659481[192] = 0.0; + out_4641299731180659481[193] = 0.0; + out_4641299731180659481[194] = 0.0; + out_4641299731180659481[195] = 0.0; + out_4641299731180659481[196] = 0.0; + out_4641299731180659481[197] = 0.0; + out_4641299731180659481[198] = 0.0; + out_4641299731180659481[199] = 0.0; + out_4641299731180659481[200] = 0.0; + out_4641299731180659481[201] = 0.0; + out_4641299731180659481[202] = 0.0; + out_4641299731180659481[203] = 0.0; + out_4641299731180659481[204] = 0.0; + out_4641299731180659481[205] = 0.0; + out_4641299731180659481[206] = 0.0; + out_4641299731180659481[207] = 0.0; + out_4641299731180659481[208] = 0.0; + out_4641299731180659481[209] = 1.0; + out_4641299731180659481[210] = 0.0; + out_4641299731180659481[211] = 0.0; + out_4641299731180659481[212] = 0.0; + out_4641299731180659481[213] = 0.0; + out_4641299731180659481[214] = 0.0; + out_4641299731180659481[215] = 0.0; + out_4641299731180659481[216] = 0.0; + out_4641299731180659481[217] = 0.0; + out_4641299731180659481[218] = 0.0; + out_4641299731180659481[219] = 0.0; + out_4641299731180659481[220] = 0.0; + out_4641299731180659481[221] = 0.0; + out_4641299731180659481[222] = 0.0; + out_4641299731180659481[223] = 0.0; + out_4641299731180659481[224] = 0.0; + out_4641299731180659481[225] = 0.0; + out_4641299731180659481[226] = 0.0; + out_4641299731180659481[227] = 0.0; + out_4641299731180659481[228] = 1.0; + out_4641299731180659481[229] = 0.0; + out_4641299731180659481[230] = 0.0; + out_4641299731180659481[231] = 0.0; + out_4641299731180659481[232] = 0.0; + out_4641299731180659481[233] = 0.0; + out_4641299731180659481[234] = 0.0; + out_4641299731180659481[235] = 0.0; + out_4641299731180659481[236] = 0.0; + out_4641299731180659481[237] = 0.0; + out_4641299731180659481[238] = 0.0; + out_4641299731180659481[239] = 0.0; + out_4641299731180659481[240] = 0.0; + out_4641299731180659481[241] = 0.0; + out_4641299731180659481[242] = 0.0; + out_4641299731180659481[243] = 0.0; + out_4641299731180659481[244] = 0.0; + out_4641299731180659481[245] = 0.0; + out_4641299731180659481[246] = 0.0; + out_4641299731180659481[247] = 1.0; + out_4641299731180659481[248] = 0.0; + out_4641299731180659481[249] = 0.0; + out_4641299731180659481[250] = 0.0; + out_4641299731180659481[251] = 0.0; + out_4641299731180659481[252] = 0.0; + out_4641299731180659481[253] = 0.0; + out_4641299731180659481[254] = 0.0; + out_4641299731180659481[255] = 0.0; + out_4641299731180659481[256] = 0.0; + out_4641299731180659481[257] = 0.0; + out_4641299731180659481[258] = 0.0; + out_4641299731180659481[259] = 0.0; + out_4641299731180659481[260] = 0.0; + out_4641299731180659481[261] = 0.0; + out_4641299731180659481[262] = 0.0; + out_4641299731180659481[263] = 0.0; + out_4641299731180659481[264] = 0.0; + out_4641299731180659481[265] = 0.0; + out_4641299731180659481[266] = 1.0; + out_4641299731180659481[267] = 0.0; + out_4641299731180659481[268] = 0.0; + out_4641299731180659481[269] = 0.0; + out_4641299731180659481[270] = 0.0; + out_4641299731180659481[271] = 0.0; + out_4641299731180659481[272] = 0.0; + out_4641299731180659481[273] = 0.0; + out_4641299731180659481[274] = 0.0; + out_4641299731180659481[275] = 0.0; + out_4641299731180659481[276] = 0.0; + out_4641299731180659481[277] = 0.0; + out_4641299731180659481[278] = 0.0; + out_4641299731180659481[279] = 0.0; + out_4641299731180659481[280] = 0.0; + out_4641299731180659481[281] = 0.0; + out_4641299731180659481[282] = 0.0; + out_4641299731180659481[283] = 0.0; + out_4641299731180659481[284] = 0.0; + out_4641299731180659481[285] = 1.0; + out_4641299731180659481[286] = 0.0; + out_4641299731180659481[287] = 0.0; + out_4641299731180659481[288] = 0.0; + out_4641299731180659481[289] = 0.0; + out_4641299731180659481[290] = 0.0; + out_4641299731180659481[291] = 0.0; + out_4641299731180659481[292] = 0.0; + out_4641299731180659481[293] = 0.0; + out_4641299731180659481[294] = 0.0; + out_4641299731180659481[295] = 0.0; + out_4641299731180659481[296] = 0.0; + out_4641299731180659481[297] = 0.0; + out_4641299731180659481[298] = 0.0; + out_4641299731180659481[299] = 0.0; + out_4641299731180659481[300] = 0.0; + out_4641299731180659481[301] = 0.0; + out_4641299731180659481[302] = 0.0; + out_4641299731180659481[303] = 0.0; + out_4641299731180659481[304] = 1.0; + out_4641299731180659481[305] = 0.0; + out_4641299731180659481[306] = 0.0; + out_4641299731180659481[307] = 0.0; + out_4641299731180659481[308] = 0.0; + out_4641299731180659481[309] = 0.0; + out_4641299731180659481[310] = 0.0; + out_4641299731180659481[311] = 0.0; + out_4641299731180659481[312] = 0.0; + out_4641299731180659481[313] = 0.0; + out_4641299731180659481[314] = 0.0; + out_4641299731180659481[315] = 0.0; + out_4641299731180659481[316] = 0.0; + out_4641299731180659481[317] = 0.0; + out_4641299731180659481[318] = 0.0; + out_4641299731180659481[319] = 0.0; + out_4641299731180659481[320] = 0.0; + out_4641299731180659481[321] = 0.0; + out_4641299731180659481[322] = 0.0; + out_4641299731180659481[323] = 1.0; } -void f_fun(double *state, double dt, double *out_972215043360367871) { - out_972215043360367871[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_972215043360367871[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_972215043360367871[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_972215043360367871[3] = dt*state[12] + state[3]; - out_972215043360367871[4] = dt*state[13] + state[4]; - out_972215043360367871[5] = dt*state[14] + state[5]; - out_972215043360367871[6] = state[6]; - out_972215043360367871[7] = state[7]; - out_972215043360367871[8] = state[8]; - out_972215043360367871[9] = state[9]; - out_972215043360367871[10] = state[10]; - out_972215043360367871[11] = state[11]; - out_972215043360367871[12] = state[12]; - out_972215043360367871[13] = state[13]; - out_972215043360367871[14] = state[14]; - out_972215043360367871[15] = state[15]; - out_972215043360367871[16] = state[16]; - out_972215043360367871[17] = state[17]; +void f_fun(double *state, double dt, double *out_4475655607956648045) { + out_4475655607956648045[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_4475655607956648045[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_4475655607956648045[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_4475655607956648045[3] = dt*state[12] + state[3]; + out_4475655607956648045[4] = dt*state[13] + state[4]; + out_4475655607956648045[5] = dt*state[14] + state[5]; + out_4475655607956648045[6] = state[6]; + out_4475655607956648045[7] = state[7]; + out_4475655607956648045[8] = state[8]; + out_4475655607956648045[9] = state[9]; + out_4475655607956648045[10] = state[10]; + out_4475655607956648045[11] = state[11]; + out_4475655607956648045[12] = state[12]; + out_4475655607956648045[13] = state[13]; + out_4475655607956648045[14] = state[14]; + out_4475655607956648045[15] = state[15]; + out_4475655607956648045[16] = state[16]; + out_4475655607956648045[17] = state[17]; } -void F_fun(double *state, double dt, double *out_9096792433349462064) { - out_9096792433349462064[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_9096792433349462064[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_9096792433349462064[2] = 0; - out_9096792433349462064[3] = 0; - out_9096792433349462064[4] = 0; - out_9096792433349462064[5] = 0; - out_9096792433349462064[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_9096792433349462064[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_9096792433349462064[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_9096792433349462064[9] = 0; - out_9096792433349462064[10] = 0; - out_9096792433349462064[11] = 0; - out_9096792433349462064[12] = 0; - out_9096792433349462064[13] = 0; - out_9096792433349462064[14] = 0; - out_9096792433349462064[15] = 0; - out_9096792433349462064[16] = 0; - out_9096792433349462064[17] = 0; - out_9096792433349462064[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_9096792433349462064[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_9096792433349462064[20] = 0; - out_9096792433349462064[21] = 0; - out_9096792433349462064[22] = 0; - out_9096792433349462064[23] = 0; - out_9096792433349462064[24] = 0; - out_9096792433349462064[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_9096792433349462064[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_9096792433349462064[27] = 0; - out_9096792433349462064[28] = 0; - out_9096792433349462064[29] = 0; - out_9096792433349462064[30] = 0; - out_9096792433349462064[31] = 0; - out_9096792433349462064[32] = 0; - out_9096792433349462064[33] = 0; - out_9096792433349462064[34] = 0; - out_9096792433349462064[35] = 0; - out_9096792433349462064[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_9096792433349462064[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_9096792433349462064[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_9096792433349462064[39] = 0; - out_9096792433349462064[40] = 0; - out_9096792433349462064[41] = 0; - out_9096792433349462064[42] = 0; - out_9096792433349462064[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_9096792433349462064[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_9096792433349462064[45] = 0; - out_9096792433349462064[46] = 0; - out_9096792433349462064[47] = 0; - out_9096792433349462064[48] = 0; - out_9096792433349462064[49] = 0; - out_9096792433349462064[50] = 0; - out_9096792433349462064[51] = 0; - out_9096792433349462064[52] = 0; - out_9096792433349462064[53] = 0; - out_9096792433349462064[54] = 0; - out_9096792433349462064[55] = 0; - out_9096792433349462064[56] = 0; - out_9096792433349462064[57] = 1; - out_9096792433349462064[58] = 0; - out_9096792433349462064[59] = 0; - out_9096792433349462064[60] = 0; - out_9096792433349462064[61] = 0; - out_9096792433349462064[62] = 0; - out_9096792433349462064[63] = 0; - out_9096792433349462064[64] = 0; - out_9096792433349462064[65] = 0; - out_9096792433349462064[66] = dt; - out_9096792433349462064[67] = 0; - out_9096792433349462064[68] = 0; - out_9096792433349462064[69] = 0; - out_9096792433349462064[70] = 0; - out_9096792433349462064[71] = 0; - out_9096792433349462064[72] = 0; - out_9096792433349462064[73] = 0; - out_9096792433349462064[74] = 0; - out_9096792433349462064[75] = 0; - out_9096792433349462064[76] = 1; - out_9096792433349462064[77] = 0; - out_9096792433349462064[78] = 0; - out_9096792433349462064[79] = 0; - out_9096792433349462064[80] = 0; - out_9096792433349462064[81] = 0; - out_9096792433349462064[82] = 0; - out_9096792433349462064[83] = 0; - out_9096792433349462064[84] = 0; - out_9096792433349462064[85] = dt; - out_9096792433349462064[86] = 0; - out_9096792433349462064[87] = 0; - out_9096792433349462064[88] = 0; - out_9096792433349462064[89] = 0; - out_9096792433349462064[90] = 0; - out_9096792433349462064[91] = 0; - out_9096792433349462064[92] = 0; - out_9096792433349462064[93] = 0; - out_9096792433349462064[94] = 0; - out_9096792433349462064[95] = 1; - out_9096792433349462064[96] = 0; - out_9096792433349462064[97] = 0; - out_9096792433349462064[98] = 0; - out_9096792433349462064[99] = 0; - out_9096792433349462064[100] = 0; - out_9096792433349462064[101] = 0; - out_9096792433349462064[102] = 0; - out_9096792433349462064[103] = 0; - out_9096792433349462064[104] = dt; - out_9096792433349462064[105] = 0; - out_9096792433349462064[106] = 0; - out_9096792433349462064[107] = 0; - out_9096792433349462064[108] = 0; - out_9096792433349462064[109] = 0; - out_9096792433349462064[110] = 0; - out_9096792433349462064[111] = 0; - out_9096792433349462064[112] = 0; - out_9096792433349462064[113] = 0; - out_9096792433349462064[114] = 1; - out_9096792433349462064[115] = 0; - out_9096792433349462064[116] = 0; - out_9096792433349462064[117] = 0; - out_9096792433349462064[118] = 0; - out_9096792433349462064[119] = 0; - out_9096792433349462064[120] = 0; - out_9096792433349462064[121] = 0; - out_9096792433349462064[122] = 0; - out_9096792433349462064[123] = 0; - out_9096792433349462064[124] = 0; - out_9096792433349462064[125] = 0; - out_9096792433349462064[126] = 0; - out_9096792433349462064[127] = 0; - out_9096792433349462064[128] = 0; - out_9096792433349462064[129] = 0; - out_9096792433349462064[130] = 0; - out_9096792433349462064[131] = 0; - out_9096792433349462064[132] = 0; - out_9096792433349462064[133] = 1; - out_9096792433349462064[134] = 0; - out_9096792433349462064[135] = 0; - out_9096792433349462064[136] = 0; - out_9096792433349462064[137] = 0; - out_9096792433349462064[138] = 0; - out_9096792433349462064[139] = 0; - out_9096792433349462064[140] = 0; - out_9096792433349462064[141] = 0; - out_9096792433349462064[142] = 0; - out_9096792433349462064[143] = 0; - out_9096792433349462064[144] = 0; - out_9096792433349462064[145] = 0; - out_9096792433349462064[146] = 0; - out_9096792433349462064[147] = 0; - out_9096792433349462064[148] = 0; - out_9096792433349462064[149] = 0; - out_9096792433349462064[150] = 0; - out_9096792433349462064[151] = 0; - out_9096792433349462064[152] = 1; - out_9096792433349462064[153] = 0; - out_9096792433349462064[154] = 0; - out_9096792433349462064[155] = 0; - out_9096792433349462064[156] = 0; - out_9096792433349462064[157] = 0; - out_9096792433349462064[158] = 0; - out_9096792433349462064[159] = 0; - out_9096792433349462064[160] = 0; - out_9096792433349462064[161] = 0; - out_9096792433349462064[162] = 0; - out_9096792433349462064[163] = 0; - out_9096792433349462064[164] = 0; - out_9096792433349462064[165] = 0; - out_9096792433349462064[166] = 0; - out_9096792433349462064[167] = 0; - out_9096792433349462064[168] = 0; - out_9096792433349462064[169] = 0; - out_9096792433349462064[170] = 0; - out_9096792433349462064[171] = 1; - out_9096792433349462064[172] = 0; - out_9096792433349462064[173] = 0; - out_9096792433349462064[174] = 0; - out_9096792433349462064[175] = 0; - out_9096792433349462064[176] = 0; - out_9096792433349462064[177] = 0; - out_9096792433349462064[178] = 0; - out_9096792433349462064[179] = 0; - out_9096792433349462064[180] = 0; - out_9096792433349462064[181] = 0; - out_9096792433349462064[182] = 0; - out_9096792433349462064[183] = 0; - out_9096792433349462064[184] = 0; - out_9096792433349462064[185] = 0; - out_9096792433349462064[186] = 0; - out_9096792433349462064[187] = 0; - out_9096792433349462064[188] = 0; - out_9096792433349462064[189] = 0; - out_9096792433349462064[190] = 1; - out_9096792433349462064[191] = 0; - out_9096792433349462064[192] = 0; - out_9096792433349462064[193] = 0; - out_9096792433349462064[194] = 0; - out_9096792433349462064[195] = 0; - out_9096792433349462064[196] = 0; - out_9096792433349462064[197] = 0; - out_9096792433349462064[198] = 0; - out_9096792433349462064[199] = 0; - out_9096792433349462064[200] = 0; - out_9096792433349462064[201] = 0; - out_9096792433349462064[202] = 0; - out_9096792433349462064[203] = 0; - out_9096792433349462064[204] = 0; - out_9096792433349462064[205] = 0; - out_9096792433349462064[206] = 0; - out_9096792433349462064[207] = 0; - out_9096792433349462064[208] = 0; - out_9096792433349462064[209] = 1; - out_9096792433349462064[210] = 0; - out_9096792433349462064[211] = 0; - out_9096792433349462064[212] = 0; - out_9096792433349462064[213] = 0; - out_9096792433349462064[214] = 0; - out_9096792433349462064[215] = 0; - out_9096792433349462064[216] = 0; - out_9096792433349462064[217] = 0; - out_9096792433349462064[218] = 0; - out_9096792433349462064[219] = 0; - out_9096792433349462064[220] = 0; - out_9096792433349462064[221] = 0; - out_9096792433349462064[222] = 0; - out_9096792433349462064[223] = 0; - out_9096792433349462064[224] = 0; - out_9096792433349462064[225] = 0; - out_9096792433349462064[226] = 0; - out_9096792433349462064[227] = 0; - out_9096792433349462064[228] = 1; - out_9096792433349462064[229] = 0; - out_9096792433349462064[230] = 0; - out_9096792433349462064[231] = 0; - out_9096792433349462064[232] = 0; - out_9096792433349462064[233] = 0; - out_9096792433349462064[234] = 0; - out_9096792433349462064[235] = 0; - out_9096792433349462064[236] = 0; - out_9096792433349462064[237] = 0; - out_9096792433349462064[238] = 0; - out_9096792433349462064[239] = 0; - out_9096792433349462064[240] = 0; - out_9096792433349462064[241] = 0; - out_9096792433349462064[242] = 0; - out_9096792433349462064[243] = 0; - out_9096792433349462064[244] = 0; - out_9096792433349462064[245] = 0; - out_9096792433349462064[246] = 0; - out_9096792433349462064[247] = 1; - out_9096792433349462064[248] = 0; - out_9096792433349462064[249] = 0; - out_9096792433349462064[250] = 0; - out_9096792433349462064[251] = 0; - out_9096792433349462064[252] = 0; - out_9096792433349462064[253] = 0; - out_9096792433349462064[254] = 0; - out_9096792433349462064[255] = 0; - out_9096792433349462064[256] = 0; - out_9096792433349462064[257] = 0; - out_9096792433349462064[258] = 0; - out_9096792433349462064[259] = 0; - out_9096792433349462064[260] = 0; - out_9096792433349462064[261] = 0; - out_9096792433349462064[262] = 0; - out_9096792433349462064[263] = 0; - out_9096792433349462064[264] = 0; - out_9096792433349462064[265] = 0; - out_9096792433349462064[266] = 1; - out_9096792433349462064[267] = 0; - out_9096792433349462064[268] = 0; - out_9096792433349462064[269] = 0; - out_9096792433349462064[270] = 0; - out_9096792433349462064[271] = 0; - out_9096792433349462064[272] = 0; - out_9096792433349462064[273] = 0; - out_9096792433349462064[274] = 0; - out_9096792433349462064[275] = 0; - out_9096792433349462064[276] = 0; - out_9096792433349462064[277] = 0; - out_9096792433349462064[278] = 0; - out_9096792433349462064[279] = 0; - out_9096792433349462064[280] = 0; - out_9096792433349462064[281] = 0; - out_9096792433349462064[282] = 0; - out_9096792433349462064[283] = 0; - out_9096792433349462064[284] = 0; - out_9096792433349462064[285] = 1; - out_9096792433349462064[286] = 0; - out_9096792433349462064[287] = 0; - out_9096792433349462064[288] = 0; - out_9096792433349462064[289] = 0; - out_9096792433349462064[290] = 0; - out_9096792433349462064[291] = 0; - out_9096792433349462064[292] = 0; - out_9096792433349462064[293] = 0; - out_9096792433349462064[294] = 0; - out_9096792433349462064[295] = 0; - out_9096792433349462064[296] = 0; - out_9096792433349462064[297] = 0; - out_9096792433349462064[298] = 0; - out_9096792433349462064[299] = 0; - out_9096792433349462064[300] = 0; - out_9096792433349462064[301] = 0; - out_9096792433349462064[302] = 0; - out_9096792433349462064[303] = 0; - out_9096792433349462064[304] = 1; - out_9096792433349462064[305] = 0; - out_9096792433349462064[306] = 0; - out_9096792433349462064[307] = 0; - out_9096792433349462064[308] = 0; - out_9096792433349462064[309] = 0; - out_9096792433349462064[310] = 0; - out_9096792433349462064[311] = 0; - out_9096792433349462064[312] = 0; - out_9096792433349462064[313] = 0; - out_9096792433349462064[314] = 0; - out_9096792433349462064[315] = 0; - out_9096792433349462064[316] = 0; - out_9096792433349462064[317] = 0; - out_9096792433349462064[318] = 0; - out_9096792433349462064[319] = 0; - out_9096792433349462064[320] = 0; - out_9096792433349462064[321] = 0; - out_9096792433349462064[322] = 0; - out_9096792433349462064[323] = 1; +void F_fun(double *state, double dt, double *out_1490364464464661376) { + out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[2] = 0; + out_1490364464464661376[3] = 0; + out_1490364464464661376[4] = 0; + out_1490364464464661376[5] = 0; + out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[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_1490364464464661376[9] = 0; + out_1490364464464661376[10] = 0; + out_1490364464464661376[11] = 0; + out_1490364464464661376[12] = 0; + out_1490364464464661376[13] = 0; + out_1490364464464661376[14] = 0; + out_1490364464464661376[15] = 0; + out_1490364464464661376[16] = 0; + out_1490364464464661376[17] = 0; + out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[20] = 0; + out_1490364464464661376[21] = 0; + out_1490364464464661376[22] = 0; + out_1490364464464661376[23] = 0; + out_1490364464464661376[24] = 0; + out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[27] = 0; + out_1490364464464661376[28] = 0; + out_1490364464464661376[29] = 0; + out_1490364464464661376[30] = 0; + out_1490364464464661376[31] = 0; + out_1490364464464661376[32] = 0; + out_1490364464464661376[33] = 0; + out_1490364464464661376[34] = 0; + out_1490364464464661376[35] = 0; + out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[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_1490364464464661376[39] = 0; + out_1490364464464661376[40] = 0; + out_1490364464464661376[41] = 0; + out_1490364464464661376[42] = 0; + out_1490364464464661376[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_1490364464464661376[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_1490364464464661376[45] = 0; + out_1490364464464661376[46] = 0; + out_1490364464464661376[47] = 0; + out_1490364464464661376[48] = 0; + out_1490364464464661376[49] = 0; + out_1490364464464661376[50] = 0; + out_1490364464464661376[51] = 0; + out_1490364464464661376[52] = 0; + out_1490364464464661376[53] = 0; + out_1490364464464661376[54] = 0; + out_1490364464464661376[55] = 0; + out_1490364464464661376[56] = 0; + out_1490364464464661376[57] = 1; + out_1490364464464661376[58] = 0; + out_1490364464464661376[59] = 0; + out_1490364464464661376[60] = 0; + out_1490364464464661376[61] = 0; + out_1490364464464661376[62] = 0; + out_1490364464464661376[63] = 0; + out_1490364464464661376[64] = 0; + out_1490364464464661376[65] = 0; + out_1490364464464661376[66] = dt; + out_1490364464464661376[67] = 0; + out_1490364464464661376[68] = 0; + out_1490364464464661376[69] = 0; + out_1490364464464661376[70] = 0; + out_1490364464464661376[71] = 0; + out_1490364464464661376[72] = 0; + out_1490364464464661376[73] = 0; + out_1490364464464661376[74] = 0; + out_1490364464464661376[75] = 0; + out_1490364464464661376[76] = 1; + out_1490364464464661376[77] = 0; + out_1490364464464661376[78] = 0; + out_1490364464464661376[79] = 0; + out_1490364464464661376[80] = 0; + out_1490364464464661376[81] = 0; + out_1490364464464661376[82] = 0; + out_1490364464464661376[83] = 0; + out_1490364464464661376[84] = 0; + out_1490364464464661376[85] = dt; + out_1490364464464661376[86] = 0; + out_1490364464464661376[87] = 0; + out_1490364464464661376[88] = 0; + out_1490364464464661376[89] = 0; + out_1490364464464661376[90] = 0; + out_1490364464464661376[91] = 0; + out_1490364464464661376[92] = 0; + out_1490364464464661376[93] = 0; + out_1490364464464661376[94] = 0; + out_1490364464464661376[95] = 1; + out_1490364464464661376[96] = 0; + out_1490364464464661376[97] = 0; + out_1490364464464661376[98] = 0; + out_1490364464464661376[99] = 0; + out_1490364464464661376[100] = 0; + out_1490364464464661376[101] = 0; + out_1490364464464661376[102] = 0; + out_1490364464464661376[103] = 0; + out_1490364464464661376[104] = dt; + out_1490364464464661376[105] = 0; + out_1490364464464661376[106] = 0; + out_1490364464464661376[107] = 0; + out_1490364464464661376[108] = 0; + out_1490364464464661376[109] = 0; + out_1490364464464661376[110] = 0; + out_1490364464464661376[111] = 0; + out_1490364464464661376[112] = 0; + out_1490364464464661376[113] = 0; + out_1490364464464661376[114] = 1; + out_1490364464464661376[115] = 0; + out_1490364464464661376[116] = 0; + out_1490364464464661376[117] = 0; + out_1490364464464661376[118] = 0; + out_1490364464464661376[119] = 0; + out_1490364464464661376[120] = 0; + out_1490364464464661376[121] = 0; + out_1490364464464661376[122] = 0; + out_1490364464464661376[123] = 0; + out_1490364464464661376[124] = 0; + out_1490364464464661376[125] = 0; + out_1490364464464661376[126] = 0; + out_1490364464464661376[127] = 0; + out_1490364464464661376[128] = 0; + out_1490364464464661376[129] = 0; + out_1490364464464661376[130] = 0; + out_1490364464464661376[131] = 0; + out_1490364464464661376[132] = 0; + out_1490364464464661376[133] = 1; + out_1490364464464661376[134] = 0; + out_1490364464464661376[135] = 0; + out_1490364464464661376[136] = 0; + out_1490364464464661376[137] = 0; + out_1490364464464661376[138] = 0; + out_1490364464464661376[139] = 0; + out_1490364464464661376[140] = 0; + out_1490364464464661376[141] = 0; + out_1490364464464661376[142] = 0; + out_1490364464464661376[143] = 0; + out_1490364464464661376[144] = 0; + out_1490364464464661376[145] = 0; + out_1490364464464661376[146] = 0; + out_1490364464464661376[147] = 0; + out_1490364464464661376[148] = 0; + out_1490364464464661376[149] = 0; + out_1490364464464661376[150] = 0; + out_1490364464464661376[151] = 0; + out_1490364464464661376[152] = 1; + out_1490364464464661376[153] = 0; + out_1490364464464661376[154] = 0; + out_1490364464464661376[155] = 0; + out_1490364464464661376[156] = 0; + out_1490364464464661376[157] = 0; + out_1490364464464661376[158] = 0; + out_1490364464464661376[159] = 0; + out_1490364464464661376[160] = 0; + out_1490364464464661376[161] = 0; + out_1490364464464661376[162] = 0; + out_1490364464464661376[163] = 0; + out_1490364464464661376[164] = 0; + out_1490364464464661376[165] = 0; + out_1490364464464661376[166] = 0; + out_1490364464464661376[167] = 0; + out_1490364464464661376[168] = 0; + out_1490364464464661376[169] = 0; + out_1490364464464661376[170] = 0; + out_1490364464464661376[171] = 1; + out_1490364464464661376[172] = 0; + out_1490364464464661376[173] = 0; + out_1490364464464661376[174] = 0; + out_1490364464464661376[175] = 0; + out_1490364464464661376[176] = 0; + out_1490364464464661376[177] = 0; + out_1490364464464661376[178] = 0; + out_1490364464464661376[179] = 0; + out_1490364464464661376[180] = 0; + out_1490364464464661376[181] = 0; + out_1490364464464661376[182] = 0; + out_1490364464464661376[183] = 0; + out_1490364464464661376[184] = 0; + out_1490364464464661376[185] = 0; + out_1490364464464661376[186] = 0; + out_1490364464464661376[187] = 0; + out_1490364464464661376[188] = 0; + out_1490364464464661376[189] = 0; + out_1490364464464661376[190] = 1; + out_1490364464464661376[191] = 0; + out_1490364464464661376[192] = 0; + out_1490364464464661376[193] = 0; + out_1490364464464661376[194] = 0; + out_1490364464464661376[195] = 0; + out_1490364464464661376[196] = 0; + out_1490364464464661376[197] = 0; + out_1490364464464661376[198] = 0; + out_1490364464464661376[199] = 0; + out_1490364464464661376[200] = 0; + out_1490364464464661376[201] = 0; + out_1490364464464661376[202] = 0; + out_1490364464464661376[203] = 0; + out_1490364464464661376[204] = 0; + out_1490364464464661376[205] = 0; + out_1490364464464661376[206] = 0; + out_1490364464464661376[207] = 0; + out_1490364464464661376[208] = 0; + out_1490364464464661376[209] = 1; + out_1490364464464661376[210] = 0; + out_1490364464464661376[211] = 0; + out_1490364464464661376[212] = 0; + out_1490364464464661376[213] = 0; + out_1490364464464661376[214] = 0; + out_1490364464464661376[215] = 0; + out_1490364464464661376[216] = 0; + out_1490364464464661376[217] = 0; + out_1490364464464661376[218] = 0; + out_1490364464464661376[219] = 0; + out_1490364464464661376[220] = 0; + out_1490364464464661376[221] = 0; + out_1490364464464661376[222] = 0; + out_1490364464464661376[223] = 0; + out_1490364464464661376[224] = 0; + out_1490364464464661376[225] = 0; + out_1490364464464661376[226] = 0; + out_1490364464464661376[227] = 0; + out_1490364464464661376[228] = 1; + out_1490364464464661376[229] = 0; + out_1490364464464661376[230] = 0; + out_1490364464464661376[231] = 0; + out_1490364464464661376[232] = 0; + out_1490364464464661376[233] = 0; + out_1490364464464661376[234] = 0; + out_1490364464464661376[235] = 0; + out_1490364464464661376[236] = 0; + out_1490364464464661376[237] = 0; + out_1490364464464661376[238] = 0; + out_1490364464464661376[239] = 0; + out_1490364464464661376[240] = 0; + out_1490364464464661376[241] = 0; + out_1490364464464661376[242] = 0; + out_1490364464464661376[243] = 0; + out_1490364464464661376[244] = 0; + out_1490364464464661376[245] = 0; + out_1490364464464661376[246] = 0; + out_1490364464464661376[247] = 1; + out_1490364464464661376[248] = 0; + out_1490364464464661376[249] = 0; + out_1490364464464661376[250] = 0; + out_1490364464464661376[251] = 0; + out_1490364464464661376[252] = 0; + out_1490364464464661376[253] = 0; + out_1490364464464661376[254] = 0; + out_1490364464464661376[255] = 0; + out_1490364464464661376[256] = 0; + out_1490364464464661376[257] = 0; + out_1490364464464661376[258] = 0; + out_1490364464464661376[259] = 0; + out_1490364464464661376[260] = 0; + out_1490364464464661376[261] = 0; + out_1490364464464661376[262] = 0; + out_1490364464464661376[263] = 0; + out_1490364464464661376[264] = 0; + out_1490364464464661376[265] = 0; + out_1490364464464661376[266] = 1; + out_1490364464464661376[267] = 0; + out_1490364464464661376[268] = 0; + out_1490364464464661376[269] = 0; + out_1490364464464661376[270] = 0; + out_1490364464464661376[271] = 0; + out_1490364464464661376[272] = 0; + out_1490364464464661376[273] = 0; + out_1490364464464661376[274] = 0; + out_1490364464464661376[275] = 0; + out_1490364464464661376[276] = 0; + out_1490364464464661376[277] = 0; + out_1490364464464661376[278] = 0; + out_1490364464464661376[279] = 0; + out_1490364464464661376[280] = 0; + out_1490364464464661376[281] = 0; + out_1490364464464661376[282] = 0; + out_1490364464464661376[283] = 0; + out_1490364464464661376[284] = 0; + out_1490364464464661376[285] = 1; + out_1490364464464661376[286] = 0; + out_1490364464464661376[287] = 0; + out_1490364464464661376[288] = 0; + out_1490364464464661376[289] = 0; + out_1490364464464661376[290] = 0; + out_1490364464464661376[291] = 0; + out_1490364464464661376[292] = 0; + out_1490364464464661376[293] = 0; + out_1490364464464661376[294] = 0; + out_1490364464464661376[295] = 0; + out_1490364464464661376[296] = 0; + out_1490364464464661376[297] = 0; + out_1490364464464661376[298] = 0; + out_1490364464464661376[299] = 0; + out_1490364464464661376[300] = 0; + out_1490364464464661376[301] = 0; + out_1490364464464661376[302] = 0; + out_1490364464464661376[303] = 0; + out_1490364464464661376[304] = 1; + out_1490364464464661376[305] = 0; + out_1490364464464661376[306] = 0; + out_1490364464464661376[307] = 0; + out_1490364464464661376[308] = 0; + out_1490364464464661376[309] = 0; + out_1490364464464661376[310] = 0; + out_1490364464464661376[311] = 0; + out_1490364464464661376[312] = 0; + out_1490364464464661376[313] = 0; + out_1490364464464661376[314] = 0; + out_1490364464464661376[315] = 0; + out_1490364464464661376[316] = 0; + out_1490364464464661376[317] = 0; + out_1490364464464661376[318] = 0; + out_1490364464464661376[319] = 0; + out_1490364464464661376[320] = 0; + out_1490364464464661376[321] = 0; + out_1490364464464661376[322] = 0; + out_1490364464464661376[323] = 1; } -void h_4(double *state, double *unused, double *out_2265781100835663155) { - out_2265781100835663155[0] = state[6] + state[9]; - out_2265781100835663155[1] = state[7] + state[10]; - out_2265781100835663155[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_4985239385195179173) { + out_4985239385195179173[0] = state[6] + state[9]; + out_4985239385195179173[1] = state[7] + state[10]; + out_4985239385195179173[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_8392468825100105284) { - out_8392468825100105284[0] = 0; - out_8392468825100105284[1] = 0; - out_8392468825100105284[2] = 0; - out_8392468825100105284[3] = 0; - out_8392468825100105284[4] = 0; - out_8392468825100105284[5] = 0; - out_8392468825100105284[6] = 1; - out_8392468825100105284[7] = 0; - out_8392468825100105284[8] = 0; - out_8392468825100105284[9] = 1; - out_8392468825100105284[10] = 0; - out_8392468825100105284[11] = 0; - out_8392468825100105284[12] = 0; - out_8392468825100105284[13] = 0; - out_8392468825100105284[14] = 0; - out_8392468825100105284[15] = 0; - out_8392468825100105284[16] = 0; - out_8392468825100105284[17] = 0; - out_8392468825100105284[18] = 0; - out_8392468825100105284[19] = 0; - out_8392468825100105284[20] = 0; - out_8392468825100105284[21] = 0; - out_8392468825100105284[22] = 0; - out_8392468825100105284[23] = 0; - out_8392468825100105284[24] = 0; - out_8392468825100105284[25] = 1; - out_8392468825100105284[26] = 0; - out_8392468825100105284[27] = 0; - out_8392468825100105284[28] = 1; - out_8392468825100105284[29] = 0; - out_8392468825100105284[30] = 0; - out_8392468825100105284[31] = 0; - out_8392468825100105284[32] = 0; - out_8392468825100105284[33] = 0; - out_8392468825100105284[34] = 0; - out_8392468825100105284[35] = 0; - out_8392468825100105284[36] = 0; - out_8392468825100105284[37] = 0; - out_8392468825100105284[38] = 0; - out_8392468825100105284[39] = 0; - out_8392468825100105284[40] = 0; - out_8392468825100105284[41] = 0; - out_8392468825100105284[42] = 0; - out_8392468825100105284[43] = 0; - out_8392468825100105284[44] = 1; - out_8392468825100105284[45] = 0; - out_8392468825100105284[46] = 0; - out_8392468825100105284[47] = 1; - out_8392468825100105284[48] = 0; - out_8392468825100105284[49] = 0; - out_8392468825100105284[50] = 0; - out_8392468825100105284[51] = 0; - out_8392468825100105284[52] = 0; - out_8392468825100105284[53] = 0; +void H_4(double *state, double *unused, double *out_2821601005274362620) { + out_2821601005274362620[0] = 0; + out_2821601005274362620[1] = 0; + out_2821601005274362620[2] = 0; + out_2821601005274362620[3] = 0; + out_2821601005274362620[4] = 0; + out_2821601005274362620[5] = 0; + out_2821601005274362620[6] = 1; + out_2821601005274362620[7] = 0; + out_2821601005274362620[8] = 0; + out_2821601005274362620[9] = 1; + out_2821601005274362620[10] = 0; + out_2821601005274362620[11] = 0; + out_2821601005274362620[12] = 0; + out_2821601005274362620[13] = 0; + out_2821601005274362620[14] = 0; + out_2821601005274362620[15] = 0; + out_2821601005274362620[16] = 0; + out_2821601005274362620[17] = 0; + out_2821601005274362620[18] = 0; + out_2821601005274362620[19] = 0; + out_2821601005274362620[20] = 0; + out_2821601005274362620[21] = 0; + out_2821601005274362620[22] = 0; + out_2821601005274362620[23] = 0; + out_2821601005274362620[24] = 0; + out_2821601005274362620[25] = 1; + out_2821601005274362620[26] = 0; + out_2821601005274362620[27] = 0; + out_2821601005274362620[28] = 1; + out_2821601005274362620[29] = 0; + out_2821601005274362620[30] = 0; + out_2821601005274362620[31] = 0; + out_2821601005274362620[32] = 0; + out_2821601005274362620[33] = 0; + out_2821601005274362620[34] = 0; + out_2821601005274362620[35] = 0; + out_2821601005274362620[36] = 0; + out_2821601005274362620[37] = 0; + out_2821601005274362620[38] = 0; + out_2821601005274362620[39] = 0; + out_2821601005274362620[40] = 0; + out_2821601005274362620[41] = 0; + out_2821601005274362620[42] = 0; + out_2821601005274362620[43] = 0; + out_2821601005274362620[44] = 1; + out_2821601005274362620[45] = 0; + out_2821601005274362620[46] = 0; + out_2821601005274362620[47] = 1; + out_2821601005274362620[48] = 0; + out_2821601005274362620[49] = 0; + out_2821601005274362620[50] = 0; + out_2821601005274362620[51] = 0; + out_2821601005274362620[52] = 0; + out_2821601005274362620[53] = 0; } -void h_10(double *state, double *unused, double *out_316139620022094761) { - out_316139620022094761[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_316139620022094761[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_316139620022094761[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_5766917160067306951) { + out_5766917160067306951[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_5766917160067306951[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_5766917160067306951[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_3533745887327993443) { - out_3533745887327993443[0] = 0; - out_3533745887327993443[1] = 9.8100000000000005*cos(state[1]); - out_3533745887327993443[2] = 0; - out_3533745887327993443[3] = 0; - out_3533745887327993443[4] = -state[8]; - out_3533745887327993443[5] = state[7]; - out_3533745887327993443[6] = 0; - out_3533745887327993443[7] = state[5]; - out_3533745887327993443[8] = -state[4]; - out_3533745887327993443[9] = 0; - out_3533745887327993443[10] = 0; - out_3533745887327993443[11] = 0; - out_3533745887327993443[12] = 1; - out_3533745887327993443[13] = 0; - out_3533745887327993443[14] = 0; - out_3533745887327993443[15] = 1; - out_3533745887327993443[16] = 0; - out_3533745887327993443[17] = 0; - out_3533745887327993443[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_3533745887327993443[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_3533745887327993443[20] = 0; - out_3533745887327993443[21] = state[8]; - out_3533745887327993443[22] = 0; - out_3533745887327993443[23] = -state[6]; - out_3533745887327993443[24] = -state[5]; - out_3533745887327993443[25] = 0; - out_3533745887327993443[26] = state[3]; - out_3533745887327993443[27] = 0; - out_3533745887327993443[28] = 0; - out_3533745887327993443[29] = 0; - out_3533745887327993443[30] = 0; - out_3533745887327993443[31] = 1; - out_3533745887327993443[32] = 0; - out_3533745887327993443[33] = 0; - out_3533745887327993443[34] = 1; - out_3533745887327993443[35] = 0; - out_3533745887327993443[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_3533745887327993443[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_3533745887327993443[38] = 0; - out_3533745887327993443[39] = -state[7]; - out_3533745887327993443[40] = state[6]; - out_3533745887327993443[41] = 0; - out_3533745887327993443[42] = state[4]; - out_3533745887327993443[43] = -state[3]; - out_3533745887327993443[44] = 0; - out_3533745887327993443[45] = 0; - out_3533745887327993443[46] = 0; - out_3533745887327993443[47] = 0; - out_3533745887327993443[48] = 0; - out_3533745887327993443[49] = 0; - out_3533745887327993443[50] = 1; - out_3533745887327993443[51] = 0; - out_3533745887327993443[52] = 0; - out_3533745887327993443[53] = 1; +void H_10(double *state, double *unused, double *out_393429258268914973) { + out_393429258268914973[0] = 0; + out_393429258268914973[1] = 9.8100000000000005*cos(state[1]); + out_393429258268914973[2] = 0; + out_393429258268914973[3] = 0; + out_393429258268914973[4] = -state[8]; + out_393429258268914973[5] = state[7]; + out_393429258268914973[6] = 0; + out_393429258268914973[7] = state[5]; + out_393429258268914973[8] = -state[4]; + out_393429258268914973[9] = 0; + out_393429258268914973[10] = 0; + out_393429258268914973[11] = 0; + out_393429258268914973[12] = 1; + out_393429258268914973[13] = 0; + out_393429258268914973[14] = 0; + out_393429258268914973[15] = 1; + out_393429258268914973[16] = 0; + out_393429258268914973[17] = 0; + out_393429258268914973[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_393429258268914973[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_393429258268914973[20] = 0; + out_393429258268914973[21] = state[8]; + out_393429258268914973[22] = 0; + out_393429258268914973[23] = -state[6]; + out_393429258268914973[24] = -state[5]; + out_393429258268914973[25] = 0; + out_393429258268914973[26] = state[3]; + out_393429258268914973[27] = 0; + out_393429258268914973[28] = 0; + out_393429258268914973[29] = 0; + out_393429258268914973[30] = 0; + out_393429258268914973[31] = 1; + out_393429258268914973[32] = 0; + out_393429258268914973[33] = 0; + out_393429258268914973[34] = 1; + out_393429258268914973[35] = 0; + out_393429258268914973[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_393429258268914973[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_393429258268914973[38] = 0; + out_393429258268914973[39] = -state[7]; + out_393429258268914973[40] = state[6]; + out_393429258268914973[41] = 0; + out_393429258268914973[42] = state[4]; + out_393429258268914973[43] = -state[3]; + out_393429258268914973[44] = 0; + out_393429258268914973[45] = 0; + out_393429258268914973[46] = 0; + out_393429258268914973[47] = 0; + out_393429258268914973[48] = 0; + out_393429258268914973[49] = 0; + out_393429258268914973[50] = 1; + out_393429258268914973[51] = 0; + out_393429258268914973[52] = 0; + out_393429258268914973[53] = 1; } -void h_13(double *state, double *unused, double *out_5506810827115614970) { - out_5506810827115614970[0] = state[3]; - out_5506810827115614970[1] = state[4]; - out_5506810827115614970[2] = state[5]; +void h_13(double *state, double *unused, double *out_7359463090555700335) { + out_7359463090555700335[0] = state[3]; + out_7359463090555700335[1] = state[4]; + out_7359463090555700335[2] = state[5]; } -void H_13(double *state, double *unused, double *out_781837616783404355) { - out_781837616783404355[0] = 0; - out_781837616783404355[1] = 0; - out_781837616783404355[2] = 0; - out_781837616783404355[3] = 1; - out_781837616783404355[4] = 0; - out_781837616783404355[5] = 0; - out_781837616783404355[6] = 0; - out_781837616783404355[7] = 0; - out_781837616783404355[8] = 0; - out_781837616783404355[9] = 0; - out_781837616783404355[10] = 0; - out_781837616783404355[11] = 0; - out_781837616783404355[12] = 0; - out_781837616783404355[13] = 0; - out_781837616783404355[14] = 0; - out_781837616783404355[15] = 0; - out_781837616783404355[16] = 0; - out_781837616783404355[17] = 0; - out_781837616783404355[18] = 0; - out_781837616783404355[19] = 0; - out_781837616783404355[20] = 0; - out_781837616783404355[21] = 0; - out_781837616783404355[22] = 1; - out_781837616783404355[23] = 0; - out_781837616783404355[24] = 0; - out_781837616783404355[25] = 0; - out_781837616783404355[26] = 0; - out_781837616783404355[27] = 0; - out_781837616783404355[28] = 0; - out_781837616783404355[29] = 0; - out_781837616783404355[30] = 0; - out_781837616783404355[31] = 0; - out_781837616783404355[32] = 0; - out_781837616783404355[33] = 0; - out_781837616783404355[34] = 0; - out_781837616783404355[35] = 0; - out_781837616783404355[36] = 0; - out_781837616783404355[37] = 0; - out_781837616783404355[38] = 0; - out_781837616783404355[39] = 0; - out_781837616783404355[40] = 0; - out_781837616783404355[41] = 1; - out_781837616783404355[42] = 0; - out_781837616783404355[43] = 0; - out_781837616783404355[44] = 0; - out_781837616783404355[45] = 0; - out_781837616783404355[46] = 0; - out_781837616783404355[47] = 0; - out_781837616783404355[48] = 0; - out_781837616783404355[49] = 0; - out_781837616783404355[50] = 0; - out_781837616783404355[51] = 0; - out_781837616783404355[52] = 0; - out_781837616783404355[53] = 0; +void H_13(double *state, double *unused, double *out_6033874830606695421) { + out_6033874830606695421[0] = 0; + out_6033874830606695421[1] = 0; + out_6033874830606695421[2] = 0; + out_6033874830606695421[3] = 1; + out_6033874830606695421[4] = 0; + out_6033874830606695421[5] = 0; + out_6033874830606695421[6] = 0; + out_6033874830606695421[7] = 0; + out_6033874830606695421[8] = 0; + out_6033874830606695421[9] = 0; + out_6033874830606695421[10] = 0; + out_6033874830606695421[11] = 0; + out_6033874830606695421[12] = 0; + out_6033874830606695421[13] = 0; + out_6033874830606695421[14] = 0; + out_6033874830606695421[15] = 0; + out_6033874830606695421[16] = 0; + out_6033874830606695421[17] = 0; + out_6033874830606695421[18] = 0; + out_6033874830606695421[19] = 0; + out_6033874830606695421[20] = 0; + out_6033874830606695421[21] = 0; + out_6033874830606695421[22] = 1; + out_6033874830606695421[23] = 0; + out_6033874830606695421[24] = 0; + out_6033874830606695421[25] = 0; + out_6033874830606695421[26] = 0; + out_6033874830606695421[27] = 0; + out_6033874830606695421[28] = 0; + out_6033874830606695421[29] = 0; + out_6033874830606695421[30] = 0; + out_6033874830606695421[31] = 0; + out_6033874830606695421[32] = 0; + out_6033874830606695421[33] = 0; + out_6033874830606695421[34] = 0; + out_6033874830606695421[35] = 0; + out_6033874830606695421[36] = 0; + out_6033874830606695421[37] = 0; + out_6033874830606695421[38] = 0; + out_6033874830606695421[39] = 0; + out_6033874830606695421[40] = 0; + out_6033874830606695421[41] = 1; + out_6033874830606695421[42] = 0; + out_6033874830606695421[43] = 0; + out_6033874830606695421[44] = 0; + out_6033874830606695421[45] = 0; + out_6033874830606695421[46] = 0; + out_6033874830606695421[47] = 0; + out_6033874830606695421[48] = 0; + out_6033874830606695421[49] = 0; + out_6033874830606695421[50] = 0; + out_6033874830606695421[51] = 0; + out_6033874830606695421[52] = 0; + out_6033874830606695421[53] = 0; } -void h_14(double *state, double *unused, double *out_3429617090731340208) { - out_3429617090731340208[0] = state[6]; - out_3429617090731340208[1] = state[7]; - out_3429617090731340208[2] = state[8]; +void h_14(double *state, double *unused, double *out_1914760434129654014) { + out_1914760434129654014[0] = state[6]; + out_1914760434129654014[1] = state[7]; + out_1914760434129654014[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4429227968760620755) { - out_4429227968760620755[0] = 0; - out_4429227968760620755[1] = 0; - out_4429227968760620755[2] = 0; - out_4429227968760620755[3] = 0; - out_4429227968760620755[4] = 0; - out_4429227968760620755[5] = 0; - out_4429227968760620755[6] = 1; - out_4429227968760620755[7] = 0; - out_4429227968760620755[8] = 0; - out_4429227968760620755[9] = 0; - out_4429227968760620755[10] = 0; - out_4429227968760620755[11] = 0; - out_4429227968760620755[12] = 0; - out_4429227968760620755[13] = 0; - out_4429227968760620755[14] = 0; - out_4429227968760620755[15] = 0; - out_4429227968760620755[16] = 0; - out_4429227968760620755[17] = 0; - out_4429227968760620755[18] = 0; - out_4429227968760620755[19] = 0; - out_4429227968760620755[20] = 0; - out_4429227968760620755[21] = 0; - out_4429227968760620755[22] = 0; - out_4429227968760620755[23] = 0; - out_4429227968760620755[24] = 0; - out_4429227968760620755[25] = 1; - out_4429227968760620755[26] = 0; - out_4429227968760620755[27] = 0; - out_4429227968760620755[28] = 0; - out_4429227968760620755[29] = 0; - out_4429227968760620755[30] = 0; - out_4429227968760620755[31] = 0; - out_4429227968760620755[32] = 0; - out_4429227968760620755[33] = 0; - out_4429227968760620755[34] = 0; - out_4429227968760620755[35] = 0; - out_4429227968760620755[36] = 0; - out_4429227968760620755[37] = 0; - out_4429227968760620755[38] = 0; - out_4429227968760620755[39] = 0; - out_4429227968760620755[40] = 0; - out_4429227968760620755[41] = 0; - out_4429227968760620755[42] = 0; - out_4429227968760620755[43] = 0; - out_4429227968760620755[44] = 1; - out_4429227968760620755[45] = 0; - out_4429227968760620755[46] = 0; - out_4429227968760620755[47] = 0; - out_4429227968760620755[48] = 0; - out_4429227968760620755[49] = 0; - out_4429227968760620755[50] = 0; - out_4429227968760620755[51] = 0; - out_4429227968760620755[52] = 0; - out_4429227968760620755[53] = 0; +void H_14(double *state, double *unused, double *out_261187427021009676) { + out_261187427021009676[0] = 0; + out_261187427021009676[1] = 0; + out_261187427021009676[2] = 0; + out_261187427021009676[3] = 0; + out_261187427021009676[4] = 0; + out_261187427021009676[5] = 0; + out_261187427021009676[6] = 1; + out_261187427021009676[7] = 0; + out_261187427021009676[8] = 0; + out_261187427021009676[9] = 0; + out_261187427021009676[10] = 0; + out_261187427021009676[11] = 0; + out_261187427021009676[12] = 0; + out_261187427021009676[13] = 0; + out_261187427021009676[14] = 0; + out_261187427021009676[15] = 0; + out_261187427021009676[16] = 0; + out_261187427021009676[17] = 0; + out_261187427021009676[18] = 0; + out_261187427021009676[19] = 0; + out_261187427021009676[20] = 0; + out_261187427021009676[21] = 0; + out_261187427021009676[22] = 0; + out_261187427021009676[23] = 0; + out_261187427021009676[24] = 0; + out_261187427021009676[25] = 1; + out_261187427021009676[26] = 0; + out_261187427021009676[27] = 0; + out_261187427021009676[28] = 0; + out_261187427021009676[29] = 0; + out_261187427021009676[30] = 0; + out_261187427021009676[31] = 0; + out_261187427021009676[32] = 0; + out_261187427021009676[33] = 0; + out_261187427021009676[34] = 0; + out_261187427021009676[35] = 0; + out_261187427021009676[36] = 0; + out_261187427021009676[37] = 0; + out_261187427021009676[38] = 0; + out_261187427021009676[39] = 0; + out_261187427021009676[40] = 0; + out_261187427021009676[41] = 0; + out_261187427021009676[42] = 0; + out_261187427021009676[43] = 0; + out_261187427021009676[44] = 1; + out_261187427021009676[45] = 0; + out_261187427021009676[46] = 0; + out_261187427021009676[47] = 0; + out_261187427021009676[48] = 0; + out_261187427021009676[49] = 0; + out_261187427021009676[50] = 0; + out_261187427021009676[51] = 0; + out_261187427021009676[52] = 0; + out_261187427021009676[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_4567767818172860295) { - err_fun(nom_x, delta_x, out_4567767818172860295); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1277882425838003162) { + err_fun(nom_x, delta_x, out_1277882425838003162); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4613807108104960206) { - inv_err_fun(nom_x, true_x, out_4613807108104960206); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5395183656796766) { + inv_err_fun(nom_x, true_x, out_5395183656796766); } -void pose_H_mod_fun(double *state, double *out_6572770099193808423) { - H_mod_fun(state, out_6572770099193808423); +void pose_H_mod_fun(double *state, double *out_4641299731180659481) { + H_mod_fun(state, out_4641299731180659481); } -void pose_f_fun(double *state, double dt, double *out_972215043360367871) { - f_fun(state, dt, out_972215043360367871); +void pose_f_fun(double *state, double dt, double *out_4475655607956648045) { + f_fun(state, dt, out_4475655607956648045); } -void pose_F_fun(double *state, double dt, double *out_9096792433349462064) { - F_fun(state, dt, out_9096792433349462064); +void pose_F_fun(double *state, double dt, double *out_1490364464464661376) { + F_fun(state, dt, out_1490364464464661376); } -void pose_h_4(double *state, double *unused, double *out_2265781100835663155) { - h_4(state, unused, out_2265781100835663155); +void pose_h_4(double *state, double *unused, double *out_4985239385195179173) { + h_4(state, unused, out_4985239385195179173); } -void pose_H_4(double *state, double *unused, double *out_8392468825100105284) { - H_4(state, unused, out_8392468825100105284); +void pose_H_4(double *state, double *unused, double *out_2821601005274362620) { + H_4(state, unused, out_2821601005274362620); } -void pose_h_10(double *state, double *unused, double *out_316139620022094761) { - h_10(state, unused, out_316139620022094761); +void pose_h_10(double *state, double *unused, double *out_5766917160067306951) { + h_10(state, unused, out_5766917160067306951); } -void pose_H_10(double *state, double *unused, double *out_3533745887327993443) { - H_10(state, unused, out_3533745887327993443); +void pose_H_10(double *state, double *unused, double *out_393429258268914973) { + H_10(state, unused, out_393429258268914973); } -void pose_h_13(double *state, double *unused, double *out_5506810827115614970) { - h_13(state, unused, out_5506810827115614970); +void pose_h_13(double *state, double *unused, double *out_7359463090555700335) { + h_13(state, unused, out_7359463090555700335); } -void pose_H_13(double *state, double *unused, double *out_781837616783404355) { - H_13(state, unused, out_781837616783404355); +void pose_H_13(double *state, double *unused, double *out_6033874830606695421) { + H_13(state, unused, out_6033874830606695421); } -void pose_h_14(double *state, double *unused, double *out_3429617090731340208) { - h_14(state, unused, out_3429617090731340208); +void pose_h_14(double *state, double *unused, double *out_1914760434129654014) { + h_14(state, unused, out_1914760434129654014); } -void pose_H_14(double *state, double *unused, double *out_4429227968760620755) { - H_14(state, unused, out_4429227968760620755); +void pose_H_14(double *state, double *unused, double *out_261187427021009676) { + H_14(state, unused, out_261187427021009676); } 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 6de21d4e2..4b3598e18 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_4567767818172860295); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4613807108104960206); -void pose_H_mod_fun(double *state, double *out_6572770099193808423); -void pose_f_fun(double *state, double dt, double *out_972215043360367871); -void pose_F_fun(double *state, double dt, double *out_9096792433349462064); -void pose_h_4(double *state, double *unused, double *out_2265781100835663155); -void pose_H_4(double *state, double *unused, double *out_8392468825100105284); -void pose_h_10(double *state, double *unused, double *out_316139620022094761); -void pose_H_10(double *state, double *unused, double *out_3533745887327993443); -void pose_h_13(double *state, double *unused, double *out_5506810827115614970); -void pose_H_13(double *state, double *unused, double *out_781837616783404355); -void pose_h_14(double *state, double *unused, double *out_3429617090731340208); -void pose_H_14(double *state, double *unused, double *out_4429227968760620755); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1277882425838003162); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5395183656796766); +void pose_H_mod_fun(double *state, double *out_4641299731180659481); +void pose_f_fun(double *state, double dt, double *out_4475655607956648045); +void pose_F_fun(double *state, double dt, double *out_1490364464464661376); +void pose_h_4(double *state, double *unused, double *out_4985239385195179173); +void pose_H_4(double *state, double *unused, double *out_2821601005274362620); +void pose_h_10(double *state, double *unused, double *out_5766917160067306951); +void pose_H_10(double *state, double *unused, double *out_393429258268914973); +void pose_h_13(double *state, double *unused, double *out_7359463090555700335); +void pose_H_13(double *state, double *unused, double *out_6033874830606695421); +void pose_h_14(double *state, double *unused, double *out_1914760434129654014); +void pose_H_14(double *state, double *unused, double *out_261187427021009676); 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 b7d7cbc86..398f86561 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 57c6143d2..4786d948e 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index e166ba195..887a7747a 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index c703dd978..beeda1ca3 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 338429af8..0c1cc0706 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 7a99dec7b..0f18504d3 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ