diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index a70d9ed52..42c8caee6 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 1d10d0024..f20430986 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 636e93663..3e08fe061 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 8b0ac0428..c8beba9bf 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 4592d180e..3ed1745de 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 0ee7a700a..2a53e9610 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 75ae3a907..cf8ed51fe 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 75ae3a907..cf8ed51fe 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 e1809cddc..094bd4cb8 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 0ee7a700a..2a53e9610 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 43fce21e9..d942ba2f4 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-09a15719-DEBUG"; +const uint8_t gitversion[19] = "DEV-35a7e430-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 319ff3926..8393407e1 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 34500587e..fbbc33d63 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 b19351871..d2f557a42 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 9398c5604..f22407199 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 bdcad4b47..d7c25f292 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 0dc70372f..bd94efff8 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 f284d3162..9d16129d5 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 c244aff10..c770f39f2 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 25ab0e8ff..5b1ff389e 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 a1ac23a5a..bf903ec41 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 3bb36e0a2..3f0c5870e 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 322a8e1ea..70870df5c 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 b5d9c8af9..516b888d4 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 651417583..e03035f6c 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 419de0946..de6932f2d 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 0e8ca925a..c63deb82e 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 0d982a32d..9fe274216 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 6393c9ebd..83abd4547 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 4a0ad0324..a15d40ec8 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 7968b9365..716fc6a33 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 eac154240..48269e623 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-09a15719-DEBUG \ No newline at end of file +DEV-35a7e430-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 4dc91be63..1b1966bce 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_6442273530103712235) { - out_6442273530103712235[0] = delta_x[0] + nom_x[0]; - out_6442273530103712235[1] = delta_x[1] + nom_x[1]; - out_6442273530103712235[2] = delta_x[2] + nom_x[2]; - out_6442273530103712235[3] = delta_x[3] + nom_x[3]; - out_6442273530103712235[4] = delta_x[4] + nom_x[4]; - out_6442273530103712235[5] = delta_x[5] + nom_x[5]; - out_6442273530103712235[6] = delta_x[6] + nom_x[6]; - out_6442273530103712235[7] = delta_x[7] + nom_x[7]; - out_6442273530103712235[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_5406094264205448922) { + out_5406094264205448922[0] = delta_x[0] + nom_x[0]; + out_5406094264205448922[1] = delta_x[1] + nom_x[1]; + out_5406094264205448922[2] = delta_x[2] + nom_x[2]; + out_5406094264205448922[3] = delta_x[3] + nom_x[3]; + out_5406094264205448922[4] = delta_x[4] + nom_x[4]; + out_5406094264205448922[5] = delta_x[5] + nom_x[5]; + out_5406094264205448922[6] = delta_x[6] + nom_x[6]; + out_5406094264205448922[7] = delta_x[7] + nom_x[7]; + out_5406094264205448922[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8945695091662043361) { - out_8945695091662043361[0] = -nom_x[0] + true_x[0]; - out_8945695091662043361[1] = -nom_x[1] + true_x[1]; - out_8945695091662043361[2] = -nom_x[2] + true_x[2]; - out_8945695091662043361[3] = -nom_x[3] + true_x[3]; - out_8945695091662043361[4] = -nom_x[4] + true_x[4]; - out_8945695091662043361[5] = -nom_x[5] + true_x[5]; - out_8945695091662043361[6] = -nom_x[6] + true_x[6]; - out_8945695091662043361[7] = -nom_x[7] + true_x[7]; - out_8945695091662043361[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_7302786751934147898) { + out_7302786751934147898[0] = -nom_x[0] + true_x[0]; + out_7302786751934147898[1] = -nom_x[1] + true_x[1]; + out_7302786751934147898[2] = -nom_x[2] + true_x[2]; + out_7302786751934147898[3] = -nom_x[3] + true_x[3]; + out_7302786751934147898[4] = -nom_x[4] + true_x[4]; + out_7302786751934147898[5] = -nom_x[5] + true_x[5]; + out_7302786751934147898[6] = -nom_x[6] + true_x[6]; + out_7302786751934147898[7] = -nom_x[7] + true_x[7]; + out_7302786751934147898[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_515428113062222019) { - out_515428113062222019[0] = 1.0; - out_515428113062222019[1] = 0.0; - out_515428113062222019[2] = 0.0; - out_515428113062222019[3] = 0.0; - out_515428113062222019[4] = 0.0; - out_515428113062222019[5] = 0.0; - out_515428113062222019[6] = 0.0; - out_515428113062222019[7] = 0.0; - out_515428113062222019[8] = 0.0; - out_515428113062222019[9] = 0.0; - out_515428113062222019[10] = 1.0; - out_515428113062222019[11] = 0.0; - out_515428113062222019[12] = 0.0; - out_515428113062222019[13] = 0.0; - out_515428113062222019[14] = 0.0; - out_515428113062222019[15] = 0.0; - out_515428113062222019[16] = 0.0; - out_515428113062222019[17] = 0.0; - out_515428113062222019[18] = 0.0; - out_515428113062222019[19] = 0.0; - out_515428113062222019[20] = 1.0; - out_515428113062222019[21] = 0.0; - out_515428113062222019[22] = 0.0; - out_515428113062222019[23] = 0.0; - out_515428113062222019[24] = 0.0; - out_515428113062222019[25] = 0.0; - out_515428113062222019[26] = 0.0; - out_515428113062222019[27] = 0.0; - out_515428113062222019[28] = 0.0; - out_515428113062222019[29] = 0.0; - out_515428113062222019[30] = 1.0; - out_515428113062222019[31] = 0.0; - out_515428113062222019[32] = 0.0; - out_515428113062222019[33] = 0.0; - out_515428113062222019[34] = 0.0; - out_515428113062222019[35] = 0.0; - out_515428113062222019[36] = 0.0; - out_515428113062222019[37] = 0.0; - out_515428113062222019[38] = 0.0; - out_515428113062222019[39] = 0.0; - out_515428113062222019[40] = 1.0; - out_515428113062222019[41] = 0.0; - out_515428113062222019[42] = 0.0; - out_515428113062222019[43] = 0.0; - out_515428113062222019[44] = 0.0; - out_515428113062222019[45] = 0.0; - out_515428113062222019[46] = 0.0; - out_515428113062222019[47] = 0.0; - out_515428113062222019[48] = 0.0; - out_515428113062222019[49] = 0.0; - out_515428113062222019[50] = 1.0; - out_515428113062222019[51] = 0.0; - out_515428113062222019[52] = 0.0; - out_515428113062222019[53] = 0.0; - out_515428113062222019[54] = 0.0; - out_515428113062222019[55] = 0.0; - out_515428113062222019[56] = 0.0; - out_515428113062222019[57] = 0.0; - out_515428113062222019[58] = 0.0; - out_515428113062222019[59] = 0.0; - out_515428113062222019[60] = 1.0; - out_515428113062222019[61] = 0.0; - out_515428113062222019[62] = 0.0; - out_515428113062222019[63] = 0.0; - out_515428113062222019[64] = 0.0; - out_515428113062222019[65] = 0.0; - out_515428113062222019[66] = 0.0; - out_515428113062222019[67] = 0.0; - out_515428113062222019[68] = 0.0; - out_515428113062222019[69] = 0.0; - out_515428113062222019[70] = 1.0; - out_515428113062222019[71] = 0.0; - out_515428113062222019[72] = 0.0; - out_515428113062222019[73] = 0.0; - out_515428113062222019[74] = 0.0; - out_515428113062222019[75] = 0.0; - out_515428113062222019[76] = 0.0; - out_515428113062222019[77] = 0.0; - out_515428113062222019[78] = 0.0; - out_515428113062222019[79] = 0.0; - out_515428113062222019[80] = 1.0; +void H_mod_fun(double *state, double *out_8564810489090289784) { + out_8564810489090289784[0] = 1.0; + out_8564810489090289784[1] = 0.0; + out_8564810489090289784[2] = 0.0; + out_8564810489090289784[3] = 0.0; + out_8564810489090289784[4] = 0.0; + out_8564810489090289784[5] = 0.0; + out_8564810489090289784[6] = 0.0; + out_8564810489090289784[7] = 0.0; + out_8564810489090289784[8] = 0.0; + out_8564810489090289784[9] = 0.0; + out_8564810489090289784[10] = 1.0; + out_8564810489090289784[11] = 0.0; + out_8564810489090289784[12] = 0.0; + out_8564810489090289784[13] = 0.0; + out_8564810489090289784[14] = 0.0; + out_8564810489090289784[15] = 0.0; + out_8564810489090289784[16] = 0.0; + out_8564810489090289784[17] = 0.0; + out_8564810489090289784[18] = 0.0; + out_8564810489090289784[19] = 0.0; + out_8564810489090289784[20] = 1.0; + out_8564810489090289784[21] = 0.0; + out_8564810489090289784[22] = 0.0; + out_8564810489090289784[23] = 0.0; + out_8564810489090289784[24] = 0.0; + out_8564810489090289784[25] = 0.0; + out_8564810489090289784[26] = 0.0; + out_8564810489090289784[27] = 0.0; + out_8564810489090289784[28] = 0.0; + out_8564810489090289784[29] = 0.0; + out_8564810489090289784[30] = 1.0; + out_8564810489090289784[31] = 0.0; + out_8564810489090289784[32] = 0.0; + out_8564810489090289784[33] = 0.0; + out_8564810489090289784[34] = 0.0; + out_8564810489090289784[35] = 0.0; + out_8564810489090289784[36] = 0.0; + out_8564810489090289784[37] = 0.0; + out_8564810489090289784[38] = 0.0; + out_8564810489090289784[39] = 0.0; + out_8564810489090289784[40] = 1.0; + out_8564810489090289784[41] = 0.0; + out_8564810489090289784[42] = 0.0; + out_8564810489090289784[43] = 0.0; + out_8564810489090289784[44] = 0.0; + out_8564810489090289784[45] = 0.0; + out_8564810489090289784[46] = 0.0; + out_8564810489090289784[47] = 0.0; + out_8564810489090289784[48] = 0.0; + out_8564810489090289784[49] = 0.0; + out_8564810489090289784[50] = 1.0; + out_8564810489090289784[51] = 0.0; + out_8564810489090289784[52] = 0.0; + out_8564810489090289784[53] = 0.0; + out_8564810489090289784[54] = 0.0; + out_8564810489090289784[55] = 0.0; + out_8564810489090289784[56] = 0.0; + out_8564810489090289784[57] = 0.0; + out_8564810489090289784[58] = 0.0; + out_8564810489090289784[59] = 0.0; + out_8564810489090289784[60] = 1.0; + out_8564810489090289784[61] = 0.0; + out_8564810489090289784[62] = 0.0; + out_8564810489090289784[63] = 0.0; + out_8564810489090289784[64] = 0.0; + out_8564810489090289784[65] = 0.0; + out_8564810489090289784[66] = 0.0; + out_8564810489090289784[67] = 0.0; + out_8564810489090289784[68] = 0.0; + out_8564810489090289784[69] = 0.0; + out_8564810489090289784[70] = 1.0; + out_8564810489090289784[71] = 0.0; + out_8564810489090289784[72] = 0.0; + out_8564810489090289784[73] = 0.0; + out_8564810489090289784[74] = 0.0; + out_8564810489090289784[75] = 0.0; + out_8564810489090289784[76] = 0.0; + out_8564810489090289784[77] = 0.0; + out_8564810489090289784[78] = 0.0; + out_8564810489090289784[79] = 0.0; + out_8564810489090289784[80] = 1.0; } -void f_fun(double *state, double dt, double *out_1011805063482308945) { - out_1011805063482308945[0] = state[0]; - out_1011805063482308945[1] = state[1]; - out_1011805063482308945[2] = state[2]; - out_1011805063482308945[3] = state[3]; - out_1011805063482308945[4] = state[4]; - out_1011805063482308945[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_1011805063482308945[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_1011805063482308945[7] = state[7]; - out_1011805063482308945[8] = state[8]; +void f_fun(double *state, double dt, double *out_7344896958720977953) { + out_7344896958720977953[0] = state[0]; + out_7344896958720977953[1] = state[1]; + out_7344896958720977953[2] = state[2]; + out_7344896958720977953[3] = state[3]; + out_7344896958720977953[4] = state[4]; + out_7344896958720977953[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_7344896958720977953[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_7344896958720977953[7] = state[7]; + out_7344896958720977953[8] = state[8]; } -void F_fun(double *state, double dt, double *out_5888946012615310003) { - out_5888946012615310003[0] = 1; - out_5888946012615310003[1] = 0; - out_5888946012615310003[2] = 0; - out_5888946012615310003[3] = 0; - out_5888946012615310003[4] = 0; - out_5888946012615310003[5] = 0; - out_5888946012615310003[6] = 0; - out_5888946012615310003[7] = 0; - out_5888946012615310003[8] = 0; - out_5888946012615310003[9] = 0; - out_5888946012615310003[10] = 1; - out_5888946012615310003[11] = 0; - out_5888946012615310003[12] = 0; - out_5888946012615310003[13] = 0; - out_5888946012615310003[14] = 0; - out_5888946012615310003[15] = 0; - out_5888946012615310003[16] = 0; - out_5888946012615310003[17] = 0; - out_5888946012615310003[18] = 0; - out_5888946012615310003[19] = 0; - out_5888946012615310003[20] = 1; - out_5888946012615310003[21] = 0; - out_5888946012615310003[22] = 0; - out_5888946012615310003[23] = 0; - out_5888946012615310003[24] = 0; - out_5888946012615310003[25] = 0; - out_5888946012615310003[26] = 0; - out_5888946012615310003[27] = 0; - out_5888946012615310003[28] = 0; - out_5888946012615310003[29] = 0; - out_5888946012615310003[30] = 1; - out_5888946012615310003[31] = 0; - out_5888946012615310003[32] = 0; - out_5888946012615310003[33] = 0; - out_5888946012615310003[34] = 0; - out_5888946012615310003[35] = 0; - out_5888946012615310003[36] = 0; - out_5888946012615310003[37] = 0; - out_5888946012615310003[38] = 0; - out_5888946012615310003[39] = 0; - out_5888946012615310003[40] = 1; - out_5888946012615310003[41] = 0; - out_5888946012615310003[42] = 0; - out_5888946012615310003[43] = 0; - out_5888946012615310003[44] = 0; - out_5888946012615310003[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_5888946012615310003[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_5888946012615310003[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_5888946012615310003[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_5888946012615310003[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_5888946012615310003[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_5888946012615310003[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_5888946012615310003[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_5888946012615310003[53] = -9.8100000000000005*dt; - out_5888946012615310003[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_5888946012615310003[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_5888946012615310003[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_5888946012615310003[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_5888946012615310003[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_5888946012615310003[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_5888946012615310003[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_5888946012615310003[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_5888946012615310003[62] = 0; - out_5888946012615310003[63] = 0; - out_5888946012615310003[64] = 0; - out_5888946012615310003[65] = 0; - out_5888946012615310003[66] = 0; - out_5888946012615310003[67] = 0; - out_5888946012615310003[68] = 0; - out_5888946012615310003[69] = 0; - out_5888946012615310003[70] = 1; - out_5888946012615310003[71] = 0; - out_5888946012615310003[72] = 0; - out_5888946012615310003[73] = 0; - out_5888946012615310003[74] = 0; - out_5888946012615310003[75] = 0; - out_5888946012615310003[76] = 0; - out_5888946012615310003[77] = 0; - out_5888946012615310003[78] = 0; - out_5888946012615310003[79] = 0; - out_5888946012615310003[80] = 1; +void F_fun(double *state, double dt, double *out_4577316669100378715) { + out_4577316669100378715[0] = 1; + out_4577316669100378715[1] = 0; + out_4577316669100378715[2] = 0; + out_4577316669100378715[3] = 0; + out_4577316669100378715[4] = 0; + out_4577316669100378715[5] = 0; + out_4577316669100378715[6] = 0; + out_4577316669100378715[7] = 0; + out_4577316669100378715[8] = 0; + out_4577316669100378715[9] = 0; + out_4577316669100378715[10] = 1; + out_4577316669100378715[11] = 0; + out_4577316669100378715[12] = 0; + out_4577316669100378715[13] = 0; + out_4577316669100378715[14] = 0; + out_4577316669100378715[15] = 0; + out_4577316669100378715[16] = 0; + out_4577316669100378715[17] = 0; + out_4577316669100378715[18] = 0; + out_4577316669100378715[19] = 0; + out_4577316669100378715[20] = 1; + out_4577316669100378715[21] = 0; + out_4577316669100378715[22] = 0; + out_4577316669100378715[23] = 0; + out_4577316669100378715[24] = 0; + out_4577316669100378715[25] = 0; + out_4577316669100378715[26] = 0; + out_4577316669100378715[27] = 0; + out_4577316669100378715[28] = 0; + out_4577316669100378715[29] = 0; + out_4577316669100378715[30] = 1; + out_4577316669100378715[31] = 0; + out_4577316669100378715[32] = 0; + out_4577316669100378715[33] = 0; + out_4577316669100378715[34] = 0; + out_4577316669100378715[35] = 0; + out_4577316669100378715[36] = 0; + out_4577316669100378715[37] = 0; + out_4577316669100378715[38] = 0; + out_4577316669100378715[39] = 0; + out_4577316669100378715[40] = 1; + out_4577316669100378715[41] = 0; + out_4577316669100378715[42] = 0; + out_4577316669100378715[43] = 0; + out_4577316669100378715[44] = 0; + out_4577316669100378715[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_4577316669100378715[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_4577316669100378715[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4577316669100378715[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4577316669100378715[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_4577316669100378715[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_4577316669100378715[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_4577316669100378715[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_4577316669100378715[53] = -9.8100000000000005*dt; + out_4577316669100378715[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_4577316669100378715[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_4577316669100378715[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4577316669100378715[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4577316669100378715[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_4577316669100378715[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_4577316669100378715[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_4577316669100378715[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4577316669100378715[62] = 0; + out_4577316669100378715[63] = 0; + out_4577316669100378715[64] = 0; + out_4577316669100378715[65] = 0; + out_4577316669100378715[66] = 0; + out_4577316669100378715[67] = 0; + out_4577316669100378715[68] = 0; + out_4577316669100378715[69] = 0; + out_4577316669100378715[70] = 1; + out_4577316669100378715[71] = 0; + out_4577316669100378715[72] = 0; + out_4577316669100378715[73] = 0; + out_4577316669100378715[74] = 0; + out_4577316669100378715[75] = 0; + out_4577316669100378715[76] = 0; + out_4577316669100378715[77] = 0; + out_4577316669100378715[78] = 0; + out_4577316669100378715[79] = 0; + out_4577316669100378715[80] = 1; } -void h_25(double *state, double *unused, double *out_765665439417021084) { - out_765665439417021084[0] = state[6]; +void h_25(double *state, double *unused, double *out_6986192086353969465) { + out_6986192086353969465[0] = state[6]; } -void H_25(double *state, double *unused, double *out_10093670861925307) { - out_10093670861925307[0] = 0; - out_10093670861925307[1] = 0; - out_10093670861925307[2] = 0; - out_10093670861925307[3] = 0; - out_10093670861925307[4] = 0; - out_10093670861925307[5] = 0; - out_10093670861925307[6] = 1; - out_10093670861925307[7] = 0; - out_10093670861925307[8] = 0; +void H_25(double *state, double *unused, double *out_628377736821947453) { + out_628377736821947453[0] = 0; + out_628377736821947453[1] = 0; + out_628377736821947453[2] = 0; + out_628377736821947453[3] = 0; + out_628377736821947453[4] = 0; + out_628377736821947453[5] = 0; + out_628377736821947453[6] = 1; + out_628377736821947453[7] = 0; + out_628377736821947453[8] = 0; } -void h_24(double *state, double *unused, double *out_7010003887632078552) { - out_7010003887632078552[0] = state[4]; - out_7010003887632078552[1] = state[5]; +void h_24(double *state, double *unused, double *out_434665502247836674) { + out_434665502247836674[0] = state[4]; + out_434665502247836674[1] = state[5]; } -void H_24(double *state, double *unused, double *out_8784986078674096201) { - out_8784986078674096201[0] = 0; - out_8784986078674096201[1] = 0; - out_8784986078674096201[2] = 0; - out_8784986078674096201[3] = 0; - out_8784986078674096201[4] = 1; - out_8784986078674096201[5] = 0; - out_8784986078674096201[6] = 0; - out_8784986078674096201[7] = 0; - out_8784986078674096201[8] = 0; - out_8784986078674096201[9] = 0; - out_8784986078674096201[10] = 0; - out_8784986078674096201[11] = 0; - out_8784986078674096201[12] = 0; - out_8784986078674096201[13] = 0; - out_8784986078674096201[14] = 1; - out_8784986078674096201[15] = 0; - out_8784986078674096201[16] = 0; - out_8784986078674096201[17] = 0; +void H_24(double *state, double *unused, double *out_2801027335827447019) { + out_2801027335827447019[0] = 0; + out_2801027335827447019[1] = 0; + out_2801027335827447019[2] = 0; + out_2801027335827447019[3] = 0; + out_2801027335827447019[4] = 1; + out_2801027335827447019[5] = 0; + out_2801027335827447019[6] = 0; + out_2801027335827447019[7] = 0; + out_2801027335827447019[8] = 0; + out_2801027335827447019[9] = 0; + out_2801027335827447019[10] = 0; + out_2801027335827447019[11] = 0; + out_2801027335827447019[12] = 0; + out_2801027335827447019[13] = 0; + out_2801027335827447019[14] = 1; + out_2801027335827447019[15] = 0; + out_2801027335827447019[16] = 0; + out_2801027335827447019[17] = 0; } -void h_30(double *state, double *unused, double *out_3713391429772506045) { - out_3713391429772506045[0] = state[4]; +void h_30(double *state, double *unused, double *out_4915078315896353947) { + out_4915078315896353947[0] = state[4]; } -void H_30(double *state, double *unused, double *out_6906596670629691448) { - out_6906596670629691448[0] = 0; - out_6906596670629691448[1] = 0; - out_6906596670629691448[2] = 0; - out_6906596670629691448[3] = 0; - out_6906596670629691448[4] = 1; - out_6906596670629691448[5] = 0; - out_6906596670629691448[6] = 0; - out_6906596670629691448[7] = 0; - out_6906596670629691448[8] = 0; +void H_30(double *state, double *unused, double *out_6288312604669669302) { + out_6288312604669669302[0] = 0; + out_6288312604669669302[1] = 0; + out_6288312604669669302[2] = 0; + out_6288312604669669302[3] = 0; + out_6288312604669669302[4] = 1; + out_6288312604669669302[5] = 0; + out_6288312604669669302[6] = 0; + out_6288312604669669302[7] = 0; + out_6288312604669669302[8] = 0; } -void h_26(double *state, double *unused, double *out_6525943533042359428) { - out_6525943533042359428[0] = state[7]; +void h_26(double *state, double *unused, double *out_4943503076008356261) { + out_4943503076008356261[0] = state[7]; } -void H_26(double *state, double *unused, double *out_3751596989735981531) { - out_3751596989735981531[0] = 0; - out_3751596989735981531[1] = 0; - out_3751596989735981531[2] = 0; - out_3751596989735981531[3] = 0; - out_3751596989735981531[4] = 0; - out_3751596989735981531[5] = 0; - out_3751596989735981531[6] = 0; - out_3751596989735981531[7] = 1; - out_3751596989735981531[8] = 0; +void H_26(double *state, double *unused, double *out_4369881055696003677) { + out_4369881055696003677[0] = 0; + out_4369881055696003677[1] = 0; + out_4369881055696003677[2] = 0; + out_4369881055696003677[3] = 0; + out_4369881055696003677[4] = 0; + out_4369881055696003677[5] = 0; + out_4369881055696003677[6] = 0; + out_4369881055696003677[7] = 1; + out_4369881055696003677[8] = 0; } -void h_27(double *state, double *unused, double *out_3564565336305065623) { - out_3564565336305065623[0] = state[3]; +void h_27(double *state, double *unused, double *out_1984518818693123820) { + out_1984518818693123820[0] = state[3]; } -void H_27(double *state, double *unused, double *out_4731833358829266537) { - out_4731833358829266537[0] = 0; - out_4731833358829266537[1] = 0; - out_4731833358829266537[2] = 0; - out_4731833358829266537[3] = 1; - out_4731833358829266537[4] = 0; - out_4731833358829266537[5] = 0; - out_4731833358829266537[6] = 0; - out_4731833358829266537[7] = 0; - out_4731833358829266537[8] = 0; +void H_27(double *state, double *unused, double *out_2932479995765612434) { + out_2932479995765612434[0] = 0; + out_2932479995765612434[1] = 0; + out_2932479995765612434[2] = 0; + out_2932479995765612434[3] = 1; + out_2932479995765612434[4] = 0; + out_2932479995765612434[5] = 0; + out_2932479995765612434[6] = 0; + out_2932479995765612434[7] = 0; + out_2932479995765612434[8] = 0; } -void h_29(double *state, double *unused, double *out_4316707314448983562) { - out_4316707314448983562[0] = state[1]; +void h_29(double *state, double *unused, double *out_963207171662361141) { + out_963207171662361141[0] = state[1]; } -void H_29(double *state, double *unused, double *out_7416828014944083632) { - out_7416828014944083632[0] = 0; - out_7416828014944083632[1] = 1; - out_7416828014944083632[2] = 0; - out_7416828014944083632[3] = 0; - out_7416828014944083632[4] = 0; - out_7416828014944083632[5] = 0; - out_7416828014944083632[6] = 0; - out_7416828014944083632[7] = 0; - out_7416828014944083632[8] = 0; +void H_29(double *state, double *unused, double *out_4645842722635163467) { + out_4645842722635163467[0] = 0; + out_4645842722635163467[1] = 1; + out_4645842722635163467[2] = 0; + out_4645842722635163467[3] = 0; + out_4645842722635163467[4] = 0; + out_4645842722635163467[5] = 0; + out_4645842722635163467[6] = 0; + out_4645842722635163467[7] = 0; + out_4645842722635163467[8] = 0; } -void h_28(double *state, double *unused, double *out_8899254898098344240) { - out_8899254898098344240[0] = state[0]; +void h_28(double *state, double *unused, double *out_1002434725357825708) { + out_1002434725357825708[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2063928385109815070) { - out_2063928385109815070[0] = 1; - out_2063928385109815070[1] = 0; - out_2063928385109815070[2] = 0; - out_2063928385109815070[3] = 0; - out_2063928385109815070[4] = 0; - out_2063928385109815070[5] = 0; - out_2063928385109815070[6] = 0; - out_2063928385109815070[7] = 0; - out_2063928385109815070[8] = 0; +void H_28(double *state, double *unused, double *out_2682212451069837216) { + out_2682212451069837216[0] = 1; + out_2682212451069837216[1] = 0; + out_2682212451069837216[2] = 0; + out_2682212451069837216[3] = 0; + out_2682212451069837216[4] = 0; + out_2682212451069837216[5] = 0; + out_2682212451069837216[6] = 0; + out_2682212451069837216[7] = 0; + out_2682212451069837216[8] = 0; } -void h_31(double *state, double *unused, double *out_7115607211337028101) { - out_7115607211337028101[0] = state[8]; +void h_31(double *state, double *unused, double *out_7261386148638475354) { + out_7261386148638475354[0] = state[8]; } -void H_31(double *state, double *unused, double *out_20552291015035121) { - out_20552291015035121[0] = 0; - out_20552291015035121[1] = 0; - out_20552291015035121[2] = 0; - out_20552291015035121[3] = 0; - out_20552291015035121[4] = 0; - out_20552291015035121[5] = 0; - out_20552291015035121[6] = 0; - out_20552291015035121[7] = 0; - out_20552291015035121[8] = 1; +void H_31(double *state, double *unused, double *out_597731774944987025) { + out_597731774944987025[0] = 0; + out_597731774944987025[1] = 0; + out_597731774944987025[2] = 0; + out_597731774944987025[3] = 0; + out_597731774944987025[4] = 0; + out_597731774944987025[5] = 0; + out_597731774944987025[6] = 0; + out_597731774944987025[7] = 0; + out_597731774944987025[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_6442273530103712235) { - err_fun(nom_x, delta_x, out_6442273530103712235); +void car_err_fun(double *nom_x, double *delta_x, double *out_5406094264205448922) { + err_fun(nom_x, delta_x, out_5406094264205448922); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8945695091662043361) { - inv_err_fun(nom_x, true_x, out_8945695091662043361); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_7302786751934147898) { + inv_err_fun(nom_x, true_x, out_7302786751934147898); } -void car_H_mod_fun(double *state, double *out_515428113062222019) { - H_mod_fun(state, out_515428113062222019); +void car_H_mod_fun(double *state, double *out_8564810489090289784) { + H_mod_fun(state, out_8564810489090289784); } -void car_f_fun(double *state, double dt, double *out_1011805063482308945) { - f_fun(state, dt, out_1011805063482308945); +void car_f_fun(double *state, double dt, double *out_7344896958720977953) { + f_fun(state, dt, out_7344896958720977953); } -void car_F_fun(double *state, double dt, double *out_5888946012615310003) { - F_fun(state, dt, out_5888946012615310003); +void car_F_fun(double *state, double dt, double *out_4577316669100378715) { + F_fun(state, dt, out_4577316669100378715); } -void car_h_25(double *state, double *unused, double *out_765665439417021084) { - h_25(state, unused, out_765665439417021084); +void car_h_25(double *state, double *unused, double *out_6986192086353969465) { + h_25(state, unused, out_6986192086353969465); } -void car_H_25(double *state, double *unused, double *out_10093670861925307) { - H_25(state, unused, out_10093670861925307); +void car_H_25(double *state, double *unused, double *out_628377736821947453) { + H_25(state, unused, out_628377736821947453); } -void car_h_24(double *state, double *unused, double *out_7010003887632078552) { - h_24(state, unused, out_7010003887632078552); +void car_h_24(double *state, double *unused, double *out_434665502247836674) { + h_24(state, unused, out_434665502247836674); } -void car_H_24(double *state, double *unused, double *out_8784986078674096201) { - H_24(state, unused, out_8784986078674096201); +void car_H_24(double *state, double *unused, double *out_2801027335827447019) { + H_24(state, unused, out_2801027335827447019); } -void car_h_30(double *state, double *unused, double *out_3713391429772506045) { - h_30(state, unused, out_3713391429772506045); +void car_h_30(double *state, double *unused, double *out_4915078315896353947) { + h_30(state, unused, out_4915078315896353947); } -void car_H_30(double *state, double *unused, double *out_6906596670629691448) { - H_30(state, unused, out_6906596670629691448); +void car_H_30(double *state, double *unused, double *out_6288312604669669302) { + H_30(state, unused, out_6288312604669669302); } -void car_h_26(double *state, double *unused, double *out_6525943533042359428) { - h_26(state, unused, out_6525943533042359428); +void car_h_26(double *state, double *unused, double *out_4943503076008356261) { + h_26(state, unused, out_4943503076008356261); } -void car_H_26(double *state, double *unused, double *out_3751596989735981531) { - H_26(state, unused, out_3751596989735981531); +void car_H_26(double *state, double *unused, double *out_4369881055696003677) { + H_26(state, unused, out_4369881055696003677); } -void car_h_27(double *state, double *unused, double *out_3564565336305065623) { - h_27(state, unused, out_3564565336305065623); +void car_h_27(double *state, double *unused, double *out_1984518818693123820) { + h_27(state, unused, out_1984518818693123820); } -void car_H_27(double *state, double *unused, double *out_4731833358829266537) { - H_27(state, unused, out_4731833358829266537); +void car_H_27(double *state, double *unused, double *out_2932479995765612434) { + H_27(state, unused, out_2932479995765612434); } -void car_h_29(double *state, double *unused, double *out_4316707314448983562) { - h_29(state, unused, out_4316707314448983562); +void car_h_29(double *state, double *unused, double *out_963207171662361141) { + h_29(state, unused, out_963207171662361141); } -void car_H_29(double *state, double *unused, double *out_7416828014944083632) { - H_29(state, unused, out_7416828014944083632); +void car_H_29(double *state, double *unused, double *out_4645842722635163467) { + H_29(state, unused, out_4645842722635163467); } -void car_h_28(double *state, double *unused, double *out_8899254898098344240) { - h_28(state, unused, out_8899254898098344240); +void car_h_28(double *state, double *unused, double *out_1002434725357825708) { + h_28(state, unused, out_1002434725357825708); } -void car_H_28(double *state, double *unused, double *out_2063928385109815070) { - H_28(state, unused, out_2063928385109815070); +void car_H_28(double *state, double *unused, double *out_2682212451069837216) { + H_28(state, unused, out_2682212451069837216); } -void car_h_31(double *state, double *unused, double *out_7115607211337028101) { - h_31(state, unused, out_7115607211337028101); +void car_h_31(double *state, double *unused, double *out_7261386148638475354) { + h_31(state, unused, out_7261386148638475354); } -void car_H_31(double *state, double *unused, double *out_20552291015035121) { - H_31(state, unused, out_20552291015035121); +void car_H_31(double *state, double *unused, double *out_597731774944987025) { + H_31(state, unused, out_597731774944987025); } 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 ca401c4b5..f8813fec3 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_6442273530103712235); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_8945695091662043361); -void car_H_mod_fun(double *state, double *out_515428113062222019); -void car_f_fun(double *state, double dt, double *out_1011805063482308945); -void car_F_fun(double *state, double dt, double *out_5888946012615310003); -void car_h_25(double *state, double *unused, double *out_765665439417021084); -void car_H_25(double *state, double *unused, double *out_10093670861925307); -void car_h_24(double *state, double *unused, double *out_7010003887632078552); -void car_H_24(double *state, double *unused, double *out_8784986078674096201); -void car_h_30(double *state, double *unused, double *out_3713391429772506045); -void car_H_30(double *state, double *unused, double *out_6906596670629691448); -void car_h_26(double *state, double *unused, double *out_6525943533042359428); -void car_H_26(double *state, double *unused, double *out_3751596989735981531); -void car_h_27(double *state, double *unused, double *out_3564565336305065623); -void car_H_27(double *state, double *unused, double *out_4731833358829266537); -void car_h_29(double *state, double *unused, double *out_4316707314448983562); -void car_H_29(double *state, double *unused, double *out_7416828014944083632); -void car_h_28(double *state, double *unused, double *out_8899254898098344240); -void car_H_28(double *state, double *unused, double *out_2063928385109815070); -void car_h_31(double *state, double *unused, double *out_7115607211337028101); -void car_H_31(double *state, double *unused, double *out_20552291015035121); +void car_err_fun(double *nom_x, double *delta_x, double *out_5406094264205448922); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_7302786751934147898); +void car_H_mod_fun(double *state, double *out_8564810489090289784); +void car_f_fun(double *state, double dt, double *out_7344896958720977953); +void car_F_fun(double *state, double dt, double *out_4577316669100378715); +void car_h_25(double *state, double *unused, double *out_6986192086353969465); +void car_H_25(double *state, double *unused, double *out_628377736821947453); +void car_h_24(double *state, double *unused, double *out_434665502247836674); +void car_H_24(double *state, double *unused, double *out_2801027335827447019); +void car_h_30(double *state, double *unused, double *out_4915078315896353947); +void car_H_30(double *state, double *unused, double *out_6288312604669669302); +void car_h_26(double *state, double *unused, double *out_4943503076008356261); +void car_H_26(double *state, double *unused, double *out_4369881055696003677); +void car_h_27(double *state, double *unused, double *out_1984518818693123820); +void car_H_27(double *state, double *unused, double *out_2932479995765612434); +void car_h_29(double *state, double *unused, double *out_963207171662361141); +void car_H_29(double *state, double *unused, double *out_4645842722635163467); +void car_h_28(double *state, double *unused, double *out_1002434725357825708); +void car_H_28(double *state, double *unused, double *out_2682212451069837216); +void car_h_31(double *state, double *unused, double *out_7261386148638475354); +void car_H_31(double *state, double *unused, double *out_597731774944987025); 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 8a1ff09ed..2ef680f47 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_5735299356181127199) { - out_5735299356181127199[0] = delta_x[0] + nom_x[0]; - out_5735299356181127199[1] = delta_x[1] + nom_x[1]; - out_5735299356181127199[2] = delta_x[2] + nom_x[2]; - out_5735299356181127199[3] = delta_x[3] + nom_x[3]; - out_5735299356181127199[4] = delta_x[4] + nom_x[4]; - out_5735299356181127199[5] = delta_x[5] + nom_x[5]; - out_5735299356181127199[6] = delta_x[6] + nom_x[6]; - out_5735299356181127199[7] = delta_x[7] + nom_x[7]; - out_5735299356181127199[8] = delta_x[8] + nom_x[8]; - out_5735299356181127199[9] = delta_x[9] + nom_x[9]; - out_5735299356181127199[10] = delta_x[10] + nom_x[10]; - out_5735299356181127199[11] = delta_x[11] + nom_x[11]; - out_5735299356181127199[12] = delta_x[12] + nom_x[12]; - out_5735299356181127199[13] = delta_x[13] + nom_x[13]; - out_5735299356181127199[14] = delta_x[14] + nom_x[14]; - out_5735299356181127199[15] = delta_x[15] + nom_x[15]; - out_5735299356181127199[16] = delta_x[16] + nom_x[16]; - out_5735299356181127199[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_6497612872484174174) { + out_6497612872484174174[0] = delta_x[0] + nom_x[0]; + out_6497612872484174174[1] = delta_x[1] + nom_x[1]; + out_6497612872484174174[2] = delta_x[2] + nom_x[2]; + out_6497612872484174174[3] = delta_x[3] + nom_x[3]; + out_6497612872484174174[4] = delta_x[4] + nom_x[4]; + out_6497612872484174174[5] = delta_x[5] + nom_x[5]; + out_6497612872484174174[6] = delta_x[6] + nom_x[6]; + out_6497612872484174174[7] = delta_x[7] + nom_x[7]; + out_6497612872484174174[8] = delta_x[8] + nom_x[8]; + out_6497612872484174174[9] = delta_x[9] + nom_x[9]; + out_6497612872484174174[10] = delta_x[10] + nom_x[10]; + out_6497612872484174174[11] = delta_x[11] + nom_x[11]; + out_6497612872484174174[12] = delta_x[12] + nom_x[12]; + out_6497612872484174174[13] = delta_x[13] + nom_x[13]; + out_6497612872484174174[14] = delta_x[14] + nom_x[14]; + out_6497612872484174174[15] = delta_x[15] + nom_x[15]; + out_6497612872484174174[16] = delta_x[16] + nom_x[16]; + out_6497612872484174174[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1848486118618361424) { - out_1848486118618361424[0] = -nom_x[0] + true_x[0]; - out_1848486118618361424[1] = -nom_x[1] + true_x[1]; - out_1848486118618361424[2] = -nom_x[2] + true_x[2]; - out_1848486118618361424[3] = -nom_x[3] + true_x[3]; - out_1848486118618361424[4] = -nom_x[4] + true_x[4]; - out_1848486118618361424[5] = -nom_x[5] + true_x[5]; - out_1848486118618361424[6] = -nom_x[6] + true_x[6]; - out_1848486118618361424[7] = -nom_x[7] + true_x[7]; - out_1848486118618361424[8] = -nom_x[8] + true_x[8]; - out_1848486118618361424[9] = -nom_x[9] + true_x[9]; - out_1848486118618361424[10] = -nom_x[10] + true_x[10]; - out_1848486118618361424[11] = -nom_x[11] + true_x[11]; - out_1848486118618361424[12] = -nom_x[12] + true_x[12]; - out_1848486118618361424[13] = -nom_x[13] + true_x[13]; - out_1848486118618361424[14] = -nom_x[14] + true_x[14]; - out_1848486118618361424[15] = -nom_x[15] + true_x[15]; - out_1848486118618361424[16] = -nom_x[16] + true_x[16]; - out_1848486118618361424[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1975580934350294204) { + out_1975580934350294204[0] = -nom_x[0] + true_x[0]; + out_1975580934350294204[1] = -nom_x[1] + true_x[1]; + out_1975580934350294204[2] = -nom_x[2] + true_x[2]; + out_1975580934350294204[3] = -nom_x[3] + true_x[3]; + out_1975580934350294204[4] = -nom_x[4] + true_x[4]; + out_1975580934350294204[5] = -nom_x[5] + true_x[5]; + out_1975580934350294204[6] = -nom_x[6] + true_x[6]; + out_1975580934350294204[7] = -nom_x[7] + true_x[7]; + out_1975580934350294204[8] = -nom_x[8] + true_x[8]; + out_1975580934350294204[9] = -nom_x[9] + true_x[9]; + out_1975580934350294204[10] = -nom_x[10] + true_x[10]; + out_1975580934350294204[11] = -nom_x[11] + true_x[11]; + out_1975580934350294204[12] = -nom_x[12] + true_x[12]; + out_1975580934350294204[13] = -nom_x[13] + true_x[13]; + out_1975580934350294204[14] = -nom_x[14] + true_x[14]; + out_1975580934350294204[15] = -nom_x[15] + true_x[15]; + out_1975580934350294204[16] = -nom_x[16] + true_x[16]; + out_1975580934350294204[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_3441620345746718845) { - out_3441620345746718845[0] = 1.0; - out_3441620345746718845[1] = 0.0; - out_3441620345746718845[2] = 0.0; - out_3441620345746718845[3] = 0.0; - out_3441620345746718845[4] = 0.0; - out_3441620345746718845[5] = 0.0; - out_3441620345746718845[6] = 0.0; - out_3441620345746718845[7] = 0.0; - out_3441620345746718845[8] = 0.0; - out_3441620345746718845[9] = 0.0; - out_3441620345746718845[10] = 0.0; - out_3441620345746718845[11] = 0.0; - out_3441620345746718845[12] = 0.0; - out_3441620345746718845[13] = 0.0; - out_3441620345746718845[14] = 0.0; - out_3441620345746718845[15] = 0.0; - out_3441620345746718845[16] = 0.0; - out_3441620345746718845[17] = 0.0; - out_3441620345746718845[18] = 0.0; - out_3441620345746718845[19] = 1.0; - out_3441620345746718845[20] = 0.0; - out_3441620345746718845[21] = 0.0; - out_3441620345746718845[22] = 0.0; - out_3441620345746718845[23] = 0.0; - out_3441620345746718845[24] = 0.0; - out_3441620345746718845[25] = 0.0; - out_3441620345746718845[26] = 0.0; - out_3441620345746718845[27] = 0.0; - out_3441620345746718845[28] = 0.0; - out_3441620345746718845[29] = 0.0; - out_3441620345746718845[30] = 0.0; - out_3441620345746718845[31] = 0.0; - out_3441620345746718845[32] = 0.0; - out_3441620345746718845[33] = 0.0; - out_3441620345746718845[34] = 0.0; - out_3441620345746718845[35] = 0.0; - out_3441620345746718845[36] = 0.0; - out_3441620345746718845[37] = 0.0; - out_3441620345746718845[38] = 1.0; - out_3441620345746718845[39] = 0.0; - out_3441620345746718845[40] = 0.0; - out_3441620345746718845[41] = 0.0; - out_3441620345746718845[42] = 0.0; - out_3441620345746718845[43] = 0.0; - out_3441620345746718845[44] = 0.0; - out_3441620345746718845[45] = 0.0; - out_3441620345746718845[46] = 0.0; - out_3441620345746718845[47] = 0.0; - out_3441620345746718845[48] = 0.0; - out_3441620345746718845[49] = 0.0; - out_3441620345746718845[50] = 0.0; - out_3441620345746718845[51] = 0.0; - out_3441620345746718845[52] = 0.0; - out_3441620345746718845[53] = 0.0; - out_3441620345746718845[54] = 0.0; - out_3441620345746718845[55] = 0.0; - out_3441620345746718845[56] = 0.0; - out_3441620345746718845[57] = 1.0; - out_3441620345746718845[58] = 0.0; - out_3441620345746718845[59] = 0.0; - out_3441620345746718845[60] = 0.0; - out_3441620345746718845[61] = 0.0; - out_3441620345746718845[62] = 0.0; - out_3441620345746718845[63] = 0.0; - out_3441620345746718845[64] = 0.0; - out_3441620345746718845[65] = 0.0; - out_3441620345746718845[66] = 0.0; - out_3441620345746718845[67] = 0.0; - out_3441620345746718845[68] = 0.0; - out_3441620345746718845[69] = 0.0; - out_3441620345746718845[70] = 0.0; - out_3441620345746718845[71] = 0.0; - out_3441620345746718845[72] = 0.0; - out_3441620345746718845[73] = 0.0; - out_3441620345746718845[74] = 0.0; - out_3441620345746718845[75] = 0.0; - out_3441620345746718845[76] = 1.0; - out_3441620345746718845[77] = 0.0; - out_3441620345746718845[78] = 0.0; - out_3441620345746718845[79] = 0.0; - out_3441620345746718845[80] = 0.0; - out_3441620345746718845[81] = 0.0; - out_3441620345746718845[82] = 0.0; - out_3441620345746718845[83] = 0.0; - out_3441620345746718845[84] = 0.0; - out_3441620345746718845[85] = 0.0; - out_3441620345746718845[86] = 0.0; - out_3441620345746718845[87] = 0.0; - out_3441620345746718845[88] = 0.0; - out_3441620345746718845[89] = 0.0; - out_3441620345746718845[90] = 0.0; - out_3441620345746718845[91] = 0.0; - out_3441620345746718845[92] = 0.0; - out_3441620345746718845[93] = 0.0; - out_3441620345746718845[94] = 0.0; - out_3441620345746718845[95] = 1.0; - out_3441620345746718845[96] = 0.0; - out_3441620345746718845[97] = 0.0; - out_3441620345746718845[98] = 0.0; - out_3441620345746718845[99] = 0.0; - out_3441620345746718845[100] = 0.0; - out_3441620345746718845[101] = 0.0; - out_3441620345746718845[102] = 0.0; - out_3441620345746718845[103] = 0.0; - out_3441620345746718845[104] = 0.0; - out_3441620345746718845[105] = 0.0; - out_3441620345746718845[106] = 0.0; - out_3441620345746718845[107] = 0.0; - out_3441620345746718845[108] = 0.0; - out_3441620345746718845[109] = 0.0; - out_3441620345746718845[110] = 0.0; - out_3441620345746718845[111] = 0.0; - out_3441620345746718845[112] = 0.0; - out_3441620345746718845[113] = 0.0; - out_3441620345746718845[114] = 1.0; - out_3441620345746718845[115] = 0.0; - out_3441620345746718845[116] = 0.0; - out_3441620345746718845[117] = 0.0; - out_3441620345746718845[118] = 0.0; - out_3441620345746718845[119] = 0.0; - out_3441620345746718845[120] = 0.0; - out_3441620345746718845[121] = 0.0; - out_3441620345746718845[122] = 0.0; - out_3441620345746718845[123] = 0.0; - out_3441620345746718845[124] = 0.0; - out_3441620345746718845[125] = 0.0; - out_3441620345746718845[126] = 0.0; - out_3441620345746718845[127] = 0.0; - out_3441620345746718845[128] = 0.0; - out_3441620345746718845[129] = 0.0; - out_3441620345746718845[130] = 0.0; - out_3441620345746718845[131] = 0.0; - out_3441620345746718845[132] = 0.0; - out_3441620345746718845[133] = 1.0; - out_3441620345746718845[134] = 0.0; - out_3441620345746718845[135] = 0.0; - out_3441620345746718845[136] = 0.0; - out_3441620345746718845[137] = 0.0; - out_3441620345746718845[138] = 0.0; - out_3441620345746718845[139] = 0.0; - out_3441620345746718845[140] = 0.0; - out_3441620345746718845[141] = 0.0; - out_3441620345746718845[142] = 0.0; - out_3441620345746718845[143] = 0.0; - out_3441620345746718845[144] = 0.0; - out_3441620345746718845[145] = 0.0; - out_3441620345746718845[146] = 0.0; - out_3441620345746718845[147] = 0.0; - out_3441620345746718845[148] = 0.0; - out_3441620345746718845[149] = 0.0; - out_3441620345746718845[150] = 0.0; - out_3441620345746718845[151] = 0.0; - out_3441620345746718845[152] = 1.0; - out_3441620345746718845[153] = 0.0; - out_3441620345746718845[154] = 0.0; - out_3441620345746718845[155] = 0.0; - out_3441620345746718845[156] = 0.0; - out_3441620345746718845[157] = 0.0; - out_3441620345746718845[158] = 0.0; - out_3441620345746718845[159] = 0.0; - out_3441620345746718845[160] = 0.0; - out_3441620345746718845[161] = 0.0; - out_3441620345746718845[162] = 0.0; - out_3441620345746718845[163] = 0.0; - out_3441620345746718845[164] = 0.0; - out_3441620345746718845[165] = 0.0; - out_3441620345746718845[166] = 0.0; - out_3441620345746718845[167] = 0.0; - out_3441620345746718845[168] = 0.0; - out_3441620345746718845[169] = 0.0; - out_3441620345746718845[170] = 0.0; - out_3441620345746718845[171] = 1.0; - out_3441620345746718845[172] = 0.0; - out_3441620345746718845[173] = 0.0; - out_3441620345746718845[174] = 0.0; - out_3441620345746718845[175] = 0.0; - out_3441620345746718845[176] = 0.0; - out_3441620345746718845[177] = 0.0; - out_3441620345746718845[178] = 0.0; - out_3441620345746718845[179] = 0.0; - out_3441620345746718845[180] = 0.0; - out_3441620345746718845[181] = 0.0; - out_3441620345746718845[182] = 0.0; - out_3441620345746718845[183] = 0.0; - out_3441620345746718845[184] = 0.0; - out_3441620345746718845[185] = 0.0; - out_3441620345746718845[186] = 0.0; - out_3441620345746718845[187] = 0.0; - out_3441620345746718845[188] = 0.0; - out_3441620345746718845[189] = 0.0; - out_3441620345746718845[190] = 1.0; - out_3441620345746718845[191] = 0.0; - out_3441620345746718845[192] = 0.0; - out_3441620345746718845[193] = 0.0; - out_3441620345746718845[194] = 0.0; - out_3441620345746718845[195] = 0.0; - out_3441620345746718845[196] = 0.0; - out_3441620345746718845[197] = 0.0; - out_3441620345746718845[198] = 0.0; - out_3441620345746718845[199] = 0.0; - out_3441620345746718845[200] = 0.0; - out_3441620345746718845[201] = 0.0; - out_3441620345746718845[202] = 0.0; - out_3441620345746718845[203] = 0.0; - out_3441620345746718845[204] = 0.0; - out_3441620345746718845[205] = 0.0; - out_3441620345746718845[206] = 0.0; - out_3441620345746718845[207] = 0.0; - out_3441620345746718845[208] = 0.0; - out_3441620345746718845[209] = 1.0; - out_3441620345746718845[210] = 0.0; - out_3441620345746718845[211] = 0.0; - out_3441620345746718845[212] = 0.0; - out_3441620345746718845[213] = 0.0; - out_3441620345746718845[214] = 0.0; - out_3441620345746718845[215] = 0.0; - out_3441620345746718845[216] = 0.0; - out_3441620345746718845[217] = 0.0; - out_3441620345746718845[218] = 0.0; - out_3441620345746718845[219] = 0.0; - out_3441620345746718845[220] = 0.0; - out_3441620345746718845[221] = 0.0; - out_3441620345746718845[222] = 0.0; - out_3441620345746718845[223] = 0.0; - out_3441620345746718845[224] = 0.0; - out_3441620345746718845[225] = 0.0; - out_3441620345746718845[226] = 0.0; - out_3441620345746718845[227] = 0.0; - out_3441620345746718845[228] = 1.0; - out_3441620345746718845[229] = 0.0; - out_3441620345746718845[230] = 0.0; - out_3441620345746718845[231] = 0.0; - out_3441620345746718845[232] = 0.0; - out_3441620345746718845[233] = 0.0; - out_3441620345746718845[234] = 0.0; - out_3441620345746718845[235] = 0.0; - out_3441620345746718845[236] = 0.0; - out_3441620345746718845[237] = 0.0; - out_3441620345746718845[238] = 0.0; - out_3441620345746718845[239] = 0.0; - out_3441620345746718845[240] = 0.0; - out_3441620345746718845[241] = 0.0; - out_3441620345746718845[242] = 0.0; - out_3441620345746718845[243] = 0.0; - out_3441620345746718845[244] = 0.0; - out_3441620345746718845[245] = 0.0; - out_3441620345746718845[246] = 0.0; - out_3441620345746718845[247] = 1.0; - out_3441620345746718845[248] = 0.0; - out_3441620345746718845[249] = 0.0; - out_3441620345746718845[250] = 0.0; - out_3441620345746718845[251] = 0.0; - out_3441620345746718845[252] = 0.0; - out_3441620345746718845[253] = 0.0; - out_3441620345746718845[254] = 0.0; - out_3441620345746718845[255] = 0.0; - out_3441620345746718845[256] = 0.0; - out_3441620345746718845[257] = 0.0; - out_3441620345746718845[258] = 0.0; - out_3441620345746718845[259] = 0.0; - out_3441620345746718845[260] = 0.0; - out_3441620345746718845[261] = 0.0; - out_3441620345746718845[262] = 0.0; - out_3441620345746718845[263] = 0.0; - out_3441620345746718845[264] = 0.0; - out_3441620345746718845[265] = 0.0; - out_3441620345746718845[266] = 1.0; - out_3441620345746718845[267] = 0.0; - out_3441620345746718845[268] = 0.0; - out_3441620345746718845[269] = 0.0; - out_3441620345746718845[270] = 0.0; - out_3441620345746718845[271] = 0.0; - out_3441620345746718845[272] = 0.0; - out_3441620345746718845[273] = 0.0; - out_3441620345746718845[274] = 0.0; - out_3441620345746718845[275] = 0.0; - out_3441620345746718845[276] = 0.0; - out_3441620345746718845[277] = 0.0; - out_3441620345746718845[278] = 0.0; - out_3441620345746718845[279] = 0.0; - out_3441620345746718845[280] = 0.0; - out_3441620345746718845[281] = 0.0; - out_3441620345746718845[282] = 0.0; - out_3441620345746718845[283] = 0.0; - out_3441620345746718845[284] = 0.0; - out_3441620345746718845[285] = 1.0; - out_3441620345746718845[286] = 0.0; - out_3441620345746718845[287] = 0.0; - out_3441620345746718845[288] = 0.0; - out_3441620345746718845[289] = 0.0; - out_3441620345746718845[290] = 0.0; - out_3441620345746718845[291] = 0.0; - out_3441620345746718845[292] = 0.0; - out_3441620345746718845[293] = 0.0; - out_3441620345746718845[294] = 0.0; - out_3441620345746718845[295] = 0.0; - out_3441620345746718845[296] = 0.0; - out_3441620345746718845[297] = 0.0; - out_3441620345746718845[298] = 0.0; - out_3441620345746718845[299] = 0.0; - out_3441620345746718845[300] = 0.0; - out_3441620345746718845[301] = 0.0; - out_3441620345746718845[302] = 0.0; - out_3441620345746718845[303] = 0.0; - out_3441620345746718845[304] = 1.0; - out_3441620345746718845[305] = 0.0; - out_3441620345746718845[306] = 0.0; - out_3441620345746718845[307] = 0.0; - out_3441620345746718845[308] = 0.0; - out_3441620345746718845[309] = 0.0; - out_3441620345746718845[310] = 0.0; - out_3441620345746718845[311] = 0.0; - out_3441620345746718845[312] = 0.0; - out_3441620345746718845[313] = 0.0; - out_3441620345746718845[314] = 0.0; - out_3441620345746718845[315] = 0.0; - out_3441620345746718845[316] = 0.0; - out_3441620345746718845[317] = 0.0; - out_3441620345746718845[318] = 0.0; - out_3441620345746718845[319] = 0.0; - out_3441620345746718845[320] = 0.0; - out_3441620345746718845[321] = 0.0; - out_3441620345746718845[322] = 0.0; - out_3441620345746718845[323] = 1.0; +void H_mod_fun(double *state, double *out_4617418789543565628) { + out_4617418789543565628[0] = 1.0; + out_4617418789543565628[1] = 0.0; + out_4617418789543565628[2] = 0.0; + out_4617418789543565628[3] = 0.0; + out_4617418789543565628[4] = 0.0; + out_4617418789543565628[5] = 0.0; + out_4617418789543565628[6] = 0.0; + out_4617418789543565628[7] = 0.0; + out_4617418789543565628[8] = 0.0; + out_4617418789543565628[9] = 0.0; + out_4617418789543565628[10] = 0.0; + out_4617418789543565628[11] = 0.0; + out_4617418789543565628[12] = 0.0; + out_4617418789543565628[13] = 0.0; + out_4617418789543565628[14] = 0.0; + out_4617418789543565628[15] = 0.0; + out_4617418789543565628[16] = 0.0; + out_4617418789543565628[17] = 0.0; + out_4617418789543565628[18] = 0.0; + out_4617418789543565628[19] = 1.0; + out_4617418789543565628[20] = 0.0; + out_4617418789543565628[21] = 0.0; + out_4617418789543565628[22] = 0.0; + out_4617418789543565628[23] = 0.0; + out_4617418789543565628[24] = 0.0; + out_4617418789543565628[25] = 0.0; + out_4617418789543565628[26] = 0.0; + out_4617418789543565628[27] = 0.0; + out_4617418789543565628[28] = 0.0; + out_4617418789543565628[29] = 0.0; + out_4617418789543565628[30] = 0.0; + out_4617418789543565628[31] = 0.0; + out_4617418789543565628[32] = 0.0; + out_4617418789543565628[33] = 0.0; + out_4617418789543565628[34] = 0.0; + out_4617418789543565628[35] = 0.0; + out_4617418789543565628[36] = 0.0; + out_4617418789543565628[37] = 0.0; + out_4617418789543565628[38] = 1.0; + out_4617418789543565628[39] = 0.0; + out_4617418789543565628[40] = 0.0; + out_4617418789543565628[41] = 0.0; + out_4617418789543565628[42] = 0.0; + out_4617418789543565628[43] = 0.0; + out_4617418789543565628[44] = 0.0; + out_4617418789543565628[45] = 0.0; + out_4617418789543565628[46] = 0.0; + out_4617418789543565628[47] = 0.0; + out_4617418789543565628[48] = 0.0; + out_4617418789543565628[49] = 0.0; + out_4617418789543565628[50] = 0.0; + out_4617418789543565628[51] = 0.0; + out_4617418789543565628[52] = 0.0; + out_4617418789543565628[53] = 0.0; + out_4617418789543565628[54] = 0.0; + out_4617418789543565628[55] = 0.0; + out_4617418789543565628[56] = 0.0; + out_4617418789543565628[57] = 1.0; + out_4617418789543565628[58] = 0.0; + out_4617418789543565628[59] = 0.0; + out_4617418789543565628[60] = 0.0; + out_4617418789543565628[61] = 0.0; + out_4617418789543565628[62] = 0.0; + out_4617418789543565628[63] = 0.0; + out_4617418789543565628[64] = 0.0; + out_4617418789543565628[65] = 0.0; + out_4617418789543565628[66] = 0.0; + out_4617418789543565628[67] = 0.0; + out_4617418789543565628[68] = 0.0; + out_4617418789543565628[69] = 0.0; + out_4617418789543565628[70] = 0.0; + out_4617418789543565628[71] = 0.0; + out_4617418789543565628[72] = 0.0; + out_4617418789543565628[73] = 0.0; + out_4617418789543565628[74] = 0.0; + out_4617418789543565628[75] = 0.0; + out_4617418789543565628[76] = 1.0; + out_4617418789543565628[77] = 0.0; + out_4617418789543565628[78] = 0.0; + out_4617418789543565628[79] = 0.0; + out_4617418789543565628[80] = 0.0; + out_4617418789543565628[81] = 0.0; + out_4617418789543565628[82] = 0.0; + out_4617418789543565628[83] = 0.0; + out_4617418789543565628[84] = 0.0; + out_4617418789543565628[85] = 0.0; + out_4617418789543565628[86] = 0.0; + out_4617418789543565628[87] = 0.0; + out_4617418789543565628[88] = 0.0; + out_4617418789543565628[89] = 0.0; + out_4617418789543565628[90] = 0.0; + out_4617418789543565628[91] = 0.0; + out_4617418789543565628[92] = 0.0; + out_4617418789543565628[93] = 0.0; + out_4617418789543565628[94] = 0.0; + out_4617418789543565628[95] = 1.0; + out_4617418789543565628[96] = 0.0; + out_4617418789543565628[97] = 0.0; + out_4617418789543565628[98] = 0.0; + out_4617418789543565628[99] = 0.0; + out_4617418789543565628[100] = 0.0; + out_4617418789543565628[101] = 0.0; + out_4617418789543565628[102] = 0.0; + out_4617418789543565628[103] = 0.0; + out_4617418789543565628[104] = 0.0; + out_4617418789543565628[105] = 0.0; + out_4617418789543565628[106] = 0.0; + out_4617418789543565628[107] = 0.0; + out_4617418789543565628[108] = 0.0; + out_4617418789543565628[109] = 0.0; + out_4617418789543565628[110] = 0.0; + out_4617418789543565628[111] = 0.0; + out_4617418789543565628[112] = 0.0; + out_4617418789543565628[113] = 0.0; + out_4617418789543565628[114] = 1.0; + out_4617418789543565628[115] = 0.0; + out_4617418789543565628[116] = 0.0; + out_4617418789543565628[117] = 0.0; + out_4617418789543565628[118] = 0.0; + out_4617418789543565628[119] = 0.0; + out_4617418789543565628[120] = 0.0; + out_4617418789543565628[121] = 0.0; + out_4617418789543565628[122] = 0.0; + out_4617418789543565628[123] = 0.0; + out_4617418789543565628[124] = 0.0; + out_4617418789543565628[125] = 0.0; + out_4617418789543565628[126] = 0.0; + out_4617418789543565628[127] = 0.0; + out_4617418789543565628[128] = 0.0; + out_4617418789543565628[129] = 0.0; + out_4617418789543565628[130] = 0.0; + out_4617418789543565628[131] = 0.0; + out_4617418789543565628[132] = 0.0; + out_4617418789543565628[133] = 1.0; + out_4617418789543565628[134] = 0.0; + out_4617418789543565628[135] = 0.0; + out_4617418789543565628[136] = 0.0; + out_4617418789543565628[137] = 0.0; + out_4617418789543565628[138] = 0.0; + out_4617418789543565628[139] = 0.0; + out_4617418789543565628[140] = 0.0; + out_4617418789543565628[141] = 0.0; + out_4617418789543565628[142] = 0.0; + out_4617418789543565628[143] = 0.0; + out_4617418789543565628[144] = 0.0; + out_4617418789543565628[145] = 0.0; + out_4617418789543565628[146] = 0.0; + out_4617418789543565628[147] = 0.0; + out_4617418789543565628[148] = 0.0; + out_4617418789543565628[149] = 0.0; + out_4617418789543565628[150] = 0.0; + out_4617418789543565628[151] = 0.0; + out_4617418789543565628[152] = 1.0; + out_4617418789543565628[153] = 0.0; + out_4617418789543565628[154] = 0.0; + out_4617418789543565628[155] = 0.0; + out_4617418789543565628[156] = 0.0; + out_4617418789543565628[157] = 0.0; + out_4617418789543565628[158] = 0.0; + out_4617418789543565628[159] = 0.0; + out_4617418789543565628[160] = 0.0; + out_4617418789543565628[161] = 0.0; + out_4617418789543565628[162] = 0.0; + out_4617418789543565628[163] = 0.0; + out_4617418789543565628[164] = 0.0; + out_4617418789543565628[165] = 0.0; + out_4617418789543565628[166] = 0.0; + out_4617418789543565628[167] = 0.0; + out_4617418789543565628[168] = 0.0; + out_4617418789543565628[169] = 0.0; + out_4617418789543565628[170] = 0.0; + out_4617418789543565628[171] = 1.0; + out_4617418789543565628[172] = 0.0; + out_4617418789543565628[173] = 0.0; + out_4617418789543565628[174] = 0.0; + out_4617418789543565628[175] = 0.0; + out_4617418789543565628[176] = 0.0; + out_4617418789543565628[177] = 0.0; + out_4617418789543565628[178] = 0.0; + out_4617418789543565628[179] = 0.0; + out_4617418789543565628[180] = 0.0; + out_4617418789543565628[181] = 0.0; + out_4617418789543565628[182] = 0.0; + out_4617418789543565628[183] = 0.0; + out_4617418789543565628[184] = 0.0; + out_4617418789543565628[185] = 0.0; + out_4617418789543565628[186] = 0.0; + out_4617418789543565628[187] = 0.0; + out_4617418789543565628[188] = 0.0; + out_4617418789543565628[189] = 0.0; + out_4617418789543565628[190] = 1.0; + out_4617418789543565628[191] = 0.0; + out_4617418789543565628[192] = 0.0; + out_4617418789543565628[193] = 0.0; + out_4617418789543565628[194] = 0.0; + out_4617418789543565628[195] = 0.0; + out_4617418789543565628[196] = 0.0; + out_4617418789543565628[197] = 0.0; + out_4617418789543565628[198] = 0.0; + out_4617418789543565628[199] = 0.0; + out_4617418789543565628[200] = 0.0; + out_4617418789543565628[201] = 0.0; + out_4617418789543565628[202] = 0.0; + out_4617418789543565628[203] = 0.0; + out_4617418789543565628[204] = 0.0; + out_4617418789543565628[205] = 0.0; + out_4617418789543565628[206] = 0.0; + out_4617418789543565628[207] = 0.0; + out_4617418789543565628[208] = 0.0; + out_4617418789543565628[209] = 1.0; + out_4617418789543565628[210] = 0.0; + out_4617418789543565628[211] = 0.0; + out_4617418789543565628[212] = 0.0; + out_4617418789543565628[213] = 0.0; + out_4617418789543565628[214] = 0.0; + out_4617418789543565628[215] = 0.0; + out_4617418789543565628[216] = 0.0; + out_4617418789543565628[217] = 0.0; + out_4617418789543565628[218] = 0.0; + out_4617418789543565628[219] = 0.0; + out_4617418789543565628[220] = 0.0; + out_4617418789543565628[221] = 0.0; + out_4617418789543565628[222] = 0.0; + out_4617418789543565628[223] = 0.0; + out_4617418789543565628[224] = 0.0; + out_4617418789543565628[225] = 0.0; + out_4617418789543565628[226] = 0.0; + out_4617418789543565628[227] = 0.0; + out_4617418789543565628[228] = 1.0; + out_4617418789543565628[229] = 0.0; + out_4617418789543565628[230] = 0.0; + out_4617418789543565628[231] = 0.0; + out_4617418789543565628[232] = 0.0; + out_4617418789543565628[233] = 0.0; + out_4617418789543565628[234] = 0.0; + out_4617418789543565628[235] = 0.0; + out_4617418789543565628[236] = 0.0; + out_4617418789543565628[237] = 0.0; + out_4617418789543565628[238] = 0.0; + out_4617418789543565628[239] = 0.0; + out_4617418789543565628[240] = 0.0; + out_4617418789543565628[241] = 0.0; + out_4617418789543565628[242] = 0.0; + out_4617418789543565628[243] = 0.0; + out_4617418789543565628[244] = 0.0; + out_4617418789543565628[245] = 0.0; + out_4617418789543565628[246] = 0.0; + out_4617418789543565628[247] = 1.0; + out_4617418789543565628[248] = 0.0; + out_4617418789543565628[249] = 0.0; + out_4617418789543565628[250] = 0.0; + out_4617418789543565628[251] = 0.0; + out_4617418789543565628[252] = 0.0; + out_4617418789543565628[253] = 0.0; + out_4617418789543565628[254] = 0.0; + out_4617418789543565628[255] = 0.0; + out_4617418789543565628[256] = 0.0; + out_4617418789543565628[257] = 0.0; + out_4617418789543565628[258] = 0.0; + out_4617418789543565628[259] = 0.0; + out_4617418789543565628[260] = 0.0; + out_4617418789543565628[261] = 0.0; + out_4617418789543565628[262] = 0.0; + out_4617418789543565628[263] = 0.0; + out_4617418789543565628[264] = 0.0; + out_4617418789543565628[265] = 0.0; + out_4617418789543565628[266] = 1.0; + out_4617418789543565628[267] = 0.0; + out_4617418789543565628[268] = 0.0; + out_4617418789543565628[269] = 0.0; + out_4617418789543565628[270] = 0.0; + out_4617418789543565628[271] = 0.0; + out_4617418789543565628[272] = 0.0; + out_4617418789543565628[273] = 0.0; + out_4617418789543565628[274] = 0.0; + out_4617418789543565628[275] = 0.0; + out_4617418789543565628[276] = 0.0; + out_4617418789543565628[277] = 0.0; + out_4617418789543565628[278] = 0.0; + out_4617418789543565628[279] = 0.0; + out_4617418789543565628[280] = 0.0; + out_4617418789543565628[281] = 0.0; + out_4617418789543565628[282] = 0.0; + out_4617418789543565628[283] = 0.0; + out_4617418789543565628[284] = 0.0; + out_4617418789543565628[285] = 1.0; + out_4617418789543565628[286] = 0.0; + out_4617418789543565628[287] = 0.0; + out_4617418789543565628[288] = 0.0; + out_4617418789543565628[289] = 0.0; + out_4617418789543565628[290] = 0.0; + out_4617418789543565628[291] = 0.0; + out_4617418789543565628[292] = 0.0; + out_4617418789543565628[293] = 0.0; + out_4617418789543565628[294] = 0.0; + out_4617418789543565628[295] = 0.0; + out_4617418789543565628[296] = 0.0; + out_4617418789543565628[297] = 0.0; + out_4617418789543565628[298] = 0.0; + out_4617418789543565628[299] = 0.0; + out_4617418789543565628[300] = 0.0; + out_4617418789543565628[301] = 0.0; + out_4617418789543565628[302] = 0.0; + out_4617418789543565628[303] = 0.0; + out_4617418789543565628[304] = 1.0; + out_4617418789543565628[305] = 0.0; + out_4617418789543565628[306] = 0.0; + out_4617418789543565628[307] = 0.0; + out_4617418789543565628[308] = 0.0; + out_4617418789543565628[309] = 0.0; + out_4617418789543565628[310] = 0.0; + out_4617418789543565628[311] = 0.0; + out_4617418789543565628[312] = 0.0; + out_4617418789543565628[313] = 0.0; + out_4617418789543565628[314] = 0.0; + out_4617418789543565628[315] = 0.0; + out_4617418789543565628[316] = 0.0; + out_4617418789543565628[317] = 0.0; + out_4617418789543565628[318] = 0.0; + out_4617418789543565628[319] = 0.0; + out_4617418789543565628[320] = 0.0; + out_4617418789543565628[321] = 0.0; + out_4617418789543565628[322] = 0.0; + out_4617418789543565628[323] = 1.0; } -void f_fun(double *state, double dt, double *out_5548337699276970526) { - out_5548337699276970526[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_5548337699276970526[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_5548337699276970526[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_5548337699276970526[3] = dt*state[12] + state[3]; - out_5548337699276970526[4] = dt*state[13] + state[4]; - out_5548337699276970526[5] = dt*state[14] + state[5]; - out_5548337699276970526[6] = state[6]; - out_5548337699276970526[7] = state[7]; - out_5548337699276970526[8] = state[8]; - out_5548337699276970526[9] = state[9]; - out_5548337699276970526[10] = state[10]; - out_5548337699276970526[11] = state[11]; - out_5548337699276970526[12] = state[12]; - out_5548337699276970526[13] = state[13]; - out_5548337699276970526[14] = state[14]; - out_5548337699276970526[15] = state[15]; - out_5548337699276970526[16] = state[16]; - out_5548337699276970526[17] = state[17]; +void f_fun(double *state, double dt, double *out_9094503671864348345) { + out_9094503671864348345[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_9094503671864348345[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_9094503671864348345[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_9094503671864348345[3] = dt*state[12] + state[3]; + out_9094503671864348345[4] = dt*state[13] + state[4]; + out_9094503671864348345[5] = dt*state[14] + state[5]; + out_9094503671864348345[6] = state[6]; + out_9094503671864348345[7] = state[7]; + out_9094503671864348345[8] = state[8]; + out_9094503671864348345[9] = state[9]; + out_9094503671864348345[10] = state[10]; + out_9094503671864348345[11] = state[11]; + out_9094503671864348345[12] = state[12]; + out_9094503671864348345[13] = state[13]; + out_9094503671864348345[14] = state[14]; + out_9094503671864348345[15] = state[15]; + out_9094503671864348345[16] = state[16]; + out_9094503671864348345[17] = state[17]; } -void F_fun(double *state, double dt, double *out_6716874081947285130) { - out_6716874081947285130[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_6716874081947285130[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_6716874081947285130[2] = 0; - out_6716874081947285130[3] = 0; - out_6716874081947285130[4] = 0; - out_6716874081947285130[5] = 0; - out_6716874081947285130[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_6716874081947285130[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_6716874081947285130[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_6716874081947285130[9] = 0; - out_6716874081947285130[10] = 0; - out_6716874081947285130[11] = 0; - out_6716874081947285130[12] = 0; - out_6716874081947285130[13] = 0; - out_6716874081947285130[14] = 0; - out_6716874081947285130[15] = 0; - out_6716874081947285130[16] = 0; - out_6716874081947285130[17] = 0; - out_6716874081947285130[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_6716874081947285130[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_6716874081947285130[20] = 0; - out_6716874081947285130[21] = 0; - out_6716874081947285130[22] = 0; - out_6716874081947285130[23] = 0; - out_6716874081947285130[24] = 0; - out_6716874081947285130[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_6716874081947285130[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_6716874081947285130[27] = 0; - out_6716874081947285130[28] = 0; - out_6716874081947285130[29] = 0; - out_6716874081947285130[30] = 0; - out_6716874081947285130[31] = 0; - out_6716874081947285130[32] = 0; - out_6716874081947285130[33] = 0; - out_6716874081947285130[34] = 0; - out_6716874081947285130[35] = 0; - out_6716874081947285130[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_6716874081947285130[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_6716874081947285130[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_6716874081947285130[39] = 0; - out_6716874081947285130[40] = 0; - out_6716874081947285130[41] = 0; - out_6716874081947285130[42] = 0; - out_6716874081947285130[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_6716874081947285130[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_6716874081947285130[45] = 0; - out_6716874081947285130[46] = 0; - out_6716874081947285130[47] = 0; - out_6716874081947285130[48] = 0; - out_6716874081947285130[49] = 0; - out_6716874081947285130[50] = 0; - out_6716874081947285130[51] = 0; - out_6716874081947285130[52] = 0; - out_6716874081947285130[53] = 0; - out_6716874081947285130[54] = 0; - out_6716874081947285130[55] = 0; - out_6716874081947285130[56] = 0; - out_6716874081947285130[57] = 1; - out_6716874081947285130[58] = 0; - out_6716874081947285130[59] = 0; - out_6716874081947285130[60] = 0; - out_6716874081947285130[61] = 0; - out_6716874081947285130[62] = 0; - out_6716874081947285130[63] = 0; - out_6716874081947285130[64] = 0; - out_6716874081947285130[65] = 0; - out_6716874081947285130[66] = dt; - out_6716874081947285130[67] = 0; - out_6716874081947285130[68] = 0; - out_6716874081947285130[69] = 0; - out_6716874081947285130[70] = 0; - out_6716874081947285130[71] = 0; - out_6716874081947285130[72] = 0; - out_6716874081947285130[73] = 0; - out_6716874081947285130[74] = 0; - out_6716874081947285130[75] = 0; - out_6716874081947285130[76] = 1; - out_6716874081947285130[77] = 0; - out_6716874081947285130[78] = 0; - out_6716874081947285130[79] = 0; - out_6716874081947285130[80] = 0; - out_6716874081947285130[81] = 0; - out_6716874081947285130[82] = 0; - out_6716874081947285130[83] = 0; - out_6716874081947285130[84] = 0; - out_6716874081947285130[85] = dt; - out_6716874081947285130[86] = 0; - out_6716874081947285130[87] = 0; - out_6716874081947285130[88] = 0; - out_6716874081947285130[89] = 0; - out_6716874081947285130[90] = 0; - out_6716874081947285130[91] = 0; - out_6716874081947285130[92] = 0; - out_6716874081947285130[93] = 0; - out_6716874081947285130[94] = 0; - out_6716874081947285130[95] = 1; - out_6716874081947285130[96] = 0; - out_6716874081947285130[97] = 0; - out_6716874081947285130[98] = 0; - out_6716874081947285130[99] = 0; - out_6716874081947285130[100] = 0; - out_6716874081947285130[101] = 0; - out_6716874081947285130[102] = 0; - out_6716874081947285130[103] = 0; - out_6716874081947285130[104] = dt; - out_6716874081947285130[105] = 0; - out_6716874081947285130[106] = 0; - out_6716874081947285130[107] = 0; - out_6716874081947285130[108] = 0; - out_6716874081947285130[109] = 0; - out_6716874081947285130[110] = 0; - out_6716874081947285130[111] = 0; - out_6716874081947285130[112] = 0; - out_6716874081947285130[113] = 0; - out_6716874081947285130[114] = 1; - out_6716874081947285130[115] = 0; - out_6716874081947285130[116] = 0; - out_6716874081947285130[117] = 0; - out_6716874081947285130[118] = 0; - out_6716874081947285130[119] = 0; - out_6716874081947285130[120] = 0; - out_6716874081947285130[121] = 0; - out_6716874081947285130[122] = 0; - out_6716874081947285130[123] = 0; - out_6716874081947285130[124] = 0; - out_6716874081947285130[125] = 0; - out_6716874081947285130[126] = 0; - out_6716874081947285130[127] = 0; - out_6716874081947285130[128] = 0; - out_6716874081947285130[129] = 0; - out_6716874081947285130[130] = 0; - out_6716874081947285130[131] = 0; - out_6716874081947285130[132] = 0; - out_6716874081947285130[133] = 1; - out_6716874081947285130[134] = 0; - out_6716874081947285130[135] = 0; - out_6716874081947285130[136] = 0; - out_6716874081947285130[137] = 0; - out_6716874081947285130[138] = 0; - out_6716874081947285130[139] = 0; - out_6716874081947285130[140] = 0; - out_6716874081947285130[141] = 0; - out_6716874081947285130[142] = 0; - out_6716874081947285130[143] = 0; - out_6716874081947285130[144] = 0; - out_6716874081947285130[145] = 0; - out_6716874081947285130[146] = 0; - out_6716874081947285130[147] = 0; - out_6716874081947285130[148] = 0; - out_6716874081947285130[149] = 0; - out_6716874081947285130[150] = 0; - out_6716874081947285130[151] = 0; - out_6716874081947285130[152] = 1; - out_6716874081947285130[153] = 0; - out_6716874081947285130[154] = 0; - out_6716874081947285130[155] = 0; - out_6716874081947285130[156] = 0; - out_6716874081947285130[157] = 0; - out_6716874081947285130[158] = 0; - out_6716874081947285130[159] = 0; - out_6716874081947285130[160] = 0; - out_6716874081947285130[161] = 0; - out_6716874081947285130[162] = 0; - out_6716874081947285130[163] = 0; - out_6716874081947285130[164] = 0; - out_6716874081947285130[165] = 0; - out_6716874081947285130[166] = 0; - out_6716874081947285130[167] = 0; - out_6716874081947285130[168] = 0; - out_6716874081947285130[169] = 0; - out_6716874081947285130[170] = 0; - out_6716874081947285130[171] = 1; - out_6716874081947285130[172] = 0; - out_6716874081947285130[173] = 0; - out_6716874081947285130[174] = 0; - out_6716874081947285130[175] = 0; - out_6716874081947285130[176] = 0; - out_6716874081947285130[177] = 0; - out_6716874081947285130[178] = 0; - out_6716874081947285130[179] = 0; - out_6716874081947285130[180] = 0; - out_6716874081947285130[181] = 0; - out_6716874081947285130[182] = 0; - out_6716874081947285130[183] = 0; - out_6716874081947285130[184] = 0; - out_6716874081947285130[185] = 0; - out_6716874081947285130[186] = 0; - out_6716874081947285130[187] = 0; - out_6716874081947285130[188] = 0; - out_6716874081947285130[189] = 0; - out_6716874081947285130[190] = 1; - out_6716874081947285130[191] = 0; - out_6716874081947285130[192] = 0; - out_6716874081947285130[193] = 0; - out_6716874081947285130[194] = 0; - out_6716874081947285130[195] = 0; - out_6716874081947285130[196] = 0; - out_6716874081947285130[197] = 0; - out_6716874081947285130[198] = 0; - out_6716874081947285130[199] = 0; - out_6716874081947285130[200] = 0; - out_6716874081947285130[201] = 0; - out_6716874081947285130[202] = 0; - out_6716874081947285130[203] = 0; - out_6716874081947285130[204] = 0; - out_6716874081947285130[205] = 0; - out_6716874081947285130[206] = 0; - out_6716874081947285130[207] = 0; - out_6716874081947285130[208] = 0; - out_6716874081947285130[209] = 1; - out_6716874081947285130[210] = 0; - out_6716874081947285130[211] = 0; - out_6716874081947285130[212] = 0; - out_6716874081947285130[213] = 0; - out_6716874081947285130[214] = 0; - out_6716874081947285130[215] = 0; - out_6716874081947285130[216] = 0; - out_6716874081947285130[217] = 0; - out_6716874081947285130[218] = 0; - out_6716874081947285130[219] = 0; - out_6716874081947285130[220] = 0; - out_6716874081947285130[221] = 0; - out_6716874081947285130[222] = 0; - out_6716874081947285130[223] = 0; - out_6716874081947285130[224] = 0; - out_6716874081947285130[225] = 0; - out_6716874081947285130[226] = 0; - out_6716874081947285130[227] = 0; - out_6716874081947285130[228] = 1; - out_6716874081947285130[229] = 0; - out_6716874081947285130[230] = 0; - out_6716874081947285130[231] = 0; - out_6716874081947285130[232] = 0; - out_6716874081947285130[233] = 0; - out_6716874081947285130[234] = 0; - out_6716874081947285130[235] = 0; - out_6716874081947285130[236] = 0; - out_6716874081947285130[237] = 0; - out_6716874081947285130[238] = 0; - out_6716874081947285130[239] = 0; - out_6716874081947285130[240] = 0; - out_6716874081947285130[241] = 0; - out_6716874081947285130[242] = 0; - out_6716874081947285130[243] = 0; - out_6716874081947285130[244] = 0; - out_6716874081947285130[245] = 0; - out_6716874081947285130[246] = 0; - out_6716874081947285130[247] = 1; - out_6716874081947285130[248] = 0; - out_6716874081947285130[249] = 0; - out_6716874081947285130[250] = 0; - out_6716874081947285130[251] = 0; - out_6716874081947285130[252] = 0; - out_6716874081947285130[253] = 0; - out_6716874081947285130[254] = 0; - out_6716874081947285130[255] = 0; - out_6716874081947285130[256] = 0; - out_6716874081947285130[257] = 0; - out_6716874081947285130[258] = 0; - out_6716874081947285130[259] = 0; - out_6716874081947285130[260] = 0; - out_6716874081947285130[261] = 0; - out_6716874081947285130[262] = 0; - out_6716874081947285130[263] = 0; - out_6716874081947285130[264] = 0; - out_6716874081947285130[265] = 0; - out_6716874081947285130[266] = 1; - out_6716874081947285130[267] = 0; - out_6716874081947285130[268] = 0; - out_6716874081947285130[269] = 0; - out_6716874081947285130[270] = 0; - out_6716874081947285130[271] = 0; - out_6716874081947285130[272] = 0; - out_6716874081947285130[273] = 0; - out_6716874081947285130[274] = 0; - out_6716874081947285130[275] = 0; - out_6716874081947285130[276] = 0; - out_6716874081947285130[277] = 0; - out_6716874081947285130[278] = 0; - out_6716874081947285130[279] = 0; - out_6716874081947285130[280] = 0; - out_6716874081947285130[281] = 0; - out_6716874081947285130[282] = 0; - out_6716874081947285130[283] = 0; - out_6716874081947285130[284] = 0; - out_6716874081947285130[285] = 1; - out_6716874081947285130[286] = 0; - out_6716874081947285130[287] = 0; - out_6716874081947285130[288] = 0; - out_6716874081947285130[289] = 0; - out_6716874081947285130[290] = 0; - out_6716874081947285130[291] = 0; - out_6716874081947285130[292] = 0; - out_6716874081947285130[293] = 0; - out_6716874081947285130[294] = 0; - out_6716874081947285130[295] = 0; - out_6716874081947285130[296] = 0; - out_6716874081947285130[297] = 0; - out_6716874081947285130[298] = 0; - out_6716874081947285130[299] = 0; - out_6716874081947285130[300] = 0; - out_6716874081947285130[301] = 0; - out_6716874081947285130[302] = 0; - out_6716874081947285130[303] = 0; - out_6716874081947285130[304] = 1; - out_6716874081947285130[305] = 0; - out_6716874081947285130[306] = 0; - out_6716874081947285130[307] = 0; - out_6716874081947285130[308] = 0; - out_6716874081947285130[309] = 0; - out_6716874081947285130[310] = 0; - out_6716874081947285130[311] = 0; - out_6716874081947285130[312] = 0; - out_6716874081947285130[313] = 0; - out_6716874081947285130[314] = 0; - out_6716874081947285130[315] = 0; - out_6716874081947285130[316] = 0; - out_6716874081947285130[317] = 0; - out_6716874081947285130[318] = 0; - out_6716874081947285130[319] = 0; - out_6716874081947285130[320] = 0; - out_6716874081947285130[321] = 0; - out_6716874081947285130[322] = 0; - out_6716874081947285130[323] = 1; +void F_fun(double *state, double dt, double *out_1312959600226932432) { + out_1312959600226932432[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_1312959600226932432[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_1312959600226932432[2] = 0; + out_1312959600226932432[3] = 0; + out_1312959600226932432[4] = 0; + out_1312959600226932432[5] = 0; + out_1312959600226932432[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_1312959600226932432[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_1312959600226932432[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_1312959600226932432[9] = 0; + out_1312959600226932432[10] = 0; + out_1312959600226932432[11] = 0; + out_1312959600226932432[12] = 0; + out_1312959600226932432[13] = 0; + out_1312959600226932432[14] = 0; + out_1312959600226932432[15] = 0; + out_1312959600226932432[16] = 0; + out_1312959600226932432[17] = 0; + out_1312959600226932432[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_1312959600226932432[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_1312959600226932432[20] = 0; + out_1312959600226932432[21] = 0; + out_1312959600226932432[22] = 0; + out_1312959600226932432[23] = 0; + out_1312959600226932432[24] = 0; + out_1312959600226932432[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_1312959600226932432[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_1312959600226932432[27] = 0; + out_1312959600226932432[28] = 0; + out_1312959600226932432[29] = 0; + out_1312959600226932432[30] = 0; + out_1312959600226932432[31] = 0; + out_1312959600226932432[32] = 0; + out_1312959600226932432[33] = 0; + out_1312959600226932432[34] = 0; + out_1312959600226932432[35] = 0; + out_1312959600226932432[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_1312959600226932432[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_1312959600226932432[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_1312959600226932432[39] = 0; + out_1312959600226932432[40] = 0; + out_1312959600226932432[41] = 0; + out_1312959600226932432[42] = 0; + out_1312959600226932432[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_1312959600226932432[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_1312959600226932432[45] = 0; + out_1312959600226932432[46] = 0; + out_1312959600226932432[47] = 0; + out_1312959600226932432[48] = 0; + out_1312959600226932432[49] = 0; + out_1312959600226932432[50] = 0; + out_1312959600226932432[51] = 0; + out_1312959600226932432[52] = 0; + out_1312959600226932432[53] = 0; + out_1312959600226932432[54] = 0; + out_1312959600226932432[55] = 0; + out_1312959600226932432[56] = 0; + out_1312959600226932432[57] = 1; + out_1312959600226932432[58] = 0; + out_1312959600226932432[59] = 0; + out_1312959600226932432[60] = 0; + out_1312959600226932432[61] = 0; + out_1312959600226932432[62] = 0; + out_1312959600226932432[63] = 0; + out_1312959600226932432[64] = 0; + out_1312959600226932432[65] = 0; + out_1312959600226932432[66] = dt; + out_1312959600226932432[67] = 0; + out_1312959600226932432[68] = 0; + out_1312959600226932432[69] = 0; + out_1312959600226932432[70] = 0; + out_1312959600226932432[71] = 0; + out_1312959600226932432[72] = 0; + out_1312959600226932432[73] = 0; + out_1312959600226932432[74] = 0; + out_1312959600226932432[75] = 0; + out_1312959600226932432[76] = 1; + out_1312959600226932432[77] = 0; + out_1312959600226932432[78] = 0; + out_1312959600226932432[79] = 0; + out_1312959600226932432[80] = 0; + out_1312959600226932432[81] = 0; + out_1312959600226932432[82] = 0; + out_1312959600226932432[83] = 0; + out_1312959600226932432[84] = 0; + out_1312959600226932432[85] = dt; + out_1312959600226932432[86] = 0; + out_1312959600226932432[87] = 0; + out_1312959600226932432[88] = 0; + out_1312959600226932432[89] = 0; + out_1312959600226932432[90] = 0; + out_1312959600226932432[91] = 0; + out_1312959600226932432[92] = 0; + out_1312959600226932432[93] = 0; + out_1312959600226932432[94] = 0; + out_1312959600226932432[95] = 1; + out_1312959600226932432[96] = 0; + out_1312959600226932432[97] = 0; + out_1312959600226932432[98] = 0; + out_1312959600226932432[99] = 0; + out_1312959600226932432[100] = 0; + out_1312959600226932432[101] = 0; + out_1312959600226932432[102] = 0; + out_1312959600226932432[103] = 0; + out_1312959600226932432[104] = dt; + out_1312959600226932432[105] = 0; + out_1312959600226932432[106] = 0; + out_1312959600226932432[107] = 0; + out_1312959600226932432[108] = 0; + out_1312959600226932432[109] = 0; + out_1312959600226932432[110] = 0; + out_1312959600226932432[111] = 0; + out_1312959600226932432[112] = 0; + out_1312959600226932432[113] = 0; + out_1312959600226932432[114] = 1; + out_1312959600226932432[115] = 0; + out_1312959600226932432[116] = 0; + out_1312959600226932432[117] = 0; + out_1312959600226932432[118] = 0; + out_1312959600226932432[119] = 0; + out_1312959600226932432[120] = 0; + out_1312959600226932432[121] = 0; + out_1312959600226932432[122] = 0; + out_1312959600226932432[123] = 0; + out_1312959600226932432[124] = 0; + out_1312959600226932432[125] = 0; + out_1312959600226932432[126] = 0; + out_1312959600226932432[127] = 0; + out_1312959600226932432[128] = 0; + out_1312959600226932432[129] = 0; + out_1312959600226932432[130] = 0; + out_1312959600226932432[131] = 0; + out_1312959600226932432[132] = 0; + out_1312959600226932432[133] = 1; + out_1312959600226932432[134] = 0; + out_1312959600226932432[135] = 0; + out_1312959600226932432[136] = 0; + out_1312959600226932432[137] = 0; + out_1312959600226932432[138] = 0; + out_1312959600226932432[139] = 0; + out_1312959600226932432[140] = 0; + out_1312959600226932432[141] = 0; + out_1312959600226932432[142] = 0; + out_1312959600226932432[143] = 0; + out_1312959600226932432[144] = 0; + out_1312959600226932432[145] = 0; + out_1312959600226932432[146] = 0; + out_1312959600226932432[147] = 0; + out_1312959600226932432[148] = 0; + out_1312959600226932432[149] = 0; + out_1312959600226932432[150] = 0; + out_1312959600226932432[151] = 0; + out_1312959600226932432[152] = 1; + out_1312959600226932432[153] = 0; + out_1312959600226932432[154] = 0; + out_1312959600226932432[155] = 0; + out_1312959600226932432[156] = 0; + out_1312959600226932432[157] = 0; + out_1312959600226932432[158] = 0; + out_1312959600226932432[159] = 0; + out_1312959600226932432[160] = 0; + out_1312959600226932432[161] = 0; + out_1312959600226932432[162] = 0; + out_1312959600226932432[163] = 0; + out_1312959600226932432[164] = 0; + out_1312959600226932432[165] = 0; + out_1312959600226932432[166] = 0; + out_1312959600226932432[167] = 0; + out_1312959600226932432[168] = 0; + out_1312959600226932432[169] = 0; + out_1312959600226932432[170] = 0; + out_1312959600226932432[171] = 1; + out_1312959600226932432[172] = 0; + out_1312959600226932432[173] = 0; + out_1312959600226932432[174] = 0; + out_1312959600226932432[175] = 0; + out_1312959600226932432[176] = 0; + out_1312959600226932432[177] = 0; + out_1312959600226932432[178] = 0; + out_1312959600226932432[179] = 0; + out_1312959600226932432[180] = 0; + out_1312959600226932432[181] = 0; + out_1312959600226932432[182] = 0; + out_1312959600226932432[183] = 0; + out_1312959600226932432[184] = 0; + out_1312959600226932432[185] = 0; + out_1312959600226932432[186] = 0; + out_1312959600226932432[187] = 0; + out_1312959600226932432[188] = 0; + out_1312959600226932432[189] = 0; + out_1312959600226932432[190] = 1; + out_1312959600226932432[191] = 0; + out_1312959600226932432[192] = 0; + out_1312959600226932432[193] = 0; + out_1312959600226932432[194] = 0; + out_1312959600226932432[195] = 0; + out_1312959600226932432[196] = 0; + out_1312959600226932432[197] = 0; + out_1312959600226932432[198] = 0; + out_1312959600226932432[199] = 0; + out_1312959600226932432[200] = 0; + out_1312959600226932432[201] = 0; + out_1312959600226932432[202] = 0; + out_1312959600226932432[203] = 0; + out_1312959600226932432[204] = 0; + out_1312959600226932432[205] = 0; + out_1312959600226932432[206] = 0; + out_1312959600226932432[207] = 0; + out_1312959600226932432[208] = 0; + out_1312959600226932432[209] = 1; + out_1312959600226932432[210] = 0; + out_1312959600226932432[211] = 0; + out_1312959600226932432[212] = 0; + out_1312959600226932432[213] = 0; + out_1312959600226932432[214] = 0; + out_1312959600226932432[215] = 0; + out_1312959600226932432[216] = 0; + out_1312959600226932432[217] = 0; + out_1312959600226932432[218] = 0; + out_1312959600226932432[219] = 0; + out_1312959600226932432[220] = 0; + out_1312959600226932432[221] = 0; + out_1312959600226932432[222] = 0; + out_1312959600226932432[223] = 0; + out_1312959600226932432[224] = 0; + out_1312959600226932432[225] = 0; + out_1312959600226932432[226] = 0; + out_1312959600226932432[227] = 0; + out_1312959600226932432[228] = 1; + out_1312959600226932432[229] = 0; + out_1312959600226932432[230] = 0; + out_1312959600226932432[231] = 0; + out_1312959600226932432[232] = 0; + out_1312959600226932432[233] = 0; + out_1312959600226932432[234] = 0; + out_1312959600226932432[235] = 0; + out_1312959600226932432[236] = 0; + out_1312959600226932432[237] = 0; + out_1312959600226932432[238] = 0; + out_1312959600226932432[239] = 0; + out_1312959600226932432[240] = 0; + out_1312959600226932432[241] = 0; + out_1312959600226932432[242] = 0; + out_1312959600226932432[243] = 0; + out_1312959600226932432[244] = 0; + out_1312959600226932432[245] = 0; + out_1312959600226932432[246] = 0; + out_1312959600226932432[247] = 1; + out_1312959600226932432[248] = 0; + out_1312959600226932432[249] = 0; + out_1312959600226932432[250] = 0; + out_1312959600226932432[251] = 0; + out_1312959600226932432[252] = 0; + out_1312959600226932432[253] = 0; + out_1312959600226932432[254] = 0; + out_1312959600226932432[255] = 0; + out_1312959600226932432[256] = 0; + out_1312959600226932432[257] = 0; + out_1312959600226932432[258] = 0; + out_1312959600226932432[259] = 0; + out_1312959600226932432[260] = 0; + out_1312959600226932432[261] = 0; + out_1312959600226932432[262] = 0; + out_1312959600226932432[263] = 0; + out_1312959600226932432[264] = 0; + out_1312959600226932432[265] = 0; + out_1312959600226932432[266] = 1; + out_1312959600226932432[267] = 0; + out_1312959600226932432[268] = 0; + out_1312959600226932432[269] = 0; + out_1312959600226932432[270] = 0; + out_1312959600226932432[271] = 0; + out_1312959600226932432[272] = 0; + out_1312959600226932432[273] = 0; + out_1312959600226932432[274] = 0; + out_1312959600226932432[275] = 0; + out_1312959600226932432[276] = 0; + out_1312959600226932432[277] = 0; + out_1312959600226932432[278] = 0; + out_1312959600226932432[279] = 0; + out_1312959600226932432[280] = 0; + out_1312959600226932432[281] = 0; + out_1312959600226932432[282] = 0; + out_1312959600226932432[283] = 0; + out_1312959600226932432[284] = 0; + out_1312959600226932432[285] = 1; + out_1312959600226932432[286] = 0; + out_1312959600226932432[287] = 0; + out_1312959600226932432[288] = 0; + out_1312959600226932432[289] = 0; + out_1312959600226932432[290] = 0; + out_1312959600226932432[291] = 0; + out_1312959600226932432[292] = 0; + out_1312959600226932432[293] = 0; + out_1312959600226932432[294] = 0; + out_1312959600226932432[295] = 0; + out_1312959600226932432[296] = 0; + out_1312959600226932432[297] = 0; + out_1312959600226932432[298] = 0; + out_1312959600226932432[299] = 0; + out_1312959600226932432[300] = 0; + out_1312959600226932432[301] = 0; + out_1312959600226932432[302] = 0; + out_1312959600226932432[303] = 0; + out_1312959600226932432[304] = 1; + out_1312959600226932432[305] = 0; + out_1312959600226932432[306] = 0; + out_1312959600226932432[307] = 0; + out_1312959600226932432[308] = 0; + out_1312959600226932432[309] = 0; + out_1312959600226932432[310] = 0; + out_1312959600226932432[311] = 0; + out_1312959600226932432[312] = 0; + out_1312959600226932432[313] = 0; + out_1312959600226932432[314] = 0; + out_1312959600226932432[315] = 0; + out_1312959600226932432[316] = 0; + out_1312959600226932432[317] = 0; + out_1312959600226932432[318] = 0; + out_1312959600226932432[319] = 0; + out_1312959600226932432[320] = 0; + out_1312959600226932432[321] = 0; + out_1312959600226932432[322] = 0; + out_1312959600226932432[323] = 1; } -void h_4(double *state, double *unused, double *out_6172444408937598103) { - out_6172444408937598103[0] = state[6] + state[9]; - out_6172444408937598103[1] = state[7] + state[10]; - out_6172444408937598103[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_6400948089178504221) { + out_6400948089178504221[0] = state[6] + state[9]; + out_6400948089178504221[1] = state[7] + state[10]; + out_6400948089178504221[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_5296575886336270293) { - out_5296575886336270293[0] = 0; - out_5296575886336270293[1] = 0; - out_5296575886336270293[2] = 0; - out_5296575886336270293[3] = 0; - out_5296575886336270293[4] = 0; - out_5296575886336270293[5] = 0; - out_5296575886336270293[6] = 1; - out_5296575886336270293[7] = 0; - out_5296575886336270293[8] = 0; - out_5296575886336270293[9] = 1; - out_5296575886336270293[10] = 0; - out_5296575886336270293[11] = 0; - out_5296575886336270293[12] = 0; - out_5296575886336270293[13] = 0; - out_5296575886336270293[14] = 0; - out_5296575886336270293[15] = 0; - out_5296575886336270293[16] = 0; - out_5296575886336270293[17] = 0; - out_5296575886336270293[18] = 0; - out_5296575886336270293[19] = 0; - out_5296575886336270293[20] = 0; - out_5296575886336270293[21] = 0; - out_5296575886336270293[22] = 0; - out_5296575886336270293[23] = 0; - out_5296575886336270293[24] = 0; - out_5296575886336270293[25] = 1; - out_5296575886336270293[26] = 0; - out_5296575886336270293[27] = 0; - out_5296575886336270293[28] = 1; - out_5296575886336270293[29] = 0; - out_5296575886336270293[30] = 0; - out_5296575886336270293[31] = 0; - out_5296575886336270293[32] = 0; - out_5296575886336270293[33] = 0; - out_5296575886336270293[34] = 0; - out_5296575886336270293[35] = 0; - out_5296575886336270293[36] = 0; - out_5296575886336270293[37] = 0; - out_5296575886336270293[38] = 0; - out_5296575886336270293[39] = 0; - out_5296575886336270293[40] = 0; - out_5296575886336270293[41] = 0; - out_5296575886336270293[42] = 0; - out_5296575886336270293[43] = 0; - out_5296575886336270293[44] = 1; - out_5296575886336270293[45] = 0; - out_5296575886336270293[46] = 0; - out_5296575886336270293[47] = 1; - out_5296575886336270293[48] = 0; - out_5296575886336270293[49] = 0; - out_5296575886336270293[50] = 0; - out_5296575886336270293[51] = 0; - out_5296575886336270293[52] = 0; - out_5296575886336270293[53] = 0; +void H_4(double *state, double *unused, double *out_3419471307660667524) { + out_3419471307660667524[0] = 0; + out_3419471307660667524[1] = 0; + out_3419471307660667524[2] = 0; + out_3419471307660667524[3] = 0; + out_3419471307660667524[4] = 0; + out_3419471307660667524[5] = 0; + out_3419471307660667524[6] = 1; + out_3419471307660667524[7] = 0; + out_3419471307660667524[8] = 0; + out_3419471307660667524[9] = 1; + out_3419471307660667524[10] = 0; + out_3419471307660667524[11] = 0; + out_3419471307660667524[12] = 0; + out_3419471307660667524[13] = 0; + out_3419471307660667524[14] = 0; + out_3419471307660667524[15] = 0; + out_3419471307660667524[16] = 0; + out_3419471307660667524[17] = 0; + out_3419471307660667524[18] = 0; + out_3419471307660667524[19] = 0; + out_3419471307660667524[20] = 0; + out_3419471307660667524[21] = 0; + out_3419471307660667524[22] = 0; + out_3419471307660667524[23] = 0; + out_3419471307660667524[24] = 0; + out_3419471307660667524[25] = 1; + out_3419471307660667524[26] = 0; + out_3419471307660667524[27] = 0; + out_3419471307660667524[28] = 1; + out_3419471307660667524[29] = 0; + out_3419471307660667524[30] = 0; + out_3419471307660667524[31] = 0; + out_3419471307660667524[32] = 0; + out_3419471307660667524[33] = 0; + out_3419471307660667524[34] = 0; + out_3419471307660667524[35] = 0; + out_3419471307660667524[36] = 0; + out_3419471307660667524[37] = 0; + out_3419471307660667524[38] = 0; + out_3419471307660667524[39] = 0; + out_3419471307660667524[40] = 0; + out_3419471307660667524[41] = 0; + out_3419471307660667524[42] = 0; + out_3419471307660667524[43] = 0; + out_3419471307660667524[44] = 1; + out_3419471307660667524[45] = 0; + out_3419471307660667524[46] = 0; + out_3419471307660667524[47] = 1; + out_3419471307660667524[48] = 0; + out_3419471307660667524[49] = 0; + out_3419471307660667524[50] = 0; + out_3419471307660667524[51] = 0; + out_3419471307660667524[52] = 0; + out_3419471307660667524[53] = 0; } -void h_10(double *state, double *unused, double *out_2819440341260442129) { - out_2819440341260442129[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_2819440341260442129[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_2819440341260442129[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_8540054287512262719) { + out_8540054287512262719[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_8540054287512262719[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_8540054287512262719[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_5042249891606824417) { - out_5042249891606824417[0] = 0; - out_5042249891606824417[1] = 9.8100000000000005*cos(state[1]); - out_5042249891606824417[2] = 0; - out_5042249891606824417[3] = 0; - out_5042249891606824417[4] = -state[8]; - out_5042249891606824417[5] = state[7]; - out_5042249891606824417[6] = 0; - out_5042249891606824417[7] = state[5]; - out_5042249891606824417[8] = -state[4]; - out_5042249891606824417[9] = 0; - out_5042249891606824417[10] = 0; - out_5042249891606824417[11] = 0; - out_5042249891606824417[12] = 1; - out_5042249891606824417[13] = 0; - out_5042249891606824417[14] = 0; - out_5042249891606824417[15] = 1; - out_5042249891606824417[16] = 0; - out_5042249891606824417[17] = 0; - out_5042249891606824417[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_5042249891606824417[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_5042249891606824417[20] = 0; - out_5042249891606824417[21] = state[8]; - out_5042249891606824417[22] = 0; - out_5042249891606824417[23] = -state[6]; - out_5042249891606824417[24] = -state[5]; - out_5042249891606824417[25] = 0; - out_5042249891606824417[26] = state[3]; - out_5042249891606824417[27] = 0; - out_5042249891606824417[28] = 0; - out_5042249891606824417[29] = 0; - out_5042249891606824417[30] = 0; - out_5042249891606824417[31] = 1; - out_5042249891606824417[32] = 0; - out_5042249891606824417[33] = 0; - out_5042249891606824417[34] = 1; - out_5042249891606824417[35] = 0; - out_5042249891606824417[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_5042249891606824417[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_5042249891606824417[38] = 0; - out_5042249891606824417[39] = -state[7]; - out_5042249891606824417[40] = state[6]; - out_5042249891606824417[41] = 0; - out_5042249891606824417[42] = state[4]; - out_5042249891606824417[43] = -state[3]; - out_5042249891606824417[44] = 0; - out_5042249891606824417[45] = 0; - out_5042249891606824417[46] = 0; - out_5042249891606824417[47] = 0; - out_5042249891606824417[48] = 0; - out_5042249891606824417[49] = 0; - out_5042249891606824417[50] = 1; - out_5042249891606824417[51] = 0; - out_5042249891606824417[52] = 0; - out_5042249891606824417[53] = 1; +void H_10(double *state, double *unused, double *out_5231659216258162963) { + out_5231659216258162963[0] = 0; + out_5231659216258162963[1] = 9.8100000000000005*cos(state[1]); + out_5231659216258162963[2] = 0; + out_5231659216258162963[3] = 0; + out_5231659216258162963[4] = -state[8]; + out_5231659216258162963[5] = state[7]; + out_5231659216258162963[6] = 0; + out_5231659216258162963[7] = state[5]; + out_5231659216258162963[8] = -state[4]; + out_5231659216258162963[9] = 0; + out_5231659216258162963[10] = 0; + out_5231659216258162963[11] = 0; + out_5231659216258162963[12] = 1; + out_5231659216258162963[13] = 0; + out_5231659216258162963[14] = 0; + out_5231659216258162963[15] = 1; + out_5231659216258162963[16] = 0; + out_5231659216258162963[17] = 0; + out_5231659216258162963[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_5231659216258162963[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_5231659216258162963[20] = 0; + out_5231659216258162963[21] = state[8]; + out_5231659216258162963[22] = 0; + out_5231659216258162963[23] = -state[6]; + out_5231659216258162963[24] = -state[5]; + out_5231659216258162963[25] = 0; + out_5231659216258162963[26] = state[3]; + out_5231659216258162963[27] = 0; + out_5231659216258162963[28] = 0; + out_5231659216258162963[29] = 0; + out_5231659216258162963[30] = 0; + out_5231659216258162963[31] = 1; + out_5231659216258162963[32] = 0; + out_5231659216258162963[33] = 0; + out_5231659216258162963[34] = 1; + out_5231659216258162963[35] = 0; + out_5231659216258162963[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_5231659216258162963[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_5231659216258162963[38] = 0; + out_5231659216258162963[39] = -state[7]; + out_5231659216258162963[40] = state[6]; + out_5231659216258162963[41] = 0; + out_5231659216258162963[42] = state[4]; + out_5231659216258162963[43] = -state[3]; + out_5231659216258162963[44] = 0; + out_5231659216258162963[45] = 0; + out_5231659216258162963[46] = 0; + out_5231659216258162963[47] = 0; + out_5231659216258162963[48] = 0; + out_5231659216258162963[49] = 0; + out_5231659216258162963[50] = 1; + out_5231659216258162963[51] = 0; + out_5231659216258162963[52] = 0; + out_5231659216258162963[53] = 1; } -void h_13(double *state, double *unused, double *out_3164753833559588683) { - out_3164753833559588683[0] = state[3]; - out_3164753833559588683[1] = state[4]; - out_3164753833559588683[2] = state[5]; +void h_13(double *state, double *unused, double *out_7953441679753433349) { + out_7953441679753433349[0] = state[3]; + out_7953441679753433349[1] = state[4]; + out_7953441679753433349[2] = state[5]; } -void H_13(double *state, double *unused, double *out_8508849711668603094) { - out_8508849711668603094[0] = 0; - out_8508849711668603094[1] = 0; - out_8508849711668603094[2] = 0; - out_8508849711668603094[3] = 1; - out_8508849711668603094[4] = 0; - out_8508849711668603094[5] = 0; - out_8508849711668603094[6] = 0; - out_8508849711668603094[7] = 0; - out_8508849711668603094[8] = 0; - out_8508849711668603094[9] = 0; - out_8508849711668603094[10] = 0; - out_8508849711668603094[11] = 0; - out_8508849711668603094[12] = 0; - out_8508849711668603094[13] = 0; - out_8508849711668603094[14] = 0; - out_8508849711668603094[15] = 0; - out_8508849711668603094[16] = 0; - out_8508849711668603094[17] = 0; - out_8508849711668603094[18] = 0; - out_8508849711668603094[19] = 0; - out_8508849711668603094[20] = 0; - out_8508849711668603094[21] = 0; - out_8508849711668603094[22] = 1; - out_8508849711668603094[23] = 0; - out_8508849711668603094[24] = 0; - out_8508849711668603094[25] = 0; - out_8508849711668603094[26] = 0; - out_8508849711668603094[27] = 0; - out_8508849711668603094[28] = 0; - out_8508849711668603094[29] = 0; - out_8508849711668603094[30] = 0; - out_8508849711668603094[31] = 0; - out_8508849711668603094[32] = 0; - out_8508849711668603094[33] = 0; - out_8508849711668603094[34] = 0; - out_8508849711668603094[35] = 0; - out_8508849711668603094[36] = 0; - out_8508849711668603094[37] = 0; - out_8508849711668603094[38] = 0; - out_8508849711668603094[39] = 0; - out_8508849711668603094[40] = 0; - out_8508849711668603094[41] = 1; - out_8508849711668603094[42] = 0; - out_8508849711668603094[43] = 0; - out_8508849711668603094[44] = 0; - out_8508849711668603094[45] = 0; - out_8508849711668603094[46] = 0; - out_8508849711668603094[47] = 0; - out_8508849711668603094[48] = 0; - out_8508849711668603094[49] = 0; - out_8508849711668603094[50] = 0; - out_8508849711668603094[51] = 0; - out_8508849711668603094[52] = 0; - out_8508849711668603094[53] = 0; +void H_13(double *state, double *unused, double *out_207197482328334723) { + out_207197482328334723[0] = 0; + out_207197482328334723[1] = 0; + out_207197482328334723[2] = 0; + out_207197482328334723[3] = 1; + out_207197482328334723[4] = 0; + out_207197482328334723[5] = 0; + out_207197482328334723[6] = 0; + out_207197482328334723[7] = 0; + out_207197482328334723[8] = 0; + out_207197482328334723[9] = 0; + out_207197482328334723[10] = 0; + out_207197482328334723[11] = 0; + out_207197482328334723[12] = 0; + out_207197482328334723[13] = 0; + out_207197482328334723[14] = 0; + out_207197482328334723[15] = 0; + out_207197482328334723[16] = 0; + out_207197482328334723[17] = 0; + out_207197482328334723[18] = 0; + out_207197482328334723[19] = 0; + out_207197482328334723[20] = 0; + out_207197482328334723[21] = 0; + out_207197482328334723[22] = 1; + out_207197482328334723[23] = 0; + out_207197482328334723[24] = 0; + out_207197482328334723[25] = 0; + out_207197482328334723[26] = 0; + out_207197482328334723[27] = 0; + out_207197482328334723[28] = 0; + out_207197482328334723[29] = 0; + out_207197482328334723[30] = 0; + out_207197482328334723[31] = 0; + out_207197482328334723[32] = 0; + out_207197482328334723[33] = 0; + out_207197482328334723[34] = 0; + out_207197482328334723[35] = 0; + out_207197482328334723[36] = 0; + out_207197482328334723[37] = 0; + out_207197482328334723[38] = 0; + out_207197482328334723[39] = 0; + out_207197482328334723[40] = 0; + out_207197482328334723[41] = 1; + out_207197482328334723[42] = 0; + out_207197482328334723[43] = 0; + out_207197482328334723[44] = 0; + out_207197482328334723[45] = 0; + out_207197482328334723[46] = 0; + out_207197482328334723[47] = 0; + out_207197482328334723[48] = 0; + out_207197482328334723[49] = 0; + out_207197482328334723[50] = 0; + out_207197482328334723[51] = 0; + out_207197482328334723[52] = 0; + out_207197482328334723[53] = 0; } -void h_14(double *state, double *unused, double *out_3107433859522874355) { - out_3107433859522874355[0] = state[6]; - out_3107433859522874355[1] = state[7]; - out_3107433859522874355[2] = state[8]; +void h_14(double *state, double *unused, double *out_8785755176393893631) { + out_8785755176393893631[0] = state[6]; + out_8785755176393893631[1] = state[7]; + out_8785755176393893631[2] = state[8]; } -void H_14(double *state, double *unused, double *out_2213787454040897997) { - out_2213787454040897997[0] = 0; - out_2213787454040897997[1] = 0; - out_2213787454040897997[2] = 0; - out_2213787454040897997[3] = 0; - out_2213787454040897997[4] = 0; - out_2213787454040897997[5] = 0; - out_2213787454040897997[6] = 1; - out_2213787454040897997[7] = 0; - out_2213787454040897997[8] = 0; - out_2213787454040897997[9] = 0; - out_2213787454040897997[10] = 0; - out_2213787454040897997[11] = 0; - out_2213787454040897997[12] = 0; - out_2213787454040897997[13] = 0; - out_2213787454040897997[14] = 0; - out_2213787454040897997[15] = 0; - out_2213787454040897997[16] = 0; - out_2213787454040897997[17] = 0; - out_2213787454040897997[18] = 0; - out_2213787454040897997[19] = 0; - out_2213787454040897997[20] = 0; - out_2213787454040897997[21] = 0; - out_2213787454040897997[22] = 0; - out_2213787454040897997[23] = 0; - out_2213787454040897997[24] = 0; - out_2213787454040897997[25] = 1; - out_2213787454040897997[26] = 0; - out_2213787454040897997[27] = 0; - out_2213787454040897997[28] = 0; - out_2213787454040897997[29] = 0; - out_2213787454040897997[30] = 0; - out_2213787454040897997[31] = 0; - out_2213787454040897997[32] = 0; - out_2213787454040897997[33] = 0; - out_2213787454040897997[34] = 0; - out_2213787454040897997[35] = 0; - out_2213787454040897997[36] = 0; - out_2213787454040897997[37] = 0; - out_2213787454040897997[38] = 0; - out_2213787454040897997[39] = 0; - out_2213787454040897997[40] = 0; - out_2213787454040897997[41] = 0; - out_2213787454040897997[42] = 0; - out_2213787454040897997[43] = 0; - out_2213787454040897997[44] = 1; - out_2213787454040897997[45] = 0; - out_2213787454040897997[46] = 0; - out_2213787454040897997[47] = 0; - out_2213787454040897997[48] = 0; - out_2213787454040897997[49] = 0; - out_2213787454040897997[50] = 0; - out_2213787454040897997[51] = 0; - out_2213787454040897997[52] = 0; - out_2213787454040897997[53] = 0; +void H_14(double *state, double *unused, double *out_543769548678817005) { + out_543769548678817005[0] = 0; + out_543769548678817005[1] = 0; + out_543769548678817005[2] = 0; + out_543769548678817005[3] = 0; + out_543769548678817005[4] = 0; + out_543769548678817005[5] = 0; + out_543769548678817005[6] = 1; + out_543769548678817005[7] = 0; + out_543769548678817005[8] = 0; + out_543769548678817005[9] = 0; + out_543769548678817005[10] = 0; + out_543769548678817005[11] = 0; + out_543769548678817005[12] = 0; + out_543769548678817005[13] = 0; + out_543769548678817005[14] = 0; + out_543769548678817005[15] = 0; + out_543769548678817005[16] = 0; + out_543769548678817005[17] = 0; + out_543769548678817005[18] = 0; + out_543769548678817005[19] = 0; + out_543769548678817005[20] = 0; + out_543769548678817005[21] = 0; + out_543769548678817005[22] = 0; + out_543769548678817005[23] = 0; + out_543769548678817005[24] = 0; + out_543769548678817005[25] = 1; + out_543769548678817005[26] = 0; + out_543769548678817005[27] = 0; + out_543769548678817005[28] = 0; + out_543769548678817005[29] = 0; + out_543769548678817005[30] = 0; + out_543769548678817005[31] = 0; + out_543769548678817005[32] = 0; + out_543769548678817005[33] = 0; + out_543769548678817005[34] = 0; + out_543769548678817005[35] = 0; + out_543769548678817005[36] = 0; + out_543769548678817005[37] = 0; + out_543769548678817005[38] = 0; + out_543769548678817005[39] = 0; + out_543769548678817005[40] = 0; + out_543769548678817005[41] = 0; + out_543769548678817005[42] = 0; + out_543769548678817005[43] = 0; + out_543769548678817005[44] = 1; + out_543769548678817005[45] = 0; + out_543769548678817005[46] = 0; + out_543769548678817005[47] = 0; + out_543769548678817005[48] = 0; + out_543769548678817005[49] = 0; + out_543769548678817005[50] = 0; + out_543769548678817005[51] = 0; + out_543769548678817005[52] = 0; + out_543769548678817005[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_5735299356181127199) { - err_fun(nom_x, delta_x, out_5735299356181127199); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6497612872484174174) { + err_fun(nom_x, delta_x, out_6497612872484174174); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1848486118618361424) { - inv_err_fun(nom_x, true_x, out_1848486118618361424); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1975580934350294204) { + inv_err_fun(nom_x, true_x, out_1975580934350294204); } -void pose_H_mod_fun(double *state, double *out_3441620345746718845) { - H_mod_fun(state, out_3441620345746718845); +void pose_H_mod_fun(double *state, double *out_4617418789543565628) { + H_mod_fun(state, out_4617418789543565628); } -void pose_f_fun(double *state, double dt, double *out_5548337699276970526) { - f_fun(state, dt, out_5548337699276970526); +void pose_f_fun(double *state, double dt, double *out_9094503671864348345) { + f_fun(state, dt, out_9094503671864348345); } -void pose_F_fun(double *state, double dt, double *out_6716874081947285130) { - F_fun(state, dt, out_6716874081947285130); +void pose_F_fun(double *state, double dt, double *out_1312959600226932432) { + F_fun(state, dt, out_1312959600226932432); } -void pose_h_4(double *state, double *unused, double *out_6172444408937598103) { - h_4(state, unused, out_6172444408937598103); +void pose_h_4(double *state, double *unused, double *out_6400948089178504221) { + h_4(state, unused, out_6400948089178504221); } -void pose_H_4(double *state, double *unused, double *out_5296575886336270293) { - H_4(state, unused, out_5296575886336270293); +void pose_H_4(double *state, double *unused, double *out_3419471307660667524) { + H_4(state, unused, out_3419471307660667524); } -void pose_h_10(double *state, double *unused, double *out_2819440341260442129) { - h_10(state, unused, out_2819440341260442129); +void pose_h_10(double *state, double *unused, double *out_8540054287512262719) { + h_10(state, unused, out_8540054287512262719); } -void pose_H_10(double *state, double *unused, double *out_5042249891606824417) { - H_10(state, unused, out_5042249891606824417); +void pose_H_10(double *state, double *unused, double *out_5231659216258162963) { + H_10(state, unused, out_5231659216258162963); } -void pose_h_13(double *state, double *unused, double *out_3164753833559588683) { - h_13(state, unused, out_3164753833559588683); +void pose_h_13(double *state, double *unused, double *out_7953441679753433349) { + h_13(state, unused, out_7953441679753433349); } -void pose_H_13(double *state, double *unused, double *out_8508849711668603094) { - H_13(state, unused, out_8508849711668603094); +void pose_H_13(double *state, double *unused, double *out_207197482328334723) { + H_13(state, unused, out_207197482328334723); } -void pose_h_14(double *state, double *unused, double *out_3107433859522874355) { - h_14(state, unused, out_3107433859522874355); +void pose_h_14(double *state, double *unused, double *out_8785755176393893631) { + h_14(state, unused, out_8785755176393893631); } -void pose_H_14(double *state, double *unused, double *out_2213787454040897997) { - H_14(state, unused, out_2213787454040897997); +void pose_H_14(double *state, double *unused, double *out_543769548678817005) { + H_14(state, unused, out_543769548678817005); } 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 bce5b65c1..e7632f322 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_5735299356181127199); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1848486118618361424); -void pose_H_mod_fun(double *state, double *out_3441620345746718845); -void pose_f_fun(double *state, double dt, double *out_5548337699276970526); -void pose_F_fun(double *state, double dt, double *out_6716874081947285130); -void pose_h_4(double *state, double *unused, double *out_6172444408937598103); -void pose_H_4(double *state, double *unused, double *out_5296575886336270293); -void pose_h_10(double *state, double *unused, double *out_2819440341260442129); -void pose_H_10(double *state, double *unused, double *out_5042249891606824417); -void pose_h_13(double *state, double *unused, double *out_3164753833559588683); -void pose_H_13(double *state, double *unused, double *out_8508849711668603094); -void pose_h_14(double *state, double *unused, double *out_3107433859522874355); -void pose_H_14(double *state, double *unused, double *out_2213787454040897997); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6497612872484174174); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1975580934350294204); +void pose_H_mod_fun(double *state, double *out_4617418789543565628); +void pose_f_fun(double *state, double dt, double *out_9094503671864348345); +void pose_F_fun(double *state, double dt, double *out_1312959600226932432); +void pose_h_4(double *state, double *unused, double *out_6400948089178504221); +void pose_H_4(double *state, double *unused, double *out_3419471307660667524); +void pose_h_10(double *state, double *unused, double *out_8540054287512262719); +void pose_H_10(double *state, double *unused, double *out_5231659216258162963); +void pose_h_13(double *state, double *unused, double *out_7953441679753433349); +void pose_H_13(double *state, double *unused, double *out_207197482328334723); +void pose_h_14(double *state, double *unused, double *out_8785755176393893631); +void pose_H_14(double *state, double *unused, double *out_543769548678817005); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 8df2d9d41..724413381 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ