diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index a5d1d2ea9..d29cf66a8 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 6cef5fc63..a9282164c 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 963077ccd..85fb1535c 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 e8e57a38c..5d7efaa1c 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 162c3d1a7..6e0e38479 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 714da20d1..8da8c0a49 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 5047381d8..8d495eb75 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 5047381d8..8d495eb75 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 407ff73e8..2d007c023 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 714da20d1..8da8c0a49 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 462f65018..b5ce301c0 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-9b97124f-DEBUG"; +const uint8_t gitversion[19] = "DEV-d1fad265-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index a8621bbc7..cf4de4556 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 f33350d60..4f14f33d5 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 042acaf07..2a2567760 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 8add8df67..228469f6c 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 32328987c..d5da7ae40 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 e09443cdd..e4dedb0c5 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 e054b3b52..f1536824e 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 2509127bc..32d77ca62 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 53e13c030..8c33bc7a7 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 e5f80ca34..06cd6099a 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 fef5be014..84e479941 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 9bb7919ab..b1bf66ff1 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 37a33ce78..71d13fe40 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 cec4332a4..1456d83a6 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 e0371050a..f8cf09fc0 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 4f96cb291..e9e048afe 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 17b71b38d..5cb1aa8cf 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 32e909937..7a55c1485 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 0047e1556..5aef14f47 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 f3d1cadab..1ba0c42d1 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 4aa67d8c0..030126d9b 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-9b97124f-DEBUG \ No newline at end of file +DEV-d1fad265-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 99d82d97f..74ad2402d 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_4611641411917091701) { - out_4611641411917091701[0] = delta_x[0] + nom_x[0]; - out_4611641411917091701[1] = delta_x[1] + nom_x[1]; - out_4611641411917091701[2] = delta_x[2] + nom_x[2]; - out_4611641411917091701[3] = delta_x[3] + nom_x[3]; - out_4611641411917091701[4] = delta_x[4] + nom_x[4]; - out_4611641411917091701[5] = delta_x[5] + nom_x[5]; - out_4611641411917091701[6] = delta_x[6] + nom_x[6]; - out_4611641411917091701[7] = delta_x[7] + nom_x[7]; - out_4611641411917091701[8] = delta_x[8] + nom_x[8]; +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 inv_err_fun(double *nom_x, double *true_x, double *out_3968532722392907058) { - out_3968532722392907058[0] = -nom_x[0] + true_x[0]; - out_3968532722392907058[1] = -nom_x[1] + true_x[1]; - out_3968532722392907058[2] = -nom_x[2] + true_x[2]; - out_3968532722392907058[3] = -nom_x[3] + true_x[3]; - out_3968532722392907058[4] = -nom_x[4] + true_x[4]; - out_3968532722392907058[5] = -nom_x[5] + true_x[5]; - out_3968532722392907058[6] = -nom_x[6] + true_x[6]; - out_3968532722392907058[7] = -nom_x[7] + true_x[7]; - out_3968532722392907058[8] = -nom_x[8] + true_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 H_mod_fun(double *state, double *out_1232909156344202498) { - out_1232909156344202498[0] = 1.0; - out_1232909156344202498[1] = 0.0; - out_1232909156344202498[2] = 0.0; - out_1232909156344202498[3] = 0.0; - out_1232909156344202498[4] = 0.0; - out_1232909156344202498[5] = 0.0; - out_1232909156344202498[6] = 0.0; - out_1232909156344202498[7] = 0.0; - out_1232909156344202498[8] = 0.0; - out_1232909156344202498[9] = 0.0; - out_1232909156344202498[10] = 1.0; - out_1232909156344202498[11] = 0.0; - out_1232909156344202498[12] = 0.0; - out_1232909156344202498[13] = 0.0; - out_1232909156344202498[14] = 0.0; - out_1232909156344202498[15] = 0.0; - out_1232909156344202498[16] = 0.0; - out_1232909156344202498[17] = 0.0; - out_1232909156344202498[18] = 0.0; - out_1232909156344202498[19] = 0.0; - out_1232909156344202498[20] = 1.0; - out_1232909156344202498[21] = 0.0; - out_1232909156344202498[22] = 0.0; - out_1232909156344202498[23] = 0.0; - out_1232909156344202498[24] = 0.0; - out_1232909156344202498[25] = 0.0; - out_1232909156344202498[26] = 0.0; - out_1232909156344202498[27] = 0.0; - out_1232909156344202498[28] = 0.0; - out_1232909156344202498[29] = 0.0; - out_1232909156344202498[30] = 1.0; - out_1232909156344202498[31] = 0.0; - out_1232909156344202498[32] = 0.0; - out_1232909156344202498[33] = 0.0; - out_1232909156344202498[34] = 0.0; - out_1232909156344202498[35] = 0.0; - out_1232909156344202498[36] = 0.0; - out_1232909156344202498[37] = 0.0; - out_1232909156344202498[38] = 0.0; - out_1232909156344202498[39] = 0.0; - out_1232909156344202498[40] = 1.0; - out_1232909156344202498[41] = 0.0; - out_1232909156344202498[42] = 0.0; - out_1232909156344202498[43] = 0.0; - out_1232909156344202498[44] = 0.0; - out_1232909156344202498[45] = 0.0; - out_1232909156344202498[46] = 0.0; - out_1232909156344202498[47] = 0.0; - out_1232909156344202498[48] = 0.0; - out_1232909156344202498[49] = 0.0; - out_1232909156344202498[50] = 1.0; - out_1232909156344202498[51] = 0.0; - out_1232909156344202498[52] = 0.0; - out_1232909156344202498[53] = 0.0; - out_1232909156344202498[54] = 0.0; - out_1232909156344202498[55] = 0.0; - out_1232909156344202498[56] = 0.0; - out_1232909156344202498[57] = 0.0; - out_1232909156344202498[58] = 0.0; - out_1232909156344202498[59] = 0.0; - out_1232909156344202498[60] = 1.0; - out_1232909156344202498[61] = 0.0; - out_1232909156344202498[62] = 0.0; - out_1232909156344202498[63] = 0.0; - out_1232909156344202498[64] = 0.0; - out_1232909156344202498[65] = 0.0; - out_1232909156344202498[66] = 0.0; - out_1232909156344202498[67] = 0.0; - out_1232909156344202498[68] = 0.0; - out_1232909156344202498[69] = 0.0; - out_1232909156344202498[70] = 1.0; - out_1232909156344202498[71] = 0.0; - out_1232909156344202498[72] = 0.0; - out_1232909156344202498[73] = 0.0; - out_1232909156344202498[74] = 0.0; - out_1232909156344202498[75] = 0.0; - out_1232909156344202498[76] = 0.0; - out_1232909156344202498[77] = 0.0; - out_1232909156344202498[78] = 0.0; - out_1232909156344202498[79] = 0.0; - out_1232909156344202498[80] = 1.0; +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 f_fun(double *state, double dt, double *out_241923337051056220) { - out_241923337051056220[0] = state[0]; - out_241923337051056220[1] = state[1]; - out_241923337051056220[2] = state[2]; - out_241923337051056220[3] = state[3]; - out_241923337051056220[4] = state[4]; - out_241923337051056220[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_241923337051056220[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_241923337051056220[7] = state[7]; - out_241923337051056220[8] = state[8]; +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_2984795699500747305) { - out_2984795699500747305[0] = 1; - out_2984795699500747305[1] = 0; - out_2984795699500747305[2] = 0; - out_2984795699500747305[3] = 0; - out_2984795699500747305[4] = 0; - out_2984795699500747305[5] = 0; - out_2984795699500747305[6] = 0; - out_2984795699500747305[7] = 0; - out_2984795699500747305[8] = 0; - out_2984795699500747305[9] = 0; - out_2984795699500747305[10] = 1; - out_2984795699500747305[11] = 0; - out_2984795699500747305[12] = 0; - out_2984795699500747305[13] = 0; - out_2984795699500747305[14] = 0; - out_2984795699500747305[15] = 0; - out_2984795699500747305[16] = 0; - out_2984795699500747305[17] = 0; - out_2984795699500747305[18] = 0; - out_2984795699500747305[19] = 0; - out_2984795699500747305[20] = 1; - out_2984795699500747305[21] = 0; - out_2984795699500747305[22] = 0; - out_2984795699500747305[23] = 0; - out_2984795699500747305[24] = 0; - out_2984795699500747305[25] = 0; - out_2984795699500747305[26] = 0; - out_2984795699500747305[27] = 0; - out_2984795699500747305[28] = 0; - out_2984795699500747305[29] = 0; - out_2984795699500747305[30] = 1; - out_2984795699500747305[31] = 0; - out_2984795699500747305[32] = 0; - out_2984795699500747305[33] = 0; - out_2984795699500747305[34] = 0; - out_2984795699500747305[35] = 0; - out_2984795699500747305[36] = 0; - out_2984795699500747305[37] = 0; - out_2984795699500747305[38] = 0; - out_2984795699500747305[39] = 0; - out_2984795699500747305[40] = 1; - out_2984795699500747305[41] = 0; - out_2984795699500747305[42] = 0; - out_2984795699500747305[43] = 0; - out_2984795699500747305[44] = 0; - out_2984795699500747305[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_2984795699500747305[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_2984795699500747305[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2984795699500747305[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_2984795699500747305[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_2984795699500747305[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_2984795699500747305[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_2984795699500747305[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_2984795699500747305[53] = -9.8100000000000005*dt; - out_2984795699500747305[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_2984795699500747305[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_2984795699500747305[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2984795699500747305[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2984795699500747305[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_2984795699500747305[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_2984795699500747305[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_2984795699500747305[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_2984795699500747305[62] = 0; - out_2984795699500747305[63] = 0; - out_2984795699500747305[64] = 0; - out_2984795699500747305[65] = 0; - out_2984795699500747305[66] = 0; - out_2984795699500747305[67] = 0; - out_2984795699500747305[68] = 0; - out_2984795699500747305[69] = 0; - out_2984795699500747305[70] = 1; - out_2984795699500747305[71] = 0; - out_2984795699500747305[72] = 0; - out_2984795699500747305[73] = 0; - out_2984795699500747305[74] = 0; - out_2984795699500747305[75] = 0; - out_2984795699500747305[76] = 0; - out_2984795699500747305[77] = 0; - out_2984795699500747305[78] = 0; - out_2984795699500747305[79] = 0; - out_2984795699500747305[80] = 1; +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 h_25(double *state, double *unused, double *out_4848168669919202886) { - out_4848168669919202886[0] = state[6]; +void h_25(double *state, double *unused, double *out_8788151648350399353) { + out_8788151648350399353[0] = state[6]; } -void H_25(double *state, double *unused, double *out_7960279069568034739) { - out_7960279069568034739[0] = 0; - out_7960279069568034739[1] = 0; - out_7960279069568034739[2] = 0; - out_7960279069568034739[3] = 0; - out_7960279069568034739[4] = 0; - out_7960279069568034739[5] = 0; - out_7960279069568034739[6] = 1; - out_7960279069568034739[7] = 0; - out_7960279069568034739[8] = 0; +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_24(double *state, double *unused, double *out_5973332433336352466) { - out_5973332433336352466[0] = state[4]; - out_5973332433336352466[1] = state[5]; +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_8313815405136017311) { - out_8313815405136017311[0] = 0; - out_8313815405136017311[1] = 0; - out_8313815405136017311[2] = 0; - out_8313815405136017311[3] = 0; - out_8313815405136017311[4] = 1; - out_8313815405136017311[5] = 0; - out_8313815405136017311[6] = 0; - out_8313815405136017311[7] = 0; - out_8313815405136017311[8] = 0; - out_8313815405136017311[9] = 0; - out_8313815405136017311[10] = 0; - out_8313815405136017311[11] = 0; - out_8313815405136017311[12] = 0; - out_8313815405136017311[13] = 0; - out_8313815405136017311[14] = 1; - out_8313815405136017311[15] = 0; - out_8313815405136017311[16] = 0; - out_8313815405136017311[17] = 0; +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_30(double *state, double *unused, double *out_7443128207949688601) { - out_7443128207949688601[0] = state[4]; +void h_30(double *state, double *unused, double *out_3908301040633649848) { + out_3908301040633649848[0] = state[4]; } -void H_30(double *state, double *unused, double *out_5441946111060786112) { - out_5441946111060786112[0] = 0; - out_5441946111060786112[1] = 0; - out_5441946111060786112[2] = 0; - out_5441946111060786112[3] = 0; - out_5441946111060786112[4] = 1; - out_5441946111060786112[5] = 0; - out_5441946111060786112[6] = 0; - out_5441946111060786112[7] = 0; - out_5441946111060786112[8] = 0; +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_26(double *state, double *unused, double *out_5333477160662303659) { - out_5333477160662303659[0] = state[7]; +void h_26(double *state, double *unused, double *out_8870681038314943080) { + out_8870681038314943080[0] = state[7]; } -void H_26(double *state, double *unused, double *out_6744961685267460653) { - out_6744961685267460653[0] = 0; - out_6744961685267460653[1] = 0; - out_6744961685267460653[2] = 0; - out_6744961685267460653[3] = 0; - out_6744961685267460653[4] = 0; - out_6744961685267460653[5] = 0; - out_6744961685267460653[6] = 0; - out_6744961685267460653[7] = 1; - out_6744961685267460653[8] = 0; +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_27(double *state, double *unused, double *out_5010412721985507322) { - out_5010412721985507322[0] = state[3]; +void h_27(double *state, double *unused, double *out_8988688432248319824) { + out_8988688432248319824[0] = state[3]; } -void H_27(double *state, double *unused, double *out_8182362745197851896) { - out_8182362745197851896[0] = 0; - out_8182362745197851896[1] = 0; - out_8182362745197851896[2] = 0; - out_8182362745197851896[3] = 1; - out_8182362745197851896[4] = 0; - out_8182362745197851896[5] = 0; - out_8182362745197851896[6] = 0; - out_8182362745197851896[7] = 0; - out_8182362745197851896[8] = 0; +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_29(double *state, double *unused, double *out_8399210544951165364) { - out_8399210544951165364[0] = state[1]; +void h_29(double *state, double *unused, double *out_149028852435531358) { + out_149028852435531358[0] = state[1]; } -void H_29(double *state, double *unused, double *out_6469000018328300863) { - out_6469000018328300863[0] = 0; - out_6469000018328300863[1] = 1; - out_6469000018328300863[2] = 0; - out_6469000018328300863[3] = 0; - out_6469000018328300863[4] = 0; - out_6469000018328300863[5] = 0; - out_6469000018328300863[6] = 0; - out_6469000018328300863[7] = 0; - out_6469000018328300863[8] = 0; +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_28(double *state, double *unused, double *out_2638932451325827020) { - out_2638932451325827020[0] = state[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_8432630289893627114) { - out_8432630289893627114[0] = 1; - out_8432630289893627114[1] = 0; - out_8432630289893627114[2] = 0; - out_8432630289893627114[3] = 0; - out_8432630289893627114[4] = 0; - out_8432630289893627114[5] = 0; - out_8432630289893627114[6] = 0; - out_8432630289893627114[7] = 0; - out_8432630289893627114[8] = 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_31(double *state, double *unused, double *out_4572974607634696997) { - out_4572974607634696997[0] = state[8]; +void h_31(double *state, double *unused, double *out_7610421138374421687) { + out_7610421138374421687[0] = state[8]; } -void H_31(double *state, double *unused, double *out_6118753583034109177) { - out_6118753583034109177[0] = 0; - out_6118753583034109177[1] = 0; - out_6118753583034109177[2] = 0; - out_6118753583034109177[3] = 0; - out_6118753583034109177[4] = 0; - out_6118753583034109177[5] = 0; - out_6118753583034109177[6] = 0; - out_6118753583034109177[7] = 0; - out_6118753583034109177[8] = 1; +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; } #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_4611641411917091701) { - err_fun(nom_x, delta_x, out_4611641411917091701); +void car_err_fun(double *nom_x, double *delta_x, double *out_8137803062968279058) { + err_fun(nom_x, delta_x, out_8137803062968279058); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3968532722392907058) { - inv_err_fun(nom_x, true_x, out_3968532722392907058); +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_H_mod_fun(double *state, double *out_1232909156344202498) { - H_mod_fun(state, out_1232909156344202498); +void car_H_mod_fun(double *state, double *out_250780943344312618) { + H_mod_fun(state, out_250780943344312618); } -void car_f_fun(double *state, double dt, double *out_241923337051056220) { - f_fun(state, dt, out_241923337051056220); +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_2984795699500747305) { - F_fun(state, dt, out_2984795699500747305); +void car_F_fun(double *state, double dt, double *out_3517018389212170475) { + F_fun(state, dt, out_3517018389212170475); } -void car_h_25(double *state, double *unused, double *out_4848168669919202886) { - h_25(state, unused, out_4848168669919202886); +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_7960279069568034739) { - H_25(state, unused, out_7960279069568034739); +void car_H_25(double *state, double *unused, double *out_3773761593333804295) { + H_25(state, unused, out_3773761593333804295); } -void car_h_24(double *state, double *unused, double *out_5973332433336352466) { - h_24(state, unused, out_5973332433336352466); +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_8313815405136017311) { - H_24(state, unused, out_8313815405136017311); +void car_H_24(double *state, double *unused, double *out_5950976016940954268) { + H_24(state, unused, out_5950976016940954268); } -void car_h_30(double *state, double *unused, double *out_7443128207949688601) { - h_30(state, unused, out_7443128207949688601); +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_5441946111060786112) { - H_30(state, unused, out_5441946111060786112); +void car_H_30(double *state, double *unused, double *out_6292094551841052922) { + H_30(state, unused, out_6292094551841052922); } -void car_h_26(double *state, double *unused, double *out_5333477160662303659) { - h_26(state, unused, out_5333477160662303659); +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_6744961685267460653) { - H_26(state, unused, out_6744961685267460653); +void car_H_26(double *state, double *unused, double *out_32258274459748071) { + H_26(state, unused, out_32258274459748071); } -void car_h_27(double *state, double *unused, double *out_5010412721985507322) { - h_27(state, unused, out_5010412721985507322); +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_8182362745197851896) { - H_27(state, unused, out_8182362745197851896); +void car_H_27(double *state, double *unused, double *out_1469659334390139314) { + H_27(state, unused, out_1469659334390139314); } -void car_h_29(double *state, double *unused, double *out_8399210544951165364) { - h_29(state, unused, out_8399210544951165364); +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_6469000018328300863) { - H_29(state, unused, out_6469000018328300863); +void car_H_29(double *state, double *unused, double *out_243703392479411719) { + H_29(state, unused, out_243703392479411719); } -void car_h_28(double *state, double *unused, double *out_2638932451325827020) { - h_28(state, unused, out_2638932451325827020); +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_8432630289893627114) { - H_28(state, unused, out_8432630289893627114); +void car_H_28(double *state, double *unused, double *out_1719926879085914532) { + H_28(state, unused, out_1719926879085914532); } -void car_h_31(double *state, double *unused, double *out_4572974607634696997) { - h_31(state, unused, out_4572974607634696997); +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_6118753583034109177) { - H_31(state, unused, out_6118753583034109177); +void car_H_31(double *state, double *unused, double *out_3804407555210764723) { + H_31(state, unused, out_3804407555210764723); } 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 2c38541f2..a06ca6eb2 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_4611641411917091701); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3968532722392907058); -void car_H_mod_fun(double *state, double *out_1232909156344202498); -void car_f_fun(double *state, double dt, double *out_241923337051056220); -void car_F_fun(double *state, double dt, double *out_2984795699500747305); -void car_h_25(double *state, double *unused, double *out_4848168669919202886); -void car_H_25(double *state, double *unused, double *out_7960279069568034739); -void car_h_24(double *state, double *unused, double *out_5973332433336352466); -void car_H_24(double *state, double *unused, double *out_8313815405136017311); -void car_h_30(double *state, double *unused, double *out_7443128207949688601); -void car_H_30(double *state, double *unused, double *out_5441946111060786112); -void car_h_26(double *state, double *unused, double *out_5333477160662303659); -void car_H_26(double *state, double *unused, double *out_6744961685267460653); -void car_h_27(double *state, double *unused, double *out_5010412721985507322); -void car_H_27(double *state, double *unused, double *out_8182362745197851896); -void car_h_29(double *state, double *unused, double *out_8399210544951165364); -void car_H_29(double *state, double *unused, double *out_6469000018328300863); -void car_h_28(double *state, double *unused, double *out_2638932451325827020); -void car_H_28(double *state, double *unused, double *out_8432630289893627114); -void car_h_31(double *state, double *unused, double *out_4572974607634696997); -void car_H_31(double *state, double *unused, double *out_6118753583034109177); +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_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 ac77c54ae..652de8634 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_8612662017984698055) { - out_8612662017984698055[0] = delta_x[0] + nom_x[0]; - out_8612662017984698055[1] = delta_x[1] + nom_x[1]; - out_8612662017984698055[2] = delta_x[2] + nom_x[2]; - out_8612662017984698055[3] = delta_x[3] + nom_x[3]; - out_8612662017984698055[4] = delta_x[4] + nom_x[4]; - out_8612662017984698055[5] = delta_x[5] + nom_x[5]; - out_8612662017984698055[6] = delta_x[6] + nom_x[6]; - out_8612662017984698055[7] = delta_x[7] + nom_x[7]; - out_8612662017984698055[8] = delta_x[8] + nom_x[8]; - out_8612662017984698055[9] = delta_x[9] + nom_x[9]; - out_8612662017984698055[10] = delta_x[10] + nom_x[10]; - out_8612662017984698055[11] = delta_x[11] + nom_x[11]; - out_8612662017984698055[12] = delta_x[12] + nom_x[12]; - out_8612662017984698055[13] = delta_x[13] + nom_x[13]; - out_8612662017984698055[14] = delta_x[14] + nom_x[14]; - out_8612662017984698055[15] = delta_x[15] + nom_x[15]; - out_8612662017984698055[16] = delta_x[16] + nom_x[16]; - out_8612662017984698055[17] = delta_x[17] + nom_x[17]; +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 inv_err_fun(double *nom_x, double *true_x, double *out_6583538545281744086) { - out_6583538545281744086[0] = -nom_x[0] + true_x[0]; - out_6583538545281744086[1] = -nom_x[1] + true_x[1]; - out_6583538545281744086[2] = -nom_x[2] + true_x[2]; - out_6583538545281744086[3] = -nom_x[3] + true_x[3]; - out_6583538545281744086[4] = -nom_x[4] + true_x[4]; - out_6583538545281744086[5] = -nom_x[5] + true_x[5]; - out_6583538545281744086[6] = -nom_x[6] + true_x[6]; - out_6583538545281744086[7] = -nom_x[7] + true_x[7]; - out_6583538545281744086[8] = -nom_x[8] + true_x[8]; - out_6583538545281744086[9] = -nom_x[9] + true_x[9]; - out_6583538545281744086[10] = -nom_x[10] + true_x[10]; - out_6583538545281744086[11] = -nom_x[11] + true_x[11]; - out_6583538545281744086[12] = -nom_x[12] + true_x[12]; - out_6583538545281744086[13] = -nom_x[13] + true_x[13]; - out_6583538545281744086[14] = -nom_x[14] + true_x[14]; - out_6583538545281744086[15] = -nom_x[15] + true_x[15]; - out_6583538545281744086[16] = -nom_x[16] + true_x[16]; - out_6583538545281744086[17] = -nom_x[17] + true_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 H_mod_fun(double *state, double *out_7537065087244600898) { - out_7537065087244600898[0] = 1.0; - out_7537065087244600898[1] = 0.0; - out_7537065087244600898[2] = 0.0; - out_7537065087244600898[3] = 0.0; - out_7537065087244600898[4] = 0.0; - out_7537065087244600898[5] = 0.0; - out_7537065087244600898[6] = 0.0; - out_7537065087244600898[7] = 0.0; - out_7537065087244600898[8] = 0.0; - out_7537065087244600898[9] = 0.0; - out_7537065087244600898[10] = 0.0; - out_7537065087244600898[11] = 0.0; - out_7537065087244600898[12] = 0.0; - out_7537065087244600898[13] = 0.0; - out_7537065087244600898[14] = 0.0; - out_7537065087244600898[15] = 0.0; - out_7537065087244600898[16] = 0.0; - out_7537065087244600898[17] = 0.0; - out_7537065087244600898[18] = 0.0; - out_7537065087244600898[19] = 1.0; - out_7537065087244600898[20] = 0.0; - out_7537065087244600898[21] = 0.0; - out_7537065087244600898[22] = 0.0; - out_7537065087244600898[23] = 0.0; - out_7537065087244600898[24] = 0.0; - out_7537065087244600898[25] = 0.0; - out_7537065087244600898[26] = 0.0; - out_7537065087244600898[27] = 0.0; - out_7537065087244600898[28] = 0.0; - out_7537065087244600898[29] = 0.0; - out_7537065087244600898[30] = 0.0; - out_7537065087244600898[31] = 0.0; - out_7537065087244600898[32] = 0.0; - out_7537065087244600898[33] = 0.0; - out_7537065087244600898[34] = 0.0; - out_7537065087244600898[35] = 0.0; - out_7537065087244600898[36] = 0.0; - out_7537065087244600898[37] = 0.0; - out_7537065087244600898[38] = 1.0; - out_7537065087244600898[39] = 0.0; - out_7537065087244600898[40] = 0.0; - out_7537065087244600898[41] = 0.0; - out_7537065087244600898[42] = 0.0; - out_7537065087244600898[43] = 0.0; - out_7537065087244600898[44] = 0.0; - out_7537065087244600898[45] = 0.0; - out_7537065087244600898[46] = 0.0; - out_7537065087244600898[47] = 0.0; - out_7537065087244600898[48] = 0.0; - out_7537065087244600898[49] = 0.0; - out_7537065087244600898[50] = 0.0; - out_7537065087244600898[51] = 0.0; - out_7537065087244600898[52] = 0.0; - out_7537065087244600898[53] = 0.0; - out_7537065087244600898[54] = 0.0; - out_7537065087244600898[55] = 0.0; - out_7537065087244600898[56] = 0.0; - out_7537065087244600898[57] = 1.0; - out_7537065087244600898[58] = 0.0; - out_7537065087244600898[59] = 0.0; - out_7537065087244600898[60] = 0.0; - out_7537065087244600898[61] = 0.0; - out_7537065087244600898[62] = 0.0; - out_7537065087244600898[63] = 0.0; - out_7537065087244600898[64] = 0.0; - out_7537065087244600898[65] = 0.0; - out_7537065087244600898[66] = 0.0; - out_7537065087244600898[67] = 0.0; - out_7537065087244600898[68] = 0.0; - out_7537065087244600898[69] = 0.0; - out_7537065087244600898[70] = 0.0; - out_7537065087244600898[71] = 0.0; - out_7537065087244600898[72] = 0.0; - out_7537065087244600898[73] = 0.0; - out_7537065087244600898[74] = 0.0; - out_7537065087244600898[75] = 0.0; - out_7537065087244600898[76] = 1.0; - out_7537065087244600898[77] = 0.0; - out_7537065087244600898[78] = 0.0; - out_7537065087244600898[79] = 0.0; - out_7537065087244600898[80] = 0.0; - out_7537065087244600898[81] = 0.0; - out_7537065087244600898[82] = 0.0; - out_7537065087244600898[83] = 0.0; - out_7537065087244600898[84] = 0.0; - out_7537065087244600898[85] = 0.0; - out_7537065087244600898[86] = 0.0; - out_7537065087244600898[87] = 0.0; - out_7537065087244600898[88] = 0.0; - out_7537065087244600898[89] = 0.0; - out_7537065087244600898[90] = 0.0; - out_7537065087244600898[91] = 0.0; - out_7537065087244600898[92] = 0.0; - out_7537065087244600898[93] = 0.0; - out_7537065087244600898[94] = 0.0; - out_7537065087244600898[95] = 1.0; - out_7537065087244600898[96] = 0.0; - out_7537065087244600898[97] = 0.0; - out_7537065087244600898[98] = 0.0; - out_7537065087244600898[99] = 0.0; - out_7537065087244600898[100] = 0.0; - out_7537065087244600898[101] = 0.0; - out_7537065087244600898[102] = 0.0; - out_7537065087244600898[103] = 0.0; - out_7537065087244600898[104] = 0.0; - out_7537065087244600898[105] = 0.0; - out_7537065087244600898[106] = 0.0; - out_7537065087244600898[107] = 0.0; - out_7537065087244600898[108] = 0.0; - out_7537065087244600898[109] = 0.0; - out_7537065087244600898[110] = 0.0; - out_7537065087244600898[111] = 0.0; - out_7537065087244600898[112] = 0.0; - out_7537065087244600898[113] = 0.0; - out_7537065087244600898[114] = 1.0; - out_7537065087244600898[115] = 0.0; - out_7537065087244600898[116] = 0.0; - out_7537065087244600898[117] = 0.0; - out_7537065087244600898[118] = 0.0; - out_7537065087244600898[119] = 0.0; - out_7537065087244600898[120] = 0.0; - out_7537065087244600898[121] = 0.0; - out_7537065087244600898[122] = 0.0; - out_7537065087244600898[123] = 0.0; - out_7537065087244600898[124] = 0.0; - out_7537065087244600898[125] = 0.0; - out_7537065087244600898[126] = 0.0; - out_7537065087244600898[127] = 0.0; - out_7537065087244600898[128] = 0.0; - out_7537065087244600898[129] = 0.0; - out_7537065087244600898[130] = 0.0; - out_7537065087244600898[131] = 0.0; - out_7537065087244600898[132] = 0.0; - out_7537065087244600898[133] = 1.0; - out_7537065087244600898[134] = 0.0; - out_7537065087244600898[135] = 0.0; - out_7537065087244600898[136] = 0.0; - out_7537065087244600898[137] = 0.0; - out_7537065087244600898[138] = 0.0; - out_7537065087244600898[139] = 0.0; - out_7537065087244600898[140] = 0.0; - out_7537065087244600898[141] = 0.0; - out_7537065087244600898[142] = 0.0; - out_7537065087244600898[143] = 0.0; - out_7537065087244600898[144] = 0.0; - out_7537065087244600898[145] = 0.0; - out_7537065087244600898[146] = 0.0; - out_7537065087244600898[147] = 0.0; - out_7537065087244600898[148] = 0.0; - out_7537065087244600898[149] = 0.0; - out_7537065087244600898[150] = 0.0; - out_7537065087244600898[151] = 0.0; - out_7537065087244600898[152] = 1.0; - out_7537065087244600898[153] = 0.0; - out_7537065087244600898[154] = 0.0; - out_7537065087244600898[155] = 0.0; - out_7537065087244600898[156] = 0.0; - out_7537065087244600898[157] = 0.0; - out_7537065087244600898[158] = 0.0; - out_7537065087244600898[159] = 0.0; - out_7537065087244600898[160] = 0.0; - out_7537065087244600898[161] = 0.0; - out_7537065087244600898[162] = 0.0; - out_7537065087244600898[163] = 0.0; - out_7537065087244600898[164] = 0.0; - out_7537065087244600898[165] = 0.0; - out_7537065087244600898[166] = 0.0; - out_7537065087244600898[167] = 0.0; - out_7537065087244600898[168] = 0.0; - out_7537065087244600898[169] = 0.0; - out_7537065087244600898[170] = 0.0; - out_7537065087244600898[171] = 1.0; - out_7537065087244600898[172] = 0.0; - out_7537065087244600898[173] = 0.0; - out_7537065087244600898[174] = 0.0; - out_7537065087244600898[175] = 0.0; - out_7537065087244600898[176] = 0.0; - out_7537065087244600898[177] = 0.0; - out_7537065087244600898[178] = 0.0; - out_7537065087244600898[179] = 0.0; - out_7537065087244600898[180] = 0.0; - out_7537065087244600898[181] = 0.0; - out_7537065087244600898[182] = 0.0; - out_7537065087244600898[183] = 0.0; - out_7537065087244600898[184] = 0.0; - out_7537065087244600898[185] = 0.0; - out_7537065087244600898[186] = 0.0; - out_7537065087244600898[187] = 0.0; - out_7537065087244600898[188] = 0.0; - out_7537065087244600898[189] = 0.0; - out_7537065087244600898[190] = 1.0; - out_7537065087244600898[191] = 0.0; - out_7537065087244600898[192] = 0.0; - out_7537065087244600898[193] = 0.0; - out_7537065087244600898[194] = 0.0; - out_7537065087244600898[195] = 0.0; - out_7537065087244600898[196] = 0.0; - out_7537065087244600898[197] = 0.0; - out_7537065087244600898[198] = 0.0; - out_7537065087244600898[199] = 0.0; - out_7537065087244600898[200] = 0.0; - out_7537065087244600898[201] = 0.0; - out_7537065087244600898[202] = 0.0; - out_7537065087244600898[203] = 0.0; - out_7537065087244600898[204] = 0.0; - out_7537065087244600898[205] = 0.0; - out_7537065087244600898[206] = 0.0; - out_7537065087244600898[207] = 0.0; - out_7537065087244600898[208] = 0.0; - out_7537065087244600898[209] = 1.0; - out_7537065087244600898[210] = 0.0; - out_7537065087244600898[211] = 0.0; - out_7537065087244600898[212] = 0.0; - out_7537065087244600898[213] = 0.0; - out_7537065087244600898[214] = 0.0; - out_7537065087244600898[215] = 0.0; - out_7537065087244600898[216] = 0.0; - out_7537065087244600898[217] = 0.0; - out_7537065087244600898[218] = 0.0; - out_7537065087244600898[219] = 0.0; - out_7537065087244600898[220] = 0.0; - out_7537065087244600898[221] = 0.0; - out_7537065087244600898[222] = 0.0; - out_7537065087244600898[223] = 0.0; - out_7537065087244600898[224] = 0.0; - out_7537065087244600898[225] = 0.0; - out_7537065087244600898[226] = 0.0; - out_7537065087244600898[227] = 0.0; - out_7537065087244600898[228] = 1.0; - out_7537065087244600898[229] = 0.0; - out_7537065087244600898[230] = 0.0; - out_7537065087244600898[231] = 0.0; - out_7537065087244600898[232] = 0.0; - out_7537065087244600898[233] = 0.0; - out_7537065087244600898[234] = 0.0; - out_7537065087244600898[235] = 0.0; - out_7537065087244600898[236] = 0.0; - out_7537065087244600898[237] = 0.0; - out_7537065087244600898[238] = 0.0; - out_7537065087244600898[239] = 0.0; - out_7537065087244600898[240] = 0.0; - out_7537065087244600898[241] = 0.0; - out_7537065087244600898[242] = 0.0; - out_7537065087244600898[243] = 0.0; - out_7537065087244600898[244] = 0.0; - out_7537065087244600898[245] = 0.0; - out_7537065087244600898[246] = 0.0; - out_7537065087244600898[247] = 1.0; - out_7537065087244600898[248] = 0.0; - out_7537065087244600898[249] = 0.0; - out_7537065087244600898[250] = 0.0; - out_7537065087244600898[251] = 0.0; - out_7537065087244600898[252] = 0.0; - out_7537065087244600898[253] = 0.0; - out_7537065087244600898[254] = 0.0; - out_7537065087244600898[255] = 0.0; - out_7537065087244600898[256] = 0.0; - out_7537065087244600898[257] = 0.0; - out_7537065087244600898[258] = 0.0; - out_7537065087244600898[259] = 0.0; - out_7537065087244600898[260] = 0.0; - out_7537065087244600898[261] = 0.0; - out_7537065087244600898[262] = 0.0; - out_7537065087244600898[263] = 0.0; - out_7537065087244600898[264] = 0.0; - out_7537065087244600898[265] = 0.0; - out_7537065087244600898[266] = 1.0; - out_7537065087244600898[267] = 0.0; - out_7537065087244600898[268] = 0.0; - out_7537065087244600898[269] = 0.0; - out_7537065087244600898[270] = 0.0; - out_7537065087244600898[271] = 0.0; - out_7537065087244600898[272] = 0.0; - out_7537065087244600898[273] = 0.0; - out_7537065087244600898[274] = 0.0; - out_7537065087244600898[275] = 0.0; - out_7537065087244600898[276] = 0.0; - out_7537065087244600898[277] = 0.0; - out_7537065087244600898[278] = 0.0; - out_7537065087244600898[279] = 0.0; - out_7537065087244600898[280] = 0.0; - out_7537065087244600898[281] = 0.0; - out_7537065087244600898[282] = 0.0; - out_7537065087244600898[283] = 0.0; - out_7537065087244600898[284] = 0.0; - out_7537065087244600898[285] = 1.0; - out_7537065087244600898[286] = 0.0; - out_7537065087244600898[287] = 0.0; - out_7537065087244600898[288] = 0.0; - out_7537065087244600898[289] = 0.0; - out_7537065087244600898[290] = 0.0; - out_7537065087244600898[291] = 0.0; - out_7537065087244600898[292] = 0.0; - out_7537065087244600898[293] = 0.0; - out_7537065087244600898[294] = 0.0; - out_7537065087244600898[295] = 0.0; - out_7537065087244600898[296] = 0.0; - out_7537065087244600898[297] = 0.0; - out_7537065087244600898[298] = 0.0; - out_7537065087244600898[299] = 0.0; - out_7537065087244600898[300] = 0.0; - out_7537065087244600898[301] = 0.0; - out_7537065087244600898[302] = 0.0; - out_7537065087244600898[303] = 0.0; - out_7537065087244600898[304] = 1.0; - out_7537065087244600898[305] = 0.0; - out_7537065087244600898[306] = 0.0; - out_7537065087244600898[307] = 0.0; - out_7537065087244600898[308] = 0.0; - out_7537065087244600898[309] = 0.0; - out_7537065087244600898[310] = 0.0; - out_7537065087244600898[311] = 0.0; - out_7537065087244600898[312] = 0.0; - out_7537065087244600898[313] = 0.0; - out_7537065087244600898[314] = 0.0; - out_7537065087244600898[315] = 0.0; - out_7537065087244600898[316] = 0.0; - out_7537065087244600898[317] = 0.0; - out_7537065087244600898[318] = 0.0; - out_7537065087244600898[319] = 0.0; - out_7537065087244600898[320] = 0.0; - out_7537065087244600898[321] = 0.0; - out_7537065087244600898[322] = 0.0; - out_7537065087244600898[323] = 1.0; +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 f_fun(double *state, double dt, double *out_6289537546199484080) { - out_6289537546199484080[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_6289537546199484080[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_6289537546199484080[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_6289537546199484080[3] = dt*state[12] + state[3]; - out_6289537546199484080[4] = dt*state[13] + state[4]; - out_6289537546199484080[5] = dt*state[14] + state[5]; - out_6289537546199484080[6] = state[6]; - out_6289537546199484080[7] = state[7]; - out_6289537546199484080[8] = state[8]; - out_6289537546199484080[9] = state[9]; - out_6289537546199484080[10] = state[10]; - out_6289537546199484080[11] = state[11]; - out_6289537546199484080[12] = state[12]; - out_6289537546199484080[13] = state[13]; - out_6289537546199484080[14] = state[14]; - out_6289537546199484080[15] = state[15]; - out_6289537546199484080[16] = state[16]; - out_6289537546199484080[17] = state[17]; +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_3263235949829739422) { - out_3263235949829739422[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_3263235949829739422[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_3263235949829739422[2] = 0; - out_3263235949829739422[3] = 0; - out_3263235949829739422[4] = 0; - out_3263235949829739422[5] = 0; - out_3263235949829739422[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_3263235949829739422[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_3263235949829739422[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_3263235949829739422[9] = 0; - out_3263235949829739422[10] = 0; - out_3263235949829739422[11] = 0; - out_3263235949829739422[12] = 0; - out_3263235949829739422[13] = 0; - out_3263235949829739422[14] = 0; - out_3263235949829739422[15] = 0; - out_3263235949829739422[16] = 0; - out_3263235949829739422[17] = 0; - out_3263235949829739422[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_3263235949829739422[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_3263235949829739422[20] = 0; - out_3263235949829739422[21] = 0; - out_3263235949829739422[22] = 0; - out_3263235949829739422[23] = 0; - out_3263235949829739422[24] = 0; - out_3263235949829739422[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_3263235949829739422[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_3263235949829739422[27] = 0; - out_3263235949829739422[28] = 0; - out_3263235949829739422[29] = 0; - out_3263235949829739422[30] = 0; - out_3263235949829739422[31] = 0; - out_3263235949829739422[32] = 0; - out_3263235949829739422[33] = 0; - out_3263235949829739422[34] = 0; - out_3263235949829739422[35] = 0; - out_3263235949829739422[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_3263235949829739422[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_3263235949829739422[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_3263235949829739422[39] = 0; - out_3263235949829739422[40] = 0; - out_3263235949829739422[41] = 0; - out_3263235949829739422[42] = 0; - out_3263235949829739422[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_3263235949829739422[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_3263235949829739422[45] = 0; - out_3263235949829739422[46] = 0; - out_3263235949829739422[47] = 0; - out_3263235949829739422[48] = 0; - out_3263235949829739422[49] = 0; - out_3263235949829739422[50] = 0; - out_3263235949829739422[51] = 0; - out_3263235949829739422[52] = 0; - out_3263235949829739422[53] = 0; - out_3263235949829739422[54] = 0; - out_3263235949829739422[55] = 0; - out_3263235949829739422[56] = 0; - out_3263235949829739422[57] = 1; - out_3263235949829739422[58] = 0; - out_3263235949829739422[59] = 0; - out_3263235949829739422[60] = 0; - out_3263235949829739422[61] = 0; - out_3263235949829739422[62] = 0; - out_3263235949829739422[63] = 0; - out_3263235949829739422[64] = 0; - out_3263235949829739422[65] = 0; - out_3263235949829739422[66] = dt; - out_3263235949829739422[67] = 0; - out_3263235949829739422[68] = 0; - out_3263235949829739422[69] = 0; - out_3263235949829739422[70] = 0; - out_3263235949829739422[71] = 0; - out_3263235949829739422[72] = 0; - out_3263235949829739422[73] = 0; - out_3263235949829739422[74] = 0; - out_3263235949829739422[75] = 0; - out_3263235949829739422[76] = 1; - out_3263235949829739422[77] = 0; - out_3263235949829739422[78] = 0; - out_3263235949829739422[79] = 0; - out_3263235949829739422[80] = 0; - out_3263235949829739422[81] = 0; - out_3263235949829739422[82] = 0; - out_3263235949829739422[83] = 0; - out_3263235949829739422[84] = 0; - out_3263235949829739422[85] = dt; - out_3263235949829739422[86] = 0; - out_3263235949829739422[87] = 0; - out_3263235949829739422[88] = 0; - out_3263235949829739422[89] = 0; - out_3263235949829739422[90] = 0; - out_3263235949829739422[91] = 0; - out_3263235949829739422[92] = 0; - out_3263235949829739422[93] = 0; - out_3263235949829739422[94] = 0; - out_3263235949829739422[95] = 1; - out_3263235949829739422[96] = 0; - out_3263235949829739422[97] = 0; - out_3263235949829739422[98] = 0; - out_3263235949829739422[99] = 0; - out_3263235949829739422[100] = 0; - out_3263235949829739422[101] = 0; - out_3263235949829739422[102] = 0; - out_3263235949829739422[103] = 0; - out_3263235949829739422[104] = dt; - out_3263235949829739422[105] = 0; - out_3263235949829739422[106] = 0; - out_3263235949829739422[107] = 0; - out_3263235949829739422[108] = 0; - out_3263235949829739422[109] = 0; - out_3263235949829739422[110] = 0; - out_3263235949829739422[111] = 0; - out_3263235949829739422[112] = 0; - out_3263235949829739422[113] = 0; - out_3263235949829739422[114] = 1; - out_3263235949829739422[115] = 0; - out_3263235949829739422[116] = 0; - out_3263235949829739422[117] = 0; - out_3263235949829739422[118] = 0; - out_3263235949829739422[119] = 0; - out_3263235949829739422[120] = 0; - out_3263235949829739422[121] = 0; - out_3263235949829739422[122] = 0; - out_3263235949829739422[123] = 0; - out_3263235949829739422[124] = 0; - out_3263235949829739422[125] = 0; - out_3263235949829739422[126] = 0; - out_3263235949829739422[127] = 0; - out_3263235949829739422[128] = 0; - out_3263235949829739422[129] = 0; - out_3263235949829739422[130] = 0; - out_3263235949829739422[131] = 0; - out_3263235949829739422[132] = 0; - out_3263235949829739422[133] = 1; - out_3263235949829739422[134] = 0; - out_3263235949829739422[135] = 0; - out_3263235949829739422[136] = 0; - out_3263235949829739422[137] = 0; - out_3263235949829739422[138] = 0; - out_3263235949829739422[139] = 0; - out_3263235949829739422[140] = 0; - out_3263235949829739422[141] = 0; - out_3263235949829739422[142] = 0; - out_3263235949829739422[143] = 0; - out_3263235949829739422[144] = 0; - out_3263235949829739422[145] = 0; - out_3263235949829739422[146] = 0; - out_3263235949829739422[147] = 0; - out_3263235949829739422[148] = 0; - out_3263235949829739422[149] = 0; - out_3263235949829739422[150] = 0; - out_3263235949829739422[151] = 0; - out_3263235949829739422[152] = 1; - out_3263235949829739422[153] = 0; - out_3263235949829739422[154] = 0; - out_3263235949829739422[155] = 0; - out_3263235949829739422[156] = 0; - out_3263235949829739422[157] = 0; - out_3263235949829739422[158] = 0; - out_3263235949829739422[159] = 0; - out_3263235949829739422[160] = 0; - out_3263235949829739422[161] = 0; - out_3263235949829739422[162] = 0; - out_3263235949829739422[163] = 0; - out_3263235949829739422[164] = 0; - out_3263235949829739422[165] = 0; - out_3263235949829739422[166] = 0; - out_3263235949829739422[167] = 0; - out_3263235949829739422[168] = 0; - out_3263235949829739422[169] = 0; - out_3263235949829739422[170] = 0; - out_3263235949829739422[171] = 1; - out_3263235949829739422[172] = 0; - out_3263235949829739422[173] = 0; - out_3263235949829739422[174] = 0; - out_3263235949829739422[175] = 0; - out_3263235949829739422[176] = 0; - out_3263235949829739422[177] = 0; - out_3263235949829739422[178] = 0; - out_3263235949829739422[179] = 0; - out_3263235949829739422[180] = 0; - out_3263235949829739422[181] = 0; - out_3263235949829739422[182] = 0; - out_3263235949829739422[183] = 0; - out_3263235949829739422[184] = 0; - out_3263235949829739422[185] = 0; - out_3263235949829739422[186] = 0; - out_3263235949829739422[187] = 0; - out_3263235949829739422[188] = 0; - out_3263235949829739422[189] = 0; - out_3263235949829739422[190] = 1; - out_3263235949829739422[191] = 0; - out_3263235949829739422[192] = 0; - out_3263235949829739422[193] = 0; - out_3263235949829739422[194] = 0; - out_3263235949829739422[195] = 0; - out_3263235949829739422[196] = 0; - out_3263235949829739422[197] = 0; - out_3263235949829739422[198] = 0; - out_3263235949829739422[199] = 0; - out_3263235949829739422[200] = 0; - out_3263235949829739422[201] = 0; - out_3263235949829739422[202] = 0; - out_3263235949829739422[203] = 0; - out_3263235949829739422[204] = 0; - out_3263235949829739422[205] = 0; - out_3263235949829739422[206] = 0; - out_3263235949829739422[207] = 0; - out_3263235949829739422[208] = 0; - out_3263235949829739422[209] = 1; - out_3263235949829739422[210] = 0; - out_3263235949829739422[211] = 0; - out_3263235949829739422[212] = 0; - out_3263235949829739422[213] = 0; - out_3263235949829739422[214] = 0; - out_3263235949829739422[215] = 0; - out_3263235949829739422[216] = 0; - out_3263235949829739422[217] = 0; - out_3263235949829739422[218] = 0; - out_3263235949829739422[219] = 0; - out_3263235949829739422[220] = 0; - out_3263235949829739422[221] = 0; - out_3263235949829739422[222] = 0; - out_3263235949829739422[223] = 0; - out_3263235949829739422[224] = 0; - out_3263235949829739422[225] = 0; - out_3263235949829739422[226] = 0; - out_3263235949829739422[227] = 0; - out_3263235949829739422[228] = 1; - out_3263235949829739422[229] = 0; - out_3263235949829739422[230] = 0; - out_3263235949829739422[231] = 0; - out_3263235949829739422[232] = 0; - out_3263235949829739422[233] = 0; - out_3263235949829739422[234] = 0; - out_3263235949829739422[235] = 0; - out_3263235949829739422[236] = 0; - out_3263235949829739422[237] = 0; - out_3263235949829739422[238] = 0; - out_3263235949829739422[239] = 0; - out_3263235949829739422[240] = 0; - out_3263235949829739422[241] = 0; - out_3263235949829739422[242] = 0; - out_3263235949829739422[243] = 0; - out_3263235949829739422[244] = 0; - out_3263235949829739422[245] = 0; - out_3263235949829739422[246] = 0; - out_3263235949829739422[247] = 1; - out_3263235949829739422[248] = 0; - out_3263235949829739422[249] = 0; - out_3263235949829739422[250] = 0; - out_3263235949829739422[251] = 0; - out_3263235949829739422[252] = 0; - out_3263235949829739422[253] = 0; - out_3263235949829739422[254] = 0; - out_3263235949829739422[255] = 0; - out_3263235949829739422[256] = 0; - out_3263235949829739422[257] = 0; - out_3263235949829739422[258] = 0; - out_3263235949829739422[259] = 0; - out_3263235949829739422[260] = 0; - out_3263235949829739422[261] = 0; - out_3263235949829739422[262] = 0; - out_3263235949829739422[263] = 0; - out_3263235949829739422[264] = 0; - out_3263235949829739422[265] = 0; - out_3263235949829739422[266] = 1; - out_3263235949829739422[267] = 0; - out_3263235949829739422[268] = 0; - out_3263235949829739422[269] = 0; - out_3263235949829739422[270] = 0; - out_3263235949829739422[271] = 0; - out_3263235949829739422[272] = 0; - out_3263235949829739422[273] = 0; - out_3263235949829739422[274] = 0; - out_3263235949829739422[275] = 0; - out_3263235949829739422[276] = 0; - out_3263235949829739422[277] = 0; - out_3263235949829739422[278] = 0; - out_3263235949829739422[279] = 0; - out_3263235949829739422[280] = 0; - out_3263235949829739422[281] = 0; - out_3263235949829739422[282] = 0; - out_3263235949829739422[283] = 0; - out_3263235949829739422[284] = 0; - out_3263235949829739422[285] = 1; - out_3263235949829739422[286] = 0; - out_3263235949829739422[287] = 0; - out_3263235949829739422[288] = 0; - out_3263235949829739422[289] = 0; - out_3263235949829739422[290] = 0; - out_3263235949829739422[291] = 0; - out_3263235949829739422[292] = 0; - out_3263235949829739422[293] = 0; - out_3263235949829739422[294] = 0; - out_3263235949829739422[295] = 0; - out_3263235949829739422[296] = 0; - out_3263235949829739422[297] = 0; - out_3263235949829739422[298] = 0; - out_3263235949829739422[299] = 0; - out_3263235949829739422[300] = 0; - out_3263235949829739422[301] = 0; - out_3263235949829739422[302] = 0; - out_3263235949829739422[303] = 0; - out_3263235949829739422[304] = 1; - out_3263235949829739422[305] = 0; - out_3263235949829739422[306] = 0; - out_3263235949829739422[307] = 0; - out_3263235949829739422[308] = 0; - out_3263235949829739422[309] = 0; - out_3263235949829739422[310] = 0; - out_3263235949829739422[311] = 0; - out_3263235949829739422[312] = 0; - out_3263235949829739422[313] = 0; - out_3263235949829739422[314] = 0; - out_3263235949829739422[315] = 0; - out_3263235949829739422[316] = 0; - out_3263235949829739422[317] = 0; - out_3263235949829739422[318] = 0; - out_3263235949829739422[319] = 0; - out_3263235949829739422[320] = 0; - out_3263235949829739422[321] = 0; - out_3263235949829739422[322] = 0; - out_3263235949829739422[323] = 1; +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 h_4(double *state, double *unused, double *out_6871661940623032085) { - out_6871661940623032085[0] = state[6] + state[9]; - out_6871661940623032085[1] = state[7] + state[10]; - out_6871661940623032085[2] = state[8] + state[11]; +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_5061597179712991997) { - out_5061597179712991997[0] = 0; - out_5061597179712991997[1] = 0; - out_5061597179712991997[2] = 0; - out_5061597179712991997[3] = 0; - out_5061597179712991997[4] = 0; - out_5061597179712991997[5] = 0; - out_5061597179712991997[6] = 1; - out_5061597179712991997[7] = 0; - out_5061597179712991997[8] = 0; - out_5061597179712991997[9] = 1; - out_5061597179712991997[10] = 0; - out_5061597179712991997[11] = 0; - out_5061597179712991997[12] = 0; - out_5061597179712991997[13] = 0; - out_5061597179712991997[14] = 0; - out_5061597179712991997[15] = 0; - out_5061597179712991997[16] = 0; - out_5061597179712991997[17] = 0; - out_5061597179712991997[18] = 0; - out_5061597179712991997[19] = 0; - out_5061597179712991997[20] = 0; - out_5061597179712991997[21] = 0; - out_5061597179712991997[22] = 0; - out_5061597179712991997[23] = 0; - out_5061597179712991997[24] = 0; - out_5061597179712991997[25] = 1; - out_5061597179712991997[26] = 0; - out_5061597179712991997[27] = 0; - out_5061597179712991997[28] = 1; - out_5061597179712991997[29] = 0; - out_5061597179712991997[30] = 0; - out_5061597179712991997[31] = 0; - out_5061597179712991997[32] = 0; - out_5061597179712991997[33] = 0; - out_5061597179712991997[34] = 0; - out_5061597179712991997[35] = 0; - out_5061597179712991997[36] = 0; - out_5061597179712991997[37] = 0; - out_5061597179712991997[38] = 0; - out_5061597179712991997[39] = 0; - out_5061597179712991997[40] = 0; - out_5061597179712991997[41] = 0; - out_5061597179712991997[42] = 0; - out_5061597179712991997[43] = 0; - out_5061597179712991997[44] = 1; - out_5061597179712991997[45] = 0; - out_5061597179712991997[46] = 0; - out_5061597179712991997[47] = 1; - out_5061597179712991997[48] = 0; - out_5061597179712991997[49] = 0; - out_5061597179712991997[50] = 0; - out_5061597179712991997[51] = 0; - out_5061597179712991997[52] = 0; - out_5061597179712991997[53] = 0; +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_10(double *state, double *unused, double *out_974669649895256171) { - out_974669649895256171[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_974669649895256171[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_974669649895256171[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_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_3862977066975629010) { - out_3862977066975629010[0] = 0; - out_3862977066975629010[1] = 9.8100000000000005*cos(state[1]); - out_3862977066975629010[2] = 0; - out_3862977066975629010[3] = 0; - out_3862977066975629010[4] = -state[8]; - out_3862977066975629010[5] = state[7]; - out_3862977066975629010[6] = 0; - out_3862977066975629010[7] = state[5]; - out_3862977066975629010[8] = -state[4]; - out_3862977066975629010[9] = 0; - out_3862977066975629010[10] = 0; - out_3862977066975629010[11] = 0; - out_3862977066975629010[12] = 1; - out_3862977066975629010[13] = 0; - out_3862977066975629010[14] = 0; - out_3862977066975629010[15] = 1; - out_3862977066975629010[16] = 0; - out_3862977066975629010[17] = 0; - out_3862977066975629010[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_3862977066975629010[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_3862977066975629010[20] = 0; - out_3862977066975629010[21] = state[8]; - out_3862977066975629010[22] = 0; - out_3862977066975629010[23] = -state[6]; - out_3862977066975629010[24] = -state[5]; - out_3862977066975629010[25] = 0; - out_3862977066975629010[26] = state[3]; - out_3862977066975629010[27] = 0; - out_3862977066975629010[28] = 0; - out_3862977066975629010[29] = 0; - out_3862977066975629010[30] = 0; - out_3862977066975629010[31] = 1; - out_3862977066975629010[32] = 0; - out_3862977066975629010[33] = 0; - out_3862977066975629010[34] = 1; - out_3862977066975629010[35] = 0; - out_3862977066975629010[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_3862977066975629010[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_3862977066975629010[38] = 0; - out_3862977066975629010[39] = -state[7]; - out_3862977066975629010[40] = state[6]; - out_3862977066975629010[41] = 0; - out_3862977066975629010[42] = state[4]; - out_3862977066975629010[43] = -state[3]; - out_3862977066975629010[44] = 0; - out_3862977066975629010[45] = 0; - out_3862977066975629010[46] = 0; - out_3862977066975629010[47] = 0; - out_3862977066975629010[48] = 0; - out_3862977066975629010[49] = 0; - out_3862977066975629010[50] = 1; - out_3862977066975629010[51] = 0; - out_3862977066975629010[52] = 0; - out_3862977066975629010[53] = 1; +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_13(double *state, double *unused, double *out_2719357598944466940) { - out_2719357598944466940[0] = state[3]; - out_2719357598944466940[1] = state[4]; - out_2719357598944466940[2] = state[5]; +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_8273871005045324798) { - out_8273871005045324798[0] = 0; - out_8273871005045324798[1] = 0; - out_8273871005045324798[2] = 0; - out_8273871005045324798[3] = 1; - out_8273871005045324798[4] = 0; - out_8273871005045324798[5] = 0; - out_8273871005045324798[6] = 0; - out_8273871005045324798[7] = 0; - out_8273871005045324798[8] = 0; - out_8273871005045324798[9] = 0; - out_8273871005045324798[10] = 0; - out_8273871005045324798[11] = 0; - out_8273871005045324798[12] = 0; - out_8273871005045324798[13] = 0; - out_8273871005045324798[14] = 0; - out_8273871005045324798[15] = 0; - out_8273871005045324798[16] = 0; - out_8273871005045324798[17] = 0; - out_8273871005045324798[18] = 0; - out_8273871005045324798[19] = 0; - out_8273871005045324798[20] = 0; - out_8273871005045324798[21] = 0; - out_8273871005045324798[22] = 1; - out_8273871005045324798[23] = 0; - out_8273871005045324798[24] = 0; - out_8273871005045324798[25] = 0; - out_8273871005045324798[26] = 0; - out_8273871005045324798[27] = 0; - out_8273871005045324798[28] = 0; - out_8273871005045324798[29] = 0; - out_8273871005045324798[30] = 0; - out_8273871005045324798[31] = 0; - out_8273871005045324798[32] = 0; - out_8273871005045324798[33] = 0; - out_8273871005045324798[34] = 0; - out_8273871005045324798[35] = 0; - out_8273871005045324798[36] = 0; - out_8273871005045324798[37] = 0; - out_8273871005045324798[38] = 0; - out_8273871005045324798[39] = 0; - out_8273871005045324798[40] = 0; - out_8273871005045324798[41] = 1; - out_8273871005045324798[42] = 0; - out_8273871005045324798[43] = 0; - out_8273871005045324798[44] = 0; - out_8273871005045324798[45] = 0; - out_8273871005045324798[46] = 0; - out_8273871005045324798[47] = 0; - out_8273871005045324798[48] = 0; - out_8273871005045324798[49] = 0; - out_8273871005045324798[50] = 0; - out_8273871005045324798[51] = 0; - out_8273871005045324798[52] = 0; - out_8273871005045324798[53] = 0; +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_14(double *state, double *unused, double *out_5270922229605318979) { - out_5270922229605318979[0] = state[6]; - out_5270922229605318979[1] = state[7]; - out_5270922229605318979[2] = state[8]; +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_9024838036052476526) { - out_9024838036052476526[0] = 0; - out_9024838036052476526[1] = 0; - out_9024838036052476526[2] = 0; - out_9024838036052476526[3] = 0; - out_9024838036052476526[4] = 0; - out_9024838036052476526[5] = 0; - out_9024838036052476526[6] = 1; - out_9024838036052476526[7] = 0; - out_9024838036052476526[8] = 0; - out_9024838036052476526[9] = 0; - out_9024838036052476526[10] = 0; - out_9024838036052476526[11] = 0; - out_9024838036052476526[12] = 0; - out_9024838036052476526[13] = 0; - out_9024838036052476526[14] = 0; - out_9024838036052476526[15] = 0; - out_9024838036052476526[16] = 0; - out_9024838036052476526[17] = 0; - out_9024838036052476526[18] = 0; - out_9024838036052476526[19] = 0; - out_9024838036052476526[20] = 0; - out_9024838036052476526[21] = 0; - out_9024838036052476526[22] = 0; - out_9024838036052476526[23] = 0; - out_9024838036052476526[24] = 0; - out_9024838036052476526[25] = 1; - out_9024838036052476526[26] = 0; - out_9024838036052476526[27] = 0; - out_9024838036052476526[28] = 0; - out_9024838036052476526[29] = 0; - out_9024838036052476526[30] = 0; - out_9024838036052476526[31] = 0; - out_9024838036052476526[32] = 0; - out_9024838036052476526[33] = 0; - out_9024838036052476526[34] = 0; - out_9024838036052476526[35] = 0; - out_9024838036052476526[36] = 0; - out_9024838036052476526[37] = 0; - out_9024838036052476526[38] = 0; - out_9024838036052476526[39] = 0; - out_9024838036052476526[40] = 0; - out_9024838036052476526[41] = 0; - out_9024838036052476526[42] = 0; - out_9024838036052476526[43] = 0; - out_9024838036052476526[44] = 1; - out_9024838036052476526[45] = 0; - out_9024838036052476526[46] = 0; - out_9024838036052476526[47] = 0; - out_9024838036052476526[48] = 0; - out_9024838036052476526[49] = 0; - out_9024838036052476526[50] = 0; - out_9024838036052476526[51] = 0; - out_9024838036052476526[52] = 0; - out_9024838036052476526[53] = 0; +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; } #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_8612662017984698055) { - err_fun(nom_x, delta_x, out_8612662017984698055); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6269171790765846050) { + err_fun(nom_x, delta_x, out_6269171790765846050); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6583538545281744086) { - inv_err_fun(nom_x, true_x, out_6583538545281744086); +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_H_mod_fun(double *state, double *out_7537065087244600898) { - H_mod_fun(state, out_7537065087244600898); +void pose_H_mod_fun(double *state, double *out_7862392065184069712) { + H_mod_fun(state, out_7862392065184069712); } -void pose_f_fun(double *state, double dt, double *out_6289537546199484080) { - f_fun(state, dt, out_6289537546199484080); +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_3263235949829739422) { - F_fun(state, dt, out_3263235949829739422); +void pose_F_fun(double *state, double dt, double *out_1703318423524786966) { + F_fun(state, dt, out_1703318423524786966); } -void pose_h_4(double *state, double *unused, double *out_6871661940623032085) { - h_4(state, unused, out_6871661940623032085); +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_5061597179712991997) { - H_4(state, unused, out_5061597179712991997); +void pose_H_4(double *state, double *unused, double *out_1639990342467660562) { + H_4(state, unused, out_1639990342467660562); } -void pose_h_10(double *state, double *unused, double *out_974669649895256171) { - h_10(state, unused, out_974669649895256171); +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_3862977066975629010) { - H_10(state, unused, out_3862977066975629010); +void pose_H_10(double *state, double *unused, double *out_5548344007433806154) { + H_10(state, unused, out_5548344007433806154); } -void pose_h_13(double *state, double *unused, double *out_2719357598944466940) { - h_13(state, unused, out_2719357598944466940); +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_8273871005045324798) { - H_13(state, unused, out_8273871005045324798); +void pose_H_13(double *state, double *unused, double *out_4852264167799993363) { + H_13(state, unused, out_4852264167799993363); } -void pose_h_14(double *state, double *unused, double *out_5270922229605318979) { - h_14(state, unused, out_5270922229605318979); +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_9024838036052476526) { - H_14(state, unused, out_9024838036052476526); +void pose_H_14(double *state, double *unused, double *out_5603231198807145091) { + H_14(state, unused, out_5603231198807145091); } 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 e544f8386..676d89a82 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_8612662017984698055); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6583538545281744086); -void pose_H_mod_fun(double *state, double *out_7537065087244600898); -void pose_f_fun(double *state, double dt, double *out_6289537546199484080); -void pose_F_fun(double *state, double dt, double *out_3263235949829739422); -void pose_h_4(double *state, double *unused, double *out_6871661940623032085); -void pose_H_4(double *state, double *unused, double *out_5061597179712991997); -void pose_h_10(double *state, double *unused, double *out_974669649895256171); -void pose_H_10(double *state, double *unused, double *out_3862977066975629010); -void pose_h_13(double *state, double *unused, double *out_2719357598944466940); -void pose_H_13(double *state, double *unused, double *out_8273871005045324798); -void pose_h_14(double *state, double *unused, double *out_5270922229605318979); -void pose_H_14(double *state, double *unused, double *out_9024838036052476526); +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_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 5b4796f8e..9857117af 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ