diff --git a/cereal/libcereal.a b/cereal/libcereal.a index 791c5c19e..5942af15e 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/common/libcommon.a b/common/libcommon.a index 7b235168a..5e5513f06 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 ae3b39099..0970e269f 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 d29cf66a8..fcac95e54 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 a9282164c..866ed3e20 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 85fb1535c..8326f2976 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 5d7efaa1c..91694fe9b 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 6e0e38479..c5beaecac 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 8da8c0a49..a4a95e3d1 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 8d495eb75..affb4386f 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 8d495eb75..affb4386f 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 2d007c023..863544f32 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 8da8c0a49..a4a95e3d1 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 b5ce301c0..3f1e55cae 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-d1fad265-DEBUG"; +const uint8_t gitversion[19] = "DEV-357d7e54-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index cf4de4556..2252cfe04 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 4f14f33d5..bcb57c060 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 2a2567760..3139211e6 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 228469f6c..8398d0876 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 d5da7ae40..bb047d8cf 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 e4dedb0c5..e373dab7e 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 f1536824e..36b1dfa8e 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 32d77ca62..665588992 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 8c33bc7a7..7886b8194 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 06cd6099a..d7afaa17b 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 84e479941..236b455b2 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 b1bf66ff1..f3e6a8244 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 71d13fe40..d47652dc8 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 1456d83a6..59e640745 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 f8cf09fc0..66484651a 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 e9e048afe..a5c41f18d 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 5cb1aa8cf..25b209cba 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 7a55c1485..f97773645 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 5aef14f47..ef33bc52a 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 1ba0c42d1..ad88a973e 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 030126d9b..9f84892ea 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-d1fad265-DEBUG \ No newline at end of file +DEV-357d7e54-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 74ad2402d..832bd8015 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_8137803062968279058) { - out_8137803062968279058[0] = delta_x[0] + nom_x[0]; - out_8137803062968279058[1] = delta_x[1] + nom_x[1]; - out_8137803062968279058[2] = delta_x[2] + nom_x[2]; - out_8137803062968279058[3] = delta_x[3] + nom_x[3]; - out_8137803062968279058[4] = delta_x[4] + nom_x[4]; - out_8137803062968279058[5] = delta_x[5] + nom_x[5]; - out_8137803062968279058[6] = delta_x[6] + nom_x[6]; - out_8137803062968279058[7] = delta_x[7] + nom_x[7]; - out_8137803062968279058[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1038389500972116158) { + out_1038389500972116158[0] = delta_x[0] + nom_x[0]; + out_1038389500972116158[1] = delta_x[1] + nom_x[1]; + out_1038389500972116158[2] = delta_x[2] + nom_x[2]; + out_1038389500972116158[3] = delta_x[3] + nom_x[3]; + out_1038389500972116158[4] = delta_x[4] + nom_x[4]; + out_1038389500972116158[5] = delta_x[5] + nom_x[5]; + out_1038389500972116158[6] = delta_x[6] + nom_x[6]; + out_1038389500972116158[7] = delta_x[7] + nom_x[7]; + out_1038389500972116158[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4803320297930984943) { - out_4803320297930984943[0] = -nom_x[0] + true_x[0]; - out_4803320297930984943[1] = -nom_x[1] + true_x[1]; - out_4803320297930984943[2] = -nom_x[2] + true_x[2]; - out_4803320297930984943[3] = -nom_x[3] + true_x[3]; - out_4803320297930984943[4] = -nom_x[4] + true_x[4]; - out_4803320297930984943[5] = -nom_x[5] + true_x[5]; - out_4803320297930984943[6] = -nom_x[6] + true_x[6]; - out_4803320297930984943[7] = -nom_x[7] + true_x[7]; - out_4803320297930984943[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6418752408535262595) { + out_6418752408535262595[0] = -nom_x[0] + true_x[0]; + out_6418752408535262595[1] = -nom_x[1] + true_x[1]; + out_6418752408535262595[2] = -nom_x[2] + true_x[2]; + out_6418752408535262595[3] = -nom_x[3] + true_x[3]; + out_6418752408535262595[4] = -nom_x[4] + true_x[4]; + out_6418752408535262595[5] = -nom_x[5] + true_x[5]; + out_6418752408535262595[6] = -nom_x[6] + true_x[6]; + out_6418752408535262595[7] = -nom_x[7] + true_x[7]; + out_6418752408535262595[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_250780943344312618) { - out_250780943344312618[0] = 1.0; - out_250780943344312618[1] = 0.0; - out_250780943344312618[2] = 0.0; - out_250780943344312618[3] = 0.0; - out_250780943344312618[4] = 0.0; - out_250780943344312618[5] = 0.0; - out_250780943344312618[6] = 0.0; - out_250780943344312618[7] = 0.0; - out_250780943344312618[8] = 0.0; - out_250780943344312618[9] = 0.0; - out_250780943344312618[10] = 1.0; - out_250780943344312618[11] = 0.0; - out_250780943344312618[12] = 0.0; - out_250780943344312618[13] = 0.0; - out_250780943344312618[14] = 0.0; - out_250780943344312618[15] = 0.0; - out_250780943344312618[16] = 0.0; - out_250780943344312618[17] = 0.0; - out_250780943344312618[18] = 0.0; - out_250780943344312618[19] = 0.0; - out_250780943344312618[20] = 1.0; - out_250780943344312618[21] = 0.0; - out_250780943344312618[22] = 0.0; - out_250780943344312618[23] = 0.0; - out_250780943344312618[24] = 0.0; - out_250780943344312618[25] = 0.0; - out_250780943344312618[26] = 0.0; - out_250780943344312618[27] = 0.0; - out_250780943344312618[28] = 0.0; - out_250780943344312618[29] = 0.0; - out_250780943344312618[30] = 1.0; - out_250780943344312618[31] = 0.0; - out_250780943344312618[32] = 0.0; - out_250780943344312618[33] = 0.0; - out_250780943344312618[34] = 0.0; - out_250780943344312618[35] = 0.0; - out_250780943344312618[36] = 0.0; - out_250780943344312618[37] = 0.0; - out_250780943344312618[38] = 0.0; - out_250780943344312618[39] = 0.0; - out_250780943344312618[40] = 1.0; - out_250780943344312618[41] = 0.0; - out_250780943344312618[42] = 0.0; - out_250780943344312618[43] = 0.0; - out_250780943344312618[44] = 0.0; - out_250780943344312618[45] = 0.0; - out_250780943344312618[46] = 0.0; - out_250780943344312618[47] = 0.0; - out_250780943344312618[48] = 0.0; - out_250780943344312618[49] = 0.0; - out_250780943344312618[50] = 1.0; - out_250780943344312618[51] = 0.0; - out_250780943344312618[52] = 0.0; - out_250780943344312618[53] = 0.0; - out_250780943344312618[54] = 0.0; - out_250780943344312618[55] = 0.0; - out_250780943344312618[56] = 0.0; - out_250780943344312618[57] = 0.0; - out_250780943344312618[58] = 0.0; - out_250780943344312618[59] = 0.0; - out_250780943344312618[60] = 1.0; - out_250780943344312618[61] = 0.0; - out_250780943344312618[62] = 0.0; - out_250780943344312618[63] = 0.0; - out_250780943344312618[64] = 0.0; - out_250780943344312618[65] = 0.0; - out_250780943344312618[66] = 0.0; - out_250780943344312618[67] = 0.0; - out_250780943344312618[68] = 0.0; - out_250780943344312618[69] = 0.0; - out_250780943344312618[70] = 1.0; - out_250780943344312618[71] = 0.0; - out_250780943344312618[72] = 0.0; - out_250780943344312618[73] = 0.0; - out_250780943344312618[74] = 0.0; - out_250780943344312618[75] = 0.0; - out_250780943344312618[76] = 0.0; - out_250780943344312618[77] = 0.0; - out_250780943344312618[78] = 0.0; - out_250780943344312618[79] = 0.0; - out_250780943344312618[80] = 1.0; +void H_mod_fun(double *state, double *out_2855368609596965229) { + out_2855368609596965229[0] = 1.0; + out_2855368609596965229[1] = 0.0; + out_2855368609596965229[2] = 0.0; + out_2855368609596965229[3] = 0.0; + out_2855368609596965229[4] = 0.0; + out_2855368609596965229[5] = 0.0; + out_2855368609596965229[6] = 0.0; + out_2855368609596965229[7] = 0.0; + out_2855368609596965229[8] = 0.0; + out_2855368609596965229[9] = 0.0; + out_2855368609596965229[10] = 1.0; + out_2855368609596965229[11] = 0.0; + out_2855368609596965229[12] = 0.0; + out_2855368609596965229[13] = 0.0; + out_2855368609596965229[14] = 0.0; + out_2855368609596965229[15] = 0.0; + out_2855368609596965229[16] = 0.0; + out_2855368609596965229[17] = 0.0; + out_2855368609596965229[18] = 0.0; + out_2855368609596965229[19] = 0.0; + out_2855368609596965229[20] = 1.0; + out_2855368609596965229[21] = 0.0; + out_2855368609596965229[22] = 0.0; + out_2855368609596965229[23] = 0.0; + out_2855368609596965229[24] = 0.0; + out_2855368609596965229[25] = 0.0; + out_2855368609596965229[26] = 0.0; + out_2855368609596965229[27] = 0.0; + out_2855368609596965229[28] = 0.0; + out_2855368609596965229[29] = 0.0; + out_2855368609596965229[30] = 1.0; + out_2855368609596965229[31] = 0.0; + out_2855368609596965229[32] = 0.0; + out_2855368609596965229[33] = 0.0; + out_2855368609596965229[34] = 0.0; + out_2855368609596965229[35] = 0.0; + out_2855368609596965229[36] = 0.0; + out_2855368609596965229[37] = 0.0; + out_2855368609596965229[38] = 0.0; + out_2855368609596965229[39] = 0.0; + out_2855368609596965229[40] = 1.0; + out_2855368609596965229[41] = 0.0; + out_2855368609596965229[42] = 0.0; + out_2855368609596965229[43] = 0.0; + out_2855368609596965229[44] = 0.0; + out_2855368609596965229[45] = 0.0; + out_2855368609596965229[46] = 0.0; + out_2855368609596965229[47] = 0.0; + out_2855368609596965229[48] = 0.0; + out_2855368609596965229[49] = 0.0; + out_2855368609596965229[50] = 1.0; + out_2855368609596965229[51] = 0.0; + out_2855368609596965229[52] = 0.0; + out_2855368609596965229[53] = 0.0; + out_2855368609596965229[54] = 0.0; + out_2855368609596965229[55] = 0.0; + out_2855368609596965229[56] = 0.0; + out_2855368609596965229[57] = 0.0; + out_2855368609596965229[58] = 0.0; + out_2855368609596965229[59] = 0.0; + out_2855368609596965229[60] = 1.0; + out_2855368609596965229[61] = 0.0; + out_2855368609596965229[62] = 0.0; + out_2855368609596965229[63] = 0.0; + out_2855368609596965229[64] = 0.0; + out_2855368609596965229[65] = 0.0; + out_2855368609596965229[66] = 0.0; + out_2855368609596965229[67] = 0.0; + out_2855368609596965229[68] = 0.0; + out_2855368609596965229[69] = 0.0; + out_2855368609596965229[70] = 1.0; + out_2855368609596965229[71] = 0.0; + out_2855368609596965229[72] = 0.0; + out_2855368609596965229[73] = 0.0; + out_2855368609596965229[74] = 0.0; + out_2855368609596965229[75] = 0.0; + out_2855368609596965229[76] = 0.0; + out_2855368609596965229[77] = 0.0; + out_2855368609596965229[78] = 0.0; + out_2855368609596965229[79] = 0.0; + out_2855368609596965229[80] = 1.0; } -void f_fun(double *state, double dt, double *out_5023554550498675071) { - out_5023554550498675071[0] = state[0]; - out_5023554550498675071[1] = state[1]; - out_5023554550498675071[2] = state[2]; - out_5023554550498675071[3] = state[3]; - out_5023554550498675071[4] = state[4]; - out_5023554550498675071[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_5023554550498675071[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_5023554550498675071[7] = state[7]; - out_5023554550498675071[8] = state[8]; +void f_fun(double *state, double dt, double *out_1397092929687677159) { + out_1397092929687677159[0] = state[0]; + out_1397092929687677159[1] = state[1]; + out_1397092929687677159[2] = state[2]; + out_1397092929687677159[3] = state[3]; + out_1397092929687677159[4] = state[4]; + out_1397092929687677159[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_1397092929687677159[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_1397092929687677159[7] = state[7]; + out_1397092929687677159[8] = state[8]; } -void F_fun(double *state, double dt, double *out_3517018389212170475) { - out_3517018389212170475[0] = 1; - out_3517018389212170475[1] = 0; - out_3517018389212170475[2] = 0; - out_3517018389212170475[3] = 0; - out_3517018389212170475[4] = 0; - out_3517018389212170475[5] = 0; - out_3517018389212170475[6] = 0; - out_3517018389212170475[7] = 0; - out_3517018389212170475[8] = 0; - out_3517018389212170475[9] = 0; - out_3517018389212170475[10] = 1; - out_3517018389212170475[11] = 0; - out_3517018389212170475[12] = 0; - out_3517018389212170475[13] = 0; - out_3517018389212170475[14] = 0; - out_3517018389212170475[15] = 0; - out_3517018389212170475[16] = 0; - out_3517018389212170475[17] = 0; - out_3517018389212170475[18] = 0; - out_3517018389212170475[19] = 0; - out_3517018389212170475[20] = 1; - out_3517018389212170475[21] = 0; - out_3517018389212170475[22] = 0; - out_3517018389212170475[23] = 0; - out_3517018389212170475[24] = 0; - out_3517018389212170475[25] = 0; - out_3517018389212170475[26] = 0; - out_3517018389212170475[27] = 0; - out_3517018389212170475[28] = 0; - out_3517018389212170475[29] = 0; - out_3517018389212170475[30] = 1; - out_3517018389212170475[31] = 0; - out_3517018389212170475[32] = 0; - out_3517018389212170475[33] = 0; - out_3517018389212170475[34] = 0; - out_3517018389212170475[35] = 0; - out_3517018389212170475[36] = 0; - out_3517018389212170475[37] = 0; - out_3517018389212170475[38] = 0; - out_3517018389212170475[39] = 0; - out_3517018389212170475[40] = 1; - out_3517018389212170475[41] = 0; - out_3517018389212170475[42] = 0; - out_3517018389212170475[43] = 0; - out_3517018389212170475[44] = 0; - out_3517018389212170475[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_3517018389212170475[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_3517018389212170475[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3517018389212170475[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3517018389212170475[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_3517018389212170475[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_3517018389212170475[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_3517018389212170475[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_3517018389212170475[53] = -9.8100000000000005*dt; - out_3517018389212170475[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_3517018389212170475[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_3517018389212170475[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3517018389212170475[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3517018389212170475[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_3517018389212170475[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_3517018389212170475[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_3517018389212170475[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3517018389212170475[62] = 0; - out_3517018389212170475[63] = 0; - out_3517018389212170475[64] = 0; - out_3517018389212170475[65] = 0; - out_3517018389212170475[66] = 0; - out_3517018389212170475[67] = 0; - out_3517018389212170475[68] = 0; - out_3517018389212170475[69] = 0; - out_3517018389212170475[70] = 1; - out_3517018389212170475[71] = 0; - out_3517018389212170475[72] = 0; - out_3517018389212170475[73] = 0; - out_3517018389212170475[74] = 0; - out_3517018389212170475[75] = 0; - out_3517018389212170475[76] = 0; - out_3517018389212170475[77] = 0; - out_3517018389212170475[78] = 0; - out_3517018389212170475[79] = 0; - out_3517018389212170475[80] = 1; +void F_fun(double *state, double dt, double *out_7780451684963738443) { + out_7780451684963738443[0] = 1; + out_7780451684963738443[1] = 0; + out_7780451684963738443[2] = 0; + out_7780451684963738443[3] = 0; + out_7780451684963738443[4] = 0; + out_7780451684963738443[5] = 0; + out_7780451684963738443[6] = 0; + out_7780451684963738443[7] = 0; + out_7780451684963738443[8] = 0; + out_7780451684963738443[9] = 0; + out_7780451684963738443[10] = 1; + out_7780451684963738443[11] = 0; + out_7780451684963738443[12] = 0; + out_7780451684963738443[13] = 0; + out_7780451684963738443[14] = 0; + out_7780451684963738443[15] = 0; + out_7780451684963738443[16] = 0; + out_7780451684963738443[17] = 0; + out_7780451684963738443[18] = 0; + out_7780451684963738443[19] = 0; + out_7780451684963738443[20] = 1; + out_7780451684963738443[21] = 0; + out_7780451684963738443[22] = 0; + out_7780451684963738443[23] = 0; + out_7780451684963738443[24] = 0; + out_7780451684963738443[25] = 0; + out_7780451684963738443[26] = 0; + out_7780451684963738443[27] = 0; + out_7780451684963738443[28] = 0; + out_7780451684963738443[29] = 0; + out_7780451684963738443[30] = 1; + out_7780451684963738443[31] = 0; + out_7780451684963738443[32] = 0; + out_7780451684963738443[33] = 0; + out_7780451684963738443[34] = 0; + out_7780451684963738443[35] = 0; + out_7780451684963738443[36] = 0; + out_7780451684963738443[37] = 0; + out_7780451684963738443[38] = 0; + out_7780451684963738443[39] = 0; + out_7780451684963738443[40] = 1; + out_7780451684963738443[41] = 0; + out_7780451684963738443[42] = 0; + out_7780451684963738443[43] = 0; + out_7780451684963738443[44] = 0; + out_7780451684963738443[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_7780451684963738443[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_7780451684963738443[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7780451684963738443[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7780451684963738443[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_7780451684963738443[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_7780451684963738443[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_7780451684963738443[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_7780451684963738443[53] = -9.8100000000000005*dt; + out_7780451684963738443[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_7780451684963738443[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_7780451684963738443[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7780451684963738443[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7780451684963738443[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_7780451684963738443[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_7780451684963738443[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_7780451684963738443[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7780451684963738443[62] = 0; + out_7780451684963738443[63] = 0; + out_7780451684963738443[64] = 0; + out_7780451684963738443[65] = 0; + out_7780451684963738443[66] = 0; + out_7780451684963738443[67] = 0; + out_7780451684963738443[68] = 0; + out_7780451684963738443[69] = 0; + out_7780451684963738443[70] = 1; + out_7780451684963738443[71] = 0; + out_7780451684963738443[72] = 0; + out_7780451684963738443[73] = 0; + out_7780451684963738443[74] = 0; + out_7780451684963738443[75] = 0; + out_7780451684963738443[76] = 0; + out_7780451684963738443[77] = 0; + out_7780451684963738443[78] = 0; + out_7780451684963738443[79] = 0; + out_7780451684963738443[80] = 1; } -void h_25(double *state, double *unused, double *out_8788151648350399353) { - out_8788151648350399353[0] = state[6]; +void h_25(double *state, double *unused, double *out_894848580994361563) { + out_894848580994361563[0] = state[6]; } -void H_25(double *state, double *unused, double *out_3773761593333804295) { - out_3773761593333804295[0] = 0; - out_3773761593333804295[1] = 0; - out_3773761593333804295[2] = 0; - out_3773761593333804295[3] = 0; - out_3773761593333804295[4] = 0; - out_3773761593333804295[5] = 0; - out_3773761593333804295[6] = 1; - out_3773761593333804295[7] = 0; - out_3773761593333804295[8] = 0; +void H_25(double *state, double *unused, double *out_6378349259586456906) { + out_6378349259586456906[0] = 0; + out_6378349259586456906[1] = 0; + out_6378349259586456906[2] = 0; + out_6378349259586456906[3] = 0; + out_6378349259586456906[4] = 0; + out_6378349259586456906[5] = 0; + out_6378349259586456906[6] = 1; + out_6378349259586456906[7] = 0; + out_6378349259586456906[8] = 0; } -void h_24(double *state, double *unused, double *out_4617115616065835450) { - out_4617115616065835450[0] = state[4]; - out_4617115616065835450[1] = state[5]; +void h_24(double *state, double *unused, double *out_6711296683837743925) { + out_6711296683837743925[0] = state[4]; + out_6711296683837743925[1] = state[5]; } -void H_24(double *state, double *unused, double *out_5950976016940954268) { - out_5950976016940954268[0] = 0; - out_5950976016940954268[1] = 0; - out_5950976016940954268[2] = 0; - out_5950976016940954268[3] = 0; - out_5950976016940954268[4] = 1; - out_5950976016940954268[5] = 0; - out_5950976016940954268[6] = 0; - out_5950976016940954268[7] = 0; - out_5950976016940954268[8] = 0; - out_5950976016940954268[9] = 0; - out_5950976016940954268[10] = 0; - out_5950976016940954268[11] = 0; - out_5950976016940954268[12] = 0; - out_5950976016940954268[13] = 0; - out_5950976016940954268[14] = 1; - out_5950976016940954268[15] = 0; - out_5950976016940954268[16] = 0; - out_5950976016940954268[17] = 0; +void H_24(double *state, double *unused, double *out_5414189356014908953) { + out_5414189356014908953[0] = 0; + out_5414189356014908953[1] = 0; + out_5414189356014908953[2] = 0; + out_5414189356014908953[3] = 0; + out_5414189356014908953[4] = 1; + out_5414189356014908953[5] = 0; + out_5414189356014908953[6] = 0; + out_5414189356014908953[7] = 0; + out_5414189356014908953[8] = 0; + out_5414189356014908953[9] = 0; + out_5414189356014908953[10] = 0; + out_5414189356014908953[11] = 0; + out_5414189356014908953[12] = 0; + out_5414189356014908953[13] = 0; + out_5414189356014908953[14] = 1; + out_5414189356014908953[15] = 0; + out_5414189356014908953[16] = 0; + out_5414189356014908953[17] = 0; } -void h_30(double *state, double *unused, double *out_3908301040633649848) { - out_3908301040633649848[0] = state[4]; +void h_30(double *state, double *unused, double *out_619654518709855674) { + out_619654518709855674[0] = state[4]; } -void H_30(double *state, double *unused, double *out_6292094551841052922) { - out_6292094551841052922[0] = 0; - out_6292094551841052922[1] = 0; - out_6292094551841052922[2] = 0; - out_6292094551841052922[3] = 0; - out_6292094551841052922[4] = 1; - out_6292094551841052922[5] = 0; - out_6292094551841052922[6] = 0; - out_6292094551841052922[7] = 0; - out_6292094551841052922[8] = 0; +void H_30(double *state, double *unused, double *out_6249010312443216836) { + out_6249010312443216836[0] = 0; + out_6249010312443216836[1] = 0; + out_6249010312443216836[2] = 0; + out_6249010312443216836[3] = 0; + out_6249010312443216836[4] = 1; + out_6249010312443216836[5] = 0; + out_6249010312443216836[6] = 0; + out_6249010312443216836[7] = 0; + out_6249010312443216836[8] = 0; } -void h_26(double *state, double *unused, double *out_8870681038314943080) { - out_8870681038314943080[0] = state[7]; +void h_26(double *state, double *unused, double *out_6497940006268570762) { + out_6497940006268570762[0] = state[7]; } -void H_26(double *state, double *unused, double *out_32258274459748071) { - out_32258274459748071[0] = 0; - out_32258274459748071[1] = 0; - out_32258274459748071[2] = 0; - out_32258274459748071[3] = 0; - out_32258274459748071[4] = 0; - out_32258274459748071[5] = 0; - out_32258274459748071[6] = 0; - out_32258274459748071[7] = 1; - out_32258274459748071[8] = 0; +void H_26(double *state, double *unused, double *out_2636845940712400682) { + out_2636845940712400682[0] = 0; + out_2636845940712400682[1] = 0; + out_2636845940712400682[2] = 0; + out_2636845940712400682[3] = 0; + out_2636845940712400682[4] = 0; + out_2636845940712400682[5] = 0; + out_2636845940712400682[6] = 0; + out_2636845940712400682[7] = 1; + out_2636845940712400682[8] = 0; } -void h_27(double *state, double *unused, double *out_8988688432248319824) { - out_8988688432248319824[0] = state[3]; +void h_27(double *state, double *unused, double *out_7293890098408210709) { + out_7293890098408210709[0] = state[3]; } -void H_27(double *state, double *unused, double *out_1469659334390139314) { - out_1469659334390139314[0] = 0; - out_1469659334390139314[1] = 0; - out_1469659334390139314[2] = 0; - out_1469659334390139314[3] = 1; - out_1469659334390139314[4] = 0; - out_1469659334390139314[5] = 0; - out_1469659334390139314[6] = 0; - out_1469659334390139314[7] = 0; - out_1469659334390139314[8] = 0; +void H_27(double *state, double *unused, double *out_4074247000642791925) { + out_4074247000642791925[0] = 0; + out_4074247000642791925[1] = 0; + out_4074247000642791925[2] = 0; + out_4074247000642791925[3] = 1; + out_4074247000642791925[4] = 0; + out_4074247000642791925[5] = 0; + out_4074247000642791925[6] = 0; + out_4074247000642791925[7] = 0; + out_4074247000642791925[8] = 0; } -void h_29(double *state, double *unused, double *out_149028852435531358) { - out_149028852435531358[0] = state[1]; +void h_29(double *state, double *unused, double *out_3432206621979709057) { + out_3432206621979709057[0] = state[1]; } -void H_29(double *state, double *unused, double *out_243703392479411719) { - out_243703392479411719[0] = 0; - out_243703392479411719[1] = 1; - out_243703392479411719[2] = 0; - out_243703392479411719[3] = 0; - out_243703392479411719[4] = 0; - out_243703392479411719[5] = 0; - out_243703392479411719[6] = 0; - out_243703392479411719[7] = 0; - out_243703392479411719[8] = 0; +void H_29(double *state, double *unused, double *out_2360884273773240892) { + out_2360884273773240892[0] = 0; + out_2360884273773240892[1] = 1; + out_2360884273773240892[2] = 0; + out_2360884273773240892[3] = 0; + out_2360884273773240892[4] = 0; + out_2360884273773240892[5] = 0; + out_2360884273773240892[6] = 0; + out_2360884273773240892[7] = 0; + out_2360884273773240892[8] = 0; } -void h_28(double *state, double *unused, double *out_4456382373800386147) { - out_4456382373800386147[0] = state[0]; +void h_28(double *state, double *unused, double *out_5622082467174375208) { + out_5622082467174375208[0] = state[0]; } -void H_28(double *state, double *unused, double *out_1719926879085914532) { - out_1719926879085914532[0] = 1; - out_1719926879085914532[1] = 0; - out_1719926879085914532[2] = 0; - out_1719926879085914532[3] = 0; - out_1719926879085914532[4] = 0; - out_1719926879085914532[5] = 0; - out_1719926879085914532[6] = 0; - out_1719926879085914532[7] = 0; - out_1719926879085914532[8] = 0; +void H_28(double *state, double *unused, double *out_2721514743296289682) { + out_2721514743296289682[0] = 1; + out_2721514743296289682[1] = 0; + out_2721514743296289682[2] = 0; + out_2721514743296289682[3] = 0; + out_2721514743296289682[4] = 0; + out_2721514743296289682[5] = 0; + out_2721514743296289682[6] = 0; + out_2721514743296289682[7] = 0; + out_2721514743296289682[8] = 0; } -void h_31(double *state, double *unused, double *out_7610421138374421687) { - out_7610421138374421687[0] = state[8]; +void h_31(double *state, double *unused, double *out_6998090913890161796) { + out_6998090913890161796[0] = state[8]; } -void H_31(double *state, double *unused, double *out_3804407555210764723) { - out_3804407555210764723[0] = 0; - out_3804407555210764723[1] = 0; - out_3804407555210764723[2] = 0; - out_3804407555210764723[3] = 0; - out_3804407555210764723[4] = 0; - out_3804407555210764723[5] = 0; - out_3804407555210764723[6] = 0; - out_3804407555210764723[7] = 0; - out_3804407555210764723[8] = 1; +void H_31(double *state, double *unused, double *out_6408995221463417334) { + out_6408995221463417334[0] = 0; + out_6408995221463417334[1] = 0; + out_6408995221463417334[2] = 0; + out_6408995221463417334[3] = 0; + out_6408995221463417334[4] = 0; + out_6408995221463417334[5] = 0; + out_6408995221463417334[6] = 0; + out_6408995221463417334[7] = 0; + out_6408995221463417334[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_8137803062968279058) { - err_fun(nom_x, delta_x, out_8137803062968279058); +void car_err_fun(double *nom_x, double *delta_x, double *out_1038389500972116158) { + err_fun(nom_x, delta_x, out_1038389500972116158); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4803320297930984943) { - inv_err_fun(nom_x, true_x, out_4803320297930984943); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_6418752408535262595) { + inv_err_fun(nom_x, true_x, out_6418752408535262595); } -void car_H_mod_fun(double *state, double *out_250780943344312618) { - H_mod_fun(state, out_250780943344312618); +void car_H_mod_fun(double *state, double *out_2855368609596965229) { + H_mod_fun(state, out_2855368609596965229); } -void car_f_fun(double *state, double dt, double *out_5023554550498675071) { - f_fun(state, dt, out_5023554550498675071); +void car_f_fun(double *state, double dt, double *out_1397092929687677159) { + f_fun(state, dt, out_1397092929687677159); } -void car_F_fun(double *state, double dt, double *out_3517018389212170475) { - F_fun(state, dt, out_3517018389212170475); +void car_F_fun(double *state, double dt, double *out_7780451684963738443) { + F_fun(state, dt, out_7780451684963738443); } -void car_h_25(double *state, double *unused, double *out_8788151648350399353) { - h_25(state, unused, out_8788151648350399353); +void car_h_25(double *state, double *unused, double *out_894848580994361563) { + h_25(state, unused, out_894848580994361563); } -void car_H_25(double *state, double *unused, double *out_3773761593333804295) { - H_25(state, unused, out_3773761593333804295); +void car_H_25(double *state, double *unused, double *out_6378349259586456906) { + H_25(state, unused, out_6378349259586456906); } -void car_h_24(double *state, double *unused, double *out_4617115616065835450) { - h_24(state, unused, out_4617115616065835450); +void car_h_24(double *state, double *unused, double *out_6711296683837743925) { + h_24(state, unused, out_6711296683837743925); } -void car_H_24(double *state, double *unused, double *out_5950976016940954268) { - H_24(state, unused, out_5950976016940954268); +void car_H_24(double *state, double *unused, double *out_5414189356014908953) { + H_24(state, unused, out_5414189356014908953); } -void car_h_30(double *state, double *unused, double *out_3908301040633649848) { - h_30(state, unused, out_3908301040633649848); +void car_h_30(double *state, double *unused, double *out_619654518709855674) { + h_30(state, unused, out_619654518709855674); } -void car_H_30(double *state, double *unused, double *out_6292094551841052922) { - H_30(state, unused, out_6292094551841052922); +void car_H_30(double *state, double *unused, double *out_6249010312443216836) { + H_30(state, unused, out_6249010312443216836); } -void car_h_26(double *state, double *unused, double *out_8870681038314943080) { - h_26(state, unused, out_8870681038314943080); +void car_h_26(double *state, double *unused, double *out_6497940006268570762) { + h_26(state, unused, out_6497940006268570762); } -void car_H_26(double *state, double *unused, double *out_32258274459748071) { - H_26(state, unused, out_32258274459748071); +void car_H_26(double *state, double *unused, double *out_2636845940712400682) { + H_26(state, unused, out_2636845940712400682); } -void car_h_27(double *state, double *unused, double *out_8988688432248319824) { - h_27(state, unused, out_8988688432248319824); +void car_h_27(double *state, double *unused, double *out_7293890098408210709) { + h_27(state, unused, out_7293890098408210709); } -void car_H_27(double *state, double *unused, double *out_1469659334390139314) { - H_27(state, unused, out_1469659334390139314); +void car_H_27(double *state, double *unused, double *out_4074247000642791925) { + H_27(state, unused, out_4074247000642791925); } -void car_h_29(double *state, double *unused, double *out_149028852435531358) { - h_29(state, unused, out_149028852435531358); +void car_h_29(double *state, double *unused, double *out_3432206621979709057) { + h_29(state, unused, out_3432206621979709057); } -void car_H_29(double *state, double *unused, double *out_243703392479411719) { - H_29(state, unused, out_243703392479411719); +void car_H_29(double *state, double *unused, double *out_2360884273773240892) { + H_29(state, unused, out_2360884273773240892); } -void car_h_28(double *state, double *unused, double *out_4456382373800386147) { - h_28(state, unused, out_4456382373800386147); +void car_h_28(double *state, double *unused, double *out_5622082467174375208) { + h_28(state, unused, out_5622082467174375208); } -void car_H_28(double *state, double *unused, double *out_1719926879085914532) { - H_28(state, unused, out_1719926879085914532); +void car_H_28(double *state, double *unused, double *out_2721514743296289682) { + H_28(state, unused, out_2721514743296289682); } -void car_h_31(double *state, double *unused, double *out_7610421138374421687) { - h_31(state, unused, out_7610421138374421687); +void car_h_31(double *state, double *unused, double *out_6998090913890161796) { + h_31(state, unused, out_6998090913890161796); } -void car_H_31(double *state, double *unused, double *out_3804407555210764723) { - H_31(state, unused, out_3804407555210764723); +void car_H_31(double *state, double *unused, double *out_6408995221463417334) { + H_31(state, unused, out_6408995221463417334); } 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 a06ca6eb2..1698991d0 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_8137803062968279058); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4803320297930984943); -void car_H_mod_fun(double *state, double *out_250780943344312618); -void car_f_fun(double *state, double dt, double *out_5023554550498675071); -void car_F_fun(double *state, double dt, double *out_3517018389212170475); -void car_h_25(double *state, double *unused, double *out_8788151648350399353); -void car_H_25(double *state, double *unused, double *out_3773761593333804295); -void car_h_24(double *state, double *unused, double *out_4617115616065835450); -void car_H_24(double *state, double *unused, double *out_5950976016940954268); -void car_h_30(double *state, double *unused, double *out_3908301040633649848); -void car_H_30(double *state, double *unused, double *out_6292094551841052922); -void car_h_26(double *state, double *unused, double *out_8870681038314943080); -void car_H_26(double *state, double *unused, double *out_32258274459748071); -void car_h_27(double *state, double *unused, double *out_8988688432248319824); -void car_H_27(double *state, double *unused, double *out_1469659334390139314); -void car_h_29(double *state, double *unused, double *out_149028852435531358); -void car_H_29(double *state, double *unused, double *out_243703392479411719); -void car_h_28(double *state, double *unused, double *out_4456382373800386147); -void car_H_28(double *state, double *unused, double *out_1719926879085914532); -void car_h_31(double *state, double *unused, double *out_7610421138374421687); -void car_H_31(double *state, double *unused, double *out_3804407555210764723); +void car_err_fun(double *nom_x, double *delta_x, double *out_1038389500972116158); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_6418752408535262595); +void car_H_mod_fun(double *state, double *out_2855368609596965229); +void car_f_fun(double *state, double dt, double *out_1397092929687677159); +void car_F_fun(double *state, double dt, double *out_7780451684963738443); +void car_h_25(double *state, double *unused, double *out_894848580994361563); +void car_H_25(double *state, double *unused, double *out_6378349259586456906); +void car_h_24(double *state, double *unused, double *out_6711296683837743925); +void car_H_24(double *state, double *unused, double *out_5414189356014908953); +void car_h_30(double *state, double *unused, double *out_619654518709855674); +void car_H_30(double *state, double *unused, double *out_6249010312443216836); +void car_h_26(double *state, double *unused, double *out_6497940006268570762); +void car_H_26(double *state, double *unused, double *out_2636845940712400682); +void car_h_27(double *state, double *unused, double *out_7293890098408210709); +void car_H_27(double *state, double *unused, double *out_4074247000642791925); +void car_h_29(double *state, double *unused, double *out_3432206621979709057); +void car_H_29(double *state, double *unused, double *out_2360884273773240892); +void car_h_28(double *state, double *unused, double *out_5622082467174375208); +void car_H_28(double *state, double *unused, double *out_2721514743296289682); +void car_h_31(double *state, double *unused, double *out_6998090913890161796); +void car_H_31(double *state, double *unused, double *out_6408995221463417334); 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 652de8634..f45ad7cde 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_6269171790765846050) { - out_6269171790765846050[0] = delta_x[0] + nom_x[0]; - out_6269171790765846050[1] = delta_x[1] + nom_x[1]; - out_6269171790765846050[2] = delta_x[2] + nom_x[2]; - out_6269171790765846050[3] = delta_x[3] + nom_x[3]; - out_6269171790765846050[4] = delta_x[4] + nom_x[4]; - out_6269171790765846050[5] = delta_x[5] + nom_x[5]; - out_6269171790765846050[6] = delta_x[6] + nom_x[6]; - out_6269171790765846050[7] = delta_x[7] + nom_x[7]; - out_6269171790765846050[8] = delta_x[8] + nom_x[8]; - out_6269171790765846050[9] = delta_x[9] + nom_x[9]; - out_6269171790765846050[10] = delta_x[10] + nom_x[10]; - out_6269171790765846050[11] = delta_x[11] + nom_x[11]; - out_6269171790765846050[12] = delta_x[12] + nom_x[12]; - out_6269171790765846050[13] = delta_x[13] + nom_x[13]; - out_6269171790765846050[14] = delta_x[14] + nom_x[14]; - out_6269171790765846050[15] = delta_x[15] + nom_x[15]; - out_6269171790765846050[16] = delta_x[16] + nom_x[16]; - out_6269171790765846050[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_6395110133292861521) { + out_6395110133292861521[0] = delta_x[0] + nom_x[0]; + out_6395110133292861521[1] = delta_x[1] + nom_x[1]; + out_6395110133292861521[2] = delta_x[2] + nom_x[2]; + out_6395110133292861521[3] = delta_x[3] + nom_x[3]; + out_6395110133292861521[4] = delta_x[4] + nom_x[4]; + out_6395110133292861521[5] = delta_x[5] + nom_x[5]; + out_6395110133292861521[6] = delta_x[6] + nom_x[6]; + out_6395110133292861521[7] = delta_x[7] + nom_x[7]; + out_6395110133292861521[8] = delta_x[8] + nom_x[8]; + out_6395110133292861521[9] = delta_x[9] + nom_x[9]; + out_6395110133292861521[10] = delta_x[10] + nom_x[10]; + out_6395110133292861521[11] = delta_x[11] + nom_x[11]; + out_6395110133292861521[12] = delta_x[12] + nom_x[12]; + out_6395110133292861521[13] = delta_x[13] + nom_x[13]; + out_6395110133292861521[14] = delta_x[14] + nom_x[14]; + out_6395110133292861521[15] = delta_x[15] + nom_x[15]; + out_6395110133292861521[16] = delta_x[16] + nom_x[16]; + out_6395110133292861521[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1497934558633100347) { - out_1497934558633100347[0] = -nom_x[0] + true_x[0]; - out_1497934558633100347[1] = -nom_x[1] + true_x[1]; - out_1497934558633100347[2] = -nom_x[2] + true_x[2]; - out_1497934558633100347[3] = -nom_x[3] + true_x[3]; - out_1497934558633100347[4] = -nom_x[4] + true_x[4]; - out_1497934558633100347[5] = -nom_x[5] + true_x[5]; - out_1497934558633100347[6] = -nom_x[6] + true_x[6]; - out_1497934558633100347[7] = -nom_x[7] + true_x[7]; - out_1497934558633100347[8] = -nom_x[8] + true_x[8]; - out_1497934558633100347[9] = -nom_x[9] + true_x[9]; - out_1497934558633100347[10] = -nom_x[10] + true_x[10]; - out_1497934558633100347[11] = -nom_x[11] + true_x[11]; - out_1497934558633100347[12] = -nom_x[12] + true_x[12]; - out_1497934558633100347[13] = -nom_x[13] + true_x[13]; - out_1497934558633100347[14] = -nom_x[14] + true_x[14]; - out_1497934558633100347[15] = -nom_x[15] + true_x[15]; - out_1497934558633100347[16] = -nom_x[16] + true_x[16]; - out_1497934558633100347[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5574902989131587335) { + out_5574902989131587335[0] = -nom_x[0] + true_x[0]; + out_5574902989131587335[1] = -nom_x[1] + true_x[1]; + out_5574902989131587335[2] = -nom_x[2] + true_x[2]; + out_5574902989131587335[3] = -nom_x[3] + true_x[3]; + out_5574902989131587335[4] = -nom_x[4] + true_x[4]; + out_5574902989131587335[5] = -nom_x[5] + true_x[5]; + out_5574902989131587335[6] = -nom_x[6] + true_x[6]; + out_5574902989131587335[7] = -nom_x[7] + true_x[7]; + out_5574902989131587335[8] = -nom_x[8] + true_x[8]; + out_5574902989131587335[9] = -nom_x[9] + true_x[9]; + out_5574902989131587335[10] = -nom_x[10] + true_x[10]; + out_5574902989131587335[11] = -nom_x[11] + true_x[11]; + out_5574902989131587335[12] = -nom_x[12] + true_x[12]; + out_5574902989131587335[13] = -nom_x[13] + true_x[13]; + out_5574902989131587335[14] = -nom_x[14] + true_x[14]; + out_5574902989131587335[15] = -nom_x[15] + true_x[15]; + out_5574902989131587335[16] = -nom_x[16] + true_x[16]; + out_5574902989131587335[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_7862392065184069712) { - out_7862392065184069712[0] = 1.0; - out_7862392065184069712[1] = 0.0; - out_7862392065184069712[2] = 0.0; - out_7862392065184069712[3] = 0.0; - out_7862392065184069712[4] = 0.0; - out_7862392065184069712[5] = 0.0; - out_7862392065184069712[6] = 0.0; - out_7862392065184069712[7] = 0.0; - out_7862392065184069712[8] = 0.0; - out_7862392065184069712[9] = 0.0; - out_7862392065184069712[10] = 0.0; - out_7862392065184069712[11] = 0.0; - out_7862392065184069712[12] = 0.0; - out_7862392065184069712[13] = 0.0; - out_7862392065184069712[14] = 0.0; - out_7862392065184069712[15] = 0.0; - out_7862392065184069712[16] = 0.0; - out_7862392065184069712[17] = 0.0; - out_7862392065184069712[18] = 0.0; - out_7862392065184069712[19] = 1.0; - out_7862392065184069712[20] = 0.0; - out_7862392065184069712[21] = 0.0; - out_7862392065184069712[22] = 0.0; - out_7862392065184069712[23] = 0.0; - out_7862392065184069712[24] = 0.0; - out_7862392065184069712[25] = 0.0; - out_7862392065184069712[26] = 0.0; - out_7862392065184069712[27] = 0.0; - out_7862392065184069712[28] = 0.0; - out_7862392065184069712[29] = 0.0; - out_7862392065184069712[30] = 0.0; - out_7862392065184069712[31] = 0.0; - out_7862392065184069712[32] = 0.0; - out_7862392065184069712[33] = 0.0; - out_7862392065184069712[34] = 0.0; - out_7862392065184069712[35] = 0.0; - out_7862392065184069712[36] = 0.0; - out_7862392065184069712[37] = 0.0; - out_7862392065184069712[38] = 1.0; - out_7862392065184069712[39] = 0.0; - out_7862392065184069712[40] = 0.0; - out_7862392065184069712[41] = 0.0; - out_7862392065184069712[42] = 0.0; - out_7862392065184069712[43] = 0.0; - out_7862392065184069712[44] = 0.0; - out_7862392065184069712[45] = 0.0; - out_7862392065184069712[46] = 0.0; - out_7862392065184069712[47] = 0.0; - out_7862392065184069712[48] = 0.0; - out_7862392065184069712[49] = 0.0; - out_7862392065184069712[50] = 0.0; - out_7862392065184069712[51] = 0.0; - out_7862392065184069712[52] = 0.0; - out_7862392065184069712[53] = 0.0; - out_7862392065184069712[54] = 0.0; - out_7862392065184069712[55] = 0.0; - out_7862392065184069712[56] = 0.0; - out_7862392065184069712[57] = 1.0; - out_7862392065184069712[58] = 0.0; - out_7862392065184069712[59] = 0.0; - out_7862392065184069712[60] = 0.0; - out_7862392065184069712[61] = 0.0; - out_7862392065184069712[62] = 0.0; - out_7862392065184069712[63] = 0.0; - out_7862392065184069712[64] = 0.0; - out_7862392065184069712[65] = 0.0; - out_7862392065184069712[66] = 0.0; - out_7862392065184069712[67] = 0.0; - out_7862392065184069712[68] = 0.0; - out_7862392065184069712[69] = 0.0; - out_7862392065184069712[70] = 0.0; - out_7862392065184069712[71] = 0.0; - out_7862392065184069712[72] = 0.0; - out_7862392065184069712[73] = 0.0; - out_7862392065184069712[74] = 0.0; - out_7862392065184069712[75] = 0.0; - out_7862392065184069712[76] = 1.0; - out_7862392065184069712[77] = 0.0; - out_7862392065184069712[78] = 0.0; - out_7862392065184069712[79] = 0.0; - out_7862392065184069712[80] = 0.0; - out_7862392065184069712[81] = 0.0; - out_7862392065184069712[82] = 0.0; - out_7862392065184069712[83] = 0.0; - out_7862392065184069712[84] = 0.0; - out_7862392065184069712[85] = 0.0; - out_7862392065184069712[86] = 0.0; - out_7862392065184069712[87] = 0.0; - out_7862392065184069712[88] = 0.0; - out_7862392065184069712[89] = 0.0; - out_7862392065184069712[90] = 0.0; - out_7862392065184069712[91] = 0.0; - out_7862392065184069712[92] = 0.0; - out_7862392065184069712[93] = 0.0; - out_7862392065184069712[94] = 0.0; - out_7862392065184069712[95] = 1.0; - out_7862392065184069712[96] = 0.0; - out_7862392065184069712[97] = 0.0; - out_7862392065184069712[98] = 0.0; - out_7862392065184069712[99] = 0.0; - out_7862392065184069712[100] = 0.0; - out_7862392065184069712[101] = 0.0; - out_7862392065184069712[102] = 0.0; - out_7862392065184069712[103] = 0.0; - out_7862392065184069712[104] = 0.0; - out_7862392065184069712[105] = 0.0; - out_7862392065184069712[106] = 0.0; - out_7862392065184069712[107] = 0.0; - out_7862392065184069712[108] = 0.0; - out_7862392065184069712[109] = 0.0; - out_7862392065184069712[110] = 0.0; - out_7862392065184069712[111] = 0.0; - out_7862392065184069712[112] = 0.0; - out_7862392065184069712[113] = 0.0; - out_7862392065184069712[114] = 1.0; - out_7862392065184069712[115] = 0.0; - out_7862392065184069712[116] = 0.0; - out_7862392065184069712[117] = 0.0; - out_7862392065184069712[118] = 0.0; - out_7862392065184069712[119] = 0.0; - out_7862392065184069712[120] = 0.0; - out_7862392065184069712[121] = 0.0; - out_7862392065184069712[122] = 0.0; - out_7862392065184069712[123] = 0.0; - out_7862392065184069712[124] = 0.0; - out_7862392065184069712[125] = 0.0; - out_7862392065184069712[126] = 0.0; - out_7862392065184069712[127] = 0.0; - out_7862392065184069712[128] = 0.0; - out_7862392065184069712[129] = 0.0; - out_7862392065184069712[130] = 0.0; - out_7862392065184069712[131] = 0.0; - out_7862392065184069712[132] = 0.0; - out_7862392065184069712[133] = 1.0; - out_7862392065184069712[134] = 0.0; - out_7862392065184069712[135] = 0.0; - out_7862392065184069712[136] = 0.0; - out_7862392065184069712[137] = 0.0; - out_7862392065184069712[138] = 0.0; - out_7862392065184069712[139] = 0.0; - out_7862392065184069712[140] = 0.0; - out_7862392065184069712[141] = 0.0; - out_7862392065184069712[142] = 0.0; - out_7862392065184069712[143] = 0.0; - out_7862392065184069712[144] = 0.0; - out_7862392065184069712[145] = 0.0; - out_7862392065184069712[146] = 0.0; - out_7862392065184069712[147] = 0.0; - out_7862392065184069712[148] = 0.0; - out_7862392065184069712[149] = 0.0; - out_7862392065184069712[150] = 0.0; - out_7862392065184069712[151] = 0.0; - out_7862392065184069712[152] = 1.0; - out_7862392065184069712[153] = 0.0; - out_7862392065184069712[154] = 0.0; - out_7862392065184069712[155] = 0.0; - out_7862392065184069712[156] = 0.0; - out_7862392065184069712[157] = 0.0; - out_7862392065184069712[158] = 0.0; - out_7862392065184069712[159] = 0.0; - out_7862392065184069712[160] = 0.0; - out_7862392065184069712[161] = 0.0; - out_7862392065184069712[162] = 0.0; - out_7862392065184069712[163] = 0.0; - out_7862392065184069712[164] = 0.0; - out_7862392065184069712[165] = 0.0; - out_7862392065184069712[166] = 0.0; - out_7862392065184069712[167] = 0.0; - out_7862392065184069712[168] = 0.0; - out_7862392065184069712[169] = 0.0; - out_7862392065184069712[170] = 0.0; - out_7862392065184069712[171] = 1.0; - out_7862392065184069712[172] = 0.0; - out_7862392065184069712[173] = 0.0; - out_7862392065184069712[174] = 0.0; - out_7862392065184069712[175] = 0.0; - out_7862392065184069712[176] = 0.0; - out_7862392065184069712[177] = 0.0; - out_7862392065184069712[178] = 0.0; - out_7862392065184069712[179] = 0.0; - out_7862392065184069712[180] = 0.0; - out_7862392065184069712[181] = 0.0; - out_7862392065184069712[182] = 0.0; - out_7862392065184069712[183] = 0.0; - out_7862392065184069712[184] = 0.0; - out_7862392065184069712[185] = 0.0; - out_7862392065184069712[186] = 0.0; - out_7862392065184069712[187] = 0.0; - out_7862392065184069712[188] = 0.0; - out_7862392065184069712[189] = 0.0; - out_7862392065184069712[190] = 1.0; - out_7862392065184069712[191] = 0.0; - out_7862392065184069712[192] = 0.0; - out_7862392065184069712[193] = 0.0; - out_7862392065184069712[194] = 0.0; - out_7862392065184069712[195] = 0.0; - out_7862392065184069712[196] = 0.0; - out_7862392065184069712[197] = 0.0; - out_7862392065184069712[198] = 0.0; - out_7862392065184069712[199] = 0.0; - out_7862392065184069712[200] = 0.0; - out_7862392065184069712[201] = 0.0; - out_7862392065184069712[202] = 0.0; - out_7862392065184069712[203] = 0.0; - out_7862392065184069712[204] = 0.0; - out_7862392065184069712[205] = 0.0; - out_7862392065184069712[206] = 0.0; - out_7862392065184069712[207] = 0.0; - out_7862392065184069712[208] = 0.0; - out_7862392065184069712[209] = 1.0; - out_7862392065184069712[210] = 0.0; - out_7862392065184069712[211] = 0.0; - out_7862392065184069712[212] = 0.0; - out_7862392065184069712[213] = 0.0; - out_7862392065184069712[214] = 0.0; - out_7862392065184069712[215] = 0.0; - out_7862392065184069712[216] = 0.0; - out_7862392065184069712[217] = 0.0; - out_7862392065184069712[218] = 0.0; - out_7862392065184069712[219] = 0.0; - out_7862392065184069712[220] = 0.0; - out_7862392065184069712[221] = 0.0; - out_7862392065184069712[222] = 0.0; - out_7862392065184069712[223] = 0.0; - out_7862392065184069712[224] = 0.0; - out_7862392065184069712[225] = 0.0; - out_7862392065184069712[226] = 0.0; - out_7862392065184069712[227] = 0.0; - out_7862392065184069712[228] = 1.0; - out_7862392065184069712[229] = 0.0; - out_7862392065184069712[230] = 0.0; - out_7862392065184069712[231] = 0.0; - out_7862392065184069712[232] = 0.0; - out_7862392065184069712[233] = 0.0; - out_7862392065184069712[234] = 0.0; - out_7862392065184069712[235] = 0.0; - out_7862392065184069712[236] = 0.0; - out_7862392065184069712[237] = 0.0; - out_7862392065184069712[238] = 0.0; - out_7862392065184069712[239] = 0.0; - out_7862392065184069712[240] = 0.0; - out_7862392065184069712[241] = 0.0; - out_7862392065184069712[242] = 0.0; - out_7862392065184069712[243] = 0.0; - out_7862392065184069712[244] = 0.0; - out_7862392065184069712[245] = 0.0; - out_7862392065184069712[246] = 0.0; - out_7862392065184069712[247] = 1.0; - out_7862392065184069712[248] = 0.0; - out_7862392065184069712[249] = 0.0; - out_7862392065184069712[250] = 0.0; - out_7862392065184069712[251] = 0.0; - out_7862392065184069712[252] = 0.0; - out_7862392065184069712[253] = 0.0; - out_7862392065184069712[254] = 0.0; - out_7862392065184069712[255] = 0.0; - out_7862392065184069712[256] = 0.0; - out_7862392065184069712[257] = 0.0; - out_7862392065184069712[258] = 0.0; - out_7862392065184069712[259] = 0.0; - out_7862392065184069712[260] = 0.0; - out_7862392065184069712[261] = 0.0; - out_7862392065184069712[262] = 0.0; - out_7862392065184069712[263] = 0.0; - out_7862392065184069712[264] = 0.0; - out_7862392065184069712[265] = 0.0; - out_7862392065184069712[266] = 1.0; - out_7862392065184069712[267] = 0.0; - out_7862392065184069712[268] = 0.0; - out_7862392065184069712[269] = 0.0; - out_7862392065184069712[270] = 0.0; - out_7862392065184069712[271] = 0.0; - out_7862392065184069712[272] = 0.0; - out_7862392065184069712[273] = 0.0; - out_7862392065184069712[274] = 0.0; - out_7862392065184069712[275] = 0.0; - out_7862392065184069712[276] = 0.0; - out_7862392065184069712[277] = 0.0; - out_7862392065184069712[278] = 0.0; - out_7862392065184069712[279] = 0.0; - out_7862392065184069712[280] = 0.0; - out_7862392065184069712[281] = 0.0; - out_7862392065184069712[282] = 0.0; - out_7862392065184069712[283] = 0.0; - out_7862392065184069712[284] = 0.0; - out_7862392065184069712[285] = 1.0; - out_7862392065184069712[286] = 0.0; - out_7862392065184069712[287] = 0.0; - out_7862392065184069712[288] = 0.0; - out_7862392065184069712[289] = 0.0; - out_7862392065184069712[290] = 0.0; - out_7862392065184069712[291] = 0.0; - out_7862392065184069712[292] = 0.0; - out_7862392065184069712[293] = 0.0; - out_7862392065184069712[294] = 0.0; - out_7862392065184069712[295] = 0.0; - out_7862392065184069712[296] = 0.0; - out_7862392065184069712[297] = 0.0; - out_7862392065184069712[298] = 0.0; - out_7862392065184069712[299] = 0.0; - out_7862392065184069712[300] = 0.0; - out_7862392065184069712[301] = 0.0; - out_7862392065184069712[302] = 0.0; - out_7862392065184069712[303] = 0.0; - out_7862392065184069712[304] = 1.0; - out_7862392065184069712[305] = 0.0; - out_7862392065184069712[306] = 0.0; - out_7862392065184069712[307] = 0.0; - out_7862392065184069712[308] = 0.0; - out_7862392065184069712[309] = 0.0; - out_7862392065184069712[310] = 0.0; - out_7862392065184069712[311] = 0.0; - out_7862392065184069712[312] = 0.0; - out_7862392065184069712[313] = 0.0; - out_7862392065184069712[314] = 0.0; - out_7862392065184069712[315] = 0.0; - out_7862392065184069712[316] = 0.0; - out_7862392065184069712[317] = 0.0; - out_7862392065184069712[318] = 0.0; - out_7862392065184069712[319] = 0.0; - out_7862392065184069712[320] = 0.0; - out_7862392065184069712[321] = 0.0; - out_7862392065184069712[322] = 0.0; - out_7862392065184069712[323] = 1.0; +void H_mod_fun(double *state, double *out_6860086072787592800) { + out_6860086072787592800[0] = 1.0; + out_6860086072787592800[1] = 0.0; + out_6860086072787592800[2] = 0.0; + out_6860086072787592800[3] = 0.0; + out_6860086072787592800[4] = 0.0; + out_6860086072787592800[5] = 0.0; + out_6860086072787592800[6] = 0.0; + out_6860086072787592800[7] = 0.0; + out_6860086072787592800[8] = 0.0; + out_6860086072787592800[9] = 0.0; + out_6860086072787592800[10] = 0.0; + out_6860086072787592800[11] = 0.0; + out_6860086072787592800[12] = 0.0; + out_6860086072787592800[13] = 0.0; + out_6860086072787592800[14] = 0.0; + out_6860086072787592800[15] = 0.0; + out_6860086072787592800[16] = 0.0; + out_6860086072787592800[17] = 0.0; + out_6860086072787592800[18] = 0.0; + out_6860086072787592800[19] = 1.0; + out_6860086072787592800[20] = 0.0; + out_6860086072787592800[21] = 0.0; + out_6860086072787592800[22] = 0.0; + out_6860086072787592800[23] = 0.0; + out_6860086072787592800[24] = 0.0; + out_6860086072787592800[25] = 0.0; + out_6860086072787592800[26] = 0.0; + out_6860086072787592800[27] = 0.0; + out_6860086072787592800[28] = 0.0; + out_6860086072787592800[29] = 0.0; + out_6860086072787592800[30] = 0.0; + out_6860086072787592800[31] = 0.0; + out_6860086072787592800[32] = 0.0; + out_6860086072787592800[33] = 0.0; + out_6860086072787592800[34] = 0.0; + out_6860086072787592800[35] = 0.0; + out_6860086072787592800[36] = 0.0; + out_6860086072787592800[37] = 0.0; + out_6860086072787592800[38] = 1.0; + out_6860086072787592800[39] = 0.0; + out_6860086072787592800[40] = 0.0; + out_6860086072787592800[41] = 0.0; + out_6860086072787592800[42] = 0.0; + out_6860086072787592800[43] = 0.0; + out_6860086072787592800[44] = 0.0; + out_6860086072787592800[45] = 0.0; + out_6860086072787592800[46] = 0.0; + out_6860086072787592800[47] = 0.0; + out_6860086072787592800[48] = 0.0; + out_6860086072787592800[49] = 0.0; + out_6860086072787592800[50] = 0.0; + out_6860086072787592800[51] = 0.0; + out_6860086072787592800[52] = 0.0; + out_6860086072787592800[53] = 0.0; + out_6860086072787592800[54] = 0.0; + out_6860086072787592800[55] = 0.0; + out_6860086072787592800[56] = 0.0; + out_6860086072787592800[57] = 1.0; + out_6860086072787592800[58] = 0.0; + out_6860086072787592800[59] = 0.0; + out_6860086072787592800[60] = 0.0; + out_6860086072787592800[61] = 0.0; + out_6860086072787592800[62] = 0.0; + out_6860086072787592800[63] = 0.0; + out_6860086072787592800[64] = 0.0; + out_6860086072787592800[65] = 0.0; + out_6860086072787592800[66] = 0.0; + out_6860086072787592800[67] = 0.0; + out_6860086072787592800[68] = 0.0; + out_6860086072787592800[69] = 0.0; + out_6860086072787592800[70] = 0.0; + out_6860086072787592800[71] = 0.0; + out_6860086072787592800[72] = 0.0; + out_6860086072787592800[73] = 0.0; + out_6860086072787592800[74] = 0.0; + out_6860086072787592800[75] = 0.0; + out_6860086072787592800[76] = 1.0; + out_6860086072787592800[77] = 0.0; + out_6860086072787592800[78] = 0.0; + out_6860086072787592800[79] = 0.0; + out_6860086072787592800[80] = 0.0; + out_6860086072787592800[81] = 0.0; + out_6860086072787592800[82] = 0.0; + out_6860086072787592800[83] = 0.0; + out_6860086072787592800[84] = 0.0; + out_6860086072787592800[85] = 0.0; + out_6860086072787592800[86] = 0.0; + out_6860086072787592800[87] = 0.0; + out_6860086072787592800[88] = 0.0; + out_6860086072787592800[89] = 0.0; + out_6860086072787592800[90] = 0.0; + out_6860086072787592800[91] = 0.0; + out_6860086072787592800[92] = 0.0; + out_6860086072787592800[93] = 0.0; + out_6860086072787592800[94] = 0.0; + out_6860086072787592800[95] = 1.0; + out_6860086072787592800[96] = 0.0; + out_6860086072787592800[97] = 0.0; + out_6860086072787592800[98] = 0.0; + out_6860086072787592800[99] = 0.0; + out_6860086072787592800[100] = 0.0; + out_6860086072787592800[101] = 0.0; + out_6860086072787592800[102] = 0.0; + out_6860086072787592800[103] = 0.0; + out_6860086072787592800[104] = 0.0; + out_6860086072787592800[105] = 0.0; + out_6860086072787592800[106] = 0.0; + out_6860086072787592800[107] = 0.0; + out_6860086072787592800[108] = 0.0; + out_6860086072787592800[109] = 0.0; + out_6860086072787592800[110] = 0.0; + out_6860086072787592800[111] = 0.0; + out_6860086072787592800[112] = 0.0; + out_6860086072787592800[113] = 0.0; + out_6860086072787592800[114] = 1.0; + out_6860086072787592800[115] = 0.0; + out_6860086072787592800[116] = 0.0; + out_6860086072787592800[117] = 0.0; + out_6860086072787592800[118] = 0.0; + out_6860086072787592800[119] = 0.0; + out_6860086072787592800[120] = 0.0; + out_6860086072787592800[121] = 0.0; + out_6860086072787592800[122] = 0.0; + out_6860086072787592800[123] = 0.0; + out_6860086072787592800[124] = 0.0; + out_6860086072787592800[125] = 0.0; + out_6860086072787592800[126] = 0.0; + out_6860086072787592800[127] = 0.0; + out_6860086072787592800[128] = 0.0; + out_6860086072787592800[129] = 0.0; + out_6860086072787592800[130] = 0.0; + out_6860086072787592800[131] = 0.0; + out_6860086072787592800[132] = 0.0; + out_6860086072787592800[133] = 1.0; + out_6860086072787592800[134] = 0.0; + out_6860086072787592800[135] = 0.0; + out_6860086072787592800[136] = 0.0; + out_6860086072787592800[137] = 0.0; + out_6860086072787592800[138] = 0.0; + out_6860086072787592800[139] = 0.0; + out_6860086072787592800[140] = 0.0; + out_6860086072787592800[141] = 0.0; + out_6860086072787592800[142] = 0.0; + out_6860086072787592800[143] = 0.0; + out_6860086072787592800[144] = 0.0; + out_6860086072787592800[145] = 0.0; + out_6860086072787592800[146] = 0.0; + out_6860086072787592800[147] = 0.0; + out_6860086072787592800[148] = 0.0; + out_6860086072787592800[149] = 0.0; + out_6860086072787592800[150] = 0.0; + out_6860086072787592800[151] = 0.0; + out_6860086072787592800[152] = 1.0; + out_6860086072787592800[153] = 0.0; + out_6860086072787592800[154] = 0.0; + out_6860086072787592800[155] = 0.0; + out_6860086072787592800[156] = 0.0; + out_6860086072787592800[157] = 0.0; + out_6860086072787592800[158] = 0.0; + out_6860086072787592800[159] = 0.0; + out_6860086072787592800[160] = 0.0; + out_6860086072787592800[161] = 0.0; + out_6860086072787592800[162] = 0.0; + out_6860086072787592800[163] = 0.0; + out_6860086072787592800[164] = 0.0; + out_6860086072787592800[165] = 0.0; + out_6860086072787592800[166] = 0.0; + out_6860086072787592800[167] = 0.0; + out_6860086072787592800[168] = 0.0; + out_6860086072787592800[169] = 0.0; + out_6860086072787592800[170] = 0.0; + out_6860086072787592800[171] = 1.0; + out_6860086072787592800[172] = 0.0; + out_6860086072787592800[173] = 0.0; + out_6860086072787592800[174] = 0.0; + out_6860086072787592800[175] = 0.0; + out_6860086072787592800[176] = 0.0; + out_6860086072787592800[177] = 0.0; + out_6860086072787592800[178] = 0.0; + out_6860086072787592800[179] = 0.0; + out_6860086072787592800[180] = 0.0; + out_6860086072787592800[181] = 0.0; + out_6860086072787592800[182] = 0.0; + out_6860086072787592800[183] = 0.0; + out_6860086072787592800[184] = 0.0; + out_6860086072787592800[185] = 0.0; + out_6860086072787592800[186] = 0.0; + out_6860086072787592800[187] = 0.0; + out_6860086072787592800[188] = 0.0; + out_6860086072787592800[189] = 0.0; + out_6860086072787592800[190] = 1.0; + out_6860086072787592800[191] = 0.0; + out_6860086072787592800[192] = 0.0; + out_6860086072787592800[193] = 0.0; + out_6860086072787592800[194] = 0.0; + out_6860086072787592800[195] = 0.0; + out_6860086072787592800[196] = 0.0; + out_6860086072787592800[197] = 0.0; + out_6860086072787592800[198] = 0.0; + out_6860086072787592800[199] = 0.0; + out_6860086072787592800[200] = 0.0; + out_6860086072787592800[201] = 0.0; + out_6860086072787592800[202] = 0.0; + out_6860086072787592800[203] = 0.0; + out_6860086072787592800[204] = 0.0; + out_6860086072787592800[205] = 0.0; + out_6860086072787592800[206] = 0.0; + out_6860086072787592800[207] = 0.0; + out_6860086072787592800[208] = 0.0; + out_6860086072787592800[209] = 1.0; + out_6860086072787592800[210] = 0.0; + out_6860086072787592800[211] = 0.0; + out_6860086072787592800[212] = 0.0; + out_6860086072787592800[213] = 0.0; + out_6860086072787592800[214] = 0.0; + out_6860086072787592800[215] = 0.0; + out_6860086072787592800[216] = 0.0; + out_6860086072787592800[217] = 0.0; + out_6860086072787592800[218] = 0.0; + out_6860086072787592800[219] = 0.0; + out_6860086072787592800[220] = 0.0; + out_6860086072787592800[221] = 0.0; + out_6860086072787592800[222] = 0.0; + out_6860086072787592800[223] = 0.0; + out_6860086072787592800[224] = 0.0; + out_6860086072787592800[225] = 0.0; + out_6860086072787592800[226] = 0.0; + out_6860086072787592800[227] = 0.0; + out_6860086072787592800[228] = 1.0; + out_6860086072787592800[229] = 0.0; + out_6860086072787592800[230] = 0.0; + out_6860086072787592800[231] = 0.0; + out_6860086072787592800[232] = 0.0; + out_6860086072787592800[233] = 0.0; + out_6860086072787592800[234] = 0.0; + out_6860086072787592800[235] = 0.0; + out_6860086072787592800[236] = 0.0; + out_6860086072787592800[237] = 0.0; + out_6860086072787592800[238] = 0.0; + out_6860086072787592800[239] = 0.0; + out_6860086072787592800[240] = 0.0; + out_6860086072787592800[241] = 0.0; + out_6860086072787592800[242] = 0.0; + out_6860086072787592800[243] = 0.0; + out_6860086072787592800[244] = 0.0; + out_6860086072787592800[245] = 0.0; + out_6860086072787592800[246] = 0.0; + out_6860086072787592800[247] = 1.0; + out_6860086072787592800[248] = 0.0; + out_6860086072787592800[249] = 0.0; + out_6860086072787592800[250] = 0.0; + out_6860086072787592800[251] = 0.0; + out_6860086072787592800[252] = 0.0; + out_6860086072787592800[253] = 0.0; + out_6860086072787592800[254] = 0.0; + out_6860086072787592800[255] = 0.0; + out_6860086072787592800[256] = 0.0; + out_6860086072787592800[257] = 0.0; + out_6860086072787592800[258] = 0.0; + out_6860086072787592800[259] = 0.0; + out_6860086072787592800[260] = 0.0; + out_6860086072787592800[261] = 0.0; + out_6860086072787592800[262] = 0.0; + out_6860086072787592800[263] = 0.0; + out_6860086072787592800[264] = 0.0; + out_6860086072787592800[265] = 0.0; + out_6860086072787592800[266] = 1.0; + out_6860086072787592800[267] = 0.0; + out_6860086072787592800[268] = 0.0; + out_6860086072787592800[269] = 0.0; + out_6860086072787592800[270] = 0.0; + out_6860086072787592800[271] = 0.0; + out_6860086072787592800[272] = 0.0; + out_6860086072787592800[273] = 0.0; + out_6860086072787592800[274] = 0.0; + out_6860086072787592800[275] = 0.0; + out_6860086072787592800[276] = 0.0; + out_6860086072787592800[277] = 0.0; + out_6860086072787592800[278] = 0.0; + out_6860086072787592800[279] = 0.0; + out_6860086072787592800[280] = 0.0; + out_6860086072787592800[281] = 0.0; + out_6860086072787592800[282] = 0.0; + out_6860086072787592800[283] = 0.0; + out_6860086072787592800[284] = 0.0; + out_6860086072787592800[285] = 1.0; + out_6860086072787592800[286] = 0.0; + out_6860086072787592800[287] = 0.0; + out_6860086072787592800[288] = 0.0; + out_6860086072787592800[289] = 0.0; + out_6860086072787592800[290] = 0.0; + out_6860086072787592800[291] = 0.0; + out_6860086072787592800[292] = 0.0; + out_6860086072787592800[293] = 0.0; + out_6860086072787592800[294] = 0.0; + out_6860086072787592800[295] = 0.0; + out_6860086072787592800[296] = 0.0; + out_6860086072787592800[297] = 0.0; + out_6860086072787592800[298] = 0.0; + out_6860086072787592800[299] = 0.0; + out_6860086072787592800[300] = 0.0; + out_6860086072787592800[301] = 0.0; + out_6860086072787592800[302] = 0.0; + out_6860086072787592800[303] = 0.0; + out_6860086072787592800[304] = 1.0; + out_6860086072787592800[305] = 0.0; + out_6860086072787592800[306] = 0.0; + out_6860086072787592800[307] = 0.0; + out_6860086072787592800[308] = 0.0; + out_6860086072787592800[309] = 0.0; + out_6860086072787592800[310] = 0.0; + out_6860086072787592800[311] = 0.0; + out_6860086072787592800[312] = 0.0; + out_6860086072787592800[313] = 0.0; + out_6860086072787592800[314] = 0.0; + out_6860086072787592800[315] = 0.0; + out_6860086072787592800[316] = 0.0; + out_6860086072787592800[317] = 0.0; + out_6860086072787592800[318] = 0.0; + out_6860086072787592800[319] = 0.0; + out_6860086072787592800[320] = 0.0; + out_6860086072787592800[321] = 0.0; + out_6860086072787592800[322] = 0.0; + out_6860086072787592800[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4855847476055627778) { - out_4855847476055627778[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_4855847476055627778[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_4855847476055627778[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_4855847476055627778[3] = dt*state[12] + state[3]; - out_4855847476055627778[4] = dt*state[13] + state[4]; - out_4855847476055627778[5] = dt*state[14] + state[5]; - out_4855847476055627778[6] = state[6]; - out_4855847476055627778[7] = state[7]; - out_4855847476055627778[8] = state[8]; - out_4855847476055627778[9] = state[9]; - out_4855847476055627778[10] = state[10]; - out_4855847476055627778[11] = state[11]; - out_4855847476055627778[12] = state[12]; - out_4855847476055627778[13] = state[13]; - out_4855847476055627778[14] = state[14]; - out_4855847476055627778[15] = state[15]; - out_4855847476055627778[16] = state[16]; - out_4855847476055627778[17] = state[17]; +void f_fun(double *state, double dt, double *out_6157473931416548839) { + out_6157473931416548839[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_6157473931416548839[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_6157473931416548839[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_6157473931416548839[3] = dt*state[12] + state[3]; + out_6157473931416548839[4] = dt*state[13] + state[4]; + out_6157473931416548839[5] = dt*state[14] + state[5]; + out_6157473931416548839[6] = state[6]; + out_6157473931416548839[7] = state[7]; + out_6157473931416548839[8] = state[8]; + out_6157473931416548839[9] = state[9]; + out_6157473931416548839[10] = state[10]; + out_6157473931416548839[11] = state[11]; + out_6157473931416548839[12] = state[12]; + out_6157473931416548839[13] = state[13]; + out_6157473931416548839[14] = state[14]; + out_6157473931416548839[15] = state[15]; + out_6157473931416548839[16] = state[16]; + out_6157473931416548839[17] = state[17]; } -void F_fun(double *state, double dt, double *out_1703318423524786966) { - out_1703318423524786966[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_1703318423524786966[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_1703318423524786966[2] = 0; - out_1703318423524786966[3] = 0; - out_1703318423524786966[4] = 0; - out_1703318423524786966[5] = 0; - out_1703318423524786966[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_1703318423524786966[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_1703318423524786966[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_1703318423524786966[9] = 0; - out_1703318423524786966[10] = 0; - out_1703318423524786966[11] = 0; - out_1703318423524786966[12] = 0; - out_1703318423524786966[13] = 0; - out_1703318423524786966[14] = 0; - out_1703318423524786966[15] = 0; - out_1703318423524786966[16] = 0; - out_1703318423524786966[17] = 0; - out_1703318423524786966[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_1703318423524786966[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_1703318423524786966[20] = 0; - out_1703318423524786966[21] = 0; - out_1703318423524786966[22] = 0; - out_1703318423524786966[23] = 0; - out_1703318423524786966[24] = 0; - out_1703318423524786966[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_1703318423524786966[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_1703318423524786966[27] = 0; - out_1703318423524786966[28] = 0; - out_1703318423524786966[29] = 0; - out_1703318423524786966[30] = 0; - out_1703318423524786966[31] = 0; - out_1703318423524786966[32] = 0; - out_1703318423524786966[33] = 0; - out_1703318423524786966[34] = 0; - out_1703318423524786966[35] = 0; - out_1703318423524786966[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_1703318423524786966[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_1703318423524786966[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_1703318423524786966[39] = 0; - out_1703318423524786966[40] = 0; - out_1703318423524786966[41] = 0; - out_1703318423524786966[42] = 0; - out_1703318423524786966[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_1703318423524786966[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_1703318423524786966[45] = 0; - out_1703318423524786966[46] = 0; - out_1703318423524786966[47] = 0; - out_1703318423524786966[48] = 0; - out_1703318423524786966[49] = 0; - out_1703318423524786966[50] = 0; - out_1703318423524786966[51] = 0; - out_1703318423524786966[52] = 0; - out_1703318423524786966[53] = 0; - out_1703318423524786966[54] = 0; - out_1703318423524786966[55] = 0; - out_1703318423524786966[56] = 0; - out_1703318423524786966[57] = 1; - out_1703318423524786966[58] = 0; - out_1703318423524786966[59] = 0; - out_1703318423524786966[60] = 0; - out_1703318423524786966[61] = 0; - out_1703318423524786966[62] = 0; - out_1703318423524786966[63] = 0; - out_1703318423524786966[64] = 0; - out_1703318423524786966[65] = 0; - out_1703318423524786966[66] = dt; - out_1703318423524786966[67] = 0; - out_1703318423524786966[68] = 0; - out_1703318423524786966[69] = 0; - out_1703318423524786966[70] = 0; - out_1703318423524786966[71] = 0; - out_1703318423524786966[72] = 0; - out_1703318423524786966[73] = 0; - out_1703318423524786966[74] = 0; - out_1703318423524786966[75] = 0; - out_1703318423524786966[76] = 1; - out_1703318423524786966[77] = 0; - out_1703318423524786966[78] = 0; - out_1703318423524786966[79] = 0; - out_1703318423524786966[80] = 0; - out_1703318423524786966[81] = 0; - out_1703318423524786966[82] = 0; - out_1703318423524786966[83] = 0; - out_1703318423524786966[84] = 0; - out_1703318423524786966[85] = dt; - out_1703318423524786966[86] = 0; - out_1703318423524786966[87] = 0; - out_1703318423524786966[88] = 0; - out_1703318423524786966[89] = 0; - out_1703318423524786966[90] = 0; - out_1703318423524786966[91] = 0; - out_1703318423524786966[92] = 0; - out_1703318423524786966[93] = 0; - out_1703318423524786966[94] = 0; - out_1703318423524786966[95] = 1; - out_1703318423524786966[96] = 0; - out_1703318423524786966[97] = 0; - out_1703318423524786966[98] = 0; - out_1703318423524786966[99] = 0; - out_1703318423524786966[100] = 0; - out_1703318423524786966[101] = 0; - out_1703318423524786966[102] = 0; - out_1703318423524786966[103] = 0; - out_1703318423524786966[104] = dt; - out_1703318423524786966[105] = 0; - out_1703318423524786966[106] = 0; - out_1703318423524786966[107] = 0; - out_1703318423524786966[108] = 0; - out_1703318423524786966[109] = 0; - out_1703318423524786966[110] = 0; - out_1703318423524786966[111] = 0; - out_1703318423524786966[112] = 0; - out_1703318423524786966[113] = 0; - out_1703318423524786966[114] = 1; - out_1703318423524786966[115] = 0; - out_1703318423524786966[116] = 0; - out_1703318423524786966[117] = 0; - out_1703318423524786966[118] = 0; - out_1703318423524786966[119] = 0; - out_1703318423524786966[120] = 0; - out_1703318423524786966[121] = 0; - out_1703318423524786966[122] = 0; - out_1703318423524786966[123] = 0; - out_1703318423524786966[124] = 0; - out_1703318423524786966[125] = 0; - out_1703318423524786966[126] = 0; - out_1703318423524786966[127] = 0; - out_1703318423524786966[128] = 0; - out_1703318423524786966[129] = 0; - out_1703318423524786966[130] = 0; - out_1703318423524786966[131] = 0; - out_1703318423524786966[132] = 0; - out_1703318423524786966[133] = 1; - out_1703318423524786966[134] = 0; - out_1703318423524786966[135] = 0; - out_1703318423524786966[136] = 0; - out_1703318423524786966[137] = 0; - out_1703318423524786966[138] = 0; - out_1703318423524786966[139] = 0; - out_1703318423524786966[140] = 0; - out_1703318423524786966[141] = 0; - out_1703318423524786966[142] = 0; - out_1703318423524786966[143] = 0; - out_1703318423524786966[144] = 0; - out_1703318423524786966[145] = 0; - out_1703318423524786966[146] = 0; - out_1703318423524786966[147] = 0; - out_1703318423524786966[148] = 0; - out_1703318423524786966[149] = 0; - out_1703318423524786966[150] = 0; - out_1703318423524786966[151] = 0; - out_1703318423524786966[152] = 1; - out_1703318423524786966[153] = 0; - out_1703318423524786966[154] = 0; - out_1703318423524786966[155] = 0; - out_1703318423524786966[156] = 0; - out_1703318423524786966[157] = 0; - out_1703318423524786966[158] = 0; - out_1703318423524786966[159] = 0; - out_1703318423524786966[160] = 0; - out_1703318423524786966[161] = 0; - out_1703318423524786966[162] = 0; - out_1703318423524786966[163] = 0; - out_1703318423524786966[164] = 0; - out_1703318423524786966[165] = 0; - out_1703318423524786966[166] = 0; - out_1703318423524786966[167] = 0; - out_1703318423524786966[168] = 0; - out_1703318423524786966[169] = 0; - out_1703318423524786966[170] = 0; - out_1703318423524786966[171] = 1; - out_1703318423524786966[172] = 0; - out_1703318423524786966[173] = 0; - out_1703318423524786966[174] = 0; - out_1703318423524786966[175] = 0; - out_1703318423524786966[176] = 0; - out_1703318423524786966[177] = 0; - out_1703318423524786966[178] = 0; - out_1703318423524786966[179] = 0; - out_1703318423524786966[180] = 0; - out_1703318423524786966[181] = 0; - out_1703318423524786966[182] = 0; - out_1703318423524786966[183] = 0; - out_1703318423524786966[184] = 0; - out_1703318423524786966[185] = 0; - out_1703318423524786966[186] = 0; - out_1703318423524786966[187] = 0; - out_1703318423524786966[188] = 0; - out_1703318423524786966[189] = 0; - out_1703318423524786966[190] = 1; - out_1703318423524786966[191] = 0; - out_1703318423524786966[192] = 0; - out_1703318423524786966[193] = 0; - out_1703318423524786966[194] = 0; - out_1703318423524786966[195] = 0; - out_1703318423524786966[196] = 0; - out_1703318423524786966[197] = 0; - out_1703318423524786966[198] = 0; - out_1703318423524786966[199] = 0; - out_1703318423524786966[200] = 0; - out_1703318423524786966[201] = 0; - out_1703318423524786966[202] = 0; - out_1703318423524786966[203] = 0; - out_1703318423524786966[204] = 0; - out_1703318423524786966[205] = 0; - out_1703318423524786966[206] = 0; - out_1703318423524786966[207] = 0; - out_1703318423524786966[208] = 0; - out_1703318423524786966[209] = 1; - out_1703318423524786966[210] = 0; - out_1703318423524786966[211] = 0; - out_1703318423524786966[212] = 0; - out_1703318423524786966[213] = 0; - out_1703318423524786966[214] = 0; - out_1703318423524786966[215] = 0; - out_1703318423524786966[216] = 0; - out_1703318423524786966[217] = 0; - out_1703318423524786966[218] = 0; - out_1703318423524786966[219] = 0; - out_1703318423524786966[220] = 0; - out_1703318423524786966[221] = 0; - out_1703318423524786966[222] = 0; - out_1703318423524786966[223] = 0; - out_1703318423524786966[224] = 0; - out_1703318423524786966[225] = 0; - out_1703318423524786966[226] = 0; - out_1703318423524786966[227] = 0; - out_1703318423524786966[228] = 1; - out_1703318423524786966[229] = 0; - out_1703318423524786966[230] = 0; - out_1703318423524786966[231] = 0; - out_1703318423524786966[232] = 0; - out_1703318423524786966[233] = 0; - out_1703318423524786966[234] = 0; - out_1703318423524786966[235] = 0; - out_1703318423524786966[236] = 0; - out_1703318423524786966[237] = 0; - out_1703318423524786966[238] = 0; - out_1703318423524786966[239] = 0; - out_1703318423524786966[240] = 0; - out_1703318423524786966[241] = 0; - out_1703318423524786966[242] = 0; - out_1703318423524786966[243] = 0; - out_1703318423524786966[244] = 0; - out_1703318423524786966[245] = 0; - out_1703318423524786966[246] = 0; - out_1703318423524786966[247] = 1; - out_1703318423524786966[248] = 0; - out_1703318423524786966[249] = 0; - out_1703318423524786966[250] = 0; - out_1703318423524786966[251] = 0; - out_1703318423524786966[252] = 0; - out_1703318423524786966[253] = 0; - out_1703318423524786966[254] = 0; - out_1703318423524786966[255] = 0; - out_1703318423524786966[256] = 0; - out_1703318423524786966[257] = 0; - out_1703318423524786966[258] = 0; - out_1703318423524786966[259] = 0; - out_1703318423524786966[260] = 0; - out_1703318423524786966[261] = 0; - out_1703318423524786966[262] = 0; - out_1703318423524786966[263] = 0; - out_1703318423524786966[264] = 0; - out_1703318423524786966[265] = 0; - out_1703318423524786966[266] = 1; - out_1703318423524786966[267] = 0; - out_1703318423524786966[268] = 0; - out_1703318423524786966[269] = 0; - out_1703318423524786966[270] = 0; - out_1703318423524786966[271] = 0; - out_1703318423524786966[272] = 0; - out_1703318423524786966[273] = 0; - out_1703318423524786966[274] = 0; - out_1703318423524786966[275] = 0; - out_1703318423524786966[276] = 0; - out_1703318423524786966[277] = 0; - out_1703318423524786966[278] = 0; - out_1703318423524786966[279] = 0; - out_1703318423524786966[280] = 0; - out_1703318423524786966[281] = 0; - out_1703318423524786966[282] = 0; - out_1703318423524786966[283] = 0; - out_1703318423524786966[284] = 0; - out_1703318423524786966[285] = 1; - out_1703318423524786966[286] = 0; - out_1703318423524786966[287] = 0; - out_1703318423524786966[288] = 0; - out_1703318423524786966[289] = 0; - out_1703318423524786966[290] = 0; - out_1703318423524786966[291] = 0; - out_1703318423524786966[292] = 0; - out_1703318423524786966[293] = 0; - out_1703318423524786966[294] = 0; - out_1703318423524786966[295] = 0; - out_1703318423524786966[296] = 0; - out_1703318423524786966[297] = 0; - out_1703318423524786966[298] = 0; - out_1703318423524786966[299] = 0; - out_1703318423524786966[300] = 0; - out_1703318423524786966[301] = 0; - out_1703318423524786966[302] = 0; - out_1703318423524786966[303] = 0; - out_1703318423524786966[304] = 1; - out_1703318423524786966[305] = 0; - out_1703318423524786966[306] = 0; - out_1703318423524786966[307] = 0; - out_1703318423524786966[308] = 0; - out_1703318423524786966[309] = 0; - out_1703318423524786966[310] = 0; - out_1703318423524786966[311] = 0; - out_1703318423524786966[312] = 0; - out_1703318423524786966[313] = 0; - out_1703318423524786966[314] = 0; - out_1703318423524786966[315] = 0; - out_1703318423524786966[316] = 0; - out_1703318423524786966[317] = 0; - out_1703318423524786966[318] = 0; - out_1703318423524786966[319] = 0; - out_1703318423524786966[320] = 0; - out_1703318423524786966[321] = 0; - out_1703318423524786966[322] = 0; - out_1703318423524786966[323] = 1; +void F_fun(double *state, double dt, double *out_2563232569638129684) { + out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[2] = 0; + out_2563232569638129684[3] = 0; + out_2563232569638129684[4] = 0; + out_2563232569638129684[5] = 0; + out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[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_2563232569638129684[9] = 0; + out_2563232569638129684[10] = 0; + out_2563232569638129684[11] = 0; + out_2563232569638129684[12] = 0; + out_2563232569638129684[13] = 0; + out_2563232569638129684[14] = 0; + out_2563232569638129684[15] = 0; + out_2563232569638129684[16] = 0; + out_2563232569638129684[17] = 0; + out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[20] = 0; + out_2563232569638129684[21] = 0; + out_2563232569638129684[22] = 0; + out_2563232569638129684[23] = 0; + out_2563232569638129684[24] = 0; + out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[27] = 0; + out_2563232569638129684[28] = 0; + out_2563232569638129684[29] = 0; + out_2563232569638129684[30] = 0; + out_2563232569638129684[31] = 0; + out_2563232569638129684[32] = 0; + out_2563232569638129684[33] = 0; + out_2563232569638129684[34] = 0; + out_2563232569638129684[35] = 0; + out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[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_2563232569638129684[39] = 0; + out_2563232569638129684[40] = 0; + out_2563232569638129684[41] = 0; + out_2563232569638129684[42] = 0; + out_2563232569638129684[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_2563232569638129684[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_2563232569638129684[45] = 0; + out_2563232569638129684[46] = 0; + out_2563232569638129684[47] = 0; + out_2563232569638129684[48] = 0; + out_2563232569638129684[49] = 0; + out_2563232569638129684[50] = 0; + out_2563232569638129684[51] = 0; + out_2563232569638129684[52] = 0; + out_2563232569638129684[53] = 0; + out_2563232569638129684[54] = 0; + out_2563232569638129684[55] = 0; + out_2563232569638129684[56] = 0; + out_2563232569638129684[57] = 1; + out_2563232569638129684[58] = 0; + out_2563232569638129684[59] = 0; + out_2563232569638129684[60] = 0; + out_2563232569638129684[61] = 0; + out_2563232569638129684[62] = 0; + out_2563232569638129684[63] = 0; + out_2563232569638129684[64] = 0; + out_2563232569638129684[65] = 0; + out_2563232569638129684[66] = dt; + out_2563232569638129684[67] = 0; + out_2563232569638129684[68] = 0; + out_2563232569638129684[69] = 0; + out_2563232569638129684[70] = 0; + out_2563232569638129684[71] = 0; + out_2563232569638129684[72] = 0; + out_2563232569638129684[73] = 0; + out_2563232569638129684[74] = 0; + out_2563232569638129684[75] = 0; + out_2563232569638129684[76] = 1; + out_2563232569638129684[77] = 0; + out_2563232569638129684[78] = 0; + out_2563232569638129684[79] = 0; + out_2563232569638129684[80] = 0; + out_2563232569638129684[81] = 0; + out_2563232569638129684[82] = 0; + out_2563232569638129684[83] = 0; + out_2563232569638129684[84] = 0; + out_2563232569638129684[85] = dt; + out_2563232569638129684[86] = 0; + out_2563232569638129684[87] = 0; + out_2563232569638129684[88] = 0; + out_2563232569638129684[89] = 0; + out_2563232569638129684[90] = 0; + out_2563232569638129684[91] = 0; + out_2563232569638129684[92] = 0; + out_2563232569638129684[93] = 0; + out_2563232569638129684[94] = 0; + out_2563232569638129684[95] = 1; + out_2563232569638129684[96] = 0; + out_2563232569638129684[97] = 0; + out_2563232569638129684[98] = 0; + out_2563232569638129684[99] = 0; + out_2563232569638129684[100] = 0; + out_2563232569638129684[101] = 0; + out_2563232569638129684[102] = 0; + out_2563232569638129684[103] = 0; + out_2563232569638129684[104] = dt; + out_2563232569638129684[105] = 0; + out_2563232569638129684[106] = 0; + out_2563232569638129684[107] = 0; + out_2563232569638129684[108] = 0; + out_2563232569638129684[109] = 0; + out_2563232569638129684[110] = 0; + out_2563232569638129684[111] = 0; + out_2563232569638129684[112] = 0; + out_2563232569638129684[113] = 0; + out_2563232569638129684[114] = 1; + out_2563232569638129684[115] = 0; + out_2563232569638129684[116] = 0; + out_2563232569638129684[117] = 0; + out_2563232569638129684[118] = 0; + out_2563232569638129684[119] = 0; + out_2563232569638129684[120] = 0; + out_2563232569638129684[121] = 0; + out_2563232569638129684[122] = 0; + out_2563232569638129684[123] = 0; + out_2563232569638129684[124] = 0; + out_2563232569638129684[125] = 0; + out_2563232569638129684[126] = 0; + out_2563232569638129684[127] = 0; + out_2563232569638129684[128] = 0; + out_2563232569638129684[129] = 0; + out_2563232569638129684[130] = 0; + out_2563232569638129684[131] = 0; + out_2563232569638129684[132] = 0; + out_2563232569638129684[133] = 1; + out_2563232569638129684[134] = 0; + out_2563232569638129684[135] = 0; + out_2563232569638129684[136] = 0; + out_2563232569638129684[137] = 0; + out_2563232569638129684[138] = 0; + out_2563232569638129684[139] = 0; + out_2563232569638129684[140] = 0; + out_2563232569638129684[141] = 0; + out_2563232569638129684[142] = 0; + out_2563232569638129684[143] = 0; + out_2563232569638129684[144] = 0; + out_2563232569638129684[145] = 0; + out_2563232569638129684[146] = 0; + out_2563232569638129684[147] = 0; + out_2563232569638129684[148] = 0; + out_2563232569638129684[149] = 0; + out_2563232569638129684[150] = 0; + out_2563232569638129684[151] = 0; + out_2563232569638129684[152] = 1; + out_2563232569638129684[153] = 0; + out_2563232569638129684[154] = 0; + out_2563232569638129684[155] = 0; + out_2563232569638129684[156] = 0; + out_2563232569638129684[157] = 0; + out_2563232569638129684[158] = 0; + out_2563232569638129684[159] = 0; + out_2563232569638129684[160] = 0; + out_2563232569638129684[161] = 0; + out_2563232569638129684[162] = 0; + out_2563232569638129684[163] = 0; + out_2563232569638129684[164] = 0; + out_2563232569638129684[165] = 0; + out_2563232569638129684[166] = 0; + out_2563232569638129684[167] = 0; + out_2563232569638129684[168] = 0; + out_2563232569638129684[169] = 0; + out_2563232569638129684[170] = 0; + out_2563232569638129684[171] = 1; + out_2563232569638129684[172] = 0; + out_2563232569638129684[173] = 0; + out_2563232569638129684[174] = 0; + out_2563232569638129684[175] = 0; + out_2563232569638129684[176] = 0; + out_2563232569638129684[177] = 0; + out_2563232569638129684[178] = 0; + out_2563232569638129684[179] = 0; + out_2563232569638129684[180] = 0; + out_2563232569638129684[181] = 0; + out_2563232569638129684[182] = 0; + out_2563232569638129684[183] = 0; + out_2563232569638129684[184] = 0; + out_2563232569638129684[185] = 0; + out_2563232569638129684[186] = 0; + out_2563232569638129684[187] = 0; + out_2563232569638129684[188] = 0; + out_2563232569638129684[189] = 0; + out_2563232569638129684[190] = 1; + out_2563232569638129684[191] = 0; + out_2563232569638129684[192] = 0; + out_2563232569638129684[193] = 0; + out_2563232569638129684[194] = 0; + out_2563232569638129684[195] = 0; + out_2563232569638129684[196] = 0; + out_2563232569638129684[197] = 0; + out_2563232569638129684[198] = 0; + out_2563232569638129684[199] = 0; + out_2563232569638129684[200] = 0; + out_2563232569638129684[201] = 0; + out_2563232569638129684[202] = 0; + out_2563232569638129684[203] = 0; + out_2563232569638129684[204] = 0; + out_2563232569638129684[205] = 0; + out_2563232569638129684[206] = 0; + out_2563232569638129684[207] = 0; + out_2563232569638129684[208] = 0; + out_2563232569638129684[209] = 1; + out_2563232569638129684[210] = 0; + out_2563232569638129684[211] = 0; + out_2563232569638129684[212] = 0; + out_2563232569638129684[213] = 0; + out_2563232569638129684[214] = 0; + out_2563232569638129684[215] = 0; + out_2563232569638129684[216] = 0; + out_2563232569638129684[217] = 0; + out_2563232569638129684[218] = 0; + out_2563232569638129684[219] = 0; + out_2563232569638129684[220] = 0; + out_2563232569638129684[221] = 0; + out_2563232569638129684[222] = 0; + out_2563232569638129684[223] = 0; + out_2563232569638129684[224] = 0; + out_2563232569638129684[225] = 0; + out_2563232569638129684[226] = 0; + out_2563232569638129684[227] = 0; + out_2563232569638129684[228] = 1; + out_2563232569638129684[229] = 0; + out_2563232569638129684[230] = 0; + out_2563232569638129684[231] = 0; + out_2563232569638129684[232] = 0; + out_2563232569638129684[233] = 0; + out_2563232569638129684[234] = 0; + out_2563232569638129684[235] = 0; + out_2563232569638129684[236] = 0; + out_2563232569638129684[237] = 0; + out_2563232569638129684[238] = 0; + out_2563232569638129684[239] = 0; + out_2563232569638129684[240] = 0; + out_2563232569638129684[241] = 0; + out_2563232569638129684[242] = 0; + out_2563232569638129684[243] = 0; + out_2563232569638129684[244] = 0; + out_2563232569638129684[245] = 0; + out_2563232569638129684[246] = 0; + out_2563232569638129684[247] = 1; + out_2563232569638129684[248] = 0; + out_2563232569638129684[249] = 0; + out_2563232569638129684[250] = 0; + out_2563232569638129684[251] = 0; + out_2563232569638129684[252] = 0; + out_2563232569638129684[253] = 0; + out_2563232569638129684[254] = 0; + out_2563232569638129684[255] = 0; + out_2563232569638129684[256] = 0; + out_2563232569638129684[257] = 0; + out_2563232569638129684[258] = 0; + out_2563232569638129684[259] = 0; + out_2563232569638129684[260] = 0; + out_2563232569638129684[261] = 0; + out_2563232569638129684[262] = 0; + out_2563232569638129684[263] = 0; + out_2563232569638129684[264] = 0; + out_2563232569638129684[265] = 0; + out_2563232569638129684[266] = 1; + out_2563232569638129684[267] = 0; + out_2563232569638129684[268] = 0; + out_2563232569638129684[269] = 0; + out_2563232569638129684[270] = 0; + out_2563232569638129684[271] = 0; + out_2563232569638129684[272] = 0; + out_2563232569638129684[273] = 0; + out_2563232569638129684[274] = 0; + out_2563232569638129684[275] = 0; + out_2563232569638129684[276] = 0; + out_2563232569638129684[277] = 0; + out_2563232569638129684[278] = 0; + out_2563232569638129684[279] = 0; + out_2563232569638129684[280] = 0; + out_2563232569638129684[281] = 0; + out_2563232569638129684[282] = 0; + out_2563232569638129684[283] = 0; + out_2563232569638129684[284] = 0; + out_2563232569638129684[285] = 1; + out_2563232569638129684[286] = 0; + out_2563232569638129684[287] = 0; + out_2563232569638129684[288] = 0; + out_2563232569638129684[289] = 0; + out_2563232569638129684[290] = 0; + out_2563232569638129684[291] = 0; + out_2563232569638129684[292] = 0; + out_2563232569638129684[293] = 0; + out_2563232569638129684[294] = 0; + out_2563232569638129684[295] = 0; + out_2563232569638129684[296] = 0; + out_2563232569638129684[297] = 0; + out_2563232569638129684[298] = 0; + out_2563232569638129684[299] = 0; + out_2563232569638129684[300] = 0; + out_2563232569638129684[301] = 0; + out_2563232569638129684[302] = 0; + out_2563232569638129684[303] = 0; + out_2563232569638129684[304] = 1; + out_2563232569638129684[305] = 0; + out_2563232569638129684[306] = 0; + out_2563232569638129684[307] = 0; + out_2563232569638129684[308] = 0; + out_2563232569638129684[309] = 0; + out_2563232569638129684[310] = 0; + out_2563232569638129684[311] = 0; + out_2563232569638129684[312] = 0; + out_2563232569638129684[313] = 0; + out_2563232569638129684[314] = 0; + out_2563232569638129684[315] = 0; + out_2563232569638129684[316] = 0; + out_2563232569638129684[317] = 0; + out_2563232569638129684[318] = 0; + out_2563232569638129684[319] = 0; + out_2563232569638129684[320] = 0; + out_2563232569638129684[321] = 0; + out_2563232569638129684[322] = 0; + out_2563232569638129684[323] = 1; } -void h_4(double *state, double *unused, double *out_7821603690376207235) { - out_7821603690376207235[0] = state[6] + state[9]; - out_7821603690376207235[1] = state[7] + state[10]; - out_7821603690376207235[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_9057789463521636950) { + out_9057789463521636950[0] = state[6] + state[9]; + out_9057789463521636950[1] = state[7] + state[10]; + out_9057789463521636950[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_1639990342467660562) { - out_1639990342467660562[0] = 0; - out_1639990342467660562[1] = 0; - out_1639990342467660562[2] = 0; - out_1639990342467660562[3] = 0; - out_1639990342467660562[4] = 0; - out_1639990342467660562[5] = 0; - out_1639990342467660562[6] = 1; - out_1639990342467660562[7] = 0; - out_1639990342467660562[8] = 0; - out_1639990342467660562[9] = 1; - out_1639990342467660562[10] = 0; - out_1639990342467660562[11] = 0; - out_1639990342467660562[12] = 0; - out_1639990342467660562[13] = 0; - out_1639990342467660562[14] = 0; - out_1639990342467660562[15] = 0; - out_1639990342467660562[16] = 0; - out_1639990342467660562[17] = 0; - out_1639990342467660562[18] = 0; - out_1639990342467660562[19] = 0; - out_1639990342467660562[20] = 0; - out_1639990342467660562[21] = 0; - out_1639990342467660562[22] = 0; - out_1639990342467660562[23] = 0; - out_1639990342467660562[24] = 0; - out_1639990342467660562[25] = 1; - out_1639990342467660562[26] = 0; - out_1639990342467660562[27] = 0; - out_1639990342467660562[28] = 1; - out_1639990342467660562[29] = 0; - out_1639990342467660562[30] = 0; - out_1639990342467660562[31] = 0; - out_1639990342467660562[32] = 0; - out_1639990342467660562[33] = 0; - out_1639990342467660562[34] = 0; - out_1639990342467660562[35] = 0; - out_1639990342467660562[36] = 0; - out_1639990342467660562[37] = 0; - out_1639990342467660562[38] = 0; - out_1639990342467660562[39] = 0; - out_1639990342467660562[40] = 0; - out_1639990342467660562[41] = 0; - out_1639990342467660562[42] = 0; - out_1639990342467660562[43] = 0; - out_1639990342467660562[44] = 1; - out_1639990342467660562[45] = 0; - out_1639990342467660562[46] = 0; - out_1639990342467660562[47] = 1; - out_1639990342467660562[48] = 0; - out_1639990342467660562[49] = 0; - out_1639990342467660562[50] = 0; - out_1639990342467660562[51] = 0; - out_1639990342467660562[52] = 0; - out_1639990342467660562[53] = 0; +void H_4(double *state, double *unused, double *out_6434139615882878081) { + out_6434139615882878081[0] = 0; + out_6434139615882878081[1] = 0; + out_6434139615882878081[2] = 0; + out_6434139615882878081[3] = 0; + out_6434139615882878081[4] = 0; + out_6434139615882878081[5] = 0; + out_6434139615882878081[6] = 1; + out_6434139615882878081[7] = 0; + out_6434139615882878081[8] = 0; + out_6434139615882878081[9] = 1; + out_6434139615882878081[10] = 0; + out_6434139615882878081[11] = 0; + out_6434139615882878081[12] = 0; + out_6434139615882878081[13] = 0; + out_6434139615882878081[14] = 0; + out_6434139615882878081[15] = 0; + out_6434139615882878081[16] = 0; + out_6434139615882878081[17] = 0; + out_6434139615882878081[18] = 0; + out_6434139615882878081[19] = 0; + out_6434139615882878081[20] = 0; + out_6434139615882878081[21] = 0; + out_6434139615882878081[22] = 0; + out_6434139615882878081[23] = 0; + out_6434139615882878081[24] = 0; + out_6434139615882878081[25] = 1; + out_6434139615882878081[26] = 0; + out_6434139615882878081[27] = 0; + out_6434139615882878081[28] = 1; + out_6434139615882878081[29] = 0; + out_6434139615882878081[30] = 0; + out_6434139615882878081[31] = 0; + out_6434139615882878081[32] = 0; + out_6434139615882878081[33] = 0; + out_6434139615882878081[34] = 0; + out_6434139615882878081[35] = 0; + out_6434139615882878081[36] = 0; + out_6434139615882878081[37] = 0; + out_6434139615882878081[38] = 0; + out_6434139615882878081[39] = 0; + out_6434139615882878081[40] = 0; + out_6434139615882878081[41] = 0; + out_6434139615882878081[42] = 0; + out_6434139615882878081[43] = 0; + out_6434139615882878081[44] = 1; + out_6434139615882878081[45] = 0; + out_6434139615882878081[46] = 0; + out_6434139615882878081[47] = 1; + out_6434139615882878081[48] = 0; + out_6434139615882878081[49] = 0; + out_6434139615882878081[50] = 0; + out_6434139615882878081[51] = 0; + out_6434139615882878081[52] = 0; + out_6434139615882878081[53] = 0; } -void h_10(double *state, double *unused, double *out_4627608962100745909) { - out_4627608962100745909[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_4627608962100745909[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_4627608962100745909[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_439040734435285181) { + out_439040734435285181[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_439040734435285181[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_439040734435285181[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_5548344007433806154) { - out_5548344007433806154[0] = 0; - out_5548344007433806154[1] = 9.8100000000000005*cos(state[1]); - out_5548344007433806154[2] = 0; - out_5548344007433806154[3] = 0; - out_5548344007433806154[4] = -state[8]; - out_5548344007433806154[5] = state[7]; - out_5548344007433806154[6] = 0; - out_5548344007433806154[7] = state[5]; - out_5548344007433806154[8] = -state[4]; - out_5548344007433806154[9] = 0; - out_5548344007433806154[10] = 0; - out_5548344007433806154[11] = 0; - out_5548344007433806154[12] = 1; - out_5548344007433806154[13] = 0; - out_5548344007433806154[14] = 0; - out_5548344007433806154[15] = 1; - out_5548344007433806154[16] = 0; - out_5548344007433806154[17] = 0; - out_5548344007433806154[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_5548344007433806154[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_5548344007433806154[20] = 0; - out_5548344007433806154[21] = state[8]; - out_5548344007433806154[22] = 0; - out_5548344007433806154[23] = -state[6]; - out_5548344007433806154[24] = -state[5]; - out_5548344007433806154[25] = 0; - out_5548344007433806154[26] = state[3]; - out_5548344007433806154[27] = 0; - out_5548344007433806154[28] = 0; - out_5548344007433806154[29] = 0; - out_5548344007433806154[30] = 0; - out_5548344007433806154[31] = 1; - out_5548344007433806154[32] = 0; - out_5548344007433806154[33] = 0; - out_5548344007433806154[34] = 1; - out_5548344007433806154[35] = 0; - out_5548344007433806154[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_5548344007433806154[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_5548344007433806154[38] = 0; - out_5548344007433806154[39] = -state[7]; - out_5548344007433806154[40] = state[6]; - out_5548344007433806154[41] = 0; - out_5548344007433806154[42] = state[4]; - out_5548344007433806154[43] = -state[3]; - out_5548344007433806154[44] = 0; - out_5548344007433806154[45] = 0; - out_5548344007433806154[46] = 0; - out_5548344007433806154[47] = 0; - out_5548344007433806154[48] = 0; - out_5548344007433806154[49] = 0; - out_5548344007433806154[50] = 1; - out_5548344007433806154[51] = 0; - out_5548344007433806154[52] = 0; - out_5548344007433806154[53] = 1; +void H_10(double *state, double *unused, double *out_1840785913394498883) { + out_1840785913394498883[0] = 0; + out_1840785913394498883[1] = 9.8100000000000005*cos(state[1]); + out_1840785913394498883[2] = 0; + out_1840785913394498883[3] = 0; + out_1840785913394498883[4] = -state[8]; + out_1840785913394498883[5] = state[7]; + out_1840785913394498883[6] = 0; + out_1840785913394498883[7] = state[5]; + out_1840785913394498883[8] = -state[4]; + out_1840785913394498883[9] = 0; + out_1840785913394498883[10] = 0; + out_1840785913394498883[11] = 0; + out_1840785913394498883[12] = 1; + out_1840785913394498883[13] = 0; + out_1840785913394498883[14] = 0; + out_1840785913394498883[15] = 1; + out_1840785913394498883[16] = 0; + out_1840785913394498883[17] = 0; + out_1840785913394498883[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_1840785913394498883[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_1840785913394498883[20] = 0; + out_1840785913394498883[21] = state[8]; + out_1840785913394498883[22] = 0; + out_1840785913394498883[23] = -state[6]; + out_1840785913394498883[24] = -state[5]; + out_1840785913394498883[25] = 0; + out_1840785913394498883[26] = state[3]; + out_1840785913394498883[27] = 0; + out_1840785913394498883[28] = 0; + out_1840785913394498883[29] = 0; + out_1840785913394498883[30] = 0; + out_1840785913394498883[31] = 1; + out_1840785913394498883[32] = 0; + out_1840785913394498883[33] = 0; + out_1840785913394498883[34] = 1; + out_1840785913394498883[35] = 0; + out_1840785913394498883[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_1840785913394498883[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_1840785913394498883[38] = 0; + out_1840785913394498883[39] = -state[7]; + out_1840785913394498883[40] = state[6]; + out_1840785913394498883[41] = 0; + out_1840785913394498883[42] = state[4]; + out_1840785913394498883[43] = -state[3]; + out_1840785913394498883[44] = 0; + out_1840785913394498883[45] = 0; + out_1840785913394498883[46] = 0; + out_1840785913394498883[47] = 0; + out_1840785913394498883[48] = 0; + out_1840785913394498883[49] = 0; + out_1840785913394498883[50] = 1; + out_1840785913394498883[51] = 0; + out_1840785913394498883[52] = 0; + out_1840785913394498883[53] = 1; } -void h_13(double *state, double *unused, double *out_5105456950148742445) { - out_5105456950148742445[0] = state[3]; - out_5105456950148742445[1] = state[4]; - out_5105456950148742445[2] = state[5]; +void h_13(double *state, double *unused, double *out_7181492765475252926) { + out_7181492765475252926[0] = state[3]; + out_7181492765475252926[1] = state[4]; + out_7181492765475252926[2] = state[5]; } -void H_13(double *state, double *unused, double *out_4852264167799993363) { - out_4852264167799993363[0] = 0; - out_4852264167799993363[1] = 0; - out_4852264167799993363[2] = 0; - out_4852264167799993363[3] = 1; - out_4852264167799993363[4] = 0; - out_4852264167799993363[5] = 0; - out_4852264167799993363[6] = 0; - out_4852264167799993363[7] = 0; - out_4852264167799993363[8] = 0; - out_4852264167799993363[9] = 0; - out_4852264167799993363[10] = 0; - out_4852264167799993363[11] = 0; - out_4852264167799993363[12] = 0; - out_4852264167799993363[13] = 0; - out_4852264167799993363[14] = 0; - out_4852264167799993363[15] = 0; - out_4852264167799993363[16] = 0; - out_4852264167799993363[17] = 0; - out_4852264167799993363[18] = 0; - out_4852264167799993363[19] = 0; - out_4852264167799993363[20] = 0; - out_4852264167799993363[21] = 0; - out_4852264167799993363[22] = 1; - out_4852264167799993363[23] = 0; - out_4852264167799993363[24] = 0; - out_4852264167799993363[25] = 0; - out_4852264167799993363[26] = 0; - out_4852264167799993363[27] = 0; - out_4852264167799993363[28] = 0; - out_4852264167799993363[29] = 0; - out_4852264167799993363[30] = 0; - out_4852264167799993363[31] = 0; - out_4852264167799993363[32] = 0; - out_4852264167799993363[33] = 0; - out_4852264167799993363[34] = 0; - out_4852264167799993363[35] = 0; - out_4852264167799993363[36] = 0; - out_4852264167799993363[37] = 0; - out_4852264167799993363[38] = 0; - out_4852264167799993363[39] = 0; - out_4852264167799993363[40] = 0; - out_4852264167799993363[41] = 1; - out_4852264167799993363[42] = 0; - out_4852264167799993363[43] = 0; - out_4852264167799993363[44] = 0; - out_4852264167799993363[45] = 0; - out_4852264167799993363[46] = 0; - out_4852264167799993363[47] = 0; - out_4852264167799993363[48] = 0; - out_4852264167799993363[49] = 0; - out_4852264167799993363[50] = 0; - out_4852264167799993363[51] = 0; - out_4852264167799993363[52] = 0; - out_4852264167799993363[53] = 0; +void H_13(double *state, double *unused, double *out_3221865790550545280) { + out_3221865790550545280[0] = 0; + out_3221865790550545280[1] = 0; + out_3221865790550545280[2] = 0; + out_3221865790550545280[3] = 1; + out_3221865790550545280[4] = 0; + out_3221865790550545280[5] = 0; + out_3221865790550545280[6] = 0; + out_3221865790550545280[7] = 0; + out_3221865790550545280[8] = 0; + out_3221865790550545280[9] = 0; + out_3221865790550545280[10] = 0; + out_3221865790550545280[11] = 0; + out_3221865790550545280[12] = 0; + out_3221865790550545280[13] = 0; + out_3221865790550545280[14] = 0; + out_3221865790550545280[15] = 0; + out_3221865790550545280[16] = 0; + out_3221865790550545280[17] = 0; + out_3221865790550545280[18] = 0; + out_3221865790550545280[19] = 0; + out_3221865790550545280[20] = 0; + out_3221865790550545280[21] = 0; + out_3221865790550545280[22] = 1; + out_3221865790550545280[23] = 0; + out_3221865790550545280[24] = 0; + out_3221865790550545280[25] = 0; + out_3221865790550545280[26] = 0; + out_3221865790550545280[27] = 0; + out_3221865790550545280[28] = 0; + out_3221865790550545280[29] = 0; + out_3221865790550545280[30] = 0; + out_3221865790550545280[31] = 0; + out_3221865790550545280[32] = 0; + out_3221865790550545280[33] = 0; + out_3221865790550545280[34] = 0; + out_3221865790550545280[35] = 0; + out_3221865790550545280[36] = 0; + out_3221865790550545280[37] = 0; + out_3221865790550545280[38] = 0; + out_3221865790550545280[39] = 0; + out_3221865790550545280[40] = 0; + out_3221865790550545280[41] = 1; + out_3221865790550545280[42] = 0; + out_3221865790550545280[43] = 0; + out_3221865790550545280[44] = 0; + out_3221865790550545280[45] = 0; + out_3221865790550545280[46] = 0; + out_3221865790550545280[47] = 0; + out_3221865790550545280[48] = 0; + out_3221865790550545280[49] = 0; + out_3221865790550545280[50] = 0; + out_3221865790550545280[51] = 0; + out_3221865790550545280[52] = 0; + out_3221865790550545280[53] = 0; } -void h_14(double *state, double *unused, double *out_7814084012328434224) { - out_7814084012328434224[0] = state[6]; - out_7814084012328434224[1] = state[7]; - out_7814084012328434224[2] = state[8]; +void h_14(double *state, double *unused, double *out_1398557426791028034) { + out_1398557426791028034[0] = state[6]; + out_1398557426791028034[1] = state[7]; + out_1398557426791028034[2] = state[8]; } -void H_14(double *state, double *unused, double *out_5603231198807145091) { - out_5603231198807145091[0] = 0; - out_5603231198807145091[1] = 0; - out_5603231198807145091[2] = 0; - out_5603231198807145091[3] = 0; - out_5603231198807145091[4] = 0; - out_5603231198807145091[5] = 0; - out_5603231198807145091[6] = 1; - out_5603231198807145091[7] = 0; - out_5603231198807145091[8] = 0; - out_5603231198807145091[9] = 0; - out_5603231198807145091[10] = 0; - out_5603231198807145091[11] = 0; - out_5603231198807145091[12] = 0; - out_5603231198807145091[13] = 0; - out_5603231198807145091[14] = 0; - out_5603231198807145091[15] = 0; - out_5603231198807145091[16] = 0; - out_5603231198807145091[17] = 0; - out_5603231198807145091[18] = 0; - out_5603231198807145091[19] = 0; - out_5603231198807145091[20] = 0; - out_5603231198807145091[21] = 0; - out_5603231198807145091[22] = 0; - out_5603231198807145091[23] = 0; - out_5603231198807145091[24] = 0; - out_5603231198807145091[25] = 1; - out_5603231198807145091[26] = 0; - out_5603231198807145091[27] = 0; - out_5603231198807145091[28] = 0; - out_5603231198807145091[29] = 0; - out_5603231198807145091[30] = 0; - out_5603231198807145091[31] = 0; - out_5603231198807145091[32] = 0; - out_5603231198807145091[33] = 0; - out_5603231198807145091[34] = 0; - out_5603231198807145091[35] = 0; - out_5603231198807145091[36] = 0; - out_5603231198807145091[37] = 0; - out_5603231198807145091[38] = 0; - out_5603231198807145091[39] = 0; - out_5603231198807145091[40] = 0; - out_5603231198807145091[41] = 0; - out_5603231198807145091[42] = 0; - out_5603231198807145091[43] = 0; - out_5603231198807145091[44] = 1; - out_5603231198807145091[45] = 0; - out_5603231198807145091[46] = 0; - out_5603231198807145091[47] = 0; - out_5603231198807145091[48] = 0; - out_5603231198807145091[49] = 0; - out_5603231198807145091[50] = 0; - out_5603231198807145091[51] = 0; - out_5603231198807145091[52] = 0; - out_5603231198807145091[53] = 0; +void H_14(double *state, double *unused, double *out_2470898759543393552) { + out_2470898759543393552[0] = 0; + out_2470898759543393552[1] = 0; + out_2470898759543393552[2] = 0; + out_2470898759543393552[3] = 0; + out_2470898759543393552[4] = 0; + out_2470898759543393552[5] = 0; + out_2470898759543393552[6] = 1; + out_2470898759543393552[7] = 0; + out_2470898759543393552[8] = 0; + out_2470898759543393552[9] = 0; + out_2470898759543393552[10] = 0; + out_2470898759543393552[11] = 0; + out_2470898759543393552[12] = 0; + out_2470898759543393552[13] = 0; + out_2470898759543393552[14] = 0; + out_2470898759543393552[15] = 0; + out_2470898759543393552[16] = 0; + out_2470898759543393552[17] = 0; + out_2470898759543393552[18] = 0; + out_2470898759543393552[19] = 0; + out_2470898759543393552[20] = 0; + out_2470898759543393552[21] = 0; + out_2470898759543393552[22] = 0; + out_2470898759543393552[23] = 0; + out_2470898759543393552[24] = 0; + out_2470898759543393552[25] = 1; + out_2470898759543393552[26] = 0; + out_2470898759543393552[27] = 0; + out_2470898759543393552[28] = 0; + out_2470898759543393552[29] = 0; + out_2470898759543393552[30] = 0; + out_2470898759543393552[31] = 0; + out_2470898759543393552[32] = 0; + out_2470898759543393552[33] = 0; + out_2470898759543393552[34] = 0; + out_2470898759543393552[35] = 0; + out_2470898759543393552[36] = 0; + out_2470898759543393552[37] = 0; + out_2470898759543393552[38] = 0; + out_2470898759543393552[39] = 0; + out_2470898759543393552[40] = 0; + out_2470898759543393552[41] = 0; + out_2470898759543393552[42] = 0; + out_2470898759543393552[43] = 0; + out_2470898759543393552[44] = 1; + out_2470898759543393552[45] = 0; + out_2470898759543393552[46] = 0; + out_2470898759543393552[47] = 0; + out_2470898759543393552[48] = 0; + out_2470898759543393552[49] = 0; + out_2470898759543393552[50] = 0; + out_2470898759543393552[51] = 0; + out_2470898759543393552[52] = 0; + out_2470898759543393552[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_6269171790765846050) { - err_fun(nom_x, delta_x, out_6269171790765846050); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6395110133292861521) { + err_fun(nom_x, delta_x, out_6395110133292861521); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1497934558633100347) { - inv_err_fun(nom_x, true_x, out_1497934558633100347); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5574902989131587335) { + inv_err_fun(nom_x, true_x, out_5574902989131587335); } -void pose_H_mod_fun(double *state, double *out_7862392065184069712) { - H_mod_fun(state, out_7862392065184069712); +void pose_H_mod_fun(double *state, double *out_6860086072787592800) { + H_mod_fun(state, out_6860086072787592800); } -void pose_f_fun(double *state, double dt, double *out_4855847476055627778) { - f_fun(state, dt, out_4855847476055627778); +void pose_f_fun(double *state, double dt, double *out_6157473931416548839) { + f_fun(state, dt, out_6157473931416548839); } -void pose_F_fun(double *state, double dt, double *out_1703318423524786966) { - F_fun(state, dt, out_1703318423524786966); +void pose_F_fun(double *state, double dt, double *out_2563232569638129684) { + F_fun(state, dt, out_2563232569638129684); } -void pose_h_4(double *state, double *unused, double *out_7821603690376207235) { - h_4(state, unused, out_7821603690376207235); +void pose_h_4(double *state, double *unused, double *out_9057789463521636950) { + h_4(state, unused, out_9057789463521636950); } -void pose_H_4(double *state, double *unused, double *out_1639990342467660562) { - H_4(state, unused, out_1639990342467660562); +void pose_H_4(double *state, double *unused, double *out_6434139615882878081) { + H_4(state, unused, out_6434139615882878081); } -void pose_h_10(double *state, double *unused, double *out_4627608962100745909) { - h_10(state, unused, out_4627608962100745909); +void pose_h_10(double *state, double *unused, double *out_439040734435285181) { + h_10(state, unused, out_439040734435285181); } -void pose_H_10(double *state, double *unused, double *out_5548344007433806154) { - H_10(state, unused, out_5548344007433806154); +void pose_H_10(double *state, double *unused, double *out_1840785913394498883) { + H_10(state, unused, out_1840785913394498883); } -void pose_h_13(double *state, double *unused, double *out_5105456950148742445) { - h_13(state, unused, out_5105456950148742445); +void pose_h_13(double *state, double *unused, double *out_7181492765475252926) { + h_13(state, unused, out_7181492765475252926); } -void pose_H_13(double *state, double *unused, double *out_4852264167799993363) { - H_13(state, unused, out_4852264167799993363); +void pose_H_13(double *state, double *unused, double *out_3221865790550545280) { + H_13(state, unused, out_3221865790550545280); } -void pose_h_14(double *state, double *unused, double *out_7814084012328434224) { - h_14(state, unused, out_7814084012328434224); +void pose_h_14(double *state, double *unused, double *out_1398557426791028034) { + h_14(state, unused, out_1398557426791028034); } -void pose_H_14(double *state, double *unused, double *out_5603231198807145091) { - H_14(state, unused, out_5603231198807145091); +void pose_H_14(double *state, double *unused, double *out_2470898759543393552) { + H_14(state, unused, out_2470898759543393552); } 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 676d89a82..ba6e3f588 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_6269171790765846050); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1497934558633100347); -void pose_H_mod_fun(double *state, double *out_7862392065184069712); -void pose_f_fun(double *state, double dt, double *out_4855847476055627778); -void pose_F_fun(double *state, double dt, double *out_1703318423524786966); -void pose_h_4(double *state, double *unused, double *out_7821603690376207235); -void pose_H_4(double *state, double *unused, double *out_1639990342467660562); -void pose_h_10(double *state, double *unused, double *out_4627608962100745909); -void pose_H_10(double *state, double *unused, double *out_5548344007433806154); -void pose_h_13(double *state, double *unused, double *out_5105456950148742445); -void pose_H_13(double *state, double *unused, double *out_4852264167799993363); -void pose_h_14(double *state, double *unused, double *out_7814084012328434224); -void pose_H_14(double *state, double *unused, double *out_5603231198807145091); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6395110133292861521); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5574902989131587335); +void pose_H_mod_fun(double *state, double *out_6860086072787592800); +void pose_f_fun(double *state, double dt, double *out_6157473931416548839); +void pose_F_fun(double *state, double dt, double *out_2563232569638129684); +void pose_h_4(double *state, double *unused, double *out_9057789463521636950); +void pose_H_4(double *state, double *unused, double *out_6434139615882878081); +void pose_h_10(double *state, double *unused, double *out_439040734435285181); +void pose_H_10(double *state, double *unused, double *out_1840785913394498883); +void pose_h_13(double *state, double *unused, double *out_7181492765475252926); +void pose_H_13(double *state, double *unused, double *out_3221865790550545280); +void pose_h_14(double *state, double *unused, double *out_1398557426791028034); +void pose_H_14(double *state, double *unused, double *out_2470898759543393552); 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 069250970..ce99870b4 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 9857117af..83c3311af 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 0f997353a..41db831bb 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 63cbebcc7..34cbc1f15 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 55ff8c610..a6069a2de 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index a9c4e3fbd..c6627a465 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ