diff --git a/cereal/libcereal.a b/cereal/libcereal.a index a88a521c5..7a1b02d1b 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/common/libcommon.a b/common/libcommon.a index 6b7769ea8..eefdbb738 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 f09e14802..4180161f6 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 ca256aa8b..893be392d 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 4cd58a9ea..21b9b4a36 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 f77be2e55..5d2ff8fef 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 d4afc3c40..dd11de06f 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 0d40d665b..499aee117 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 809124e2a..336168e81 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 82315a460..1bb58d3aa 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 82315a460..1bb58d3aa 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 797acc5a0..223cfea79 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 809124e2a..336168e81 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 74a554785..b34ebbadb 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-e372ae84-DEBUG"; +const uint8_t gitversion[19] = "DEV-9872c5fb-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 952edc5c9..734ad0752 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 1c171b619..67b23febf 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 9648cf6b9..49d284242 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 6d81737b5..8b53fad86 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 c8c8d7f2b..55afd931c 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 55f9184fb..bc340c1aa 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 c35c5734b..1b2870103 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 d740b2ee3..67a6e0d91 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 3000fdbd7..2c95b9e59 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 547ce49ca..d268ca875 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 51615f0d3..b6279c376 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 989168f23..3f8414200 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 a3c8f90f4..010541fa1 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 72728a192..caff4ac4a 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 8c047162f..2c381d651 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 89326e9dd..ffdba6f67 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 d7f95b071..658c30a3e 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 e66e01fcc..6581a2d9d 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 81c09eb93..a06ea42a7 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 4078a6766..48866407a 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 869abb531..806424228 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-e372ae84-DEBUG \ No newline at end of file +DEV-9872c5fb-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 645b5ee5f..372c81ac9 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_8318180392745555938) { - out_8318180392745555938[0] = delta_x[0] + nom_x[0]; - out_8318180392745555938[1] = delta_x[1] + nom_x[1]; - out_8318180392745555938[2] = delta_x[2] + nom_x[2]; - out_8318180392745555938[3] = delta_x[3] + nom_x[3]; - out_8318180392745555938[4] = delta_x[4] + nom_x[4]; - out_8318180392745555938[5] = delta_x[5] + nom_x[5]; - out_8318180392745555938[6] = delta_x[6] + nom_x[6]; - out_8318180392745555938[7] = delta_x[7] + nom_x[7]; - out_8318180392745555938[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_8928034227051207060) { + out_8928034227051207060[0] = delta_x[0] + nom_x[0]; + out_8928034227051207060[1] = delta_x[1] + nom_x[1]; + out_8928034227051207060[2] = delta_x[2] + nom_x[2]; + out_8928034227051207060[3] = delta_x[3] + nom_x[3]; + out_8928034227051207060[4] = delta_x[4] + nom_x[4]; + out_8928034227051207060[5] = delta_x[5] + nom_x[5]; + out_8928034227051207060[6] = delta_x[6] + nom_x[6]; + out_8928034227051207060[7] = delta_x[7] + nom_x[7]; + out_8928034227051207060[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5934219001129015779) { - out_5934219001129015779[0] = -nom_x[0] + true_x[0]; - out_5934219001129015779[1] = -nom_x[1] + true_x[1]; - out_5934219001129015779[2] = -nom_x[2] + true_x[2]; - out_5934219001129015779[3] = -nom_x[3] + true_x[3]; - out_5934219001129015779[4] = -nom_x[4] + true_x[4]; - out_5934219001129015779[5] = -nom_x[5] + true_x[5]; - out_5934219001129015779[6] = -nom_x[6] + true_x[6]; - out_5934219001129015779[7] = -nom_x[7] + true_x[7]; - out_5934219001129015779[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_4996368579389837639) { + out_4996368579389837639[0] = -nom_x[0] + true_x[0]; + out_4996368579389837639[1] = -nom_x[1] + true_x[1]; + out_4996368579389837639[2] = -nom_x[2] + true_x[2]; + out_4996368579389837639[3] = -nom_x[3] + true_x[3]; + out_4996368579389837639[4] = -nom_x[4] + true_x[4]; + out_4996368579389837639[5] = -nom_x[5] + true_x[5]; + out_4996368579389837639[6] = -nom_x[6] + true_x[6]; + out_4996368579389837639[7] = -nom_x[7] + true_x[7]; + out_4996368579389837639[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_1926641469114901966) { - out_1926641469114901966[0] = 1.0; - out_1926641469114901966[1] = 0.0; - out_1926641469114901966[2] = 0.0; - out_1926641469114901966[3] = 0.0; - out_1926641469114901966[4] = 0.0; - out_1926641469114901966[5] = 0.0; - out_1926641469114901966[6] = 0.0; - out_1926641469114901966[7] = 0.0; - out_1926641469114901966[8] = 0.0; - out_1926641469114901966[9] = 0.0; - out_1926641469114901966[10] = 1.0; - out_1926641469114901966[11] = 0.0; - out_1926641469114901966[12] = 0.0; - out_1926641469114901966[13] = 0.0; - out_1926641469114901966[14] = 0.0; - out_1926641469114901966[15] = 0.0; - out_1926641469114901966[16] = 0.0; - out_1926641469114901966[17] = 0.0; - out_1926641469114901966[18] = 0.0; - out_1926641469114901966[19] = 0.0; - out_1926641469114901966[20] = 1.0; - out_1926641469114901966[21] = 0.0; - out_1926641469114901966[22] = 0.0; - out_1926641469114901966[23] = 0.0; - out_1926641469114901966[24] = 0.0; - out_1926641469114901966[25] = 0.0; - out_1926641469114901966[26] = 0.0; - out_1926641469114901966[27] = 0.0; - out_1926641469114901966[28] = 0.0; - out_1926641469114901966[29] = 0.0; - out_1926641469114901966[30] = 1.0; - out_1926641469114901966[31] = 0.0; - out_1926641469114901966[32] = 0.0; - out_1926641469114901966[33] = 0.0; - out_1926641469114901966[34] = 0.0; - out_1926641469114901966[35] = 0.0; - out_1926641469114901966[36] = 0.0; - out_1926641469114901966[37] = 0.0; - out_1926641469114901966[38] = 0.0; - out_1926641469114901966[39] = 0.0; - out_1926641469114901966[40] = 1.0; - out_1926641469114901966[41] = 0.0; - out_1926641469114901966[42] = 0.0; - out_1926641469114901966[43] = 0.0; - out_1926641469114901966[44] = 0.0; - out_1926641469114901966[45] = 0.0; - out_1926641469114901966[46] = 0.0; - out_1926641469114901966[47] = 0.0; - out_1926641469114901966[48] = 0.0; - out_1926641469114901966[49] = 0.0; - out_1926641469114901966[50] = 1.0; - out_1926641469114901966[51] = 0.0; - out_1926641469114901966[52] = 0.0; - out_1926641469114901966[53] = 0.0; - out_1926641469114901966[54] = 0.0; - out_1926641469114901966[55] = 0.0; - out_1926641469114901966[56] = 0.0; - out_1926641469114901966[57] = 0.0; - out_1926641469114901966[58] = 0.0; - out_1926641469114901966[59] = 0.0; - out_1926641469114901966[60] = 1.0; - out_1926641469114901966[61] = 0.0; - out_1926641469114901966[62] = 0.0; - out_1926641469114901966[63] = 0.0; - out_1926641469114901966[64] = 0.0; - out_1926641469114901966[65] = 0.0; - out_1926641469114901966[66] = 0.0; - out_1926641469114901966[67] = 0.0; - out_1926641469114901966[68] = 0.0; - out_1926641469114901966[69] = 0.0; - out_1926641469114901966[70] = 1.0; - out_1926641469114901966[71] = 0.0; - out_1926641469114901966[72] = 0.0; - out_1926641469114901966[73] = 0.0; - out_1926641469114901966[74] = 0.0; - out_1926641469114901966[75] = 0.0; - out_1926641469114901966[76] = 0.0; - out_1926641469114901966[77] = 0.0; - out_1926641469114901966[78] = 0.0; - out_1926641469114901966[79] = 0.0; - out_1926641469114901966[80] = 1.0; +void H_mod_fun(double *state, double *out_8354260345920403618) { + out_8354260345920403618[0] = 1.0; + out_8354260345920403618[1] = 0.0; + out_8354260345920403618[2] = 0.0; + out_8354260345920403618[3] = 0.0; + out_8354260345920403618[4] = 0.0; + out_8354260345920403618[5] = 0.0; + out_8354260345920403618[6] = 0.0; + out_8354260345920403618[7] = 0.0; + out_8354260345920403618[8] = 0.0; + out_8354260345920403618[9] = 0.0; + out_8354260345920403618[10] = 1.0; + out_8354260345920403618[11] = 0.0; + out_8354260345920403618[12] = 0.0; + out_8354260345920403618[13] = 0.0; + out_8354260345920403618[14] = 0.0; + out_8354260345920403618[15] = 0.0; + out_8354260345920403618[16] = 0.0; + out_8354260345920403618[17] = 0.0; + out_8354260345920403618[18] = 0.0; + out_8354260345920403618[19] = 0.0; + out_8354260345920403618[20] = 1.0; + out_8354260345920403618[21] = 0.0; + out_8354260345920403618[22] = 0.0; + out_8354260345920403618[23] = 0.0; + out_8354260345920403618[24] = 0.0; + out_8354260345920403618[25] = 0.0; + out_8354260345920403618[26] = 0.0; + out_8354260345920403618[27] = 0.0; + out_8354260345920403618[28] = 0.0; + out_8354260345920403618[29] = 0.0; + out_8354260345920403618[30] = 1.0; + out_8354260345920403618[31] = 0.0; + out_8354260345920403618[32] = 0.0; + out_8354260345920403618[33] = 0.0; + out_8354260345920403618[34] = 0.0; + out_8354260345920403618[35] = 0.0; + out_8354260345920403618[36] = 0.0; + out_8354260345920403618[37] = 0.0; + out_8354260345920403618[38] = 0.0; + out_8354260345920403618[39] = 0.0; + out_8354260345920403618[40] = 1.0; + out_8354260345920403618[41] = 0.0; + out_8354260345920403618[42] = 0.0; + out_8354260345920403618[43] = 0.0; + out_8354260345920403618[44] = 0.0; + out_8354260345920403618[45] = 0.0; + out_8354260345920403618[46] = 0.0; + out_8354260345920403618[47] = 0.0; + out_8354260345920403618[48] = 0.0; + out_8354260345920403618[49] = 0.0; + out_8354260345920403618[50] = 1.0; + out_8354260345920403618[51] = 0.0; + out_8354260345920403618[52] = 0.0; + out_8354260345920403618[53] = 0.0; + out_8354260345920403618[54] = 0.0; + out_8354260345920403618[55] = 0.0; + out_8354260345920403618[56] = 0.0; + out_8354260345920403618[57] = 0.0; + out_8354260345920403618[58] = 0.0; + out_8354260345920403618[59] = 0.0; + out_8354260345920403618[60] = 1.0; + out_8354260345920403618[61] = 0.0; + out_8354260345920403618[62] = 0.0; + out_8354260345920403618[63] = 0.0; + out_8354260345920403618[64] = 0.0; + out_8354260345920403618[65] = 0.0; + out_8354260345920403618[66] = 0.0; + out_8354260345920403618[67] = 0.0; + out_8354260345920403618[68] = 0.0; + out_8354260345920403618[69] = 0.0; + out_8354260345920403618[70] = 1.0; + out_8354260345920403618[71] = 0.0; + out_8354260345920403618[72] = 0.0; + out_8354260345920403618[73] = 0.0; + out_8354260345920403618[74] = 0.0; + out_8354260345920403618[75] = 0.0; + out_8354260345920403618[76] = 0.0; + out_8354260345920403618[77] = 0.0; + out_8354260345920403618[78] = 0.0; + out_8354260345920403618[79] = 0.0; + out_8354260345920403618[80] = 1.0; } -void f_fun(double *state, double dt, double *out_5221835679523029107) { - out_5221835679523029107[0] = state[0]; - out_5221835679523029107[1] = state[1]; - out_5221835679523029107[2] = state[2]; - out_5221835679523029107[3] = state[3]; - out_5221835679523029107[4] = state[4]; - out_5221835679523029107[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5]; - out_5221835679523029107[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6]; - out_5221835679523029107[7] = state[7]; - out_5221835679523029107[8] = state[8]; +void f_fun(double *state, double dt, double *out_18975094040046356) { + out_18975094040046356[0] = state[0]; + out_18975094040046356[1] = state[1]; + out_18975094040046356[2] = state[2]; + out_18975094040046356[3] = state[3]; + out_18975094040046356[4] = state[4]; + out_18975094040046356[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_18975094040046356[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_18975094040046356[7] = state[7]; + out_18975094040046356[8] = state[8]; } -void F_fun(double *state, double dt, double *out_1809598832794027362) { - out_1809598832794027362[0] = 1; - out_1809598832794027362[1] = 0; - out_1809598832794027362[2] = 0; - out_1809598832794027362[3] = 0; - out_1809598832794027362[4] = 0; - out_1809598832794027362[5] = 0; - out_1809598832794027362[6] = 0; - out_1809598832794027362[7] = 0; - out_1809598832794027362[8] = 0; - out_1809598832794027362[9] = 0; - out_1809598832794027362[10] = 1; - out_1809598832794027362[11] = 0; - out_1809598832794027362[12] = 0; - out_1809598832794027362[13] = 0; - out_1809598832794027362[14] = 0; - out_1809598832794027362[15] = 0; - out_1809598832794027362[16] = 0; - out_1809598832794027362[17] = 0; - out_1809598832794027362[18] = 0; - out_1809598832794027362[19] = 0; - out_1809598832794027362[20] = 1; - out_1809598832794027362[21] = 0; - out_1809598832794027362[22] = 0; - out_1809598832794027362[23] = 0; - out_1809598832794027362[24] = 0; - out_1809598832794027362[25] = 0; - out_1809598832794027362[26] = 0; - out_1809598832794027362[27] = 0; - out_1809598832794027362[28] = 0; - out_1809598832794027362[29] = 0; - out_1809598832794027362[30] = 1; - out_1809598832794027362[31] = 0; - out_1809598832794027362[32] = 0; - out_1809598832794027362[33] = 0; - out_1809598832794027362[34] = 0; - out_1809598832794027362[35] = 0; - out_1809598832794027362[36] = 0; - out_1809598832794027362[37] = 0; - out_1809598832794027362[38] = 0; - out_1809598832794027362[39] = 0; - out_1809598832794027362[40] = 1; - out_1809598832794027362[41] = 0; - out_1809598832794027362[42] = 0; - out_1809598832794027362[43] = 0; - out_1809598832794027362[44] = 0; - out_1809598832794027362[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4])); - out_1809598832794027362[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_1809598832794027362[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_1809598832794027362[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_1809598832794027362[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2))); - out_1809598832794027362[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_1809598832794027362[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_1809598832794027362[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_1809598832794027362[53] = -9.8100000000000005*dt; - out_1809598832794027362[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4])); - out_1809598832794027362[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_1809598832794027362[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_1809598832794027362[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_1809598832794027362[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2))); - out_1809598832794027362[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_1809598832794027362[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1; - out_1809598832794027362[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_1809598832794027362[62] = 0; - out_1809598832794027362[63] = 0; - out_1809598832794027362[64] = 0; - out_1809598832794027362[65] = 0; - out_1809598832794027362[66] = 0; - out_1809598832794027362[67] = 0; - out_1809598832794027362[68] = 0; - out_1809598832794027362[69] = 0; - out_1809598832794027362[70] = 1; - out_1809598832794027362[71] = 0; - out_1809598832794027362[72] = 0; - out_1809598832794027362[73] = 0; - out_1809598832794027362[74] = 0; - out_1809598832794027362[75] = 0; - out_1809598832794027362[76] = 0; - out_1809598832794027362[77] = 0; - out_1809598832794027362[78] = 0; - out_1809598832794027362[79] = 0; - out_1809598832794027362[80] = 1; +void F_fun(double *state, double dt, double *out_8298920985484652828) { + out_8298920985484652828[0] = 1; + out_8298920985484652828[1] = 0; + out_8298920985484652828[2] = 0; + out_8298920985484652828[3] = 0; + out_8298920985484652828[4] = 0; + out_8298920985484652828[5] = 0; + out_8298920985484652828[6] = 0; + out_8298920985484652828[7] = 0; + out_8298920985484652828[8] = 0; + out_8298920985484652828[9] = 0; + out_8298920985484652828[10] = 1; + out_8298920985484652828[11] = 0; + out_8298920985484652828[12] = 0; + out_8298920985484652828[13] = 0; + out_8298920985484652828[14] = 0; + out_8298920985484652828[15] = 0; + out_8298920985484652828[16] = 0; + out_8298920985484652828[17] = 0; + out_8298920985484652828[18] = 0; + out_8298920985484652828[19] = 0; + out_8298920985484652828[20] = 1; + out_8298920985484652828[21] = 0; + out_8298920985484652828[22] = 0; + out_8298920985484652828[23] = 0; + out_8298920985484652828[24] = 0; + out_8298920985484652828[25] = 0; + out_8298920985484652828[26] = 0; + out_8298920985484652828[27] = 0; + out_8298920985484652828[28] = 0; + out_8298920985484652828[29] = 0; + out_8298920985484652828[30] = 1; + out_8298920985484652828[31] = 0; + out_8298920985484652828[32] = 0; + out_8298920985484652828[33] = 0; + out_8298920985484652828[34] = 0; + out_8298920985484652828[35] = 0; + out_8298920985484652828[36] = 0; + out_8298920985484652828[37] = 0; + out_8298920985484652828[38] = 0; + out_8298920985484652828[39] = 0; + out_8298920985484652828[40] = 1; + out_8298920985484652828[41] = 0; + out_8298920985484652828[42] = 0; + out_8298920985484652828[43] = 0; + out_8298920985484652828[44] = 0; + out_8298920985484652828[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_8298920985484652828[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_8298920985484652828[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8298920985484652828[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8298920985484652828[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_8298920985484652828[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_8298920985484652828[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_8298920985484652828[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_8298920985484652828[53] = -9.8100000000000005*dt; + out_8298920985484652828[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_8298920985484652828[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_8298920985484652828[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8298920985484652828[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8298920985484652828[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_8298920985484652828[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_8298920985484652828[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_8298920985484652828[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8298920985484652828[62] = 0; + out_8298920985484652828[63] = 0; + out_8298920985484652828[64] = 0; + out_8298920985484652828[65] = 0; + out_8298920985484652828[66] = 0; + out_8298920985484652828[67] = 0; + out_8298920985484652828[68] = 0; + out_8298920985484652828[69] = 0; + out_8298920985484652828[70] = 1; + out_8298920985484652828[71] = 0; + out_8298920985484652828[72] = 0; + out_8298920985484652828[73] = 0; + out_8298920985484652828[74] = 0; + out_8298920985484652828[75] = 0; + out_8298920985484652828[76] = 0; + out_8298920985484652828[77] = 0; + out_8298920985484652828[78] = 0; + out_8298920985484652828[79] = 0; + out_8298920985484652828[80] = 1; } -void h_25(double *state, double *unused, double *out_1465140640781784100) { - out_1465140640781784100[0] = state[6]; +void h_25(double *state, double *unused, double *out_3416906007265727326) { + out_3416906007265727326[0] = state[6]; } -void H_25(double *state, double *unused, double *out_4563198394130328160) { - out_4563198394130328160[0] = 0; - out_4563198394130328160[1] = 0; - out_4563198394130328160[2] = 0; - out_4563198394130328160[3] = 0; - out_4563198394130328160[4] = 0; - out_4563198394130328160[5] = 0; - out_4563198394130328160[6] = 1; - out_4563198394130328160[7] = 0; - out_4563198394130328160[8] = 0; +void H_25(double *state, double *unused, double *out_6569503077799656321) { + out_6569503077799656321[0] = 0; + out_6569503077799656321[1] = 0; + out_6569503077799656321[2] = 0; + out_6569503077799656321[3] = 0; + out_6569503077799656321[4] = 0; + out_6569503077799656321[5] = 0; + out_6569503077799656321[6] = 1; + out_6569503077799656321[7] = 0; + out_6569503077799656321[8] = 0; } -void h_24(double *state, double *unused, double *out_1585421017382478864) { - out_1585421017382478864[0] = state[4]; - out_1585421017382478864[1] = state[5]; +void h_24(double *state, double *unused, double *out_6593726717602388151) { + out_6593726717602388151[0] = state[4]; + out_6593726717602388151[1] = state[5]; } -void H_24(double *state, double *unused, double *out_6440871086590507136) { - out_6440871086590507136[0] = 0; - out_6440871086590507136[1] = 0; - out_6440871086590507136[2] = 0; - out_6440871086590507136[3] = 0; - out_6440871086590507136[4] = 1; - out_6440871086590507136[5] = 0; - out_6440871086590507136[6] = 0; - out_6440871086590507136[7] = 0; - out_6440871086590507136[8] = 0; - out_6440871086590507136[9] = 0; - out_6440871086590507136[10] = 0; - out_6440871086590507136[11] = 0; - out_6440871086590507136[12] = 0; - out_6440871086590507136[13] = 0; - out_6440871086590507136[14] = 1; - out_6440871086590507136[15] = 0; - out_6440871086590507136[16] = 0; - out_6440871086590507136[17] = 0; +void H_24(double *state, double *unused, double *out_3011577478997333185) { + out_3011577478997333185[0] = 0; + out_3011577478997333185[1] = 0; + out_3011577478997333185[2] = 0; + out_3011577478997333185[3] = 0; + out_3011577478997333185[4] = 1; + out_3011577478997333185[5] = 0; + out_3011577478997333185[6] = 0; + out_3011577478997333185[7] = 0; + out_3011577478997333185[8] = 0; + out_3011577478997333185[9] = 0; + out_3011577478997333185[10] = 0; + out_3011577478997333185[11] = 0; + out_3011577478997333185[12] = 0; + out_3011577478997333185[13] = 0; + out_3011577478997333185[14] = 1; + out_3011577478997333185[15] = 0; + out_3011577478997333185[16] = 0; + out_3011577478997333185[17] = 0; } -void h_30(double *state, double *unused, double *out_1189946578497278211) { - out_1189946578497278211[0] = state[4]; +void h_30(double *state, double *unused, double *out_4267886705157640301) { + out_4267886705157640301[0] = state[4]; } -void H_30(double *state, double *unused, double *out_4692537341273568230) { - out_4692537341273568230[0] = 0; - out_4692537341273568230[1] = 0; - out_4692537341273568230[2] = 0; - out_4692537341273568230[3] = 0; - out_4692537341273568230[4] = 1; - out_4692537341273568230[5] = 0; - out_4692537341273568230[6] = 0; - out_4692537341273568230[7] = 0; - out_4692537341273568230[8] = 0; +void H_30(double *state, double *unused, double *out_6698842024942896391) { + out_6698842024942896391[0] = 0; + out_6698842024942896391[1] = 0; + out_6698842024942896391[2] = 0; + out_6698842024942896391[3] = 0; + out_6698842024942896391[4] = 1; + out_6698842024942896391[5] = 0; + out_6698842024942896391[6] = 0; + out_6698842024942896391[7] = 0; + out_6698842024942896391[8] = 0; } -void h_26(double *state, double *unused, double *out_1793262463173755728) { - out_1793262463173755728[0] = state[7]; +void h_26(double *state, double *unused, double *out_9177184100891065670) { + out_9177184100891065670[0] = state[7]; } -void H_26(double *state, double *unused, double *out_8304701713004384384) { - out_8304701713004384384[0] = 0; - out_8304701713004384384[1] = 0; - out_8304701713004384384[2] = 0; - out_8304701713004384384[3] = 0; - out_8304701713004384384[4] = 0; - out_8304701713004384384[5] = 0; - out_8304701713004384384[6] = 0; - out_8304701713004384384[7] = 1; - out_8304701713004384384[8] = 0; +void H_26(double *state, double *unused, double *out_8135737677035839071) { + out_8135737677035839071[0] = 0; + out_8135737677035839071[1] = 0; + out_8135737677035839071[2] = 0; + out_8135737677035839071[3] = 0; + out_8135737677035839071[4] = 0; + out_8135737677035839071[5] = 0; + out_8135737677035839071[6] = 0; + out_8135737677035839071[7] = 1; + out_8135737677035839071[8] = 0; } -void h_27(double *state, double *unused, double *out_7864182158195633246) { - out_7864182158195633246[0] = state[3]; +void h_27(double *state, double *unused, double *out_6233528602177827150) { + out_6233528602177827150[0] = state[3]; } -void H_27(double *state, double *unused, double *out_6867300653073993141) { - out_6867300653073993141[0] = 0; - out_6867300653073993141[1] = 0; - out_6867300653073993141[2] = 0; - out_6867300653073993141[3] = 1; - out_6867300653073993141[4] = 0; - out_6867300653073993141[5] = 0; - out_6867300653073993141[6] = 0; - out_6867300653073993141[7] = 0; - out_6867300653073993141[8] = 0; +void H_27(double *state, double *unused, double *out_8873605336743321302) { + out_8873605336743321302[0] = 0; + out_8873605336743321302[1] = 0; + out_8873605336743321302[2] = 0; + out_8873605336743321302[3] = 1; + out_8873605336743321302[4] = 0; + out_8873605336743321302[5] = 0; + out_8873605336743321302[6] = 0; + out_8873605336743321302[7] = 0; + out_8873605336743321302[8] = 0; } -void h_29(double *state, double *unused, double *out_4002498681767131594) { - out_4002498681767131594[0] = state[1]; +void h_29(double *state, double *unused, double *out_3440336872306683482) { + out_3440336872306683482[0] = state[1]; } -void H_29(double *state, double *unused, double *out_8580663379943544174) { - out_8580663379943544174[0] = 0; - out_8580663379943544174[1] = 1; - out_8580663379943544174[2] = 0; - out_8580663379943544174[3] = 0; - out_8580663379943544174[4] = 0; - out_8580663379943544174[5] = 0; - out_8580663379943544174[6] = 0; - out_8580663379943544174[7] = 0; - out_8580663379943544174[8] = 0; +void H_29(double *state, double *unused, double *out_6188610680628504207) { + out_6188610680628504207[0] = 0; + out_6188610680628504207[1] = 1; + out_6188610680628504207[2] = 0; + out_6188610680628504207[3] = 0; + out_6188610680628504207[4] = 0; + out_6188610680628504207[5] = 0; + out_6188610680628504207[6] = 0; + out_6188610680628504207[7] = 0; + out_6188610680628504207[8] = 0; } -void h_28(double *state, double *unused, double *out_8807692011097295681) { - out_8807692011097295681[0] = state[0]; +void h_28(double *state, double *unused, double *out_2162754552967525717) { + out_2162754552967525717[0] = state[0]; } -void H_28(double *state, double *unused, double *out_4783681676696476868) { - out_4783681676696476868[0] = 1; - out_4783681676696476868[1] = 0; - out_4783681676696476868[2] = 0; - out_4783681676696476868[3] = 0; - out_4783681676696476868[4] = 0; - out_4783681676696476868[5] = 0; - out_4783681676696476868[6] = 0; - out_4783681676696476868[7] = 0; - out_4783681676696476868[8] = 0; +void H_28(double *state, double *unused, double *out_2777376993027148707) { + out_2777376993027148707[0] = 1; + out_2777376993027148707[1] = 0; + out_2777376993027148707[2] = 0; + out_2777376993027148707[3] = 0; + out_2777376993027148707[4] = 0; + out_2777376993027148707[5] = 0; + out_2777376993027148707[6] = 0; + out_2777376993027148707[7] = 0; + out_2777376993027148707[8] = 0; } -void h_31(double *state, double *unused, double *out_7031520040481955892) { - out_7031520040481955892[0] = state[8]; +void h_31(double *state, double *unused, double *out_8180530295696168540) { + out_8180530295696168540[0] = state[8]; } -void H_31(double *state, double *unused, double *out_4532552432253367732) { - out_4532552432253367732[0] = 0; - out_4532552432253367732[1] = 0; - out_4532552432253367732[2] = 0; - out_4532552432253367732[3] = 0; - out_4532552432253367732[4] = 0; - out_4532552432253367732[5] = 0; - out_4532552432253367732[6] = 0; - out_4532552432253367732[7] = 0; - out_4532552432253367732[8] = 1; +void H_31(double *state, double *unused, double *out_6538857115922695893) { + out_6538857115922695893[0] = 0; + out_6538857115922695893[1] = 0; + out_6538857115922695893[2] = 0; + out_6538857115922695893[3] = 0; + out_6538857115922695893[4] = 0; + out_6538857115922695893[5] = 0; + out_6538857115922695893[6] = 0; + out_6538857115922695893[7] = 0; + out_6538857115922695893[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_8318180392745555938) { - err_fun(nom_x, delta_x, out_8318180392745555938); +void car_err_fun(double *nom_x, double *delta_x, double *out_8928034227051207060) { + err_fun(nom_x, delta_x, out_8928034227051207060); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_5934219001129015779) { - inv_err_fun(nom_x, true_x, out_5934219001129015779); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_4996368579389837639) { + inv_err_fun(nom_x, true_x, out_4996368579389837639); } -void car_H_mod_fun(double *state, double *out_1926641469114901966) { - H_mod_fun(state, out_1926641469114901966); +void car_H_mod_fun(double *state, double *out_8354260345920403618) { + H_mod_fun(state, out_8354260345920403618); } -void car_f_fun(double *state, double dt, double *out_5221835679523029107) { - f_fun(state, dt, out_5221835679523029107); +void car_f_fun(double *state, double dt, double *out_18975094040046356) { + f_fun(state, dt, out_18975094040046356); } -void car_F_fun(double *state, double dt, double *out_1809598832794027362) { - F_fun(state, dt, out_1809598832794027362); +void car_F_fun(double *state, double dt, double *out_8298920985484652828) { + F_fun(state, dt, out_8298920985484652828); } -void car_h_25(double *state, double *unused, double *out_1465140640781784100) { - h_25(state, unused, out_1465140640781784100); +void car_h_25(double *state, double *unused, double *out_3416906007265727326) { + h_25(state, unused, out_3416906007265727326); } -void car_H_25(double *state, double *unused, double *out_4563198394130328160) { - H_25(state, unused, out_4563198394130328160); +void car_H_25(double *state, double *unused, double *out_6569503077799656321) { + H_25(state, unused, out_6569503077799656321); } -void car_h_24(double *state, double *unused, double *out_1585421017382478864) { - h_24(state, unused, out_1585421017382478864); +void car_h_24(double *state, double *unused, double *out_6593726717602388151) { + h_24(state, unused, out_6593726717602388151); } -void car_H_24(double *state, double *unused, double *out_6440871086590507136) { - H_24(state, unused, out_6440871086590507136); +void car_H_24(double *state, double *unused, double *out_3011577478997333185) { + H_24(state, unused, out_3011577478997333185); } -void car_h_30(double *state, double *unused, double *out_1189946578497278211) { - h_30(state, unused, out_1189946578497278211); +void car_h_30(double *state, double *unused, double *out_4267886705157640301) { + h_30(state, unused, out_4267886705157640301); } -void car_H_30(double *state, double *unused, double *out_4692537341273568230) { - H_30(state, unused, out_4692537341273568230); +void car_H_30(double *state, double *unused, double *out_6698842024942896391) { + H_30(state, unused, out_6698842024942896391); } -void car_h_26(double *state, double *unused, double *out_1793262463173755728) { - h_26(state, unused, out_1793262463173755728); +void car_h_26(double *state, double *unused, double *out_9177184100891065670) { + h_26(state, unused, out_9177184100891065670); } -void car_H_26(double *state, double *unused, double *out_8304701713004384384) { - H_26(state, unused, out_8304701713004384384); +void car_H_26(double *state, double *unused, double *out_8135737677035839071) { + H_26(state, unused, out_8135737677035839071); } -void car_h_27(double *state, double *unused, double *out_7864182158195633246) { - h_27(state, unused, out_7864182158195633246); +void car_h_27(double *state, double *unused, double *out_6233528602177827150) { + h_27(state, unused, out_6233528602177827150); } -void car_H_27(double *state, double *unused, double *out_6867300653073993141) { - H_27(state, unused, out_6867300653073993141); +void car_H_27(double *state, double *unused, double *out_8873605336743321302) { + H_27(state, unused, out_8873605336743321302); } -void car_h_29(double *state, double *unused, double *out_4002498681767131594) { - h_29(state, unused, out_4002498681767131594); +void car_h_29(double *state, double *unused, double *out_3440336872306683482) { + h_29(state, unused, out_3440336872306683482); } -void car_H_29(double *state, double *unused, double *out_8580663379943544174) { - H_29(state, unused, out_8580663379943544174); +void car_H_29(double *state, double *unused, double *out_6188610680628504207) { + H_29(state, unused, out_6188610680628504207); } -void car_h_28(double *state, double *unused, double *out_8807692011097295681) { - h_28(state, unused, out_8807692011097295681); +void car_h_28(double *state, double *unused, double *out_2162754552967525717) { + h_28(state, unused, out_2162754552967525717); } -void car_H_28(double *state, double *unused, double *out_4783681676696476868) { - H_28(state, unused, out_4783681676696476868); +void car_H_28(double *state, double *unused, double *out_2777376993027148707) { + H_28(state, unused, out_2777376993027148707); } -void car_h_31(double *state, double *unused, double *out_7031520040481955892) { - h_31(state, unused, out_7031520040481955892); +void car_h_31(double *state, double *unused, double *out_8180530295696168540) { + h_31(state, unused, out_8180530295696168540); } -void car_H_31(double *state, double *unused, double *out_4532552432253367732) { - H_31(state, unused, out_4532552432253367732); +void car_H_31(double *state, double *unused, double *out_6538857115922695893) { + H_31(state, unused, out_6538857115922695893); } 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 a5bfcbd79..7ece7960f 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_8318180392745555938); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_5934219001129015779); -void car_H_mod_fun(double *state, double *out_1926641469114901966); -void car_f_fun(double *state, double dt, double *out_5221835679523029107); -void car_F_fun(double *state, double dt, double *out_1809598832794027362); -void car_h_25(double *state, double *unused, double *out_1465140640781784100); -void car_H_25(double *state, double *unused, double *out_4563198394130328160); -void car_h_24(double *state, double *unused, double *out_1585421017382478864); -void car_H_24(double *state, double *unused, double *out_6440871086590507136); -void car_h_30(double *state, double *unused, double *out_1189946578497278211); -void car_H_30(double *state, double *unused, double *out_4692537341273568230); -void car_h_26(double *state, double *unused, double *out_1793262463173755728); -void car_H_26(double *state, double *unused, double *out_8304701713004384384); -void car_h_27(double *state, double *unused, double *out_7864182158195633246); -void car_H_27(double *state, double *unused, double *out_6867300653073993141); -void car_h_29(double *state, double *unused, double *out_4002498681767131594); -void car_H_29(double *state, double *unused, double *out_8580663379943544174); -void car_h_28(double *state, double *unused, double *out_8807692011097295681); -void car_H_28(double *state, double *unused, double *out_4783681676696476868); -void car_h_31(double *state, double *unused, double *out_7031520040481955892); -void car_H_31(double *state, double *unused, double *out_4532552432253367732); +void car_err_fun(double *nom_x, double *delta_x, double *out_8928034227051207060); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_4996368579389837639); +void car_H_mod_fun(double *state, double *out_8354260345920403618); +void car_f_fun(double *state, double dt, double *out_18975094040046356); +void car_F_fun(double *state, double dt, double *out_8298920985484652828); +void car_h_25(double *state, double *unused, double *out_3416906007265727326); +void car_H_25(double *state, double *unused, double *out_6569503077799656321); +void car_h_24(double *state, double *unused, double *out_6593726717602388151); +void car_H_24(double *state, double *unused, double *out_3011577478997333185); +void car_h_30(double *state, double *unused, double *out_4267886705157640301); +void car_H_30(double *state, double *unused, double *out_6698842024942896391); +void car_h_26(double *state, double *unused, double *out_9177184100891065670); +void car_H_26(double *state, double *unused, double *out_8135737677035839071); +void car_h_27(double *state, double *unused, double *out_6233528602177827150); +void car_H_27(double *state, double *unused, double *out_8873605336743321302); +void car_h_29(double *state, double *unused, double *out_3440336872306683482); +void car_H_29(double *state, double *unused, double *out_6188610680628504207); +void car_h_28(double *state, double *unused, double *out_2162754552967525717); +void car_H_28(double *state, double *unused, double *out_2777376993027148707); +void car_h_31(double *state, double *unused, double *out_8180530295696168540); +void car_H_31(double *state, double *unused, double *out_6538857115922695893); 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 863baa776..264cf9a2a 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_7993353667610184364) { - out_7993353667610184364[0] = delta_x[0] + nom_x[0]; - out_7993353667610184364[1] = delta_x[1] + nom_x[1]; - out_7993353667610184364[2] = delta_x[2] + nom_x[2]; - out_7993353667610184364[3] = delta_x[3] + nom_x[3]; - out_7993353667610184364[4] = delta_x[4] + nom_x[4]; - out_7993353667610184364[5] = delta_x[5] + nom_x[5]; - out_7993353667610184364[6] = delta_x[6] + nom_x[6]; - out_7993353667610184364[7] = delta_x[7] + nom_x[7]; - out_7993353667610184364[8] = delta_x[8] + nom_x[8]; - out_7993353667610184364[9] = delta_x[9] + nom_x[9]; - out_7993353667610184364[10] = delta_x[10] + nom_x[10]; - out_7993353667610184364[11] = delta_x[11] + nom_x[11]; - out_7993353667610184364[12] = delta_x[12] + nom_x[12]; - out_7993353667610184364[13] = delta_x[13] + nom_x[13]; - out_7993353667610184364[14] = delta_x[14] + nom_x[14]; - out_7993353667610184364[15] = delta_x[15] + nom_x[15]; - out_7993353667610184364[16] = delta_x[16] + nom_x[16]; - out_7993353667610184364[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_6009088715125097596) { + out_6009088715125097596[0] = delta_x[0] + nom_x[0]; + out_6009088715125097596[1] = delta_x[1] + nom_x[1]; + out_6009088715125097596[2] = delta_x[2] + nom_x[2]; + out_6009088715125097596[3] = delta_x[3] + nom_x[3]; + out_6009088715125097596[4] = delta_x[4] + nom_x[4]; + out_6009088715125097596[5] = delta_x[5] + nom_x[5]; + out_6009088715125097596[6] = delta_x[6] + nom_x[6]; + out_6009088715125097596[7] = delta_x[7] + nom_x[7]; + out_6009088715125097596[8] = delta_x[8] + nom_x[8]; + out_6009088715125097596[9] = delta_x[9] + nom_x[9]; + out_6009088715125097596[10] = delta_x[10] + nom_x[10]; + out_6009088715125097596[11] = delta_x[11] + nom_x[11]; + out_6009088715125097596[12] = delta_x[12] + nom_x[12]; + out_6009088715125097596[13] = delta_x[13] + nom_x[13]; + out_6009088715125097596[14] = delta_x[14] + nom_x[14]; + out_6009088715125097596[15] = delta_x[15] + nom_x[15]; + out_6009088715125097596[16] = delta_x[16] + nom_x[16]; + out_6009088715125097596[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_533608207331381553) { - out_533608207331381553[0] = -nom_x[0] + true_x[0]; - out_533608207331381553[1] = -nom_x[1] + true_x[1]; - out_533608207331381553[2] = -nom_x[2] + true_x[2]; - out_533608207331381553[3] = -nom_x[3] + true_x[3]; - out_533608207331381553[4] = -nom_x[4] + true_x[4]; - out_533608207331381553[5] = -nom_x[5] + true_x[5]; - out_533608207331381553[6] = -nom_x[6] + true_x[6]; - out_533608207331381553[7] = -nom_x[7] + true_x[7]; - out_533608207331381553[8] = -nom_x[8] + true_x[8]; - out_533608207331381553[9] = -nom_x[9] + true_x[9]; - out_533608207331381553[10] = -nom_x[10] + true_x[10]; - out_533608207331381553[11] = -nom_x[11] + true_x[11]; - out_533608207331381553[12] = -nom_x[12] + true_x[12]; - out_533608207331381553[13] = -nom_x[13] + true_x[13]; - out_533608207331381553[14] = -nom_x[14] + true_x[14]; - out_533608207331381553[15] = -nom_x[15] + true_x[15]; - out_533608207331381553[16] = -nom_x[16] + true_x[16]; - out_533608207331381553[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5460684038605246490) { + out_5460684038605246490[0] = -nom_x[0] + true_x[0]; + out_5460684038605246490[1] = -nom_x[1] + true_x[1]; + out_5460684038605246490[2] = -nom_x[2] + true_x[2]; + out_5460684038605246490[3] = -nom_x[3] + true_x[3]; + out_5460684038605246490[4] = -nom_x[4] + true_x[4]; + out_5460684038605246490[5] = -nom_x[5] + true_x[5]; + out_5460684038605246490[6] = -nom_x[6] + true_x[6]; + out_5460684038605246490[7] = -nom_x[7] + true_x[7]; + out_5460684038605246490[8] = -nom_x[8] + true_x[8]; + out_5460684038605246490[9] = -nom_x[9] + true_x[9]; + out_5460684038605246490[10] = -nom_x[10] + true_x[10]; + out_5460684038605246490[11] = -nom_x[11] + true_x[11]; + out_5460684038605246490[12] = -nom_x[12] + true_x[12]; + out_5460684038605246490[13] = -nom_x[13] + true_x[13]; + out_5460684038605246490[14] = -nom_x[14] + true_x[14]; + out_5460684038605246490[15] = -nom_x[15] + true_x[15]; + out_5460684038605246490[16] = -nom_x[16] + true_x[16]; + out_5460684038605246490[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_5230913290931847385) { - out_5230913290931847385[0] = 1.0; - out_5230913290931847385[1] = 0.0; - out_5230913290931847385[2] = 0.0; - out_5230913290931847385[3] = 0.0; - out_5230913290931847385[4] = 0.0; - out_5230913290931847385[5] = 0.0; - out_5230913290931847385[6] = 0.0; - out_5230913290931847385[7] = 0.0; - out_5230913290931847385[8] = 0.0; - out_5230913290931847385[9] = 0.0; - out_5230913290931847385[10] = 0.0; - out_5230913290931847385[11] = 0.0; - out_5230913290931847385[12] = 0.0; - out_5230913290931847385[13] = 0.0; - out_5230913290931847385[14] = 0.0; - out_5230913290931847385[15] = 0.0; - out_5230913290931847385[16] = 0.0; - out_5230913290931847385[17] = 0.0; - out_5230913290931847385[18] = 0.0; - out_5230913290931847385[19] = 1.0; - out_5230913290931847385[20] = 0.0; - out_5230913290931847385[21] = 0.0; - out_5230913290931847385[22] = 0.0; - out_5230913290931847385[23] = 0.0; - out_5230913290931847385[24] = 0.0; - out_5230913290931847385[25] = 0.0; - out_5230913290931847385[26] = 0.0; - out_5230913290931847385[27] = 0.0; - out_5230913290931847385[28] = 0.0; - out_5230913290931847385[29] = 0.0; - out_5230913290931847385[30] = 0.0; - out_5230913290931847385[31] = 0.0; - out_5230913290931847385[32] = 0.0; - out_5230913290931847385[33] = 0.0; - out_5230913290931847385[34] = 0.0; - out_5230913290931847385[35] = 0.0; - out_5230913290931847385[36] = 0.0; - out_5230913290931847385[37] = 0.0; - out_5230913290931847385[38] = 1.0; - out_5230913290931847385[39] = 0.0; - out_5230913290931847385[40] = 0.0; - out_5230913290931847385[41] = 0.0; - out_5230913290931847385[42] = 0.0; - out_5230913290931847385[43] = 0.0; - out_5230913290931847385[44] = 0.0; - out_5230913290931847385[45] = 0.0; - out_5230913290931847385[46] = 0.0; - out_5230913290931847385[47] = 0.0; - out_5230913290931847385[48] = 0.0; - out_5230913290931847385[49] = 0.0; - out_5230913290931847385[50] = 0.0; - out_5230913290931847385[51] = 0.0; - out_5230913290931847385[52] = 0.0; - out_5230913290931847385[53] = 0.0; - out_5230913290931847385[54] = 0.0; - out_5230913290931847385[55] = 0.0; - out_5230913290931847385[56] = 0.0; - out_5230913290931847385[57] = 1.0; - out_5230913290931847385[58] = 0.0; - out_5230913290931847385[59] = 0.0; - out_5230913290931847385[60] = 0.0; - out_5230913290931847385[61] = 0.0; - out_5230913290931847385[62] = 0.0; - out_5230913290931847385[63] = 0.0; - out_5230913290931847385[64] = 0.0; - out_5230913290931847385[65] = 0.0; - out_5230913290931847385[66] = 0.0; - out_5230913290931847385[67] = 0.0; - out_5230913290931847385[68] = 0.0; - out_5230913290931847385[69] = 0.0; - out_5230913290931847385[70] = 0.0; - out_5230913290931847385[71] = 0.0; - out_5230913290931847385[72] = 0.0; - out_5230913290931847385[73] = 0.0; - out_5230913290931847385[74] = 0.0; - out_5230913290931847385[75] = 0.0; - out_5230913290931847385[76] = 1.0; - out_5230913290931847385[77] = 0.0; - out_5230913290931847385[78] = 0.0; - out_5230913290931847385[79] = 0.0; - out_5230913290931847385[80] = 0.0; - out_5230913290931847385[81] = 0.0; - out_5230913290931847385[82] = 0.0; - out_5230913290931847385[83] = 0.0; - out_5230913290931847385[84] = 0.0; - out_5230913290931847385[85] = 0.0; - out_5230913290931847385[86] = 0.0; - out_5230913290931847385[87] = 0.0; - out_5230913290931847385[88] = 0.0; - out_5230913290931847385[89] = 0.0; - out_5230913290931847385[90] = 0.0; - out_5230913290931847385[91] = 0.0; - out_5230913290931847385[92] = 0.0; - out_5230913290931847385[93] = 0.0; - out_5230913290931847385[94] = 0.0; - out_5230913290931847385[95] = 1.0; - out_5230913290931847385[96] = 0.0; - out_5230913290931847385[97] = 0.0; - out_5230913290931847385[98] = 0.0; - out_5230913290931847385[99] = 0.0; - out_5230913290931847385[100] = 0.0; - out_5230913290931847385[101] = 0.0; - out_5230913290931847385[102] = 0.0; - out_5230913290931847385[103] = 0.0; - out_5230913290931847385[104] = 0.0; - out_5230913290931847385[105] = 0.0; - out_5230913290931847385[106] = 0.0; - out_5230913290931847385[107] = 0.0; - out_5230913290931847385[108] = 0.0; - out_5230913290931847385[109] = 0.0; - out_5230913290931847385[110] = 0.0; - out_5230913290931847385[111] = 0.0; - out_5230913290931847385[112] = 0.0; - out_5230913290931847385[113] = 0.0; - out_5230913290931847385[114] = 1.0; - out_5230913290931847385[115] = 0.0; - out_5230913290931847385[116] = 0.0; - out_5230913290931847385[117] = 0.0; - out_5230913290931847385[118] = 0.0; - out_5230913290931847385[119] = 0.0; - out_5230913290931847385[120] = 0.0; - out_5230913290931847385[121] = 0.0; - out_5230913290931847385[122] = 0.0; - out_5230913290931847385[123] = 0.0; - out_5230913290931847385[124] = 0.0; - out_5230913290931847385[125] = 0.0; - out_5230913290931847385[126] = 0.0; - out_5230913290931847385[127] = 0.0; - out_5230913290931847385[128] = 0.0; - out_5230913290931847385[129] = 0.0; - out_5230913290931847385[130] = 0.0; - out_5230913290931847385[131] = 0.0; - out_5230913290931847385[132] = 0.0; - out_5230913290931847385[133] = 1.0; - out_5230913290931847385[134] = 0.0; - out_5230913290931847385[135] = 0.0; - out_5230913290931847385[136] = 0.0; - out_5230913290931847385[137] = 0.0; - out_5230913290931847385[138] = 0.0; - out_5230913290931847385[139] = 0.0; - out_5230913290931847385[140] = 0.0; - out_5230913290931847385[141] = 0.0; - out_5230913290931847385[142] = 0.0; - out_5230913290931847385[143] = 0.0; - out_5230913290931847385[144] = 0.0; - out_5230913290931847385[145] = 0.0; - out_5230913290931847385[146] = 0.0; - out_5230913290931847385[147] = 0.0; - out_5230913290931847385[148] = 0.0; - out_5230913290931847385[149] = 0.0; - out_5230913290931847385[150] = 0.0; - out_5230913290931847385[151] = 0.0; - out_5230913290931847385[152] = 1.0; - out_5230913290931847385[153] = 0.0; - out_5230913290931847385[154] = 0.0; - out_5230913290931847385[155] = 0.0; - out_5230913290931847385[156] = 0.0; - out_5230913290931847385[157] = 0.0; - out_5230913290931847385[158] = 0.0; - out_5230913290931847385[159] = 0.0; - out_5230913290931847385[160] = 0.0; - out_5230913290931847385[161] = 0.0; - out_5230913290931847385[162] = 0.0; - out_5230913290931847385[163] = 0.0; - out_5230913290931847385[164] = 0.0; - out_5230913290931847385[165] = 0.0; - out_5230913290931847385[166] = 0.0; - out_5230913290931847385[167] = 0.0; - out_5230913290931847385[168] = 0.0; - out_5230913290931847385[169] = 0.0; - out_5230913290931847385[170] = 0.0; - out_5230913290931847385[171] = 1.0; - out_5230913290931847385[172] = 0.0; - out_5230913290931847385[173] = 0.0; - out_5230913290931847385[174] = 0.0; - out_5230913290931847385[175] = 0.0; - out_5230913290931847385[176] = 0.0; - out_5230913290931847385[177] = 0.0; - out_5230913290931847385[178] = 0.0; - out_5230913290931847385[179] = 0.0; - out_5230913290931847385[180] = 0.0; - out_5230913290931847385[181] = 0.0; - out_5230913290931847385[182] = 0.0; - out_5230913290931847385[183] = 0.0; - out_5230913290931847385[184] = 0.0; - out_5230913290931847385[185] = 0.0; - out_5230913290931847385[186] = 0.0; - out_5230913290931847385[187] = 0.0; - out_5230913290931847385[188] = 0.0; - out_5230913290931847385[189] = 0.0; - out_5230913290931847385[190] = 1.0; - out_5230913290931847385[191] = 0.0; - out_5230913290931847385[192] = 0.0; - out_5230913290931847385[193] = 0.0; - out_5230913290931847385[194] = 0.0; - out_5230913290931847385[195] = 0.0; - out_5230913290931847385[196] = 0.0; - out_5230913290931847385[197] = 0.0; - out_5230913290931847385[198] = 0.0; - out_5230913290931847385[199] = 0.0; - out_5230913290931847385[200] = 0.0; - out_5230913290931847385[201] = 0.0; - out_5230913290931847385[202] = 0.0; - out_5230913290931847385[203] = 0.0; - out_5230913290931847385[204] = 0.0; - out_5230913290931847385[205] = 0.0; - out_5230913290931847385[206] = 0.0; - out_5230913290931847385[207] = 0.0; - out_5230913290931847385[208] = 0.0; - out_5230913290931847385[209] = 1.0; - out_5230913290931847385[210] = 0.0; - out_5230913290931847385[211] = 0.0; - out_5230913290931847385[212] = 0.0; - out_5230913290931847385[213] = 0.0; - out_5230913290931847385[214] = 0.0; - out_5230913290931847385[215] = 0.0; - out_5230913290931847385[216] = 0.0; - out_5230913290931847385[217] = 0.0; - out_5230913290931847385[218] = 0.0; - out_5230913290931847385[219] = 0.0; - out_5230913290931847385[220] = 0.0; - out_5230913290931847385[221] = 0.0; - out_5230913290931847385[222] = 0.0; - out_5230913290931847385[223] = 0.0; - out_5230913290931847385[224] = 0.0; - out_5230913290931847385[225] = 0.0; - out_5230913290931847385[226] = 0.0; - out_5230913290931847385[227] = 0.0; - out_5230913290931847385[228] = 1.0; - out_5230913290931847385[229] = 0.0; - out_5230913290931847385[230] = 0.0; - out_5230913290931847385[231] = 0.0; - out_5230913290931847385[232] = 0.0; - out_5230913290931847385[233] = 0.0; - out_5230913290931847385[234] = 0.0; - out_5230913290931847385[235] = 0.0; - out_5230913290931847385[236] = 0.0; - out_5230913290931847385[237] = 0.0; - out_5230913290931847385[238] = 0.0; - out_5230913290931847385[239] = 0.0; - out_5230913290931847385[240] = 0.0; - out_5230913290931847385[241] = 0.0; - out_5230913290931847385[242] = 0.0; - out_5230913290931847385[243] = 0.0; - out_5230913290931847385[244] = 0.0; - out_5230913290931847385[245] = 0.0; - out_5230913290931847385[246] = 0.0; - out_5230913290931847385[247] = 1.0; - out_5230913290931847385[248] = 0.0; - out_5230913290931847385[249] = 0.0; - out_5230913290931847385[250] = 0.0; - out_5230913290931847385[251] = 0.0; - out_5230913290931847385[252] = 0.0; - out_5230913290931847385[253] = 0.0; - out_5230913290931847385[254] = 0.0; - out_5230913290931847385[255] = 0.0; - out_5230913290931847385[256] = 0.0; - out_5230913290931847385[257] = 0.0; - out_5230913290931847385[258] = 0.0; - out_5230913290931847385[259] = 0.0; - out_5230913290931847385[260] = 0.0; - out_5230913290931847385[261] = 0.0; - out_5230913290931847385[262] = 0.0; - out_5230913290931847385[263] = 0.0; - out_5230913290931847385[264] = 0.0; - out_5230913290931847385[265] = 0.0; - out_5230913290931847385[266] = 1.0; - out_5230913290931847385[267] = 0.0; - out_5230913290931847385[268] = 0.0; - out_5230913290931847385[269] = 0.0; - out_5230913290931847385[270] = 0.0; - out_5230913290931847385[271] = 0.0; - out_5230913290931847385[272] = 0.0; - out_5230913290931847385[273] = 0.0; - out_5230913290931847385[274] = 0.0; - out_5230913290931847385[275] = 0.0; - out_5230913290931847385[276] = 0.0; - out_5230913290931847385[277] = 0.0; - out_5230913290931847385[278] = 0.0; - out_5230913290931847385[279] = 0.0; - out_5230913290931847385[280] = 0.0; - out_5230913290931847385[281] = 0.0; - out_5230913290931847385[282] = 0.0; - out_5230913290931847385[283] = 0.0; - out_5230913290931847385[284] = 0.0; - out_5230913290931847385[285] = 1.0; - out_5230913290931847385[286] = 0.0; - out_5230913290931847385[287] = 0.0; - out_5230913290931847385[288] = 0.0; - out_5230913290931847385[289] = 0.0; - out_5230913290931847385[290] = 0.0; - out_5230913290931847385[291] = 0.0; - out_5230913290931847385[292] = 0.0; - out_5230913290931847385[293] = 0.0; - out_5230913290931847385[294] = 0.0; - out_5230913290931847385[295] = 0.0; - out_5230913290931847385[296] = 0.0; - out_5230913290931847385[297] = 0.0; - out_5230913290931847385[298] = 0.0; - out_5230913290931847385[299] = 0.0; - out_5230913290931847385[300] = 0.0; - out_5230913290931847385[301] = 0.0; - out_5230913290931847385[302] = 0.0; - out_5230913290931847385[303] = 0.0; - out_5230913290931847385[304] = 1.0; - out_5230913290931847385[305] = 0.0; - out_5230913290931847385[306] = 0.0; - out_5230913290931847385[307] = 0.0; - out_5230913290931847385[308] = 0.0; - out_5230913290931847385[309] = 0.0; - out_5230913290931847385[310] = 0.0; - out_5230913290931847385[311] = 0.0; - out_5230913290931847385[312] = 0.0; - out_5230913290931847385[313] = 0.0; - out_5230913290931847385[314] = 0.0; - out_5230913290931847385[315] = 0.0; - out_5230913290931847385[316] = 0.0; - out_5230913290931847385[317] = 0.0; - out_5230913290931847385[318] = 0.0; - out_5230913290931847385[319] = 0.0; - out_5230913290931847385[320] = 0.0; - out_5230913290931847385[321] = 0.0; - out_5230913290931847385[322] = 0.0; - out_5230913290931847385[323] = 1.0; +void H_mod_fun(double *state, double *out_2180245129448668184) { + out_2180245129448668184[0] = 1.0; + out_2180245129448668184[1] = 0.0; + out_2180245129448668184[2] = 0.0; + out_2180245129448668184[3] = 0.0; + out_2180245129448668184[4] = 0.0; + out_2180245129448668184[5] = 0.0; + out_2180245129448668184[6] = 0.0; + out_2180245129448668184[7] = 0.0; + out_2180245129448668184[8] = 0.0; + out_2180245129448668184[9] = 0.0; + out_2180245129448668184[10] = 0.0; + out_2180245129448668184[11] = 0.0; + out_2180245129448668184[12] = 0.0; + out_2180245129448668184[13] = 0.0; + out_2180245129448668184[14] = 0.0; + out_2180245129448668184[15] = 0.0; + out_2180245129448668184[16] = 0.0; + out_2180245129448668184[17] = 0.0; + out_2180245129448668184[18] = 0.0; + out_2180245129448668184[19] = 1.0; + out_2180245129448668184[20] = 0.0; + out_2180245129448668184[21] = 0.0; + out_2180245129448668184[22] = 0.0; + out_2180245129448668184[23] = 0.0; + out_2180245129448668184[24] = 0.0; + out_2180245129448668184[25] = 0.0; + out_2180245129448668184[26] = 0.0; + out_2180245129448668184[27] = 0.0; + out_2180245129448668184[28] = 0.0; + out_2180245129448668184[29] = 0.0; + out_2180245129448668184[30] = 0.0; + out_2180245129448668184[31] = 0.0; + out_2180245129448668184[32] = 0.0; + out_2180245129448668184[33] = 0.0; + out_2180245129448668184[34] = 0.0; + out_2180245129448668184[35] = 0.0; + out_2180245129448668184[36] = 0.0; + out_2180245129448668184[37] = 0.0; + out_2180245129448668184[38] = 1.0; + out_2180245129448668184[39] = 0.0; + out_2180245129448668184[40] = 0.0; + out_2180245129448668184[41] = 0.0; + out_2180245129448668184[42] = 0.0; + out_2180245129448668184[43] = 0.0; + out_2180245129448668184[44] = 0.0; + out_2180245129448668184[45] = 0.0; + out_2180245129448668184[46] = 0.0; + out_2180245129448668184[47] = 0.0; + out_2180245129448668184[48] = 0.0; + out_2180245129448668184[49] = 0.0; + out_2180245129448668184[50] = 0.0; + out_2180245129448668184[51] = 0.0; + out_2180245129448668184[52] = 0.0; + out_2180245129448668184[53] = 0.0; + out_2180245129448668184[54] = 0.0; + out_2180245129448668184[55] = 0.0; + out_2180245129448668184[56] = 0.0; + out_2180245129448668184[57] = 1.0; + out_2180245129448668184[58] = 0.0; + out_2180245129448668184[59] = 0.0; + out_2180245129448668184[60] = 0.0; + out_2180245129448668184[61] = 0.0; + out_2180245129448668184[62] = 0.0; + out_2180245129448668184[63] = 0.0; + out_2180245129448668184[64] = 0.0; + out_2180245129448668184[65] = 0.0; + out_2180245129448668184[66] = 0.0; + out_2180245129448668184[67] = 0.0; + out_2180245129448668184[68] = 0.0; + out_2180245129448668184[69] = 0.0; + out_2180245129448668184[70] = 0.0; + out_2180245129448668184[71] = 0.0; + out_2180245129448668184[72] = 0.0; + out_2180245129448668184[73] = 0.0; + out_2180245129448668184[74] = 0.0; + out_2180245129448668184[75] = 0.0; + out_2180245129448668184[76] = 1.0; + out_2180245129448668184[77] = 0.0; + out_2180245129448668184[78] = 0.0; + out_2180245129448668184[79] = 0.0; + out_2180245129448668184[80] = 0.0; + out_2180245129448668184[81] = 0.0; + out_2180245129448668184[82] = 0.0; + out_2180245129448668184[83] = 0.0; + out_2180245129448668184[84] = 0.0; + out_2180245129448668184[85] = 0.0; + out_2180245129448668184[86] = 0.0; + out_2180245129448668184[87] = 0.0; + out_2180245129448668184[88] = 0.0; + out_2180245129448668184[89] = 0.0; + out_2180245129448668184[90] = 0.0; + out_2180245129448668184[91] = 0.0; + out_2180245129448668184[92] = 0.0; + out_2180245129448668184[93] = 0.0; + out_2180245129448668184[94] = 0.0; + out_2180245129448668184[95] = 1.0; + out_2180245129448668184[96] = 0.0; + out_2180245129448668184[97] = 0.0; + out_2180245129448668184[98] = 0.0; + out_2180245129448668184[99] = 0.0; + out_2180245129448668184[100] = 0.0; + out_2180245129448668184[101] = 0.0; + out_2180245129448668184[102] = 0.0; + out_2180245129448668184[103] = 0.0; + out_2180245129448668184[104] = 0.0; + out_2180245129448668184[105] = 0.0; + out_2180245129448668184[106] = 0.0; + out_2180245129448668184[107] = 0.0; + out_2180245129448668184[108] = 0.0; + out_2180245129448668184[109] = 0.0; + out_2180245129448668184[110] = 0.0; + out_2180245129448668184[111] = 0.0; + out_2180245129448668184[112] = 0.0; + out_2180245129448668184[113] = 0.0; + out_2180245129448668184[114] = 1.0; + out_2180245129448668184[115] = 0.0; + out_2180245129448668184[116] = 0.0; + out_2180245129448668184[117] = 0.0; + out_2180245129448668184[118] = 0.0; + out_2180245129448668184[119] = 0.0; + out_2180245129448668184[120] = 0.0; + out_2180245129448668184[121] = 0.0; + out_2180245129448668184[122] = 0.0; + out_2180245129448668184[123] = 0.0; + out_2180245129448668184[124] = 0.0; + out_2180245129448668184[125] = 0.0; + out_2180245129448668184[126] = 0.0; + out_2180245129448668184[127] = 0.0; + out_2180245129448668184[128] = 0.0; + out_2180245129448668184[129] = 0.0; + out_2180245129448668184[130] = 0.0; + out_2180245129448668184[131] = 0.0; + out_2180245129448668184[132] = 0.0; + out_2180245129448668184[133] = 1.0; + out_2180245129448668184[134] = 0.0; + out_2180245129448668184[135] = 0.0; + out_2180245129448668184[136] = 0.0; + out_2180245129448668184[137] = 0.0; + out_2180245129448668184[138] = 0.0; + out_2180245129448668184[139] = 0.0; + out_2180245129448668184[140] = 0.0; + out_2180245129448668184[141] = 0.0; + out_2180245129448668184[142] = 0.0; + out_2180245129448668184[143] = 0.0; + out_2180245129448668184[144] = 0.0; + out_2180245129448668184[145] = 0.0; + out_2180245129448668184[146] = 0.0; + out_2180245129448668184[147] = 0.0; + out_2180245129448668184[148] = 0.0; + out_2180245129448668184[149] = 0.0; + out_2180245129448668184[150] = 0.0; + out_2180245129448668184[151] = 0.0; + out_2180245129448668184[152] = 1.0; + out_2180245129448668184[153] = 0.0; + out_2180245129448668184[154] = 0.0; + out_2180245129448668184[155] = 0.0; + out_2180245129448668184[156] = 0.0; + out_2180245129448668184[157] = 0.0; + out_2180245129448668184[158] = 0.0; + out_2180245129448668184[159] = 0.0; + out_2180245129448668184[160] = 0.0; + out_2180245129448668184[161] = 0.0; + out_2180245129448668184[162] = 0.0; + out_2180245129448668184[163] = 0.0; + out_2180245129448668184[164] = 0.0; + out_2180245129448668184[165] = 0.0; + out_2180245129448668184[166] = 0.0; + out_2180245129448668184[167] = 0.0; + out_2180245129448668184[168] = 0.0; + out_2180245129448668184[169] = 0.0; + out_2180245129448668184[170] = 0.0; + out_2180245129448668184[171] = 1.0; + out_2180245129448668184[172] = 0.0; + out_2180245129448668184[173] = 0.0; + out_2180245129448668184[174] = 0.0; + out_2180245129448668184[175] = 0.0; + out_2180245129448668184[176] = 0.0; + out_2180245129448668184[177] = 0.0; + out_2180245129448668184[178] = 0.0; + out_2180245129448668184[179] = 0.0; + out_2180245129448668184[180] = 0.0; + out_2180245129448668184[181] = 0.0; + out_2180245129448668184[182] = 0.0; + out_2180245129448668184[183] = 0.0; + out_2180245129448668184[184] = 0.0; + out_2180245129448668184[185] = 0.0; + out_2180245129448668184[186] = 0.0; + out_2180245129448668184[187] = 0.0; + out_2180245129448668184[188] = 0.0; + out_2180245129448668184[189] = 0.0; + out_2180245129448668184[190] = 1.0; + out_2180245129448668184[191] = 0.0; + out_2180245129448668184[192] = 0.0; + out_2180245129448668184[193] = 0.0; + out_2180245129448668184[194] = 0.0; + out_2180245129448668184[195] = 0.0; + out_2180245129448668184[196] = 0.0; + out_2180245129448668184[197] = 0.0; + out_2180245129448668184[198] = 0.0; + out_2180245129448668184[199] = 0.0; + out_2180245129448668184[200] = 0.0; + out_2180245129448668184[201] = 0.0; + out_2180245129448668184[202] = 0.0; + out_2180245129448668184[203] = 0.0; + out_2180245129448668184[204] = 0.0; + out_2180245129448668184[205] = 0.0; + out_2180245129448668184[206] = 0.0; + out_2180245129448668184[207] = 0.0; + out_2180245129448668184[208] = 0.0; + out_2180245129448668184[209] = 1.0; + out_2180245129448668184[210] = 0.0; + out_2180245129448668184[211] = 0.0; + out_2180245129448668184[212] = 0.0; + out_2180245129448668184[213] = 0.0; + out_2180245129448668184[214] = 0.0; + out_2180245129448668184[215] = 0.0; + out_2180245129448668184[216] = 0.0; + out_2180245129448668184[217] = 0.0; + out_2180245129448668184[218] = 0.0; + out_2180245129448668184[219] = 0.0; + out_2180245129448668184[220] = 0.0; + out_2180245129448668184[221] = 0.0; + out_2180245129448668184[222] = 0.0; + out_2180245129448668184[223] = 0.0; + out_2180245129448668184[224] = 0.0; + out_2180245129448668184[225] = 0.0; + out_2180245129448668184[226] = 0.0; + out_2180245129448668184[227] = 0.0; + out_2180245129448668184[228] = 1.0; + out_2180245129448668184[229] = 0.0; + out_2180245129448668184[230] = 0.0; + out_2180245129448668184[231] = 0.0; + out_2180245129448668184[232] = 0.0; + out_2180245129448668184[233] = 0.0; + out_2180245129448668184[234] = 0.0; + out_2180245129448668184[235] = 0.0; + out_2180245129448668184[236] = 0.0; + out_2180245129448668184[237] = 0.0; + out_2180245129448668184[238] = 0.0; + out_2180245129448668184[239] = 0.0; + out_2180245129448668184[240] = 0.0; + out_2180245129448668184[241] = 0.0; + out_2180245129448668184[242] = 0.0; + out_2180245129448668184[243] = 0.0; + out_2180245129448668184[244] = 0.0; + out_2180245129448668184[245] = 0.0; + out_2180245129448668184[246] = 0.0; + out_2180245129448668184[247] = 1.0; + out_2180245129448668184[248] = 0.0; + out_2180245129448668184[249] = 0.0; + out_2180245129448668184[250] = 0.0; + out_2180245129448668184[251] = 0.0; + out_2180245129448668184[252] = 0.0; + out_2180245129448668184[253] = 0.0; + out_2180245129448668184[254] = 0.0; + out_2180245129448668184[255] = 0.0; + out_2180245129448668184[256] = 0.0; + out_2180245129448668184[257] = 0.0; + out_2180245129448668184[258] = 0.0; + out_2180245129448668184[259] = 0.0; + out_2180245129448668184[260] = 0.0; + out_2180245129448668184[261] = 0.0; + out_2180245129448668184[262] = 0.0; + out_2180245129448668184[263] = 0.0; + out_2180245129448668184[264] = 0.0; + out_2180245129448668184[265] = 0.0; + out_2180245129448668184[266] = 1.0; + out_2180245129448668184[267] = 0.0; + out_2180245129448668184[268] = 0.0; + out_2180245129448668184[269] = 0.0; + out_2180245129448668184[270] = 0.0; + out_2180245129448668184[271] = 0.0; + out_2180245129448668184[272] = 0.0; + out_2180245129448668184[273] = 0.0; + out_2180245129448668184[274] = 0.0; + out_2180245129448668184[275] = 0.0; + out_2180245129448668184[276] = 0.0; + out_2180245129448668184[277] = 0.0; + out_2180245129448668184[278] = 0.0; + out_2180245129448668184[279] = 0.0; + out_2180245129448668184[280] = 0.0; + out_2180245129448668184[281] = 0.0; + out_2180245129448668184[282] = 0.0; + out_2180245129448668184[283] = 0.0; + out_2180245129448668184[284] = 0.0; + out_2180245129448668184[285] = 1.0; + out_2180245129448668184[286] = 0.0; + out_2180245129448668184[287] = 0.0; + out_2180245129448668184[288] = 0.0; + out_2180245129448668184[289] = 0.0; + out_2180245129448668184[290] = 0.0; + out_2180245129448668184[291] = 0.0; + out_2180245129448668184[292] = 0.0; + out_2180245129448668184[293] = 0.0; + out_2180245129448668184[294] = 0.0; + out_2180245129448668184[295] = 0.0; + out_2180245129448668184[296] = 0.0; + out_2180245129448668184[297] = 0.0; + out_2180245129448668184[298] = 0.0; + out_2180245129448668184[299] = 0.0; + out_2180245129448668184[300] = 0.0; + out_2180245129448668184[301] = 0.0; + out_2180245129448668184[302] = 0.0; + out_2180245129448668184[303] = 0.0; + out_2180245129448668184[304] = 1.0; + out_2180245129448668184[305] = 0.0; + out_2180245129448668184[306] = 0.0; + out_2180245129448668184[307] = 0.0; + out_2180245129448668184[308] = 0.0; + out_2180245129448668184[309] = 0.0; + out_2180245129448668184[310] = 0.0; + out_2180245129448668184[311] = 0.0; + out_2180245129448668184[312] = 0.0; + out_2180245129448668184[313] = 0.0; + out_2180245129448668184[314] = 0.0; + out_2180245129448668184[315] = 0.0; + out_2180245129448668184[316] = 0.0; + out_2180245129448668184[317] = 0.0; + out_2180245129448668184[318] = 0.0; + out_2180245129448668184[319] = 0.0; + out_2180245129448668184[320] = 0.0; + out_2180245129448668184[321] = 0.0; + out_2180245129448668184[322] = 0.0; + out_2180245129448668184[323] = 1.0; } -void f_fun(double *state, double dt, double *out_60436790132025778) { - out_60436790132025778[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); - out_60436790132025778[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); - out_60436790132025778[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); - out_60436790132025778[3] = dt*state[12] + state[3]; - out_60436790132025778[4] = dt*state[13] + state[4]; - out_60436790132025778[5] = dt*state[14] + state[5]; - out_60436790132025778[6] = state[6]; - out_60436790132025778[7] = state[7]; - out_60436790132025778[8] = state[8]; - out_60436790132025778[9] = state[9]; - out_60436790132025778[10] = state[10]; - out_60436790132025778[11] = state[11]; - out_60436790132025778[12] = state[12]; - out_60436790132025778[13] = state[13]; - out_60436790132025778[14] = state[14]; - out_60436790132025778[15] = state[15]; - out_60436790132025778[16] = state[16]; - out_60436790132025778[17] = state[17]; +void f_fun(double *state, double dt, double *out_1359935913624561504) { + out_1359935913624561504[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_1359935913624561504[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_1359935913624561504[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_1359935913624561504[3] = dt*state[12] + state[3]; + out_1359935913624561504[4] = dt*state[13] + state[4]; + out_1359935913624561504[5] = dt*state[14] + state[5]; + out_1359935913624561504[6] = state[6]; + out_1359935913624561504[7] = state[7]; + out_1359935913624561504[8] = state[8]; + out_1359935913624561504[9] = state[9]; + out_1359935913624561504[10] = state[10]; + out_1359935913624561504[11] = state[11]; + out_1359935913624561504[12] = state[12]; + out_1359935913624561504[13] = state[13]; + out_1359935913624561504[14] = state[14]; + out_1359935913624561504[15] = state[15]; + out_1359935913624561504[16] = state[16]; + out_1359935913624561504[17] = state[17]; } -void F_fun(double *state, double dt, double *out_8562706762949720732) { - out_8562706762949720732[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_8562706762949720732[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_8562706762949720732[2] = 0; - out_8562706762949720732[3] = 0; - out_8562706762949720732[4] = 0; - out_8562706762949720732[5] = 0; - out_8562706762949720732[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_8562706762949720732[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_8562706762949720732[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_8562706762949720732[9] = 0; - out_8562706762949720732[10] = 0; - out_8562706762949720732[11] = 0; - out_8562706762949720732[12] = 0; - out_8562706762949720732[13] = 0; - out_8562706762949720732[14] = 0; - out_8562706762949720732[15] = 0; - out_8562706762949720732[16] = 0; - out_8562706762949720732[17] = 0; - out_8562706762949720732[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_8562706762949720732[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_8562706762949720732[20] = 0; - out_8562706762949720732[21] = 0; - out_8562706762949720732[22] = 0; - out_8562706762949720732[23] = 0; - out_8562706762949720732[24] = 0; - out_8562706762949720732[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_8562706762949720732[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_8562706762949720732[27] = 0; - out_8562706762949720732[28] = 0; - out_8562706762949720732[29] = 0; - out_8562706762949720732[30] = 0; - out_8562706762949720732[31] = 0; - out_8562706762949720732[32] = 0; - out_8562706762949720732[33] = 0; - out_8562706762949720732[34] = 0; - out_8562706762949720732[35] = 0; - out_8562706762949720732[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_8562706762949720732[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_8562706762949720732[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_8562706762949720732[39] = 0; - out_8562706762949720732[40] = 0; - out_8562706762949720732[41] = 0; - out_8562706762949720732[42] = 0; - out_8562706762949720732[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_8562706762949720732[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_8562706762949720732[45] = 0; - out_8562706762949720732[46] = 0; - out_8562706762949720732[47] = 0; - out_8562706762949720732[48] = 0; - out_8562706762949720732[49] = 0; - out_8562706762949720732[50] = 0; - out_8562706762949720732[51] = 0; - out_8562706762949720732[52] = 0; - out_8562706762949720732[53] = 0; - out_8562706762949720732[54] = 0; - out_8562706762949720732[55] = 0; - out_8562706762949720732[56] = 0; - out_8562706762949720732[57] = 1; - out_8562706762949720732[58] = 0; - out_8562706762949720732[59] = 0; - out_8562706762949720732[60] = 0; - out_8562706762949720732[61] = 0; - out_8562706762949720732[62] = 0; - out_8562706762949720732[63] = 0; - out_8562706762949720732[64] = 0; - out_8562706762949720732[65] = 0; - out_8562706762949720732[66] = dt; - out_8562706762949720732[67] = 0; - out_8562706762949720732[68] = 0; - out_8562706762949720732[69] = 0; - out_8562706762949720732[70] = 0; - out_8562706762949720732[71] = 0; - out_8562706762949720732[72] = 0; - out_8562706762949720732[73] = 0; - out_8562706762949720732[74] = 0; - out_8562706762949720732[75] = 0; - out_8562706762949720732[76] = 1; - out_8562706762949720732[77] = 0; - out_8562706762949720732[78] = 0; - out_8562706762949720732[79] = 0; - out_8562706762949720732[80] = 0; - out_8562706762949720732[81] = 0; - out_8562706762949720732[82] = 0; - out_8562706762949720732[83] = 0; - out_8562706762949720732[84] = 0; - out_8562706762949720732[85] = dt; - out_8562706762949720732[86] = 0; - out_8562706762949720732[87] = 0; - out_8562706762949720732[88] = 0; - out_8562706762949720732[89] = 0; - out_8562706762949720732[90] = 0; - out_8562706762949720732[91] = 0; - out_8562706762949720732[92] = 0; - out_8562706762949720732[93] = 0; - out_8562706762949720732[94] = 0; - out_8562706762949720732[95] = 1; - out_8562706762949720732[96] = 0; - out_8562706762949720732[97] = 0; - out_8562706762949720732[98] = 0; - out_8562706762949720732[99] = 0; - out_8562706762949720732[100] = 0; - out_8562706762949720732[101] = 0; - out_8562706762949720732[102] = 0; - out_8562706762949720732[103] = 0; - out_8562706762949720732[104] = dt; - out_8562706762949720732[105] = 0; - out_8562706762949720732[106] = 0; - out_8562706762949720732[107] = 0; - out_8562706762949720732[108] = 0; - out_8562706762949720732[109] = 0; - out_8562706762949720732[110] = 0; - out_8562706762949720732[111] = 0; - out_8562706762949720732[112] = 0; - out_8562706762949720732[113] = 0; - out_8562706762949720732[114] = 1; - out_8562706762949720732[115] = 0; - out_8562706762949720732[116] = 0; - out_8562706762949720732[117] = 0; - out_8562706762949720732[118] = 0; - out_8562706762949720732[119] = 0; - out_8562706762949720732[120] = 0; - out_8562706762949720732[121] = 0; - out_8562706762949720732[122] = 0; - out_8562706762949720732[123] = 0; - out_8562706762949720732[124] = 0; - out_8562706762949720732[125] = 0; - out_8562706762949720732[126] = 0; - out_8562706762949720732[127] = 0; - out_8562706762949720732[128] = 0; - out_8562706762949720732[129] = 0; - out_8562706762949720732[130] = 0; - out_8562706762949720732[131] = 0; - out_8562706762949720732[132] = 0; - out_8562706762949720732[133] = 1; - out_8562706762949720732[134] = 0; - out_8562706762949720732[135] = 0; - out_8562706762949720732[136] = 0; - out_8562706762949720732[137] = 0; - out_8562706762949720732[138] = 0; - out_8562706762949720732[139] = 0; - out_8562706762949720732[140] = 0; - out_8562706762949720732[141] = 0; - out_8562706762949720732[142] = 0; - out_8562706762949720732[143] = 0; - out_8562706762949720732[144] = 0; - out_8562706762949720732[145] = 0; - out_8562706762949720732[146] = 0; - out_8562706762949720732[147] = 0; - out_8562706762949720732[148] = 0; - out_8562706762949720732[149] = 0; - out_8562706762949720732[150] = 0; - out_8562706762949720732[151] = 0; - out_8562706762949720732[152] = 1; - out_8562706762949720732[153] = 0; - out_8562706762949720732[154] = 0; - out_8562706762949720732[155] = 0; - out_8562706762949720732[156] = 0; - out_8562706762949720732[157] = 0; - out_8562706762949720732[158] = 0; - out_8562706762949720732[159] = 0; - out_8562706762949720732[160] = 0; - out_8562706762949720732[161] = 0; - out_8562706762949720732[162] = 0; - out_8562706762949720732[163] = 0; - out_8562706762949720732[164] = 0; - out_8562706762949720732[165] = 0; - out_8562706762949720732[166] = 0; - out_8562706762949720732[167] = 0; - out_8562706762949720732[168] = 0; - out_8562706762949720732[169] = 0; - out_8562706762949720732[170] = 0; - out_8562706762949720732[171] = 1; - out_8562706762949720732[172] = 0; - out_8562706762949720732[173] = 0; - out_8562706762949720732[174] = 0; - out_8562706762949720732[175] = 0; - out_8562706762949720732[176] = 0; - out_8562706762949720732[177] = 0; - out_8562706762949720732[178] = 0; - out_8562706762949720732[179] = 0; - out_8562706762949720732[180] = 0; - out_8562706762949720732[181] = 0; - out_8562706762949720732[182] = 0; - out_8562706762949720732[183] = 0; - out_8562706762949720732[184] = 0; - out_8562706762949720732[185] = 0; - out_8562706762949720732[186] = 0; - out_8562706762949720732[187] = 0; - out_8562706762949720732[188] = 0; - out_8562706762949720732[189] = 0; - out_8562706762949720732[190] = 1; - out_8562706762949720732[191] = 0; - out_8562706762949720732[192] = 0; - out_8562706762949720732[193] = 0; - out_8562706762949720732[194] = 0; - out_8562706762949720732[195] = 0; - out_8562706762949720732[196] = 0; - out_8562706762949720732[197] = 0; - out_8562706762949720732[198] = 0; - out_8562706762949720732[199] = 0; - out_8562706762949720732[200] = 0; - out_8562706762949720732[201] = 0; - out_8562706762949720732[202] = 0; - out_8562706762949720732[203] = 0; - out_8562706762949720732[204] = 0; - out_8562706762949720732[205] = 0; - out_8562706762949720732[206] = 0; - out_8562706762949720732[207] = 0; - out_8562706762949720732[208] = 0; - out_8562706762949720732[209] = 1; - out_8562706762949720732[210] = 0; - out_8562706762949720732[211] = 0; - out_8562706762949720732[212] = 0; - out_8562706762949720732[213] = 0; - out_8562706762949720732[214] = 0; - out_8562706762949720732[215] = 0; - out_8562706762949720732[216] = 0; - out_8562706762949720732[217] = 0; - out_8562706762949720732[218] = 0; - out_8562706762949720732[219] = 0; - out_8562706762949720732[220] = 0; - out_8562706762949720732[221] = 0; - out_8562706762949720732[222] = 0; - out_8562706762949720732[223] = 0; - out_8562706762949720732[224] = 0; - out_8562706762949720732[225] = 0; - out_8562706762949720732[226] = 0; - out_8562706762949720732[227] = 0; - out_8562706762949720732[228] = 1; - out_8562706762949720732[229] = 0; - out_8562706762949720732[230] = 0; - out_8562706762949720732[231] = 0; - out_8562706762949720732[232] = 0; - out_8562706762949720732[233] = 0; - out_8562706762949720732[234] = 0; - out_8562706762949720732[235] = 0; - out_8562706762949720732[236] = 0; - out_8562706762949720732[237] = 0; - out_8562706762949720732[238] = 0; - out_8562706762949720732[239] = 0; - out_8562706762949720732[240] = 0; - out_8562706762949720732[241] = 0; - out_8562706762949720732[242] = 0; - out_8562706762949720732[243] = 0; - out_8562706762949720732[244] = 0; - out_8562706762949720732[245] = 0; - out_8562706762949720732[246] = 0; - out_8562706762949720732[247] = 1; - out_8562706762949720732[248] = 0; - out_8562706762949720732[249] = 0; - out_8562706762949720732[250] = 0; - out_8562706762949720732[251] = 0; - out_8562706762949720732[252] = 0; - out_8562706762949720732[253] = 0; - out_8562706762949720732[254] = 0; - out_8562706762949720732[255] = 0; - out_8562706762949720732[256] = 0; - out_8562706762949720732[257] = 0; - out_8562706762949720732[258] = 0; - out_8562706762949720732[259] = 0; - out_8562706762949720732[260] = 0; - out_8562706762949720732[261] = 0; - out_8562706762949720732[262] = 0; - out_8562706762949720732[263] = 0; - out_8562706762949720732[264] = 0; - out_8562706762949720732[265] = 0; - out_8562706762949720732[266] = 1; - out_8562706762949720732[267] = 0; - out_8562706762949720732[268] = 0; - out_8562706762949720732[269] = 0; - out_8562706762949720732[270] = 0; - out_8562706762949720732[271] = 0; - out_8562706762949720732[272] = 0; - out_8562706762949720732[273] = 0; - out_8562706762949720732[274] = 0; - out_8562706762949720732[275] = 0; - out_8562706762949720732[276] = 0; - out_8562706762949720732[277] = 0; - out_8562706762949720732[278] = 0; - out_8562706762949720732[279] = 0; - out_8562706762949720732[280] = 0; - out_8562706762949720732[281] = 0; - out_8562706762949720732[282] = 0; - out_8562706762949720732[283] = 0; - out_8562706762949720732[284] = 0; - out_8562706762949720732[285] = 1; - out_8562706762949720732[286] = 0; - out_8562706762949720732[287] = 0; - out_8562706762949720732[288] = 0; - out_8562706762949720732[289] = 0; - out_8562706762949720732[290] = 0; - out_8562706762949720732[291] = 0; - out_8562706762949720732[292] = 0; - out_8562706762949720732[293] = 0; - out_8562706762949720732[294] = 0; - out_8562706762949720732[295] = 0; - out_8562706762949720732[296] = 0; - out_8562706762949720732[297] = 0; - out_8562706762949720732[298] = 0; - out_8562706762949720732[299] = 0; - out_8562706762949720732[300] = 0; - out_8562706762949720732[301] = 0; - out_8562706762949720732[302] = 0; - out_8562706762949720732[303] = 0; - out_8562706762949720732[304] = 1; - out_8562706762949720732[305] = 0; - out_8562706762949720732[306] = 0; - out_8562706762949720732[307] = 0; - out_8562706762949720732[308] = 0; - out_8562706762949720732[309] = 0; - out_8562706762949720732[310] = 0; - out_8562706762949720732[311] = 0; - out_8562706762949720732[312] = 0; - out_8562706762949720732[313] = 0; - out_8562706762949720732[314] = 0; - out_8562706762949720732[315] = 0; - out_8562706762949720732[316] = 0; - out_8562706762949720732[317] = 0; - out_8562706762949720732[318] = 0; - out_8562706762949720732[319] = 0; - out_8562706762949720732[320] = 0; - out_8562706762949720732[321] = 0; - out_8562706762949720732[322] = 0; - out_8562706762949720732[323] = 1; +void F_fun(double *state, double dt, double *out_3401373591342797666) { + out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[2] = 0; + out_3401373591342797666[3] = 0; + out_3401373591342797666[4] = 0; + out_3401373591342797666[5] = 0; + out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[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_3401373591342797666[9] = 0; + out_3401373591342797666[10] = 0; + out_3401373591342797666[11] = 0; + out_3401373591342797666[12] = 0; + out_3401373591342797666[13] = 0; + out_3401373591342797666[14] = 0; + out_3401373591342797666[15] = 0; + out_3401373591342797666[16] = 0; + out_3401373591342797666[17] = 0; + out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[20] = 0; + out_3401373591342797666[21] = 0; + out_3401373591342797666[22] = 0; + out_3401373591342797666[23] = 0; + out_3401373591342797666[24] = 0; + out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[27] = 0; + out_3401373591342797666[28] = 0; + out_3401373591342797666[29] = 0; + out_3401373591342797666[30] = 0; + out_3401373591342797666[31] = 0; + out_3401373591342797666[32] = 0; + out_3401373591342797666[33] = 0; + out_3401373591342797666[34] = 0; + out_3401373591342797666[35] = 0; + out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[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_3401373591342797666[39] = 0; + out_3401373591342797666[40] = 0; + out_3401373591342797666[41] = 0; + out_3401373591342797666[42] = 0; + out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[45] = 0; + out_3401373591342797666[46] = 0; + out_3401373591342797666[47] = 0; + out_3401373591342797666[48] = 0; + out_3401373591342797666[49] = 0; + out_3401373591342797666[50] = 0; + out_3401373591342797666[51] = 0; + out_3401373591342797666[52] = 0; + out_3401373591342797666[53] = 0; + out_3401373591342797666[54] = 0; + out_3401373591342797666[55] = 0; + out_3401373591342797666[56] = 0; + out_3401373591342797666[57] = 1; + out_3401373591342797666[58] = 0; + out_3401373591342797666[59] = 0; + out_3401373591342797666[60] = 0; + out_3401373591342797666[61] = 0; + out_3401373591342797666[62] = 0; + out_3401373591342797666[63] = 0; + out_3401373591342797666[64] = 0; + out_3401373591342797666[65] = 0; + out_3401373591342797666[66] = dt; + out_3401373591342797666[67] = 0; + out_3401373591342797666[68] = 0; + out_3401373591342797666[69] = 0; + out_3401373591342797666[70] = 0; + out_3401373591342797666[71] = 0; + out_3401373591342797666[72] = 0; + out_3401373591342797666[73] = 0; + out_3401373591342797666[74] = 0; + out_3401373591342797666[75] = 0; + out_3401373591342797666[76] = 1; + out_3401373591342797666[77] = 0; + out_3401373591342797666[78] = 0; + out_3401373591342797666[79] = 0; + out_3401373591342797666[80] = 0; + out_3401373591342797666[81] = 0; + out_3401373591342797666[82] = 0; + out_3401373591342797666[83] = 0; + out_3401373591342797666[84] = 0; + out_3401373591342797666[85] = dt; + out_3401373591342797666[86] = 0; + out_3401373591342797666[87] = 0; + out_3401373591342797666[88] = 0; + out_3401373591342797666[89] = 0; + out_3401373591342797666[90] = 0; + out_3401373591342797666[91] = 0; + out_3401373591342797666[92] = 0; + out_3401373591342797666[93] = 0; + out_3401373591342797666[94] = 0; + out_3401373591342797666[95] = 1; + out_3401373591342797666[96] = 0; + out_3401373591342797666[97] = 0; + out_3401373591342797666[98] = 0; + out_3401373591342797666[99] = 0; + out_3401373591342797666[100] = 0; + out_3401373591342797666[101] = 0; + out_3401373591342797666[102] = 0; + out_3401373591342797666[103] = 0; + out_3401373591342797666[104] = dt; + out_3401373591342797666[105] = 0; + out_3401373591342797666[106] = 0; + out_3401373591342797666[107] = 0; + out_3401373591342797666[108] = 0; + out_3401373591342797666[109] = 0; + out_3401373591342797666[110] = 0; + out_3401373591342797666[111] = 0; + out_3401373591342797666[112] = 0; + out_3401373591342797666[113] = 0; + out_3401373591342797666[114] = 1; + out_3401373591342797666[115] = 0; + out_3401373591342797666[116] = 0; + out_3401373591342797666[117] = 0; + out_3401373591342797666[118] = 0; + out_3401373591342797666[119] = 0; + out_3401373591342797666[120] = 0; + out_3401373591342797666[121] = 0; + out_3401373591342797666[122] = 0; + out_3401373591342797666[123] = 0; + out_3401373591342797666[124] = 0; + out_3401373591342797666[125] = 0; + out_3401373591342797666[126] = 0; + out_3401373591342797666[127] = 0; + out_3401373591342797666[128] = 0; + out_3401373591342797666[129] = 0; + out_3401373591342797666[130] = 0; + out_3401373591342797666[131] = 0; + out_3401373591342797666[132] = 0; + out_3401373591342797666[133] = 1; + out_3401373591342797666[134] = 0; + out_3401373591342797666[135] = 0; + out_3401373591342797666[136] = 0; + out_3401373591342797666[137] = 0; + out_3401373591342797666[138] = 0; + out_3401373591342797666[139] = 0; + out_3401373591342797666[140] = 0; + out_3401373591342797666[141] = 0; + out_3401373591342797666[142] = 0; + out_3401373591342797666[143] = 0; + out_3401373591342797666[144] = 0; + out_3401373591342797666[145] = 0; + out_3401373591342797666[146] = 0; + out_3401373591342797666[147] = 0; + out_3401373591342797666[148] = 0; + out_3401373591342797666[149] = 0; + out_3401373591342797666[150] = 0; + out_3401373591342797666[151] = 0; + out_3401373591342797666[152] = 1; + out_3401373591342797666[153] = 0; + out_3401373591342797666[154] = 0; + out_3401373591342797666[155] = 0; + out_3401373591342797666[156] = 0; + out_3401373591342797666[157] = 0; + out_3401373591342797666[158] = 0; + out_3401373591342797666[159] = 0; + out_3401373591342797666[160] = 0; + out_3401373591342797666[161] = 0; + out_3401373591342797666[162] = 0; + out_3401373591342797666[163] = 0; + out_3401373591342797666[164] = 0; + out_3401373591342797666[165] = 0; + out_3401373591342797666[166] = 0; + out_3401373591342797666[167] = 0; + out_3401373591342797666[168] = 0; + out_3401373591342797666[169] = 0; + out_3401373591342797666[170] = 0; + out_3401373591342797666[171] = 1; + out_3401373591342797666[172] = 0; + out_3401373591342797666[173] = 0; + out_3401373591342797666[174] = 0; + out_3401373591342797666[175] = 0; + out_3401373591342797666[176] = 0; + out_3401373591342797666[177] = 0; + out_3401373591342797666[178] = 0; + out_3401373591342797666[179] = 0; + out_3401373591342797666[180] = 0; + out_3401373591342797666[181] = 0; + out_3401373591342797666[182] = 0; + out_3401373591342797666[183] = 0; + out_3401373591342797666[184] = 0; + out_3401373591342797666[185] = 0; + out_3401373591342797666[186] = 0; + out_3401373591342797666[187] = 0; + out_3401373591342797666[188] = 0; + out_3401373591342797666[189] = 0; + out_3401373591342797666[190] = 1; + out_3401373591342797666[191] = 0; + out_3401373591342797666[192] = 0; + out_3401373591342797666[193] = 0; + out_3401373591342797666[194] = 0; + out_3401373591342797666[195] = 0; + out_3401373591342797666[196] = 0; + out_3401373591342797666[197] = 0; + out_3401373591342797666[198] = 0; + out_3401373591342797666[199] = 0; + out_3401373591342797666[200] = 0; + out_3401373591342797666[201] = 0; + out_3401373591342797666[202] = 0; + out_3401373591342797666[203] = 0; + out_3401373591342797666[204] = 0; + out_3401373591342797666[205] = 0; + out_3401373591342797666[206] = 0; + out_3401373591342797666[207] = 0; + out_3401373591342797666[208] = 0; + out_3401373591342797666[209] = 1; + out_3401373591342797666[210] = 0; + out_3401373591342797666[211] = 0; + out_3401373591342797666[212] = 0; + out_3401373591342797666[213] = 0; + out_3401373591342797666[214] = 0; + out_3401373591342797666[215] = 0; + out_3401373591342797666[216] = 0; + out_3401373591342797666[217] = 0; + out_3401373591342797666[218] = 0; + out_3401373591342797666[219] = 0; + out_3401373591342797666[220] = 0; + out_3401373591342797666[221] = 0; + out_3401373591342797666[222] = 0; + out_3401373591342797666[223] = 0; + out_3401373591342797666[224] = 0; + out_3401373591342797666[225] = 0; + out_3401373591342797666[226] = 0; + out_3401373591342797666[227] = 0; + out_3401373591342797666[228] = 1; + out_3401373591342797666[229] = 0; + out_3401373591342797666[230] = 0; + out_3401373591342797666[231] = 0; + out_3401373591342797666[232] = 0; + out_3401373591342797666[233] = 0; + out_3401373591342797666[234] = 0; + out_3401373591342797666[235] = 0; + out_3401373591342797666[236] = 0; + out_3401373591342797666[237] = 0; + out_3401373591342797666[238] = 0; + out_3401373591342797666[239] = 0; + out_3401373591342797666[240] = 0; + out_3401373591342797666[241] = 0; + out_3401373591342797666[242] = 0; + out_3401373591342797666[243] = 0; + out_3401373591342797666[244] = 0; + out_3401373591342797666[245] = 0; + out_3401373591342797666[246] = 0; + out_3401373591342797666[247] = 1; + out_3401373591342797666[248] = 0; + out_3401373591342797666[249] = 0; + out_3401373591342797666[250] = 0; + out_3401373591342797666[251] = 0; + out_3401373591342797666[252] = 0; + out_3401373591342797666[253] = 0; + out_3401373591342797666[254] = 0; + out_3401373591342797666[255] = 0; + out_3401373591342797666[256] = 0; + out_3401373591342797666[257] = 0; + out_3401373591342797666[258] = 0; + out_3401373591342797666[259] = 0; + out_3401373591342797666[260] = 0; + out_3401373591342797666[261] = 0; + out_3401373591342797666[262] = 0; + out_3401373591342797666[263] = 0; + out_3401373591342797666[264] = 0; + out_3401373591342797666[265] = 0; + out_3401373591342797666[266] = 1; + out_3401373591342797666[267] = 0; + out_3401373591342797666[268] = 0; + out_3401373591342797666[269] = 0; + out_3401373591342797666[270] = 0; + out_3401373591342797666[271] = 0; + out_3401373591342797666[272] = 0; + out_3401373591342797666[273] = 0; + out_3401373591342797666[274] = 0; + out_3401373591342797666[275] = 0; + out_3401373591342797666[276] = 0; + out_3401373591342797666[277] = 0; + out_3401373591342797666[278] = 0; + out_3401373591342797666[279] = 0; + out_3401373591342797666[280] = 0; + out_3401373591342797666[281] = 0; + out_3401373591342797666[282] = 0; + out_3401373591342797666[283] = 0; + out_3401373591342797666[284] = 0; + out_3401373591342797666[285] = 1; + out_3401373591342797666[286] = 0; + out_3401373591342797666[287] = 0; + out_3401373591342797666[288] = 0; + out_3401373591342797666[289] = 0; + out_3401373591342797666[290] = 0; + out_3401373591342797666[291] = 0; + out_3401373591342797666[292] = 0; + out_3401373591342797666[293] = 0; + out_3401373591342797666[294] = 0; + out_3401373591342797666[295] = 0; + out_3401373591342797666[296] = 0; + out_3401373591342797666[297] = 0; + out_3401373591342797666[298] = 0; + out_3401373591342797666[299] = 0; + out_3401373591342797666[300] = 0; + out_3401373591342797666[301] = 0; + out_3401373591342797666[302] = 0; + out_3401373591342797666[303] = 0; + out_3401373591342797666[304] = 1; + out_3401373591342797666[305] = 0; + out_3401373591342797666[306] = 0; + out_3401373591342797666[307] = 0; + out_3401373591342797666[308] = 0; + out_3401373591342797666[309] = 0; + out_3401373591342797666[310] = 0; + out_3401373591342797666[311] = 0; + out_3401373591342797666[312] = 0; + out_3401373591342797666[313] = 0; + out_3401373591342797666[314] = 0; + out_3401373591342797666[315] = 0; + out_3401373591342797666[316] = 0; + out_3401373591342797666[317] = 0; + out_3401373591342797666[318] = 0; + out_3401373591342797666[319] = 0; + out_3401373591342797666[320] = 0; + out_3401373591342797666[321] = 0; + out_3401373591342797666[322] = 0; + out_3401373591342797666[323] = 1; } -void h_4(double *state, double *unused, double *out_493571647291527942) { - out_493571647291527942[0] = state[6] + state[9]; - out_493571647291527942[1] = state[7] + state[10]; - out_493571647291527942[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_2493922315843674136) { + out_2493922315843674136[0] = state[6] + state[9]; + out_2493922315843674136[1] = state[7] + state[10]; + out_2493922315843674136[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_6428860772814745489) { - out_6428860772814745489[0] = 0; - out_6428860772814745489[1] = 0; - out_6428860772814745489[2] = 0; - out_6428860772814745489[3] = 0; - out_6428860772814745489[4] = 0; - out_6428860772814745489[5] = 0; - out_6428860772814745489[6] = 1; - out_6428860772814745489[7] = 0; - out_6428860772814745489[8] = 0; - out_6428860772814745489[9] = 1; - out_6428860772814745489[10] = 0; - out_6428860772814745489[11] = 0; - out_6428860772814745489[12] = 0; - out_6428860772814745489[13] = 0; - out_6428860772814745489[14] = 0; - out_6428860772814745489[15] = 0; - out_6428860772814745489[16] = 0; - out_6428860772814745489[17] = 0; - out_6428860772814745489[18] = 0; - out_6428860772814745489[19] = 0; - out_6428860772814745489[20] = 0; - out_6428860772814745489[21] = 0; - out_6428860772814745489[22] = 0; - out_6428860772814745489[23] = 0; - out_6428860772814745489[24] = 0; - out_6428860772814745489[25] = 1; - out_6428860772814745489[26] = 0; - out_6428860772814745489[27] = 0; - out_6428860772814745489[28] = 1; - out_6428860772814745489[29] = 0; - out_6428860772814745489[30] = 0; - out_6428860772814745489[31] = 0; - out_6428860772814745489[32] = 0; - out_6428860772814745489[33] = 0; - out_6428860772814745489[34] = 0; - out_6428860772814745489[35] = 0; - out_6428860772814745489[36] = 0; - out_6428860772814745489[37] = 0; - out_6428860772814745489[38] = 0; - out_6428860772814745489[39] = 0; - out_6428860772814745489[40] = 0; - out_6428860772814745489[41] = 0; - out_6428860772814745489[42] = 0; - out_6428860772814745489[43] = 0; - out_6428860772814745489[44] = 1; - out_6428860772814745489[45] = 0; - out_6428860772814745489[46] = 0; - out_6428860772814745489[47] = 1; - out_6428860772814745489[48] = 0; - out_6428860772814745489[49] = 0; - out_6428860772814745489[50] = 0; - out_6428860772814745489[51] = 0; - out_6428860772814745489[52] = 0; - out_6428860772814745489[53] = 0; +void H_4(double *state, double *unused, double *out_1093976949342281069) { + out_1093976949342281069[0] = 0; + out_1093976949342281069[1] = 0; + out_1093976949342281069[2] = 0; + out_1093976949342281069[3] = 0; + out_1093976949342281069[4] = 0; + out_1093976949342281069[5] = 0; + out_1093976949342281069[6] = 1; + out_1093976949342281069[7] = 0; + out_1093976949342281069[8] = 0; + out_1093976949342281069[9] = 1; + out_1093976949342281069[10] = 0; + out_1093976949342281069[11] = 0; + out_1093976949342281069[12] = 0; + out_1093976949342281069[13] = 0; + out_1093976949342281069[14] = 0; + out_1093976949342281069[15] = 0; + out_1093976949342281069[16] = 0; + out_1093976949342281069[17] = 0; + out_1093976949342281069[18] = 0; + out_1093976949342281069[19] = 0; + out_1093976949342281069[20] = 0; + out_1093976949342281069[21] = 0; + out_1093976949342281069[22] = 0; + out_1093976949342281069[23] = 0; + out_1093976949342281069[24] = 0; + out_1093976949342281069[25] = 1; + out_1093976949342281069[26] = 0; + out_1093976949342281069[27] = 0; + out_1093976949342281069[28] = 1; + out_1093976949342281069[29] = 0; + out_1093976949342281069[30] = 0; + out_1093976949342281069[31] = 0; + out_1093976949342281069[32] = 0; + out_1093976949342281069[33] = 0; + out_1093976949342281069[34] = 0; + out_1093976949342281069[35] = 0; + out_1093976949342281069[36] = 0; + out_1093976949342281069[37] = 0; + out_1093976949342281069[38] = 0; + out_1093976949342281069[39] = 0; + out_1093976949342281069[40] = 0; + out_1093976949342281069[41] = 0; + out_1093976949342281069[42] = 0; + out_1093976949342281069[43] = 0; + out_1093976949342281069[44] = 1; + out_1093976949342281069[45] = 0; + out_1093976949342281069[46] = 0; + out_1093976949342281069[47] = 1; + out_1093976949342281069[48] = 0; + out_1093976949342281069[49] = 0; + out_1093976949342281069[50] = 0; + out_1093976949342281069[51] = 0; + out_1093976949342281069[52] = 0; + out_1093976949342281069[53] = 0; } -void h_10(double *state, double *unused, double *out_7477536671061017990) { - out_7477536671061017990[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_7477536671061017990[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_7477536671061017990[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; +void h_10(double *state, double *unused, double *out_288808481596522343) { + out_288808481596522343[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_288808481596522343[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_288808481596522343[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_3483601252945400253) { - out_3483601252945400253[0] = 0; - out_3483601252945400253[1] = 9.8100000000000005*cos(state[1]); - out_3483601252945400253[2] = 0; - out_3483601252945400253[3] = 0; - out_3483601252945400253[4] = -state[8]; - out_3483601252945400253[5] = state[7]; - out_3483601252945400253[6] = 0; - out_3483601252945400253[7] = state[5]; - out_3483601252945400253[8] = -state[4]; - out_3483601252945400253[9] = 0; - out_3483601252945400253[10] = 0; - out_3483601252945400253[11] = 0; - out_3483601252945400253[12] = 1; - out_3483601252945400253[13] = 0; - out_3483601252945400253[14] = 0; - out_3483601252945400253[15] = 1; - out_3483601252945400253[16] = 0; - out_3483601252945400253[17] = 0; - out_3483601252945400253[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_3483601252945400253[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_3483601252945400253[20] = 0; - out_3483601252945400253[21] = state[8]; - out_3483601252945400253[22] = 0; - out_3483601252945400253[23] = -state[6]; - out_3483601252945400253[24] = -state[5]; - out_3483601252945400253[25] = 0; - out_3483601252945400253[26] = state[3]; - out_3483601252945400253[27] = 0; - out_3483601252945400253[28] = 0; - out_3483601252945400253[29] = 0; - out_3483601252945400253[30] = 0; - out_3483601252945400253[31] = 1; - out_3483601252945400253[32] = 0; - out_3483601252945400253[33] = 0; - out_3483601252945400253[34] = 1; - out_3483601252945400253[35] = 0; - out_3483601252945400253[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_3483601252945400253[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_3483601252945400253[38] = 0; - out_3483601252945400253[39] = -state[7]; - out_3483601252945400253[40] = state[6]; - out_3483601252945400253[41] = 0; - out_3483601252945400253[42] = state[4]; - out_3483601252945400253[43] = -state[3]; - out_3483601252945400253[44] = 0; - out_3483601252945400253[45] = 0; - out_3483601252945400253[46] = 0; - out_3483601252945400253[47] = 0; - out_3483601252945400253[48] = 0; - out_3483601252945400253[49] = 0; - out_3483601252945400253[50] = 1; - out_3483601252945400253[51] = 0; - out_3483601252945400253[52] = 0; - out_3483601252945400253[53] = 1; +void H_10(double *state, double *unused, double *out_4474711072663792050) { + out_4474711072663792050[0] = 0; + out_4474711072663792050[1] = 9.8100000000000005*cos(state[1]); + out_4474711072663792050[2] = 0; + out_4474711072663792050[3] = 0; + out_4474711072663792050[4] = -state[8]; + out_4474711072663792050[5] = state[7]; + out_4474711072663792050[6] = 0; + out_4474711072663792050[7] = state[5]; + out_4474711072663792050[8] = -state[4]; + out_4474711072663792050[9] = 0; + out_4474711072663792050[10] = 0; + out_4474711072663792050[11] = 0; + out_4474711072663792050[12] = 1; + out_4474711072663792050[13] = 0; + out_4474711072663792050[14] = 0; + out_4474711072663792050[15] = 1; + out_4474711072663792050[16] = 0; + out_4474711072663792050[17] = 0; + out_4474711072663792050[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_4474711072663792050[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_4474711072663792050[20] = 0; + out_4474711072663792050[21] = state[8]; + out_4474711072663792050[22] = 0; + out_4474711072663792050[23] = -state[6]; + out_4474711072663792050[24] = -state[5]; + out_4474711072663792050[25] = 0; + out_4474711072663792050[26] = state[3]; + out_4474711072663792050[27] = 0; + out_4474711072663792050[28] = 0; + out_4474711072663792050[29] = 0; + out_4474711072663792050[30] = 0; + out_4474711072663792050[31] = 1; + out_4474711072663792050[32] = 0; + out_4474711072663792050[33] = 0; + out_4474711072663792050[34] = 1; + out_4474711072663792050[35] = 0; + out_4474711072663792050[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_4474711072663792050[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_4474711072663792050[38] = 0; + out_4474711072663792050[39] = -state[7]; + out_4474711072663792050[40] = state[6]; + out_4474711072663792050[41] = 0; + out_4474711072663792050[42] = state[4]; + out_4474711072663792050[43] = -state[3]; + out_4474711072663792050[44] = 0; + out_4474711072663792050[45] = 0; + out_4474711072663792050[46] = 0; + out_4474711072663792050[47] = 0; + out_4474711072663792050[48] = 0; + out_4474711072663792050[49] = 0; + out_4474711072663792050[50] = 1; + out_4474711072663792050[51] = 0; + out_4474711072663792050[52] = 0; + out_4474711072663792050[53] = 1; } -void h_13(double *state, double *unused, double *out_2889929992082413223) { - out_2889929992082413223[0] = state[3]; - out_2889929992082413223[1] = state[4]; - out_2889929992082413223[2] = state[5]; +void h_13(double *state, double *unused, double *out_5108938482451502751) { + out_5108938482451502751[0] = state[3]; + out_5108938482451502751[1] = state[4]; + out_5108938482451502751[2] = state[5]; } -void H_13(double *state, double *unused, double *out_8805609475562473326) { - out_8805609475562473326[0] = 0; - out_8805609475562473326[1] = 0; - out_8805609475562473326[2] = 0; - out_8805609475562473326[3] = 1; - out_8805609475562473326[4] = 0; - out_8805609475562473326[5] = 0; - out_8805609475562473326[6] = 0; - out_8805609475562473326[7] = 0; - out_8805609475562473326[8] = 0; - out_8805609475562473326[9] = 0; - out_8805609475562473326[10] = 0; - out_8805609475562473326[11] = 0; - out_8805609475562473326[12] = 0; - out_8805609475562473326[13] = 0; - out_8805609475562473326[14] = 0; - out_8805609475562473326[15] = 0; - out_8805609475562473326[16] = 0; - out_8805609475562473326[17] = 0; - out_8805609475562473326[18] = 0; - out_8805609475562473326[19] = 0; - out_8805609475562473326[20] = 0; - out_8805609475562473326[21] = 0; - out_8805609475562473326[22] = 1; - out_8805609475562473326[23] = 0; - out_8805609475562473326[24] = 0; - out_8805609475562473326[25] = 0; - out_8805609475562473326[26] = 0; - out_8805609475562473326[27] = 0; - out_8805609475562473326[28] = 0; - out_8805609475562473326[29] = 0; - out_8805609475562473326[30] = 0; - out_8805609475562473326[31] = 0; - out_8805609475562473326[32] = 0; - out_8805609475562473326[33] = 0; - out_8805609475562473326[34] = 0; - out_8805609475562473326[35] = 0; - out_8805609475562473326[36] = 0; - out_8805609475562473326[37] = 0; - out_8805609475562473326[38] = 0; - out_8805609475562473326[39] = 0; - out_8805609475562473326[40] = 0; - out_8805609475562473326[41] = 1; - out_8805609475562473326[42] = 0; - out_8805609475562473326[43] = 0; - out_8805609475562473326[44] = 0; - out_8805609475562473326[45] = 0; - out_8805609475562473326[46] = 0; - out_8805609475562473326[47] = 0; - out_8805609475562473326[48] = 0; - out_8805609475562473326[49] = 0; - out_8805609475562473326[50] = 0; - out_8805609475562473326[51] = 0; - out_8805609475562473326[52] = 0; - out_8805609475562473326[53] = 0; +void H_13(double *state, double *unused, double *out_2118296875990051732) { + out_2118296875990051732[0] = 0; + out_2118296875990051732[1] = 0; + out_2118296875990051732[2] = 0; + out_2118296875990051732[3] = 1; + out_2118296875990051732[4] = 0; + out_2118296875990051732[5] = 0; + out_2118296875990051732[6] = 0; + out_2118296875990051732[7] = 0; + out_2118296875990051732[8] = 0; + out_2118296875990051732[9] = 0; + out_2118296875990051732[10] = 0; + out_2118296875990051732[11] = 0; + out_2118296875990051732[12] = 0; + out_2118296875990051732[13] = 0; + out_2118296875990051732[14] = 0; + out_2118296875990051732[15] = 0; + out_2118296875990051732[16] = 0; + out_2118296875990051732[17] = 0; + out_2118296875990051732[18] = 0; + out_2118296875990051732[19] = 0; + out_2118296875990051732[20] = 0; + out_2118296875990051732[21] = 0; + out_2118296875990051732[22] = 1; + out_2118296875990051732[23] = 0; + out_2118296875990051732[24] = 0; + out_2118296875990051732[25] = 0; + out_2118296875990051732[26] = 0; + out_2118296875990051732[27] = 0; + out_2118296875990051732[28] = 0; + out_2118296875990051732[29] = 0; + out_2118296875990051732[30] = 0; + out_2118296875990051732[31] = 0; + out_2118296875990051732[32] = 0; + out_2118296875990051732[33] = 0; + out_2118296875990051732[34] = 0; + out_2118296875990051732[35] = 0; + out_2118296875990051732[36] = 0; + out_2118296875990051732[37] = 0; + out_2118296875990051732[38] = 0; + out_2118296875990051732[39] = 0; + out_2118296875990051732[40] = 0; + out_2118296875990051732[41] = 1; + out_2118296875990051732[42] = 0; + out_2118296875990051732[43] = 0; + out_2118296875990051732[44] = 0; + out_2118296875990051732[45] = 0; + out_2118296875990051732[46] = 0; + out_2118296875990051732[47] = 0; + out_2118296875990051732[48] = 0; + out_2118296875990051732[49] = 0; + out_2118296875990051732[50] = 0; + out_2118296875990051732[51] = 0; + out_2118296875990051732[52] = 0; + out_2118296875990051732[53] = 0; } -void h_14(double *state, double *unused, double *out_207203314381018955) { - out_207203314381018955[0] = state[6]; - out_207203314381018955[1] = state[7]; - out_207203314381018955[2] = state[8]; +void h_14(double *state, double *unused, double *out_5863792870354861000) { + out_5863792870354861000[0] = state[6]; + out_5863792870354861000[1] = state[7]; + out_5863792870354861000[2] = state[8]; } -void H_14(double *state, double *unused, double *out_3346072340519373193) { - out_3346072340519373193[0] = 0; - out_3346072340519373193[1] = 0; - out_3346072340519373193[2] = 0; - out_3346072340519373193[3] = 0; - out_3346072340519373193[4] = 0; - out_3346072340519373193[5] = 0; - out_3346072340519373193[6] = 1; - out_3346072340519373193[7] = 0; - out_3346072340519373193[8] = 0; - out_3346072340519373193[9] = 0; - out_3346072340519373193[10] = 0; - out_3346072340519373193[11] = 0; - out_3346072340519373193[12] = 0; - out_3346072340519373193[13] = 0; - out_3346072340519373193[14] = 0; - out_3346072340519373193[15] = 0; - out_3346072340519373193[16] = 0; - out_3346072340519373193[17] = 0; - out_3346072340519373193[18] = 0; - out_3346072340519373193[19] = 0; - out_3346072340519373193[20] = 0; - out_3346072340519373193[21] = 0; - out_3346072340519373193[22] = 0; - out_3346072340519373193[23] = 0; - out_3346072340519373193[24] = 0; - out_3346072340519373193[25] = 1; - out_3346072340519373193[26] = 0; - out_3346072340519373193[27] = 0; - out_3346072340519373193[28] = 0; - out_3346072340519373193[29] = 0; - out_3346072340519373193[30] = 0; - out_3346072340519373193[31] = 0; - out_3346072340519373193[32] = 0; - out_3346072340519373193[33] = 0; - out_3346072340519373193[34] = 0; - out_3346072340519373193[35] = 0; - out_3346072340519373193[36] = 0; - out_3346072340519373193[37] = 0; - out_3346072340519373193[38] = 0; - out_3346072340519373193[39] = 0; - out_3346072340519373193[40] = 0; - out_3346072340519373193[41] = 0; - out_3346072340519373193[42] = 0; - out_3346072340519373193[43] = 0; - out_3346072340519373193[44] = 1; - out_3346072340519373193[45] = 0; - out_3346072340519373193[46] = 0; - out_3346072340519373193[47] = 0; - out_3346072340519373193[48] = 0; - out_3346072340519373193[49] = 0; - out_3346072340519373193[50] = 0; - out_3346072340519373193[51] = 0; - out_3346072340519373193[52] = 0; - out_3346072340519373193[53] = 0; +void H_14(double *state, double *unused, double *out_2869263906997203460) { + out_2869263906997203460[0] = 0; + out_2869263906997203460[1] = 0; + out_2869263906997203460[2] = 0; + out_2869263906997203460[3] = 0; + out_2869263906997203460[4] = 0; + out_2869263906997203460[5] = 0; + out_2869263906997203460[6] = 1; + out_2869263906997203460[7] = 0; + out_2869263906997203460[8] = 0; + out_2869263906997203460[9] = 0; + out_2869263906997203460[10] = 0; + out_2869263906997203460[11] = 0; + out_2869263906997203460[12] = 0; + out_2869263906997203460[13] = 0; + out_2869263906997203460[14] = 0; + out_2869263906997203460[15] = 0; + out_2869263906997203460[16] = 0; + out_2869263906997203460[17] = 0; + out_2869263906997203460[18] = 0; + out_2869263906997203460[19] = 0; + out_2869263906997203460[20] = 0; + out_2869263906997203460[21] = 0; + out_2869263906997203460[22] = 0; + out_2869263906997203460[23] = 0; + out_2869263906997203460[24] = 0; + out_2869263906997203460[25] = 1; + out_2869263906997203460[26] = 0; + out_2869263906997203460[27] = 0; + out_2869263906997203460[28] = 0; + out_2869263906997203460[29] = 0; + out_2869263906997203460[30] = 0; + out_2869263906997203460[31] = 0; + out_2869263906997203460[32] = 0; + out_2869263906997203460[33] = 0; + out_2869263906997203460[34] = 0; + out_2869263906997203460[35] = 0; + out_2869263906997203460[36] = 0; + out_2869263906997203460[37] = 0; + out_2869263906997203460[38] = 0; + out_2869263906997203460[39] = 0; + out_2869263906997203460[40] = 0; + out_2869263906997203460[41] = 0; + out_2869263906997203460[42] = 0; + out_2869263906997203460[43] = 0; + out_2869263906997203460[44] = 1; + out_2869263906997203460[45] = 0; + out_2869263906997203460[46] = 0; + out_2869263906997203460[47] = 0; + out_2869263906997203460[48] = 0; + out_2869263906997203460[49] = 0; + out_2869263906997203460[50] = 0; + out_2869263906997203460[51] = 0; + out_2869263906997203460[52] = 0; + out_2869263906997203460[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_7993353667610184364) { - err_fun(nom_x, delta_x, out_7993353667610184364); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6009088715125097596) { + err_fun(nom_x, delta_x, out_6009088715125097596); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_533608207331381553) { - inv_err_fun(nom_x, true_x, out_533608207331381553); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5460684038605246490) { + inv_err_fun(nom_x, true_x, out_5460684038605246490); } -void pose_H_mod_fun(double *state, double *out_5230913290931847385) { - H_mod_fun(state, out_5230913290931847385); +void pose_H_mod_fun(double *state, double *out_2180245129448668184) { + H_mod_fun(state, out_2180245129448668184); } -void pose_f_fun(double *state, double dt, double *out_60436790132025778) { - f_fun(state, dt, out_60436790132025778); +void pose_f_fun(double *state, double dt, double *out_1359935913624561504) { + f_fun(state, dt, out_1359935913624561504); } -void pose_F_fun(double *state, double dt, double *out_8562706762949720732) { - F_fun(state, dt, out_8562706762949720732); +void pose_F_fun(double *state, double dt, double *out_3401373591342797666) { + F_fun(state, dt, out_3401373591342797666); } -void pose_h_4(double *state, double *unused, double *out_493571647291527942) { - h_4(state, unused, out_493571647291527942); +void pose_h_4(double *state, double *unused, double *out_2493922315843674136) { + h_4(state, unused, out_2493922315843674136); } -void pose_H_4(double *state, double *unused, double *out_6428860772814745489) { - H_4(state, unused, out_6428860772814745489); +void pose_H_4(double *state, double *unused, double *out_1093976949342281069) { + H_4(state, unused, out_1093976949342281069); } -void pose_h_10(double *state, double *unused, double *out_7477536671061017990) { - h_10(state, unused, out_7477536671061017990); +void pose_h_10(double *state, double *unused, double *out_288808481596522343) { + h_10(state, unused, out_288808481596522343); } -void pose_H_10(double *state, double *unused, double *out_3483601252945400253) { - H_10(state, unused, out_3483601252945400253); +void pose_H_10(double *state, double *unused, double *out_4474711072663792050) { + H_10(state, unused, out_4474711072663792050); } -void pose_h_13(double *state, double *unused, double *out_2889929992082413223) { - h_13(state, unused, out_2889929992082413223); +void pose_h_13(double *state, double *unused, double *out_5108938482451502751) { + h_13(state, unused, out_5108938482451502751); } -void pose_H_13(double *state, double *unused, double *out_8805609475562473326) { - H_13(state, unused, out_8805609475562473326); +void pose_H_13(double *state, double *unused, double *out_2118296875990051732) { + H_13(state, unused, out_2118296875990051732); } -void pose_h_14(double *state, double *unused, double *out_207203314381018955) { - h_14(state, unused, out_207203314381018955); +void pose_h_14(double *state, double *unused, double *out_5863792870354861000) { + h_14(state, unused, out_5863792870354861000); } -void pose_H_14(double *state, double *unused, double *out_3346072340519373193) { - H_14(state, unused, out_3346072340519373193); +void pose_H_14(double *state, double *unused, double *out_2869263906997203460) { + H_14(state, unused, out_2869263906997203460); } 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 0952276be..fb3aeec79 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_7993353667610184364); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_533608207331381553); -void pose_H_mod_fun(double *state, double *out_5230913290931847385); -void pose_f_fun(double *state, double dt, double *out_60436790132025778); -void pose_F_fun(double *state, double dt, double *out_8562706762949720732); -void pose_h_4(double *state, double *unused, double *out_493571647291527942); -void pose_H_4(double *state, double *unused, double *out_6428860772814745489); -void pose_h_10(double *state, double *unused, double *out_7477536671061017990); -void pose_H_10(double *state, double *unused, double *out_3483601252945400253); -void pose_h_13(double *state, double *unused, double *out_2889929992082413223); -void pose_H_13(double *state, double *unused, double *out_8805609475562473326); -void pose_h_14(double *state, double *unused, double *out_207203314381018955); -void pose_H_14(double *state, double *unused, double *out_3346072340519373193); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6009088715125097596); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5460684038605246490); +void pose_H_mod_fun(double *state, double *out_2180245129448668184); +void pose_f_fun(double *state, double dt, double *out_1359935913624561504); +void pose_F_fun(double *state, double dt, double *out_3401373591342797666); +void pose_h_4(double *state, double *unused, double *out_2493922315843674136); +void pose_H_4(double *state, double *unused, double *out_1093976949342281069); +void pose_h_10(double *state, double *unused, double *out_288808481596522343); +void pose_H_10(double *state, double *unused, double *out_4474711072663792050); +void pose_h_13(double *state, double *unused, double *out_5108938482451502751); +void pose_H_13(double *state, double *unused, double *out_2118296875990051732); +void pose_h_14(double *state, double *unused, double *out_5863792870354861000); +void pose_H_14(double *state, double *unused, double *out_2869263906997203460); 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 6a50ba1c4..21e9658db 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 8e23521da..d168c8528 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index ef6a99b7d..7dad5af8f 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 97fc92789..6161a4843 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 4e4029f4f..3f0845caf 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index a72e60002..5172a36c7 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ