diff --git a/common/libcommon.a b/common/libcommon.a index eefdbb738..a1fd84de7 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 4180161f6..d865e11da 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 615f3a9aa..a3efe5598 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 079f1d6e0..5cb8c74b2 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 8f5710c10..5b198d22a 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 3e6d52c0e..206d10872 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 96417cfe1..bbf90d44b 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 0a8e4cd7b..332744491 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 15495e734..aaa73d828 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 15495e734..aaa73d828 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 431b6e629..e9d96ced8 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 0a8e4cd7b..332744491 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 21f2273fb..2bfca92a3 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-0ce8161c-DEBUG"; +const uint8_t gitversion[19] = "DEV-a67c8f98-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 5ae77d288..1179eed39 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 b75551e54..2a9de4a78 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 a8a0ceb74..c400150bd 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 efa7bd221..c290153e5 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 4509f1e04..48ee10364 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 9e5b539c3..b293f27eb 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 7d4414bdc..19fe3a412 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 98bb99daa..b83d52b8a 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 113467175..71312d0d2 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 0b48f1fb0..f9b47892f 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 db491cca5..c46ab7385 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 0c2448b31..db64b5fc5 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 581f3cce8..fe7925562 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 fa3f90731..88c4a7140 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 c9c1e06d8..3f52e0597 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 782f85202..489eeacf2 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 e1214a15a..9ec06505e 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 45c1751b9..7727e1195 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 415c25078..a587ba49a 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 260a9d8c3..f6f885759 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 ea6b4d0f7..1d9a6681c 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-0ce8161c-DEBUG \ No newline at end of file +DEV-a67c8f98-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 10a962e81..eb5d83568 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_5325374827696631328) { - out_5325374827696631328[0] = delta_x[0] + nom_x[0]; - out_5325374827696631328[1] = delta_x[1] + nom_x[1]; - out_5325374827696631328[2] = delta_x[2] + nom_x[2]; - out_5325374827696631328[3] = delta_x[3] + nom_x[3]; - out_5325374827696631328[4] = delta_x[4] + nom_x[4]; - out_5325374827696631328[5] = delta_x[5] + nom_x[5]; - out_5325374827696631328[6] = delta_x[6] + nom_x[6]; - out_5325374827696631328[7] = delta_x[7] + nom_x[7]; - out_5325374827696631328[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_3886210737280219279) { + out_3886210737280219279[0] = delta_x[0] + nom_x[0]; + out_3886210737280219279[1] = delta_x[1] + nom_x[1]; + out_3886210737280219279[2] = delta_x[2] + nom_x[2]; + out_3886210737280219279[3] = delta_x[3] + nom_x[3]; + out_3886210737280219279[4] = delta_x[4] + nom_x[4]; + out_3886210737280219279[5] = delta_x[5] + nom_x[5]; + out_3886210737280219279[6] = delta_x[6] + nom_x[6]; + out_3886210737280219279[7] = delta_x[7] + nom_x[7]; + out_3886210737280219279[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_263695042767338590) { - out_263695042767338590[0] = -nom_x[0] + true_x[0]; - out_263695042767338590[1] = -nom_x[1] + true_x[1]; - out_263695042767338590[2] = -nom_x[2] + true_x[2]; - out_263695042767338590[3] = -nom_x[3] + true_x[3]; - out_263695042767338590[4] = -nom_x[4] + true_x[4]; - out_263695042767338590[5] = -nom_x[5] + true_x[5]; - out_263695042767338590[6] = -nom_x[6] + true_x[6]; - out_263695042767338590[7] = -nom_x[7] + true_x[7]; - out_263695042767338590[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_9203878995777804762) { + out_9203878995777804762[0] = -nom_x[0] + true_x[0]; + out_9203878995777804762[1] = -nom_x[1] + true_x[1]; + out_9203878995777804762[2] = -nom_x[2] + true_x[2]; + out_9203878995777804762[3] = -nom_x[3] + true_x[3]; + out_9203878995777804762[4] = -nom_x[4] + true_x[4]; + out_9203878995777804762[5] = -nom_x[5] + true_x[5]; + out_9203878995777804762[6] = -nom_x[6] + true_x[6]; + out_9203878995777804762[7] = -nom_x[7] + true_x[7]; + out_9203878995777804762[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_2663737869404174494) { - out_2663737869404174494[0] = 1.0; - out_2663737869404174494[1] = 0.0; - out_2663737869404174494[2] = 0.0; - out_2663737869404174494[3] = 0.0; - out_2663737869404174494[4] = 0.0; - out_2663737869404174494[5] = 0.0; - out_2663737869404174494[6] = 0.0; - out_2663737869404174494[7] = 0.0; - out_2663737869404174494[8] = 0.0; - out_2663737869404174494[9] = 0.0; - out_2663737869404174494[10] = 1.0; - out_2663737869404174494[11] = 0.0; - out_2663737869404174494[12] = 0.0; - out_2663737869404174494[13] = 0.0; - out_2663737869404174494[14] = 0.0; - out_2663737869404174494[15] = 0.0; - out_2663737869404174494[16] = 0.0; - out_2663737869404174494[17] = 0.0; - out_2663737869404174494[18] = 0.0; - out_2663737869404174494[19] = 0.0; - out_2663737869404174494[20] = 1.0; - out_2663737869404174494[21] = 0.0; - out_2663737869404174494[22] = 0.0; - out_2663737869404174494[23] = 0.0; - out_2663737869404174494[24] = 0.0; - out_2663737869404174494[25] = 0.0; - out_2663737869404174494[26] = 0.0; - out_2663737869404174494[27] = 0.0; - out_2663737869404174494[28] = 0.0; - out_2663737869404174494[29] = 0.0; - out_2663737869404174494[30] = 1.0; - out_2663737869404174494[31] = 0.0; - out_2663737869404174494[32] = 0.0; - out_2663737869404174494[33] = 0.0; - out_2663737869404174494[34] = 0.0; - out_2663737869404174494[35] = 0.0; - out_2663737869404174494[36] = 0.0; - out_2663737869404174494[37] = 0.0; - out_2663737869404174494[38] = 0.0; - out_2663737869404174494[39] = 0.0; - out_2663737869404174494[40] = 1.0; - out_2663737869404174494[41] = 0.0; - out_2663737869404174494[42] = 0.0; - out_2663737869404174494[43] = 0.0; - out_2663737869404174494[44] = 0.0; - out_2663737869404174494[45] = 0.0; - out_2663737869404174494[46] = 0.0; - out_2663737869404174494[47] = 0.0; - out_2663737869404174494[48] = 0.0; - out_2663737869404174494[49] = 0.0; - out_2663737869404174494[50] = 1.0; - out_2663737869404174494[51] = 0.0; - out_2663737869404174494[52] = 0.0; - out_2663737869404174494[53] = 0.0; - out_2663737869404174494[54] = 0.0; - out_2663737869404174494[55] = 0.0; - out_2663737869404174494[56] = 0.0; - out_2663737869404174494[57] = 0.0; - out_2663737869404174494[58] = 0.0; - out_2663737869404174494[59] = 0.0; - out_2663737869404174494[60] = 1.0; - out_2663737869404174494[61] = 0.0; - out_2663737869404174494[62] = 0.0; - out_2663737869404174494[63] = 0.0; - out_2663737869404174494[64] = 0.0; - out_2663737869404174494[65] = 0.0; - out_2663737869404174494[66] = 0.0; - out_2663737869404174494[67] = 0.0; - out_2663737869404174494[68] = 0.0; - out_2663737869404174494[69] = 0.0; - out_2663737869404174494[70] = 1.0; - out_2663737869404174494[71] = 0.0; - out_2663737869404174494[72] = 0.0; - out_2663737869404174494[73] = 0.0; - out_2663737869404174494[74] = 0.0; - out_2663737869404174494[75] = 0.0; - out_2663737869404174494[76] = 0.0; - out_2663737869404174494[77] = 0.0; - out_2663737869404174494[78] = 0.0; - out_2663737869404174494[79] = 0.0; - out_2663737869404174494[80] = 1.0; +void H_mod_fun(double *state, double *out_8506196822923476348) { + out_8506196822923476348[0] = 1.0; + out_8506196822923476348[1] = 0.0; + out_8506196822923476348[2] = 0.0; + out_8506196822923476348[3] = 0.0; + out_8506196822923476348[4] = 0.0; + out_8506196822923476348[5] = 0.0; + out_8506196822923476348[6] = 0.0; + out_8506196822923476348[7] = 0.0; + out_8506196822923476348[8] = 0.0; + out_8506196822923476348[9] = 0.0; + out_8506196822923476348[10] = 1.0; + out_8506196822923476348[11] = 0.0; + out_8506196822923476348[12] = 0.0; + out_8506196822923476348[13] = 0.0; + out_8506196822923476348[14] = 0.0; + out_8506196822923476348[15] = 0.0; + out_8506196822923476348[16] = 0.0; + out_8506196822923476348[17] = 0.0; + out_8506196822923476348[18] = 0.0; + out_8506196822923476348[19] = 0.0; + out_8506196822923476348[20] = 1.0; + out_8506196822923476348[21] = 0.0; + out_8506196822923476348[22] = 0.0; + out_8506196822923476348[23] = 0.0; + out_8506196822923476348[24] = 0.0; + out_8506196822923476348[25] = 0.0; + out_8506196822923476348[26] = 0.0; + out_8506196822923476348[27] = 0.0; + out_8506196822923476348[28] = 0.0; + out_8506196822923476348[29] = 0.0; + out_8506196822923476348[30] = 1.0; + out_8506196822923476348[31] = 0.0; + out_8506196822923476348[32] = 0.0; + out_8506196822923476348[33] = 0.0; + out_8506196822923476348[34] = 0.0; + out_8506196822923476348[35] = 0.0; + out_8506196822923476348[36] = 0.0; + out_8506196822923476348[37] = 0.0; + out_8506196822923476348[38] = 0.0; + out_8506196822923476348[39] = 0.0; + out_8506196822923476348[40] = 1.0; + out_8506196822923476348[41] = 0.0; + out_8506196822923476348[42] = 0.0; + out_8506196822923476348[43] = 0.0; + out_8506196822923476348[44] = 0.0; + out_8506196822923476348[45] = 0.0; + out_8506196822923476348[46] = 0.0; + out_8506196822923476348[47] = 0.0; + out_8506196822923476348[48] = 0.0; + out_8506196822923476348[49] = 0.0; + out_8506196822923476348[50] = 1.0; + out_8506196822923476348[51] = 0.0; + out_8506196822923476348[52] = 0.0; + out_8506196822923476348[53] = 0.0; + out_8506196822923476348[54] = 0.0; + out_8506196822923476348[55] = 0.0; + out_8506196822923476348[56] = 0.0; + out_8506196822923476348[57] = 0.0; + out_8506196822923476348[58] = 0.0; + out_8506196822923476348[59] = 0.0; + out_8506196822923476348[60] = 1.0; + out_8506196822923476348[61] = 0.0; + out_8506196822923476348[62] = 0.0; + out_8506196822923476348[63] = 0.0; + out_8506196822923476348[64] = 0.0; + out_8506196822923476348[65] = 0.0; + out_8506196822923476348[66] = 0.0; + out_8506196822923476348[67] = 0.0; + out_8506196822923476348[68] = 0.0; + out_8506196822923476348[69] = 0.0; + out_8506196822923476348[70] = 1.0; + out_8506196822923476348[71] = 0.0; + out_8506196822923476348[72] = 0.0; + out_8506196822923476348[73] = 0.0; + out_8506196822923476348[74] = 0.0; + out_8506196822923476348[75] = 0.0; + out_8506196822923476348[76] = 0.0; + out_8506196822923476348[77] = 0.0; + out_8506196822923476348[78] = 0.0; + out_8506196822923476348[79] = 0.0; + out_8506196822923476348[80] = 1.0; } -void f_fun(double *state, double dt, double *out_8230420991833863893) { - out_8230420991833863893[0] = state[0]; - out_8230420991833863893[1] = state[1]; - out_8230420991833863893[2] = state[2]; - out_8230420991833863893[3] = state[3]; - out_8230420991833863893[4] = state[4]; - out_8230420991833863893[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_8230420991833863893[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_8230420991833863893[7] = state[7]; - out_8230420991833863893[8] = state[8]; +void f_fun(double *state, double dt, double *out_2212415948681408961) { + out_2212415948681408961[0] = state[0]; + out_2212415948681408961[1] = state[1]; + out_2212415948681408961[2] = state[2]; + out_2212415948681408961[3] = state[3]; + out_2212415948681408961[4] = state[4]; + out_2212415948681408961[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_2212415948681408961[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_2212415948681408961[7] = state[7]; + out_2212415948681408961[8] = state[8]; } -void F_fun(double *state, double dt, double *out_2756618938696411588) { - out_2756618938696411588[0] = 1; - out_2756618938696411588[1] = 0; - out_2756618938696411588[2] = 0; - out_2756618938696411588[3] = 0; - out_2756618938696411588[4] = 0; - out_2756618938696411588[5] = 0; - out_2756618938696411588[6] = 0; - out_2756618938696411588[7] = 0; - out_2756618938696411588[8] = 0; - out_2756618938696411588[9] = 0; - out_2756618938696411588[10] = 1; - out_2756618938696411588[11] = 0; - out_2756618938696411588[12] = 0; - out_2756618938696411588[13] = 0; - out_2756618938696411588[14] = 0; - out_2756618938696411588[15] = 0; - out_2756618938696411588[16] = 0; - out_2756618938696411588[17] = 0; - out_2756618938696411588[18] = 0; - out_2756618938696411588[19] = 0; - out_2756618938696411588[20] = 1; - out_2756618938696411588[21] = 0; - out_2756618938696411588[22] = 0; - out_2756618938696411588[23] = 0; - out_2756618938696411588[24] = 0; - out_2756618938696411588[25] = 0; - out_2756618938696411588[26] = 0; - out_2756618938696411588[27] = 0; - out_2756618938696411588[28] = 0; - out_2756618938696411588[29] = 0; - out_2756618938696411588[30] = 1; - out_2756618938696411588[31] = 0; - out_2756618938696411588[32] = 0; - out_2756618938696411588[33] = 0; - out_2756618938696411588[34] = 0; - out_2756618938696411588[35] = 0; - out_2756618938696411588[36] = 0; - out_2756618938696411588[37] = 0; - out_2756618938696411588[38] = 0; - out_2756618938696411588[39] = 0; - out_2756618938696411588[40] = 1; - out_2756618938696411588[41] = 0; - out_2756618938696411588[42] = 0; - out_2756618938696411588[43] = 0; - out_2756618938696411588[44] = 0; - out_2756618938696411588[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_2756618938696411588[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_2756618938696411588[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2756618938696411588[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2756618938696411588[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_2756618938696411588[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_2756618938696411588[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_2756618938696411588[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_2756618938696411588[53] = -9.8100000000000005*dt; - out_2756618938696411588[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_2756618938696411588[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_2756618938696411588[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2756618938696411588[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2756618938696411588[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_2756618938696411588[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_2756618938696411588[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_2756618938696411588[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2756618938696411588[62] = 0; - out_2756618938696411588[63] = 0; - out_2756618938696411588[64] = 0; - out_2756618938696411588[65] = 0; - out_2756618938696411588[66] = 0; - out_2756618938696411588[67] = 0; - out_2756618938696411588[68] = 0; - out_2756618938696411588[69] = 0; - out_2756618938696411588[70] = 1; - out_2756618938696411588[71] = 0; - out_2756618938696411588[72] = 0; - out_2756618938696411588[73] = 0; - out_2756618938696411588[74] = 0; - out_2756618938696411588[75] = 0; - out_2756618938696411588[76] = 0; - out_2756618938696411588[77] = 0; - out_2756618938696411588[78] = 0; - out_2756618938696411588[79] = 0; - out_2756618938696411588[80] = 1; +void F_fun(double *state, double dt, double *out_8605893895722033804) { + out_8605893895722033804[0] = 1; + out_8605893895722033804[1] = 0; + out_8605893895722033804[2] = 0; + out_8605893895722033804[3] = 0; + out_8605893895722033804[4] = 0; + out_8605893895722033804[5] = 0; + out_8605893895722033804[6] = 0; + out_8605893895722033804[7] = 0; + out_8605893895722033804[8] = 0; + out_8605893895722033804[9] = 0; + out_8605893895722033804[10] = 1; + out_8605893895722033804[11] = 0; + out_8605893895722033804[12] = 0; + out_8605893895722033804[13] = 0; + out_8605893895722033804[14] = 0; + out_8605893895722033804[15] = 0; + out_8605893895722033804[16] = 0; + out_8605893895722033804[17] = 0; + out_8605893895722033804[18] = 0; + out_8605893895722033804[19] = 0; + out_8605893895722033804[20] = 1; + out_8605893895722033804[21] = 0; + out_8605893895722033804[22] = 0; + out_8605893895722033804[23] = 0; + out_8605893895722033804[24] = 0; + out_8605893895722033804[25] = 0; + out_8605893895722033804[26] = 0; + out_8605893895722033804[27] = 0; + out_8605893895722033804[28] = 0; + out_8605893895722033804[29] = 0; + out_8605893895722033804[30] = 1; + out_8605893895722033804[31] = 0; + out_8605893895722033804[32] = 0; + out_8605893895722033804[33] = 0; + out_8605893895722033804[34] = 0; + out_8605893895722033804[35] = 0; + out_8605893895722033804[36] = 0; + out_8605893895722033804[37] = 0; + out_8605893895722033804[38] = 0; + out_8605893895722033804[39] = 0; + out_8605893895722033804[40] = 1; + out_8605893895722033804[41] = 0; + out_8605893895722033804[42] = 0; + out_8605893895722033804[43] = 0; + out_8605893895722033804[44] = 0; + out_8605893895722033804[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_8605893895722033804[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_8605893895722033804[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8605893895722033804[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8605893895722033804[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_8605893895722033804[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_8605893895722033804[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_8605893895722033804[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_8605893895722033804[53] = -9.8100000000000005*dt; + out_8605893895722033804[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_8605893895722033804[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_8605893895722033804[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8605893895722033804[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8605893895722033804[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_8605893895722033804[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_8605893895722033804[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_8605893895722033804[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8605893895722033804[62] = 0; + out_8605893895722033804[63] = 0; + out_8605893895722033804[64] = 0; + out_8605893895722033804[65] = 0; + out_8605893895722033804[66] = 0; + out_8605893895722033804[67] = 0; + out_8605893895722033804[68] = 0; + out_8605893895722033804[69] = 0; + out_8605893895722033804[70] = 1; + out_8605893895722033804[71] = 0; + out_8605893895722033804[72] = 0; + out_8605893895722033804[73] = 0; + out_8605893895722033804[74] = 0; + out_8605893895722033804[75] = 0; + out_8605893895722033804[76] = 0; + out_8605893895722033804[77] = 0; + out_8605893895722033804[78] = 0; + out_8605893895722033804[79] = 0; + out_8605893895722033804[80] = 1; } -void h_25(double *state, double *unused, double *out_2585399580963523364) { - out_2585399580963523364[0] = state[6]; +void h_25(double *state, double *unused, double *out_80606407557009467) { + out_80606407557009467[0] = state[6]; } -void H_25(double *state, double *unused, double *out_4993569966627829866) { - out_4993569966627829866[0] = 0; - out_4993569966627829866[1] = 0; - out_4993569966627829866[2] = 0; - out_4993569966627829866[3] = 0; - out_4993569966627829866[4] = 0; - out_4993569966627829866[5] = 0; - out_4993569966627829866[6] = 1; - out_4993569966627829866[7] = 0; - out_4993569966627829866[8] = 0; +void H_25(double *state, double *unused, double *out_2019209217812215463) { + out_2019209217812215463[0] = 0; + out_2019209217812215463[1] = 0; + out_2019209217812215463[2] = 0; + out_2019209217812215463[3] = 0; + out_2019209217812215463[4] = 0; + out_2019209217812215463[5] = 0; + out_2019209217812215463[6] = 1; + out_2019209217812215463[7] = 0; + out_2019209217812215463[8] = 0; } -void h_24(double *state, double *unused, double *out_7866369606921911129) { - out_7866369606921911129[0] = state[4]; - out_7866369606921911129[1] = state[5]; +void h_24(double *state, double *unused, double *out_8156817116559759487) { + out_8156817116559759487[0] = state[4]; + out_8156817116559759487[1] = state[5]; } -void H_24(double *state, double *unused, double *out_124755101600123014) { - out_124755101600123014[0] = 0; - out_124755101600123014[1] = 0; - out_124755101600123014[2] = 0; - out_124755101600123014[3] = 0; - out_124755101600123014[4] = 1; - out_124755101600123014[5] = 0; - out_124755101600123014[6] = 0; - out_124755101600123014[7] = 0; - out_124755101600123014[8] = 0; - out_124755101600123014[9] = 0; - out_124755101600123014[10] = 0; - out_124755101600123014[11] = 0; - out_124755101600123014[12] = 0; - out_124755101600123014[13] = 0; - out_124755101600123014[14] = 1; - out_124755101600123014[15] = 0; - out_124755101600123014[16] = 0; - out_124755101600123014[17] = 0; +void H_24(double *state, double *unused, double *out_7208855968256979762) { + out_7208855968256979762[0] = 0; + out_7208855968256979762[1] = 0; + out_7208855968256979762[2] = 0; + out_7208855968256979762[3] = 0; + out_7208855968256979762[4] = 1; + out_7208855968256979762[5] = 0; + out_7208855968256979762[6] = 0; + out_7208855968256979762[7] = 0; + out_7208855968256979762[8] = 0; + out_7208855968256979762[9] = 0; + out_7208855968256979762[10] = 0; + out_7208855968256979762[11] = 0; + out_7208855968256979762[12] = 0; + out_7208855968256979762[13] = 0; + out_7208855968256979762[14] = 1; + out_7208855968256979762[15] = 0; + out_7208855968256979762[16] = 0; + out_7208855968256979762[17] = 0; } -void h_30(double *state, double *unused, double *out_1052049947290498986) { - out_1052049947290498986[0] = state[4]; +void h_30(double *state, double *unused, double *out_7682658849263662974) { + out_7682658849263662974[0] = state[4]; } -void H_30(double *state, double *unused, double *out_465873636500221668) { - out_465873636500221668[0] = 0; - out_465873636500221668[1] = 0; - out_465873636500221668[2] = 0; - out_465873636500221668[3] = 0; - out_465873636500221668[4] = 1; - out_465873636500221668[5] = 0; - out_465873636500221668[6] = 0; - out_465873636500221668[7] = 0; - out_465873636500221668[8] = 0; +void H_30(double *state, double *unused, double *out_6546905547939823661) { + out_6546905547939823661[0] = 0; + out_6546905547939823661[1] = 0; + out_6546905547939823661[2] = 0; + out_6546905547939823661[3] = 0; + out_6546905547939823661[4] = 1; + out_6546905547939823661[5] = 0; + out_6546905547939823661[6] = 0; + out_6546905547939823661[7] = 0; + out_6546905547939823661[8] = 0; } -void h_26(double *state, double *unused, double *out_3137755351529974294) { - out_3137755351529974294[0] = state[7]; +void h_26(double *state, double *unused, double *out_2046248304577196316) { + out_2046248304577196316[0] = state[7]; } -void H_26(double *state, double *unused, double *out_1252066647753773642) { - out_1252066647753773642[0] = 0; - out_1252066647753773642[1] = 0; - out_1252066647753773642[2] = 0; - out_1252066647753773642[3] = 0; - out_1252066647753773642[4] = 0; - out_1252066647753773642[5] = 0; - out_1252066647753773642[6] = 0; - out_1252066647753773642[7] = 1; - out_1252066647753773642[8] = 0; +void H_26(double *state, double *unused, double *out_5760712536686271687) { + out_5760712536686271687[0] = 0; + out_5760712536686271687[1] = 0; + out_5760712536686271687[2] = 0; + out_5760712536686271687[3] = 0; + out_5760712536686271687[4] = 0; + out_5760712536686271687[5] = 0; + out_5760712536686271687[6] = 0; + out_5760712536686271687[7] = 1; + out_5760712536686271687[8] = 0; } -void h_27(double *state, double *unused, double *out_3292885906595191724) { - out_3292885906595191724[0] = state[3]; +void h_27(double *state, double *unused, double *out_4505967322855241000) { + out_4505967322855241000[0] = state[3]; } -void H_27(double *state, double *unused, double *out_2689467707684164885) { - out_2689467707684164885[0] = 0; - out_2689467707684164885[1] = 0; - out_2689467707684164885[2] = 0; - out_2689467707684164885[3] = 1; - out_2689467707684164885[4] = 0; - out_2689467707684164885[5] = 0; - out_2689467707684164885[6] = 0; - out_2689467707684164885[7] = 0; - out_2689467707684164885[8] = 0; +void H_27(double *state, double *unused, double *out_8721668859740248572) { + out_8721668859740248572[0] = 0; + out_8721668859740248572[1] = 0; + out_8721668859740248572[2] = 0; + out_8721668859740248572[3] = 1; + out_8721668859740248572[4] = 0; + out_8721668859740248572[5] = 0; + out_8721668859740248572[6] = 0; + out_8721668859740248572[7] = 0; + out_8721668859740248572[8] = 0; } -void h_29(double *state, double *unused, double *out_5346991570123350160) { - out_5346991570123350160[0] = state[1]; +void h_29(double *state, double *unused, double *out_1934047599768798804) { + out_1934047599768798804[0] = state[1]; } -void H_29(double *state, double *unused, double *out_976104980814613852) { - out_976104980814613852[0] = 0; - out_976104980814613852[1] = 1; - out_976104980814613852[2] = 0; - out_976104980814613852[3] = 0; - out_976104980814613852[4] = 0; - out_976104980814613852[5] = 0; - out_976104980814613852[6] = 0; - out_976104980814613852[7] = 0; - out_976104980814613852[8] = 0; +void H_29(double *state, double *unused, double *out_6036674203625431477) { + out_6036674203625431477[0] = 0; + out_6036674203625431477[1] = 1; + out_6036674203625431477[2] = 0; + out_6036674203625431477[3] = 0; + out_6036674203625431477[4] = 0; + out_6036674203625431477[5] = 0; + out_6036674203625431477[6] = 0; + out_6036674203625431477[7] = 0; + out_6036674203625431477[8] = 0; } -void h_28(double *state, double *unused, double *out_6565695485309518484) { - out_6565695485309518484[0] = state[0]; +void h_28(double *state, double *unused, double *out_3899689496788985653) { + out_3899689496788985653[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2939735252379940103) { - out_2939735252379940103[0] = 1; - out_2939735252379940103[1] = 0; - out_2939735252379940103[2] = 0; - out_2939735252379940103[3] = 0; - out_2939735252379940103[4] = 0; - out_2939735252379940103[5] = 0; - out_2939735252379940103[6] = 0; - out_2939735252379940103[7] = 0; - out_2939735252379940103[8] = 0; +void H_28(double *state, double *unused, double *out_7327670853014589565) { + out_7327670853014589565[0] = 1; + out_7327670853014589565[1] = 0; + out_7327670853014589565[2] = 0; + out_7327670853014589565[3] = 0; + out_7327670853014589565[4] = 0; + out_7327670853014589565[5] = 0; + out_7327670853014589565[6] = 0; + out_7327670853014589565[7] = 0; + out_7327670853014589565[8] = 0; } -void h_31(double *state, double *unused, double *out_7496068987004855670) { - out_7496068987004855670[0] = state[8]; +void h_31(double *state, double *unused, double *out_40967961750261831) { + out_40967961750261831[0] = state[8]; } -void H_31(double *state, double *unused, double *out_625858545520422166) { - out_625858545520422166[0] = 0; - out_625858545520422166[1] = 0; - out_625858545520422166[2] = 0; - out_625858545520422166[3] = 0; - out_625858545520422166[4] = 0; - out_625858545520422166[5] = 0; - out_625858545520422166[6] = 0; - out_625858545520422166[7] = 0; - out_625858545520422166[8] = 1; +void H_31(double *state, double *unused, double *out_6386920638919623163) { + out_6386920638919623163[0] = 0; + out_6386920638919623163[1] = 0; + out_6386920638919623163[2] = 0; + out_6386920638919623163[3] = 0; + out_6386920638919623163[4] = 0; + out_6386920638919623163[5] = 0; + out_6386920638919623163[6] = 0; + out_6386920638919623163[7] = 0; + out_6386920638919623163[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_5325374827696631328) { - err_fun(nom_x, delta_x, out_5325374827696631328); +void car_err_fun(double *nom_x, double *delta_x, double *out_3886210737280219279) { + err_fun(nom_x, delta_x, out_3886210737280219279); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_263695042767338590) { - inv_err_fun(nom_x, true_x, out_263695042767338590); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_9203878995777804762) { + inv_err_fun(nom_x, true_x, out_9203878995777804762); } -void car_H_mod_fun(double *state, double *out_2663737869404174494) { - H_mod_fun(state, out_2663737869404174494); +void car_H_mod_fun(double *state, double *out_8506196822923476348) { + H_mod_fun(state, out_8506196822923476348); } -void car_f_fun(double *state, double dt, double *out_8230420991833863893) { - f_fun(state, dt, out_8230420991833863893); +void car_f_fun(double *state, double dt, double *out_2212415948681408961) { + f_fun(state, dt, out_2212415948681408961); } -void car_F_fun(double *state, double dt, double *out_2756618938696411588) { - F_fun(state, dt, out_2756618938696411588); +void car_F_fun(double *state, double dt, double *out_8605893895722033804) { + F_fun(state, dt, out_8605893895722033804); } -void car_h_25(double *state, double *unused, double *out_2585399580963523364) { - h_25(state, unused, out_2585399580963523364); +void car_h_25(double *state, double *unused, double *out_80606407557009467) { + h_25(state, unused, out_80606407557009467); } -void car_H_25(double *state, double *unused, double *out_4993569966627829866) { - H_25(state, unused, out_4993569966627829866); +void car_H_25(double *state, double *unused, double *out_2019209217812215463) { + H_25(state, unused, out_2019209217812215463); } -void car_h_24(double *state, double *unused, double *out_7866369606921911129) { - h_24(state, unused, out_7866369606921911129); +void car_h_24(double *state, double *unused, double *out_8156817116559759487) { + h_24(state, unused, out_8156817116559759487); } -void car_H_24(double *state, double *unused, double *out_124755101600123014) { - H_24(state, unused, out_124755101600123014); +void car_H_24(double *state, double *unused, double *out_7208855968256979762) { + H_24(state, unused, out_7208855968256979762); } -void car_h_30(double *state, double *unused, double *out_1052049947290498986) { - h_30(state, unused, out_1052049947290498986); +void car_h_30(double *state, double *unused, double *out_7682658849263662974) { + h_30(state, unused, out_7682658849263662974); } -void car_H_30(double *state, double *unused, double *out_465873636500221668) { - H_30(state, unused, out_465873636500221668); +void car_H_30(double *state, double *unused, double *out_6546905547939823661) { + H_30(state, unused, out_6546905547939823661); } -void car_h_26(double *state, double *unused, double *out_3137755351529974294) { - h_26(state, unused, out_3137755351529974294); +void car_h_26(double *state, double *unused, double *out_2046248304577196316) { + h_26(state, unused, out_2046248304577196316); } -void car_H_26(double *state, double *unused, double *out_1252066647753773642) { - H_26(state, unused, out_1252066647753773642); +void car_H_26(double *state, double *unused, double *out_5760712536686271687) { + H_26(state, unused, out_5760712536686271687); } -void car_h_27(double *state, double *unused, double *out_3292885906595191724) { - h_27(state, unused, out_3292885906595191724); +void car_h_27(double *state, double *unused, double *out_4505967322855241000) { + h_27(state, unused, out_4505967322855241000); } -void car_H_27(double *state, double *unused, double *out_2689467707684164885) { - H_27(state, unused, out_2689467707684164885); +void car_H_27(double *state, double *unused, double *out_8721668859740248572) { + H_27(state, unused, out_8721668859740248572); } -void car_h_29(double *state, double *unused, double *out_5346991570123350160) { - h_29(state, unused, out_5346991570123350160); +void car_h_29(double *state, double *unused, double *out_1934047599768798804) { + h_29(state, unused, out_1934047599768798804); } -void car_H_29(double *state, double *unused, double *out_976104980814613852) { - H_29(state, unused, out_976104980814613852); +void car_H_29(double *state, double *unused, double *out_6036674203625431477) { + H_29(state, unused, out_6036674203625431477); } -void car_h_28(double *state, double *unused, double *out_6565695485309518484) { - h_28(state, unused, out_6565695485309518484); +void car_h_28(double *state, double *unused, double *out_3899689496788985653) { + h_28(state, unused, out_3899689496788985653); } -void car_H_28(double *state, double *unused, double *out_2939735252379940103) { - H_28(state, unused, out_2939735252379940103); +void car_H_28(double *state, double *unused, double *out_7327670853014589565) { + H_28(state, unused, out_7327670853014589565); } -void car_h_31(double *state, double *unused, double *out_7496068987004855670) { - h_31(state, unused, out_7496068987004855670); +void car_h_31(double *state, double *unused, double *out_40967961750261831) { + h_31(state, unused, out_40967961750261831); } -void car_H_31(double *state, double *unused, double *out_625858545520422166) { - H_31(state, unused, out_625858545520422166); +void car_H_31(double *state, double *unused, double *out_6386920638919623163) { + H_31(state, unused, out_6386920638919623163); } 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 442334395..4eb2a4c85 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_5325374827696631328); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_263695042767338590); -void car_H_mod_fun(double *state, double *out_2663737869404174494); -void car_f_fun(double *state, double dt, double *out_8230420991833863893); -void car_F_fun(double *state, double dt, double *out_2756618938696411588); -void car_h_25(double *state, double *unused, double *out_2585399580963523364); -void car_H_25(double *state, double *unused, double *out_4993569966627829866); -void car_h_24(double *state, double *unused, double *out_7866369606921911129); -void car_H_24(double *state, double *unused, double *out_124755101600123014); -void car_h_30(double *state, double *unused, double *out_1052049947290498986); -void car_H_30(double *state, double *unused, double *out_465873636500221668); -void car_h_26(double *state, double *unused, double *out_3137755351529974294); -void car_H_26(double *state, double *unused, double *out_1252066647753773642); -void car_h_27(double *state, double *unused, double *out_3292885906595191724); -void car_H_27(double *state, double *unused, double *out_2689467707684164885); -void car_h_29(double *state, double *unused, double *out_5346991570123350160); -void car_H_29(double *state, double *unused, double *out_976104980814613852); -void car_h_28(double *state, double *unused, double *out_6565695485309518484); -void car_H_28(double *state, double *unused, double *out_2939735252379940103); -void car_h_31(double *state, double *unused, double *out_7496068987004855670); -void car_H_31(double *state, double *unused, double *out_625858545520422166); +void car_err_fun(double *nom_x, double *delta_x, double *out_3886210737280219279); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_9203878995777804762); +void car_H_mod_fun(double *state, double *out_8506196822923476348); +void car_f_fun(double *state, double dt, double *out_2212415948681408961); +void car_F_fun(double *state, double dt, double *out_8605893895722033804); +void car_h_25(double *state, double *unused, double *out_80606407557009467); +void car_H_25(double *state, double *unused, double *out_2019209217812215463); +void car_h_24(double *state, double *unused, double *out_8156817116559759487); +void car_H_24(double *state, double *unused, double *out_7208855968256979762); +void car_h_30(double *state, double *unused, double *out_7682658849263662974); +void car_H_30(double *state, double *unused, double *out_6546905547939823661); +void car_h_26(double *state, double *unused, double *out_2046248304577196316); +void car_H_26(double *state, double *unused, double *out_5760712536686271687); +void car_h_27(double *state, double *unused, double *out_4505967322855241000); +void car_H_27(double *state, double *unused, double *out_8721668859740248572); +void car_h_29(double *state, double *unused, double *out_1934047599768798804); +void car_H_29(double *state, double *unused, double *out_6036674203625431477); +void car_h_28(double *state, double *unused, double *out_3899689496788985653); +void car_H_28(double *state, double *unused, double *out_7327670853014589565); +void car_h_31(double *state, double *unused, double *out_40967961750261831); +void car_H_31(double *state, double *unused, double *out_6386920638919623163); 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 84702ace9..70afd633d 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_4154280524256563890) { - out_4154280524256563890[0] = delta_x[0] + nom_x[0]; - out_4154280524256563890[1] = delta_x[1] + nom_x[1]; - out_4154280524256563890[2] = delta_x[2] + nom_x[2]; - out_4154280524256563890[3] = delta_x[3] + nom_x[3]; - out_4154280524256563890[4] = delta_x[4] + nom_x[4]; - out_4154280524256563890[5] = delta_x[5] + nom_x[5]; - out_4154280524256563890[6] = delta_x[6] + nom_x[6]; - out_4154280524256563890[7] = delta_x[7] + nom_x[7]; - out_4154280524256563890[8] = delta_x[8] + nom_x[8]; - out_4154280524256563890[9] = delta_x[9] + nom_x[9]; - out_4154280524256563890[10] = delta_x[10] + nom_x[10]; - out_4154280524256563890[11] = delta_x[11] + nom_x[11]; - out_4154280524256563890[12] = delta_x[12] + nom_x[12]; - out_4154280524256563890[13] = delta_x[13] + nom_x[13]; - out_4154280524256563890[14] = delta_x[14] + nom_x[14]; - out_4154280524256563890[15] = delta_x[15] + nom_x[15]; - out_4154280524256563890[16] = delta_x[16] + nom_x[16]; - out_4154280524256563890[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7365448194845582757) { + out_7365448194845582757[0] = delta_x[0] + nom_x[0]; + out_7365448194845582757[1] = delta_x[1] + nom_x[1]; + out_7365448194845582757[2] = delta_x[2] + nom_x[2]; + out_7365448194845582757[3] = delta_x[3] + nom_x[3]; + out_7365448194845582757[4] = delta_x[4] + nom_x[4]; + out_7365448194845582757[5] = delta_x[5] + nom_x[5]; + out_7365448194845582757[6] = delta_x[6] + nom_x[6]; + out_7365448194845582757[7] = delta_x[7] + nom_x[7]; + out_7365448194845582757[8] = delta_x[8] + nom_x[8]; + out_7365448194845582757[9] = delta_x[9] + nom_x[9]; + out_7365448194845582757[10] = delta_x[10] + nom_x[10]; + out_7365448194845582757[11] = delta_x[11] + nom_x[11]; + out_7365448194845582757[12] = delta_x[12] + nom_x[12]; + out_7365448194845582757[13] = delta_x[13] + nom_x[13]; + out_7365448194845582757[14] = delta_x[14] + nom_x[14]; + out_7365448194845582757[15] = delta_x[15] + nom_x[15]; + out_7365448194845582757[16] = delta_x[16] + nom_x[16]; + out_7365448194845582757[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5487211563177375728) { - out_5487211563177375728[0] = -nom_x[0] + true_x[0]; - out_5487211563177375728[1] = -nom_x[1] + true_x[1]; - out_5487211563177375728[2] = -nom_x[2] + true_x[2]; - out_5487211563177375728[3] = -nom_x[3] + true_x[3]; - out_5487211563177375728[4] = -nom_x[4] + true_x[4]; - out_5487211563177375728[5] = -nom_x[5] + true_x[5]; - out_5487211563177375728[6] = -nom_x[6] + true_x[6]; - out_5487211563177375728[7] = -nom_x[7] + true_x[7]; - out_5487211563177375728[8] = -nom_x[8] + true_x[8]; - out_5487211563177375728[9] = -nom_x[9] + true_x[9]; - out_5487211563177375728[10] = -nom_x[10] + true_x[10]; - out_5487211563177375728[11] = -nom_x[11] + true_x[11]; - out_5487211563177375728[12] = -nom_x[12] + true_x[12]; - out_5487211563177375728[13] = -nom_x[13] + true_x[13]; - out_5487211563177375728[14] = -nom_x[14] + true_x[14]; - out_5487211563177375728[15] = -nom_x[15] + true_x[15]; - out_5487211563177375728[16] = -nom_x[16] + true_x[16]; - out_5487211563177375728[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1967800537697910635) { + out_1967800537697910635[0] = -nom_x[0] + true_x[0]; + out_1967800537697910635[1] = -nom_x[1] + true_x[1]; + out_1967800537697910635[2] = -nom_x[2] + true_x[2]; + out_1967800537697910635[3] = -nom_x[3] + true_x[3]; + out_1967800537697910635[4] = -nom_x[4] + true_x[4]; + out_1967800537697910635[5] = -nom_x[5] + true_x[5]; + out_1967800537697910635[6] = -nom_x[6] + true_x[6]; + out_1967800537697910635[7] = -nom_x[7] + true_x[7]; + out_1967800537697910635[8] = -nom_x[8] + true_x[8]; + out_1967800537697910635[9] = -nom_x[9] + true_x[9]; + out_1967800537697910635[10] = -nom_x[10] + true_x[10]; + out_1967800537697910635[11] = -nom_x[11] + true_x[11]; + out_1967800537697910635[12] = -nom_x[12] + true_x[12]; + out_1967800537697910635[13] = -nom_x[13] + true_x[13]; + out_1967800537697910635[14] = -nom_x[14] + true_x[14]; + out_1967800537697910635[15] = -nom_x[15] + true_x[15]; + out_1967800537697910635[16] = -nom_x[16] + true_x[16]; + out_1967800537697910635[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_6762039440372011872) { - out_6762039440372011872[0] = 1.0; - out_6762039440372011872[1] = 0.0; - out_6762039440372011872[2] = 0.0; - out_6762039440372011872[3] = 0.0; - out_6762039440372011872[4] = 0.0; - out_6762039440372011872[5] = 0.0; - out_6762039440372011872[6] = 0.0; - out_6762039440372011872[7] = 0.0; - out_6762039440372011872[8] = 0.0; - out_6762039440372011872[9] = 0.0; - out_6762039440372011872[10] = 0.0; - out_6762039440372011872[11] = 0.0; - out_6762039440372011872[12] = 0.0; - out_6762039440372011872[13] = 0.0; - out_6762039440372011872[14] = 0.0; - out_6762039440372011872[15] = 0.0; - out_6762039440372011872[16] = 0.0; - out_6762039440372011872[17] = 0.0; - out_6762039440372011872[18] = 0.0; - out_6762039440372011872[19] = 1.0; - out_6762039440372011872[20] = 0.0; - out_6762039440372011872[21] = 0.0; - out_6762039440372011872[22] = 0.0; - out_6762039440372011872[23] = 0.0; - out_6762039440372011872[24] = 0.0; - out_6762039440372011872[25] = 0.0; - out_6762039440372011872[26] = 0.0; - out_6762039440372011872[27] = 0.0; - out_6762039440372011872[28] = 0.0; - out_6762039440372011872[29] = 0.0; - out_6762039440372011872[30] = 0.0; - out_6762039440372011872[31] = 0.0; - out_6762039440372011872[32] = 0.0; - out_6762039440372011872[33] = 0.0; - out_6762039440372011872[34] = 0.0; - out_6762039440372011872[35] = 0.0; - out_6762039440372011872[36] = 0.0; - out_6762039440372011872[37] = 0.0; - out_6762039440372011872[38] = 1.0; - out_6762039440372011872[39] = 0.0; - out_6762039440372011872[40] = 0.0; - out_6762039440372011872[41] = 0.0; - out_6762039440372011872[42] = 0.0; - out_6762039440372011872[43] = 0.0; - out_6762039440372011872[44] = 0.0; - out_6762039440372011872[45] = 0.0; - out_6762039440372011872[46] = 0.0; - out_6762039440372011872[47] = 0.0; - out_6762039440372011872[48] = 0.0; - out_6762039440372011872[49] = 0.0; - out_6762039440372011872[50] = 0.0; - out_6762039440372011872[51] = 0.0; - out_6762039440372011872[52] = 0.0; - out_6762039440372011872[53] = 0.0; - out_6762039440372011872[54] = 0.0; - out_6762039440372011872[55] = 0.0; - out_6762039440372011872[56] = 0.0; - out_6762039440372011872[57] = 1.0; - out_6762039440372011872[58] = 0.0; - out_6762039440372011872[59] = 0.0; - out_6762039440372011872[60] = 0.0; - out_6762039440372011872[61] = 0.0; - out_6762039440372011872[62] = 0.0; - out_6762039440372011872[63] = 0.0; - out_6762039440372011872[64] = 0.0; - out_6762039440372011872[65] = 0.0; - out_6762039440372011872[66] = 0.0; - out_6762039440372011872[67] = 0.0; - out_6762039440372011872[68] = 0.0; - out_6762039440372011872[69] = 0.0; - out_6762039440372011872[70] = 0.0; - out_6762039440372011872[71] = 0.0; - out_6762039440372011872[72] = 0.0; - out_6762039440372011872[73] = 0.0; - out_6762039440372011872[74] = 0.0; - out_6762039440372011872[75] = 0.0; - out_6762039440372011872[76] = 1.0; - out_6762039440372011872[77] = 0.0; - out_6762039440372011872[78] = 0.0; - out_6762039440372011872[79] = 0.0; - out_6762039440372011872[80] = 0.0; - out_6762039440372011872[81] = 0.0; - out_6762039440372011872[82] = 0.0; - out_6762039440372011872[83] = 0.0; - out_6762039440372011872[84] = 0.0; - out_6762039440372011872[85] = 0.0; - out_6762039440372011872[86] = 0.0; - out_6762039440372011872[87] = 0.0; - out_6762039440372011872[88] = 0.0; - out_6762039440372011872[89] = 0.0; - out_6762039440372011872[90] = 0.0; - out_6762039440372011872[91] = 0.0; - out_6762039440372011872[92] = 0.0; - out_6762039440372011872[93] = 0.0; - out_6762039440372011872[94] = 0.0; - out_6762039440372011872[95] = 1.0; - out_6762039440372011872[96] = 0.0; - out_6762039440372011872[97] = 0.0; - out_6762039440372011872[98] = 0.0; - out_6762039440372011872[99] = 0.0; - out_6762039440372011872[100] = 0.0; - out_6762039440372011872[101] = 0.0; - out_6762039440372011872[102] = 0.0; - out_6762039440372011872[103] = 0.0; - out_6762039440372011872[104] = 0.0; - out_6762039440372011872[105] = 0.0; - out_6762039440372011872[106] = 0.0; - out_6762039440372011872[107] = 0.0; - out_6762039440372011872[108] = 0.0; - out_6762039440372011872[109] = 0.0; - out_6762039440372011872[110] = 0.0; - out_6762039440372011872[111] = 0.0; - out_6762039440372011872[112] = 0.0; - out_6762039440372011872[113] = 0.0; - out_6762039440372011872[114] = 1.0; - out_6762039440372011872[115] = 0.0; - out_6762039440372011872[116] = 0.0; - out_6762039440372011872[117] = 0.0; - out_6762039440372011872[118] = 0.0; - out_6762039440372011872[119] = 0.0; - out_6762039440372011872[120] = 0.0; - out_6762039440372011872[121] = 0.0; - out_6762039440372011872[122] = 0.0; - out_6762039440372011872[123] = 0.0; - out_6762039440372011872[124] = 0.0; - out_6762039440372011872[125] = 0.0; - out_6762039440372011872[126] = 0.0; - out_6762039440372011872[127] = 0.0; - out_6762039440372011872[128] = 0.0; - out_6762039440372011872[129] = 0.0; - out_6762039440372011872[130] = 0.0; - out_6762039440372011872[131] = 0.0; - out_6762039440372011872[132] = 0.0; - out_6762039440372011872[133] = 1.0; - out_6762039440372011872[134] = 0.0; - out_6762039440372011872[135] = 0.0; - out_6762039440372011872[136] = 0.0; - out_6762039440372011872[137] = 0.0; - out_6762039440372011872[138] = 0.0; - out_6762039440372011872[139] = 0.0; - out_6762039440372011872[140] = 0.0; - out_6762039440372011872[141] = 0.0; - out_6762039440372011872[142] = 0.0; - out_6762039440372011872[143] = 0.0; - out_6762039440372011872[144] = 0.0; - out_6762039440372011872[145] = 0.0; - out_6762039440372011872[146] = 0.0; - out_6762039440372011872[147] = 0.0; - out_6762039440372011872[148] = 0.0; - out_6762039440372011872[149] = 0.0; - out_6762039440372011872[150] = 0.0; - out_6762039440372011872[151] = 0.0; - out_6762039440372011872[152] = 1.0; - out_6762039440372011872[153] = 0.0; - out_6762039440372011872[154] = 0.0; - out_6762039440372011872[155] = 0.0; - out_6762039440372011872[156] = 0.0; - out_6762039440372011872[157] = 0.0; - out_6762039440372011872[158] = 0.0; - out_6762039440372011872[159] = 0.0; - out_6762039440372011872[160] = 0.0; - out_6762039440372011872[161] = 0.0; - out_6762039440372011872[162] = 0.0; - out_6762039440372011872[163] = 0.0; - out_6762039440372011872[164] = 0.0; - out_6762039440372011872[165] = 0.0; - out_6762039440372011872[166] = 0.0; - out_6762039440372011872[167] = 0.0; - out_6762039440372011872[168] = 0.0; - out_6762039440372011872[169] = 0.0; - out_6762039440372011872[170] = 0.0; - out_6762039440372011872[171] = 1.0; - out_6762039440372011872[172] = 0.0; - out_6762039440372011872[173] = 0.0; - out_6762039440372011872[174] = 0.0; - out_6762039440372011872[175] = 0.0; - out_6762039440372011872[176] = 0.0; - out_6762039440372011872[177] = 0.0; - out_6762039440372011872[178] = 0.0; - out_6762039440372011872[179] = 0.0; - out_6762039440372011872[180] = 0.0; - out_6762039440372011872[181] = 0.0; - out_6762039440372011872[182] = 0.0; - out_6762039440372011872[183] = 0.0; - out_6762039440372011872[184] = 0.0; - out_6762039440372011872[185] = 0.0; - out_6762039440372011872[186] = 0.0; - out_6762039440372011872[187] = 0.0; - out_6762039440372011872[188] = 0.0; - out_6762039440372011872[189] = 0.0; - out_6762039440372011872[190] = 1.0; - out_6762039440372011872[191] = 0.0; - out_6762039440372011872[192] = 0.0; - out_6762039440372011872[193] = 0.0; - out_6762039440372011872[194] = 0.0; - out_6762039440372011872[195] = 0.0; - out_6762039440372011872[196] = 0.0; - out_6762039440372011872[197] = 0.0; - out_6762039440372011872[198] = 0.0; - out_6762039440372011872[199] = 0.0; - out_6762039440372011872[200] = 0.0; - out_6762039440372011872[201] = 0.0; - out_6762039440372011872[202] = 0.0; - out_6762039440372011872[203] = 0.0; - out_6762039440372011872[204] = 0.0; - out_6762039440372011872[205] = 0.0; - out_6762039440372011872[206] = 0.0; - out_6762039440372011872[207] = 0.0; - out_6762039440372011872[208] = 0.0; - out_6762039440372011872[209] = 1.0; - out_6762039440372011872[210] = 0.0; - out_6762039440372011872[211] = 0.0; - out_6762039440372011872[212] = 0.0; - out_6762039440372011872[213] = 0.0; - out_6762039440372011872[214] = 0.0; - out_6762039440372011872[215] = 0.0; - out_6762039440372011872[216] = 0.0; - out_6762039440372011872[217] = 0.0; - out_6762039440372011872[218] = 0.0; - out_6762039440372011872[219] = 0.0; - out_6762039440372011872[220] = 0.0; - out_6762039440372011872[221] = 0.0; - out_6762039440372011872[222] = 0.0; - out_6762039440372011872[223] = 0.0; - out_6762039440372011872[224] = 0.0; - out_6762039440372011872[225] = 0.0; - out_6762039440372011872[226] = 0.0; - out_6762039440372011872[227] = 0.0; - out_6762039440372011872[228] = 1.0; - out_6762039440372011872[229] = 0.0; - out_6762039440372011872[230] = 0.0; - out_6762039440372011872[231] = 0.0; - out_6762039440372011872[232] = 0.0; - out_6762039440372011872[233] = 0.0; - out_6762039440372011872[234] = 0.0; - out_6762039440372011872[235] = 0.0; - out_6762039440372011872[236] = 0.0; - out_6762039440372011872[237] = 0.0; - out_6762039440372011872[238] = 0.0; - out_6762039440372011872[239] = 0.0; - out_6762039440372011872[240] = 0.0; - out_6762039440372011872[241] = 0.0; - out_6762039440372011872[242] = 0.0; - out_6762039440372011872[243] = 0.0; - out_6762039440372011872[244] = 0.0; - out_6762039440372011872[245] = 0.0; - out_6762039440372011872[246] = 0.0; - out_6762039440372011872[247] = 1.0; - out_6762039440372011872[248] = 0.0; - out_6762039440372011872[249] = 0.0; - out_6762039440372011872[250] = 0.0; - out_6762039440372011872[251] = 0.0; - out_6762039440372011872[252] = 0.0; - out_6762039440372011872[253] = 0.0; - out_6762039440372011872[254] = 0.0; - out_6762039440372011872[255] = 0.0; - out_6762039440372011872[256] = 0.0; - out_6762039440372011872[257] = 0.0; - out_6762039440372011872[258] = 0.0; - out_6762039440372011872[259] = 0.0; - out_6762039440372011872[260] = 0.0; - out_6762039440372011872[261] = 0.0; - out_6762039440372011872[262] = 0.0; - out_6762039440372011872[263] = 0.0; - out_6762039440372011872[264] = 0.0; - out_6762039440372011872[265] = 0.0; - out_6762039440372011872[266] = 1.0; - out_6762039440372011872[267] = 0.0; - out_6762039440372011872[268] = 0.0; - out_6762039440372011872[269] = 0.0; - out_6762039440372011872[270] = 0.0; - out_6762039440372011872[271] = 0.0; - out_6762039440372011872[272] = 0.0; - out_6762039440372011872[273] = 0.0; - out_6762039440372011872[274] = 0.0; - out_6762039440372011872[275] = 0.0; - out_6762039440372011872[276] = 0.0; - out_6762039440372011872[277] = 0.0; - out_6762039440372011872[278] = 0.0; - out_6762039440372011872[279] = 0.0; - out_6762039440372011872[280] = 0.0; - out_6762039440372011872[281] = 0.0; - out_6762039440372011872[282] = 0.0; - out_6762039440372011872[283] = 0.0; - out_6762039440372011872[284] = 0.0; - out_6762039440372011872[285] = 1.0; - out_6762039440372011872[286] = 0.0; - out_6762039440372011872[287] = 0.0; - out_6762039440372011872[288] = 0.0; - out_6762039440372011872[289] = 0.0; - out_6762039440372011872[290] = 0.0; - out_6762039440372011872[291] = 0.0; - out_6762039440372011872[292] = 0.0; - out_6762039440372011872[293] = 0.0; - out_6762039440372011872[294] = 0.0; - out_6762039440372011872[295] = 0.0; - out_6762039440372011872[296] = 0.0; - out_6762039440372011872[297] = 0.0; - out_6762039440372011872[298] = 0.0; - out_6762039440372011872[299] = 0.0; - out_6762039440372011872[300] = 0.0; - out_6762039440372011872[301] = 0.0; - out_6762039440372011872[302] = 0.0; - out_6762039440372011872[303] = 0.0; - out_6762039440372011872[304] = 1.0; - out_6762039440372011872[305] = 0.0; - out_6762039440372011872[306] = 0.0; - out_6762039440372011872[307] = 0.0; - out_6762039440372011872[308] = 0.0; - out_6762039440372011872[309] = 0.0; - out_6762039440372011872[310] = 0.0; - out_6762039440372011872[311] = 0.0; - out_6762039440372011872[312] = 0.0; - out_6762039440372011872[313] = 0.0; - out_6762039440372011872[314] = 0.0; - out_6762039440372011872[315] = 0.0; - out_6762039440372011872[316] = 0.0; - out_6762039440372011872[317] = 0.0; - out_6762039440372011872[318] = 0.0; - out_6762039440372011872[319] = 0.0; - out_6762039440372011872[320] = 0.0; - out_6762039440372011872[321] = 0.0; - out_6762039440372011872[322] = 0.0; - out_6762039440372011872[323] = 1.0; +void H_mod_fun(double *state, double *out_8390959249454907977) { + out_8390959249454907977[0] = 1.0; + out_8390959249454907977[1] = 0.0; + out_8390959249454907977[2] = 0.0; + out_8390959249454907977[3] = 0.0; + out_8390959249454907977[4] = 0.0; + out_8390959249454907977[5] = 0.0; + out_8390959249454907977[6] = 0.0; + out_8390959249454907977[7] = 0.0; + out_8390959249454907977[8] = 0.0; + out_8390959249454907977[9] = 0.0; + out_8390959249454907977[10] = 0.0; + out_8390959249454907977[11] = 0.0; + out_8390959249454907977[12] = 0.0; + out_8390959249454907977[13] = 0.0; + out_8390959249454907977[14] = 0.0; + out_8390959249454907977[15] = 0.0; + out_8390959249454907977[16] = 0.0; + out_8390959249454907977[17] = 0.0; + out_8390959249454907977[18] = 0.0; + out_8390959249454907977[19] = 1.0; + out_8390959249454907977[20] = 0.0; + out_8390959249454907977[21] = 0.0; + out_8390959249454907977[22] = 0.0; + out_8390959249454907977[23] = 0.0; + out_8390959249454907977[24] = 0.0; + out_8390959249454907977[25] = 0.0; + out_8390959249454907977[26] = 0.0; + out_8390959249454907977[27] = 0.0; + out_8390959249454907977[28] = 0.0; + out_8390959249454907977[29] = 0.0; + out_8390959249454907977[30] = 0.0; + out_8390959249454907977[31] = 0.0; + out_8390959249454907977[32] = 0.0; + out_8390959249454907977[33] = 0.0; + out_8390959249454907977[34] = 0.0; + out_8390959249454907977[35] = 0.0; + out_8390959249454907977[36] = 0.0; + out_8390959249454907977[37] = 0.0; + out_8390959249454907977[38] = 1.0; + out_8390959249454907977[39] = 0.0; + out_8390959249454907977[40] = 0.0; + out_8390959249454907977[41] = 0.0; + out_8390959249454907977[42] = 0.0; + out_8390959249454907977[43] = 0.0; + out_8390959249454907977[44] = 0.0; + out_8390959249454907977[45] = 0.0; + out_8390959249454907977[46] = 0.0; + out_8390959249454907977[47] = 0.0; + out_8390959249454907977[48] = 0.0; + out_8390959249454907977[49] = 0.0; + out_8390959249454907977[50] = 0.0; + out_8390959249454907977[51] = 0.0; + out_8390959249454907977[52] = 0.0; + out_8390959249454907977[53] = 0.0; + out_8390959249454907977[54] = 0.0; + out_8390959249454907977[55] = 0.0; + out_8390959249454907977[56] = 0.0; + out_8390959249454907977[57] = 1.0; + out_8390959249454907977[58] = 0.0; + out_8390959249454907977[59] = 0.0; + out_8390959249454907977[60] = 0.0; + out_8390959249454907977[61] = 0.0; + out_8390959249454907977[62] = 0.0; + out_8390959249454907977[63] = 0.0; + out_8390959249454907977[64] = 0.0; + out_8390959249454907977[65] = 0.0; + out_8390959249454907977[66] = 0.0; + out_8390959249454907977[67] = 0.0; + out_8390959249454907977[68] = 0.0; + out_8390959249454907977[69] = 0.0; + out_8390959249454907977[70] = 0.0; + out_8390959249454907977[71] = 0.0; + out_8390959249454907977[72] = 0.0; + out_8390959249454907977[73] = 0.0; + out_8390959249454907977[74] = 0.0; + out_8390959249454907977[75] = 0.0; + out_8390959249454907977[76] = 1.0; + out_8390959249454907977[77] = 0.0; + out_8390959249454907977[78] = 0.0; + out_8390959249454907977[79] = 0.0; + out_8390959249454907977[80] = 0.0; + out_8390959249454907977[81] = 0.0; + out_8390959249454907977[82] = 0.0; + out_8390959249454907977[83] = 0.0; + out_8390959249454907977[84] = 0.0; + out_8390959249454907977[85] = 0.0; + out_8390959249454907977[86] = 0.0; + out_8390959249454907977[87] = 0.0; + out_8390959249454907977[88] = 0.0; + out_8390959249454907977[89] = 0.0; + out_8390959249454907977[90] = 0.0; + out_8390959249454907977[91] = 0.0; + out_8390959249454907977[92] = 0.0; + out_8390959249454907977[93] = 0.0; + out_8390959249454907977[94] = 0.0; + out_8390959249454907977[95] = 1.0; + out_8390959249454907977[96] = 0.0; + out_8390959249454907977[97] = 0.0; + out_8390959249454907977[98] = 0.0; + out_8390959249454907977[99] = 0.0; + out_8390959249454907977[100] = 0.0; + out_8390959249454907977[101] = 0.0; + out_8390959249454907977[102] = 0.0; + out_8390959249454907977[103] = 0.0; + out_8390959249454907977[104] = 0.0; + out_8390959249454907977[105] = 0.0; + out_8390959249454907977[106] = 0.0; + out_8390959249454907977[107] = 0.0; + out_8390959249454907977[108] = 0.0; + out_8390959249454907977[109] = 0.0; + out_8390959249454907977[110] = 0.0; + out_8390959249454907977[111] = 0.0; + out_8390959249454907977[112] = 0.0; + out_8390959249454907977[113] = 0.0; + out_8390959249454907977[114] = 1.0; + out_8390959249454907977[115] = 0.0; + out_8390959249454907977[116] = 0.0; + out_8390959249454907977[117] = 0.0; + out_8390959249454907977[118] = 0.0; + out_8390959249454907977[119] = 0.0; + out_8390959249454907977[120] = 0.0; + out_8390959249454907977[121] = 0.0; + out_8390959249454907977[122] = 0.0; + out_8390959249454907977[123] = 0.0; + out_8390959249454907977[124] = 0.0; + out_8390959249454907977[125] = 0.0; + out_8390959249454907977[126] = 0.0; + out_8390959249454907977[127] = 0.0; + out_8390959249454907977[128] = 0.0; + out_8390959249454907977[129] = 0.0; + out_8390959249454907977[130] = 0.0; + out_8390959249454907977[131] = 0.0; + out_8390959249454907977[132] = 0.0; + out_8390959249454907977[133] = 1.0; + out_8390959249454907977[134] = 0.0; + out_8390959249454907977[135] = 0.0; + out_8390959249454907977[136] = 0.0; + out_8390959249454907977[137] = 0.0; + out_8390959249454907977[138] = 0.0; + out_8390959249454907977[139] = 0.0; + out_8390959249454907977[140] = 0.0; + out_8390959249454907977[141] = 0.0; + out_8390959249454907977[142] = 0.0; + out_8390959249454907977[143] = 0.0; + out_8390959249454907977[144] = 0.0; + out_8390959249454907977[145] = 0.0; + out_8390959249454907977[146] = 0.0; + out_8390959249454907977[147] = 0.0; + out_8390959249454907977[148] = 0.0; + out_8390959249454907977[149] = 0.0; + out_8390959249454907977[150] = 0.0; + out_8390959249454907977[151] = 0.0; + out_8390959249454907977[152] = 1.0; + out_8390959249454907977[153] = 0.0; + out_8390959249454907977[154] = 0.0; + out_8390959249454907977[155] = 0.0; + out_8390959249454907977[156] = 0.0; + out_8390959249454907977[157] = 0.0; + out_8390959249454907977[158] = 0.0; + out_8390959249454907977[159] = 0.0; + out_8390959249454907977[160] = 0.0; + out_8390959249454907977[161] = 0.0; + out_8390959249454907977[162] = 0.0; + out_8390959249454907977[163] = 0.0; + out_8390959249454907977[164] = 0.0; + out_8390959249454907977[165] = 0.0; + out_8390959249454907977[166] = 0.0; + out_8390959249454907977[167] = 0.0; + out_8390959249454907977[168] = 0.0; + out_8390959249454907977[169] = 0.0; + out_8390959249454907977[170] = 0.0; + out_8390959249454907977[171] = 1.0; + out_8390959249454907977[172] = 0.0; + out_8390959249454907977[173] = 0.0; + out_8390959249454907977[174] = 0.0; + out_8390959249454907977[175] = 0.0; + out_8390959249454907977[176] = 0.0; + out_8390959249454907977[177] = 0.0; + out_8390959249454907977[178] = 0.0; + out_8390959249454907977[179] = 0.0; + out_8390959249454907977[180] = 0.0; + out_8390959249454907977[181] = 0.0; + out_8390959249454907977[182] = 0.0; + out_8390959249454907977[183] = 0.0; + out_8390959249454907977[184] = 0.0; + out_8390959249454907977[185] = 0.0; + out_8390959249454907977[186] = 0.0; + out_8390959249454907977[187] = 0.0; + out_8390959249454907977[188] = 0.0; + out_8390959249454907977[189] = 0.0; + out_8390959249454907977[190] = 1.0; + out_8390959249454907977[191] = 0.0; + out_8390959249454907977[192] = 0.0; + out_8390959249454907977[193] = 0.0; + out_8390959249454907977[194] = 0.0; + out_8390959249454907977[195] = 0.0; + out_8390959249454907977[196] = 0.0; + out_8390959249454907977[197] = 0.0; + out_8390959249454907977[198] = 0.0; + out_8390959249454907977[199] = 0.0; + out_8390959249454907977[200] = 0.0; + out_8390959249454907977[201] = 0.0; + out_8390959249454907977[202] = 0.0; + out_8390959249454907977[203] = 0.0; + out_8390959249454907977[204] = 0.0; + out_8390959249454907977[205] = 0.0; + out_8390959249454907977[206] = 0.0; + out_8390959249454907977[207] = 0.0; + out_8390959249454907977[208] = 0.0; + out_8390959249454907977[209] = 1.0; + out_8390959249454907977[210] = 0.0; + out_8390959249454907977[211] = 0.0; + out_8390959249454907977[212] = 0.0; + out_8390959249454907977[213] = 0.0; + out_8390959249454907977[214] = 0.0; + out_8390959249454907977[215] = 0.0; + out_8390959249454907977[216] = 0.0; + out_8390959249454907977[217] = 0.0; + out_8390959249454907977[218] = 0.0; + out_8390959249454907977[219] = 0.0; + out_8390959249454907977[220] = 0.0; + out_8390959249454907977[221] = 0.0; + out_8390959249454907977[222] = 0.0; + out_8390959249454907977[223] = 0.0; + out_8390959249454907977[224] = 0.0; + out_8390959249454907977[225] = 0.0; + out_8390959249454907977[226] = 0.0; + out_8390959249454907977[227] = 0.0; + out_8390959249454907977[228] = 1.0; + out_8390959249454907977[229] = 0.0; + out_8390959249454907977[230] = 0.0; + out_8390959249454907977[231] = 0.0; + out_8390959249454907977[232] = 0.0; + out_8390959249454907977[233] = 0.0; + out_8390959249454907977[234] = 0.0; + out_8390959249454907977[235] = 0.0; + out_8390959249454907977[236] = 0.0; + out_8390959249454907977[237] = 0.0; + out_8390959249454907977[238] = 0.0; + out_8390959249454907977[239] = 0.0; + out_8390959249454907977[240] = 0.0; + out_8390959249454907977[241] = 0.0; + out_8390959249454907977[242] = 0.0; + out_8390959249454907977[243] = 0.0; + out_8390959249454907977[244] = 0.0; + out_8390959249454907977[245] = 0.0; + out_8390959249454907977[246] = 0.0; + out_8390959249454907977[247] = 1.0; + out_8390959249454907977[248] = 0.0; + out_8390959249454907977[249] = 0.0; + out_8390959249454907977[250] = 0.0; + out_8390959249454907977[251] = 0.0; + out_8390959249454907977[252] = 0.0; + out_8390959249454907977[253] = 0.0; + out_8390959249454907977[254] = 0.0; + out_8390959249454907977[255] = 0.0; + out_8390959249454907977[256] = 0.0; + out_8390959249454907977[257] = 0.0; + out_8390959249454907977[258] = 0.0; + out_8390959249454907977[259] = 0.0; + out_8390959249454907977[260] = 0.0; + out_8390959249454907977[261] = 0.0; + out_8390959249454907977[262] = 0.0; + out_8390959249454907977[263] = 0.0; + out_8390959249454907977[264] = 0.0; + out_8390959249454907977[265] = 0.0; + out_8390959249454907977[266] = 1.0; + out_8390959249454907977[267] = 0.0; + out_8390959249454907977[268] = 0.0; + out_8390959249454907977[269] = 0.0; + out_8390959249454907977[270] = 0.0; + out_8390959249454907977[271] = 0.0; + out_8390959249454907977[272] = 0.0; + out_8390959249454907977[273] = 0.0; + out_8390959249454907977[274] = 0.0; + out_8390959249454907977[275] = 0.0; + out_8390959249454907977[276] = 0.0; + out_8390959249454907977[277] = 0.0; + out_8390959249454907977[278] = 0.0; + out_8390959249454907977[279] = 0.0; + out_8390959249454907977[280] = 0.0; + out_8390959249454907977[281] = 0.0; + out_8390959249454907977[282] = 0.0; + out_8390959249454907977[283] = 0.0; + out_8390959249454907977[284] = 0.0; + out_8390959249454907977[285] = 1.0; + out_8390959249454907977[286] = 0.0; + out_8390959249454907977[287] = 0.0; + out_8390959249454907977[288] = 0.0; + out_8390959249454907977[289] = 0.0; + out_8390959249454907977[290] = 0.0; + out_8390959249454907977[291] = 0.0; + out_8390959249454907977[292] = 0.0; + out_8390959249454907977[293] = 0.0; + out_8390959249454907977[294] = 0.0; + out_8390959249454907977[295] = 0.0; + out_8390959249454907977[296] = 0.0; + out_8390959249454907977[297] = 0.0; + out_8390959249454907977[298] = 0.0; + out_8390959249454907977[299] = 0.0; + out_8390959249454907977[300] = 0.0; + out_8390959249454907977[301] = 0.0; + out_8390959249454907977[302] = 0.0; + out_8390959249454907977[303] = 0.0; + out_8390959249454907977[304] = 1.0; + out_8390959249454907977[305] = 0.0; + out_8390959249454907977[306] = 0.0; + out_8390959249454907977[307] = 0.0; + out_8390959249454907977[308] = 0.0; + out_8390959249454907977[309] = 0.0; + out_8390959249454907977[310] = 0.0; + out_8390959249454907977[311] = 0.0; + out_8390959249454907977[312] = 0.0; + out_8390959249454907977[313] = 0.0; + out_8390959249454907977[314] = 0.0; + out_8390959249454907977[315] = 0.0; + out_8390959249454907977[316] = 0.0; + out_8390959249454907977[317] = 0.0; + out_8390959249454907977[318] = 0.0; + out_8390959249454907977[319] = 0.0; + out_8390959249454907977[320] = 0.0; + out_8390959249454907977[321] = 0.0; + out_8390959249454907977[322] = 0.0; + out_8390959249454907977[323] = 1.0; } -void f_fun(double *state, double dt, double *out_2487690271541209027) { - out_2487690271541209027[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_2487690271541209027[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_2487690271541209027[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_2487690271541209027[3] = dt*state[12] + state[3]; - out_2487690271541209027[4] = dt*state[13] + state[4]; - out_2487690271541209027[5] = dt*state[14] + state[5]; - out_2487690271541209027[6] = state[6]; - out_2487690271541209027[7] = state[7]; - out_2487690271541209027[8] = state[8]; - out_2487690271541209027[9] = state[9]; - out_2487690271541209027[10] = state[10]; - out_2487690271541209027[11] = state[11]; - out_2487690271541209027[12] = state[12]; - out_2487690271541209027[13] = state[13]; - out_2487690271541209027[14] = state[14]; - out_2487690271541209027[15] = state[15]; - out_2487690271541209027[16] = state[16]; - out_2487690271541209027[17] = state[17]; +void f_fun(double *state, double dt, double *out_9042230065077868228) { + out_9042230065077868228[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_9042230065077868228[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_9042230065077868228[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_9042230065077868228[3] = dt*state[12] + state[3]; + out_9042230065077868228[4] = dt*state[13] + state[4]; + out_9042230065077868228[5] = dt*state[14] + state[5]; + out_9042230065077868228[6] = state[6]; + out_9042230065077868228[7] = state[7]; + out_9042230065077868228[8] = state[8]; + out_9042230065077868228[9] = state[9]; + out_9042230065077868228[10] = state[10]; + out_9042230065077868228[11] = state[11]; + out_9042230065077868228[12] = state[12]; + out_9042230065077868228[13] = state[13]; + out_9042230065077868228[14] = state[14]; + out_9042230065077868228[15] = state[15]; + out_9042230065077868228[16] = state[16]; + out_9042230065077868228[17] = state[17]; } -void F_fun(double *state, double dt, double *out_950182517979020623) { - out_950182517979020623[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_950182517979020623[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_950182517979020623[2] = 0; - out_950182517979020623[3] = 0; - out_950182517979020623[4] = 0; - out_950182517979020623[5] = 0; - out_950182517979020623[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_950182517979020623[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_950182517979020623[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_950182517979020623[9] = 0; - out_950182517979020623[10] = 0; - out_950182517979020623[11] = 0; - out_950182517979020623[12] = 0; - out_950182517979020623[13] = 0; - out_950182517979020623[14] = 0; - out_950182517979020623[15] = 0; - out_950182517979020623[16] = 0; - out_950182517979020623[17] = 0; - out_950182517979020623[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_950182517979020623[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_950182517979020623[20] = 0; - out_950182517979020623[21] = 0; - out_950182517979020623[22] = 0; - out_950182517979020623[23] = 0; - out_950182517979020623[24] = 0; - out_950182517979020623[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_950182517979020623[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_950182517979020623[27] = 0; - out_950182517979020623[28] = 0; - out_950182517979020623[29] = 0; - out_950182517979020623[30] = 0; - out_950182517979020623[31] = 0; - out_950182517979020623[32] = 0; - out_950182517979020623[33] = 0; - out_950182517979020623[34] = 0; - out_950182517979020623[35] = 0; - out_950182517979020623[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_950182517979020623[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_950182517979020623[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_950182517979020623[39] = 0; - out_950182517979020623[40] = 0; - out_950182517979020623[41] = 0; - out_950182517979020623[42] = 0; - out_950182517979020623[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_950182517979020623[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_950182517979020623[45] = 0; - out_950182517979020623[46] = 0; - out_950182517979020623[47] = 0; - out_950182517979020623[48] = 0; - out_950182517979020623[49] = 0; - out_950182517979020623[50] = 0; - out_950182517979020623[51] = 0; - out_950182517979020623[52] = 0; - out_950182517979020623[53] = 0; - out_950182517979020623[54] = 0; - out_950182517979020623[55] = 0; - out_950182517979020623[56] = 0; - out_950182517979020623[57] = 1; - out_950182517979020623[58] = 0; - out_950182517979020623[59] = 0; - out_950182517979020623[60] = 0; - out_950182517979020623[61] = 0; - out_950182517979020623[62] = 0; - out_950182517979020623[63] = 0; - out_950182517979020623[64] = 0; - out_950182517979020623[65] = 0; - out_950182517979020623[66] = dt; - out_950182517979020623[67] = 0; - out_950182517979020623[68] = 0; - out_950182517979020623[69] = 0; - out_950182517979020623[70] = 0; - out_950182517979020623[71] = 0; - out_950182517979020623[72] = 0; - out_950182517979020623[73] = 0; - out_950182517979020623[74] = 0; - out_950182517979020623[75] = 0; - out_950182517979020623[76] = 1; - out_950182517979020623[77] = 0; - out_950182517979020623[78] = 0; - out_950182517979020623[79] = 0; - out_950182517979020623[80] = 0; - out_950182517979020623[81] = 0; - out_950182517979020623[82] = 0; - out_950182517979020623[83] = 0; - out_950182517979020623[84] = 0; - out_950182517979020623[85] = dt; - out_950182517979020623[86] = 0; - out_950182517979020623[87] = 0; - out_950182517979020623[88] = 0; - out_950182517979020623[89] = 0; - out_950182517979020623[90] = 0; - out_950182517979020623[91] = 0; - out_950182517979020623[92] = 0; - out_950182517979020623[93] = 0; - out_950182517979020623[94] = 0; - out_950182517979020623[95] = 1; - out_950182517979020623[96] = 0; - out_950182517979020623[97] = 0; - out_950182517979020623[98] = 0; - out_950182517979020623[99] = 0; - out_950182517979020623[100] = 0; - out_950182517979020623[101] = 0; - out_950182517979020623[102] = 0; - out_950182517979020623[103] = 0; - out_950182517979020623[104] = dt; - out_950182517979020623[105] = 0; - out_950182517979020623[106] = 0; - out_950182517979020623[107] = 0; - out_950182517979020623[108] = 0; - out_950182517979020623[109] = 0; - out_950182517979020623[110] = 0; - out_950182517979020623[111] = 0; - out_950182517979020623[112] = 0; - out_950182517979020623[113] = 0; - out_950182517979020623[114] = 1; - out_950182517979020623[115] = 0; - out_950182517979020623[116] = 0; - out_950182517979020623[117] = 0; - out_950182517979020623[118] = 0; - out_950182517979020623[119] = 0; - out_950182517979020623[120] = 0; - out_950182517979020623[121] = 0; - out_950182517979020623[122] = 0; - out_950182517979020623[123] = 0; - out_950182517979020623[124] = 0; - out_950182517979020623[125] = 0; - out_950182517979020623[126] = 0; - out_950182517979020623[127] = 0; - out_950182517979020623[128] = 0; - out_950182517979020623[129] = 0; - out_950182517979020623[130] = 0; - out_950182517979020623[131] = 0; - out_950182517979020623[132] = 0; - out_950182517979020623[133] = 1; - out_950182517979020623[134] = 0; - out_950182517979020623[135] = 0; - out_950182517979020623[136] = 0; - out_950182517979020623[137] = 0; - out_950182517979020623[138] = 0; - out_950182517979020623[139] = 0; - out_950182517979020623[140] = 0; - out_950182517979020623[141] = 0; - out_950182517979020623[142] = 0; - out_950182517979020623[143] = 0; - out_950182517979020623[144] = 0; - out_950182517979020623[145] = 0; - out_950182517979020623[146] = 0; - out_950182517979020623[147] = 0; - out_950182517979020623[148] = 0; - out_950182517979020623[149] = 0; - out_950182517979020623[150] = 0; - out_950182517979020623[151] = 0; - out_950182517979020623[152] = 1; - out_950182517979020623[153] = 0; - out_950182517979020623[154] = 0; - out_950182517979020623[155] = 0; - out_950182517979020623[156] = 0; - out_950182517979020623[157] = 0; - out_950182517979020623[158] = 0; - out_950182517979020623[159] = 0; - out_950182517979020623[160] = 0; - out_950182517979020623[161] = 0; - out_950182517979020623[162] = 0; - out_950182517979020623[163] = 0; - out_950182517979020623[164] = 0; - out_950182517979020623[165] = 0; - out_950182517979020623[166] = 0; - out_950182517979020623[167] = 0; - out_950182517979020623[168] = 0; - out_950182517979020623[169] = 0; - out_950182517979020623[170] = 0; - out_950182517979020623[171] = 1; - out_950182517979020623[172] = 0; - out_950182517979020623[173] = 0; - out_950182517979020623[174] = 0; - out_950182517979020623[175] = 0; - out_950182517979020623[176] = 0; - out_950182517979020623[177] = 0; - out_950182517979020623[178] = 0; - out_950182517979020623[179] = 0; - out_950182517979020623[180] = 0; - out_950182517979020623[181] = 0; - out_950182517979020623[182] = 0; - out_950182517979020623[183] = 0; - out_950182517979020623[184] = 0; - out_950182517979020623[185] = 0; - out_950182517979020623[186] = 0; - out_950182517979020623[187] = 0; - out_950182517979020623[188] = 0; - out_950182517979020623[189] = 0; - out_950182517979020623[190] = 1; - out_950182517979020623[191] = 0; - out_950182517979020623[192] = 0; - out_950182517979020623[193] = 0; - out_950182517979020623[194] = 0; - out_950182517979020623[195] = 0; - out_950182517979020623[196] = 0; - out_950182517979020623[197] = 0; - out_950182517979020623[198] = 0; - out_950182517979020623[199] = 0; - out_950182517979020623[200] = 0; - out_950182517979020623[201] = 0; - out_950182517979020623[202] = 0; - out_950182517979020623[203] = 0; - out_950182517979020623[204] = 0; - out_950182517979020623[205] = 0; - out_950182517979020623[206] = 0; - out_950182517979020623[207] = 0; - out_950182517979020623[208] = 0; - out_950182517979020623[209] = 1; - out_950182517979020623[210] = 0; - out_950182517979020623[211] = 0; - out_950182517979020623[212] = 0; - out_950182517979020623[213] = 0; - out_950182517979020623[214] = 0; - out_950182517979020623[215] = 0; - out_950182517979020623[216] = 0; - out_950182517979020623[217] = 0; - out_950182517979020623[218] = 0; - out_950182517979020623[219] = 0; - out_950182517979020623[220] = 0; - out_950182517979020623[221] = 0; - out_950182517979020623[222] = 0; - out_950182517979020623[223] = 0; - out_950182517979020623[224] = 0; - out_950182517979020623[225] = 0; - out_950182517979020623[226] = 0; - out_950182517979020623[227] = 0; - out_950182517979020623[228] = 1; - out_950182517979020623[229] = 0; - out_950182517979020623[230] = 0; - out_950182517979020623[231] = 0; - out_950182517979020623[232] = 0; - out_950182517979020623[233] = 0; - out_950182517979020623[234] = 0; - out_950182517979020623[235] = 0; - out_950182517979020623[236] = 0; - out_950182517979020623[237] = 0; - out_950182517979020623[238] = 0; - out_950182517979020623[239] = 0; - out_950182517979020623[240] = 0; - out_950182517979020623[241] = 0; - out_950182517979020623[242] = 0; - out_950182517979020623[243] = 0; - out_950182517979020623[244] = 0; - out_950182517979020623[245] = 0; - out_950182517979020623[246] = 0; - out_950182517979020623[247] = 1; - out_950182517979020623[248] = 0; - out_950182517979020623[249] = 0; - out_950182517979020623[250] = 0; - out_950182517979020623[251] = 0; - out_950182517979020623[252] = 0; - out_950182517979020623[253] = 0; - out_950182517979020623[254] = 0; - out_950182517979020623[255] = 0; - out_950182517979020623[256] = 0; - out_950182517979020623[257] = 0; - out_950182517979020623[258] = 0; - out_950182517979020623[259] = 0; - out_950182517979020623[260] = 0; - out_950182517979020623[261] = 0; - out_950182517979020623[262] = 0; - out_950182517979020623[263] = 0; - out_950182517979020623[264] = 0; - out_950182517979020623[265] = 0; - out_950182517979020623[266] = 1; - out_950182517979020623[267] = 0; - out_950182517979020623[268] = 0; - out_950182517979020623[269] = 0; - out_950182517979020623[270] = 0; - out_950182517979020623[271] = 0; - out_950182517979020623[272] = 0; - out_950182517979020623[273] = 0; - out_950182517979020623[274] = 0; - out_950182517979020623[275] = 0; - out_950182517979020623[276] = 0; - out_950182517979020623[277] = 0; - out_950182517979020623[278] = 0; - out_950182517979020623[279] = 0; - out_950182517979020623[280] = 0; - out_950182517979020623[281] = 0; - out_950182517979020623[282] = 0; - out_950182517979020623[283] = 0; - out_950182517979020623[284] = 0; - out_950182517979020623[285] = 1; - out_950182517979020623[286] = 0; - out_950182517979020623[287] = 0; - out_950182517979020623[288] = 0; - out_950182517979020623[289] = 0; - out_950182517979020623[290] = 0; - out_950182517979020623[291] = 0; - out_950182517979020623[292] = 0; - out_950182517979020623[293] = 0; - out_950182517979020623[294] = 0; - out_950182517979020623[295] = 0; - out_950182517979020623[296] = 0; - out_950182517979020623[297] = 0; - out_950182517979020623[298] = 0; - out_950182517979020623[299] = 0; - out_950182517979020623[300] = 0; - out_950182517979020623[301] = 0; - out_950182517979020623[302] = 0; - out_950182517979020623[303] = 0; - out_950182517979020623[304] = 1; - out_950182517979020623[305] = 0; - out_950182517979020623[306] = 0; - out_950182517979020623[307] = 0; - out_950182517979020623[308] = 0; - out_950182517979020623[309] = 0; - out_950182517979020623[310] = 0; - out_950182517979020623[311] = 0; - out_950182517979020623[312] = 0; - out_950182517979020623[313] = 0; - out_950182517979020623[314] = 0; - out_950182517979020623[315] = 0; - out_950182517979020623[316] = 0; - out_950182517979020623[317] = 0; - out_950182517979020623[318] = 0; - out_950182517979020623[319] = 0; - out_950182517979020623[320] = 0; - out_950182517979020623[321] = 0; - out_950182517979020623[322] = 0; - out_950182517979020623[323] = 1; +void F_fun(double *state, double dt, double *out_3211704972733304042) { + out_3211704972733304042[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_3211704972733304042[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_3211704972733304042[2] = 0; + out_3211704972733304042[3] = 0; + out_3211704972733304042[4] = 0; + out_3211704972733304042[5] = 0; + out_3211704972733304042[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_3211704972733304042[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_3211704972733304042[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_3211704972733304042[9] = 0; + out_3211704972733304042[10] = 0; + out_3211704972733304042[11] = 0; + out_3211704972733304042[12] = 0; + out_3211704972733304042[13] = 0; + out_3211704972733304042[14] = 0; + out_3211704972733304042[15] = 0; + out_3211704972733304042[16] = 0; + out_3211704972733304042[17] = 0; + out_3211704972733304042[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_3211704972733304042[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_3211704972733304042[20] = 0; + out_3211704972733304042[21] = 0; + out_3211704972733304042[22] = 0; + out_3211704972733304042[23] = 0; + out_3211704972733304042[24] = 0; + out_3211704972733304042[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_3211704972733304042[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_3211704972733304042[27] = 0; + out_3211704972733304042[28] = 0; + out_3211704972733304042[29] = 0; + out_3211704972733304042[30] = 0; + out_3211704972733304042[31] = 0; + out_3211704972733304042[32] = 0; + out_3211704972733304042[33] = 0; + out_3211704972733304042[34] = 0; + out_3211704972733304042[35] = 0; + out_3211704972733304042[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_3211704972733304042[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_3211704972733304042[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_3211704972733304042[39] = 0; + out_3211704972733304042[40] = 0; + out_3211704972733304042[41] = 0; + out_3211704972733304042[42] = 0; + out_3211704972733304042[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_3211704972733304042[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_3211704972733304042[45] = 0; + out_3211704972733304042[46] = 0; + out_3211704972733304042[47] = 0; + out_3211704972733304042[48] = 0; + out_3211704972733304042[49] = 0; + out_3211704972733304042[50] = 0; + out_3211704972733304042[51] = 0; + out_3211704972733304042[52] = 0; + out_3211704972733304042[53] = 0; + out_3211704972733304042[54] = 0; + out_3211704972733304042[55] = 0; + out_3211704972733304042[56] = 0; + out_3211704972733304042[57] = 1; + out_3211704972733304042[58] = 0; + out_3211704972733304042[59] = 0; + out_3211704972733304042[60] = 0; + out_3211704972733304042[61] = 0; + out_3211704972733304042[62] = 0; + out_3211704972733304042[63] = 0; + out_3211704972733304042[64] = 0; + out_3211704972733304042[65] = 0; + out_3211704972733304042[66] = dt; + out_3211704972733304042[67] = 0; + out_3211704972733304042[68] = 0; + out_3211704972733304042[69] = 0; + out_3211704972733304042[70] = 0; + out_3211704972733304042[71] = 0; + out_3211704972733304042[72] = 0; + out_3211704972733304042[73] = 0; + out_3211704972733304042[74] = 0; + out_3211704972733304042[75] = 0; + out_3211704972733304042[76] = 1; + out_3211704972733304042[77] = 0; + out_3211704972733304042[78] = 0; + out_3211704972733304042[79] = 0; + out_3211704972733304042[80] = 0; + out_3211704972733304042[81] = 0; + out_3211704972733304042[82] = 0; + out_3211704972733304042[83] = 0; + out_3211704972733304042[84] = 0; + out_3211704972733304042[85] = dt; + out_3211704972733304042[86] = 0; + out_3211704972733304042[87] = 0; + out_3211704972733304042[88] = 0; + out_3211704972733304042[89] = 0; + out_3211704972733304042[90] = 0; + out_3211704972733304042[91] = 0; + out_3211704972733304042[92] = 0; + out_3211704972733304042[93] = 0; + out_3211704972733304042[94] = 0; + out_3211704972733304042[95] = 1; + out_3211704972733304042[96] = 0; + out_3211704972733304042[97] = 0; + out_3211704972733304042[98] = 0; + out_3211704972733304042[99] = 0; + out_3211704972733304042[100] = 0; + out_3211704972733304042[101] = 0; + out_3211704972733304042[102] = 0; + out_3211704972733304042[103] = 0; + out_3211704972733304042[104] = dt; + out_3211704972733304042[105] = 0; + out_3211704972733304042[106] = 0; + out_3211704972733304042[107] = 0; + out_3211704972733304042[108] = 0; + out_3211704972733304042[109] = 0; + out_3211704972733304042[110] = 0; + out_3211704972733304042[111] = 0; + out_3211704972733304042[112] = 0; + out_3211704972733304042[113] = 0; + out_3211704972733304042[114] = 1; + out_3211704972733304042[115] = 0; + out_3211704972733304042[116] = 0; + out_3211704972733304042[117] = 0; + out_3211704972733304042[118] = 0; + out_3211704972733304042[119] = 0; + out_3211704972733304042[120] = 0; + out_3211704972733304042[121] = 0; + out_3211704972733304042[122] = 0; + out_3211704972733304042[123] = 0; + out_3211704972733304042[124] = 0; + out_3211704972733304042[125] = 0; + out_3211704972733304042[126] = 0; + out_3211704972733304042[127] = 0; + out_3211704972733304042[128] = 0; + out_3211704972733304042[129] = 0; + out_3211704972733304042[130] = 0; + out_3211704972733304042[131] = 0; + out_3211704972733304042[132] = 0; + out_3211704972733304042[133] = 1; + out_3211704972733304042[134] = 0; + out_3211704972733304042[135] = 0; + out_3211704972733304042[136] = 0; + out_3211704972733304042[137] = 0; + out_3211704972733304042[138] = 0; + out_3211704972733304042[139] = 0; + out_3211704972733304042[140] = 0; + out_3211704972733304042[141] = 0; + out_3211704972733304042[142] = 0; + out_3211704972733304042[143] = 0; + out_3211704972733304042[144] = 0; + out_3211704972733304042[145] = 0; + out_3211704972733304042[146] = 0; + out_3211704972733304042[147] = 0; + out_3211704972733304042[148] = 0; + out_3211704972733304042[149] = 0; + out_3211704972733304042[150] = 0; + out_3211704972733304042[151] = 0; + out_3211704972733304042[152] = 1; + out_3211704972733304042[153] = 0; + out_3211704972733304042[154] = 0; + out_3211704972733304042[155] = 0; + out_3211704972733304042[156] = 0; + out_3211704972733304042[157] = 0; + out_3211704972733304042[158] = 0; + out_3211704972733304042[159] = 0; + out_3211704972733304042[160] = 0; + out_3211704972733304042[161] = 0; + out_3211704972733304042[162] = 0; + out_3211704972733304042[163] = 0; + out_3211704972733304042[164] = 0; + out_3211704972733304042[165] = 0; + out_3211704972733304042[166] = 0; + out_3211704972733304042[167] = 0; + out_3211704972733304042[168] = 0; + out_3211704972733304042[169] = 0; + out_3211704972733304042[170] = 0; + out_3211704972733304042[171] = 1; + out_3211704972733304042[172] = 0; + out_3211704972733304042[173] = 0; + out_3211704972733304042[174] = 0; + out_3211704972733304042[175] = 0; + out_3211704972733304042[176] = 0; + out_3211704972733304042[177] = 0; + out_3211704972733304042[178] = 0; + out_3211704972733304042[179] = 0; + out_3211704972733304042[180] = 0; + out_3211704972733304042[181] = 0; + out_3211704972733304042[182] = 0; + out_3211704972733304042[183] = 0; + out_3211704972733304042[184] = 0; + out_3211704972733304042[185] = 0; + out_3211704972733304042[186] = 0; + out_3211704972733304042[187] = 0; + out_3211704972733304042[188] = 0; + out_3211704972733304042[189] = 0; + out_3211704972733304042[190] = 1; + out_3211704972733304042[191] = 0; + out_3211704972733304042[192] = 0; + out_3211704972733304042[193] = 0; + out_3211704972733304042[194] = 0; + out_3211704972733304042[195] = 0; + out_3211704972733304042[196] = 0; + out_3211704972733304042[197] = 0; + out_3211704972733304042[198] = 0; + out_3211704972733304042[199] = 0; + out_3211704972733304042[200] = 0; + out_3211704972733304042[201] = 0; + out_3211704972733304042[202] = 0; + out_3211704972733304042[203] = 0; + out_3211704972733304042[204] = 0; + out_3211704972733304042[205] = 0; + out_3211704972733304042[206] = 0; + out_3211704972733304042[207] = 0; + out_3211704972733304042[208] = 0; + out_3211704972733304042[209] = 1; + out_3211704972733304042[210] = 0; + out_3211704972733304042[211] = 0; + out_3211704972733304042[212] = 0; + out_3211704972733304042[213] = 0; + out_3211704972733304042[214] = 0; + out_3211704972733304042[215] = 0; + out_3211704972733304042[216] = 0; + out_3211704972733304042[217] = 0; + out_3211704972733304042[218] = 0; + out_3211704972733304042[219] = 0; + out_3211704972733304042[220] = 0; + out_3211704972733304042[221] = 0; + out_3211704972733304042[222] = 0; + out_3211704972733304042[223] = 0; + out_3211704972733304042[224] = 0; + out_3211704972733304042[225] = 0; + out_3211704972733304042[226] = 0; + out_3211704972733304042[227] = 0; + out_3211704972733304042[228] = 1; + out_3211704972733304042[229] = 0; + out_3211704972733304042[230] = 0; + out_3211704972733304042[231] = 0; + out_3211704972733304042[232] = 0; + out_3211704972733304042[233] = 0; + out_3211704972733304042[234] = 0; + out_3211704972733304042[235] = 0; + out_3211704972733304042[236] = 0; + out_3211704972733304042[237] = 0; + out_3211704972733304042[238] = 0; + out_3211704972733304042[239] = 0; + out_3211704972733304042[240] = 0; + out_3211704972733304042[241] = 0; + out_3211704972733304042[242] = 0; + out_3211704972733304042[243] = 0; + out_3211704972733304042[244] = 0; + out_3211704972733304042[245] = 0; + out_3211704972733304042[246] = 0; + out_3211704972733304042[247] = 1; + out_3211704972733304042[248] = 0; + out_3211704972733304042[249] = 0; + out_3211704972733304042[250] = 0; + out_3211704972733304042[251] = 0; + out_3211704972733304042[252] = 0; + out_3211704972733304042[253] = 0; + out_3211704972733304042[254] = 0; + out_3211704972733304042[255] = 0; + out_3211704972733304042[256] = 0; + out_3211704972733304042[257] = 0; + out_3211704972733304042[258] = 0; + out_3211704972733304042[259] = 0; + out_3211704972733304042[260] = 0; + out_3211704972733304042[261] = 0; + out_3211704972733304042[262] = 0; + out_3211704972733304042[263] = 0; + out_3211704972733304042[264] = 0; + out_3211704972733304042[265] = 0; + out_3211704972733304042[266] = 1; + out_3211704972733304042[267] = 0; + out_3211704972733304042[268] = 0; + out_3211704972733304042[269] = 0; + out_3211704972733304042[270] = 0; + out_3211704972733304042[271] = 0; + out_3211704972733304042[272] = 0; + out_3211704972733304042[273] = 0; + out_3211704972733304042[274] = 0; + out_3211704972733304042[275] = 0; + out_3211704972733304042[276] = 0; + out_3211704972733304042[277] = 0; + out_3211704972733304042[278] = 0; + out_3211704972733304042[279] = 0; + out_3211704972733304042[280] = 0; + out_3211704972733304042[281] = 0; + out_3211704972733304042[282] = 0; + out_3211704972733304042[283] = 0; + out_3211704972733304042[284] = 0; + out_3211704972733304042[285] = 1; + out_3211704972733304042[286] = 0; + out_3211704972733304042[287] = 0; + out_3211704972733304042[288] = 0; + out_3211704972733304042[289] = 0; + out_3211704972733304042[290] = 0; + out_3211704972733304042[291] = 0; + out_3211704972733304042[292] = 0; + out_3211704972733304042[293] = 0; + out_3211704972733304042[294] = 0; + out_3211704972733304042[295] = 0; + out_3211704972733304042[296] = 0; + out_3211704972733304042[297] = 0; + out_3211704972733304042[298] = 0; + out_3211704972733304042[299] = 0; + out_3211704972733304042[300] = 0; + out_3211704972733304042[301] = 0; + out_3211704972733304042[302] = 0; + out_3211704972733304042[303] = 0; + out_3211704972733304042[304] = 1; + out_3211704972733304042[305] = 0; + out_3211704972733304042[306] = 0; + out_3211704972733304042[307] = 0; + out_3211704972733304042[308] = 0; + out_3211704972733304042[309] = 0; + out_3211704972733304042[310] = 0; + out_3211704972733304042[311] = 0; + out_3211704972733304042[312] = 0; + out_3211704972733304042[313] = 0; + out_3211704972733304042[314] = 0; + out_3211704972733304042[315] = 0; + out_3211704972733304042[316] = 0; + out_3211704972733304042[317] = 0; + out_3211704972733304042[318] = 0; + out_3211704972733304042[319] = 0; + out_3211704972733304042[320] = 0; + out_3211704972733304042[321] = 0; + out_3211704972733304042[322] = 0; + out_3211704972733304042[323] = 1; } -void h_4(double *state, double *unused, double *out_1064982969821053038) { - out_1064982969821053038[0] = state[6] + state[9]; - out_1064982969821053038[1] = state[7] + state[10]; - out_1064982969821053038[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_4618547932677025290) { + out_4618547932677025290[0] = state[6] + state[9]; + out_4618547932677025290[1] = state[7] + state[10]; + out_4618547932677025290[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_1976156791710977266) { - out_1976156791710977266[0] = 0; - out_1976156791710977266[1] = 0; - out_1976156791710977266[2] = 0; - out_1976156791710977266[3] = 0; - out_1976156791710977266[4] = 0; - out_1976156791710977266[5] = 0; - out_1976156791710977266[6] = 1; - out_1976156791710977266[7] = 0; - out_1976156791710977266[8] = 0; - out_1976156791710977266[9] = 1; - out_1976156791710977266[10] = 0; - out_1976156791710977266[11] = 0; - out_1976156791710977266[12] = 0; - out_1976156791710977266[13] = 0; - out_1976156791710977266[14] = 0; - out_1976156791710977266[15] = 0; - out_1976156791710977266[16] = 0; - out_1976156791710977266[17] = 0; - out_1976156791710977266[18] = 0; - out_1976156791710977266[19] = 0; - out_1976156791710977266[20] = 0; - out_1976156791710977266[21] = 0; - out_1976156791710977266[22] = 0; - out_1976156791710977266[23] = 0; - out_1976156791710977266[24] = 0; - out_1976156791710977266[25] = 1; - out_1976156791710977266[26] = 0; - out_1976156791710977266[27] = 0; - out_1976156791710977266[28] = 1; - out_1976156791710977266[29] = 0; - out_1976156791710977266[30] = 0; - out_1976156791710977266[31] = 0; - out_1976156791710977266[32] = 0; - out_1976156791710977266[33] = 0; - out_1976156791710977266[34] = 0; - out_1976156791710977266[35] = 0; - out_1976156791710977266[36] = 0; - out_1976156791710977266[37] = 0; - out_1976156791710977266[38] = 0; - out_1976156791710977266[39] = 0; - out_1976156791710977266[40] = 0; - out_1976156791710977266[41] = 0; - out_1976156791710977266[42] = 0; - out_1976156791710977266[43] = 0; - out_1976156791710977266[44] = 1; - out_1976156791710977266[45] = 0; - out_1976156791710977266[46] = 0; - out_1976156791710977266[47] = 1; - out_1976156791710977266[48] = 0; - out_1976156791710977266[49] = 0; - out_1976156791710977266[50] = 0; - out_1976156791710977266[51] = 0; - out_1976156791710977266[52] = 0; - out_1976156791710977266[53] = 0; +void H_4(double *state, double *unused, double *out_8857837342371745535) { + out_8857837342371745535[0] = 0; + out_8857837342371745535[1] = 0; + out_8857837342371745535[2] = 0; + out_8857837342371745535[3] = 0; + out_8857837342371745535[4] = 0; + out_8857837342371745535[5] = 0; + out_8857837342371745535[6] = 1; + out_8857837342371745535[7] = 0; + out_8857837342371745535[8] = 0; + out_8857837342371745535[9] = 1; + out_8857837342371745535[10] = 0; + out_8857837342371745535[11] = 0; + out_8857837342371745535[12] = 0; + out_8857837342371745535[13] = 0; + out_8857837342371745535[14] = 0; + out_8857837342371745535[15] = 0; + out_8857837342371745535[16] = 0; + out_8857837342371745535[17] = 0; + out_8857837342371745535[18] = 0; + out_8857837342371745535[19] = 0; + out_8857837342371745535[20] = 0; + out_8857837342371745535[21] = 0; + out_8857837342371745535[22] = 0; + out_8857837342371745535[23] = 0; + out_8857837342371745535[24] = 0; + out_8857837342371745535[25] = 1; + out_8857837342371745535[26] = 0; + out_8857837342371745535[27] = 0; + out_8857837342371745535[28] = 1; + out_8857837342371745535[29] = 0; + out_8857837342371745535[30] = 0; + out_8857837342371745535[31] = 0; + out_8857837342371745535[32] = 0; + out_8857837342371745535[33] = 0; + out_8857837342371745535[34] = 0; + out_8857837342371745535[35] = 0; + out_8857837342371745535[36] = 0; + out_8857837342371745535[37] = 0; + out_8857837342371745535[38] = 0; + out_8857837342371745535[39] = 0; + out_8857837342371745535[40] = 0; + out_8857837342371745535[41] = 0; + out_8857837342371745535[42] = 0; + out_8857837342371745535[43] = 0; + out_8857837342371745535[44] = 1; + out_8857837342371745535[45] = 0; + out_8857837342371745535[46] = 0; + out_8857837342371745535[47] = 1; + out_8857837342371745535[48] = 0; + out_8857837342371745535[49] = 0; + out_8857837342371745535[50] = 0; + out_8857837342371745535[51] = 0; + out_8857837342371745535[52] = 0; + out_8857837342371745535[53] = 0; } -void h_10(double *state, double *unused, double *out_9039008373436645978) { - out_9039008373436645978[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_9039008373436645978[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_9039008373436645978[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_286079926766125973) { + out_286079926766125973[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_286079926766125973[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_286079926766125973[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_6832717040485250243) { - out_6832717040485250243[0] = 0; - out_6832717040485250243[1] = 9.8100000000000005*cos(state[1]); - out_6832717040485250243[2] = 0; - out_6832717040485250243[3] = 0; - out_6832717040485250243[4] = -state[8]; - out_6832717040485250243[5] = state[7]; - out_6832717040485250243[6] = 0; - out_6832717040485250243[7] = state[5]; - out_6832717040485250243[8] = -state[4]; - out_6832717040485250243[9] = 0; - out_6832717040485250243[10] = 0; - out_6832717040485250243[11] = 0; - out_6832717040485250243[12] = 1; - out_6832717040485250243[13] = 0; - out_6832717040485250243[14] = 0; - out_6832717040485250243[15] = 1; - out_6832717040485250243[16] = 0; - out_6832717040485250243[17] = 0; - out_6832717040485250243[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_6832717040485250243[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_6832717040485250243[20] = 0; - out_6832717040485250243[21] = state[8]; - out_6832717040485250243[22] = 0; - out_6832717040485250243[23] = -state[6]; - out_6832717040485250243[24] = -state[5]; - out_6832717040485250243[25] = 0; - out_6832717040485250243[26] = state[3]; - out_6832717040485250243[27] = 0; - out_6832717040485250243[28] = 0; - out_6832717040485250243[29] = 0; - out_6832717040485250243[30] = 0; - out_6832717040485250243[31] = 1; - out_6832717040485250243[32] = 0; - out_6832717040485250243[33] = 0; - out_6832717040485250243[34] = 1; - out_6832717040485250243[35] = 0; - out_6832717040485250243[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_6832717040485250243[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_6832717040485250243[38] = 0; - out_6832717040485250243[39] = -state[7]; - out_6832717040485250243[40] = state[6]; - out_6832717040485250243[41] = 0; - out_6832717040485250243[42] = state[4]; - out_6832717040485250243[43] = -state[3]; - out_6832717040485250243[44] = 0; - out_6832717040485250243[45] = 0; - out_6832717040485250243[46] = 0; - out_6832717040485250243[47] = 0; - out_6832717040485250243[48] = 0; - out_6832717040485250243[49] = 0; - out_6832717040485250243[50] = 1; - out_6832717040485250243[51] = 0; - out_6832717040485250243[52] = 0; - out_6832717040485250243[53] = 1; +void H_10(double *state, double *unused, double *out_2479119896376040884) { + out_2479119896376040884[0] = 0; + out_2479119896376040884[1] = 9.8100000000000005*cos(state[1]); + out_2479119896376040884[2] = 0; + out_2479119896376040884[3] = 0; + out_2479119896376040884[4] = -state[8]; + out_2479119896376040884[5] = state[7]; + out_2479119896376040884[6] = 0; + out_2479119896376040884[7] = state[5]; + out_2479119896376040884[8] = -state[4]; + out_2479119896376040884[9] = 0; + out_2479119896376040884[10] = 0; + out_2479119896376040884[11] = 0; + out_2479119896376040884[12] = 1; + out_2479119896376040884[13] = 0; + out_2479119896376040884[14] = 0; + out_2479119896376040884[15] = 1; + out_2479119896376040884[16] = 0; + out_2479119896376040884[17] = 0; + out_2479119896376040884[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_2479119896376040884[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_2479119896376040884[20] = 0; + out_2479119896376040884[21] = state[8]; + out_2479119896376040884[22] = 0; + out_2479119896376040884[23] = -state[6]; + out_2479119896376040884[24] = -state[5]; + out_2479119896376040884[25] = 0; + out_2479119896376040884[26] = state[3]; + out_2479119896376040884[27] = 0; + out_2479119896376040884[28] = 0; + out_2479119896376040884[29] = 0; + out_2479119896376040884[30] = 0; + out_2479119896376040884[31] = 1; + out_2479119896376040884[32] = 0; + out_2479119896376040884[33] = 0; + out_2479119896376040884[34] = 1; + out_2479119896376040884[35] = 0; + out_2479119896376040884[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_2479119896376040884[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_2479119896376040884[38] = 0; + out_2479119896376040884[39] = -state[7]; + out_2479119896376040884[40] = state[6]; + out_2479119896376040884[41] = 0; + out_2479119896376040884[42] = state[4]; + out_2479119896376040884[43] = -state[3]; + out_2479119896376040884[44] = 0; + out_2479119896376040884[45] = 0; + out_2479119896376040884[46] = 0; + out_2479119896376040884[47] = 0; + out_2479119896376040884[48] = 0; + out_2479119896376040884[49] = 0; + out_2479119896376040884[50] = 1; + out_2479119896376040884[51] = 0; + out_2479119896376040884[52] = 0; + out_2479119896376040884[53] = 1; } -void h_13(double *state, double *unused, double *out_6939466162668529956) { - out_6939466162668529956[0] = state[3]; - out_6939466162668529956[1] = state[4]; - out_6939466162668529956[2] = state[5]; +void h_13(double *state, double *unused, double *out_829945007780659418) { + out_829945007780659418[0] = state[3]; + out_829945007780659418[1] = state[4]; + out_829945007780659418[2] = state[5]; } -void H_13(double *state, double *unused, double *out_2540758711392821370) { - out_2540758711392821370[0] = 0; - out_2540758711392821370[1] = 0; - out_2540758711392821370[2] = 0; - out_2540758711392821370[3] = 1; - out_2540758711392821370[4] = 0; - out_2540758711392821370[5] = 0; - out_2540758711392821370[6] = 0; - out_2540758711392821370[7] = 0; - out_2540758711392821370[8] = 0; - out_2540758711392821370[9] = 0; - out_2540758711392821370[10] = 0; - out_2540758711392821370[11] = 0; - out_2540758711392821370[12] = 0; - out_2540758711392821370[13] = 0; - out_2540758711392821370[14] = 0; - out_2540758711392821370[15] = 0; - out_2540758711392821370[16] = 0; - out_2540758711392821370[17] = 0; - out_2540758711392821370[18] = 0; - out_2540758711392821370[19] = 0; - out_2540758711392821370[20] = 0; - out_2540758711392821370[21] = 0; - out_2540758711392821370[22] = 1; - out_2540758711392821370[23] = 0; - out_2540758711392821370[24] = 0; - out_2540758711392821370[25] = 0; - out_2540758711392821370[26] = 0; - out_2540758711392821370[27] = 0; - out_2540758711392821370[28] = 0; - out_2540758711392821370[29] = 0; - out_2540758711392821370[30] = 0; - out_2540758711392821370[31] = 0; - out_2540758711392821370[32] = 0; - out_2540758711392821370[33] = 0; - out_2540758711392821370[34] = 0; - out_2540758711392821370[35] = 0; - out_2540758711392821370[36] = 0; - out_2540758711392821370[37] = 0; - out_2540758711392821370[38] = 0; - out_2540758711392821370[39] = 0; - out_2540758711392821370[40] = 0; - out_2540758711392821370[41] = 1; - out_2540758711392821370[42] = 0; - out_2540758711392821370[43] = 0; - out_2540758711392821370[44] = 0; - out_2540758711392821370[45] = 0; - out_2540758711392821370[46] = 0; - out_2540758711392821370[47] = 0; - out_2540758711392821370[48] = 0; - out_2540758711392821370[49] = 0; - out_2540758711392821370[50] = 0; - out_2540758711392821370[51] = 0; - out_2540758711392821370[52] = 0; - out_2540758711392821370[53] = 0; +void H_13(double *state, double *unused, double *out_5755151268035282057) { + out_5755151268035282057[0] = 0; + out_5755151268035282057[1] = 0; + out_5755151268035282057[2] = 0; + out_5755151268035282057[3] = 1; + out_5755151268035282057[4] = 0; + out_5755151268035282057[5] = 0; + out_5755151268035282057[6] = 0; + out_5755151268035282057[7] = 0; + out_5755151268035282057[8] = 0; + out_5755151268035282057[9] = 0; + out_5755151268035282057[10] = 0; + out_5755151268035282057[11] = 0; + out_5755151268035282057[12] = 0; + out_5755151268035282057[13] = 0; + out_5755151268035282057[14] = 0; + out_5755151268035282057[15] = 0; + out_5755151268035282057[16] = 0; + out_5755151268035282057[17] = 0; + out_5755151268035282057[18] = 0; + out_5755151268035282057[19] = 0; + out_5755151268035282057[20] = 0; + out_5755151268035282057[21] = 0; + out_5755151268035282057[22] = 1; + out_5755151268035282057[23] = 0; + out_5755151268035282057[24] = 0; + out_5755151268035282057[25] = 0; + out_5755151268035282057[26] = 0; + out_5755151268035282057[27] = 0; + out_5755151268035282057[28] = 0; + out_5755151268035282057[29] = 0; + out_5755151268035282057[30] = 0; + out_5755151268035282057[31] = 0; + out_5755151268035282057[32] = 0; + out_5755151268035282057[33] = 0; + out_5755151268035282057[34] = 0; + out_5755151268035282057[35] = 0; + out_5755151268035282057[36] = 0; + out_5755151268035282057[37] = 0; + out_5755151268035282057[38] = 0; + out_5755151268035282057[39] = 0; + out_5755151268035282057[40] = 0; + out_5755151268035282057[41] = 1; + out_5755151268035282057[42] = 0; + out_5755151268035282057[43] = 0; + out_5755151268035282057[44] = 0; + out_5755151268035282057[45] = 0; + out_5755151268035282057[46] = 0; + out_5755151268035282057[47] = 0; + out_5755151268035282057[48] = 0; + out_5755151268035282057[49] = 0; + out_5755151268035282057[50] = 0; + out_5755151268035282057[51] = 0; + out_5755151268035282057[52] = 0; + out_5755151268035282057[53] = 0; } -void h_14(double *state, double *unused, double *out_542665533068743617) { - out_542665533068743617[0] = state[6]; - out_542665533068743617[1] = state[7]; - out_542665533068743617[2] = state[8]; +void h_14(double *state, double *unused, double *out_1492834352067073969) { + out_1492834352067073969[0] = state[6]; + out_1492834352067073969[1] = state[7]; + out_1492834352067073969[2] = state[8]; } -void H_14(double *state, double *unused, double *out_1106631640584395030) { - out_1106631640584395030[0] = 0; - out_1106631640584395030[1] = 0; - out_1106631640584395030[2] = 0; - out_1106631640584395030[3] = 0; - out_1106631640584395030[4] = 0; - out_1106631640584395030[5] = 0; - out_1106631640584395030[6] = 1; - out_1106631640584395030[7] = 0; - out_1106631640584395030[8] = 0; - out_1106631640584395030[9] = 0; - out_1106631640584395030[10] = 0; - out_1106631640584395030[11] = 0; - out_1106631640584395030[12] = 0; - out_1106631640584395030[13] = 0; - out_1106631640584395030[14] = 0; - out_1106631640584395030[15] = 0; - out_1106631640584395030[16] = 0; - out_1106631640584395030[17] = 0; - out_1106631640584395030[18] = 0; - out_1106631640584395030[19] = 0; - out_1106631640584395030[20] = 0; - out_1106631640584395030[21] = 0; - out_1106631640584395030[22] = 0; - out_1106631640584395030[23] = 0; - out_1106631640584395030[24] = 0; - out_1106631640584395030[25] = 1; - out_1106631640584395030[26] = 0; - out_1106631640584395030[27] = 0; - out_1106631640584395030[28] = 0; - out_1106631640584395030[29] = 0; - out_1106631640584395030[30] = 0; - out_1106631640584395030[31] = 0; - out_1106631640584395030[32] = 0; - out_1106631640584395030[33] = 0; - out_1106631640584395030[34] = 0; - out_1106631640584395030[35] = 0; - out_1106631640584395030[36] = 0; - out_1106631640584395030[37] = 0; - out_1106631640584395030[38] = 0; - out_1106631640584395030[39] = 0; - out_1106631640584395030[40] = 0; - out_1106631640584395030[41] = 0; - out_1106631640584395030[42] = 0; - out_1106631640584395030[43] = 0; - out_1106631640584395030[44] = 1; - out_1106631640584395030[45] = 0; - out_1106631640584395030[46] = 0; - out_1106631640584395030[47] = 0; - out_1106631640584395030[48] = 0; - out_1106631640584395030[49] = 0; - out_1106631640584395030[50] = 0; - out_1106631640584395030[51] = 0; - out_1106631640584395030[52] = 0; - out_1106631640584395030[53] = 0; +void H_14(double *state, double *unused, double *out_6506118299042433785) { + out_6506118299042433785[0] = 0; + out_6506118299042433785[1] = 0; + out_6506118299042433785[2] = 0; + out_6506118299042433785[3] = 0; + out_6506118299042433785[4] = 0; + out_6506118299042433785[5] = 0; + out_6506118299042433785[6] = 1; + out_6506118299042433785[7] = 0; + out_6506118299042433785[8] = 0; + out_6506118299042433785[9] = 0; + out_6506118299042433785[10] = 0; + out_6506118299042433785[11] = 0; + out_6506118299042433785[12] = 0; + out_6506118299042433785[13] = 0; + out_6506118299042433785[14] = 0; + out_6506118299042433785[15] = 0; + out_6506118299042433785[16] = 0; + out_6506118299042433785[17] = 0; + out_6506118299042433785[18] = 0; + out_6506118299042433785[19] = 0; + out_6506118299042433785[20] = 0; + out_6506118299042433785[21] = 0; + out_6506118299042433785[22] = 0; + out_6506118299042433785[23] = 0; + out_6506118299042433785[24] = 0; + out_6506118299042433785[25] = 1; + out_6506118299042433785[26] = 0; + out_6506118299042433785[27] = 0; + out_6506118299042433785[28] = 0; + out_6506118299042433785[29] = 0; + out_6506118299042433785[30] = 0; + out_6506118299042433785[31] = 0; + out_6506118299042433785[32] = 0; + out_6506118299042433785[33] = 0; + out_6506118299042433785[34] = 0; + out_6506118299042433785[35] = 0; + out_6506118299042433785[36] = 0; + out_6506118299042433785[37] = 0; + out_6506118299042433785[38] = 0; + out_6506118299042433785[39] = 0; + out_6506118299042433785[40] = 0; + out_6506118299042433785[41] = 0; + out_6506118299042433785[42] = 0; + out_6506118299042433785[43] = 0; + out_6506118299042433785[44] = 1; + out_6506118299042433785[45] = 0; + out_6506118299042433785[46] = 0; + out_6506118299042433785[47] = 0; + out_6506118299042433785[48] = 0; + out_6506118299042433785[49] = 0; + out_6506118299042433785[50] = 0; + out_6506118299042433785[51] = 0; + out_6506118299042433785[52] = 0; + out_6506118299042433785[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_4154280524256563890) { - err_fun(nom_x, delta_x, out_4154280524256563890); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7365448194845582757) { + err_fun(nom_x, delta_x, out_7365448194845582757); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5487211563177375728) { - inv_err_fun(nom_x, true_x, out_5487211563177375728); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1967800537697910635) { + inv_err_fun(nom_x, true_x, out_1967800537697910635); } -void pose_H_mod_fun(double *state, double *out_6762039440372011872) { - H_mod_fun(state, out_6762039440372011872); +void pose_H_mod_fun(double *state, double *out_8390959249454907977) { + H_mod_fun(state, out_8390959249454907977); } -void pose_f_fun(double *state, double dt, double *out_2487690271541209027) { - f_fun(state, dt, out_2487690271541209027); +void pose_f_fun(double *state, double dt, double *out_9042230065077868228) { + f_fun(state, dt, out_9042230065077868228); } -void pose_F_fun(double *state, double dt, double *out_950182517979020623) { - F_fun(state, dt, out_950182517979020623); +void pose_F_fun(double *state, double dt, double *out_3211704972733304042) { + F_fun(state, dt, out_3211704972733304042); } -void pose_h_4(double *state, double *unused, double *out_1064982969821053038) { - h_4(state, unused, out_1064982969821053038); +void pose_h_4(double *state, double *unused, double *out_4618547932677025290) { + h_4(state, unused, out_4618547932677025290); } -void pose_H_4(double *state, double *unused, double *out_1976156791710977266) { - H_4(state, unused, out_1976156791710977266); +void pose_H_4(double *state, double *unused, double *out_8857837342371745535) { + H_4(state, unused, out_8857837342371745535); } -void pose_h_10(double *state, double *unused, double *out_9039008373436645978) { - h_10(state, unused, out_9039008373436645978); +void pose_h_10(double *state, double *unused, double *out_286079926766125973) { + h_10(state, unused, out_286079926766125973); } -void pose_H_10(double *state, double *unused, double *out_6832717040485250243) { - H_10(state, unused, out_6832717040485250243); +void pose_H_10(double *state, double *unused, double *out_2479119896376040884) { + H_10(state, unused, out_2479119896376040884); } -void pose_h_13(double *state, double *unused, double *out_6939466162668529956) { - h_13(state, unused, out_6939466162668529956); +void pose_h_13(double *state, double *unused, double *out_829945007780659418) { + h_13(state, unused, out_829945007780659418); } -void pose_H_13(double *state, double *unused, double *out_2540758711392821370) { - H_13(state, unused, out_2540758711392821370); +void pose_H_13(double *state, double *unused, double *out_5755151268035282057) { + H_13(state, unused, out_5755151268035282057); } -void pose_h_14(double *state, double *unused, double *out_542665533068743617) { - h_14(state, unused, out_542665533068743617); +void pose_h_14(double *state, double *unused, double *out_1492834352067073969) { + h_14(state, unused, out_1492834352067073969); } -void pose_H_14(double *state, double *unused, double *out_1106631640584395030) { - H_14(state, unused, out_1106631640584395030); +void pose_H_14(double *state, double *unused, double *out_6506118299042433785) { + H_14(state, unused, out_6506118299042433785); } 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 b33fb04e2..9dff96d39 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_4154280524256563890); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5487211563177375728); -void pose_H_mod_fun(double *state, double *out_6762039440372011872); -void pose_f_fun(double *state, double dt, double *out_2487690271541209027); -void pose_F_fun(double *state, double dt, double *out_950182517979020623); -void pose_h_4(double *state, double *unused, double *out_1064982969821053038); -void pose_H_4(double *state, double *unused, double *out_1976156791710977266); -void pose_h_10(double *state, double *unused, double *out_9039008373436645978); -void pose_H_10(double *state, double *unused, double *out_6832717040485250243); -void pose_h_13(double *state, double *unused, double *out_6939466162668529956); -void pose_H_13(double *state, double *unused, double *out_2540758711392821370); -void pose_h_14(double *state, double *unused, double *out_542665533068743617); -void pose_H_14(double *state, double *unused, double *out_1106631640584395030); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7365448194845582757); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1967800537697910635); +void pose_H_mod_fun(double *state, double *out_8390959249454907977); +void pose_f_fun(double *state, double dt, double *out_9042230065077868228); +void pose_F_fun(double *state, double dt, double *out_3211704972733304042); +void pose_h_4(double *state, double *unused, double *out_4618547932677025290); +void pose_H_4(double *state, double *unused, double *out_8857837342371745535); +void pose_h_10(double *state, double *unused, double *out_286079926766125973); +void pose_H_10(double *state, double *unused, double *out_2479119896376040884); +void pose_h_13(double *state, double *unused, double *out_829945007780659418); +void pose_H_13(double *state, double *unused, double *out_5755151268035282057); +void pose_h_14(double *state, double *unused, double *out_1492834352067073969); +void pose_H_14(double *state, double *unused, double *out_6506118299042433785); 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 21e9658db..ad313a897 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index b6f773bf7..58988e073 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 7dad5af8f..f2c3eed0e 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 6161a4843..a583dd32a 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 3f0845caf..8c36a90b2 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 5172a36c7..eec000ee0 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ