diff --git a/common/libcommon.a b/common/libcommon.a index 831c50d68..7b235168a 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 8c88b2fe7..ae3b39099 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc b/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc index 91b1f60a5..fb9b336c1 100644 --- a/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc +++ b/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc @@ -822,10 +822,8 @@ BO_ 736 MANUAL_SPEED_LIMIT_ASSIST: 32 XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX SG_ MSLA_STATUS : 26|2@1+ (1,0) [0|3] "" XXX SG_ MSLA_ENABLED : 38|1@1+ (1,0) [0|1] "" XXX - SG_ EV_REGEN_STATE : 104|8@1+ (1,0) [0|255] "" XXX SG_ MAX_SPEED : 55|8@0+ (1,0) [0|255] "" XXX SG_ MAX_SPEED_COPY : 144|8@1+ (1,0) [0|255] "" XXX - SG_ EV_REGEN_STATE_2 : 184|8@1+ (1,0) [0|255] "" XXX BO_ 837 ADRV_0x345: 8 ADRV SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 6faf6848d..fc980abed 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 5fe6d827d..71c95e779 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 a87ec6f60..98a41155b 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 b56c7b6ed..1cd2eda49 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 aa5a86d87..86cea3daa 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 66c355069..ebae42aa4 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 c07f9733b..0ab251ee7 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 c07f9733b..0ab251ee7 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 32f7a538b..e3e17e93e 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 66c355069..ebae42aa4 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 61184ab24..9257bb2be 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-7e481891-DEBUG"; +const uint8_t gitversion[19] = "DEV-1ddbc24a-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 1938f9a00..8af996c60 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 05ffdb599..ad6651685 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 cbb3f0b2b..e23edf21c 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 2b57970ff..93c1b1096 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 43d828038..7ebea8cb9 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 1ac003ea8..7a4eaff8d 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 90950f694..d2b68ff38 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 74a0bb5a5..9f5314e45 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 4c4af1a5e..17997bb1f 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 866a27c1f..299be005a 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 0a487e103..e6a10d97b 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 c519d4138..474d3b317 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 6d6146f0c..e5ca7460e 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 47b836018..3cb961a94 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 7d5e817fe..bdfed0b5a 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 181229324..f97bc0139 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 7a430cb6b..8ffd4afef 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 9519987bc..75f3269e0 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 d9225d9fb..a9dd35af1 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 30926ea6e..f264de1ae 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 519305feb..60009d6e4 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-7e481891-DEBUG \ No newline at end of file +DEV-1ddbc24a-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 504b3a8ad..94f9510e5 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_1697783337874095353) { - out_1697783337874095353[0] = delta_x[0] + nom_x[0]; - out_1697783337874095353[1] = delta_x[1] + nom_x[1]; - out_1697783337874095353[2] = delta_x[2] + nom_x[2]; - out_1697783337874095353[3] = delta_x[3] + nom_x[3]; - out_1697783337874095353[4] = delta_x[4] + nom_x[4]; - out_1697783337874095353[5] = delta_x[5] + nom_x[5]; - out_1697783337874095353[6] = delta_x[6] + nom_x[6]; - out_1697783337874095353[7] = delta_x[7] + nom_x[7]; - out_1697783337874095353[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_5457591731668657994) { + out_5457591731668657994[0] = delta_x[0] + nom_x[0]; + out_5457591731668657994[1] = delta_x[1] + nom_x[1]; + out_5457591731668657994[2] = delta_x[2] + nom_x[2]; + out_5457591731668657994[3] = delta_x[3] + nom_x[3]; + out_5457591731668657994[4] = delta_x[4] + nom_x[4]; + out_5457591731668657994[5] = delta_x[5] + nom_x[5]; + out_5457591731668657994[6] = delta_x[6] + nom_x[6]; + out_5457591731668657994[7] = delta_x[7] + nom_x[7]; + out_5457591731668657994[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3236445373879696441) { - out_3236445373879696441[0] = -nom_x[0] + true_x[0]; - out_3236445373879696441[1] = -nom_x[1] + true_x[1]; - out_3236445373879696441[2] = -nom_x[2] + true_x[2]; - out_3236445373879696441[3] = -nom_x[3] + true_x[3]; - out_3236445373879696441[4] = -nom_x[4] + true_x[4]; - out_3236445373879696441[5] = -nom_x[5] + true_x[5]; - out_3236445373879696441[6] = -nom_x[6] + true_x[6]; - out_3236445373879696441[7] = -nom_x[7] + true_x[7]; - out_3236445373879696441[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_625468362349040739) { + out_625468362349040739[0] = -nom_x[0] + true_x[0]; + out_625468362349040739[1] = -nom_x[1] + true_x[1]; + out_625468362349040739[2] = -nom_x[2] + true_x[2]; + out_625468362349040739[3] = -nom_x[3] + true_x[3]; + out_625468362349040739[4] = -nom_x[4] + true_x[4]; + out_625468362349040739[5] = -nom_x[5] + true_x[5]; + out_625468362349040739[6] = -nom_x[6] + true_x[6]; + out_625468362349040739[7] = -nom_x[7] + true_x[7]; + out_625468362349040739[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_4178247856660158459) { - out_4178247856660158459[0] = 1.0; - out_4178247856660158459[1] = 0.0; - out_4178247856660158459[2] = 0.0; - out_4178247856660158459[3] = 0.0; - out_4178247856660158459[4] = 0.0; - out_4178247856660158459[5] = 0.0; - out_4178247856660158459[6] = 0.0; - out_4178247856660158459[7] = 0.0; - out_4178247856660158459[8] = 0.0; - out_4178247856660158459[9] = 0.0; - out_4178247856660158459[10] = 1.0; - out_4178247856660158459[11] = 0.0; - out_4178247856660158459[12] = 0.0; - out_4178247856660158459[13] = 0.0; - out_4178247856660158459[14] = 0.0; - out_4178247856660158459[15] = 0.0; - out_4178247856660158459[16] = 0.0; - out_4178247856660158459[17] = 0.0; - out_4178247856660158459[18] = 0.0; - out_4178247856660158459[19] = 0.0; - out_4178247856660158459[20] = 1.0; - out_4178247856660158459[21] = 0.0; - out_4178247856660158459[22] = 0.0; - out_4178247856660158459[23] = 0.0; - out_4178247856660158459[24] = 0.0; - out_4178247856660158459[25] = 0.0; - out_4178247856660158459[26] = 0.0; - out_4178247856660158459[27] = 0.0; - out_4178247856660158459[28] = 0.0; - out_4178247856660158459[29] = 0.0; - out_4178247856660158459[30] = 1.0; - out_4178247856660158459[31] = 0.0; - out_4178247856660158459[32] = 0.0; - out_4178247856660158459[33] = 0.0; - out_4178247856660158459[34] = 0.0; - out_4178247856660158459[35] = 0.0; - out_4178247856660158459[36] = 0.0; - out_4178247856660158459[37] = 0.0; - out_4178247856660158459[38] = 0.0; - out_4178247856660158459[39] = 0.0; - out_4178247856660158459[40] = 1.0; - out_4178247856660158459[41] = 0.0; - out_4178247856660158459[42] = 0.0; - out_4178247856660158459[43] = 0.0; - out_4178247856660158459[44] = 0.0; - out_4178247856660158459[45] = 0.0; - out_4178247856660158459[46] = 0.0; - out_4178247856660158459[47] = 0.0; - out_4178247856660158459[48] = 0.0; - out_4178247856660158459[49] = 0.0; - out_4178247856660158459[50] = 1.0; - out_4178247856660158459[51] = 0.0; - out_4178247856660158459[52] = 0.0; - out_4178247856660158459[53] = 0.0; - out_4178247856660158459[54] = 0.0; - out_4178247856660158459[55] = 0.0; - out_4178247856660158459[56] = 0.0; - out_4178247856660158459[57] = 0.0; - out_4178247856660158459[58] = 0.0; - out_4178247856660158459[59] = 0.0; - out_4178247856660158459[60] = 1.0; - out_4178247856660158459[61] = 0.0; - out_4178247856660158459[62] = 0.0; - out_4178247856660158459[63] = 0.0; - out_4178247856660158459[64] = 0.0; - out_4178247856660158459[65] = 0.0; - out_4178247856660158459[66] = 0.0; - out_4178247856660158459[67] = 0.0; - out_4178247856660158459[68] = 0.0; - out_4178247856660158459[69] = 0.0; - out_4178247856660158459[70] = 1.0; - out_4178247856660158459[71] = 0.0; - out_4178247856660158459[72] = 0.0; - out_4178247856660158459[73] = 0.0; - out_4178247856660158459[74] = 0.0; - out_4178247856660158459[75] = 0.0; - out_4178247856660158459[76] = 0.0; - out_4178247856660158459[77] = 0.0; - out_4178247856660158459[78] = 0.0; - out_4178247856660158459[79] = 0.0; - out_4178247856660158459[80] = 1.0; +void H_mod_fun(double *state, double *out_308877862146519903) { + out_308877862146519903[0] = 1.0; + out_308877862146519903[1] = 0.0; + out_308877862146519903[2] = 0.0; + out_308877862146519903[3] = 0.0; + out_308877862146519903[4] = 0.0; + out_308877862146519903[5] = 0.0; + out_308877862146519903[6] = 0.0; + out_308877862146519903[7] = 0.0; + out_308877862146519903[8] = 0.0; + out_308877862146519903[9] = 0.0; + out_308877862146519903[10] = 1.0; + out_308877862146519903[11] = 0.0; + out_308877862146519903[12] = 0.0; + out_308877862146519903[13] = 0.0; + out_308877862146519903[14] = 0.0; + out_308877862146519903[15] = 0.0; + out_308877862146519903[16] = 0.0; + out_308877862146519903[17] = 0.0; + out_308877862146519903[18] = 0.0; + out_308877862146519903[19] = 0.0; + out_308877862146519903[20] = 1.0; + out_308877862146519903[21] = 0.0; + out_308877862146519903[22] = 0.0; + out_308877862146519903[23] = 0.0; + out_308877862146519903[24] = 0.0; + out_308877862146519903[25] = 0.0; + out_308877862146519903[26] = 0.0; + out_308877862146519903[27] = 0.0; + out_308877862146519903[28] = 0.0; + out_308877862146519903[29] = 0.0; + out_308877862146519903[30] = 1.0; + out_308877862146519903[31] = 0.0; + out_308877862146519903[32] = 0.0; + out_308877862146519903[33] = 0.0; + out_308877862146519903[34] = 0.0; + out_308877862146519903[35] = 0.0; + out_308877862146519903[36] = 0.0; + out_308877862146519903[37] = 0.0; + out_308877862146519903[38] = 0.0; + out_308877862146519903[39] = 0.0; + out_308877862146519903[40] = 1.0; + out_308877862146519903[41] = 0.0; + out_308877862146519903[42] = 0.0; + out_308877862146519903[43] = 0.0; + out_308877862146519903[44] = 0.0; + out_308877862146519903[45] = 0.0; + out_308877862146519903[46] = 0.0; + out_308877862146519903[47] = 0.0; + out_308877862146519903[48] = 0.0; + out_308877862146519903[49] = 0.0; + out_308877862146519903[50] = 1.0; + out_308877862146519903[51] = 0.0; + out_308877862146519903[52] = 0.0; + out_308877862146519903[53] = 0.0; + out_308877862146519903[54] = 0.0; + out_308877862146519903[55] = 0.0; + out_308877862146519903[56] = 0.0; + out_308877862146519903[57] = 0.0; + out_308877862146519903[58] = 0.0; + out_308877862146519903[59] = 0.0; + out_308877862146519903[60] = 1.0; + out_308877862146519903[61] = 0.0; + out_308877862146519903[62] = 0.0; + out_308877862146519903[63] = 0.0; + out_308877862146519903[64] = 0.0; + out_308877862146519903[65] = 0.0; + out_308877862146519903[66] = 0.0; + out_308877862146519903[67] = 0.0; + out_308877862146519903[68] = 0.0; + out_308877862146519903[69] = 0.0; + out_308877862146519903[70] = 1.0; + out_308877862146519903[71] = 0.0; + out_308877862146519903[72] = 0.0; + out_308877862146519903[73] = 0.0; + out_308877862146519903[74] = 0.0; + out_308877862146519903[75] = 0.0; + out_308877862146519903[76] = 0.0; + out_308877862146519903[77] = 0.0; + out_308877862146519903[78] = 0.0; + out_308877862146519903[79] = 0.0; + out_308877862146519903[80] = 1.0; } -void f_fun(double *state, double dt, double *out_6661857716321963649) { - out_6661857716321963649[0] = state[0]; - out_6661857716321963649[1] = state[1]; - out_6661857716321963649[2] = state[2]; - out_6661857716321963649[3] = state[3]; - out_6661857716321963649[4] = state[4]; - out_6661857716321963649[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_6661857716321963649[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_6661857716321963649[7] = state[7]; - out_6661857716321963649[8] = state[8]; +void f_fun(double *state, double dt, double *out_9068006394827680313) { + out_9068006394827680313[0] = state[0]; + out_9068006394827680313[1] = state[1]; + out_9068006394827680313[2] = state[2]; + out_9068006394827680313[3] = state[3]; + out_9068006394827680313[4] = state[4]; + out_9068006394827680313[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_9068006394827680313[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_9068006394827680313[7] = state[7]; + out_9068006394827680313[8] = state[8]; } -void F_fun(double *state, double dt, double *out_4395736809983768071) { - out_4395736809983768071[0] = 1; - out_4395736809983768071[1] = 0; - out_4395736809983768071[2] = 0; - out_4395736809983768071[3] = 0; - out_4395736809983768071[4] = 0; - out_4395736809983768071[5] = 0; - out_4395736809983768071[6] = 0; - out_4395736809983768071[7] = 0; - out_4395736809983768071[8] = 0; - out_4395736809983768071[9] = 0; - out_4395736809983768071[10] = 1; - out_4395736809983768071[11] = 0; - out_4395736809983768071[12] = 0; - out_4395736809983768071[13] = 0; - out_4395736809983768071[14] = 0; - out_4395736809983768071[15] = 0; - out_4395736809983768071[16] = 0; - out_4395736809983768071[17] = 0; - out_4395736809983768071[18] = 0; - out_4395736809983768071[19] = 0; - out_4395736809983768071[20] = 1; - out_4395736809983768071[21] = 0; - out_4395736809983768071[22] = 0; - out_4395736809983768071[23] = 0; - out_4395736809983768071[24] = 0; - out_4395736809983768071[25] = 0; - out_4395736809983768071[26] = 0; - out_4395736809983768071[27] = 0; - out_4395736809983768071[28] = 0; - out_4395736809983768071[29] = 0; - out_4395736809983768071[30] = 1; - out_4395736809983768071[31] = 0; - out_4395736809983768071[32] = 0; - out_4395736809983768071[33] = 0; - out_4395736809983768071[34] = 0; - out_4395736809983768071[35] = 0; - out_4395736809983768071[36] = 0; - out_4395736809983768071[37] = 0; - out_4395736809983768071[38] = 0; - out_4395736809983768071[39] = 0; - out_4395736809983768071[40] = 1; - out_4395736809983768071[41] = 0; - out_4395736809983768071[42] = 0; - out_4395736809983768071[43] = 0; - out_4395736809983768071[44] = 0; - out_4395736809983768071[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_4395736809983768071[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_4395736809983768071[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4395736809983768071[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4395736809983768071[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_4395736809983768071[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_4395736809983768071[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_4395736809983768071[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_4395736809983768071[53] = -9.8100000000000005*dt; - out_4395736809983768071[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_4395736809983768071[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_4395736809983768071[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4395736809983768071[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4395736809983768071[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_4395736809983768071[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_4395736809983768071[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_4395736809983768071[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4395736809983768071[62] = 0; - out_4395736809983768071[63] = 0; - out_4395736809983768071[64] = 0; - out_4395736809983768071[65] = 0; - out_4395736809983768071[66] = 0; - out_4395736809983768071[67] = 0; - out_4395736809983768071[68] = 0; - out_4395736809983768071[69] = 0; - out_4395736809983768071[70] = 1; - out_4395736809983768071[71] = 0; - out_4395736809983768071[72] = 0; - out_4395736809983768071[73] = 0; - out_4395736809983768071[74] = 0; - out_4395736809983768071[75] = 0; - out_4395736809983768071[76] = 0; - out_4395736809983768071[77] = 0; - out_4395736809983768071[78] = 0; - out_4395736809983768071[79] = 0; - out_4395736809983768071[80] = 1; +void F_fun(double *state, double dt, double *out_203771605761279388) { + out_203771605761279388[0] = 1; + out_203771605761279388[1] = 0; + out_203771605761279388[2] = 0; + out_203771605761279388[3] = 0; + out_203771605761279388[4] = 0; + out_203771605761279388[5] = 0; + out_203771605761279388[6] = 0; + out_203771605761279388[7] = 0; + out_203771605761279388[8] = 0; + out_203771605761279388[9] = 0; + out_203771605761279388[10] = 1; + out_203771605761279388[11] = 0; + out_203771605761279388[12] = 0; + out_203771605761279388[13] = 0; + out_203771605761279388[14] = 0; + out_203771605761279388[15] = 0; + out_203771605761279388[16] = 0; + out_203771605761279388[17] = 0; + out_203771605761279388[18] = 0; + out_203771605761279388[19] = 0; + out_203771605761279388[20] = 1; + out_203771605761279388[21] = 0; + out_203771605761279388[22] = 0; + out_203771605761279388[23] = 0; + out_203771605761279388[24] = 0; + out_203771605761279388[25] = 0; + out_203771605761279388[26] = 0; + out_203771605761279388[27] = 0; + out_203771605761279388[28] = 0; + out_203771605761279388[29] = 0; + out_203771605761279388[30] = 1; + out_203771605761279388[31] = 0; + out_203771605761279388[32] = 0; + out_203771605761279388[33] = 0; + out_203771605761279388[34] = 0; + out_203771605761279388[35] = 0; + out_203771605761279388[36] = 0; + out_203771605761279388[37] = 0; + out_203771605761279388[38] = 0; + out_203771605761279388[39] = 0; + out_203771605761279388[40] = 1; + out_203771605761279388[41] = 0; + out_203771605761279388[42] = 0; + out_203771605761279388[43] = 0; + out_203771605761279388[44] = 0; + out_203771605761279388[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_203771605761279388[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_203771605761279388[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_203771605761279388[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_203771605761279388[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_203771605761279388[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_203771605761279388[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_203771605761279388[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_203771605761279388[53] = -9.8100000000000005*dt; + out_203771605761279388[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_203771605761279388[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_203771605761279388[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_203771605761279388[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_203771605761279388[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_203771605761279388[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_203771605761279388[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_203771605761279388[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_203771605761279388[62] = 0; + out_203771605761279388[63] = 0; + out_203771605761279388[64] = 0; + out_203771605761279388[65] = 0; + out_203771605761279388[66] = 0; + out_203771605761279388[67] = 0; + out_203771605761279388[68] = 0; + out_203771605761279388[69] = 0; + out_203771605761279388[70] = 1; + out_203771605761279388[71] = 0; + out_203771605761279388[72] = 0; + out_203771605761279388[73] = 0; + out_203771605761279388[74] = 0; + out_203771605761279388[75] = 0; + out_203771605761279388[76] = 0; + out_203771605761279388[77] = 0; + out_203771605761279388[78] = 0; + out_203771605761279388[79] = 0; + out_203771605761279388[80] = 1; } -void h_25(double *state, double *unused, double *out_2333658162275422577) { - out_2333658162275422577[0] = state[6]; +void h_25(double *state, double *unused, double *out_5131794037626248505) { + out_5131794037626248505[0] = state[6]; } -void H_25(double *state, double *unused, double *out_725443968524506381) { - out_725443968524506381[0] = 0; - out_725443968524506381[1] = 0; - out_725443968524506381[2] = 0; - out_725443968524506381[3] = 0; - out_725443968524506381[4] = 0; - out_725443968524506381[5] = 0; - out_725443968524506381[6] = 1; - out_725443968524506381[7] = 0; - out_725443968524506381[8] = 0; +void H_25(double *state, double *unused, double *out_640243059881962306) { + out_640243059881962306[0] = 0; + out_640243059881962306[1] = 0; + out_640243059881962306[2] = 0; + out_640243059881962306[3] = 0; + out_640243059881962306[4] = 0; + out_640243059881962306[5] = 0; + out_640243059881962306[6] = 1; + out_640243059881962306[7] = 0; + out_640243059881962306[8] = 0; } -void h_24(double *state, double *unused, double *out_7377061376079036481) { - out_7377061376079036481[0] = state[4]; - out_7377061376079036481[1] = state[5]; +void h_24(double *state, double *unused, double *out_2029903564179396636) { + out_2029903564179396636[0] = state[4]; + out_2029903564179396636[1] = state[5]; } -void H_24(double *state, double *unused, double *out_1570440577308201780) { - out_1570440577308201780[0] = 0; - out_1570440577308201780[1] = 0; - out_1570440577308201780[2] = 0; - out_1570440577308201780[3] = 0; - out_1570440577308201780[4] = 1; - out_1570440577308201780[5] = 0; - out_1570440577308201780[6] = 0; - out_1570440577308201780[7] = 0; - out_1570440577308201780[8] = 0; - out_1570440577308201780[9] = 0; - out_1570440577308201780[10] = 0; - out_1570440577308201780[11] = 0; - out_1570440577308201780[12] = 0; - out_1570440577308201780[13] = 0; - out_1570440577308201780[14] = 1; - out_1570440577308201780[15] = 0; - out_1570440577308201780[16] = 0; - out_1570440577308201780[17] = 0; +void H_24(double *state, double *unused, double *out_2865950843860830868) { + out_2865950843860830868[0] = 0; + out_2865950843860830868[1] = 0; + out_2865950843860830868[2] = 0; + out_2865950843860830868[3] = 0; + out_2865950843860830868[4] = 1; + out_2865950843860830868[5] = 0; + out_2865950843860830868[6] = 0; + out_2865950843860830868[7] = 0; + out_2865950843860830868[8] = 0; + out_2865950843860830868[9] = 0; + out_2865950843860830868[10] = 0; + out_2865950843860830868[11] = 0; + out_2865950843860830868[12] = 0; + out_2865950843860830868[13] = 0; + out_2865950843860830868[14] = 1; + out_2865950843860830868[15] = 0; + out_2865950843860830868[16] = 0; + out_2865950843860830868[17] = 0; } -void h_30(double *state, double *unused, double *out_2058464099990916688) { - out_2058464099990916688[0] = state[4]; +void h_30(double *state, double *unused, double *out_8651236171946894111) { + out_8651236171946894111[0] = state[4]; } -void H_30(double *state, double *unused, double *out_3243776927031755008) { - out_3243776927031755008[0] = 0; - out_3243776927031755008[1] = 0; - out_3243776927031755008[2] = 0; - out_3243776927031755008[3] = 0; - out_3243776927031755008[4] = 1; - out_3243776927031755008[5] = 0; - out_3243776927031755008[6] = 0; - out_3243776927031755008[7] = 0; - out_3243776927031755008[8] = 0; +void H_30(double *state, double *unused, double *out_510904112738722236) { + out_510904112738722236[0] = 0; + out_510904112738722236[1] = 0; + out_510904112738722236[2] = 0; + out_510904112738722236[3] = 0; + out_510904112738722236[4] = 1; + out_510904112738722236[5] = 0; + out_510904112738722236[6] = 0; + out_510904112738722236[7] = 0; + out_510904112738722236[8] = 0; } -void h_26(double *state, double *unused, double *out_8738934800407300586) { - out_8738934800407300586[0] = state[7]; +void h_26(double *state, double *unused, double *out_2252194654533044965) { + out_2252194654533044965[0] = state[7]; } -void H_26(double *state, double *unused, double *out_3016059350349549843) { - out_3016059350349549843[0] = 0; - out_3016059350349549843[1] = 0; - out_3016059350349549843[2] = 0; - out_3016059350349549843[3] = 0; - out_3016059350349549843[4] = 0; - out_3016059350349549843[5] = 0; - out_3016059350349549843[6] = 0; - out_3016059350349549843[7] = 1; - out_3016059350349549843[8] = 0; +void H_26(double *state, double *unused, double *out_3101260258992093918) { + out_3101260258992093918[0] = 0; + out_3101260258992093918[1] = 0; + out_3101260258992093918[2] = 0; + out_3101260258992093918[3] = 0; + out_3101260258992093918[4] = 0; + out_3101260258992093918[5] = 0; + out_3101260258992093918[6] = 0; + out_3101260258992093918[7] = 1; + out_3101260258992093918[8] = 0; } -void h_27(double *state, double *unused, double *out_3277168015177085012) { - out_3277168015177085012[0] = state[3]; +void h_27(double *state, double *unused, double *out_3283700363150443165) { + out_3283700363150443165[0] = state[3]; } -void H_27(double *state, double *unused, double *out_5467370998215698225) { - out_5467370998215698225[0] = 0; - out_5467370998215698225[1] = 0; - out_5467370998215698225[2] = 0; - out_5467370998215698225[3] = 1; - out_5467370998215698225[4] = 0; - out_5467370998215698225[5] = 0; - out_5467370998215698225[6] = 0; - out_5467370998215698225[7] = 0; - out_5467370998215698225[8] = 0; +void H_27(double *state, double *unused, double *out_1663859199061702675) { + out_1663859199061702675[0] = 0; + out_1663859199061702675[1] = 0; + out_1663859199061702675[2] = 0; + out_1663859199061702675[3] = 1; + out_1663859199061702675[4] = 0; + out_1663859199061702675[5] = 0; + out_1663859199061702675[6] = 0; + out_1663859199061702675[7] = 0; + out_1663859199061702675[8] = 0; } -void h_29(double *state, double *unused, double *out_4679132473327541980) { - out_4679132473327541980[0] = state[1]; +void h_29(double *state, double *unused, double *out_3008506300865937276) { + out_3008506300865937276[0] = state[1]; } -void H_29(double *state, double *unused, double *out_3754008271346147192) { - out_3754008271346147192[0] = 0; - out_3754008271346147192[1] = 1; - out_3754008271346147192[2] = 0; - out_3754008271346147192[3] = 0; - out_3754008271346147192[4] = 0; - out_3754008271346147192[5] = 0; - out_3754008271346147192[6] = 0; - out_3754008271346147192[7] = 0; - out_3754008271346147192[8] = 0; +void H_29(double *state, double *unused, double *out_1021135457053114420) { + out_1021135457053114420[0] = 0; + out_1021135457053114420[1] = 1; + out_1021135457053114420[2] = 0; + out_1021135457053114420[3] = 0; + out_1021135457053114420[4] = 0; + out_1021135457053114420[5] = 0; + out_1021135457053114420[6] = 0; + out_1021135457053114420[7] = 0; + out_1021135457053114420[8] = 0; } -void h_28(double *state, double *unused, double *out_1724505252483760633) { - out_1724505252483760633[0] = state[0]; +void h_28(double *state, double *unused, double *out_5663871735784661514) { + out_5663871735784661514[0] = state[0]; } -void H_28(double *state, double *unused, double *out_1328390745723383382) { - out_1328390745723383382[0] = 1; - out_1328390745723383382[1] = 0; - out_1328390745723383382[2] = 0; - out_1328390745723383382[3] = 0; - out_1328390745723383382[4] = 0; - out_1328390745723383382[5] = 0; - out_1328390745723383382[6] = 0; - out_1328390745723383382[7] = 0; - out_1328390745723383382[8] = 0; +void H_28(double *state, double *unused, double *out_2984765728618440671) { + out_2984765728618440671[0] = 1; + out_2984765728618440671[1] = 0; + out_2984765728618440671[2] = 0; + out_2984765728618440671[3] = 0; + out_2984765728618440671[4] = 0; + out_2984765728618440671[5] = 0; + out_2984765728618440671[6] = 0; + out_2984765728618440671[7] = 0; + out_2984765728618440671[8] = 0; } -void h_31(double *state, double *unused, double *out_6530453262617598609) { - out_6530453262617598609[0] = state[8]; +void h_31(double *state, double *unused, double *out_7524465776080943032) { + out_7524465776080943032[0] = state[8]; } -void H_31(double *state, double *unused, double *out_756089930401466809) { - out_756089930401466809[0] = 0; - out_756089930401466809[1] = 0; - out_756089930401466809[2] = 0; - out_756089930401466809[3] = 0; - out_756089930401466809[4] = 0; - out_756089930401466809[5] = 0; - out_756089930401466809[6] = 0; - out_756089930401466809[7] = 0; - out_756089930401466809[8] = 1; +void H_31(double *state, double *unused, double *out_670889021758922734) { + out_670889021758922734[0] = 0; + out_670889021758922734[1] = 0; + out_670889021758922734[2] = 0; + out_670889021758922734[3] = 0; + out_670889021758922734[4] = 0; + out_670889021758922734[5] = 0; + out_670889021758922734[6] = 0; + out_670889021758922734[7] = 0; + out_670889021758922734[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_1697783337874095353) { - err_fun(nom_x, delta_x, out_1697783337874095353); +void car_err_fun(double *nom_x, double *delta_x, double *out_5457591731668657994) { + err_fun(nom_x, delta_x, out_5457591731668657994); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3236445373879696441) { - inv_err_fun(nom_x, true_x, out_3236445373879696441); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_625468362349040739) { + inv_err_fun(nom_x, true_x, out_625468362349040739); } -void car_H_mod_fun(double *state, double *out_4178247856660158459) { - H_mod_fun(state, out_4178247856660158459); +void car_H_mod_fun(double *state, double *out_308877862146519903) { + H_mod_fun(state, out_308877862146519903); } -void car_f_fun(double *state, double dt, double *out_6661857716321963649) { - f_fun(state, dt, out_6661857716321963649); +void car_f_fun(double *state, double dt, double *out_9068006394827680313) { + f_fun(state, dt, out_9068006394827680313); } -void car_F_fun(double *state, double dt, double *out_4395736809983768071) { - F_fun(state, dt, out_4395736809983768071); +void car_F_fun(double *state, double dt, double *out_203771605761279388) { + F_fun(state, dt, out_203771605761279388); } -void car_h_25(double *state, double *unused, double *out_2333658162275422577) { - h_25(state, unused, out_2333658162275422577); +void car_h_25(double *state, double *unused, double *out_5131794037626248505) { + h_25(state, unused, out_5131794037626248505); } -void car_H_25(double *state, double *unused, double *out_725443968524506381) { - H_25(state, unused, out_725443968524506381); +void car_H_25(double *state, double *unused, double *out_640243059881962306) { + H_25(state, unused, out_640243059881962306); } -void car_h_24(double *state, double *unused, double *out_7377061376079036481) { - h_24(state, unused, out_7377061376079036481); +void car_h_24(double *state, double *unused, double *out_2029903564179396636) { + h_24(state, unused, out_2029903564179396636); } -void car_H_24(double *state, double *unused, double *out_1570440577308201780) { - H_24(state, unused, out_1570440577308201780); +void car_H_24(double *state, double *unused, double *out_2865950843860830868) { + H_24(state, unused, out_2865950843860830868); } -void car_h_30(double *state, double *unused, double *out_2058464099990916688) { - h_30(state, unused, out_2058464099990916688); +void car_h_30(double *state, double *unused, double *out_8651236171946894111) { + h_30(state, unused, out_8651236171946894111); } -void car_H_30(double *state, double *unused, double *out_3243776927031755008) { - H_30(state, unused, out_3243776927031755008); +void car_H_30(double *state, double *unused, double *out_510904112738722236) { + H_30(state, unused, out_510904112738722236); } -void car_h_26(double *state, double *unused, double *out_8738934800407300586) { - h_26(state, unused, out_8738934800407300586); +void car_h_26(double *state, double *unused, double *out_2252194654533044965) { + h_26(state, unused, out_2252194654533044965); } -void car_H_26(double *state, double *unused, double *out_3016059350349549843) { - H_26(state, unused, out_3016059350349549843); +void car_H_26(double *state, double *unused, double *out_3101260258992093918) { + H_26(state, unused, out_3101260258992093918); } -void car_h_27(double *state, double *unused, double *out_3277168015177085012) { - h_27(state, unused, out_3277168015177085012); +void car_h_27(double *state, double *unused, double *out_3283700363150443165) { + h_27(state, unused, out_3283700363150443165); } -void car_H_27(double *state, double *unused, double *out_5467370998215698225) { - H_27(state, unused, out_5467370998215698225); +void car_H_27(double *state, double *unused, double *out_1663859199061702675) { + H_27(state, unused, out_1663859199061702675); } -void car_h_29(double *state, double *unused, double *out_4679132473327541980) { - h_29(state, unused, out_4679132473327541980); +void car_h_29(double *state, double *unused, double *out_3008506300865937276) { + h_29(state, unused, out_3008506300865937276); } -void car_H_29(double *state, double *unused, double *out_3754008271346147192) { - H_29(state, unused, out_3754008271346147192); +void car_H_29(double *state, double *unused, double *out_1021135457053114420) { + H_29(state, unused, out_1021135457053114420); } -void car_h_28(double *state, double *unused, double *out_1724505252483760633) { - h_28(state, unused, out_1724505252483760633); +void car_h_28(double *state, double *unused, double *out_5663871735784661514) { + h_28(state, unused, out_5663871735784661514); } -void car_H_28(double *state, double *unused, double *out_1328390745723383382) { - H_28(state, unused, out_1328390745723383382); +void car_H_28(double *state, double *unused, double *out_2984765728618440671) { + H_28(state, unused, out_2984765728618440671); } -void car_h_31(double *state, double *unused, double *out_6530453262617598609) { - h_31(state, unused, out_6530453262617598609); +void car_h_31(double *state, double *unused, double *out_7524465776080943032) { + h_31(state, unused, out_7524465776080943032); } -void car_H_31(double *state, double *unused, double *out_756089930401466809) { - H_31(state, unused, out_756089930401466809); +void car_H_31(double *state, double *unused, double *out_670889021758922734) { + H_31(state, unused, out_670889021758922734); } 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 c50cf50d8..dac997474 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_1697783337874095353); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3236445373879696441); -void car_H_mod_fun(double *state, double *out_4178247856660158459); -void car_f_fun(double *state, double dt, double *out_6661857716321963649); -void car_F_fun(double *state, double dt, double *out_4395736809983768071); -void car_h_25(double *state, double *unused, double *out_2333658162275422577); -void car_H_25(double *state, double *unused, double *out_725443968524506381); -void car_h_24(double *state, double *unused, double *out_7377061376079036481); -void car_H_24(double *state, double *unused, double *out_1570440577308201780); -void car_h_30(double *state, double *unused, double *out_2058464099990916688); -void car_H_30(double *state, double *unused, double *out_3243776927031755008); -void car_h_26(double *state, double *unused, double *out_8738934800407300586); -void car_H_26(double *state, double *unused, double *out_3016059350349549843); -void car_h_27(double *state, double *unused, double *out_3277168015177085012); -void car_H_27(double *state, double *unused, double *out_5467370998215698225); -void car_h_29(double *state, double *unused, double *out_4679132473327541980); -void car_H_29(double *state, double *unused, double *out_3754008271346147192); -void car_h_28(double *state, double *unused, double *out_1724505252483760633); -void car_H_28(double *state, double *unused, double *out_1328390745723383382); -void car_h_31(double *state, double *unused, double *out_6530453262617598609); -void car_H_31(double *state, double *unused, double *out_756089930401466809); +void car_err_fun(double *nom_x, double *delta_x, double *out_5457591731668657994); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_625468362349040739); +void car_H_mod_fun(double *state, double *out_308877862146519903); +void car_f_fun(double *state, double dt, double *out_9068006394827680313); +void car_F_fun(double *state, double dt, double *out_203771605761279388); +void car_h_25(double *state, double *unused, double *out_5131794037626248505); +void car_H_25(double *state, double *unused, double *out_640243059881962306); +void car_h_24(double *state, double *unused, double *out_2029903564179396636); +void car_H_24(double *state, double *unused, double *out_2865950843860830868); +void car_h_30(double *state, double *unused, double *out_8651236171946894111); +void car_H_30(double *state, double *unused, double *out_510904112738722236); +void car_h_26(double *state, double *unused, double *out_2252194654533044965); +void car_H_26(double *state, double *unused, double *out_3101260258992093918); +void car_h_27(double *state, double *unused, double *out_3283700363150443165); +void car_H_27(double *state, double *unused, double *out_1663859199061702675); +void car_h_29(double *state, double *unused, double *out_3008506300865937276); +void car_H_29(double *state, double *unused, double *out_1021135457053114420); +void car_h_28(double *state, double *unused, double *out_5663871735784661514); +void car_H_28(double *state, double *unused, double *out_2984765728618440671); +void car_h_31(double *state, double *unused, double *out_7524465776080943032); +void car_H_31(double *state, double *unused, double *out_670889021758922734); 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 7725b0c2c..2818cff3a 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_4940663026929446750) { - out_4940663026929446750[0] = delta_x[0] + nom_x[0]; - out_4940663026929446750[1] = delta_x[1] + nom_x[1]; - out_4940663026929446750[2] = delta_x[2] + nom_x[2]; - out_4940663026929446750[3] = delta_x[3] + nom_x[3]; - out_4940663026929446750[4] = delta_x[4] + nom_x[4]; - out_4940663026929446750[5] = delta_x[5] + nom_x[5]; - out_4940663026929446750[6] = delta_x[6] + nom_x[6]; - out_4940663026929446750[7] = delta_x[7] + nom_x[7]; - out_4940663026929446750[8] = delta_x[8] + nom_x[8]; - out_4940663026929446750[9] = delta_x[9] + nom_x[9]; - out_4940663026929446750[10] = delta_x[10] + nom_x[10]; - out_4940663026929446750[11] = delta_x[11] + nom_x[11]; - out_4940663026929446750[12] = delta_x[12] + nom_x[12]; - out_4940663026929446750[13] = delta_x[13] + nom_x[13]; - out_4940663026929446750[14] = delta_x[14] + nom_x[14]; - out_4940663026929446750[15] = delta_x[15] + nom_x[15]; - out_4940663026929446750[16] = delta_x[16] + nom_x[16]; - out_4940663026929446750[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_3212432162194349052) { + out_3212432162194349052[0] = delta_x[0] + nom_x[0]; + out_3212432162194349052[1] = delta_x[1] + nom_x[1]; + out_3212432162194349052[2] = delta_x[2] + nom_x[2]; + out_3212432162194349052[3] = delta_x[3] + nom_x[3]; + out_3212432162194349052[4] = delta_x[4] + nom_x[4]; + out_3212432162194349052[5] = delta_x[5] + nom_x[5]; + out_3212432162194349052[6] = delta_x[6] + nom_x[6]; + out_3212432162194349052[7] = delta_x[7] + nom_x[7]; + out_3212432162194349052[8] = delta_x[8] + nom_x[8]; + out_3212432162194349052[9] = delta_x[9] + nom_x[9]; + out_3212432162194349052[10] = delta_x[10] + nom_x[10]; + out_3212432162194349052[11] = delta_x[11] + nom_x[11]; + out_3212432162194349052[12] = delta_x[12] + nom_x[12]; + out_3212432162194349052[13] = delta_x[13] + nom_x[13]; + out_3212432162194349052[14] = delta_x[14] + nom_x[14]; + out_3212432162194349052[15] = delta_x[15] + nom_x[15]; + out_3212432162194349052[16] = delta_x[16] + nom_x[16]; + out_3212432162194349052[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1207218016783825442) { - out_1207218016783825442[0] = -nom_x[0] + true_x[0]; - out_1207218016783825442[1] = -nom_x[1] + true_x[1]; - out_1207218016783825442[2] = -nom_x[2] + true_x[2]; - out_1207218016783825442[3] = -nom_x[3] + true_x[3]; - out_1207218016783825442[4] = -nom_x[4] + true_x[4]; - out_1207218016783825442[5] = -nom_x[5] + true_x[5]; - out_1207218016783825442[6] = -nom_x[6] + true_x[6]; - out_1207218016783825442[7] = -nom_x[7] + true_x[7]; - out_1207218016783825442[8] = -nom_x[8] + true_x[8]; - out_1207218016783825442[9] = -nom_x[9] + true_x[9]; - out_1207218016783825442[10] = -nom_x[10] + true_x[10]; - out_1207218016783825442[11] = -nom_x[11] + true_x[11]; - out_1207218016783825442[12] = -nom_x[12] + true_x[12]; - out_1207218016783825442[13] = -nom_x[13] + true_x[13]; - out_1207218016783825442[14] = -nom_x[14] + true_x[14]; - out_1207218016783825442[15] = -nom_x[15] + true_x[15]; - out_1207218016783825442[16] = -nom_x[16] + true_x[16]; - out_1207218016783825442[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8973271523308122061) { + out_8973271523308122061[0] = -nom_x[0] + true_x[0]; + out_8973271523308122061[1] = -nom_x[1] + true_x[1]; + out_8973271523308122061[2] = -nom_x[2] + true_x[2]; + out_8973271523308122061[3] = -nom_x[3] + true_x[3]; + out_8973271523308122061[4] = -nom_x[4] + true_x[4]; + out_8973271523308122061[5] = -nom_x[5] + true_x[5]; + out_8973271523308122061[6] = -nom_x[6] + true_x[6]; + out_8973271523308122061[7] = -nom_x[7] + true_x[7]; + out_8973271523308122061[8] = -nom_x[8] + true_x[8]; + out_8973271523308122061[9] = -nom_x[9] + true_x[9]; + out_8973271523308122061[10] = -nom_x[10] + true_x[10]; + out_8973271523308122061[11] = -nom_x[11] + true_x[11]; + out_8973271523308122061[12] = -nom_x[12] + true_x[12]; + out_8973271523308122061[13] = -nom_x[13] + true_x[13]; + out_8973271523308122061[14] = -nom_x[14] + true_x[14]; + out_8973271523308122061[15] = -nom_x[15] + true_x[15]; + out_8973271523308122061[16] = -nom_x[16] + true_x[16]; + out_8973271523308122061[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_2820660951987973699) { - out_2820660951987973699[0] = 1.0; - out_2820660951987973699[1] = 0.0; - out_2820660951987973699[2] = 0.0; - out_2820660951987973699[3] = 0.0; - out_2820660951987973699[4] = 0.0; - out_2820660951987973699[5] = 0.0; - out_2820660951987973699[6] = 0.0; - out_2820660951987973699[7] = 0.0; - out_2820660951987973699[8] = 0.0; - out_2820660951987973699[9] = 0.0; - out_2820660951987973699[10] = 0.0; - out_2820660951987973699[11] = 0.0; - out_2820660951987973699[12] = 0.0; - out_2820660951987973699[13] = 0.0; - out_2820660951987973699[14] = 0.0; - out_2820660951987973699[15] = 0.0; - out_2820660951987973699[16] = 0.0; - out_2820660951987973699[17] = 0.0; - out_2820660951987973699[18] = 0.0; - out_2820660951987973699[19] = 1.0; - out_2820660951987973699[20] = 0.0; - out_2820660951987973699[21] = 0.0; - out_2820660951987973699[22] = 0.0; - out_2820660951987973699[23] = 0.0; - out_2820660951987973699[24] = 0.0; - out_2820660951987973699[25] = 0.0; - out_2820660951987973699[26] = 0.0; - out_2820660951987973699[27] = 0.0; - out_2820660951987973699[28] = 0.0; - out_2820660951987973699[29] = 0.0; - out_2820660951987973699[30] = 0.0; - out_2820660951987973699[31] = 0.0; - out_2820660951987973699[32] = 0.0; - out_2820660951987973699[33] = 0.0; - out_2820660951987973699[34] = 0.0; - out_2820660951987973699[35] = 0.0; - out_2820660951987973699[36] = 0.0; - out_2820660951987973699[37] = 0.0; - out_2820660951987973699[38] = 1.0; - out_2820660951987973699[39] = 0.0; - out_2820660951987973699[40] = 0.0; - out_2820660951987973699[41] = 0.0; - out_2820660951987973699[42] = 0.0; - out_2820660951987973699[43] = 0.0; - out_2820660951987973699[44] = 0.0; - out_2820660951987973699[45] = 0.0; - out_2820660951987973699[46] = 0.0; - out_2820660951987973699[47] = 0.0; - out_2820660951987973699[48] = 0.0; - out_2820660951987973699[49] = 0.0; - out_2820660951987973699[50] = 0.0; - out_2820660951987973699[51] = 0.0; - out_2820660951987973699[52] = 0.0; - out_2820660951987973699[53] = 0.0; - out_2820660951987973699[54] = 0.0; - out_2820660951987973699[55] = 0.0; - out_2820660951987973699[56] = 0.0; - out_2820660951987973699[57] = 1.0; - out_2820660951987973699[58] = 0.0; - out_2820660951987973699[59] = 0.0; - out_2820660951987973699[60] = 0.0; - out_2820660951987973699[61] = 0.0; - out_2820660951987973699[62] = 0.0; - out_2820660951987973699[63] = 0.0; - out_2820660951987973699[64] = 0.0; - out_2820660951987973699[65] = 0.0; - out_2820660951987973699[66] = 0.0; - out_2820660951987973699[67] = 0.0; - out_2820660951987973699[68] = 0.0; - out_2820660951987973699[69] = 0.0; - out_2820660951987973699[70] = 0.0; - out_2820660951987973699[71] = 0.0; - out_2820660951987973699[72] = 0.0; - out_2820660951987973699[73] = 0.0; - out_2820660951987973699[74] = 0.0; - out_2820660951987973699[75] = 0.0; - out_2820660951987973699[76] = 1.0; - out_2820660951987973699[77] = 0.0; - out_2820660951987973699[78] = 0.0; - out_2820660951987973699[79] = 0.0; - out_2820660951987973699[80] = 0.0; - out_2820660951987973699[81] = 0.0; - out_2820660951987973699[82] = 0.0; - out_2820660951987973699[83] = 0.0; - out_2820660951987973699[84] = 0.0; - out_2820660951987973699[85] = 0.0; - out_2820660951987973699[86] = 0.0; - out_2820660951987973699[87] = 0.0; - out_2820660951987973699[88] = 0.0; - out_2820660951987973699[89] = 0.0; - out_2820660951987973699[90] = 0.0; - out_2820660951987973699[91] = 0.0; - out_2820660951987973699[92] = 0.0; - out_2820660951987973699[93] = 0.0; - out_2820660951987973699[94] = 0.0; - out_2820660951987973699[95] = 1.0; - out_2820660951987973699[96] = 0.0; - out_2820660951987973699[97] = 0.0; - out_2820660951987973699[98] = 0.0; - out_2820660951987973699[99] = 0.0; - out_2820660951987973699[100] = 0.0; - out_2820660951987973699[101] = 0.0; - out_2820660951987973699[102] = 0.0; - out_2820660951987973699[103] = 0.0; - out_2820660951987973699[104] = 0.0; - out_2820660951987973699[105] = 0.0; - out_2820660951987973699[106] = 0.0; - out_2820660951987973699[107] = 0.0; - out_2820660951987973699[108] = 0.0; - out_2820660951987973699[109] = 0.0; - out_2820660951987973699[110] = 0.0; - out_2820660951987973699[111] = 0.0; - out_2820660951987973699[112] = 0.0; - out_2820660951987973699[113] = 0.0; - out_2820660951987973699[114] = 1.0; - out_2820660951987973699[115] = 0.0; - out_2820660951987973699[116] = 0.0; - out_2820660951987973699[117] = 0.0; - out_2820660951987973699[118] = 0.0; - out_2820660951987973699[119] = 0.0; - out_2820660951987973699[120] = 0.0; - out_2820660951987973699[121] = 0.0; - out_2820660951987973699[122] = 0.0; - out_2820660951987973699[123] = 0.0; - out_2820660951987973699[124] = 0.0; - out_2820660951987973699[125] = 0.0; - out_2820660951987973699[126] = 0.0; - out_2820660951987973699[127] = 0.0; - out_2820660951987973699[128] = 0.0; - out_2820660951987973699[129] = 0.0; - out_2820660951987973699[130] = 0.0; - out_2820660951987973699[131] = 0.0; - out_2820660951987973699[132] = 0.0; - out_2820660951987973699[133] = 1.0; - out_2820660951987973699[134] = 0.0; - out_2820660951987973699[135] = 0.0; - out_2820660951987973699[136] = 0.0; - out_2820660951987973699[137] = 0.0; - out_2820660951987973699[138] = 0.0; - out_2820660951987973699[139] = 0.0; - out_2820660951987973699[140] = 0.0; - out_2820660951987973699[141] = 0.0; - out_2820660951987973699[142] = 0.0; - out_2820660951987973699[143] = 0.0; - out_2820660951987973699[144] = 0.0; - out_2820660951987973699[145] = 0.0; - out_2820660951987973699[146] = 0.0; - out_2820660951987973699[147] = 0.0; - out_2820660951987973699[148] = 0.0; - out_2820660951987973699[149] = 0.0; - out_2820660951987973699[150] = 0.0; - out_2820660951987973699[151] = 0.0; - out_2820660951987973699[152] = 1.0; - out_2820660951987973699[153] = 0.0; - out_2820660951987973699[154] = 0.0; - out_2820660951987973699[155] = 0.0; - out_2820660951987973699[156] = 0.0; - out_2820660951987973699[157] = 0.0; - out_2820660951987973699[158] = 0.0; - out_2820660951987973699[159] = 0.0; - out_2820660951987973699[160] = 0.0; - out_2820660951987973699[161] = 0.0; - out_2820660951987973699[162] = 0.0; - out_2820660951987973699[163] = 0.0; - out_2820660951987973699[164] = 0.0; - out_2820660951987973699[165] = 0.0; - out_2820660951987973699[166] = 0.0; - out_2820660951987973699[167] = 0.0; - out_2820660951987973699[168] = 0.0; - out_2820660951987973699[169] = 0.0; - out_2820660951987973699[170] = 0.0; - out_2820660951987973699[171] = 1.0; - out_2820660951987973699[172] = 0.0; - out_2820660951987973699[173] = 0.0; - out_2820660951987973699[174] = 0.0; - out_2820660951987973699[175] = 0.0; - out_2820660951987973699[176] = 0.0; - out_2820660951987973699[177] = 0.0; - out_2820660951987973699[178] = 0.0; - out_2820660951987973699[179] = 0.0; - out_2820660951987973699[180] = 0.0; - out_2820660951987973699[181] = 0.0; - out_2820660951987973699[182] = 0.0; - out_2820660951987973699[183] = 0.0; - out_2820660951987973699[184] = 0.0; - out_2820660951987973699[185] = 0.0; - out_2820660951987973699[186] = 0.0; - out_2820660951987973699[187] = 0.0; - out_2820660951987973699[188] = 0.0; - out_2820660951987973699[189] = 0.0; - out_2820660951987973699[190] = 1.0; - out_2820660951987973699[191] = 0.0; - out_2820660951987973699[192] = 0.0; - out_2820660951987973699[193] = 0.0; - out_2820660951987973699[194] = 0.0; - out_2820660951987973699[195] = 0.0; - out_2820660951987973699[196] = 0.0; - out_2820660951987973699[197] = 0.0; - out_2820660951987973699[198] = 0.0; - out_2820660951987973699[199] = 0.0; - out_2820660951987973699[200] = 0.0; - out_2820660951987973699[201] = 0.0; - out_2820660951987973699[202] = 0.0; - out_2820660951987973699[203] = 0.0; - out_2820660951987973699[204] = 0.0; - out_2820660951987973699[205] = 0.0; - out_2820660951987973699[206] = 0.0; - out_2820660951987973699[207] = 0.0; - out_2820660951987973699[208] = 0.0; - out_2820660951987973699[209] = 1.0; - out_2820660951987973699[210] = 0.0; - out_2820660951987973699[211] = 0.0; - out_2820660951987973699[212] = 0.0; - out_2820660951987973699[213] = 0.0; - out_2820660951987973699[214] = 0.0; - out_2820660951987973699[215] = 0.0; - out_2820660951987973699[216] = 0.0; - out_2820660951987973699[217] = 0.0; - out_2820660951987973699[218] = 0.0; - out_2820660951987973699[219] = 0.0; - out_2820660951987973699[220] = 0.0; - out_2820660951987973699[221] = 0.0; - out_2820660951987973699[222] = 0.0; - out_2820660951987973699[223] = 0.0; - out_2820660951987973699[224] = 0.0; - out_2820660951987973699[225] = 0.0; - out_2820660951987973699[226] = 0.0; - out_2820660951987973699[227] = 0.0; - out_2820660951987973699[228] = 1.0; - out_2820660951987973699[229] = 0.0; - out_2820660951987973699[230] = 0.0; - out_2820660951987973699[231] = 0.0; - out_2820660951987973699[232] = 0.0; - out_2820660951987973699[233] = 0.0; - out_2820660951987973699[234] = 0.0; - out_2820660951987973699[235] = 0.0; - out_2820660951987973699[236] = 0.0; - out_2820660951987973699[237] = 0.0; - out_2820660951987973699[238] = 0.0; - out_2820660951987973699[239] = 0.0; - out_2820660951987973699[240] = 0.0; - out_2820660951987973699[241] = 0.0; - out_2820660951987973699[242] = 0.0; - out_2820660951987973699[243] = 0.0; - out_2820660951987973699[244] = 0.0; - out_2820660951987973699[245] = 0.0; - out_2820660951987973699[246] = 0.0; - out_2820660951987973699[247] = 1.0; - out_2820660951987973699[248] = 0.0; - out_2820660951987973699[249] = 0.0; - out_2820660951987973699[250] = 0.0; - out_2820660951987973699[251] = 0.0; - out_2820660951987973699[252] = 0.0; - out_2820660951987973699[253] = 0.0; - out_2820660951987973699[254] = 0.0; - out_2820660951987973699[255] = 0.0; - out_2820660951987973699[256] = 0.0; - out_2820660951987973699[257] = 0.0; - out_2820660951987973699[258] = 0.0; - out_2820660951987973699[259] = 0.0; - out_2820660951987973699[260] = 0.0; - out_2820660951987973699[261] = 0.0; - out_2820660951987973699[262] = 0.0; - out_2820660951987973699[263] = 0.0; - out_2820660951987973699[264] = 0.0; - out_2820660951987973699[265] = 0.0; - out_2820660951987973699[266] = 1.0; - out_2820660951987973699[267] = 0.0; - out_2820660951987973699[268] = 0.0; - out_2820660951987973699[269] = 0.0; - out_2820660951987973699[270] = 0.0; - out_2820660951987973699[271] = 0.0; - out_2820660951987973699[272] = 0.0; - out_2820660951987973699[273] = 0.0; - out_2820660951987973699[274] = 0.0; - out_2820660951987973699[275] = 0.0; - out_2820660951987973699[276] = 0.0; - out_2820660951987973699[277] = 0.0; - out_2820660951987973699[278] = 0.0; - out_2820660951987973699[279] = 0.0; - out_2820660951987973699[280] = 0.0; - out_2820660951987973699[281] = 0.0; - out_2820660951987973699[282] = 0.0; - out_2820660951987973699[283] = 0.0; - out_2820660951987973699[284] = 0.0; - out_2820660951987973699[285] = 1.0; - out_2820660951987973699[286] = 0.0; - out_2820660951987973699[287] = 0.0; - out_2820660951987973699[288] = 0.0; - out_2820660951987973699[289] = 0.0; - out_2820660951987973699[290] = 0.0; - out_2820660951987973699[291] = 0.0; - out_2820660951987973699[292] = 0.0; - out_2820660951987973699[293] = 0.0; - out_2820660951987973699[294] = 0.0; - out_2820660951987973699[295] = 0.0; - out_2820660951987973699[296] = 0.0; - out_2820660951987973699[297] = 0.0; - out_2820660951987973699[298] = 0.0; - out_2820660951987973699[299] = 0.0; - out_2820660951987973699[300] = 0.0; - out_2820660951987973699[301] = 0.0; - out_2820660951987973699[302] = 0.0; - out_2820660951987973699[303] = 0.0; - out_2820660951987973699[304] = 1.0; - out_2820660951987973699[305] = 0.0; - out_2820660951987973699[306] = 0.0; - out_2820660951987973699[307] = 0.0; - out_2820660951987973699[308] = 0.0; - out_2820660951987973699[309] = 0.0; - out_2820660951987973699[310] = 0.0; - out_2820660951987973699[311] = 0.0; - out_2820660951987973699[312] = 0.0; - out_2820660951987973699[313] = 0.0; - out_2820660951987973699[314] = 0.0; - out_2820660951987973699[315] = 0.0; - out_2820660951987973699[316] = 0.0; - out_2820660951987973699[317] = 0.0; - out_2820660951987973699[318] = 0.0; - out_2820660951987973699[319] = 0.0; - out_2820660951987973699[320] = 0.0; - out_2820660951987973699[321] = 0.0; - out_2820660951987973699[322] = 0.0; - out_2820660951987973699[323] = 1.0; +void H_mod_fun(double *state, double *out_7731698092980355432) { + out_7731698092980355432[0] = 1.0; + out_7731698092980355432[1] = 0.0; + out_7731698092980355432[2] = 0.0; + out_7731698092980355432[3] = 0.0; + out_7731698092980355432[4] = 0.0; + out_7731698092980355432[5] = 0.0; + out_7731698092980355432[6] = 0.0; + out_7731698092980355432[7] = 0.0; + out_7731698092980355432[8] = 0.0; + out_7731698092980355432[9] = 0.0; + out_7731698092980355432[10] = 0.0; + out_7731698092980355432[11] = 0.0; + out_7731698092980355432[12] = 0.0; + out_7731698092980355432[13] = 0.0; + out_7731698092980355432[14] = 0.0; + out_7731698092980355432[15] = 0.0; + out_7731698092980355432[16] = 0.0; + out_7731698092980355432[17] = 0.0; + out_7731698092980355432[18] = 0.0; + out_7731698092980355432[19] = 1.0; + out_7731698092980355432[20] = 0.0; + out_7731698092980355432[21] = 0.0; + out_7731698092980355432[22] = 0.0; + out_7731698092980355432[23] = 0.0; + out_7731698092980355432[24] = 0.0; + out_7731698092980355432[25] = 0.0; + out_7731698092980355432[26] = 0.0; + out_7731698092980355432[27] = 0.0; + out_7731698092980355432[28] = 0.0; + out_7731698092980355432[29] = 0.0; + out_7731698092980355432[30] = 0.0; + out_7731698092980355432[31] = 0.0; + out_7731698092980355432[32] = 0.0; + out_7731698092980355432[33] = 0.0; + out_7731698092980355432[34] = 0.0; + out_7731698092980355432[35] = 0.0; + out_7731698092980355432[36] = 0.0; + out_7731698092980355432[37] = 0.0; + out_7731698092980355432[38] = 1.0; + out_7731698092980355432[39] = 0.0; + out_7731698092980355432[40] = 0.0; + out_7731698092980355432[41] = 0.0; + out_7731698092980355432[42] = 0.0; + out_7731698092980355432[43] = 0.0; + out_7731698092980355432[44] = 0.0; + out_7731698092980355432[45] = 0.0; + out_7731698092980355432[46] = 0.0; + out_7731698092980355432[47] = 0.0; + out_7731698092980355432[48] = 0.0; + out_7731698092980355432[49] = 0.0; + out_7731698092980355432[50] = 0.0; + out_7731698092980355432[51] = 0.0; + out_7731698092980355432[52] = 0.0; + out_7731698092980355432[53] = 0.0; + out_7731698092980355432[54] = 0.0; + out_7731698092980355432[55] = 0.0; + out_7731698092980355432[56] = 0.0; + out_7731698092980355432[57] = 1.0; + out_7731698092980355432[58] = 0.0; + out_7731698092980355432[59] = 0.0; + out_7731698092980355432[60] = 0.0; + out_7731698092980355432[61] = 0.0; + out_7731698092980355432[62] = 0.0; + out_7731698092980355432[63] = 0.0; + out_7731698092980355432[64] = 0.0; + out_7731698092980355432[65] = 0.0; + out_7731698092980355432[66] = 0.0; + out_7731698092980355432[67] = 0.0; + out_7731698092980355432[68] = 0.0; + out_7731698092980355432[69] = 0.0; + out_7731698092980355432[70] = 0.0; + out_7731698092980355432[71] = 0.0; + out_7731698092980355432[72] = 0.0; + out_7731698092980355432[73] = 0.0; + out_7731698092980355432[74] = 0.0; + out_7731698092980355432[75] = 0.0; + out_7731698092980355432[76] = 1.0; + out_7731698092980355432[77] = 0.0; + out_7731698092980355432[78] = 0.0; + out_7731698092980355432[79] = 0.0; + out_7731698092980355432[80] = 0.0; + out_7731698092980355432[81] = 0.0; + out_7731698092980355432[82] = 0.0; + out_7731698092980355432[83] = 0.0; + out_7731698092980355432[84] = 0.0; + out_7731698092980355432[85] = 0.0; + out_7731698092980355432[86] = 0.0; + out_7731698092980355432[87] = 0.0; + out_7731698092980355432[88] = 0.0; + out_7731698092980355432[89] = 0.0; + out_7731698092980355432[90] = 0.0; + out_7731698092980355432[91] = 0.0; + out_7731698092980355432[92] = 0.0; + out_7731698092980355432[93] = 0.0; + out_7731698092980355432[94] = 0.0; + out_7731698092980355432[95] = 1.0; + out_7731698092980355432[96] = 0.0; + out_7731698092980355432[97] = 0.0; + out_7731698092980355432[98] = 0.0; + out_7731698092980355432[99] = 0.0; + out_7731698092980355432[100] = 0.0; + out_7731698092980355432[101] = 0.0; + out_7731698092980355432[102] = 0.0; + out_7731698092980355432[103] = 0.0; + out_7731698092980355432[104] = 0.0; + out_7731698092980355432[105] = 0.0; + out_7731698092980355432[106] = 0.0; + out_7731698092980355432[107] = 0.0; + out_7731698092980355432[108] = 0.0; + out_7731698092980355432[109] = 0.0; + out_7731698092980355432[110] = 0.0; + out_7731698092980355432[111] = 0.0; + out_7731698092980355432[112] = 0.0; + out_7731698092980355432[113] = 0.0; + out_7731698092980355432[114] = 1.0; + out_7731698092980355432[115] = 0.0; + out_7731698092980355432[116] = 0.0; + out_7731698092980355432[117] = 0.0; + out_7731698092980355432[118] = 0.0; + out_7731698092980355432[119] = 0.0; + out_7731698092980355432[120] = 0.0; + out_7731698092980355432[121] = 0.0; + out_7731698092980355432[122] = 0.0; + out_7731698092980355432[123] = 0.0; + out_7731698092980355432[124] = 0.0; + out_7731698092980355432[125] = 0.0; + out_7731698092980355432[126] = 0.0; + out_7731698092980355432[127] = 0.0; + out_7731698092980355432[128] = 0.0; + out_7731698092980355432[129] = 0.0; + out_7731698092980355432[130] = 0.0; + out_7731698092980355432[131] = 0.0; + out_7731698092980355432[132] = 0.0; + out_7731698092980355432[133] = 1.0; + out_7731698092980355432[134] = 0.0; + out_7731698092980355432[135] = 0.0; + out_7731698092980355432[136] = 0.0; + out_7731698092980355432[137] = 0.0; + out_7731698092980355432[138] = 0.0; + out_7731698092980355432[139] = 0.0; + out_7731698092980355432[140] = 0.0; + out_7731698092980355432[141] = 0.0; + out_7731698092980355432[142] = 0.0; + out_7731698092980355432[143] = 0.0; + out_7731698092980355432[144] = 0.0; + out_7731698092980355432[145] = 0.0; + out_7731698092980355432[146] = 0.0; + out_7731698092980355432[147] = 0.0; + out_7731698092980355432[148] = 0.0; + out_7731698092980355432[149] = 0.0; + out_7731698092980355432[150] = 0.0; + out_7731698092980355432[151] = 0.0; + out_7731698092980355432[152] = 1.0; + out_7731698092980355432[153] = 0.0; + out_7731698092980355432[154] = 0.0; + out_7731698092980355432[155] = 0.0; + out_7731698092980355432[156] = 0.0; + out_7731698092980355432[157] = 0.0; + out_7731698092980355432[158] = 0.0; + out_7731698092980355432[159] = 0.0; + out_7731698092980355432[160] = 0.0; + out_7731698092980355432[161] = 0.0; + out_7731698092980355432[162] = 0.0; + out_7731698092980355432[163] = 0.0; + out_7731698092980355432[164] = 0.0; + out_7731698092980355432[165] = 0.0; + out_7731698092980355432[166] = 0.0; + out_7731698092980355432[167] = 0.0; + out_7731698092980355432[168] = 0.0; + out_7731698092980355432[169] = 0.0; + out_7731698092980355432[170] = 0.0; + out_7731698092980355432[171] = 1.0; + out_7731698092980355432[172] = 0.0; + out_7731698092980355432[173] = 0.0; + out_7731698092980355432[174] = 0.0; + out_7731698092980355432[175] = 0.0; + out_7731698092980355432[176] = 0.0; + out_7731698092980355432[177] = 0.0; + out_7731698092980355432[178] = 0.0; + out_7731698092980355432[179] = 0.0; + out_7731698092980355432[180] = 0.0; + out_7731698092980355432[181] = 0.0; + out_7731698092980355432[182] = 0.0; + out_7731698092980355432[183] = 0.0; + out_7731698092980355432[184] = 0.0; + out_7731698092980355432[185] = 0.0; + out_7731698092980355432[186] = 0.0; + out_7731698092980355432[187] = 0.0; + out_7731698092980355432[188] = 0.0; + out_7731698092980355432[189] = 0.0; + out_7731698092980355432[190] = 1.0; + out_7731698092980355432[191] = 0.0; + out_7731698092980355432[192] = 0.0; + out_7731698092980355432[193] = 0.0; + out_7731698092980355432[194] = 0.0; + out_7731698092980355432[195] = 0.0; + out_7731698092980355432[196] = 0.0; + out_7731698092980355432[197] = 0.0; + out_7731698092980355432[198] = 0.0; + out_7731698092980355432[199] = 0.0; + out_7731698092980355432[200] = 0.0; + out_7731698092980355432[201] = 0.0; + out_7731698092980355432[202] = 0.0; + out_7731698092980355432[203] = 0.0; + out_7731698092980355432[204] = 0.0; + out_7731698092980355432[205] = 0.0; + out_7731698092980355432[206] = 0.0; + out_7731698092980355432[207] = 0.0; + out_7731698092980355432[208] = 0.0; + out_7731698092980355432[209] = 1.0; + out_7731698092980355432[210] = 0.0; + out_7731698092980355432[211] = 0.0; + out_7731698092980355432[212] = 0.0; + out_7731698092980355432[213] = 0.0; + out_7731698092980355432[214] = 0.0; + out_7731698092980355432[215] = 0.0; + out_7731698092980355432[216] = 0.0; + out_7731698092980355432[217] = 0.0; + out_7731698092980355432[218] = 0.0; + out_7731698092980355432[219] = 0.0; + out_7731698092980355432[220] = 0.0; + out_7731698092980355432[221] = 0.0; + out_7731698092980355432[222] = 0.0; + out_7731698092980355432[223] = 0.0; + out_7731698092980355432[224] = 0.0; + out_7731698092980355432[225] = 0.0; + out_7731698092980355432[226] = 0.0; + out_7731698092980355432[227] = 0.0; + out_7731698092980355432[228] = 1.0; + out_7731698092980355432[229] = 0.0; + out_7731698092980355432[230] = 0.0; + out_7731698092980355432[231] = 0.0; + out_7731698092980355432[232] = 0.0; + out_7731698092980355432[233] = 0.0; + out_7731698092980355432[234] = 0.0; + out_7731698092980355432[235] = 0.0; + out_7731698092980355432[236] = 0.0; + out_7731698092980355432[237] = 0.0; + out_7731698092980355432[238] = 0.0; + out_7731698092980355432[239] = 0.0; + out_7731698092980355432[240] = 0.0; + out_7731698092980355432[241] = 0.0; + out_7731698092980355432[242] = 0.0; + out_7731698092980355432[243] = 0.0; + out_7731698092980355432[244] = 0.0; + out_7731698092980355432[245] = 0.0; + out_7731698092980355432[246] = 0.0; + out_7731698092980355432[247] = 1.0; + out_7731698092980355432[248] = 0.0; + out_7731698092980355432[249] = 0.0; + out_7731698092980355432[250] = 0.0; + out_7731698092980355432[251] = 0.0; + out_7731698092980355432[252] = 0.0; + out_7731698092980355432[253] = 0.0; + out_7731698092980355432[254] = 0.0; + out_7731698092980355432[255] = 0.0; + out_7731698092980355432[256] = 0.0; + out_7731698092980355432[257] = 0.0; + out_7731698092980355432[258] = 0.0; + out_7731698092980355432[259] = 0.0; + out_7731698092980355432[260] = 0.0; + out_7731698092980355432[261] = 0.0; + out_7731698092980355432[262] = 0.0; + out_7731698092980355432[263] = 0.0; + out_7731698092980355432[264] = 0.0; + out_7731698092980355432[265] = 0.0; + out_7731698092980355432[266] = 1.0; + out_7731698092980355432[267] = 0.0; + out_7731698092980355432[268] = 0.0; + out_7731698092980355432[269] = 0.0; + out_7731698092980355432[270] = 0.0; + out_7731698092980355432[271] = 0.0; + out_7731698092980355432[272] = 0.0; + out_7731698092980355432[273] = 0.0; + out_7731698092980355432[274] = 0.0; + out_7731698092980355432[275] = 0.0; + out_7731698092980355432[276] = 0.0; + out_7731698092980355432[277] = 0.0; + out_7731698092980355432[278] = 0.0; + out_7731698092980355432[279] = 0.0; + out_7731698092980355432[280] = 0.0; + out_7731698092980355432[281] = 0.0; + out_7731698092980355432[282] = 0.0; + out_7731698092980355432[283] = 0.0; + out_7731698092980355432[284] = 0.0; + out_7731698092980355432[285] = 1.0; + out_7731698092980355432[286] = 0.0; + out_7731698092980355432[287] = 0.0; + out_7731698092980355432[288] = 0.0; + out_7731698092980355432[289] = 0.0; + out_7731698092980355432[290] = 0.0; + out_7731698092980355432[291] = 0.0; + out_7731698092980355432[292] = 0.0; + out_7731698092980355432[293] = 0.0; + out_7731698092980355432[294] = 0.0; + out_7731698092980355432[295] = 0.0; + out_7731698092980355432[296] = 0.0; + out_7731698092980355432[297] = 0.0; + out_7731698092980355432[298] = 0.0; + out_7731698092980355432[299] = 0.0; + out_7731698092980355432[300] = 0.0; + out_7731698092980355432[301] = 0.0; + out_7731698092980355432[302] = 0.0; + out_7731698092980355432[303] = 0.0; + out_7731698092980355432[304] = 1.0; + out_7731698092980355432[305] = 0.0; + out_7731698092980355432[306] = 0.0; + out_7731698092980355432[307] = 0.0; + out_7731698092980355432[308] = 0.0; + out_7731698092980355432[309] = 0.0; + out_7731698092980355432[310] = 0.0; + out_7731698092980355432[311] = 0.0; + out_7731698092980355432[312] = 0.0; + out_7731698092980355432[313] = 0.0; + out_7731698092980355432[314] = 0.0; + out_7731698092980355432[315] = 0.0; + out_7731698092980355432[316] = 0.0; + out_7731698092980355432[317] = 0.0; + out_7731698092980355432[318] = 0.0; + out_7731698092980355432[319] = 0.0; + out_7731698092980355432[320] = 0.0; + out_7731698092980355432[321] = 0.0; + out_7731698092980355432[322] = 0.0; + out_7731698092980355432[323] = 1.0; } -void f_fun(double *state, double dt, double *out_2310584025056258416) { - out_2310584025056258416[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_2310584025056258416[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_2310584025056258416[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_2310584025056258416[3] = dt*state[12] + state[3]; - out_2310584025056258416[4] = dt*state[13] + state[4]; - out_2310584025056258416[5] = dt*state[14] + state[5]; - out_2310584025056258416[6] = state[6]; - out_2310584025056258416[7] = state[7]; - out_2310584025056258416[8] = state[8]; - out_2310584025056258416[9] = state[9]; - out_2310584025056258416[10] = state[10]; - out_2310584025056258416[11] = state[11]; - out_2310584025056258416[12] = state[12]; - out_2310584025056258416[13] = state[13]; - out_2310584025056258416[14] = state[14]; - out_2310584025056258416[15] = state[15]; - out_2310584025056258416[16] = state[16]; - out_2310584025056258416[17] = state[17]; +void f_fun(double *state, double dt, double *out_8923016311880444304) { + out_8923016311880444304[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_8923016311880444304[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_8923016311880444304[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_8923016311880444304[3] = dt*state[12] + state[3]; + out_8923016311880444304[4] = dt*state[13] + state[4]; + out_8923016311880444304[5] = dt*state[14] + state[5]; + out_8923016311880444304[6] = state[6]; + out_8923016311880444304[7] = state[7]; + out_8923016311880444304[8] = state[8]; + out_8923016311880444304[9] = state[9]; + out_8923016311880444304[10] = state[10]; + out_8923016311880444304[11] = state[11]; + out_8923016311880444304[12] = state[12]; + out_8923016311880444304[13] = state[13]; + out_8923016311880444304[14] = state[14]; + out_8923016311880444304[15] = state[15]; + out_8923016311880444304[16] = state[16]; + out_8923016311880444304[17] = state[17]; } -void F_fun(double *state, double dt, double *out_6557190266781458971) { - out_6557190266781458971[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_6557190266781458971[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_6557190266781458971[2] = 0; - out_6557190266781458971[3] = 0; - out_6557190266781458971[4] = 0; - out_6557190266781458971[5] = 0; - out_6557190266781458971[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_6557190266781458971[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_6557190266781458971[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_6557190266781458971[9] = 0; - out_6557190266781458971[10] = 0; - out_6557190266781458971[11] = 0; - out_6557190266781458971[12] = 0; - out_6557190266781458971[13] = 0; - out_6557190266781458971[14] = 0; - out_6557190266781458971[15] = 0; - out_6557190266781458971[16] = 0; - out_6557190266781458971[17] = 0; - out_6557190266781458971[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_6557190266781458971[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_6557190266781458971[20] = 0; - out_6557190266781458971[21] = 0; - out_6557190266781458971[22] = 0; - out_6557190266781458971[23] = 0; - out_6557190266781458971[24] = 0; - out_6557190266781458971[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_6557190266781458971[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_6557190266781458971[27] = 0; - out_6557190266781458971[28] = 0; - out_6557190266781458971[29] = 0; - out_6557190266781458971[30] = 0; - out_6557190266781458971[31] = 0; - out_6557190266781458971[32] = 0; - out_6557190266781458971[33] = 0; - out_6557190266781458971[34] = 0; - out_6557190266781458971[35] = 0; - out_6557190266781458971[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_6557190266781458971[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_6557190266781458971[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_6557190266781458971[39] = 0; - out_6557190266781458971[40] = 0; - out_6557190266781458971[41] = 0; - out_6557190266781458971[42] = 0; - out_6557190266781458971[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_6557190266781458971[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_6557190266781458971[45] = 0; - out_6557190266781458971[46] = 0; - out_6557190266781458971[47] = 0; - out_6557190266781458971[48] = 0; - out_6557190266781458971[49] = 0; - out_6557190266781458971[50] = 0; - out_6557190266781458971[51] = 0; - out_6557190266781458971[52] = 0; - out_6557190266781458971[53] = 0; - out_6557190266781458971[54] = 0; - out_6557190266781458971[55] = 0; - out_6557190266781458971[56] = 0; - out_6557190266781458971[57] = 1; - out_6557190266781458971[58] = 0; - out_6557190266781458971[59] = 0; - out_6557190266781458971[60] = 0; - out_6557190266781458971[61] = 0; - out_6557190266781458971[62] = 0; - out_6557190266781458971[63] = 0; - out_6557190266781458971[64] = 0; - out_6557190266781458971[65] = 0; - out_6557190266781458971[66] = dt; - out_6557190266781458971[67] = 0; - out_6557190266781458971[68] = 0; - out_6557190266781458971[69] = 0; - out_6557190266781458971[70] = 0; - out_6557190266781458971[71] = 0; - out_6557190266781458971[72] = 0; - out_6557190266781458971[73] = 0; - out_6557190266781458971[74] = 0; - out_6557190266781458971[75] = 0; - out_6557190266781458971[76] = 1; - out_6557190266781458971[77] = 0; - out_6557190266781458971[78] = 0; - out_6557190266781458971[79] = 0; - out_6557190266781458971[80] = 0; - out_6557190266781458971[81] = 0; - out_6557190266781458971[82] = 0; - out_6557190266781458971[83] = 0; - out_6557190266781458971[84] = 0; - out_6557190266781458971[85] = dt; - out_6557190266781458971[86] = 0; - out_6557190266781458971[87] = 0; - out_6557190266781458971[88] = 0; - out_6557190266781458971[89] = 0; - out_6557190266781458971[90] = 0; - out_6557190266781458971[91] = 0; - out_6557190266781458971[92] = 0; - out_6557190266781458971[93] = 0; - out_6557190266781458971[94] = 0; - out_6557190266781458971[95] = 1; - out_6557190266781458971[96] = 0; - out_6557190266781458971[97] = 0; - out_6557190266781458971[98] = 0; - out_6557190266781458971[99] = 0; - out_6557190266781458971[100] = 0; - out_6557190266781458971[101] = 0; - out_6557190266781458971[102] = 0; - out_6557190266781458971[103] = 0; - out_6557190266781458971[104] = dt; - out_6557190266781458971[105] = 0; - out_6557190266781458971[106] = 0; - out_6557190266781458971[107] = 0; - out_6557190266781458971[108] = 0; - out_6557190266781458971[109] = 0; - out_6557190266781458971[110] = 0; - out_6557190266781458971[111] = 0; - out_6557190266781458971[112] = 0; - out_6557190266781458971[113] = 0; - out_6557190266781458971[114] = 1; - out_6557190266781458971[115] = 0; - out_6557190266781458971[116] = 0; - out_6557190266781458971[117] = 0; - out_6557190266781458971[118] = 0; - out_6557190266781458971[119] = 0; - out_6557190266781458971[120] = 0; - out_6557190266781458971[121] = 0; - out_6557190266781458971[122] = 0; - out_6557190266781458971[123] = 0; - out_6557190266781458971[124] = 0; - out_6557190266781458971[125] = 0; - out_6557190266781458971[126] = 0; - out_6557190266781458971[127] = 0; - out_6557190266781458971[128] = 0; - out_6557190266781458971[129] = 0; - out_6557190266781458971[130] = 0; - out_6557190266781458971[131] = 0; - out_6557190266781458971[132] = 0; - out_6557190266781458971[133] = 1; - out_6557190266781458971[134] = 0; - out_6557190266781458971[135] = 0; - out_6557190266781458971[136] = 0; - out_6557190266781458971[137] = 0; - out_6557190266781458971[138] = 0; - out_6557190266781458971[139] = 0; - out_6557190266781458971[140] = 0; - out_6557190266781458971[141] = 0; - out_6557190266781458971[142] = 0; - out_6557190266781458971[143] = 0; - out_6557190266781458971[144] = 0; - out_6557190266781458971[145] = 0; - out_6557190266781458971[146] = 0; - out_6557190266781458971[147] = 0; - out_6557190266781458971[148] = 0; - out_6557190266781458971[149] = 0; - out_6557190266781458971[150] = 0; - out_6557190266781458971[151] = 0; - out_6557190266781458971[152] = 1; - out_6557190266781458971[153] = 0; - out_6557190266781458971[154] = 0; - out_6557190266781458971[155] = 0; - out_6557190266781458971[156] = 0; - out_6557190266781458971[157] = 0; - out_6557190266781458971[158] = 0; - out_6557190266781458971[159] = 0; - out_6557190266781458971[160] = 0; - out_6557190266781458971[161] = 0; - out_6557190266781458971[162] = 0; - out_6557190266781458971[163] = 0; - out_6557190266781458971[164] = 0; - out_6557190266781458971[165] = 0; - out_6557190266781458971[166] = 0; - out_6557190266781458971[167] = 0; - out_6557190266781458971[168] = 0; - out_6557190266781458971[169] = 0; - out_6557190266781458971[170] = 0; - out_6557190266781458971[171] = 1; - out_6557190266781458971[172] = 0; - out_6557190266781458971[173] = 0; - out_6557190266781458971[174] = 0; - out_6557190266781458971[175] = 0; - out_6557190266781458971[176] = 0; - out_6557190266781458971[177] = 0; - out_6557190266781458971[178] = 0; - out_6557190266781458971[179] = 0; - out_6557190266781458971[180] = 0; - out_6557190266781458971[181] = 0; - out_6557190266781458971[182] = 0; - out_6557190266781458971[183] = 0; - out_6557190266781458971[184] = 0; - out_6557190266781458971[185] = 0; - out_6557190266781458971[186] = 0; - out_6557190266781458971[187] = 0; - out_6557190266781458971[188] = 0; - out_6557190266781458971[189] = 0; - out_6557190266781458971[190] = 1; - out_6557190266781458971[191] = 0; - out_6557190266781458971[192] = 0; - out_6557190266781458971[193] = 0; - out_6557190266781458971[194] = 0; - out_6557190266781458971[195] = 0; - out_6557190266781458971[196] = 0; - out_6557190266781458971[197] = 0; - out_6557190266781458971[198] = 0; - out_6557190266781458971[199] = 0; - out_6557190266781458971[200] = 0; - out_6557190266781458971[201] = 0; - out_6557190266781458971[202] = 0; - out_6557190266781458971[203] = 0; - out_6557190266781458971[204] = 0; - out_6557190266781458971[205] = 0; - out_6557190266781458971[206] = 0; - out_6557190266781458971[207] = 0; - out_6557190266781458971[208] = 0; - out_6557190266781458971[209] = 1; - out_6557190266781458971[210] = 0; - out_6557190266781458971[211] = 0; - out_6557190266781458971[212] = 0; - out_6557190266781458971[213] = 0; - out_6557190266781458971[214] = 0; - out_6557190266781458971[215] = 0; - out_6557190266781458971[216] = 0; - out_6557190266781458971[217] = 0; - out_6557190266781458971[218] = 0; - out_6557190266781458971[219] = 0; - out_6557190266781458971[220] = 0; - out_6557190266781458971[221] = 0; - out_6557190266781458971[222] = 0; - out_6557190266781458971[223] = 0; - out_6557190266781458971[224] = 0; - out_6557190266781458971[225] = 0; - out_6557190266781458971[226] = 0; - out_6557190266781458971[227] = 0; - out_6557190266781458971[228] = 1; - out_6557190266781458971[229] = 0; - out_6557190266781458971[230] = 0; - out_6557190266781458971[231] = 0; - out_6557190266781458971[232] = 0; - out_6557190266781458971[233] = 0; - out_6557190266781458971[234] = 0; - out_6557190266781458971[235] = 0; - out_6557190266781458971[236] = 0; - out_6557190266781458971[237] = 0; - out_6557190266781458971[238] = 0; - out_6557190266781458971[239] = 0; - out_6557190266781458971[240] = 0; - out_6557190266781458971[241] = 0; - out_6557190266781458971[242] = 0; - out_6557190266781458971[243] = 0; - out_6557190266781458971[244] = 0; - out_6557190266781458971[245] = 0; - out_6557190266781458971[246] = 0; - out_6557190266781458971[247] = 1; - out_6557190266781458971[248] = 0; - out_6557190266781458971[249] = 0; - out_6557190266781458971[250] = 0; - out_6557190266781458971[251] = 0; - out_6557190266781458971[252] = 0; - out_6557190266781458971[253] = 0; - out_6557190266781458971[254] = 0; - out_6557190266781458971[255] = 0; - out_6557190266781458971[256] = 0; - out_6557190266781458971[257] = 0; - out_6557190266781458971[258] = 0; - out_6557190266781458971[259] = 0; - out_6557190266781458971[260] = 0; - out_6557190266781458971[261] = 0; - out_6557190266781458971[262] = 0; - out_6557190266781458971[263] = 0; - out_6557190266781458971[264] = 0; - out_6557190266781458971[265] = 0; - out_6557190266781458971[266] = 1; - out_6557190266781458971[267] = 0; - out_6557190266781458971[268] = 0; - out_6557190266781458971[269] = 0; - out_6557190266781458971[270] = 0; - out_6557190266781458971[271] = 0; - out_6557190266781458971[272] = 0; - out_6557190266781458971[273] = 0; - out_6557190266781458971[274] = 0; - out_6557190266781458971[275] = 0; - out_6557190266781458971[276] = 0; - out_6557190266781458971[277] = 0; - out_6557190266781458971[278] = 0; - out_6557190266781458971[279] = 0; - out_6557190266781458971[280] = 0; - out_6557190266781458971[281] = 0; - out_6557190266781458971[282] = 0; - out_6557190266781458971[283] = 0; - out_6557190266781458971[284] = 0; - out_6557190266781458971[285] = 1; - out_6557190266781458971[286] = 0; - out_6557190266781458971[287] = 0; - out_6557190266781458971[288] = 0; - out_6557190266781458971[289] = 0; - out_6557190266781458971[290] = 0; - out_6557190266781458971[291] = 0; - out_6557190266781458971[292] = 0; - out_6557190266781458971[293] = 0; - out_6557190266781458971[294] = 0; - out_6557190266781458971[295] = 0; - out_6557190266781458971[296] = 0; - out_6557190266781458971[297] = 0; - out_6557190266781458971[298] = 0; - out_6557190266781458971[299] = 0; - out_6557190266781458971[300] = 0; - out_6557190266781458971[301] = 0; - out_6557190266781458971[302] = 0; - out_6557190266781458971[303] = 0; - out_6557190266781458971[304] = 1; - out_6557190266781458971[305] = 0; - out_6557190266781458971[306] = 0; - out_6557190266781458971[307] = 0; - out_6557190266781458971[308] = 0; - out_6557190266781458971[309] = 0; - out_6557190266781458971[310] = 0; - out_6557190266781458971[311] = 0; - out_6557190266781458971[312] = 0; - out_6557190266781458971[313] = 0; - out_6557190266781458971[314] = 0; - out_6557190266781458971[315] = 0; - out_6557190266781458971[316] = 0; - out_6557190266781458971[317] = 0; - out_6557190266781458971[318] = 0; - out_6557190266781458971[319] = 0; - out_6557190266781458971[320] = 0; - out_6557190266781458971[321] = 0; - out_6557190266781458971[322] = 0; - out_6557190266781458971[323] = 1; +void F_fun(double *state, double dt, double *out_210116950988546947) { + out_210116950988546947[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_210116950988546947[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_210116950988546947[2] = 0; + out_210116950988546947[3] = 0; + out_210116950988546947[4] = 0; + out_210116950988546947[5] = 0; + out_210116950988546947[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_210116950988546947[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_210116950988546947[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_210116950988546947[9] = 0; + out_210116950988546947[10] = 0; + out_210116950988546947[11] = 0; + out_210116950988546947[12] = 0; + out_210116950988546947[13] = 0; + out_210116950988546947[14] = 0; + out_210116950988546947[15] = 0; + out_210116950988546947[16] = 0; + out_210116950988546947[17] = 0; + out_210116950988546947[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_210116950988546947[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_210116950988546947[20] = 0; + out_210116950988546947[21] = 0; + out_210116950988546947[22] = 0; + out_210116950988546947[23] = 0; + out_210116950988546947[24] = 0; + out_210116950988546947[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_210116950988546947[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_210116950988546947[27] = 0; + out_210116950988546947[28] = 0; + out_210116950988546947[29] = 0; + out_210116950988546947[30] = 0; + out_210116950988546947[31] = 0; + out_210116950988546947[32] = 0; + out_210116950988546947[33] = 0; + out_210116950988546947[34] = 0; + out_210116950988546947[35] = 0; + out_210116950988546947[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_210116950988546947[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_210116950988546947[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_210116950988546947[39] = 0; + out_210116950988546947[40] = 0; + out_210116950988546947[41] = 0; + out_210116950988546947[42] = 0; + out_210116950988546947[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_210116950988546947[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_210116950988546947[45] = 0; + out_210116950988546947[46] = 0; + out_210116950988546947[47] = 0; + out_210116950988546947[48] = 0; + out_210116950988546947[49] = 0; + out_210116950988546947[50] = 0; + out_210116950988546947[51] = 0; + out_210116950988546947[52] = 0; + out_210116950988546947[53] = 0; + out_210116950988546947[54] = 0; + out_210116950988546947[55] = 0; + out_210116950988546947[56] = 0; + out_210116950988546947[57] = 1; + out_210116950988546947[58] = 0; + out_210116950988546947[59] = 0; + out_210116950988546947[60] = 0; + out_210116950988546947[61] = 0; + out_210116950988546947[62] = 0; + out_210116950988546947[63] = 0; + out_210116950988546947[64] = 0; + out_210116950988546947[65] = 0; + out_210116950988546947[66] = dt; + out_210116950988546947[67] = 0; + out_210116950988546947[68] = 0; + out_210116950988546947[69] = 0; + out_210116950988546947[70] = 0; + out_210116950988546947[71] = 0; + out_210116950988546947[72] = 0; + out_210116950988546947[73] = 0; + out_210116950988546947[74] = 0; + out_210116950988546947[75] = 0; + out_210116950988546947[76] = 1; + out_210116950988546947[77] = 0; + out_210116950988546947[78] = 0; + out_210116950988546947[79] = 0; + out_210116950988546947[80] = 0; + out_210116950988546947[81] = 0; + out_210116950988546947[82] = 0; + out_210116950988546947[83] = 0; + out_210116950988546947[84] = 0; + out_210116950988546947[85] = dt; + out_210116950988546947[86] = 0; + out_210116950988546947[87] = 0; + out_210116950988546947[88] = 0; + out_210116950988546947[89] = 0; + out_210116950988546947[90] = 0; + out_210116950988546947[91] = 0; + out_210116950988546947[92] = 0; + out_210116950988546947[93] = 0; + out_210116950988546947[94] = 0; + out_210116950988546947[95] = 1; + out_210116950988546947[96] = 0; + out_210116950988546947[97] = 0; + out_210116950988546947[98] = 0; + out_210116950988546947[99] = 0; + out_210116950988546947[100] = 0; + out_210116950988546947[101] = 0; + out_210116950988546947[102] = 0; + out_210116950988546947[103] = 0; + out_210116950988546947[104] = dt; + out_210116950988546947[105] = 0; + out_210116950988546947[106] = 0; + out_210116950988546947[107] = 0; + out_210116950988546947[108] = 0; + out_210116950988546947[109] = 0; + out_210116950988546947[110] = 0; + out_210116950988546947[111] = 0; + out_210116950988546947[112] = 0; + out_210116950988546947[113] = 0; + out_210116950988546947[114] = 1; + out_210116950988546947[115] = 0; + out_210116950988546947[116] = 0; + out_210116950988546947[117] = 0; + out_210116950988546947[118] = 0; + out_210116950988546947[119] = 0; + out_210116950988546947[120] = 0; + out_210116950988546947[121] = 0; + out_210116950988546947[122] = 0; + out_210116950988546947[123] = 0; + out_210116950988546947[124] = 0; + out_210116950988546947[125] = 0; + out_210116950988546947[126] = 0; + out_210116950988546947[127] = 0; + out_210116950988546947[128] = 0; + out_210116950988546947[129] = 0; + out_210116950988546947[130] = 0; + out_210116950988546947[131] = 0; + out_210116950988546947[132] = 0; + out_210116950988546947[133] = 1; + out_210116950988546947[134] = 0; + out_210116950988546947[135] = 0; + out_210116950988546947[136] = 0; + out_210116950988546947[137] = 0; + out_210116950988546947[138] = 0; + out_210116950988546947[139] = 0; + out_210116950988546947[140] = 0; + out_210116950988546947[141] = 0; + out_210116950988546947[142] = 0; + out_210116950988546947[143] = 0; + out_210116950988546947[144] = 0; + out_210116950988546947[145] = 0; + out_210116950988546947[146] = 0; + out_210116950988546947[147] = 0; + out_210116950988546947[148] = 0; + out_210116950988546947[149] = 0; + out_210116950988546947[150] = 0; + out_210116950988546947[151] = 0; + out_210116950988546947[152] = 1; + out_210116950988546947[153] = 0; + out_210116950988546947[154] = 0; + out_210116950988546947[155] = 0; + out_210116950988546947[156] = 0; + out_210116950988546947[157] = 0; + out_210116950988546947[158] = 0; + out_210116950988546947[159] = 0; + out_210116950988546947[160] = 0; + out_210116950988546947[161] = 0; + out_210116950988546947[162] = 0; + out_210116950988546947[163] = 0; + out_210116950988546947[164] = 0; + out_210116950988546947[165] = 0; + out_210116950988546947[166] = 0; + out_210116950988546947[167] = 0; + out_210116950988546947[168] = 0; + out_210116950988546947[169] = 0; + out_210116950988546947[170] = 0; + out_210116950988546947[171] = 1; + out_210116950988546947[172] = 0; + out_210116950988546947[173] = 0; + out_210116950988546947[174] = 0; + out_210116950988546947[175] = 0; + out_210116950988546947[176] = 0; + out_210116950988546947[177] = 0; + out_210116950988546947[178] = 0; + out_210116950988546947[179] = 0; + out_210116950988546947[180] = 0; + out_210116950988546947[181] = 0; + out_210116950988546947[182] = 0; + out_210116950988546947[183] = 0; + out_210116950988546947[184] = 0; + out_210116950988546947[185] = 0; + out_210116950988546947[186] = 0; + out_210116950988546947[187] = 0; + out_210116950988546947[188] = 0; + out_210116950988546947[189] = 0; + out_210116950988546947[190] = 1; + out_210116950988546947[191] = 0; + out_210116950988546947[192] = 0; + out_210116950988546947[193] = 0; + out_210116950988546947[194] = 0; + out_210116950988546947[195] = 0; + out_210116950988546947[196] = 0; + out_210116950988546947[197] = 0; + out_210116950988546947[198] = 0; + out_210116950988546947[199] = 0; + out_210116950988546947[200] = 0; + out_210116950988546947[201] = 0; + out_210116950988546947[202] = 0; + out_210116950988546947[203] = 0; + out_210116950988546947[204] = 0; + out_210116950988546947[205] = 0; + out_210116950988546947[206] = 0; + out_210116950988546947[207] = 0; + out_210116950988546947[208] = 0; + out_210116950988546947[209] = 1; + out_210116950988546947[210] = 0; + out_210116950988546947[211] = 0; + out_210116950988546947[212] = 0; + out_210116950988546947[213] = 0; + out_210116950988546947[214] = 0; + out_210116950988546947[215] = 0; + out_210116950988546947[216] = 0; + out_210116950988546947[217] = 0; + out_210116950988546947[218] = 0; + out_210116950988546947[219] = 0; + out_210116950988546947[220] = 0; + out_210116950988546947[221] = 0; + out_210116950988546947[222] = 0; + out_210116950988546947[223] = 0; + out_210116950988546947[224] = 0; + out_210116950988546947[225] = 0; + out_210116950988546947[226] = 0; + out_210116950988546947[227] = 0; + out_210116950988546947[228] = 1; + out_210116950988546947[229] = 0; + out_210116950988546947[230] = 0; + out_210116950988546947[231] = 0; + out_210116950988546947[232] = 0; + out_210116950988546947[233] = 0; + out_210116950988546947[234] = 0; + out_210116950988546947[235] = 0; + out_210116950988546947[236] = 0; + out_210116950988546947[237] = 0; + out_210116950988546947[238] = 0; + out_210116950988546947[239] = 0; + out_210116950988546947[240] = 0; + out_210116950988546947[241] = 0; + out_210116950988546947[242] = 0; + out_210116950988546947[243] = 0; + out_210116950988546947[244] = 0; + out_210116950988546947[245] = 0; + out_210116950988546947[246] = 0; + out_210116950988546947[247] = 1; + out_210116950988546947[248] = 0; + out_210116950988546947[249] = 0; + out_210116950988546947[250] = 0; + out_210116950988546947[251] = 0; + out_210116950988546947[252] = 0; + out_210116950988546947[253] = 0; + out_210116950988546947[254] = 0; + out_210116950988546947[255] = 0; + out_210116950988546947[256] = 0; + out_210116950988546947[257] = 0; + out_210116950988546947[258] = 0; + out_210116950988546947[259] = 0; + out_210116950988546947[260] = 0; + out_210116950988546947[261] = 0; + out_210116950988546947[262] = 0; + out_210116950988546947[263] = 0; + out_210116950988546947[264] = 0; + out_210116950988546947[265] = 0; + out_210116950988546947[266] = 1; + out_210116950988546947[267] = 0; + out_210116950988546947[268] = 0; + out_210116950988546947[269] = 0; + out_210116950988546947[270] = 0; + out_210116950988546947[271] = 0; + out_210116950988546947[272] = 0; + out_210116950988546947[273] = 0; + out_210116950988546947[274] = 0; + out_210116950988546947[275] = 0; + out_210116950988546947[276] = 0; + out_210116950988546947[277] = 0; + out_210116950988546947[278] = 0; + out_210116950988546947[279] = 0; + out_210116950988546947[280] = 0; + out_210116950988546947[281] = 0; + out_210116950988546947[282] = 0; + out_210116950988546947[283] = 0; + out_210116950988546947[284] = 0; + out_210116950988546947[285] = 1; + out_210116950988546947[286] = 0; + out_210116950988546947[287] = 0; + out_210116950988546947[288] = 0; + out_210116950988546947[289] = 0; + out_210116950988546947[290] = 0; + out_210116950988546947[291] = 0; + out_210116950988546947[292] = 0; + out_210116950988546947[293] = 0; + out_210116950988546947[294] = 0; + out_210116950988546947[295] = 0; + out_210116950988546947[296] = 0; + out_210116950988546947[297] = 0; + out_210116950988546947[298] = 0; + out_210116950988546947[299] = 0; + out_210116950988546947[300] = 0; + out_210116950988546947[301] = 0; + out_210116950988546947[302] = 0; + out_210116950988546947[303] = 0; + out_210116950988546947[304] = 1; + out_210116950988546947[305] = 0; + out_210116950988546947[306] = 0; + out_210116950988546947[307] = 0; + out_210116950988546947[308] = 0; + out_210116950988546947[309] = 0; + out_210116950988546947[310] = 0; + out_210116950988546947[311] = 0; + out_210116950988546947[312] = 0; + out_210116950988546947[313] = 0; + out_210116950988546947[314] = 0; + out_210116950988546947[315] = 0; + out_210116950988546947[316] = 0; + out_210116950988546947[317] = 0; + out_210116950988546947[318] = 0; + out_210116950988546947[319] = 0; + out_210116950988546947[320] = 0; + out_210116950988546947[321] = 0; + out_210116950988546947[322] = 0; + out_210116950988546947[323] = 1; } -void h_4(double *state, double *unused, double *out_8177424571477984765) { - out_8177424571477984765[0] = state[6] + state[9]; - out_8177424571477984765[1] = state[7] + state[10]; - out_8177424571477984765[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_7648624411257127562) { + out_7648624411257127562[0] = state[6] + state[9]; + out_7648624411257127562[1] = state[7] + state[10]; + out_7648624411257127562[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_6592468161831881271) { - out_6592468161831881271[0] = 0; - out_6592468161831881271[1] = 0; - out_6592468161831881271[2] = 0; - out_6592468161831881271[3] = 0; - out_6592468161831881271[4] = 0; - out_6592468161831881271[5] = 0; - out_6592468161831881271[6] = 1; - out_6592468161831881271[7] = 0; - out_6592468161831881271[8] = 0; - out_6592468161831881271[9] = 1; - out_6592468161831881271[10] = 0; - out_6592468161831881271[11] = 0; - out_6592468161831881271[12] = 0; - out_6592468161831881271[13] = 0; - out_6592468161831881271[14] = 0; - out_6592468161831881271[15] = 0; - out_6592468161831881271[16] = 0; - out_6592468161831881271[17] = 0; - out_6592468161831881271[18] = 0; - out_6592468161831881271[19] = 0; - out_6592468161831881271[20] = 0; - out_6592468161831881271[21] = 0; - out_6592468161831881271[22] = 0; - out_6592468161831881271[23] = 0; - out_6592468161831881271[24] = 0; - out_6592468161831881271[25] = 1; - out_6592468161831881271[26] = 0; - out_6592468161831881271[27] = 0; - out_6592468161831881271[28] = 1; - out_6592468161831881271[29] = 0; - out_6592468161831881271[30] = 0; - out_6592468161831881271[31] = 0; - out_6592468161831881271[32] = 0; - out_6592468161831881271[33] = 0; - out_6592468161831881271[34] = 0; - out_6592468161831881271[35] = 0; - out_6592468161831881271[36] = 0; - out_6592468161831881271[37] = 0; - out_6592468161831881271[38] = 0; - out_6592468161831881271[39] = 0; - out_6592468161831881271[40] = 0; - out_6592468161831881271[41] = 0; - out_6592468161831881271[42] = 0; - out_6592468161831881271[43] = 0; - out_6592468161831881271[44] = 1; - out_6592468161831881271[45] = 0; - out_6592468161831881271[46] = 0; - out_6592468161831881271[47] = 1; - out_6592468161831881271[48] = 0; - out_6592468161831881271[49] = 0; - out_6592468161831881271[50] = 0; - out_6592468161831881271[51] = 0; - out_6592468161831881271[52] = 0; - out_6592468161831881271[53] = 0; +void H_4(double *state, double *unused, double *out_3959890883136447860) { + out_3959890883136447860[0] = 0; + out_3959890883136447860[1] = 0; + out_3959890883136447860[2] = 0; + out_3959890883136447860[3] = 0; + out_3959890883136447860[4] = 0; + out_3959890883136447860[5] = 0; + out_3959890883136447860[6] = 1; + out_3959890883136447860[7] = 0; + out_3959890883136447860[8] = 0; + out_3959890883136447860[9] = 1; + out_3959890883136447860[10] = 0; + out_3959890883136447860[11] = 0; + out_3959890883136447860[12] = 0; + out_3959890883136447860[13] = 0; + out_3959890883136447860[14] = 0; + out_3959890883136447860[15] = 0; + out_3959890883136447860[16] = 0; + out_3959890883136447860[17] = 0; + out_3959890883136447860[18] = 0; + out_3959890883136447860[19] = 0; + out_3959890883136447860[20] = 0; + out_3959890883136447860[21] = 0; + out_3959890883136447860[22] = 0; + out_3959890883136447860[23] = 0; + out_3959890883136447860[24] = 0; + out_3959890883136447860[25] = 1; + out_3959890883136447860[26] = 0; + out_3959890883136447860[27] = 0; + out_3959890883136447860[28] = 1; + out_3959890883136447860[29] = 0; + out_3959890883136447860[30] = 0; + out_3959890883136447860[31] = 0; + out_3959890883136447860[32] = 0; + out_3959890883136447860[33] = 0; + out_3959890883136447860[34] = 0; + out_3959890883136447860[35] = 0; + out_3959890883136447860[36] = 0; + out_3959890883136447860[37] = 0; + out_3959890883136447860[38] = 0; + out_3959890883136447860[39] = 0; + out_3959890883136447860[40] = 0; + out_3959890883136447860[41] = 0; + out_3959890883136447860[42] = 0; + out_3959890883136447860[43] = 0; + out_3959890883136447860[44] = 1; + out_3959890883136447860[45] = 0; + out_3959890883136447860[46] = 0; + out_3959890883136447860[47] = 1; + out_3959890883136447860[48] = 0; + out_3959890883136447860[49] = 0; + out_3959890883136447860[50] = 0; + out_3959890883136447860[51] = 0; + out_3959890883136447860[52] = 0; + out_3959890883136447860[53] = 0; } -void h_10(double *state, double *unused, double *out_4220186743788310408) { - out_4220186743788310408[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_4220186743788310408[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_4220186743788310408[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_6099133755902511145) { + out_6099133755902511145[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_6099133755902511145[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_6099133755902511145[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_2494414403690571241) { - out_2494414403690571241[0] = 0; - out_2494414403690571241[1] = 9.8100000000000005*cos(state[1]); - out_2494414403690571241[2] = 0; - out_2494414403690571241[3] = 0; - out_2494414403690571241[4] = -state[8]; - out_2494414403690571241[5] = state[7]; - out_2494414403690571241[6] = 0; - out_2494414403690571241[7] = state[5]; - out_2494414403690571241[8] = -state[4]; - out_2494414403690571241[9] = 0; - out_2494414403690571241[10] = 0; - out_2494414403690571241[11] = 0; - out_2494414403690571241[12] = 1; - out_2494414403690571241[13] = 0; - out_2494414403690571241[14] = 0; - out_2494414403690571241[15] = 1; - out_2494414403690571241[16] = 0; - out_2494414403690571241[17] = 0; - out_2494414403690571241[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_2494414403690571241[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_2494414403690571241[20] = 0; - out_2494414403690571241[21] = state[8]; - out_2494414403690571241[22] = 0; - out_2494414403690571241[23] = -state[6]; - out_2494414403690571241[24] = -state[5]; - out_2494414403690571241[25] = 0; - out_2494414403690571241[26] = state[3]; - out_2494414403690571241[27] = 0; - out_2494414403690571241[28] = 0; - out_2494414403690571241[29] = 0; - out_2494414403690571241[30] = 0; - out_2494414403690571241[31] = 1; - out_2494414403690571241[32] = 0; - out_2494414403690571241[33] = 0; - out_2494414403690571241[34] = 1; - out_2494414403690571241[35] = 0; - out_2494414403690571241[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_2494414403690571241[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_2494414403690571241[38] = 0; - out_2494414403690571241[39] = -state[7]; - out_2494414403690571241[40] = state[6]; - out_2494414403690571241[41] = 0; - out_2494414403690571241[42] = state[4]; - out_2494414403690571241[43] = -state[3]; - out_2494414403690571241[44] = 0; - out_2494414403690571241[45] = 0; - out_2494414403690571241[46] = 0; - out_2494414403690571241[47] = 0; - out_2494414403690571241[48] = 0; - out_2494414403690571241[49] = 0; - out_2494414403690571241[50] = 1; - out_2494414403690571241[51] = 0; - out_2494414403690571241[52] = 0; - out_2494414403690571241[53] = 1; +void H_10(double *state, double *unused, double *out_2569809615037315587) { + out_2569809615037315587[0] = 0; + out_2569809615037315587[1] = 9.8100000000000005*cos(state[1]); + out_2569809615037315587[2] = 0; + out_2569809615037315587[3] = 0; + out_2569809615037315587[4] = -state[8]; + out_2569809615037315587[5] = state[7]; + out_2569809615037315587[6] = 0; + out_2569809615037315587[7] = state[5]; + out_2569809615037315587[8] = -state[4]; + out_2569809615037315587[9] = 0; + out_2569809615037315587[10] = 0; + out_2569809615037315587[11] = 0; + out_2569809615037315587[12] = 1; + out_2569809615037315587[13] = 0; + out_2569809615037315587[14] = 0; + out_2569809615037315587[15] = 1; + out_2569809615037315587[16] = 0; + out_2569809615037315587[17] = 0; + out_2569809615037315587[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_2569809615037315587[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_2569809615037315587[20] = 0; + out_2569809615037315587[21] = state[8]; + out_2569809615037315587[22] = 0; + out_2569809615037315587[23] = -state[6]; + out_2569809615037315587[24] = -state[5]; + out_2569809615037315587[25] = 0; + out_2569809615037315587[26] = state[3]; + out_2569809615037315587[27] = 0; + out_2569809615037315587[28] = 0; + out_2569809615037315587[29] = 0; + out_2569809615037315587[30] = 0; + out_2569809615037315587[31] = 1; + out_2569809615037315587[32] = 0; + out_2569809615037315587[33] = 0; + out_2569809615037315587[34] = 1; + out_2569809615037315587[35] = 0; + out_2569809615037315587[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_2569809615037315587[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_2569809615037315587[38] = 0; + out_2569809615037315587[39] = -state[7]; + out_2569809615037315587[40] = state[6]; + out_2569809615037315587[41] = 0; + out_2569809615037315587[42] = state[4]; + out_2569809615037315587[43] = -state[3]; + out_2569809615037315587[44] = 0; + out_2569809615037315587[45] = 0; + out_2569809615037315587[46] = 0; + out_2569809615037315587[47] = 0; + out_2569809615037315587[48] = 0; + out_2569809615037315587[49] = 0; + out_2569809615037315587[50] = 1; + out_2569809615037315587[51] = 0; + out_2569809615037315587[52] = 0; + out_2569809615037315587[53] = 1; } -void h_13(double *state, double *unused, double *out_5780074361933493057) { - out_5780074361933493057[0] = state[3]; - out_5780074361933493057[1] = state[4]; - out_5780074361933493057[2] = state[5]; +void h_13(double *state, double *unused, double *out_2653699383913433253) { + out_2653699383913433253[0] = state[3]; + out_2653699383913433253[1] = state[4]; + out_2653699383913433253[2] = state[5]; } -void H_13(double *state, double *unused, double *out_8642002086545337544) { - out_8642002086545337544[0] = 0; - out_8642002086545337544[1] = 0; - out_8642002086545337544[2] = 0; - out_8642002086545337544[3] = 1; - out_8642002086545337544[4] = 0; - out_8642002086545337544[5] = 0; - out_8642002086545337544[6] = 0; - out_8642002086545337544[7] = 0; - out_8642002086545337544[8] = 0; - out_8642002086545337544[9] = 0; - out_8642002086545337544[10] = 0; - out_8642002086545337544[11] = 0; - out_8642002086545337544[12] = 0; - out_8642002086545337544[13] = 0; - out_8642002086545337544[14] = 0; - out_8642002086545337544[15] = 0; - out_8642002086545337544[16] = 0; - out_8642002086545337544[17] = 0; - out_8642002086545337544[18] = 0; - out_8642002086545337544[19] = 0; - out_8642002086545337544[20] = 0; - out_8642002086545337544[21] = 0; - out_8642002086545337544[22] = 1; - out_8642002086545337544[23] = 0; - out_8642002086545337544[24] = 0; - out_8642002086545337544[25] = 0; - out_8642002086545337544[26] = 0; - out_8642002086545337544[27] = 0; - out_8642002086545337544[28] = 0; - out_8642002086545337544[29] = 0; - out_8642002086545337544[30] = 0; - out_8642002086545337544[31] = 0; - out_8642002086545337544[32] = 0; - out_8642002086545337544[33] = 0; - out_8642002086545337544[34] = 0; - out_8642002086545337544[35] = 0; - out_8642002086545337544[36] = 0; - out_8642002086545337544[37] = 0; - out_8642002086545337544[38] = 0; - out_8642002086545337544[39] = 0; - out_8642002086545337544[40] = 0; - out_8642002086545337544[41] = 1; - out_8642002086545337544[42] = 0; - out_8642002086545337544[43] = 0; - out_8642002086545337544[44] = 0; - out_8642002086545337544[45] = 0; - out_8642002086545337544[46] = 0; - out_8642002086545337544[47] = 0; - out_8642002086545337544[48] = 0; - out_8642002086545337544[49] = 0; - out_8642002086545337544[50] = 0; - out_8642002086545337544[51] = 0; - out_8642002086545337544[52] = 0; - out_8642002086545337544[53] = 0; +void H_13(double *state, double *unused, double *out_747617057804115059) { + out_747617057804115059[0] = 0; + out_747617057804115059[1] = 0; + out_747617057804115059[2] = 0; + out_747617057804115059[3] = 1; + out_747617057804115059[4] = 0; + out_747617057804115059[5] = 0; + out_747617057804115059[6] = 0; + out_747617057804115059[7] = 0; + out_747617057804115059[8] = 0; + out_747617057804115059[9] = 0; + out_747617057804115059[10] = 0; + out_747617057804115059[11] = 0; + out_747617057804115059[12] = 0; + out_747617057804115059[13] = 0; + out_747617057804115059[14] = 0; + out_747617057804115059[15] = 0; + out_747617057804115059[16] = 0; + out_747617057804115059[17] = 0; + out_747617057804115059[18] = 0; + out_747617057804115059[19] = 0; + out_747617057804115059[20] = 0; + out_747617057804115059[21] = 0; + out_747617057804115059[22] = 1; + out_747617057804115059[23] = 0; + out_747617057804115059[24] = 0; + out_747617057804115059[25] = 0; + out_747617057804115059[26] = 0; + out_747617057804115059[27] = 0; + out_747617057804115059[28] = 0; + out_747617057804115059[29] = 0; + out_747617057804115059[30] = 0; + out_747617057804115059[31] = 0; + out_747617057804115059[32] = 0; + out_747617057804115059[33] = 0; + out_747617057804115059[34] = 0; + out_747617057804115059[35] = 0; + out_747617057804115059[36] = 0; + out_747617057804115059[37] = 0; + out_747617057804115059[38] = 0; + out_747617057804115059[39] = 0; + out_747617057804115059[40] = 0; + out_747617057804115059[41] = 1; + out_747617057804115059[42] = 0; + out_747617057804115059[43] = 0; + out_747617057804115059[44] = 0; + out_747617057804115059[45] = 0; + out_747617057804115059[46] = 0; + out_747617057804115059[47] = 0; + out_747617057804115059[48] = 0; + out_747617057804115059[49] = 0; + out_747617057804115059[50] = 0; + out_747617057804115059[51] = 0; + out_747617057804115059[52] = 0; + out_747617057804115059[53] = 0; } -void h_14(double *state, double *unused, double *out_3473232613379301184) { - out_3473232613379301184[0] = state[6]; - out_3473232613379301184[1] = state[7]; - out_3473232613379301184[2] = state[8]; +void h_14(double *state, double *unused, double *out_7344654912082976031) { + out_7344654912082976031[0] = state[6]; + out_7344654912082976031[1] = state[7]; + out_7344654912082976031[2] = state[8]; } -void H_14(double *state, double *unused, double *out_3509679729536508975) { - out_3509679729536508975[0] = 0; - out_3509679729536508975[1] = 0; - out_3509679729536508975[2] = 0; - out_3509679729536508975[3] = 0; - out_3509679729536508975[4] = 0; - out_3509679729536508975[5] = 0; - out_3509679729536508975[6] = 1; - out_3509679729536508975[7] = 0; - out_3509679729536508975[8] = 0; - out_3509679729536508975[9] = 0; - out_3509679729536508975[10] = 0; - out_3509679729536508975[11] = 0; - out_3509679729536508975[12] = 0; - out_3509679729536508975[13] = 0; - out_3509679729536508975[14] = 0; - out_3509679729536508975[15] = 0; - out_3509679729536508975[16] = 0; - out_3509679729536508975[17] = 0; - out_3509679729536508975[18] = 0; - out_3509679729536508975[19] = 0; - out_3509679729536508975[20] = 0; - out_3509679729536508975[21] = 0; - out_3509679729536508975[22] = 0; - out_3509679729536508975[23] = 0; - out_3509679729536508975[24] = 0; - out_3509679729536508975[25] = 1; - out_3509679729536508975[26] = 0; - out_3509679729536508975[27] = 0; - out_3509679729536508975[28] = 0; - out_3509679729536508975[29] = 0; - out_3509679729536508975[30] = 0; - out_3509679729536508975[31] = 0; - out_3509679729536508975[32] = 0; - out_3509679729536508975[33] = 0; - out_3509679729536508975[34] = 0; - out_3509679729536508975[35] = 0; - out_3509679729536508975[36] = 0; - out_3509679729536508975[37] = 0; - out_3509679729536508975[38] = 0; - out_3509679729536508975[39] = 0; - out_3509679729536508975[40] = 0; - out_3509679729536508975[41] = 0; - out_3509679729536508975[42] = 0; - out_3509679729536508975[43] = 0; - out_3509679729536508975[44] = 1; - out_3509679729536508975[45] = 0; - out_3509679729536508975[46] = 0; - out_3509679729536508975[47] = 0; - out_3509679729536508975[48] = 0; - out_3509679729536508975[49] = 0; - out_3509679729536508975[50] = 0; - out_3509679729536508975[51] = 0; - out_3509679729536508975[52] = 0; - out_3509679729536508975[53] = 0; +void H_14(double *state, double *unused, double *out_3349973203036669) { + out_3349973203036669[0] = 0; + out_3349973203036669[1] = 0; + out_3349973203036669[2] = 0; + out_3349973203036669[3] = 0; + out_3349973203036669[4] = 0; + out_3349973203036669[5] = 0; + out_3349973203036669[6] = 1; + out_3349973203036669[7] = 0; + out_3349973203036669[8] = 0; + out_3349973203036669[9] = 0; + out_3349973203036669[10] = 0; + out_3349973203036669[11] = 0; + out_3349973203036669[12] = 0; + out_3349973203036669[13] = 0; + out_3349973203036669[14] = 0; + out_3349973203036669[15] = 0; + out_3349973203036669[16] = 0; + out_3349973203036669[17] = 0; + out_3349973203036669[18] = 0; + out_3349973203036669[19] = 0; + out_3349973203036669[20] = 0; + out_3349973203036669[21] = 0; + out_3349973203036669[22] = 0; + out_3349973203036669[23] = 0; + out_3349973203036669[24] = 0; + out_3349973203036669[25] = 1; + out_3349973203036669[26] = 0; + out_3349973203036669[27] = 0; + out_3349973203036669[28] = 0; + out_3349973203036669[29] = 0; + out_3349973203036669[30] = 0; + out_3349973203036669[31] = 0; + out_3349973203036669[32] = 0; + out_3349973203036669[33] = 0; + out_3349973203036669[34] = 0; + out_3349973203036669[35] = 0; + out_3349973203036669[36] = 0; + out_3349973203036669[37] = 0; + out_3349973203036669[38] = 0; + out_3349973203036669[39] = 0; + out_3349973203036669[40] = 0; + out_3349973203036669[41] = 0; + out_3349973203036669[42] = 0; + out_3349973203036669[43] = 0; + out_3349973203036669[44] = 1; + out_3349973203036669[45] = 0; + out_3349973203036669[46] = 0; + out_3349973203036669[47] = 0; + out_3349973203036669[48] = 0; + out_3349973203036669[49] = 0; + out_3349973203036669[50] = 0; + out_3349973203036669[51] = 0; + out_3349973203036669[52] = 0; + out_3349973203036669[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_4940663026929446750) { - err_fun(nom_x, delta_x, out_4940663026929446750); +void pose_err_fun(double *nom_x, double *delta_x, double *out_3212432162194349052) { + err_fun(nom_x, delta_x, out_3212432162194349052); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1207218016783825442) { - inv_err_fun(nom_x, true_x, out_1207218016783825442); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8973271523308122061) { + inv_err_fun(nom_x, true_x, out_8973271523308122061); } -void pose_H_mod_fun(double *state, double *out_2820660951987973699) { - H_mod_fun(state, out_2820660951987973699); +void pose_H_mod_fun(double *state, double *out_7731698092980355432) { + H_mod_fun(state, out_7731698092980355432); } -void pose_f_fun(double *state, double dt, double *out_2310584025056258416) { - f_fun(state, dt, out_2310584025056258416); +void pose_f_fun(double *state, double dt, double *out_8923016311880444304) { + f_fun(state, dt, out_8923016311880444304); } -void pose_F_fun(double *state, double dt, double *out_6557190266781458971) { - F_fun(state, dt, out_6557190266781458971); +void pose_F_fun(double *state, double dt, double *out_210116950988546947) { + F_fun(state, dt, out_210116950988546947); } -void pose_h_4(double *state, double *unused, double *out_8177424571477984765) { - h_4(state, unused, out_8177424571477984765); +void pose_h_4(double *state, double *unused, double *out_7648624411257127562) { + h_4(state, unused, out_7648624411257127562); } -void pose_H_4(double *state, double *unused, double *out_6592468161831881271) { - H_4(state, unused, out_6592468161831881271); +void pose_H_4(double *state, double *unused, double *out_3959890883136447860) { + H_4(state, unused, out_3959890883136447860); } -void pose_h_10(double *state, double *unused, double *out_4220186743788310408) { - h_10(state, unused, out_4220186743788310408); +void pose_h_10(double *state, double *unused, double *out_6099133755902511145) { + h_10(state, unused, out_6099133755902511145); } -void pose_H_10(double *state, double *unused, double *out_2494414403690571241) { - H_10(state, unused, out_2494414403690571241); +void pose_H_10(double *state, double *unused, double *out_2569809615037315587) { + H_10(state, unused, out_2569809615037315587); } -void pose_h_13(double *state, double *unused, double *out_5780074361933493057) { - h_13(state, unused, out_5780074361933493057); +void pose_h_13(double *state, double *unused, double *out_2653699383913433253) { + h_13(state, unused, out_2653699383913433253); } -void pose_H_13(double *state, double *unused, double *out_8642002086545337544) { - H_13(state, unused, out_8642002086545337544); +void pose_H_13(double *state, double *unused, double *out_747617057804115059) { + H_13(state, unused, out_747617057804115059); } -void pose_h_14(double *state, double *unused, double *out_3473232613379301184) { - h_14(state, unused, out_3473232613379301184); +void pose_h_14(double *state, double *unused, double *out_7344654912082976031) { + h_14(state, unused, out_7344654912082976031); } -void pose_H_14(double *state, double *unused, double *out_3509679729536508975) { - H_14(state, unused, out_3509679729536508975); +void pose_H_14(double *state, double *unused, double *out_3349973203036669) { + H_14(state, unused, out_3349973203036669); } 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 7637a03f3..7d8159191 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_4940663026929446750); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1207218016783825442); -void pose_H_mod_fun(double *state, double *out_2820660951987973699); -void pose_f_fun(double *state, double dt, double *out_2310584025056258416); -void pose_F_fun(double *state, double dt, double *out_6557190266781458971); -void pose_h_4(double *state, double *unused, double *out_8177424571477984765); -void pose_H_4(double *state, double *unused, double *out_6592468161831881271); -void pose_h_10(double *state, double *unused, double *out_4220186743788310408); -void pose_H_10(double *state, double *unused, double *out_2494414403690571241); -void pose_h_13(double *state, double *unused, double *out_5780074361933493057); -void pose_H_13(double *state, double *unused, double *out_8642002086545337544); -void pose_h_14(double *state, double *unused, double *out_3473232613379301184); -void pose_H_14(double *state, double *unused, double *out_3509679729536508975); +void pose_err_fun(double *nom_x, double *delta_x, double *out_3212432162194349052); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8973271523308122061); +void pose_H_mod_fun(double *state, double *out_7731698092980355432); +void pose_f_fun(double *state, double dt, double *out_8923016311880444304); +void pose_F_fun(double *state, double dt, double *out_210116950988546947); +void pose_h_4(double *state, double *unused, double *out_7648624411257127562); +void pose_H_4(double *state, double *unused, double *out_3959890883136447860); +void pose_h_10(double *state, double *unused, double *out_6099133755902511145); +void pose_H_10(double *state, double *unused, double *out_2569809615037315587); +void pose_h_13(double *state, double *unused, double *out_2653699383913433253); +void pose_H_13(double *state, double *unused, double *out_747617057804115059); +void pose_h_14(double *state, double *unused, double *out_7344654912082976031); +void pose_H_14(double *state, double *unused, double *out_3349973203036669); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index d03843660..069250970 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 7a0c9ccd5..ccda39c52 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 3cf9b0c43..0f997353a 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 90da6be9d..63cbebcc7 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index a047bdbb3..55ff8c610 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 2c11d3ec2..a9c4e3fbd 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ