diff --git a/cereal/libcereal.a b/cereal/libcereal.a index cf9990eb6..269d7c0a0 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/common/libcommon.a b/common/libcommon.a index 8d5b3ff37..82ab0c0c0 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 68631c98f..1c988c8ef 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index a211aa66a..484a13cda 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 d6fca60e2..babce9260 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 530750600..8dd087f79 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 def638767..d7cce1efa 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 7fcc530aa..e76760f49 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 9926ff453..804bc713f 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 71b6ce6a8..1cc7f8785 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 71b6ce6a8..1cc7f8785 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 2753b0abd..c4ee4ceff 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 9926ff453..804bc713f 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 11d206413..d6dd9c696 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-efdc4898-DEBUG"; +const uint8_t gitversion[19] = "DEV-f20b1f2a-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 9f66f8c2e..d1c20f860 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 99fbee7a4..1eb04fd9c 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 21f1a5126..9cc71242b 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 2d7c81b22..349e25f06 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 ff4bc0540..4f9443c25 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 317c29188..285360a1a 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 683de40ad..63a354f56 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 705aac424..d17eb9526 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 e02f6806c..5f5c0f601 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 f9a3bd92f..5ea845a0f 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 23a92f878..bfc9e3860 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 ed08316b9..6d8b9a4ae 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 674b81081..e64a8030d 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 4b0c84831..ec9707e72 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 582d86338..904f67ea9 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 425514a77..cc0ad6e48 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 26dde6644..81c01d854 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 481692bf4..bdaf8795f 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 049598468..805ea08eb 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 2c2c0fdf9..6e2601ed4 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 00f88da0e..28b50c240 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-efdc4898-DEBUG \ No newline at end of file +DEV-f20b1f2a-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index db92055fd..687b78dae 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_8558166113930061773) { - out_8558166113930061773[0] = delta_x[0] + nom_x[0]; - out_8558166113930061773[1] = delta_x[1] + nom_x[1]; - out_8558166113930061773[2] = delta_x[2] + nom_x[2]; - out_8558166113930061773[3] = delta_x[3] + nom_x[3]; - out_8558166113930061773[4] = delta_x[4] + nom_x[4]; - out_8558166113930061773[5] = delta_x[5] + nom_x[5]; - out_8558166113930061773[6] = delta_x[6] + nom_x[6]; - out_8558166113930061773[7] = delta_x[7] + nom_x[7]; - out_8558166113930061773[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_5063363100648596688) { + out_5063363100648596688[0] = delta_x[0] + nom_x[0]; + out_5063363100648596688[1] = delta_x[1] + nom_x[1]; + out_5063363100648596688[2] = delta_x[2] + nom_x[2]; + out_5063363100648596688[3] = delta_x[3] + nom_x[3]; + out_5063363100648596688[4] = delta_x[4] + nom_x[4]; + out_5063363100648596688[5] = delta_x[5] + nom_x[5]; + out_5063363100648596688[6] = delta_x[6] + nom_x[6]; + out_5063363100648596688[7] = delta_x[7] + nom_x[7]; + out_5063363100648596688[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_2269670274683655268) { - out_2269670274683655268[0] = -nom_x[0] + true_x[0]; - out_2269670274683655268[1] = -nom_x[1] + true_x[1]; - out_2269670274683655268[2] = -nom_x[2] + true_x[2]; - out_2269670274683655268[3] = -nom_x[3] + true_x[3]; - out_2269670274683655268[4] = -nom_x[4] + true_x[4]; - out_2269670274683655268[5] = -nom_x[5] + true_x[5]; - out_2269670274683655268[6] = -nom_x[6] + true_x[6]; - out_2269670274683655268[7] = -nom_x[7] + true_x[7]; - out_2269670274683655268[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1051973791362877167) { + out_1051973791362877167[0] = -nom_x[0] + true_x[0]; + out_1051973791362877167[1] = -nom_x[1] + true_x[1]; + out_1051973791362877167[2] = -nom_x[2] + true_x[2]; + out_1051973791362877167[3] = -nom_x[3] + true_x[3]; + out_1051973791362877167[4] = -nom_x[4] + true_x[4]; + out_1051973791362877167[5] = -nom_x[5] + true_x[5]; + out_1051973791362877167[6] = -nom_x[6] + true_x[6]; + out_1051973791362877167[7] = -nom_x[7] + true_x[7]; + out_1051973791362877167[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_6407701768448647232) { - out_6407701768448647232[0] = 1.0; - out_6407701768448647232[1] = 0.0; - out_6407701768448647232[2] = 0.0; - out_6407701768448647232[3] = 0.0; - out_6407701768448647232[4] = 0.0; - out_6407701768448647232[5] = 0.0; - out_6407701768448647232[6] = 0.0; - out_6407701768448647232[7] = 0.0; - out_6407701768448647232[8] = 0.0; - out_6407701768448647232[9] = 0.0; - out_6407701768448647232[10] = 1.0; - out_6407701768448647232[11] = 0.0; - out_6407701768448647232[12] = 0.0; - out_6407701768448647232[13] = 0.0; - out_6407701768448647232[14] = 0.0; - out_6407701768448647232[15] = 0.0; - out_6407701768448647232[16] = 0.0; - out_6407701768448647232[17] = 0.0; - out_6407701768448647232[18] = 0.0; - out_6407701768448647232[19] = 0.0; - out_6407701768448647232[20] = 1.0; - out_6407701768448647232[21] = 0.0; - out_6407701768448647232[22] = 0.0; - out_6407701768448647232[23] = 0.0; - out_6407701768448647232[24] = 0.0; - out_6407701768448647232[25] = 0.0; - out_6407701768448647232[26] = 0.0; - out_6407701768448647232[27] = 0.0; - out_6407701768448647232[28] = 0.0; - out_6407701768448647232[29] = 0.0; - out_6407701768448647232[30] = 1.0; - out_6407701768448647232[31] = 0.0; - out_6407701768448647232[32] = 0.0; - out_6407701768448647232[33] = 0.0; - out_6407701768448647232[34] = 0.0; - out_6407701768448647232[35] = 0.0; - out_6407701768448647232[36] = 0.0; - out_6407701768448647232[37] = 0.0; - out_6407701768448647232[38] = 0.0; - out_6407701768448647232[39] = 0.0; - out_6407701768448647232[40] = 1.0; - out_6407701768448647232[41] = 0.0; - out_6407701768448647232[42] = 0.0; - out_6407701768448647232[43] = 0.0; - out_6407701768448647232[44] = 0.0; - out_6407701768448647232[45] = 0.0; - out_6407701768448647232[46] = 0.0; - out_6407701768448647232[47] = 0.0; - out_6407701768448647232[48] = 0.0; - out_6407701768448647232[49] = 0.0; - out_6407701768448647232[50] = 1.0; - out_6407701768448647232[51] = 0.0; - out_6407701768448647232[52] = 0.0; - out_6407701768448647232[53] = 0.0; - out_6407701768448647232[54] = 0.0; - out_6407701768448647232[55] = 0.0; - out_6407701768448647232[56] = 0.0; - out_6407701768448647232[57] = 0.0; - out_6407701768448647232[58] = 0.0; - out_6407701768448647232[59] = 0.0; - out_6407701768448647232[60] = 1.0; - out_6407701768448647232[61] = 0.0; - out_6407701768448647232[62] = 0.0; - out_6407701768448647232[63] = 0.0; - out_6407701768448647232[64] = 0.0; - out_6407701768448647232[65] = 0.0; - out_6407701768448647232[66] = 0.0; - out_6407701768448647232[67] = 0.0; - out_6407701768448647232[68] = 0.0; - out_6407701768448647232[69] = 0.0; - out_6407701768448647232[70] = 1.0; - out_6407701768448647232[71] = 0.0; - out_6407701768448647232[72] = 0.0; - out_6407701768448647232[73] = 0.0; - out_6407701768448647232[74] = 0.0; - out_6407701768448647232[75] = 0.0; - out_6407701768448647232[76] = 0.0; - out_6407701768448647232[77] = 0.0; - out_6407701768448647232[78] = 0.0; - out_6407701768448647232[79] = 0.0; - out_6407701768448647232[80] = 1.0; +void H_mod_fun(double *state, double *out_5315869906856267240) { + out_5315869906856267240[0] = 1.0; + out_5315869906856267240[1] = 0.0; + out_5315869906856267240[2] = 0.0; + out_5315869906856267240[3] = 0.0; + out_5315869906856267240[4] = 0.0; + out_5315869906856267240[5] = 0.0; + out_5315869906856267240[6] = 0.0; + out_5315869906856267240[7] = 0.0; + out_5315869906856267240[8] = 0.0; + out_5315869906856267240[9] = 0.0; + out_5315869906856267240[10] = 1.0; + out_5315869906856267240[11] = 0.0; + out_5315869906856267240[12] = 0.0; + out_5315869906856267240[13] = 0.0; + out_5315869906856267240[14] = 0.0; + out_5315869906856267240[15] = 0.0; + out_5315869906856267240[16] = 0.0; + out_5315869906856267240[17] = 0.0; + out_5315869906856267240[18] = 0.0; + out_5315869906856267240[19] = 0.0; + out_5315869906856267240[20] = 1.0; + out_5315869906856267240[21] = 0.0; + out_5315869906856267240[22] = 0.0; + out_5315869906856267240[23] = 0.0; + out_5315869906856267240[24] = 0.0; + out_5315869906856267240[25] = 0.0; + out_5315869906856267240[26] = 0.0; + out_5315869906856267240[27] = 0.0; + out_5315869906856267240[28] = 0.0; + out_5315869906856267240[29] = 0.0; + out_5315869906856267240[30] = 1.0; + out_5315869906856267240[31] = 0.0; + out_5315869906856267240[32] = 0.0; + out_5315869906856267240[33] = 0.0; + out_5315869906856267240[34] = 0.0; + out_5315869906856267240[35] = 0.0; + out_5315869906856267240[36] = 0.0; + out_5315869906856267240[37] = 0.0; + out_5315869906856267240[38] = 0.0; + out_5315869906856267240[39] = 0.0; + out_5315869906856267240[40] = 1.0; + out_5315869906856267240[41] = 0.0; + out_5315869906856267240[42] = 0.0; + out_5315869906856267240[43] = 0.0; + out_5315869906856267240[44] = 0.0; + out_5315869906856267240[45] = 0.0; + out_5315869906856267240[46] = 0.0; + out_5315869906856267240[47] = 0.0; + out_5315869906856267240[48] = 0.0; + out_5315869906856267240[49] = 0.0; + out_5315869906856267240[50] = 1.0; + out_5315869906856267240[51] = 0.0; + out_5315869906856267240[52] = 0.0; + out_5315869906856267240[53] = 0.0; + out_5315869906856267240[54] = 0.0; + out_5315869906856267240[55] = 0.0; + out_5315869906856267240[56] = 0.0; + out_5315869906856267240[57] = 0.0; + out_5315869906856267240[58] = 0.0; + out_5315869906856267240[59] = 0.0; + out_5315869906856267240[60] = 1.0; + out_5315869906856267240[61] = 0.0; + out_5315869906856267240[62] = 0.0; + out_5315869906856267240[63] = 0.0; + out_5315869906856267240[64] = 0.0; + out_5315869906856267240[65] = 0.0; + out_5315869906856267240[66] = 0.0; + out_5315869906856267240[67] = 0.0; + out_5315869906856267240[68] = 0.0; + out_5315869906856267240[69] = 0.0; + out_5315869906856267240[70] = 1.0; + out_5315869906856267240[71] = 0.0; + out_5315869906856267240[72] = 0.0; + out_5315869906856267240[73] = 0.0; + out_5315869906856267240[74] = 0.0; + out_5315869906856267240[75] = 0.0; + out_5315869906856267240[76] = 0.0; + out_5315869906856267240[77] = 0.0; + out_5315869906856267240[78] = 0.0; + out_5315869906856267240[79] = 0.0; + out_5315869906856267240[80] = 1.0; } -void f_fun(double *state, double dt, double *out_648904423540471901) { - out_648904423540471901[0] = state[0]; - out_648904423540471901[1] = state[1]; - out_648904423540471901[2] = state[2]; - out_648904423540471901[3] = state[3]; - out_648904423540471901[4] = state[4]; - out_648904423540471901[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_648904423540471901[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_648904423540471901[7] = state[7]; - out_648904423540471901[8] = state[8]; +void f_fun(double *state, double dt, double *out_2679807483918754558) { + out_2679807483918754558[0] = state[0]; + out_2679807483918754558[1] = state[1]; + out_2679807483918754558[2] = state[2]; + out_2679807483918754558[3] = state[3]; + out_2679807483918754558[4] = state[4]; + out_2679807483918754558[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_2679807483918754558[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_2679807483918754558[7] = state[7]; + out_2679807483918754558[8] = state[8]; } -void F_fun(double *state, double dt, double *out_7503564954401744509) { - out_7503564954401744509[0] = 1; - out_7503564954401744509[1] = 0; - out_7503564954401744509[2] = 0; - out_7503564954401744509[3] = 0; - out_7503564954401744509[4] = 0; - out_7503564954401744509[5] = 0; - out_7503564954401744509[6] = 0; - out_7503564954401744509[7] = 0; - out_7503564954401744509[8] = 0; - out_7503564954401744509[9] = 0; - out_7503564954401744509[10] = 1; - out_7503564954401744509[11] = 0; - out_7503564954401744509[12] = 0; - out_7503564954401744509[13] = 0; - out_7503564954401744509[14] = 0; - out_7503564954401744509[15] = 0; - out_7503564954401744509[16] = 0; - out_7503564954401744509[17] = 0; - out_7503564954401744509[18] = 0; - out_7503564954401744509[19] = 0; - out_7503564954401744509[20] = 1; - out_7503564954401744509[21] = 0; - out_7503564954401744509[22] = 0; - out_7503564954401744509[23] = 0; - out_7503564954401744509[24] = 0; - out_7503564954401744509[25] = 0; - out_7503564954401744509[26] = 0; - out_7503564954401744509[27] = 0; - out_7503564954401744509[28] = 0; - out_7503564954401744509[29] = 0; - out_7503564954401744509[30] = 1; - out_7503564954401744509[31] = 0; - out_7503564954401744509[32] = 0; - out_7503564954401744509[33] = 0; - out_7503564954401744509[34] = 0; - out_7503564954401744509[35] = 0; - out_7503564954401744509[36] = 0; - out_7503564954401744509[37] = 0; - out_7503564954401744509[38] = 0; - out_7503564954401744509[39] = 0; - out_7503564954401744509[40] = 1; - out_7503564954401744509[41] = 0; - out_7503564954401744509[42] = 0; - out_7503564954401744509[43] = 0; - out_7503564954401744509[44] = 0; - out_7503564954401744509[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_7503564954401744509[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_7503564954401744509[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_7503564954401744509[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_7503564954401744509[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_7503564954401744509[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_7503564954401744509[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_7503564954401744509[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_7503564954401744509[53] = -9.8100000000000005*dt; - out_7503564954401744509[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_7503564954401744509[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_7503564954401744509[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7503564954401744509[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7503564954401744509[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_7503564954401744509[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_7503564954401744509[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_7503564954401744509[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_7503564954401744509[62] = 0; - out_7503564954401744509[63] = 0; - out_7503564954401744509[64] = 0; - out_7503564954401744509[65] = 0; - out_7503564954401744509[66] = 0; - out_7503564954401744509[67] = 0; - out_7503564954401744509[68] = 0; - out_7503564954401744509[69] = 0; - out_7503564954401744509[70] = 1; - out_7503564954401744509[71] = 0; - out_7503564954401744509[72] = 0; - out_7503564954401744509[73] = 0; - out_7503564954401744509[74] = 0; - out_7503564954401744509[75] = 0; - out_7503564954401744509[76] = 0; - out_7503564954401744509[77] = 0; - out_7503564954401744509[78] = 0; - out_7503564954401744509[79] = 0; - out_7503564954401744509[80] = 1; +void F_fun(double *state, double dt, double *out_8053926087528305413) { + out_8053926087528305413[0] = 1; + out_8053926087528305413[1] = 0; + out_8053926087528305413[2] = 0; + out_8053926087528305413[3] = 0; + out_8053926087528305413[4] = 0; + out_8053926087528305413[5] = 0; + out_8053926087528305413[6] = 0; + out_8053926087528305413[7] = 0; + out_8053926087528305413[8] = 0; + out_8053926087528305413[9] = 0; + out_8053926087528305413[10] = 1; + out_8053926087528305413[11] = 0; + out_8053926087528305413[12] = 0; + out_8053926087528305413[13] = 0; + out_8053926087528305413[14] = 0; + out_8053926087528305413[15] = 0; + out_8053926087528305413[16] = 0; + out_8053926087528305413[17] = 0; + out_8053926087528305413[18] = 0; + out_8053926087528305413[19] = 0; + out_8053926087528305413[20] = 1; + out_8053926087528305413[21] = 0; + out_8053926087528305413[22] = 0; + out_8053926087528305413[23] = 0; + out_8053926087528305413[24] = 0; + out_8053926087528305413[25] = 0; + out_8053926087528305413[26] = 0; + out_8053926087528305413[27] = 0; + out_8053926087528305413[28] = 0; + out_8053926087528305413[29] = 0; + out_8053926087528305413[30] = 1; + out_8053926087528305413[31] = 0; + out_8053926087528305413[32] = 0; + out_8053926087528305413[33] = 0; + out_8053926087528305413[34] = 0; + out_8053926087528305413[35] = 0; + out_8053926087528305413[36] = 0; + out_8053926087528305413[37] = 0; + out_8053926087528305413[38] = 0; + out_8053926087528305413[39] = 0; + out_8053926087528305413[40] = 1; + out_8053926087528305413[41] = 0; + out_8053926087528305413[42] = 0; + out_8053926087528305413[43] = 0; + out_8053926087528305413[44] = 0; + out_8053926087528305413[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_8053926087528305413[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_8053926087528305413[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8053926087528305413[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8053926087528305413[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_8053926087528305413[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_8053926087528305413[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_8053926087528305413[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_8053926087528305413[53] = -9.8100000000000005*dt; + out_8053926087528305413[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_8053926087528305413[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_8053926087528305413[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8053926087528305413[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8053926087528305413[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_8053926087528305413[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_8053926087528305413[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_8053926087528305413[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8053926087528305413[62] = 0; + out_8053926087528305413[63] = 0; + out_8053926087528305413[64] = 0; + out_8053926087528305413[65] = 0; + out_8053926087528305413[66] = 0; + out_8053926087528305413[67] = 0; + out_8053926087528305413[68] = 0; + out_8053926087528305413[69] = 0; + out_8053926087528305413[70] = 1; + out_8053926087528305413[71] = 0; + out_8053926087528305413[72] = 0; + out_8053926087528305413[73] = 0; + out_8053926087528305413[74] = 0; + out_8053926087528305413[75] = 0; + out_8053926087528305413[76] = 0; + out_8053926087528305413[77] = 0; + out_8053926087528305413[78] = 0; + out_8053926087528305413[79] = 0; + out_8053926087528305413[80] = 1; } -void h_25(double *state, double *unused, double *out_7864801379551668384) { - out_7864801379551668384[0] = state[6]; +void h_25(double *state, double *unused, double *out_5716369086896639497) { + out_5716369086896639497[0] = state[6]; } -void H_25(double *state, double *unused, double *out_7135350480076239544) { - out_7135350480076239544[0] = 0; - out_7135350480076239544[1] = 0; - out_7135350480076239544[2] = 0; - out_7135350480076239544[3] = 0; - out_7135350480076239544[4] = 0; - out_7135350480076239544[5] = 0; - out_7135350480076239544[6] = 1; - out_7135350480076239544[7] = 0; - out_7135350480076239544[8] = 0; +void H_25(double *state, double *unused, double *out_3877318319055969997) { + out_3877318319055969997[0] = 0; + out_3877318319055969997[1] = 0; + out_3877318319055969997[2] = 0; + out_3877318319055969997[3] = 0; + out_3877318319055969997[4] = 0; + out_3877318319055969997[5] = 0; + out_3877318319055969997[6] = 1; + out_3877318319055969997[7] = 0; + out_3877318319055969997[8] = 0; } -void h_24(double *state, double *unused, double *out_7538847613518647590) { - out_7538847613518647590[0] = state[4]; - out_7538847613518647590[1] = state[5]; +void h_24(double *state, double *unused, double *out_8949486097258288203) { + out_8949486097258288203[0] = state[4]; + out_8949486097258288203[1] = state[5]; } -void H_24(double *state, double *unused, double *out_3868719000644595752) { - out_3868719000644595752[0] = 0; - out_3868719000644595752[1] = 0; - out_3868719000644595752[2] = 0; - out_3868719000644595752[3] = 0; - out_3868719000644595752[4] = 1; - out_3868719000644595752[5] = 0; - out_3868719000644595752[6] = 0; - out_3868719000644595752[7] = 0; - out_3868719000644595752[8] = 0; - out_3868719000644595752[9] = 0; - out_3868719000644595752[10] = 0; - out_3868719000644595752[11] = 0; - out_3868719000644595752[12] = 0; - out_3868719000644595752[13] = 0; - out_3868719000644595752[14] = 1; - out_3868719000644595752[15] = 0; - out_3868719000644595752[16] = 0; - out_3868719000644595752[17] = 0; +void H_24(double *state, double *unused, double *out_6049967918061469563) { + out_6049967918061469563[0] = 0; + out_6049967918061469563[1] = 0; + out_6049967918061469563[2] = 0; + out_6049967918061469563[3] = 0; + out_6049967918061469563[4] = 1; + out_6049967918061469563[5] = 0; + out_6049967918061469563[6] = 0; + out_6049967918061469563[7] = 0; + out_6049967918061469563[8] = 0; + out_6049967918061469563[9] = 0; + out_6049967918061469563[10] = 0; + out_6049967918061469563[11] = 0; + out_6049967918061469563[12] = 0; + out_6049967918061469563[13] = 0; + out_6049967918061469563[14] = 1; + out_6049967918061469563[15] = 0; + out_6049967918061469563[16] = 0; + out_6049967918061469563[17] = 0; } -void h_30(double *state, double *unused, double *out_8021988047902797529) { - out_8021988047902797529[0] = state[4]; +void h_30(double *state, double *unused, double *out_5873555755247768642) { + out_5873555755247768642[0] = state[4]; } -void H_30(double *state, double *unused, double *out_6783697263505703874) { - out_6783697263505703874[0] = 0; - out_6783697263505703874[1] = 0; - out_6783697263505703874[2] = 0; - out_6783697263505703874[3] = 0; - out_6783697263505703874[4] = 1; - out_6783697263505703874[5] = 0; - out_6783697263505703874[6] = 0; - out_6783697263505703874[7] = 0; - out_6783697263505703874[8] = 0; +void H_30(double *state, double *unused, double *out_3039372022435646758) { + out_3039372022435646758[0] = 0; + out_3039372022435646758[1] = 0; + out_3039372022435646758[2] = 0; + out_3039372022435646758[3] = 0; + out_3039372022435646758[4] = 1; + out_3039372022435646758[5] = 0; + out_3039372022435646758[6] = 0; + out_3039372022435646758[7] = 0; + out_3039372022435646758[8] = 0; } -void h_26(double *state, double *unused, double *out_4978851268883674033) { - out_4978851268883674033[0] = state[7]; +void h_26(double *state, double *unused, double *out_8147827403163551441) { + out_8147827403163551441[0] = state[7]; } -void H_26(double *state, double *unused, double *out_7569890274759255848) { - out_7569890274759255848[0] = 0; - out_7569890274759255848[1] = 0; - out_7569890274759255848[2] = 0; - out_7569890274759255848[3] = 0; - out_7569890274759255848[4] = 0; - out_7569890274759255848[5] = 0; - out_7569890274759255848[6] = 0; - out_7569890274759255848[7] = 1; - out_7569890274759255848[8] = 0; +void H_26(double *state, double *unused, double *out_7618821637930026221) { + out_7618821637930026221[0] = 0; + out_7618821637930026221[1] = 0; + out_7618821637930026221[2] = 0; + out_7618821637930026221[3] = 0; + out_7618821637930026221[4] = 0; + out_7618821637930026221[5] = 0; + out_7618821637930026221[6] = 0; + out_7618821637930026221[7] = 1; + out_7618821637930026221[8] = 0; } -void h_27(double *state, double *unused, double *out_5096858662817050777) { - out_5096858662817050777[0] = state[3]; +void h_27(double *state, double *unused, double *out_3868275412420834157) { + out_3868275412420834157[0] = state[3]; } -void H_27(double *state, double *unused, double *out_9007291334689647091) { - out_9007291334689647091[0] = 0; - out_9007291334689647091[1] = 0; - out_9007291334689647091[2] = 0; - out_9007291334689647091[3] = 1; - out_9007291334689647091[4] = 0; - out_9007291334689647091[5] = 0; - out_9007291334689647091[6] = 0; - out_9007291334689647091[7] = 0; - out_9007291334689647091[8] = 0; +void H_27(double *state, double *unused, double *out_864608710635221847) { + out_864608710635221847[0] = 0; + out_864608710635221847[1] = 0; + out_864608710635221847[2] = 0; + out_864608710635221847[3] = 1; + out_864608710635221847[4] = 0; + out_864608710635221847[5] = 0; + out_864608710635221847[6] = 0; + out_864608710635221847[7] = 0; + out_864608710635221847[8] = 0; } -void h_29(double *state, double *unused, double *out_998555364537678913) { - out_998555364537678913[0] = state[1]; +void h_29(double *state, double *unused, double *out_2922753709018228498) { + out_2922753709018228498[0] = state[1]; } -void H_29(double *state, double *unused, double *out_7293928607820096058) { - out_7293928607820096058[0] = 0; - out_7293928607820096058[1] = 1; - out_7293928607820096058[2] = 0; - out_7293928607820096058[3] = 0; - out_7293928607820096058[4] = 0; - out_7293928607820096058[5] = 0; - out_7293928607820096058[6] = 0; - out_7293928607820096058[7] = 0; - out_7293928607820096058[8] = 0; +void H_29(double *state, double *unused, double *out_3549603366750038942) { + out_3549603366750038942[0] = 0; + out_3549603366750038942[1] = 1; + out_3549603366750038942[2] = 0; + out_3549603366750038942[3] = 0; + out_3549603366750038942[4] = 0; + out_3549603366750038942[5] = 0; + out_3549603366750038942[6] = 0; + out_3549603366750038942[7] = 0; + out_3549603366750038942[8] = 0; } -void h_28(double *state, double *unused, double *out_8372706475564507366) { - out_8372706475564507366[0] = state[0]; +void h_28(double *state, double *unused, double *out_2837524384607109846) { + out_2837524384607109846[0] = state[0]; } -void H_28(double *state, double *unused, double *out_9189185194324129307) { - out_9189185194324129307[0] = 1; - out_9189185194324129307[1] = 0; - out_9189185194324129307[2] = 0; - out_9189185194324129307[3] = 0; - out_9189185194324129307[4] = 0; - out_9189185194324129307[5] = 0; - out_9189185194324129307[6] = 0; - out_9189185194324129307[7] = 0; - out_9189185194324129307[8] = 0; +void H_28(double *state, double *unused, double *out_5931153033303859760) { + out_5931153033303859760[0] = 1; + out_5931153033303859760[1] = 0; + out_5931153033303859760[2] = 0; + out_5931153033303859760[3] = 0; + out_5931153033303859760[4] = 0; + out_5931153033303859760[5] = 0; + out_5931153033303859760[6] = 0; + out_5931153033303859760[7] = 0; + out_5931153033303859760[8] = 0; } -void h_31(double *state, double *unused, double *out_180008667128300757) { - out_180008667128300757[0] = state[8]; +void h_31(double *state, double *unused, double *out_7419317287452796635) { + out_7419317287452796635[0] = state[8]; } -void H_31(double *state, double *unused, double *out_6943682172525904372) { - out_6943682172525904372[0] = 0; - out_6943682172525904372[1] = 0; - out_6943682172525904372[2] = 0; - out_6943682172525904372[3] = 0; - out_6943682172525904372[4] = 0; - out_6943682172525904372[5] = 0; - out_6943682172525904372[6] = 0; - out_6943682172525904372[7] = 0; - out_6943682172525904372[8] = 1; +void H_31(double *state, double *unused, double *out_3846672357179009569) { + out_3846672357179009569[0] = 0; + out_3846672357179009569[1] = 0; + out_3846672357179009569[2] = 0; + out_3846672357179009569[3] = 0; + out_3846672357179009569[4] = 0; + out_3846672357179009569[5] = 0; + out_3846672357179009569[6] = 0; + out_3846672357179009569[7] = 0; + out_3846672357179009569[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_8558166113930061773) { - err_fun(nom_x, delta_x, out_8558166113930061773); +void car_err_fun(double *nom_x, double *delta_x, double *out_5063363100648596688) { + err_fun(nom_x, delta_x, out_5063363100648596688); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_2269670274683655268) { - inv_err_fun(nom_x, true_x, out_2269670274683655268); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1051973791362877167) { + inv_err_fun(nom_x, true_x, out_1051973791362877167); } -void car_H_mod_fun(double *state, double *out_6407701768448647232) { - H_mod_fun(state, out_6407701768448647232); +void car_H_mod_fun(double *state, double *out_5315869906856267240) { + H_mod_fun(state, out_5315869906856267240); } -void car_f_fun(double *state, double dt, double *out_648904423540471901) { - f_fun(state, dt, out_648904423540471901); +void car_f_fun(double *state, double dt, double *out_2679807483918754558) { + f_fun(state, dt, out_2679807483918754558); } -void car_F_fun(double *state, double dt, double *out_7503564954401744509) { - F_fun(state, dt, out_7503564954401744509); +void car_F_fun(double *state, double dt, double *out_8053926087528305413) { + F_fun(state, dt, out_8053926087528305413); } -void car_h_25(double *state, double *unused, double *out_7864801379551668384) { - h_25(state, unused, out_7864801379551668384); +void car_h_25(double *state, double *unused, double *out_5716369086896639497) { + h_25(state, unused, out_5716369086896639497); } -void car_H_25(double *state, double *unused, double *out_7135350480076239544) { - H_25(state, unused, out_7135350480076239544); +void car_H_25(double *state, double *unused, double *out_3877318319055969997) { + H_25(state, unused, out_3877318319055969997); } -void car_h_24(double *state, double *unused, double *out_7538847613518647590) { - h_24(state, unused, out_7538847613518647590); +void car_h_24(double *state, double *unused, double *out_8949486097258288203) { + h_24(state, unused, out_8949486097258288203); } -void car_H_24(double *state, double *unused, double *out_3868719000644595752) { - H_24(state, unused, out_3868719000644595752); +void car_H_24(double *state, double *unused, double *out_6049967918061469563) { + H_24(state, unused, out_6049967918061469563); } -void car_h_30(double *state, double *unused, double *out_8021988047902797529) { - h_30(state, unused, out_8021988047902797529); +void car_h_30(double *state, double *unused, double *out_5873555755247768642) { + h_30(state, unused, out_5873555755247768642); } -void car_H_30(double *state, double *unused, double *out_6783697263505703874) { - H_30(state, unused, out_6783697263505703874); +void car_H_30(double *state, double *unused, double *out_3039372022435646758) { + H_30(state, unused, out_3039372022435646758); } -void car_h_26(double *state, double *unused, double *out_4978851268883674033) { - h_26(state, unused, out_4978851268883674033); +void car_h_26(double *state, double *unused, double *out_8147827403163551441) { + h_26(state, unused, out_8147827403163551441); } -void car_H_26(double *state, double *unused, double *out_7569890274759255848) { - H_26(state, unused, out_7569890274759255848); +void car_H_26(double *state, double *unused, double *out_7618821637930026221) { + H_26(state, unused, out_7618821637930026221); } -void car_h_27(double *state, double *unused, double *out_5096858662817050777) { - h_27(state, unused, out_5096858662817050777); +void car_h_27(double *state, double *unused, double *out_3868275412420834157) { + h_27(state, unused, out_3868275412420834157); } -void car_H_27(double *state, double *unused, double *out_9007291334689647091) { - H_27(state, unused, out_9007291334689647091); +void car_H_27(double *state, double *unused, double *out_864608710635221847) { + H_27(state, unused, out_864608710635221847); } -void car_h_29(double *state, double *unused, double *out_998555364537678913) { - h_29(state, unused, out_998555364537678913); +void car_h_29(double *state, double *unused, double *out_2922753709018228498) { + h_29(state, unused, out_2922753709018228498); } -void car_H_29(double *state, double *unused, double *out_7293928607820096058) { - H_29(state, unused, out_7293928607820096058); +void car_H_29(double *state, double *unused, double *out_3549603366750038942) { + H_29(state, unused, out_3549603366750038942); } -void car_h_28(double *state, double *unused, double *out_8372706475564507366) { - h_28(state, unused, out_8372706475564507366); +void car_h_28(double *state, double *unused, double *out_2837524384607109846) { + h_28(state, unused, out_2837524384607109846); } -void car_H_28(double *state, double *unused, double *out_9189185194324129307) { - H_28(state, unused, out_9189185194324129307); +void car_H_28(double *state, double *unused, double *out_5931153033303859760) { + H_28(state, unused, out_5931153033303859760); } -void car_h_31(double *state, double *unused, double *out_180008667128300757) { - h_31(state, unused, out_180008667128300757); +void car_h_31(double *state, double *unused, double *out_7419317287452796635) { + h_31(state, unused, out_7419317287452796635); } -void car_H_31(double *state, double *unused, double *out_6943682172525904372) { - H_31(state, unused, out_6943682172525904372); +void car_H_31(double *state, double *unused, double *out_3846672357179009569) { + H_31(state, unused, out_3846672357179009569); } 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 e7a04d78b..ba5ed14f4 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_8558166113930061773); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_2269670274683655268); -void car_H_mod_fun(double *state, double *out_6407701768448647232); -void car_f_fun(double *state, double dt, double *out_648904423540471901); -void car_F_fun(double *state, double dt, double *out_7503564954401744509); -void car_h_25(double *state, double *unused, double *out_7864801379551668384); -void car_H_25(double *state, double *unused, double *out_7135350480076239544); -void car_h_24(double *state, double *unused, double *out_7538847613518647590); -void car_H_24(double *state, double *unused, double *out_3868719000644595752); -void car_h_30(double *state, double *unused, double *out_8021988047902797529); -void car_H_30(double *state, double *unused, double *out_6783697263505703874); -void car_h_26(double *state, double *unused, double *out_4978851268883674033); -void car_H_26(double *state, double *unused, double *out_7569890274759255848); -void car_h_27(double *state, double *unused, double *out_5096858662817050777); -void car_H_27(double *state, double *unused, double *out_9007291334689647091); -void car_h_29(double *state, double *unused, double *out_998555364537678913); -void car_H_29(double *state, double *unused, double *out_7293928607820096058); -void car_h_28(double *state, double *unused, double *out_8372706475564507366); -void car_H_28(double *state, double *unused, double *out_9189185194324129307); -void car_h_31(double *state, double *unused, double *out_180008667128300757); -void car_H_31(double *state, double *unused, double *out_6943682172525904372); +void car_err_fun(double *nom_x, double *delta_x, double *out_5063363100648596688); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1051973791362877167); +void car_H_mod_fun(double *state, double *out_5315869906856267240); +void car_f_fun(double *state, double dt, double *out_2679807483918754558); +void car_F_fun(double *state, double dt, double *out_8053926087528305413); +void car_h_25(double *state, double *unused, double *out_5716369086896639497); +void car_H_25(double *state, double *unused, double *out_3877318319055969997); +void car_h_24(double *state, double *unused, double *out_8949486097258288203); +void car_H_24(double *state, double *unused, double *out_6049967918061469563); +void car_h_30(double *state, double *unused, double *out_5873555755247768642); +void car_H_30(double *state, double *unused, double *out_3039372022435646758); +void car_h_26(double *state, double *unused, double *out_8147827403163551441); +void car_H_26(double *state, double *unused, double *out_7618821637930026221); +void car_h_27(double *state, double *unused, double *out_3868275412420834157); +void car_H_27(double *state, double *unused, double *out_864608710635221847); +void car_h_29(double *state, double *unused, double *out_2922753709018228498); +void car_H_29(double *state, double *unused, double *out_3549603366750038942); +void car_h_28(double *state, double *unused, double *out_2837524384607109846); +void car_H_28(double *state, double *unused, double *out_5931153033303859760); +void car_h_31(double *state, double *unused, double *out_7419317287452796635); +void car_H_31(double *state, double *unused, double *out_3846672357179009569); 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 7f075b686..9bf6cd5f6 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_3524911542636248309) { - out_3524911542636248309[0] = delta_x[0] + nom_x[0]; - out_3524911542636248309[1] = delta_x[1] + nom_x[1]; - out_3524911542636248309[2] = delta_x[2] + nom_x[2]; - out_3524911542636248309[3] = delta_x[3] + nom_x[3]; - out_3524911542636248309[4] = delta_x[4] + nom_x[4]; - out_3524911542636248309[5] = delta_x[5] + nom_x[5]; - out_3524911542636248309[6] = delta_x[6] + nom_x[6]; - out_3524911542636248309[7] = delta_x[7] + nom_x[7]; - out_3524911542636248309[8] = delta_x[8] + nom_x[8]; - out_3524911542636248309[9] = delta_x[9] + nom_x[9]; - out_3524911542636248309[10] = delta_x[10] + nom_x[10]; - out_3524911542636248309[11] = delta_x[11] + nom_x[11]; - out_3524911542636248309[12] = delta_x[12] + nom_x[12]; - out_3524911542636248309[13] = delta_x[13] + nom_x[13]; - out_3524911542636248309[14] = delta_x[14] + nom_x[14]; - out_3524911542636248309[15] = delta_x[15] + nom_x[15]; - out_3524911542636248309[16] = delta_x[16] + nom_x[16]; - out_3524911542636248309[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_3341780452292505997) { + out_3341780452292505997[0] = delta_x[0] + nom_x[0]; + out_3341780452292505997[1] = delta_x[1] + nom_x[1]; + out_3341780452292505997[2] = delta_x[2] + nom_x[2]; + out_3341780452292505997[3] = delta_x[3] + nom_x[3]; + out_3341780452292505997[4] = delta_x[4] + nom_x[4]; + out_3341780452292505997[5] = delta_x[5] + nom_x[5]; + out_3341780452292505997[6] = delta_x[6] + nom_x[6]; + out_3341780452292505997[7] = delta_x[7] + nom_x[7]; + out_3341780452292505997[8] = delta_x[8] + nom_x[8]; + out_3341780452292505997[9] = delta_x[9] + nom_x[9]; + out_3341780452292505997[10] = delta_x[10] + nom_x[10]; + out_3341780452292505997[11] = delta_x[11] + nom_x[11]; + out_3341780452292505997[12] = delta_x[12] + nom_x[12]; + out_3341780452292505997[13] = delta_x[13] + nom_x[13]; + out_3341780452292505997[14] = delta_x[14] + nom_x[14]; + out_3341780452292505997[15] = delta_x[15] + nom_x[15]; + out_3341780452292505997[16] = delta_x[16] + nom_x[16]; + out_3341780452292505997[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_9101758542759318568) { - out_9101758542759318568[0] = -nom_x[0] + true_x[0]; - out_9101758542759318568[1] = -nom_x[1] + true_x[1]; - out_9101758542759318568[2] = -nom_x[2] + true_x[2]; - out_9101758542759318568[3] = -nom_x[3] + true_x[3]; - out_9101758542759318568[4] = -nom_x[4] + true_x[4]; - out_9101758542759318568[5] = -nom_x[5] + true_x[5]; - out_9101758542759318568[6] = -nom_x[6] + true_x[6]; - out_9101758542759318568[7] = -nom_x[7] + true_x[7]; - out_9101758542759318568[8] = -nom_x[8] + true_x[8]; - out_9101758542759318568[9] = -nom_x[9] + true_x[9]; - out_9101758542759318568[10] = -nom_x[10] + true_x[10]; - out_9101758542759318568[11] = -nom_x[11] + true_x[11]; - out_9101758542759318568[12] = -nom_x[12] + true_x[12]; - out_9101758542759318568[13] = -nom_x[13] + true_x[13]; - out_9101758542759318568[14] = -nom_x[14] + true_x[14]; - out_9101758542759318568[15] = -nom_x[15] + true_x[15]; - out_9101758542759318568[16] = -nom_x[16] + true_x[16]; - out_9101758542759318568[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5409150910353518766) { + out_5409150910353518766[0] = -nom_x[0] + true_x[0]; + out_5409150910353518766[1] = -nom_x[1] + true_x[1]; + out_5409150910353518766[2] = -nom_x[2] + true_x[2]; + out_5409150910353518766[3] = -nom_x[3] + true_x[3]; + out_5409150910353518766[4] = -nom_x[4] + true_x[4]; + out_5409150910353518766[5] = -nom_x[5] + true_x[5]; + out_5409150910353518766[6] = -nom_x[6] + true_x[6]; + out_5409150910353518766[7] = -nom_x[7] + true_x[7]; + out_5409150910353518766[8] = -nom_x[8] + true_x[8]; + out_5409150910353518766[9] = -nom_x[9] + true_x[9]; + out_5409150910353518766[10] = -nom_x[10] + true_x[10]; + out_5409150910353518766[11] = -nom_x[11] + true_x[11]; + out_5409150910353518766[12] = -nom_x[12] + true_x[12]; + out_5409150910353518766[13] = -nom_x[13] + true_x[13]; + out_5409150910353518766[14] = -nom_x[14] + true_x[14]; + out_5409150910353518766[15] = -nom_x[15] + true_x[15]; + out_5409150910353518766[16] = -nom_x[16] + true_x[16]; + out_5409150910353518766[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_202451455870783186) { - out_202451455870783186[0] = 1.0; - out_202451455870783186[1] = 0.0; - out_202451455870783186[2] = 0.0; - out_202451455870783186[3] = 0.0; - out_202451455870783186[4] = 0.0; - out_202451455870783186[5] = 0.0; - out_202451455870783186[6] = 0.0; - out_202451455870783186[7] = 0.0; - out_202451455870783186[8] = 0.0; - out_202451455870783186[9] = 0.0; - out_202451455870783186[10] = 0.0; - out_202451455870783186[11] = 0.0; - out_202451455870783186[12] = 0.0; - out_202451455870783186[13] = 0.0; - out_202451455870783186[14] = 0.0; - out_202451455870783186[15] = 0.0; - out_202451455870783186[16] = 0.0; - out_202451455870783186[17] = 0.0; - out_202451455870783186[18] = 0.0; - out_202451455870783186[19] = 1.0; - out_202451455870783186[20] = 0.0; - out_202451455870783186[21] = 0.0; - out_202451455870783186[22] = 0.0; - out_202451455870783186[23] = 0.0; - out_202451455870783186[24] = 0.0; - out_202451455870783186[25] = 0.0; - out_202451455870783186[26] = 0.0; - out_202451455870783186[27] = 0.0; - out_202451455870783186[28] = 0.0; - out_202451455870783186[29] = 0.0; - out_202451455870783186[30] = 0.0; - out_202451455870783186[31] = 0.0; - out_202451455870783186[32] = 0.0; - out_202451455870783186[33] = 0.0; - out_202451455870783186[34] = 0.0; - out_202451455870783186[35] = 0.0; - out_202451455870783186[36] = 0.0; - out_202451455870783186[37] = 0.0; - out_202451455870783186[38] = 1.0; - out_202451455870783186[39] = 0.0; - out_202451455870783186[40] = 0.0; - out_202451455870783186[41] = 0.0; - out_202451455870783186[42] = 0.0; - out_202451455870783186[43] = 0.0; - out_202451455870783186[44] = 0.0; - out_202451455870783186[45] = 0.0; - out_202451455870783186[46] = 0.0; - out_202451455870783186[47] = 0.0; - out_202451455870783186[48] = 0.0; - out_202451455870783186[49] = 0.0; - out_202451455870783186[50] = 0.0; - out_202451455870783186[51] = 0.0; - out_202451455870783186[52] = 0.0; - out_202451455870783186[53] = 0.0; - out_202451455870783186[54] = 0.0; - out_202451455870783186[55] = 0.0; - out_202451455870783186[56] = 0.0; - out_202451455870783186[57] = 1.0; - out_202451455870783186[58] = 0.0; - out_202451455870783186[59] = 0.0; - out_202451455870783186[60] = 0.0; - out_202451455870783186[61] = 0.0; - out_202451455870783186[62] = 0.0; - out_202451455870783186[63] = 0.0; - out_202451455870783186[64] = 0.0; - out_202451455870783186[65] = 0.0; - out_202451455870783186[66] = 0.0; - out_202451455870783186[67] = 0.0; - out_202451455870783186[68] = 0.0; - out_202451455870783186[69] = 0.0; - out_202451455870783186[70] = 0.0; - out_202451455870783186[71] = 0.0; - out_202451455870783186[72] = 0.0; - out_202451455870783186[73] = 0.0; - out_202451455870783186[74] = 0.0; - out_202451455870783186[75] = 0.0; - out_202451455870783186[76] = 1.0; - out_202451455870783186[77] = 0.0; - out_202451455870783186[78] = 0.0; - out_202451455870783186[79] = 0.0; - out_202451455870783186[80] = 0.0; - out_202451455870783186[81] = 0.0; - out_202451455870783186[82] = 0.0; - out_202451455870783186[83] = 0.0; - out_202451455870783186[84] = 0.0; - out_202451455870783186[85] = 0.0; - out_202451455870783186[86] = 0.0; - out_202451455870783186[87] = 0.0; - out_202451455870783186[88] = 0.0; - out_202451455870783186[89] = 0.0; - out_202451455870783186[90] = 0.0; - out_202451455870783186[91] = 0.0; - out_202451455870783186[92] = 0.0; - out_202451455870783186[93] = 0.0; - out_202451455870783186[94] = 0.0; - out_202451455870783186[95] = 1.0; - out_202451455870783186[96] = 0.0; - out_202451455870783186[97] = 0.0; - out_202451455870783186[98] = 0.0; - out_202451455870783186[99] = 0.0; - out_202451455870783186[100] = 0.0; - out_202451455870783186[101] = 0.0; - out_202451455870783186[102] = 0.0; - out_202451455870783186[103] = 0.0; - out_202451455870783186[104] = 0.0; - out_202451455870783186[105] = 0.0; - out_202451455870783186[106] = 0.0; - out_202451455870783186[107] = 0.0; - out_202451455870783186[108] = 0.0; - out_202451455870783186[109] = 0.0; - out_202451455870783186[110] = 0.0; - out_202451455870783186[111] = 0.0; - out_202451455870783186[112] = 0.0; - out_202451455870783186[113] = 0.0; - out_202451455870783186[114] = 1.0; - out_202451455870783186[115] = 0.0; - out_202451455870783186[116] = 0.0; - out_202451455870783186[117] = 0.0; - out_202451455870783186[118] = 0.0; - out_202451455870783186[119] = 0.0; - out_202451455870783186[120] = 0.0; - out_202451455870783186[121] = 0.0; - out_202451455870783186[122] = 0.0; - out_202451455870783186[123] = 0.0; - out_202451455870783186[124] = 0.0; - out_202451455870783186[125] = 0.0; - out_202451455870783186[126] = 0.0; - out_202451455870783186[127] = 0.0; - out_202451455870783186[128] = 0.0; - out_202451455870783186[129] = 0.0; - out_202451455870783186[130] = 0.0; - out_202451455870783186[131] = 0.0; - out_202451455870783186[132] = 0.0; - out_202451455870783186[133] = 1.0; - out_202451455870783186[134] = 0.0; - out_202451455870783186[135] = 0.0; - out_202451455870783186[136] = 0.0; - out_202451455870783186[137] = 0.0; - out_202451455870783186[138] = 0.0; - out_202451455870783186[139] = 0.0; - out_202451455870783186[140] = 0.0; - out_202451455870783186[141] = 0.0; - out_202451455870783186[142] = 0.0; - out_202451455870783186[143] = 0.0; - out_202451455870783186[144] = 0.0; - out_202451455870783186[145] = 0.0; - out_202451455870783186[146] = 0.0; - out_202451455870783186[147] = 0.0; - out_202451455870783186[148] = 0.0; - out_202451455870783186[149] = 0.0; - out_202451455870783186[150] = 0.0; - out_202451455870783186[151] = 0.0; - out_202451455870783186[152] = 1.0; - out_202451455870783186[153] = 0.0; - out_202451455870783186[154] = 0.0; - out_202451455870783186[155] = 0.0; - out_202451455870783186[156] = 0.0; - out_202451455870783186[157] = 0.0; - out_202451455870783186[158] = 0.0; - out_202451455870783186[159] = 0.0; - out_202451455870783186[160] = 0.0; - out_202451455870783186[161] = 0.0; - out_202451455870783186[162] = 0.0; - out_202451455870783186[163] = 0.0; - out_202451455870783186[164] = 0.0; - out_202451455870783186[165] = 0.0; - out_202451455870783186[166] = 0.0; - out_202451455870783186[167] = 0.0; - out_202451455870783186[168] = 0.0; - out_202451455870783186[169] = 0.0; - out_202451455870783186[170] = 0.0; - out_202451455870783186[171] = 1.0; - out_202451455870783186[172] = 0.0; - out_202451455870783186[173] = 0.0; - out_202451455870783186[174] = 0.0; - out_202451455870783186[175] = 0.0; - out_202451455870783186[176] = 0.0; - out_202451455870783186[177] = 0.0; - out_202451455870783186[178] = 0.0; - out_202451455870783186[179] = 0.0; - out_202451455870783186[180] = 0.0; - out_202451455870783186[181] = 0.0; - out_202451455870783186[182] = 0.0; - out_202451455870783186[183] = 0.0; - out_202451455870783186[184] = 0.0; - out_202451455870783186[185] = 0.0; - out_202451455870783186[186] = 0.0; - out_202451455870783186[187] = 0.0; - out_202451455870783186[188] = 0.0; - out_202451455870783186[189] = 0.0; - out_202451455870783186[190] = 1.0; - out_202451455870783186[191] = 0.0; - out_202451455870783186[192] = 0.0; - out_202451455870783186[193] = 0.0; - out_202451455870783186[194] = 0.0; - out_202451455870783186[195] = 0.0; - out_202451455870783186[196] = 0.0; - out_202451455870783186[197] = 0.0; - out_202451455870783186[198] = 0.0; - out_202451455870783186[199] = 0.0; - out_202451455870783186[200] = 0.0; - out_202451455870783186[201] = 0.0; - out_202451455870783186[202] = 0.0; - out_202451455870783186[203] = 0.0; - out_202451455870783186[204] = 0.0; - out_202451455870783186[205] = 0.0; - out_202451455870783186[206] = 0.0; - out_202451455870783186[207] = 0.0; - out_202451455870783186[208] = 0.0; - out_202451455870783186[209] = 1.0; - out_202451455870783186[210] = 0.0; - out_202451455870783186[211] = 0.0; - out_202451455870783186[212] = 0.0; - out_202451455870783186[213] = 0.0; - out_202451455870783186[214] = 0.0; - out_202451455870783186[215] = 0.0; - out_202451455870783186[216] = 0.0; - out_202451455870783186[217] = 0.0; - out_202451455870783186[218] = 0.0; - out_202451455870783186[219] = 0.0; - out_202451455870783186[220] = 0.0; - out_202451455870783186[221] = 0.0; - out_202451455870783186[222] = 0.0; - out_202451455870783186[223] = 0.0; - out_202451455870783186[224] = 0.0; - out_202451455870783186[225] = 0.0; - out_202451455870783186[226] = 0.0; - out_202451455870783186[227] = 0.0; - out_202451455870783186[228] = 1.0; - out_202451455870783186[229] = 0.0; - out_202451455870783186[230] = 0.0; - out_202451455870783186[231] = 0.0; - out_202451455870783186[232] = 0.0; - out_202451455870783186[233] = 0.0; - out_202451455870783186[234] = 0.0; - out_202451455870783186[235] = 0.0; - out_202451455870783186[236] = 0.0; - out_202451455870783186[237] = 0.0; - out_202451455870783186[238] = 0.0; - out_202451455870783186[239] = 0.0; - out_202451455870783186[240] = 0.0; - out_202451455870783186[241] = 0.0; - out_202451455870783186[242] = 0.0; - out_202451455870783186[243] = 0.0; - out_202451455870783186[244] = 0.0; - out_202451455870783186[245] = 0.0; - out_202451455870783186[246] = 0.0; - out_202451455870783186[247] = 1.0; - out_202451455870783186[248] = 0.0; - out_202451455870783186[249] = 0.0; - out_202451455870783186[250] = 0.0; - out_202451455870783186[251] = 0.0; - out_202451455870783186[252] = 0.0; - out_202451455870783186[253] = 0.0; - out_202451455870783186[254] = 0.0; - out_202451455870783186[255] = 0.0; - out_202451455870783186[256] = 0.0; - out_202451455870783186[257] = 0.0; - out_202451455870783186[258] = 0.0; - out_202451455870783186[259] = 0.0; - out_202451455870783186[260] = 0.0; - out_202451455870783186[261] = 0.0; - out_202451455870783186[262] = 0.0; - out_202451455870783186[263] = 0.0; - out_202451455870783186[264] = 0.0; - out_202451455870783186[265] = 0.0; - out_202451455870783186[266] = 1.0; - out_202451455870783186[267] = 0.0; - out_202451455870783186[268] = 0.0; - out_202451455870783186[269] = 0.0; - out_202451455870783186[270] = 0.0; - out_202451455870783186[271] = 0.0; - out_202451455870783186[272] = 0.0; - out_202451455870783186[273] = 0.0; - out_202451455870783186[274] = 0.0; - out_202451455870783186[275] = 0.0; - out_202451455870783186[276] = 0.0; - out_202451455870783186[277] = 0.0; - out_202451455870783186[278] = 0.0; - out_202451455870783186[279] = 0.0; - out_202451455870783186[280] = 0.0; - out_202451455870783186[281] = 0.0; - out_202451455870783186[282] = 0.0; - out_202451455870783186[283] = 0.0; - out_202451455870783186[284] = 0.0; - out_202451455870783186[285] = 1.0; - out_202451455870783186[286] = 0.0; - out_202451455870783186[287] = 0.0; - out_202451455870783186[288] = 0.0; - out_202451455870783186[289] = 0.0; - out_202451455870783186[290] = 0.0; - out_202451455870783186[291] = 0.0; - out_202451455870783186[292] = 0.0; - out_202451455870783186[293] = 0.0; - out_202451455870783186[294] = 0.0; - out_202451455870783186[295] = 0.0; - out_202451455870783186[296] = 0.0; - out_202451455870783186[297] = 0.0; - out_202451455870783186[298] = 0.0; - out_202451455870783186[299] = 0.0; - out_202451455870783186[300] = 0.0; - out_202451455870783186[301] = 0.0; - out_202451455870783186[302] = 0.0; - out_202451455870783186[303] = 0.0; - out_202451455870783186[304] = 1.0; - out_202451455870783186[305] = 0.0; - out_202451455870783186[306] = 0.0; - out_202451455870783186[307] = 0.0; - out_202451455870783186[308] = 0.0; - out_202451455870783186[309] = 0.0; - out_202451455870783186[310] = 0.0; - out_202451455870783186[311] = 0.0; - out_202451455870783186[312] = 0.0; - out_202451455870783186[313] = 0.0; - out_202451455870783186[314] = 0.0; - out_202451455870783186[315] = 0.0; - out_202451455870783186[316] = 0.0; - out_202451455870783186[317] = 0.0; - out_202451455870783186[318] = 0.0; - out_202451455870783186[319] = 0.0; - out_202451455870783186[320] = 0.0; - out_202451455870783186[321] = 0.0; - out_202451455870783186[322] = 0.0; - out_202451455870783186[323] = 1.0; +void H_mod_fun(double *state, double *out_8300172317114018464) { + out_8300172317114018464[0] = 1.0; + out_8300172317114018464[1] = 0.0; + out_8300172317114018464[2] = 0.0; + out_8300172317114018464[3] = 0.0; + out_8300172317114018464[4] = 0.0; + out_8300172317114018464[5] = 0.0; + out_8300172317114018464[6] = 0.0; + out_8300172317114018464[7] = 0.0; + out_8300172317114018464[8] = 0.0; + out_8300172317114018464[9] = 0.0; + out_8300172317114018464[10] = 0.0; + out_8300172317114018464[11] = 0.0; + out_8300172317114018464[12] = 0.0; + out_8300172317114018464[13] = 0.0; + out_8300172317114018464[14] = 0.0; + out_8300172317114018464[15] = 0.0; + out_8300172317114018464[16] = 0.0; + out_8300172317114018464[17] = 0.0; + out_8300172317114018464[18] = 0.0; + out_8300172317114018464[19] = 1.0; + out_8300172317114018464[20] = 0.0; + out_8300172317114018464[21] = 0.0; + out_8300172317114018464[22] = 0.0; + out_8300172317114018464[23] = 0.0; + out_8300172317114018464[24] = 0.0; + out_8300172317114018464[25] = 0.0; + out_8300172317114018464[26] = 0.0; + out_8300172317114018464[27] = 0.0; + out_8300172317114018464[28] = 0.0; + out_8300172317114018464[29] = 0.0; + out_8300172317114018464[30] = 0.0; + out_8300172317114018464[31] = 0.0; + out_8300172317114018464[32] = 0.0; + out_8300172317114018464[33] = 0.0; + out_8300172317114018464[34] = 0.0; + out_8300172317114018464[35] = 0.0; + out_8300172317114018464[36] = 0.0; + out_8300172317114018464[37] = 0.0; + out_8300172317114018464[38] = 1.0; + out_8300172317114018464[39] = 0.0; + out_8300172317114018464[40] = 0.0; + out_8300172317114018464[41] = 0.0; + out_8300172317114018464[42] = 0.0; + out_8300172317114018464[43] = 0.0; + out_8300172317114018464[44] = 0.0; + out_8300172317114018464[45] = 0.0; + out_8300172317114018464[46] = 0.0; + out_8300172317114018464[47] = 0.0; + out_8300172317114018464[48] = 0.0; + out_8300172317114018464[49] = 0.0; + out_8300172317114018464[50] = 0.0; + out_8300172317114018464[51] = 0.0; + out_8300172317114018464[52] = 0.0; + out_8300172317114018464[53] = 0.0; + out_8300172317114018464[54] = 0.0; + out_8300172317114018464[55] = 0.0; + out_8300172317114018464[56] = 0.0; + out_8300172317114018464[57] = 1.0; + out_8300172317114018464[58] = 0.0; + out_8300172317114018464[59] = 0.0; + out_8300172317114018464[60] = 0.0; + out_8300172317114018464[61] = 0.0; + out_8300172317114018464[62] = 0.0; + out_8300172317114018464[63] = 0.0; + out_8300172317114018464[64] = 0.0; + out_8300172317114018464[65] = 0.0; + out_8300172317114018464[66] = 0.0; + out_8300172317114018464[67] = 0.0; + out_8300172317114018464[68] = 0.0; + out_8300172317114018464[69] = 0.0; + out_8300172317114018464[70] = 0.0; + out_8300172317114018464[71] = 0.0; + out_8300172317114018464[72] = 0.0; + out_8300172317114018464[73] = 0.0; + out_8300172317114018464[74] = 0.0; + out_8300172317114018464[75] = 0.0; + out_8300172317114018464[76] = 1.0; + out_8300172317114018464[77] = 0.0; + out_8300172317114018464[78] = 0.0; + out_8300172317114018464[79] = 0.0; + out_8300172317114018464[80] = 0.0; + out_8300172317114018464[81] = 0.0; + out_8300172317114018464[82] = 0.0; + out_8300172317114018464[83] = 0.0; + out_8300172317114018464[84] = 0.0; + out_8300172317114018464[85] = 0.0; + out_8300172317114018464[86] = 0.0; + out_8300172317114018464[87] = 0.0; + out_8300172317114018464[88] = 0.0; + out_8300172317114018464[89] = 0.0; + out_8300172317114018464[90] = 0.0; + out_8300172317114018464[91] = 0.0; + out_8300172317114018464[92] = 0.0; + out_8300172317114018464[93] = 0.0; + out_8300172317114018464[94] = 0.0; + out_8300172317114018464[95] = 1.0; + out_8300172317114018464[96] = 0.0; + out_8300172317114018464[97] = 0.0; + out_8300172317114018464[98] = 0.0; + out_8300172317114018464[99] = 0.0; + out_8300172317114018464[100] = 0.0; + out_8300172317114018464[101] = 0.0; + out_8300172317114018464[102] = 0.0; + out_8300172317114018464[103] = 0.0; + out_8300172317114018464[104] = 0.0; + out_8300172317114018464[105] = 0.0; + out_8300172317114018464[106] = 0.0; + out_8300172317114018464[107] = 0.0; + out_8300172317114018464[108] = 0.0; + out_8300172317114018464[109] = 0.0; + out_8300172317114018464[110] = 0.0; + out_8300172317114018464[111] = 0.0; + out_8300172317114018464[112] = 0.0; + out_8300172317114018464[113] = 0.0; + out_8300172317114018464[114] = 1.0; + out_8300172317114018464[115] = 0.0; + out_8300172317114018464[116] = 0.0; + out_8300172317114018464[117] = 0.0; + out_8300172317114018464[118] = 0.0; + out_8300172317114018464[119] = 0.0; + out_8300172317114018464[120] = 0.0; + out_8300172317114018464[121] = 0.0; + out_8300172317114018464[122] = 0.0; + out_8300172317114018464[123] = 0.0; + out_8300172317114018464[124] = 0.0; + out_8300172317114018464[125] = 0.0; + out_8300172317114018464[126] = 0.0; + out_8300172317114018464[127] = 0.0; + out_8300172317114018464[128] = 0.0; + out_8300172317114018464[129] = 0.0; + out_8300172317114018464[130] = 0.0; + out_8300172317114018464[131] = 0.0; + out_8300172317114018464[132] = 0.0; + out_8300172317114018464[133] = 1.0; + out_8300172317114018464[134] = 0.0; + out_8300172317114018464[135] = 0.0; + out_8300172317114018464[136] = 0.0; + out_8300172317114018464[137] = 0.0; + out_8300172317114018464[138] = 0.0; + out_8300172317114018464[139] = 0.0; + out_8300172317114018464[140] = 0.0; + out_8300172317114018464[141] = 0.0; + out_8300172317114018464[142] = 0.0; + out_8300172317114018464[143] = 0.0; + out_8300172317114018464[144] = 0.0; + out_8300172317114018464[145] = 0.0; + out_8300172317114018464[146] = 0.0; + out_8300172317114018464[147] = 0.0; + out_8300172317114018464[148] = 0.0; + out_8300172317114018464[149] = 0.0; + out_8300172317114018464[150] = 0.0; + out_8300172317114018464[151] = 0.0; + out_8300172317114018464[152] = 1.0; + out_8300172317114018464[153] = 0.0; + out_8300172317114018464[154] = 0.0; + out_8300172317114018464[155] = 0.0; + out_8300172317114018464[156] = 0.0; + out_8300172317114018464[157] = 0.0; + out_8300172317114018464[158] = 0.0; + out_8300172317114018464[159] = 0.0; + out_8300172317114018464[160] = 0.0; + out_8300172317114018464[161] = 0.0; + out_8300172317114018464[162] = 0.0; + out_8300172317114018464[163] = 0.0; + out_8300172317114018464[164] = 0.0; + out_8300172317114018464[165] = 0.0; + out_8300172317114018464[166] = 0.0; + out_8300172317114018464[167] = 0.0; + out_8300172317114018464[168] = 0.0; + out_8300172317114018464[169] = 0.0; + out_8300172317114018464[170] = 0.0; + out_8300172317114018464[171] = 1.0; + out_8300172317114018464[172] = 0.0; + out_8300172317114018464[173] = 0.0; + out_8300172317114018464[174] = 0.0; + out_8300172317114018464[175] = 0.0; + out_8300172317114018464[176] = 0.0; + out_8300172317114018464[177] = 0.0; + out_8300172317114018464[178] = 0.0; + out_8300172317114018464[179] = 0.0; + out_8300172317114018464[180] = 0.0; + out_8300172317114018464[181] = 0.0; + out_8300172317114018464[182] = 0.0; + out_8300172317114018464[183] = 0.0; + out_8300172317114018464[184] = 0.0; + out_8300172317114018464[185] = 0.0; + out_8300172317114018464[186] = 0.0; + out_8300172317114018464[187] = 0.0; + out_8300172317114018464[188] = 0.0; + out_8300172317114018464[189] = 0.0; + out_8300172317114018464[190] = 1.0; + out_8300172317114018464[191] = 0.0; + out_8300172317114018464[192] = 0.0; + out_8300172317114018464[193] = 0.0; + out_8300172317114018464[194] = 0.0; + out_8300172317114018464[195] = 0.0; + out_8300172317114018464[196] = 0.0; + out_8300172317114018464[197] = 0.0; + out_8300172317114018464[198] = 0.0; + out_8300172317114018464[199] = 0.0; + out_8300172317114018464[200] = 0.0; + out_8300172317114018464[201] = 0.0; + out_8300172317114018464[202] = 0.0; + out_8300172317114018464[203] = 0.0; + out_8300172317114018464[204] = 0.0; + out_8300172317114018464[205] = 0.0; + out_8300172317114018464[206] = 0.0; + out_8300172317114018464[207] = 0.0; + out_8300172317114018464[208] = 0.0; + out_8300172317114018464[209] = 1.0; + out_8300172317114018464[210] = 0.0; + out_8300172317114018464[211] = 0.0; + out_8300172317114018464[212] = 0.0; + out_8300172317114018464[213] = 0.0; + out_8300172317114018464[214] = 0.0; + out_8300172317114018464[215] = 0.0; + out_8300172317114018464[216] = 0.0; + out_8300172317114018464[217] = 0.0; + out_8300172317114018464[218] = 0.0; + out_8300172317114018464[219] = 0.0; + out_8300172317114018464[220] = 0.0; + out_8300172317114018464[221] = 0.0; + out_8300172317114018464[222] = 0.0; + out_8300172317114018464[223] = 0.0; + out_8300172317114018464[224] = 0.0; + out_8300172317114018464[225] = 0.0; + out_8300172317114018464[226] = 0.0; + out_8300172317114018464[227] = 0.0; + out_8300172317114018464[228] = 1.0; + out_8300172317114018464[229] = 0.0; + out_8300172317114018464[230] = 0.0; + out_8300172317114018464[231] = 0.0; + out_8300172317114018464[232] = 0.0; + out_8300172317114018464[233] = 0.0; + out_8300172317114018464[234] = 0.0; + out_8300172317114018464[235] = 0.0; + out_8300172317114018464[236] = 0.0; + out_8300172317114018464[237] = 0.0; + out_8300172317114018464[238] = 0.0; + out_8300172317114018464[239] = 0.0; + out_8300172317114018464[240] = 0.0; + out_8300172317114018464[241] = 0.0; + out_8300172317114018464[242] = 0.0; + out_8300172317114018464[243] = 0.0; + out_8300172317114018464[244] = 0.0; + out_8300172317114018464[245] = 0.0; + out_8300172317114018464[246] = 0.0; + out_8300172317114018464[247] = 1.0; + out_8300172317114018464[248] = 0.0; + out_8300172317114018464[249] = 0.0; + out_8300172317114018464[250] = 0.0; + out_8300172317114018464[251] = 0.0; + out_8300172317114018464[252] = 0.0; + out_8300172317114018464[253] = 0.0; + out_8300172317114018464[254] = 0.0; + out_8300172317114018464[255] = 0.0; + out_8300172317114018464[256] = 0.0; + out_8300172317114018464[257] = 0.0; + out_8300172317114018464[258] = 0.0; + out_8300172317114018464[259] = 0.0; + out_8300172317114018464[260] = 0.0; + out_8300172317114018464[261] = 0.0; + out_8300172317114018464[262] = 0.0; + out_8300172317114018464[263] = 0.0; + out_8300172317114018464[264] = 0.0; + out_8300172317114018464[265] = 0.0; + out_8300172317114018464[266] = 1.0; + out_8300172317114018464[267] = 0.0; + out_8300172317114018464[268] = 0.0; + out_8300172317114018464[269] = 0.0; + out_8300172317114018464[270] = 0.0; + out_8300172317114018464[271] = 0.0; + out_8300172317114018464[272] = 0.0; + out_8300172317114018464[273] = 0.0; + out_8300172317114018464[274] = 0.0; + out_8300172317114018464[275] = 0.0; + out_8300172317114018464[276] = 0.0; + out_8300172317114018464[277] = 0.0; + out_8300172317114018464[278] = 0.0; + out_8300172317114018464[279] = 0.0; + out_8300172317114018464[280] = 0.0; + out_8300172317114018464[281] = 0.0; + out_8300172317114018464[282] = 0.0; + out_8300172317114018464[283] = 0.0; + out_8300172317114018464[284] = 0.0; + out_8300172317114018464[285] = 1.0; + out_8300172317114018464[286] = 0.0; + out_8300172317114018464[287] = 0.0; + out_8300172317114018464[288] = 0.0; + out_8300172317114018464[289] = 0.0; + out_8300172317114018464[290] = 0.0; + out_8300172317114018464[291] = 0.0; + out_8300172317114018464[292] = 0.0; + out_8300172317114018464[293] = 0.0; + out_8300172317114018464[294] = 0.0; + out_8300172317114018464[295] = 0.0; + out_8300172317114018464[296] = 0.0; + out_8300172317114018464[297] = 0.0; + out_8300172317114018464[298] = 0.0; + out_8300172317114018464[299] = 0.0; + out_8300172317114018464[300] = 0.0; + out_8300172317114018464[301] = 0.0; + out_8300172317114018464[302] = 0.0; + out_8300172317114018464[303] = 0.0; + out_8300172317114018464[304] = 1.0; + out_8300172317114018464[305] = 0.0; + out_8300172317114018464[306] = 0.0; + out_8300172317114018464[307] = 0.0; + out_8300172317114018464[308] = 0.0; + out_8300172317114018464[309] = 0.0; + out_8300172317114018464[310] = 0.0; + out_8300172317114018464[311] = 0.0; + out_8300172317114018464[312] = 0.0; + out_8300172317114018464[313] = 0.0; + out_8300172317114018464[314] = 0.0; + out_8300172317114018464[315] = 0.0; + out_8300172317114018464[316] = 0.0; + out_8300172317114018464[317] = 0.0; + out_8300172317114018464[318] = 0.0; + out_8300172317114018464[319] = 0.0; + out_8300172317114018464[320] = 0.0; + out_8300172317114018464[321] = 0.0; + out_8300172317114018464[322] = 0.0; + out_8300172317114018464[323] = 1.0; } -void f_fun(double *state, double dt, double *out_3404078345216964699) { - out_3404078345216964699[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_3404078345216964699[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_3404078345216964699[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_3404078345216964699[3] = dt*state[12] + state[3]; - out_3404078345216964699[4] = dt*state[13] + state[4]; - out_3404078345216964699[5] = dt*state[14] + state[5]; - out_3404078345216964699[6] = state[6]; - out_3404078345216964699[7] = state[7]; - out_3404078345216964699[8] = state[8]; - out_3404078345216964699[9] = state[9]; - out_3404078345216964699[10] = state[10]; - out_3404078345216964699[11] = state[11]; - out_3404078345216964699[12] = state[12]; - out_3404078345216964699[13] = state[13]; - out_3404078345216964699[14] = state[14]; - out_3404078345216964699[15] = state[15]; - out_3404078345216964699[16] = state[16]; - out_3404078345216964699[17] = state[17]; +void f_fun(double *state, double dt, double *out_4063489601932878054) { + out_4063489601932878054[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_4063489601932878054[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_4063489601932878054[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_4063489601932878054[3] = dt*state[12] + state[3]; + out_4063489601932878054[4] = dt*state[13] + state[4]; + out_4063489601932878054[5] = dt*state[14] + state[5]; + out_4063489601932878054[6] = state[6]; + out_4063489601932878054[7] = state[7]; + out_4063489601932878054[8] = state[8]; + out_4063489601932878054[9] = state[9]; + out_4063489601932878054[10] = state[10]; + out_4063489601932878054[11] = state[11]; + out_4063489601932878054[12] = state[12]; + out_4063489601932878054[13] = state[13]; + out_4063489601932878054[14] = state[14]; + out_4063489601932878054[15] = state[15]; + out_4063489601932878054[16] = state[16]; + out_4063489601932878054[17] = state[17]; } -void F_fun(double *state, double dt, double *out_9161898751948597001) { - out_9161898751948597001[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_9161898751948597001[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_9161898751948597001[2] = 0; - out_9161898751948597001[3] = 0; - out_9161898751948597001[4] = 0; - out_9161898751948597001[5] = 0; - out_9161898751948597001[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_9161898751948597001[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_9161898751948597001[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_9161898751948597001[9] = 0; - out_9161898751948597001[10] = 0; - out_9161898751948597001[11] = 0; - out_9161898751948597001[12] = 0; - out_9161898751948597001[13] = 0; - out_9161898751948597001[14] = 0; - out_9161898751948597001[15] = 0; - out_9161898751948597001[16] = 0; - out_9161898751948597001[17] = 0; - out_9161898751948597001[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_9161898751948597001[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_9161898751948597001[20] = 0; - out_9161898751948597001[21] = 0; - out_9161898751948597001[22] = 0; - out_9161898751948597001[23] = 0; - out_9161898751948597001[24] = 0; - out_9161898751948597001[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_9161898751948597001[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_9161898751948597001[27] = 0; - out_9161898751948597001[28] = 0; - out_9161898751948597001[29] = 0; - out_9161898751948597001[30] = 0; - out_9161898751948597001[31] = 0; - out_9161898751948597001[32] = 0; - out_9161898751948597001[33] = 0; - out_9161898751948597001[34] = 0; - out_9161898751948597001[35] = 0; - out_9161898751948597001[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_9161898751948597001[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_9161898751948597001[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_9161898751948597001[39] = 0; - out_9161898751948597001[40] = 0; - out_9161898751948597001[41] = 0; - out_9161898751948597001[42] = 0; - out_9161898751948597001[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_9161898751948597001[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_9161898751948597001[45] = 0; - out_9161898751948597001[46] = 0; - out_9161898751948597001[47] = 0; - out_9161898751948597001[48] = 0; - out_9161898751948597001[49] = 0; - out_9161898751948597001[50] = 0; - out_9161898751948597001[51] = 0; - out_9161898751948597001[52] = 0; - out_9161898751948597001[53] = 0; - out_9161898751948597001[54] = 0; - out_9161898751948597001[55] = 0; - out_9161898751948597001[56] = 0; - out_9161898751948597001[57] = 1; - out_9161898751948597001[58] = 0; - out_9161898751948597001[59] = 0; - out_9161898751948597001[60] = 0; - out_9161898751948597001[61] = 0; - out_9161898751948597001[62] = 0; - out_9161898751948597001[63] = 0; - out_9161898751948597001[64] = 0; - out_9161898751948597001[65] = 0; - out_9161898751948597001[66] = dt; - out_9161898751948597001[67] = 0; - out_9161898751948597001[68] = 0; - out_9161898751948597001[69] = 0; - out_9161898751948597001[70] = 0; - out_9161898751948597001[71] = 0; - out_9161898751948597001[72] = 0; - out_9161898751948597001[73] = 0; - out_9161898751948597001[74] = 0; - out_9161898751948597001[75] = 0; - out_9161898751948597001[76] = 1; - out_9161898751948597001[77] = 0; - out_9161898751948597001[78] = 0; - out_9161898751948597001[79] = 0; - out_9161898751948597001[80] = 0; - out_9161898751948597001[81] = 0; - out_9161898751948597001[82] = 0; - out_9161898751948597001[83] = 0; - out_9161898751948597001[84] = 0; - out_9161898751948597001[85] = dt; - out_9161898751948597001[86] = 0; - out_9161898751948597001[87] = 0; - out_9161898751948597001[88] = 0; - out_9161898751948597001[89] = 0; - out_9161898751948597001[90] = 0; - out_9161898751948597001[91] = 0; - out_9161898751948597001[92] = 0; - out_9161898751948597001[93] = 0; - out_9161898751948597001[94] = 0; - out_9161898751948597001[95] = 1; - out_9161898751948597001[96] = 0; - out_9161898751948597001[97] = 0; - out_9161898751948597001[98] = 0; - out_9161898751948597001[99] = 0; - out_9161898751948597001[100] = 0; - out_9161898751948597001[101] = 0; - out_9161898751948597001[102] = 0; - out_9161898751948597001[103] = 0; - out_9161898751948597001[104] = dt; - out_9161898751948597001[105] = 0; - out_9161898751948597001[106] = 0; - out_9161898751948597001[107] = 0; - out_9161898751948597001[108] = 0; - out_9161898751948597001[109] = 0; - out_9161898751948597001[110] = 0; - out_9161898751948597001[111] = 0; - out_9161898751948597001[112] = 0; - out_9161898751948597001[113] = 0; - out_9161898751948597001[114] = 1; - out_9161898751948597001[115] = 0; - out_9161898751948597001[116] = 0; - out_9161898751948597001[117] = 0; - out_9161898751948597001[118] = 0; - out_9161898751948597001[119] = 0; - out_9161898751948597001[120] = 0; - out_9161898751948597001[121] = 0; - out_9161898751948597001[122] = 0; - out_9161898751948597001[123] = 0; - out_9161898751948597001[124] = 0; - out_9161898751948597001[125] = 0; - out_9161898751948597001[126] = 0; - out_9161898751948597001[127] = 0; - out_9161898751948597001[128] = 0; - out_9161898751948597001[129] = 0; - out_9161898751948597001[130] = 0; - out_9161898751948597001[131] = 0; - out_9161898751948597001[132] = 0; - out_9161898751948597001[133] = 1; - out_9161898751948597001[134] = 0; - out_9161898751948597001[135] = 0; - out_9161898751948597001[136] = 0; - out_9161898751948597001[137] = 0; - out_9161898751948597001[138] = 0; - out_9161898751948597001[139] = 0; - out_9161898751948597001[140] = 0; - out_9161898751948597001[141] = 0; - out_9161898751948597001[142] = 0; - out_9161898751948597001[143] = 0; - out_9161898751948597001[144] = 0; - out_9161898751948597001[145] = 0; - out_9161898751948597001[146] = 0; - out_9161898751948597001[147] = 0; - out_9161898751948597001[148] = 0; - out_9161898751948597001[149] = 0; - out_9161898751948597001[150] = 0; - out_9161898751948597001[151] = 0; - out_9161898751948597001[152] = 1; - out_9161898751948597001[153] = 0; - out_9161898751948597001[154] = 0; - out_9161898751948597001[155] = 0; - out_9161898751948597001[156] = 0; - out_9161898751948597001[157] = 0; - out_9161898751948597001[158] = 0; - out_9161898751948597001[159] = 0; - out_9161898751948597001[160] = 0; - out_9161898751948597001[161] = 0; - out_9161898751948597001[162] = 0; - out_9161898751948597001[163] = 0; - out_9161898751948597001[164] = 0; - out_9161898751948597001[165] = 0; - out_9161898751948597001[166] = 0; - out_9161898751948597001[167] = 0; - out_9161898751948597001[168] = 0; - out_9161898751948597001[169] = 0; - out_9161898751948597001[170] = 0; - out_9161898751948597001[171] = 1; - out_9161898751948597001[172] = 0; - out_9161898751948597001[173] = 0; - out_9161898751948597001[174] = 0; - out_9161898751948597001[175] = 0; - out_9161898751948597001[176] = 0; - out_9161898751948597001[177] = 0; - out_9161898751948597001[178] = 0; - out_9161898751948597001[179] = 0; - out_9161898751948597001[180] = 0; - out_9161898751948597001[181] = 0; - out_9161898751948597001[182] = 0; - out_9161898751948597001[183] = 0; - out_9161898751948597001[184] = 0; - out_9161898751948597001[185] = 0; - out_9161898751948597001[186] = 0; - out_9161898751948597001[187] = 0; - out_9161898751948597001[188] = 0; - out_9161898751948597001[189] = 0; - out_9161898751948597001[190] = 1; - out_9161898751948597001[191] = 0; - out_9161898751948597001[192] = 0; - out_9161898751948597001[193] = 0; - out_9161898751948597001[194] = 0; - out_9161898751948597001[195] = 0; - out_9161898751948597001[196] = 0; - out_9161898751948597001[197] = 0; - out_9161898751948597001[198] = 0; - out_9161898751948597001[199] = 0; - out_9161898751948597001[200] = 0; - out_9161898751948597001[201] = 0; - out_9161898751948597001[202] = 0; - out_9161898751948597001[203] = 0; - out_9161898751948597001[204] = 0; - out_9161898751948597001[205] = 0; - out_9161898751948597001[206] = 0; - out_9161898751948597001[207] = 0; - out_9161898751948597001[208] = 0; - out_9161898751948597001[209] = 1; - out_9161898751948597001[210] = 0; - out_9161898751948597001[211] = 0; - out_9161898751948597001[212] = 0; - out_9161898751948597001[213] = 0; - out_9161898751948597001[214] = 0; - out_9161898751948597001[215] = 0; - out_9161898751948597001[216] = 0; - out_9161898751948597001[217] = 0; - out_9161898751948597001[218] = 0; - out_9161898751948597001[219] = 0; - out_9161898751948597001[220] = 0; - out_9161898751948597001[221] = 0; - out_9161898751948597001[222] = 0; - out_9161898751948597001[223] = 0; - out_9161898751948597001[224] = 0; - out_9161898751948597001[225] = 0; - out_9161898751948597001[226] = 0; - out_9161898751948597001[227] = 0; - out_9161898751948597001[228] = 1; - out_9161898751948597001[229] = 0; - out_9161898751948597001[230] = 0; - out_9161898751948597001[231] = 0; - out_9161898751948597001[232] = 0; - out_9161898751948597001[233] = 0; - out_9161898751948597001[234] = 0; - out_9161898751948597001[235] = 0; - out_9161898751948597001[236] = 0; - out_9161898751948597001[237] = 0; - out_9161898751948597001[238] = 0; - out_9161898751948597001[239] = 0; - out_9161898751948597001[240] = 0; - out_9161898751948597001[241] = 0; - out_9161898751948597001[242] = 0; - out_9161898751948597001[243] = 0; - out_9161898751948597001[244] = 0; - out_9161898751948597001[245] = 0; - out_9161898751948597001[246] = 0; - out_9161898751948597001[247] = 1; - out_9161898751948597001[248] = 0; - out_9161898751948597001[249] = 0; - out_9161898751948597001[250] = 0; - out_9161898751948597001[251] = 0; - out_9161898751948597001[252] = 0; - out_9161898751948597001[253] = 0; - out_9161898751948597001[254] = 0; - out_9161898751948597001[255] = 0; - out_9161898751948597001[256] = 0; - out_9161898751948597001[257] = 0; - out_9161898751948597001[258] = 0; - out_9161898751948597001[259] = 0; - out_9161898751948597001[260] = 0; - out_9161898751948597001[261] = 0; - out_9161898751948597001[262] = 0; - out_9161898751948597001[263] = 0; - out_9161898751948597001[264] = 0; - out_9161898751948597001[265] = 0; - out_9161898751948597001[266] = 1; - out_9161898751948597001[267] = 0; - out_9161898751948597001[268] = 0; - out_9161898751948597001[269] = 0; - out_9161898751948597001[270] = 0; - out_9161898751948597001[271] = 0; - out_9161898751948597001[272] = 0; - out_9161898751948597001[273] = 0; - out_9161898751948597001[274] = 0; - out_9161898751948597001[275] = 0; - out_9161898751948597001[276] = 0; - out_9161898751948597001[277] = 0; - out_9161898751948597001[278] = 0; - out_9161898751948597001[279] = 0; - out_9161898751948597001[280] = 0; - out_9161898751948597001[281] = 0; - out_9161898751948597001[282] = 0; - out_9161898751948597001[283] = 0; - out_9161898751948597001[284] = 0; - out_9161898751948597001[285] = 1; - out_9161898751948597001[286] = 0; - out_9161898751948597001[287] = 0; - out_9161898751948597001[288] = 0; - out_9161898751948597001[289] = 0; - out_9161898751948597001[290] = 0; - out_9161898751948597001[291] = 0; - out_9161898751948597001[292] = 0; - out_9161898751948597001[293] = 0; - out_9161898751948597001[294] = 0; - out_9161898751948597001[295] = 0; - out_9161898751948597001[296] = 0; - out_9161898751948597001[297] = 0; - out_9161898751948597001[298] = 0; - out_9161898751948597001[299] = 0; - out_9161898751948597001[300] = 0; - out_9161898751948597001[301] = 0; - out_9161898751948597001[302] = 0; - out_9161898751948597001[303] = 0; - out_9161898751948597001[304] = 1; - out_9161898751948597001[305] = 0; - out_9161898751948597001[306] = 0; - out_9161898751948597001[307] = 0; - out_9161898751948597001[308] = 0; - out_9161898751948597001[309] = 0; - out_9161898751948597001[310] = 0; - out_9161898751948597001[311] = 0; - out_9161898751948597001[312] = 0; - out_9161898751948597001[313] = 0; - out_9161898751948597001[314] = 0; - out_9161898751948597001[315] = 0; - out_9161898751948597001[316] = 0; - out_9161898751948597001[317] = 0; - out_9161898751948597001[318] = 0; - out_9161898751948597001[319] = 0; - out_9161898751948597001[320] = 0; - out_9161898751948597001[321] = 0; - out_9161898751948597001[322] = 0; - out_9161898751948597001[323] = 1; +void F_fun(double *state, double dt, double *out_493800117352810078) { + out_493800117352810078[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_493800117352810078[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_493800117352810078[2] = 0; + out_493800117352810078[3] = 0; + out_493800117352810078[4] = 0; + out_493800117352810078[5] = 0; + out_493800117352810078[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_493800117352810078[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_493800117352810078[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_493800117352810078[9] = 0; + out_493800117352810078[10] = 0; + out_493800117352810078[11] = 0; + out_493800117352810078[12] = 0; + out_493800117352810078[13] = 0; + out_493800117352810078[14] = 0; + out_493800117352810078[15] = 0; + out_493800117352810078[16] = 0; + out_493800117352810078[17] = 0; + out_493800117352810078[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_493800117352810078[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_493800117352810078[20] = 0; + out_493800117352810078[21] = 0; + out_493800117352810078[22] = 0; + out_493800117352810078[23] = 0; + out_493800117352810078[24] = 0; + out_493800117352810078[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_493800117352810078[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_493800117352810078[27] = 0; + out_493800117352810078[28] = 0; + out_493800117352810078[29] = 0; + out_493800117352810078[30] = 0; + out_493800117352810078[31] = 0; + out_493800117352810078[32] = 0; + out_493800117352810078[33] = 0; + out_493800117352810078[34] = 0; + out_493800117352810078[35] = 0; + out_493800117352810078[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_493800117352810078[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_493800117352810078[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_493800117352810078[39] = 0; + out_493800117352810078[40] = 0; + out_493800117352810078[41] = 0; + out_493800117352810078[42] = 0; + out_493800117352810078[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_493800117352810078[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_493800117352810078[45] = 0; + out_493800117352810078[46] = 0; + out_493800117352810078[47] = 0; + out_493800117352810078[48] = 0; + out_493800117352810078[49] = 0; + out_493800117352810078[50] = 0; + out_493800117352810078[51] = 0; + out_493800117352810078[52] = 0; + out_493800117352810078[53] = 0; + out_493800117352810078[54] = 0; + out_493800117352810078[55] = 0; + out_493800117352810078[56] = 0; + out_493800117352810078[57] = 1; + out_493800117352810078[58] = 0; + out_493800117352810078[59] = 0; + out_493800117352810078[60] = 0; + out_493800117352810078[61] = 0; + out_493800117352810078[62] = 0; + out_493800117352810078[63] = 0; + out_493800117352810078[64] = 0; + out_493800117352810078[65] = 0; + out_493800117352810078[66] = dt; + out_493800117352810078[67] = 0; + out_493800117352810078[68] = 0; + out_493800117352810078[69] = 0; + out_493800117352810078[70] = 0; + out_493800117352810078[71] = 0; + out_493800117352810078[72] = 0; + out_493800117352810078[73] = 0; + out_493800117352810078[74] = 0; + out_493800117352810078[75] = 0; + out_493800117352810078[76] = 1; + out_493800117352810078[77] = 0; + out_493800117352810078[78] = 0; + out_493800117352810078[79] = 0; + out_493800117352810078[80] = 0; + out_493800117352810078[81] = 0; + out_493800117352810078[82] = 0; + out_493800117352810078[83] = 0; + out_493800117352810078[84] = 0; + out_493800117352810078[85] = dt; + out_493800117352810078[86] = 0; + out_493800117352810078[87] = 0; + out_493800117352810078[88] = 0; + out_493800117352810078[89] = 0; + out_493800117352810078[90] = 0; + out_493800117352810078[91] = 0; + out_493800117352810078[92] = 0; + out_493800117352810078[93] = 0; + out_493800117352810078[94] = 0; + out_493800117352810078[95] = 1; + out_493800117352810078[96] = 0; + out_493800117352810078[97] = 0; + out_493800117352810078[98] = 0; + out_493800117352810078[99] = 0; + out_493800117352810078[100] = 0; + out_493800117352810078[101] = 0; + out_493800117352810078[102] = 0; + out_493800117352810078[103] = 0; + out_493800117352810078[104] = dt; + out_493800117352810078[105] = 0; + out_493800117352810078[106] = 0; + out_493800117352810078[107] = 0; + out_493800117352810078[108] = 0; + out_493800117352810078[109] = 0; + out_493800117352810078[110] = 0; + out_493800117352810078[111] = 0; + out_493800117352810078[112] = 0; + out_493800117352810078[113] = 0; + out_493800117352810078[114] = 1; + out_493800117352810078[115] = 0; + out_493800117352810078[116] = 0; + out_493800117352810078[117] = 0; + out_493800117352810078[118] = 0; + out_493800117352810078[119] = 0; + out_493800117352810078[120] = 0; + out_493800117352810078[121] = 0; + out_493800117352810078[122] = 0; + out_493800117352810078[123] = 0; + out_493800117352810078[124] = 0; + out_493800117352810078[125] = 0; + out_493800117352810078[126] = 0; + out_493800117352810078[127] = 0; + out_493800117352810078[128] = 0; + out_493800117352810078[129] = 0; + out_493800117352810078[130] = 0; + out_493800117352810078[131] = 0; + out_493800117352810078[132] = 0; + out_493800117352810078[133] = 1; + out_493800117352810078[134] = 0; + out_493800117352810078[135] = 0; + out_493800117352810078[136] = 0; + out_493800117352810078[137] = 0; + out_493800117352810078[138] = 0; + out_493800117352810078[139] = 0; + out_493800117352810078[140] = 0; + out_493800117352810078[141] = 0; + out_493800117352810078[142] = 0; + out_493800117352810078[143] = 0; + out_493800117352810078[144] = 0; + out_493800117352810078[145] = 0; + out_493800117352810078[146] = 0; + out_493800117352810078[147] = 0; + out_493800117352810078[148] = 0; + out_493800117352810078[149] = 0; + out_493800117352810078[150] = 0; + out_493800117352810078[151] = 0; + out_493800117352810078[152] = 1; + out_493800117352810078[153] = 0; + out_493800117352810078[154] = 0; + out_493800117352810078[155] = 0; + out_493800117352810078[156] = 0; + out_493800117352810078[157] = 0; + out_493800117352810078[158] = 0; + out_493800117352810078[159] = 0; + out_493800117352810078[160] = 0; + out_493800117352810078[161] = 0; + out_493800117352810078[162] = 0; + out_493800117352810078[163] = 0; + out_493800117352810078[164] = 0; + out_493800117352810078[165] = 0; + out_493800117352810078[166] = 0; + out_493800117352810078[167] = 0; + out_493800117352810078[168] = 0; + out_493800117352810078[169] = 0; + out_493800117352810078[170] = 0; + out_493800117352810078[171] = 1; + out_493800117352810078[172] = 0; + out_493800117352810078[173] = 0; + out_493800117352810078[174] = 0; + out_493800117352810078[175] = 0; + out_493800117352810078[176] = 0; + out_493800117352810078[177] = 0; + out_493800117352810078[178] = 0; + out_493800117352810078[179] = 0; + out_493800117352810078[180] = 0; + out_493800117352810078[181] = 0; + out_493800117352810078[182] = 0; + out_493800117352810078[183] = 0; + out_493800117352810078[184] = 0; + out_493800117352810078[185] = 0; + out_493800117352810078[186] = 0; + out_493800117352810078[187] = 0; + out_493800117352810078[188] = 0; + out_493800117352810078[189] = 0; + out_493800117352810078[190] = 1; + out_493800117352810078[191] = 0; + out_493800117352810078[192] = 0; + out_493800117352810078[193] = 0; + out_493800117352810078[194] = 0; + out_493800117352810078[195] = 0; + out_493800117352810078[196] = 0; + out_493800117352810078[197] = 0; + out_493800117352810078[198] = 0; + out_493800117352810078[199] = 0; + out_493800117352810078[200] = 0; + out_493800117352810078[201] = 0; + out_493800117352810078[202] = 0; + out_493800117352810078[203] = 0; + out_493800117352810078[204] = 0; + out_493800117352810078[205] = 0; + out_493800117352810078[206] = 0; + out_493800117352810078[207] = 0; + out_493800117352810078[208] = 0; + out_493800117352810078[209] = 1; + out_493800117352810078[210] = 0; + out_493800117352810078[211] = 0; + out_493800117352810078[212] = 0; + out_493800117352810078[213] = 0; + out_493800117352810078[214] = 0; + out_493800117352810078[215] = 0; + out_493800117352810078[216] = 0; + out_493800117352810078[217] = 0; + out_493800117352810078[218] = 0; + out_493800117352810078[219] = 0; + out_493800117352810078[220] = 0; + out_493800117352810078[221] = 0; + out_493800117352810078[222] = 0; + out_493800117352810078[223] = 0; + out_493800117352810078[224] = 0; + out_493800117352810078[225] = 0; + out_493800117352810078[226] = 0; + out_493800117352810078[227] = 0; + out_493800117352810078[228] = 1; + out_493800117352810078[229] = 0; + out_493800117352810078[230] = 0; + out_493800117352810078[231] = 0; + out_493800117352810078[232] = 0; + out_493800117352810078[233] = 0; + out_493800117352810078[234] = 0; + out_493800117352810078[235] = 0; + out_493800117352810078[236] = 0; + out_493800117352810078[237] = 0; + out_493800117352810078[238] = 0; + out_493800117352810078[239] = 0; + out_493800117352810078[240] = 0; + out_493800117352810078[241] = 0; + out_493800117352810078[242] = 0; + out_493800117352810078[243] = 0; + out_493800117352810078[244] = 0; + out_493800117352810078[245] = 0; + out_493800117352810078[246] = 0; + out_493800117352810078[247] = 1; + out_493800117352810078[248] = 0; + out_493800117352810078[249] = 0; + out_493800117352810078[250] = 0; + out_493800117352810078[251] = 0; + out_493800117352810078[252] = 0; + out_493800117352810078[253] = 0; + out_493800117352810078[254] = 0; + out_493800117352810078[255] = 0; + out_493800117352810078[256] = 0; + out_493800117352810078[257] = 0; + out_493800117352810078[258] = 0; + out_493800117352810078[259] = 0; + out_493800117352810078[260] = 0; + out_493800117352810078[261] = 0; + out_493800117352810078[262] = 0; + out_493800117352810078[263] = 0; + out_493800117352810078[264] = 0; + out_493800117352810078[265] = 0; + out_493800117352810078[266] = 1; + out_493800117352810078[267] = 0; + out_493800117352810078[268] = 0; + out_493800117352810078[269] = 0; + out_493800117352810078[270] = 0; + out_493800117352810078[271] = 0; + out_493800117352810078[272] = 0; + out_493800117352810078[273] = 0; + out_493800117352810078[274] = 0; + out_493800117352810078[275] = 0; + out_493800117352810078[276] = 0; + out_493800117352810078[277] = 0; + out_493800117352810078[278] = 0; + out_493800117352810078[279] = 0; + out_493800117352810078[280] = 0; + out_493800117352810078[281] = 0; + out_493800117352810078[282] = 0; + out_493800117352810078[283] = 0; + out_493800117352810078[284] = 0; + out_493800117352810078[285] = 1; + out_493800117352810078[286] = 0; + out_493800117352810078[287] = 0; + out_493800117352810078[288] = 0; + out_493800117352810078[289] = 0; + out_493800117352810078[290] = 0; + out_493800117352810078[291] = 0; + out_493800117352810078[292] = 0; + out_493800117352810078[293] = 0; + out_493800117352810078[294] = 0; + out_493800117352810078[295] = 0; + out_493800117352810078[296] = 0; + out_493800117352810078[297] = 0; + out_493800117352810078[298] = 0; + out_493800117352810078[299] = 0; + out_493800117352810078[300] = 0; + out_493800117352810078[301] = 0; + out_493800117352810078[302] = 0; + out_493800117352810078[303] = 0; + out_493800117352810078[304] = 1; + out_493800117352810078[305] = 0; + out_493800117352810078[306] = 0; + out_493800117352810078[307] = 0; + out_493800117352810078[308] = 0; + out_493800117352810078[309] = 0; + out_493800117352810078[310] = 0; + out_493800117352810078[311] = 0; + out_493800117352810078[312] = 0; + out_493800117352810078[313] = 0; + out_493800117352810078[314] = 0; + out_493800117352810078[315] = 0; + out_493800117352810078[316] = 0; + out_493800117352810078[317] = 0; + out_493800117352810078[318] = 0; + out_493800117352810078[319] = 0; + out_493800117352810078[320] = 0; + out_493800117352810078[321] = 0; + out_493800117352810078[322] = 0; + out_493800117352810078[323] = 1; } -void h_4(double *state, double *unused, double *out_3298849734810867951) { - out_3298849734810867951[0] = state[6] + state[9]; - out_3298849734810867951[1] = state[7] + state[10]; - out_3298849734810867951[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_6026765849154187968) { + out_6026765849154187968[0] = state[6] + state[9]; + out_6026765849154187968[1] = state[7] + state[10]; + out_6026765849154187968[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_3476673534661732439) { - out_3476673534661732439[0] = 0; - out_3476673534661732439[1] = 0; - out_3476673534661732439[2] = 0; - out_3476673534661732439[3] = 0; - out_3476673534661732439[4] = 0; - out_3476673534661732439[5] = 0; - out_3476673534661732439[6] = 1; - out_3476673534661732439[7] = 0; - out_3476673534661732439[8] = 0; - out_3476673534661732439[9] = 1; - out_3476673534661732439[10] = 0; - out_3476673534661732439[11] = 0; - out_3476673534661732439[12] = 0; - out_3476673534661732439[13] = 0; - out_3476673534661732439[14] = 0; - out_3476673534661732439[15] = 0; - out_3476673534661732439[16] = 0; - out_3476673534661732439[17] = 0; - out_3476673534661732439[18] = 0; - out_3476673534661732439[19] = 0; - out_3476673534661732439[20] = 0; - out_3476673534661732439[21] = 0; - out_3476673534661732439[22] = 0; - out_3476673534661732439[23] = 0; - out_3476673534661732439[24] = 0; - out_3476673534661732439[25] = 1; - out_3476673534661732439[26] = 0; - out_3476673534661732439[27] = 0; - out_3476673534661732439[28] = 1; - out_3476673534661732439[29] = 0; - out_3476673534661732439[30] = 0; - out_3476673534661732439[31] = 0; - out_3476673534661732439[32] = 0; - out_3476673534661732439[33] = 0; - out_3476673534661732439[34] = 0; - out_3476673534661732439[35] = 0; - out_3476673534661732439[36] = 0; - out_3476673534661732439[37] = 0; - out_3476673534661732439[38] = 0; - out_3476673534661732439[39] = 0; - out_3476673534661732439[40] = 0; - out_3476673534661732439[41] = 0; - out_3476673534661732439[42] = 0; - out_3476673534661732439[43] = 0; - out_3476673534661732439[44] = 1; - out_3476673534661732439[45] = 0; - out_3476673534661732439[46] = 0; - out_3476673534661732439[47] = 1; - out_3476673534661732439[48] = 0; - out_3476673534661732439[49] = 0; - out_3476673534661732439[50] = 0; - out_3476673534661732439[51] = 0; - out_3476673534661732439[52] = 0; - out_3476673534661732439[53] = 0; +void H_4(double *state, double *unused, double *out_7102224835231120360) { + out_7102224835231120360[0] = 0; + out_7102224835231120360[1] = 0; + out_7102224835231120360[2] = 0; + out_7102224835231120360[3] = 0; + out_7102224835231120360[4] = 0; + out_7102224835231120360[5] = 0; + out_7102224835231120360[6] = 1; + out_7102224835231120360[7] = 0; + out_7102224835231120360[8] = 0; + out_7102224835231120360[9] = 1; + out_7102224835231120360[10] = 0; + out_7102224835231120360[11] = 0; + out_7102224835231120360[12] = 0; + out_7102224835231120360[13] = 0; + out_7102224835231120360[14] = 0; + out_7102224835231120360[15] = 0; + out_7102224835231120360[16] = 0; + out_7102224835231120360[17] = 0; + out_7102224835231120360[18] = 0; + out_7102224835231120360[19] = 0; + out_7102224835231120360[20] = 0; + out_7102224835231120360[21] = 0; + out_7102224835231120360[22] = 0; + out_7102224835231120360[23] = 0; + out_7102224835231120360[24] = 0; + out_7102224835231120360[25] = 1; + out_7102224835231120360[26] = 0; + out_7102224835231120360[27] = 0; + out_7102224835231120360[28] = 1; + out_7102224835231120360[29] = 0; + out_7102224835231120360[30] = 0; + out_7102224835231120360[31] = 0; + out_7102224835231120360[32] = 0; + out_7102224835231120360[33] = 0; + out_7102224835231120360[34] = 0; + out_7102224835231120360[35] = 0; + out_7102224835231120360[36] = 0; + out_7102224835231120360[37] = 0; + out_7102224835231120360[38] = 0; + out_7102224835231120360[39] = 0; + out_7102224835231120360[40] = 0; + out_7102224835231120360[41] = 0; + out_7102224835231120360[42] = 0; + out_7102224835231120360[43] = 0; + out_7102224835231120360[44] = 1; + out_7102224835231120360[45] = 0; + out_7102224835231120360[46] = 0; + out_7102224835231120360[47] = 1; + out_7102224835231120360[48] = 0; + out_7102224835231120360[49] = 0; + out_7102224835231120360[50] = 0; + out_7102224835231120360[51] = 0; + out_7102224835231120360[52] = 0; + out_7102224835231120360[53] = 0; } -void h_10(double *state, double *unused, double *out_1490778051551919602) { - out_1490778051551919602[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_1490778051551919602[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_1490778051551919602[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_1866086890220189324) { + out_1866086890220189324[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_1866086890220189324[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_1866086890220189324[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_3065010150012391749) { - out_3065010150012391749[0] = 0; - out_3065010150012391749[1] = 9.8100000000000005*cos(state[1]); - out_3065010150012391749[2] = 0; - out_3065010150012391749[3] = 0; - out_3065010150012391749[4] = -state[8]; - out_3065010150012391749[5] = state[7]; - out_3065010150012391749[6] = 0; - out_3065010150012391749[7] = state[5]; - out_3065010150012391749[8] = -state[4]; - out_3065010150012391749[9] = 0; - out_3065010150012391749[10] = 0; - out_3065010150012391749[11] = 0; - out_3065010150012391749[12] = 1; - out_3065010150012391749[13] = 0; - out_3065010150012391749[14] = 0; - out_3065010150012391749[15] = 1; - out_3065010150012391749[16] = 0; - out_3065010150012391749[17] = 0; - out_3065010150012391749[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_3065010150012391749[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_3065010150012391749[20] = 0; - out_3065010150012391749[21] = state[8]; - out_3065010150012391749[22] = 0; - out_3065010150012391749[23] = -state[6]; - out_3065010150012391749[24] = -state[5]; - out_3065010150012391749[25] = 0; - out_3065010150012391749[26] = state[3]; - out_3065010150012391749[27] = 0; - out_3065010150012391749[28] = 0; - out_3065010150012391749[29] = 0; - out_3065010150012391749[30] = 0; - out_3065010150012391749[31] = 1; - out_3065010150012391749[32] = 0; - out_3065010150012391749[33] = 0; - out_3065010150012391749[34] = 1; - out_3065010150012391749[35] = 0; - out_3065010150012391749[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_3065010150012391749[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_3065010150012391749[38] = 0; - out_3065010150012391749[39] = -state[7]; - out_3065010150012391749[40] = state[6]; - out_3065010150012391749[41] = 0; - out_3065010150012391749[42] = state[4]; - out_3065010150012391749[43] = -state[3]; - out_3065010150012391749[44] = 0; - out_3065010150012391749[45] = 0; - out_3065010150012391749[46] = 0; - out_3065010150012391749[47] = 0; - out_3065010150012391749[48] = 0; - out_3065010150012391749[49] = 0; - out_3065010150012391749[50] = 1; - out_3065010150012391749[51] = 0; - out_3065010150012391749[52] = 0; - out_3065010150012391749[53] = 1; +void H_10(double *state, double *unused, double *out_834802170779042492) { + out_834802170779042492[0] = 0; + out_834802170779042492[1] = 9.8100000000000005*cos(state[1]); + out_834802170779042492[2] = 0; + out_834802170779042492[3] = 0; + out_834802170779042492[4] = -state[8]; + out_834802170779042492[5] = state[7]; + out_834802170779042492[6] = 0; + out_834802170779042492[7] = state[5]; + out_834802170779042492[8] = -state[4]; + out_834802170779042492[9] = 0; + out_834802170779042492[10] = 0; + out_834802170779042492[11] = 0; + out_834802170779042492[12] = 1; + out_834802170779042492[13] = 0; + out_834802170779042492[14] = 0; + out_834802170779042492[15] = 1; + out_834802170779042492[16] = 0; + out_834802170779042492[17] = 0; + out_834802170779042492[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_834802170779042492[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_834802170779042492[20] = 0; + out_834802170779042492[21] = state[8]; + out_834802170779042492[22] = 0; + out_834802170779042492[23] = -state[6]; + out_834802170779042492[24] = -state[5]; + out_834802170779042492[25] = 0; + out_834802170779042492[26] = state[3]; + out_834802170779042492[27] = 0; + out_834802170779042492[28] = 0; + out_834802170779042492[29] = 0; + out_834802170779042492[30] = 0; + out_834802170779042492[31] = 1; + out_834802170779042492[32] = 0; + out_834802170779042492[33] = 0; + out_834802170779042492[34] = 1; + out_834802170779042492[35] = 0; + out_834802170779042492[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_834802170779042492[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_834802170779042492[38] = 0; + out_834802170779042492[39] = -state[7]; + out_834802170779042492[40] = state[6]; + out_834802170779042492[41] = 0; + out_834802170779042492[42] = state[4]; + out_834802170779042492[43] = -state[3]; + out_834802170779042492[44] = 0; + out_834802170779042492[45] = 0; + out_834802170779042492[46] = 0; + out_834802170779042492[47] = 0; + out_834802170779042492[48] = 0; + out_834802170779042492[49] = 0; + out_834802170779042492[50] = 1; + out_834802170779042492[51] = 0; + out_834802170779042492[52] = 0; + out_834802170779042492[53] = 1; } -void h_13(double *state, double *unused, double *out_2293000846655251698) { - out_2293000846655251698[0] = state[3]; - out_2293000846655251698[1] = state[4]; - out_2293000846655251698[2] = state[5]; +void h_13(double *state, double *unused, double *out_4520826989648677276) { + out_4520826989648677276[0] = state[3]; + out_4520826989648677276[1] = state[4]; + out_4520826989648677276[2] = state[5]; } -void H_13(double *state, double *unused, double *out_4133957673654968490) { - out_4133957673654968490[0] = 0; - out_4133957673654968490[1] = 0; - out_4133957673654968490[2] = 0; - out_4133957673654968490[3] = 1; - out_4133957673654968490[4] = 0; - out_4133957673654968490[5] = 0; - out_4133957673654968490[6] = 0; - out_4133957673654968490[7] = 0; - out_4133957673654968490[8] = 0; - out_4133957673654968490[9] = 0; - out_4133957673654968490[10] = 0; - out_4133957673654968490[11] = 0; - out_4133957673654968490[12] = 0; - out_4133957673654968490[13] = 0; - out_4133957673654968490[14] = 0; - out_4133957673654968490[15] = 0; - out_4133957673654968490[16] = 0; - out_4133957673654968490[17] = 0; - out_4133957673654968490[18] = 0; - out_4133957673654968490[19] = 0; - out_4133957673654968490[20] = 0; - out_4133957673654968490[21] = 0; - out_4133957673654968490[22] = 1; - out_4133957673654968490[23] = 0; - out_4133957673654968490[24] = 0; - out_4133957673654968490[25] = 0; - out_4133957673654968490[26] = 0; - out_4133957673654968490[27] = 0; - out_4133957673654968490[28] = 0; - out_4133957673654968490[29] = 0; - out_4133957673654968490[30] = 0; - out_4133957673654968490[31] = 0; - out_4133957673654968490[32] = 0; - out_4133957673654968490[33] = 0; - out_4133957673654968490[34] = 0; - out_4133957673654968490[35] = 0; - out_4133957673654968490[36] = 0; - out_4133957673654968490[37] = 0; - out_4133957673654968490[38] = 0; - out_4133957673654968490[39] = 0; - out_4133957673654968490[40] = 0; - out_4133957673654968490[41] = 1; - out_4133957673654968490[42] = 0; - out_4133957673654968490[43] = 0; - out_4133957673654968490[44] = 0; - out_4133957673654968490[45] = 0; - out_4133957673654968490[46] = 0; - out_4133957673654968490[47] = 0; - out_4133957673654968490[48] = 0; - out_4133957673654968490[49] = 0; - out_4133957673654968490[50] = 0; - out_4133957673654968490[51] = 0; - out_4133957673654968490[52] = 0; - out_4133957673654968490[53] = 0; +void H_13(double *state, double *unused, double *out_3889951009898787559) { + out_3889951009898787559[0] = 0; + out_3889951009898787559[1] = 0; + out_3889951009898787559[2] = 0; + out_3889951009898787559[3] = 1; + out_3889951009898787559[4] = 0; + out_3889951009898787559[5] = 0; + out_3889951009898787559[6] = 0; + out_3889951009898787559[7] = 0; + out_3889951009898787559[8] = 0; + out_3889951009898787559[9] = 0; + out_3889951009898787559[10] = 0; + out_3889951009898787559[11] = 0; + out_3889951009898787559[12] = 0; + out_3889951009898787559[13] = 0; + out_3889951009898787559[14] = 0; + out_3889951009898787559[15] = 0; + out_3889951009898787559[16] = 0; + out_3889951009898787559[17] = 0; + out_3889951009898787559[18] = 0; + out_3889951009898787559[19] = 0; + out_3889951009898787559[20] = 0; + out_3889951009898787559[21] = 0; + out_3889951009898787559[22] = 1; + out_3889951009898787559[23] = 0; + out_3889951009898787559[24] = 0; + out_3889951009898787559[25] = 0; + out_3889951009898787559[26] = 0; + out_3889951009898787559[27] = 0; + out_3889951009898787559[28] = 0; + out_3889951009898787559[29] = 0; + out_3889951009898787559[30] = 0; + out_3889951009898787559[31] = 0; + out_3889951009898787559[32] = 0; + out_3889951009898787559[33] = 0; + out_3889951009898787559[34] = 0; + out_3889951009898787559[35] = 0; + out_3889951009898787559[36] = 0; + out_3889951009898787559[37] = 0; + out_3889951009898787559[38] = 0; + out_3889951009898787559[39] = 0; + out_3889951009898787559[40] = 0; + out_3889951009898787559[41] = 1; + out_3889951009898787559[42] = 0; + out_3889951009898787559[43] = 0; + out_3889951009898787559[44] = 0; + out_3889951009898787559[45] = 0; + out_3889951009898787559[46] = 0; + out_3889951009898787559[47] = 0; + out_3889951009898787559[48] = 0; + out_3889951009898787559[49] = 0; + out_3889951009898787559[50] = 0; + out_3889951009898787559[51] = 0; + out_3889951009898787559[52] = 0; + out_3889951009898787559[53] = 0; } -void h_14(double *state, double *unused, double *out_6732417031145183728) { - out_6732417031145183728[0] = state[6]; - out_6732417031145183728[1] = state[7]; - out_6732417031145183728[2] = state[8]; +void h_14(double *state, double *unused, double *out_733646167343188736) { + out_733646167343188736[0] = state[6]; + out_733646167343188736[1] = state[7]; + out_733646167343188736[2] = state[8]; } -void H_14(double *state, double *unused, double *out_486567321677752090) { - out_486567321677752090[0] = 0; - out_486567321677752090[1] = 0; - out_486567321677752090[2] = 0; - out_486567321677752090[3] = 0; - out_486567321677752090[4] = 0; - out_486567321677752090[5] = 0; - out_486567321677752090[6] = 1; - out_486567321677752090[7] = 0; - out_486567321677752090[8] = 0; - out_486567321677752090[9] = 0; - out_486567321677752090[10] = 0; - out_486567321677752090[11] = 0; - out_486567321677752090[12] = 0; - out_486567321677752090[13] = 0; - out_486567321677752090[14] = 0; - out_486567321677752090[15] = 0; - out_486567321677752090[16] = 0; - out_486567321677752090[17] = 0; - out_486567321677752090[18] = 0; - out_486567321677752090[19] = 0; - out_486567321677752090[20] = 0; - out_486567321677752090[21] = 0; - out_486567321677752090[22] = 0; - out_486567321677752090[23] = 0; - out_486567321677752090[24] = 0; - out_486567321677752090[25] = 1; - out_486567321677752090[26] = 0; - out_486567321677752090[27] = 0; - out_486567321677752090[28] = 0; - out_486567321677752090[29] = 0; - out_486567321677752090[30] = 0; - out_486567321677752090[31] = 0; - out_486567321677752090[32] = 0; - out_486567321677752090[33] = 0; - out_486567321677752090[34] = 0; - out_486567321677752090[35] = 0; - out_486567321677752090[36] = 0; - out_486567321677752090[37] = 0; - out_486567321677752090[38] = 0; - out_486567321677752090[39] = 0; - out_486567321677752090[40] = 0; - out_486567321677752090[41] = 0; - out_486567321677752090[42] = 0; - out_486567321677752090[43] = 0; - out_486567321677752090[44] = 1; - out_486567321677752090[45] = 0; - out_486567321677752090[46] = 0; - out_486567321677752090[47] = 0; - out_486567321677752090[48] = 0; - out_486567321677752090[49] = 0; - out_486567321677752090[50] = 0; - out_486567321677752090[51] = 0; - out_486567321677752090[52] = 0; - out_486567321677752090[53] = 0; +void H_14(double *state, double *unused, double *out_8261730806183058960) { + out_8261730806183058960[0] = 0; + out_8261730806183058960[1] = 0; + out_8261730806183058960[2] = 0; + out_8261730806183058960[3] = 0; + out_8261730806183058960[4] = 0; + out_8261730806183058960[5] = 0; + out_8261730806183058960[6] = 1; + out_8261730806183058960[7] = 0; + out_8261730806183058960[8] = 0; + out_8261730806183058960[9] = 0; + out_8261730806183058960[10] = 0; + out_8261730806183058960[11] = 0; + out_8261730806183058960[12] = 0; + out_8261730806183058960[13] = 0; + out_8261730806183058960[14] = 0; + out_8261730806183058960[15] = 0; + out_8261730806183058960[16] = 0; + out_8261730806183058960[17] = 0; + out_8261730806183058960[18] = 0; + out_8261730806183058960[19] = 0; + out_8261730806183058960[20] = 0; + out_8261730806183058960[21] = 0; + out_8261730806183058960[22] = 0; + out_8261730806183058960[23] = 0; + out_8261730806183058960[24] = 0; + out_8261730806183058960[25] = 1; + out_8261730806183058960[26] = 0; + out_8261730806183058960[27] = 0; + out_8261730806183058960[28] = 0; + out_8261730806183058960[29] = 0; + out_8261730806183058960[30] = 0; + out_8261730806183058960[31] = 0; + out_8261730806183058960[32] = 0; + out_8261730806183058960[33] = 0; + out_8261730806183058960[34] = 0; + out_8261730806183058960[35] = 0; + out_8261730806183058960[36] = 0; + out_8261730806183058960[37] = 0; + out_8261730806183058960[38] = 0; + out_8261730806183058960[39] = 0; + out_8261730806183058960[40] = 0; + out_8261730806183058960[41] = 0; + out_8261730806183058960[42] = 0; + out_8261730806183058960[43] = 0; + out_8261730806183058960[44] = 1; + out_8261730806183058960[45] = 0; + out_8261730806183058960[46] = 0; + out_8261730806183058960[47] = 0; + out_8261730806183058960[48] = 0; + out_8261730806183058960[49] = 0; + out_8261730806183058960[50] = 0; + out_8261730806183058960[51] = 0; + out_8261730806183058960[52] = 0; + out_8261730806183058960[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_3524911542636248309) { - err_fun(nom_x, delta_x, out_3524911542636248309); +void pose_err_fun(double *nom_x, double *delta_x, double *out_3341780452292505997) { + err_fun(nom_x, delta_x, out_3341780452292505997); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_9101758542759318568) { - inv_err_fun(nom_x, true_x, out_9101758542759318568); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5409150910353518766) { + inv_err_fun(nom_x, true_x, out_5409150910353518766); } -void pose_H_mod_fun(double *state, double *out_202451455870783186) { - H_mod_fun(state, out_202451455870783186); +void pose_H_mod_fun(double *state, double *out_8300172317114018464) { + H_mod_fun(state, out_8300172317114018464); } -void pose_f_fun(double *state, double dt, double *out_3404078345216964699) { - f_fun(state, dt, out_3404078345216964699); +void pose_f_fun(double *state, double dt, double *out_4063489601932878054) { + f_fun(state, dt, out_4063489601932878054); } -void pose_F_fun(double *state, double dt, double *out_9161898751948597001) { - F_fun(state, dt, out_9161898751948597001); +void pose_F_fun(double *state, double dt, double *out_493800117352810078) { + F_fun(state, dt, out_493800117352810078); } -void pose_h_4(double *state, double *unused, double *out_3298849734810867951) { - h_4(state, unused, out_3298849734810867951); +void pose_h_4(double *state, double *unused, double *out_6026765849154187968) { + h_4(state, unused, out_6026765849154187968); } -void pose_H_4(double *state, double *unused, double *out_3476673534661732439) { - H_4(state, unused, out_3476673534661732439); +void pose_H_4(double *state, double *unused, double *out_7102224835231120360) { + H_4(state, unused, out_7102224835231120360); } -void pose_h_10(double *state, double *unused, double *out_1490778051551919602) { - h_10(state, unused, out_1490778051551919602); +void pose_h_10(double *state, double *unused, double *out_1866086890220189324) { + h_10(state, unused, out_1866086890220189324); } -void pose_H_10(double *state, double *unused, double *out_3065010150012391749) { - H_10(state, unused, out_3065010150012391749); +void pose_H_10(double *state, double *unused, double *out_834802170779042492) { + H_10(state, unused, out_834802170779042492); } -void pose_h_13(double *state, double *unused, double *out_2293000846655251698) { - h_13(state, unused, out_2293000846655251698); +void pose_h_13(double *state, double *unused, double *out_4520826989648677276) { + h_13(state, unused, out_4520826989648677276); } -void pose_H_13(double *state, double *unused, double *out_4133957673654968490) { - H_13(state, unused, out_4133957673654968490); +void pose_H_13(double *state, double *unused, double *out_3889951009898787559) { + H_13(state, unused, out_3889951009898787559); } -void pose_h_14(double *state, double *unused, double *out_6732417031145183728) { - h_14(state, unused, out_6732417031145183728); +void pose_h_14(double *state, double *unused, double *out_733646167343188736) { + h_14(state, unused, out_733646167343188736); } -void pose_H_14(double *state, double *unused, double *out_486567321677752090) { - H_14(state, unused, out_486567321677752090); +void pose_H_14(double *state, double *unused, double *out_8261730806183058960) { + H_14(state, unused, out_8261730806183058960); } 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 04132e762..68e915733 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_3524911542636248309); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_9101758542759318568); -void pose_H_mod_fun(double *state, double *out_202451455870783186); -void pose_f_fun(double *state, double dt, double *out_3404078345216964699); -void pose_F_fun(double *state, double dt, double *out_9161898751948597001); -void pose_h_4(double *state, double *unused, double *out_3298849734810867951); -void pose_H_4(double *state, double *unused, double *out_3476673534661732439); -void pose_h_10(double *state, double *unused, double *out_1490778051551919602); -void pose_H_10(double *state, double *unused, double *out_3065010150012391749); -void pose_h_13(double *state, double *unused, double *out_2293000846655251698); -void pose_H_13(double *state, double *unused, double *out_4133957673654968490); -void pose_h_14(double *state, double *unused, double *out_6732417031145183728); -void pose_H_14(double *state, double *unused, double *out_486567321677752090); +void pose_err_fun(double *nom_x, double *delta_x, double *out_3341780452292505997); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5409150910353518766); +void pose_H_mod_fun(double *state, double *out_8300172317114018464); +void pose_f_fun(double *state, double dt, double *out_4063489601932878054); +void pose_F_fun(double *state, double dt, double *out_493800117352810078); +void pose_h_4(double *state, double *unused, double *out_6026765849154187968); +void pose_H_4(double *state, double *unused, double *out_7102224835231120360); +void pose_h_10(double *state, double *unused, double *out_1866086890220189324); +void pose_H_10(double *state, double *unused, double *out_834802170779042492); +void pose_h_13(double *state, double *unused, double *out_4520826989648677276); +void pose_H_13(double *state, double *unused, double *out_3889951009898787559); +void pose_h_14(double *state, double *unused, double *out_733646167343188736); +void pose_H_14(double *state, double *unused, double *out_8261730806183058960); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 84d9e1451..6fd78681f 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 48ab86fa5..c1ad733d5 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index e7be6a32d..c735d4582 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index efe46dd0f..1ad7a3639 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 1487c5120..811670dca 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 4297efe84..883a3232d 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ