diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 893be392d..5c10d0b22 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 21b9b4a36..e626c777b 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 5d2ff8fef..ca109e506 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 dd11de06f..45c767c15 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 499aee117..d4248baa4 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 336168e81..689edce8f 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 1bb58d3aa..40bd738a4 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 1bb58d3aa..40bd738a4 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 223cfea79..2209872be 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 336168e81..689edce8f 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 b34ebbadb..d3efc2a8a 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-9872c5fb-DEBUG"; +const uint8_t gitversion[19] = "DEV-4254090c-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 734ad0752..fe44ee969 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 67b23febf..c9d17bea4 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 49d284242..f2f76af0d 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 8b53fad86..513bf77af 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 55afd931c..70689eb3d 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 bc340c1aa..beedef32e 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 1b2870103..94936d129 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 67a6e0d91..1a61fd33a 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 2c95b9e59..736950f66 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 d268ca875..7ad868e22 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 b6279c376..5ca99a3f7 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 3f8414200..16ed3fb16 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 010541fa1..dd6036a7f 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 caff4ac4a..463ee9ed0 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 2c381d651..0d1c24a07 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 ffdba6f67..a386295e5 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 658c30a3e..602e53e77 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 6581a2d9d..d323db13e 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 a06ea42a7..c939135b7 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 48866407a..c32283703 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 806424228..265e3df67 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-9872c5fb-DEBUG \ No newline at end of file +DEV-4254090c-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 372c81ac9..7de48ab80 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_8928034227051207060) { - out_8928034227051207060[0] = delta_x[0] + nom_x[0]; - out_8928034227051207060[1] = delta_x[1] + nom_x[1]; - out_8928034227051207060[2] = delta_x[2] + nom_x[2]; - out_8928034227051207060[3] = delta_x[3] + nom_x[3]; - out_8928034227051207060[4] = delta_x[4] + nom_x[4]; - out_8928034227051207060[5] = delta_x[5] + nom_x[5]; - out_8928034227051207060[6] = delta_x[6] + nom_x[6]; - out_8928034227051207060[7] = delta_x[7] + nom_x[7]; - out_8928034227051207060[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_8796389704996072171) { + out_8796389704996072171[0] = delta_x[0] + nom_x[0]; + out_8796389704996072171[1] = delta_x[1] + nom_x[1]; + out_8796389704996072171[2] = delta_x[2] + nom_x[2]; + out_8796389704996072171[3] = delta_x[3] + nom_x[3]; + out_8796389704996072171[4] = delta_x[4] + nom_x[4]; + out_8796389704996072171[5] = delta_x[5] + nom_x[5]; + out_8796389704996072171[6] = delta_x[6] + nom_x[6]; + out_8796389704996072171[7] = delta_x[7] + nom_x[7]; + out_8796389704996072171[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4996368579389837639) { - out_4996368579389837639[0] = -nom_x[0] + true_x[0]; - out_4996368579389837639[1] = -nom_x[1] + true_x[1]; - out_4996368579389837639[2] = -nom_x[2] + true_x[2]; - out_4996368579389837639[3] = -nom_x[3] + true_x[3]; - out_4996368579389837639[4] = -nom_x[4] + true_x[4]; - out_4996368579389837639[5] = -nom_x[5] + true_x[5]; - out_4996368579389837639[6] = -nom_x[6] + true_x[6]; - out_4996368579389837639[7] = -nom_x[7] + true_x[7]; - out_4996368579389837639[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8849207833158339340) { + out_8849207833158339340[0] = -nom_x[0] + true_x[0]; + out_8849207833158339340[1] = -nom_x[1] + true_x[1]; + out_8849207833158339340[2] = -nom_x[2] + true_x[2]; + out_8849207833158339340[3] = -nom_x[3] + true_x[3]; + out_8849207833158339340[4] = -nom_x[4] + true_x[4]; + out_8849207833158339340[5] = -nom_x[5] + true_x[5]; + out_8849207833158339340[6] = -nom_x[6] + true_x[6]; + out_8849207833158339340[7] = -nom_x[7] + true_x[7]; + out_8849207833158339340[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_8354260345920403618) { - out_8354260345920403618[0] = 1.0; - out_8354260345920403618[1] = 0.0; - out_8354260345920403618[2] = 0.0; - out_8354260345920403618[3] = 0.0; - out_8354260345920403618[4] = 0.0; - out_8354260345920403618[5] = 0.0; - out_8354260345920403618[6] = 0.0; - out_8354260345920403618[7] = 0.0; - out_8354260345920403618[8] = 0.0; - out_8354260345920403618[9] = 0.0; - out_8354260345920403618[10] = 1.0; - out_8354260345920403618[11] = 0.0; - out_8354260345920403618[12] = 0.0; - out_8354260345920403618[13] = 0.0; - out_8354260345920403618[14] = 0.0; - out_8354260345920403618[15] = 0.0; - out_8354260345920403618[16] = 0.0; - out_8354260345920403618[17] = 0.0; - out_8354260345920403618[18] = 0.0; - out_8354260345920403618[19] = 0.0; - out_8354260345920403618[20] = 1.0; - out_8354260345920403618[21] = 0.0; - out_8354260345920403618[22] = 0.0; - out_8354260345920403618[23] = 0.0; - out_8354260345920403618[24] = 0.0; - out_8354260345920403618[25] = 0.0; - out_8354260345920403618[26] = 0.0; - out_8354260345920403618[27] = 0.0; - out_8354260345920403618[28] = 0.0; - out_8354260345920403618[29] = 0.0; - out_8354260345920403618[30] = 1.0; - out_8354260345920403618[31] = 0.0; - out_8354260345920403618[32] = 0.0; - out_8354260345920403618[33] = 0.0; - out_8354260345920403618[34] = 0.0; - out_8354260345920403618[35] = 0.0; - out_8354260345920403618[36] = 0.0; - out_8354260345920403618[37] = 0.0; - out_8354260345920403618[38] = 0.0; - out_8354260345920403618[39] = 0.0; - out_8354260345920403618[40] = 1.0; - out_8354260345920403618[41] = 0.0; - out_8354260345920403618[42] = 0.0; - out_8354260345920403618[43] = 0.0; - out_8354260345920403618[44] = 0.0; - out_8354260345920403618[45] = 0.0; - out_8354260345920403618[46] = 0.0; - out_8354260345920403618[47] = 0.0; - out_8354260345920403618[48] = 0.0; - out_8354260345920403618[49] = 0.0; - out_8354260345920403618[50] = 1.0; - out_8354260345920403618[51] = 0.0; - out_8354260345920403618[52] = 0.0; - out_8354260345920403618[53] = 0.0; - out_8354260345920403618[54] = 0.0; - out_8354260345920403618[55] = 0.0; - out_8354260345920403618[56] = 0.0; - out_8354260345920403618[57] = 0.0; - out_8354260345920403618[58] = 0.0; - out_8354260345920403618[59] = 0.0; - out_8354260345920403618[60] = 1.0; - out_8354260345920403618[61] = 0.0; - out_8354260345920403618[62] = 0.0; - out_8354260345920403618[63] = 0.0; - out_8354260345920403618[64] = 0.0; - out_8354260345920403618[65] = 0.0; - out_8354260345920403618[66] = 0.0; - out_8354260345920403618[67] = 0.0; - out_8354260345920403618[68] = 0.0; - out_8354260345920403618[69] = 0.0; - out_8354260345920403618[70] = 1.0; - out_8354260345920403618[71] = 0.0; - out_8354260345920403618[72] = 0.0; - out_8354260345920403618[73] = 0.0; - out_8354260345920403618[74] = 0.0; - out_8354260345920403618[75] = 0.0; - out_8354260345920403618[76] = 0.0; - out_8354260345920403618[77] = 0.0; - out_8354260345920403618[78] = 0.0; - out_8354260345920403618[79] = 0.0; - out_8354260345920403618[80] = 1.0; +void H_mod_fun(double *state, double *out_1622786580378488408) { + out_1622786580378488408[0] = 1.0; + out_1622786580378488408[1] = 0.0; + out_1622786580378488408[2] = 0.0; + out_1622786580378488408[3] = 0.0; + out_1622786580378488408[4] = 0.0; + out_1622786580378488408[5] = 0.0; + out_1622786580378488408[6] = 0.0; + out_1622786580378488408[7] = 0.0; + out_1622786580378488408[8] = 0.0; + out_1622786580378488408[9] = 0.0; + out_1622786580378488408[10] = 1.0; + out_1622786580378488408[11] = 0.0; + out_1622786580378488408[12] = 0.0; + out_1622786580378488408[13] = 0.0; + out_1622786580378488408[14] = 0.0; + out_1622786580378488408[15] = 0.0; + out_1622786580378488408[16] = 0.0; + out_1622786580378488408[17] = 0.0; + out_1622786580378488408[18] = 0.0; + out_1622786580378488408[19] = 0.0; + out_1622786580378488408[20] = 1.0; + out_1622786580378488408[21] = 0.0; + out_1622786580378488408[22] = 0.0; + out_1622786580378488408[23] = 0.0; + out_1622786580378488408[24] = 0.0; + out_1622786580378488408[25] = 0.0; + out_1622786580378488408[26] = 0.0; + out_1622786580378488408[27] = 0.0; + out_1622786580378488408[28] = 0.0; + out_1622786580378488408[29] = 0.0; + out_1622786580378488408[30] = 1.0; + out_1622786580378488408[31] = 0.0; + out_1622786580378488408[32] = 0.0; + out_1622786580378488408[33] = 0.0; + out_1622786580378488408[34] = 0.0; + out_1622786580378488408[35] = 0.0; + out_1622786580378488408[36] = 0.0; + out_1622786580378488408[37] = 0.0; + out_1622786580378488408[38] = 0.0; + out_1622786580378488408[39] = 0.0; + out_1622786580378488408[40] = 1.0; + out_1622786580378488408[41] = 0.0; + out_1622786580378488408[42] = 0.0; + out_1622786580378488408[43] = 0.0; + out_1622786580378488408[44] = 0.0; + out_1622786580378488408[45] = 0.0; + out_1622786580378488408[46] = 0.0; + out_1622786580378488408[47] = 0.0; + out_1622786580378488408[48] = 0.0; + out_1622786580378488408[49] = 0.0; + out_1622786580378488408[50] = 1.0; + out_1622786580378488408[51] = 0.0; + out_1622786580378488408[52] = 0.0; + out_1622786580378488408[53] = 0.0; + out_1622786580378488408[54] = 0.0; + out_1622786580378488408[55] = 0.0; + out_1622786580378488408[56] = 0.0; + out_1622786580378488408[57] = 0.0; + out_1622786580378488408[58] = 0.0; + out_1622786580378488408[59] = 0.0; + out_1622786580378488408[60] = 1.0; + out_1622786580378488408[61] = 0.0; + out_1622786580378488408[62] = 0.0; + out_1622786580378488408[63] = 0.0; + out_1622786580378488408[64] = 0.0; + out_1622786580378488408[65] = 0.0; + out_1622786580378488408[66] = 0.0; + out_1622786580378488408[67] = 0.0; + out_1622786580378488408[68] = 0.0; + out_1622786580378488408[69] = 0.0; + out_1622786580378488408[70] = 1.0; + out_1622786580378488408[71] = 0.0; + out_1622786580378488408[72] = 0.0; + out_1622786580378488408[73] = 0.0; + out_1622786580378488408[74] = 0.0; + out_1622786580378488408[75] = 0.0; + out_1622786580378488408[76] = 0.0; + out_1622786580378488408[77] = 0.0; + out_1622786580378488408[78] = 0.0; + out_1622786580378488408[79] = 0.0; + out_1622786580378488408[80] = 1.0; } -void f_fun(double *state, double dt, double *out_18975094040046356) { - out_18975094040046356[0] = state[0]; - out_18975094040046356[1] = state[1]; - out_18975094040046356[2] = state[2]; - out_18975094040046356[3] = state[3]; - out_18975094040046356[4] = state[4]; - out_18975094040046356[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_18975094040046356[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_18975094040046356[7] = state[7]; - out_18975094040046356[8] = state[8]; +void f_fun(double *state, double dt, double *out_1981095305174081329) { + out_1981095305174081329[0] = state[0]; + out_1981095305174081329[1] = state[1]; + out_1981095305174081329[2] = state[2]; + out_1981095305174081329[3] = state[3]; + out_1981095305174081329[4] = state[4]; + out_1981095305174081329[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_1981095305174081329[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_1981095305174081329[7] = state[7]; + out_1981095305174081329[8] = state[8]; } -void F_fun(double *state, double dt, double *out_8298920985484652828) { - out_8298920985484652828[0] = 1; - out_8298920985484652828[1] = 0; - out_8298920985484652828[2] = 0; - out_8298920985484652828[3] = 0; - out_8298920985484652828[4] = 0; - out_8298920985484652828[5] = 0; - out_8298920985484652828[6] = 0; - out_8298920985484652828[7] = 0; - out_8298920985484652828[8] = 0; - out_8298920985484652828[9] = 0; - out_8298920985484652828[10] = 1; - out_8298920985484652828[11] = 0; - out_8298920985484652828[12] = 0; - out_8298920985484652828[13] = 0; - out_8298920985484652828[14] = 0; - out_8298920985484652828[15] = 0; - out_8298920985484652828[16] = 0; - out_8298920985484652828[17] = 0; - out_8298920985484652828[18] = 0; - out_8298920985484652828[19] = 0; - out_8298920985484652828[20] = 1; - out_8298920985484652828[21] = 0; - out_8298920985484652828[22] = 0; - out_8298920985484652828[23] = 0; - out_8298920985484652828[24] = 0; - out_8298920985484652828[25] = 0; - out_8298920985484652828[26] = 0; - out_8298920985484652828[27] = 0; - out_8298920985484652828[28] = 0; - out_8298920985484652828[29] = 0; - out_8298920985484652828[30] = 1; - out_8298920985484652828[31] = 0; - out_8298920985484652828[32] = 0; - out_8298920985484652828[33] = 0; - out_8298920985484652828[34] = 0; - out_8298920985484652828[35] = 0; - out_8298920985484652828[36] = 0; - out_8298920985484652828[37] = 0; - out_8298920985484652828[38] = 0; - out_8298920985484652828[39] = 0; - out_8298920985484652828[40] = 1; - out_8298920985484652828[41] = 0; - out_8298920985484652828[42] = 0; - out_8298920985484652828[43] = 0; - out_8298920985484652828[44] = 0; - out_8298920985484652828[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_8298920985484652828[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_8298920985484652828[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8298920985484652828[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8298920985484652828[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_8298920985484652828[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_8298920985484652828[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_8298920985484652828[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_8298920985484652828[53] = -9.8100000000000005*dt; - out_8298920985484652828[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_8298920985484652828[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_8298920985484652828[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8298920985484652828[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8298920985484652828[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_8298920985484652828[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_8298920985484652828[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_8298920985484652828[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8298920985484652828[62] = 0; - out_8298920985484652828[63] = 0; - out_8298920985484652828[64] = 0; - out_8298920985484652828[65] = 0; - out_8298920985484652828[66] = 0; - out_8298920985484652828[67] = 0; - out_8298920985484652828[68] = 0; - out_8298920985484652828[69] = 0; - out_8298920985484652828[70] = 1; - out_8298920985484652828[71] = 0; - out_8298920985484652828[72] = 0; - out_8298920985484652828[73] = 0; - out_8298920985484652828[74] = 0; - out_8298920985484652828[75] = 0; - out_8298920985484652828[76] = 0; - out_8298920985484652828[77] = 0; - out_8298920985484652828[78] = 0; - out_8298920985484652828[79] = 0; - out_8298920985484652828[80] = 1; +void F_fun(double *state, double dt, double *out_3561550856242897713) { + out_3561550856242897713[0] = 1; + out_3561550856242897713[1] = 0; + out_3561550856242897713[2] = 0; + out_3561550856242897713[3] = 0; + out_3561550856242897713[4] = 0; + out_3561550856242897713[5] = 0; + out_3561550856242897713[6] = 0; + out_3561550856242897713[7] = 0; + out_3561550856242897713[8] = 0; + out_3561550856242897713[9] = 0; + out_3561550856242897713[10] = 1; + out_3561550856242897713[11] = 0; + out_3561550856242897713[12] = 0; + out_3561550856242897713[13] = 0; + out_3561550856242897713[14] = 0; + out_3561550856242897713[15] = 0; + out_3561550856242897713[16] = 0; + out_3561550856242897713[17] = 0; + out_3561550856242897713[18] = 0; + out_3561550856242897713[19] = 0; + out_3561550856242897713[20] = 1; + out_3561550856242897713[21] = 0; + out_3561550856242897713[22] = 0; + out_3561550856242897713[23] = 0; + out_3561550856242897713[24] = 0; + out_3561550856242897713[25] = 0; + out_3561550856242897713[26] = 0; + out_3561550856242897713[27] = 0; + out_3561550856242897713[28] = 0; + out_3561550856242897713[29] = 0; + out_3561550856242897713[30] = 1; + out_3561550856242897713[31] = 0; + out_3561550856242897713[32] = 0; + out_3561550856242897713[33] = 0; + out_3561550856242897713[34] = 0; + out_3561550856242897713[35] = 0; + out_3561550856242897713[36] = 0; + out_3561550856242897713[37] = 0; + out_3561550856242897713[38] = 0; + out_3561550856242897713[39] = 0; + out_3561550856242897713[40] = 1; + out_3561550856242897713[41] = 0; + out_3561550856242897713[42] = 0; + out_3561550856242897713[43] = 0; + out_3561550856242897713[44] = 0; + out_3561550856242897713[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_3561550856242897713[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_3561550856242897713[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3561550856242897713[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3561550856242897713[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_3561550856242897713[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_3561550856242897713[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_3561550856242897713[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_3561550856242897713[53] = -9.8100000000000005*dt; + out_3561550856242897713[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_3561550856242897713[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_3561550856242897713[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3561550856242897713[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3561550856242897713[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_3561550856242897713[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_3561550856242897713[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_3561550856242897713[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3561550856242897713[62] = 0; + out_3561550856242897713[63] = 0; + out_3561550856242897713[64] = 0; + out_3561550856242897713[65] = 0; + out_3561550856242897713[66] = 0; + out_3561550856242897713[67] = 0; + out_3561550856242897713[68] = 0; + out_3561550856242897713[69] = 0; + out_3561550856242897713[70] = 1; + out_3561550856242897713[71] = 0; + out_3561550856242897713[72] = 0; + out_3561550856242897713[73] = 0; + out_3561550856242897713[74] = 0; + out_3561550856242897713[75] = 0; + out_3561550856242897713[76] = 0; + out_3561550856242897713[77] = 0; + out_3561550856242897713[78] = 0; + out_3561550856242897713[79] = 0; + out_3561550856242897713[80] = 1; } -void h_25(double *state, double *unused, double *out_3416906007265727326) { - out_3416906007265727326[0] = state[6]; +void h_25(double *state, double *unused, double *out_3280789364307995853) { + out_3280789364307995853[0] = state[6]; } -void H_25(double *state, double *unused, double *out_6569503077799656321) { - out_6569503077799656321[0] = 0; - out_6569503077799656321[1] = 0; - out_6569503077799656321[2] = 0; - out_6569503077799656321[3] = 0; - out_6569503077799656321[4] = 0; - out_6569503077799656321[5] = 0; - out_6569503077799656321[6] = 1; - out_6569503077799656321[7] = 0; - out_6569503077799656321[8] = 0; +void H_25(double *state, double *unused, double *out_445738705382224348) { + out_445738705382224348[0] = 0; + out_445738705382224348[1] = 0; + out_445738705382224348[2] = 0; + out_445738705382224348[3] = 0; + out_445738705382224348[4] = 0; + out_445738705382224348[5] = 0; + out_445738705382224348[6] = 1; + out_445738705382224348[7] = 0; + out_445738705382224348[8] = 0; } -void h_24(double *state, double *unused, double *out_6593726717602388151) { - out_6593726717602388151[0] = state[4]; - out_6593726717602388151[1] = state[5]; +void h_24(double *state, double *unused, double *out_5100775928185398157) { + out_5100775928185398157[0] = state[4]; + out_5100775928185398157[1] = state[5]; } -void H_24(double *state, double *unused, double *out_3011577478997333185) { - out_3011577478997333185[0] = 0; - out_3011577478997333185[1] = 0; - out_3011577478997333185[2] = 0; - out_3011577478997333185[3] = 0; - out_3011577478997333185[4] = 1; - out_3011577478997333185[5] = 0; - out_3011577478997333185[6] = 0; - out_3011577478997333185[7] = 0; - out_3011577478997333185[8] = 0; - out_3011577478997333185[9] = 0; - out_3011577478997333185[10] = 0; - out_3011577478997333185[11] = 0; - out_3011577478997333185[12] = 0; - out_3011577478997333185[13] = 0; - out_3011577478997333185[14] = 1; - out_3011577478997333185[15] = 0; - out_3011577478997333185[16] = 0; - out_3011577478997333185[17] = 0; +void H_24(double *state, double *unused, double *out_399257903401471051) { + out_399257903401471051[0] = 0; + out_399257903401471051[1] = 0; + out_399257903401471051[2] = 0; + out_399257903401471051[3] = 0; + out_399257903401471051[4] = 1; + out_399257903401471051[5] = 0; + out_399257903401471051[6] = 0; + out_399257903401471051[7] = 0; + out_399257903401471051[8] = 0; + out_399257903401471051[9] = 0; + out_399257903401471051[10] = 0; + out_399257903401471051[11] = 0; + out_399257903401471051[12] = 0; + out_399257903401471051[13] = 0; + out_399257903401471051[14] = 1; + out_399257903401471051[15] = 0; + out_399257903401471051[16] = 0; + out_399257903401471051[17] = 0; } -void h_30(double *state, double *unused, double *out_4267886705157640301) { - out_4267886705157640301[0] = state[4]; +void h_30(double *state, double *unused, double *out_3331749478418022440) { + out_3331749478418022440[0] = state[4]; } -void H_30(double *state, double *unused, double *out_6698842024942896391) { - out_6698842024942896391[0] = 0; - out_6698842024942896391[1] = 0; - out_6698842024942896391[2] = 0; - out_6698842024942896391[3] = 0; - out_6698842024942896391[4] = 1; - out_6698842024942896391[5] = 0; - out_6698842024942896391[6] = 0; - out_6698842024942896391[7] = 0; - out_6698842024942896391[8] = 0; +void H_30(double *state, double *unused, double *out_2072594253125024279) { + out_2072594253125024279[0] = 0; + out_2072594253125024279[1] = 0; + out_2072594253125024279[2] = 0; + out_2072594253125024279[3] = 0; + out_2072594253125024279[4] = 1; + out_2072594253125024279[5] = 0; + out_2072594253125024279[6] = 0; + out_2072594253125024279[7] = 0; + out_2072594253125024279[8] = 0; } -void h_26(double *state, double *unused, double *out_9177184100891065670) { - out_9177184100891065670[0] = state[7]; +void h_26(double *state, double *unused, double *out_1066202602863606572) { + out_1066202602863606572[0] = state[7]; } -void H_26(double *state, double *unused, double *out_8135737677035839071) { - out_8135737677035839071[0] = 0; - out_8135737677035839071[1] = 0; - out_8135737677035839071[2] = 0; - out_8135737677035839071[3] = 0; - out_8135737677035839071[4] = 0; - out_8135737677035839071[5] = 0; - out_8135737677035839071[6] = 0; - out_8135737677035839071[7] = 1; - out_8135737677035839071[8] = 0; +void H_26(double *state, double *unused, double *out_4187242024256280572) { + out_4187242024256280572[0] = 0; + out_4187242024256280572[1] = 0; + out_4187242024256280572[2] = 0; + out_4187242024256280572[3] = 0; + out_4187242024256280572[4] = 0; + out_4187242024256280572[5] = 0; + out_4187242024256280572[6] = 0; + out_4187242024256280572[7] = 1; + out_4187242024256280572[8] = 0; } -void h_27(double *state, double *unused, double *out_6233528602177827150) { - out_6233528602177827150[0] = state[3]; +void h_27(double *state, double *unused, double *out_3118252153105853293) { + out_3118252153105853293[0] = state[3]; } -void H_27(double *state, double *unused, double *out_8873605336743321302) { - out_8873605336743321302[0] = 0; - out_8873605336743321302[1] = 0; - out_8873605336743321302[2] = 0; - out_8873605336743321302[3] = 1; - out_8873605336743321302[4] = 0; - out_8873605336743321302[5] = 0; - out_8873605336743321302[6] = 0; - out_8873605336743321302[7] = 0; - out_8873605336743321302[8] = 0; +void H_27(double *state, double *unused, double *out_4296188324308967496) { + out_4296188324308967496[0] = 0; + out_4296188324308967496[1] = 0; + out_4296188324308967496[2] = 0; + out_4296188324308967496[3] = 1; + out_4296188324308967496[4] = 0; + out_4296188324308967496[5] = 0; + out_4296188324308967496[6] = 0; + out_4296188324308967496[7] = 0; + out_4296188324308967496[8] = 0; } -void h_29(double *state, double *unused, double *out_3440336872306683482) { - out_3440336872306683482[0] = state[1]; +void h_29(double *state, double *unused, double *out_8557656694253347387) { + out_8557656694253347387[0] = state[1]; } -void H_29(double *state, double *unused, double *out_6188610680628504207) { - out_6188610680628504207[0] = 0; - out_6188610680628504207[1] = 1; - out_6188610680628504207[2] = 0; - out_6188610680628504207[3] = 0; - out_6188610680628504207[4] = 0; - out_6188610680628504207[5] = 0; - out_6188610680628504207[6] = 0; - out_6188610680628504207[7] = 0; - out_6188610680628504207[8] = 0; +void H_29(double *state, double *unused, double *out_2582825597439416463) { + out_2582825597439416463[0] = 0; + out_2582825597439416463[1] = 1; + out_2582825597439416463[2] = 0; + out_2582825597439416463[3] = 0; + out_2582825597439416463[4] = 0; + out_2582825597439416463[5] = 0; + out_2582825597439416463[6] = 0; + out_2582825597439416463[7] = 0; + out_2582825597439416463[8] = 0; } -void h_28(double *state, double *unused, double *out_2162754552967525717) { - out_2162754552967525717[0] = state[0]; +void h_28(double *state, double *unused, double *out_4285995954181995030) { + out_4285995954181995030[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2777376993027148707) { - out_2777376993027148707[0] = 1; - out_2777376993027148707[1] = 0; - out_2777376993027148707[2] = 0; - out_2777376993027148707[3] = 0; - out_2777376993027148707[4] = 0; - out_2777376993027148707[5] = 0; - out_2777376993027148707[6] = 0; - out_2777376993027148707[7] = 0; - out_2777376993027148707[8] = 0; +void H_28(double *state, double *unused, double *out_2499573419630114111) { + out_2499573419630114111[0] = 1; + out_2499573419630114111[1] = 0; + out_2499573419630114111[2] = 0; + out_2499573419630114111[3] = 0; + out_2499573419630114111[4] = 0; + out_2499573419630114111[5] = 0; + out_2499573419630114111[6] = 0; + out_2499573419630114111[7] = 0; + out_2499573419630114111[8] = 0; } -void h_31(double *state, double *unused, double *out_8180530295696168540) { - out_8180530295696168540[0] = state[8]; +void h_31(double *state, double *unused, double *out_6669294028137815771) { + out_6669294028137815771[0] = state[8]; } -void H_31(double *state, double *unused, double *out_6538857115922695893) { - out_6538857115922695893[0] = 0; - out_6538857115922695893[1] = 0; - out_6538857115922695893[2] = 0; - out_6538857115922695893[3] = 0; - out_6538857115922695893[4] = 0; - out_6538857115922695893[5] = 0; - out_6538857115922695893[6] = 0; - out_6538857115922695893[7] = 0; - out_6538857115922695893[8] = 1; +void H_31(double *state, double *unused, double *out_415092743505263920) { + out_415092743505263920[0] = 0; + out_415092743505263920[1] = 0; + out_415092743505263920[2] = 0; + out_415092743505263920[3] = 0; + out_415092743505263920[4] = 0; + out_415092743505263920[5] = 0; + out_415092743505263920[6] = 0; + out_415092743505263920[7] = 0; + out_415092743505263920[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_8928034227051207060) { - err_fun(nom_x, delta_x, out_8928034227051207060); +void car_err_fun(double *nom_x, double *delta_x, double *out_8796389704996072171) { + err_fun(nom_x, delta_x, out_8796389704996072171); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4996368579389837639) { - inv_err_fun(nom_x, true_x, out_4996368579389837639); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8849207833158339340) { + inv_err_fun(nom_x, true_x, out_8849207833158339340); } -void car_H_mod_fun(double *state, double *out_8354260345920403618) { - H_mod_fun(state, out_8354260345920403618); +void car_H_mod_fun(double *state, double *out_1622786580378488408) { + H_mod_fun(state, out_1622786580378488408); } -void car_f_fun(double *state, double dt, double *out_18975094040046356) { - f_fun(state, dt, out_18975094040046356); +void car_f_fun(double *state, double dt, double *out_1981095305174081329) { + f_fun(state, dt, out_1981095305174081329); } -void car_F_fun(double *state, double dt, double *out_8298920985484652828) { - F_fun(state, dt, out_8298920985484652828); +void car_F_fun(double *state, double dt, double *out_3561550856242897713) { + F_fun(state, dt, out_3561550856242897713); } -void car_h_25(double *state, double *unused, double *out_3416906007265727326) { - h_25(state, unused, out_3416906007265727326); +void car_h_25(double *state, double *unused, double *out_3280789364307995853) { + h_25(state, unused, out_3280789364307995853); } -void car_H_25(double *state, double *unused, double *out_6569503077799656321) { - H_25(state, unused, out_6569503077799656321); +void car_H_25(double *state, double *unused, double *out_445738705382224348) { + H_25(state, unused, out_445738705382224348); } -void car_h_24(double *state, double *unused, double *out_6593726717602388151) { - h_24(state, unused, out_6593726717602388151); +void car_h_24(double *state, double *unused, double *out_5100775928185398157) { + h_24(state, unused, out_5100775928185398157); } -void car_H_24(double *state, double *unused, double *out_3011577478997333185) { - H_24(state, unused, out_3011577478997333185); +void car_H_24(double *state, double *unused, double *out_399257903401471051) { + H_24(state, unused, out_399257903401471051); } -void car_h_30(double *state, double *unused, double *out_4267886705157640301) { - h_30(state, unused, out_4267886705157640301); +void car_h_30(double *state, double *unused, double *out_3331749478418022440) { + h_30(state, unused, out_3331749478418022440); } -void car_H_30(double *state, double *unused, double *out_6698842024942896391) { - H_30(state, unused, out_6698842024942896391); +void car_H_30(double *state, double *unused, double *out_2072594253125024279) { + H_30(state, unused, out_2072594253125024279); } -void car_h_26(double *state, double *unused, double *out_9177184100891065670) { - h_26(state, unused, out_9177184100891065670); +void car_h_26(double *state, double *unused, double *out_1066202602863606572) { + h_26(state, unused, out_1066202602863606572); } -void car_H_26(double *state, double *unused, double *out_8135737677035839071) { - H_26(state, unused, out_8135737677035839071); +void car_H_26(double *state, double *unused, double *out_4187242024256280572) { + H_26(state, unused, out_4187242024256280572); } -void car_h_27(double *state, double *unused, double *out_6233528602177827150) { - h_27(state, unused, out_6233528602177827150); +void car_h_27(double *state, double *unused, double *out_3118252153105853293) { + h_27(state, unused, out_3118252153105853293); } -void car_H_27(double *state, double *unused, double *out_8873605336743321302) { - H_27(state, unused, out_8873605336743321302); +void car_H_27(double *state, double *unused, double *out_4296188324308967496) { + H_27(state, unused, out_4296188324308967496); } -void car_h_29(double *state, double *unused, double *out_3440336872306683482) { - h_29(state, unused, out_3440336872306683482); +void car_h_29(double *state, double *unused, double *out_8557656694253347387) { + h_29(state, unused, out_8557656694253347387); } -void car_H_29(double *state, double *unused, double *out_6188610680628504207) { - H_29(state, unused, out_6188610680628504207); +void car_H_29(double *state, double *unused, double *out_2582825597439416463) { + H_29(state, unused, out_2582825597439416463); } -void car_h_28(double *state, double *unused, double *out_2162754552967525717) { - h_28(state, unused, out_2162754552967525717); +void car_h_28(double *state, double *unused, double *out_4285995954181995030) { + h_28(state, unused, out_4285995954181995030); } -void car_H_28(double *state, double *unused, double *out_2777376993027148707) { - H_28(state, unused, out_2777376993027148707); +void car_H_28(double *state, double *unused, double *out_2499573419630114111) { + H_28(state, unused, out_2499573419630114111); } -void car_h_31(double *state, double *unused, double *out_8180530295696168540) { - h_31(state, unused, out_8180530295696168540); +void car_h_31(double *state, double *unused, double *out_6669294028137815771) { + h_31(state, unused, out_6669294028137815771); } -void car_H_31(double *state, double *unused, double *out_6538857115922695893) { - H_31(state, unused, out_6538857115922695893); +void car_H_31(double *state, double *unused, double *out_415092743505263920) { + H_31(state, unused, out_415092743505263920); } 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 7ece7960f..5cc4e3315 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_8928034227051207060); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4996368579389837639); -void car_H_mod_fun(double *state, double *out_8354260345920403618); -void car_f_fun(double *state, double dt, double *out_18975094040046356); -void car_F_fun(double *state, double dt, double *out_8298920985484652828); -void car_h_25(double *state, double *unused, double *out_3416906007265727326); -void car_H_25(double *state, double *unused, double *out_6569503077799656321); -void car_h_24(double *state, double *unused, double *out_6593726717602388151); -void car_H_24(double *state, double *unused, double *out_3011577478997333185); -void car_h_30(double *state, double *unused, double *out_4267886705157640301); -void car_H_30(double *state, double *unused, double *out_6698842024942896391); -void car_h_26(double *state, double *unused, double *out_9177184100891065670); -void car_H_26(double *state, double *unused, double *out_8135737677035839071); -void car_h_27(double *state, double *unused, double *out_6233528602177827150); -void car_H_27(double *state, double *unused, double *out_8873605336743321302); -void car_h_29(double *state, double *unused, double *out_3440336872306683482); -void car_H_29(double *state, double *unused, double *out_6188610680628504207); -void car_h_28(double *state, double *unused, double *out_2162754552967525717); -void car_H_28(double *state, double *unused, double *out_2777376993027148707); -void car_h_31(double *state, double *unused, double *out_8180530295696168540); -void car_H_31(double *state, double *unused, double *out_6538857115922695893); +void car_err_fun(double *nom_x, double *delta_x, double *out_8796389704996072171); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8849207833158339340); +void car_H_mod_fun(double *state, double *out_1622786580378488408); +void car_f_fun(double *state, double dt, double *out_1981095305174081329); +void car_F_fun(double *state, double dt, double *out_3561550856242897713); +void car_h_25(double *state, double *unused, double *out_3280789364307995853); +void car_H_25(double *state, double *unused, double *out_445738705382224348); +void car_h_24(double *state, double *unused, double *out_5100775928185398157); +void car_H_24(double *state, double *unused, double *out_399257903401471051); +void car_h_30(double *state, double *unused, double *out_3331749478418022440); +void car_H_30(double *state, double *unused, double *out_2072594253125024279); +void car_h_26(double *state, double *unused, double *out_1066202602863606572); +void car_H_26(double *state, double *unused, double *out_4187242024256280572); +void car_h_27(double *state, double *unused, double *out_3118252153105853293); +void car_H_27(double *state, double *unused, double *out_4296188324308967496); +void car_h_29(double *state, double *unused, double *out_8557656694253347387); +void car_H_29(double *state, double *unused, double *out_2582825597439416463); +void car_h_28(double *state, double *unused, double *out_4285995954181995030); +void car_H_28(double *state, double *unused, double *out_2499573419630114111); +void car_h_31(double *state, double *unused, double *out_6669294028137815771); +void car_H_31(double *state, double *unused, double *out_415092743505263920); 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 264cf9a2a..bb03e9348 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_6009088715125097596) { - out_6009088715125097596[0] = delta_x[0] + nom_x[0]; - out_6009088715125097596[1] = delta_x[1] + nom_x[1]; - out_6009088715125097596[2] = delta_x[2] + nom_x[2]; - out_6009088715125097596[3] = delta_x[3] + nom_x[3]; - out_6009088715125097596[4] = delta_x[4] + nom_x[4]; - out_6009088715125097596[5] = delta_x[5] + nom_x[5]; - out_6009088715125097596[6] = delta_x[6] + nom_x[6]; - out_6009088715125097596[7] = delta_x[7] + nom_x[7]; - out_6009088715125097596[8] = delta_x[8] + nom_x[8]; - out_6009088715125097596[9] = delta_x[9] + nom_x[9]; - out_6009088715125097596[10] = delta_x[10] + nom_x[10]; - out_6009088715125097596[11] = delta_x[11] + nom_x[11]; - out_6009088715125097596[12] = delta_x[12] + nom_x[12]; - out_6009088715125097596[13] = delta_x[13] + nom_x[13]; - out_6009088715125097596[14] = delta_x[14] + nom_x[14]; - out_6009088715125097596[15] = delta_x[15] + nom_x[15]; - out_6009088715125097596[16] = delta_x[16] + nom_x[16]; - out_6009088715125097596[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_2383575066684203465) { + out_2383575066684203465[0] = delta_x[0] + nom_x[0]; + out_2383575066684203465[1] = delta_x[1] + nom_x[1]; + out_2383575066684203465[2] = delta_x[2] + nom_x[2]; + out_2383575066684203465[3] = delta_x[3] + nom_x[3]; + out_2383575066684203465[4] = delta_x[4] + nom_x[4]; + out_2383575066684203465[5] = delta_x[5] + nom_x[5]; + out_2383575066684203465[6] = delta_x[6] + nom_x[6]; + out_2383575066684203465[7] = delta_x[7] + nom_x[7]; + out_2383575066684203465[8] = delta_x[8] + nom_x[8]; + out_2383575066684203465[9] = delta_x[9] + nom_x[9]; + out_2383575066684203465[10] = delta_x[10] + nom_x[10]; + out_2383575066684203465[11] = delta_x[11] + nom_x[11]; + out_2383575066684203465[12] = delta_x[12] + nom_x[12]; + out_2383575066684203465[13] = delta_x[13] + nom_x[13]; + out_2383575066684203465[14] = delta_x[14] + nom_x[14]; + out_2383575066684203465[15] = delta_x[15] + nom_x[15]; + out_2383575066684203465[16] = delta_x[16] + nom_x[16]; + out_2383575066684203465[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5460684038605246490) { - out_5460684038605246490[0] = -nom_x[0] + true_x[0]; - out_5460684038605246490[1] = -nom_x[1] + true_x[1]; - out_5460684038605246490[2] = -nom_x[2] + true_x[2]; - out_5460684038605246490[3] = -nom_x[3] + true_x[3]; - out_5460684038605246490[4] = -nom_x[4] + true_x[4]; - out_5460684038605246490[5] = -nom_x[5] + true_x[5]; - out_5460684038605246490[6] = -nom_x[6] + true_x[6]; - out_5460684038605246490[7] = -nom_x[7] + true_x[7]; - out_5460684038605246490[8] = -nom_x[8] + true_x[8]; - out_5460684038605246490[9] = -nom_x[9] + true_x[9]; - out_5460684038605246490[10] = -nom_x[10] + true_x[10]; - out_5460684038605246490[11] = -nom_x[11] + true_x[11]; - out_5460684038605246490[12] = -nom_x[12] + true_x[12]; - out_5460684038605246490[13] = -nom_x[13] + true_x[13]; - out_5460684038605246490[14] = -nom_x[14] + true_x[14]; - out_5460684038605246490[15] = -nom_x[15] + true_x[15]; - out_5460684038605246490[16] = -nom_x[16] + true_x[16]; - out_5460684038605246490[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2531792213831641268) { + out_2531792213831641268[0] = -nom_x[0] + true_x[0]; + out_2531792213831641268[1] = -nom_x[1] + true_x[1]; + out_2531792213831641268[2] = -nom_x[2] + true_x[2]; + out_2531792213831641268[3] = -nom_x[3] + true_x[3]; + out_2531792213831641268[4] = -nom_x[4] + true_x[4]; + out_2531792213831641268[5] = -nom_x[5] + true_x[5]; + out_2531792213831641268[6] = -nom_x[6] + true_x[6]; + out_2531792213831641268[7] = -nom_x[7] + true_x[7]; + out_2531792213831641268[8] = -nom_x[8] + true_x[8]; + out_2531792213831641268[9] = -nom_x[9] + true_x[9]; + out_2531792213831641268[10] = -nom_x[10] + true_x[10]; + out_2531792213831641268[11] = -nom_x[11] + true_x[11]; + out_2531792213831641268[12] = -nom_x[12] + true_x[12]; + out_2531792213831641268[13] = -nom_x[13] + true_x[13]; + out_2531792213831641268[14] = -nom_x[14] + true_x[14]; + out_2531792213831641268[15] = -nom_x[15] + true_x[15]; + out_2531792213831641268[16] = -nom_x[16] + true_x[16]; + out_2531792213831641268[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_2180245129448668184) { - out_2180245129448668184[0] = 1.0; - out_2180245129448668184[1] = 0.0; - out_2180245129448668184[2] = 0.0; - out_2180245129448668184[3] = 0.0; - out_2180245129448668184[4] = 0.0; - out_2180245129448668184[5] = 0.0; - out_2180245129448668184[6] = 0.0; - out_2180245129448668184[7] = 0.0; - out_2180245129448668184[8] = 0.0; - out_2180245129448668184[9] = 0.0; - out_2180245129448668184[10] = 0.0; - out_2180245129448668184[11] = 0.0; - out_2180245129448668184[12] = 0.0; - out_2180245129448668184[13] = 0.0; - out_2180245129448668184[14] = 0.0; - out_2180245129448668184[15] = 0.0; - out_2180245129448668184[16] = 0.0; - out_2180245129448668184[17] = 0.0; - out_2180245129448668184[18] = 0.0; - out_2180245129448668184[19] = 1.0; - out_2180245129448668184[20] = 0.0; - out_2180245129448668184[21] = 0.0; - out_2180245129448668184[22] = 0.0; - out_2180245129448668184[23] = 0.0; - out_2180245129448668184[24] = 0.0; - out_2180245129448668184[25] = 0.0; - out_2180245129448668184[26] = 0.0; - out_2180245129448668184[27] = 0.0; - out_2180245129448668184[28] = 0.0; - out_2180245129448668184[29] = 0.0; - out_2180245129448668184[30] = 0.0; - out_2180245129448668184[31] = 0.0; - out_2180245129448668184[32] = 0.0; - out_2180245129448668184[33] = 0.0; - out_2180245129448668184[34] = 0.0; - out_2180245129448668184[35] = 0.0; - out_2180245129448668184[36] = 0.0; - out_2180245129448668184[37] = 0.0; - out_2180245129448668184[38] = 1.0; - out_2180245129448668184[39] = 0.0; - out_2180245129448668184[40] = 0.0; - out_2180245129448668184[41] = 0.0; - out_2180245129448668184[42] = 0.0; - out_2180245129448668184[43] = 0.0; - out_2180245129448668184[44] = 0.0; - out_2180245129448668184[45] = 0.0; - out_2180245129448668184[46] = 0.0; - out_2180245129448668184[47] = 0.0; - out_2180245129448668184[48] = 0.0; - out_2180245129448668184[49] = 0.0; - out_2180245129448668184[50] = 0.0; - out_2180245129448668184[51] = 0.0; - out_2180245129448668184[52] = 0.0; - out_2180245129448668184[53] = 0.0; - out_2180245129448668184[54] = 0.0; - out_2180245129448668184[55] = 0.0; - out_2180245129448668184[56] = 0.0; - out_2180245129448668184[57] = 1.0; - out_2180245129448668184[58] = 0.0; - out_2180245129448668184[59] = 0.0; - out_2180245129448668184[60] = 0.0; - out_2180245129448668184[61] = 0.0; - out_2180245129448668184[62] = 0.0; - out_2180245129448668184[63] = 0.0; - out_2180245129448668184[64] = 0.0; - out_2180245129448668184[65] = 0.0; - out_2180245129448668184[66] = 0.0; - out_2180245129448668184[67] = 0.0; - out_2180245129448668184[68] = 0.0; - out_2180245129448668184[69] = 0.0; - out_2180245129448668184[70] = 0.0; - out_2180245129448668184[71] = 0.0; - out_2180245129448668184[72] = 0.0; - out_2180245129448668184[73] = 0.0; - out_2180245129448668184[74] = 0.0; - out_2180245129448668184[75] = 0.0; - out_2180245129448668184[76] = 1.0; - out_2180245129448668184[77] = 0.0; - out_2180245129448668184[78] = 0.0; - out_2180245129448668184[79] = 0.0; - out_2180245129448668184[80] = 0.0; - out_2180245129448668184[81] = 0.0; - out_2180245129448668184[82] = 0.0; - out_2180245129448668184[83] = 0.0; - out_2180245129448668184[84] = 0.0; - out_2180245129448668184[85] = 0.0; - out_2180245129448668184[86] = 0.0; - out_2180245129448668184[87] = 0.0; - out_2180245129448668184[88] = 0.0; - out_2180245129448668184[89] = 0.0; - out_2180245129448668184[90] = 0.0; - out_2180245129448668184[91] = 0.0; - out_2180245129448668184[92] = 0.0; - out_2180245129448668184[93] = 0.0; - out_2180245129448668184[94] = 0.0; - out_2180245129448668184[95] = 1.0; - out_2180245129448668184[96] = 0.0; - out_2180245129448668184[97] = 0.0; - out_2180245129448668184[98] = 0.0; - out_2180245129448668184[99] = 0.0; - out_2180245129448668184[100] = 0.0; - out_2180245129448668184[101] = 0.0; - out_2180245129448668184[102] = 0.0; - out_2180245129448668184[103] = 0.0; - out_2180245129448668184[104] = 0.0; - out_2180245129448668184[105] = 0.0; - out_2180245129448668184[106] = 0.0; - out_2180245129448668184[107] = 0.0; - out_2180245129448668184[108] = 0.0; - out_2180245129448668184[109] = 0.0; - out_2180245129448668184[110] = 0.0; - out_2180245129448668184[111] = 0.0; - out_2180245129448668184[112] = 0.0; - out_2180245129448668184[113] = 0.0; - out_2180245129448668184[114] = 1.0; - out_2180245129448668184[115] = 0.0; - out_2180245129448668184[116] = 0.0; - out_2180245129448668184[117] = 0.0; - out_2180245129448668184[118] = 0.0; - out_2180245129448668184[119] = 0.0; - out_2180245129448668184[120] = 0.0; - out_2180245129448668184[121] = 0.0; - out_2180245129448668184[122] = 0.0; - out_2180245129448668184[123] = 0.0; - out_2180245129448668184[124] = 0.0; - out_2180245129448668184[125] = 0.0; - out_2180245129448668184[126] = 0.0; - out_2180245129448668184[127] = 0.0; - out_2180245129448668184[128] = 0.0; - out_2180245129448668184[129] = 0.0; - out_2180245129448668184[130] = 0.0; - out_2180245129448668184[131] = 0.0; - out_2180245129448668184[132] = 0.0; - out_2180245129448668184[133] = 1.0; - out_2180245129448668184[134] = 0.0; - out_2180245129448668184[135] = 0.0; - out_2180245129448668184[136] = 0.0; - out_2180245129448668184[137] = 0.0; - out_2180245129448668184[138] = 0.0; - out_2180245129448668184[139] = 0.0; - out_2180245129448668184[140] = 0.0; - out_2180245129448668184[141] = 0.0; - out_2180245129448668184[142] = 0.0; - out_2180245129448668184[143] = 0.0; - out_2180245129448668184[144] = 0.0; - out_2180245129448668184[145] = 0.0; - out_2180245129448668184[146] = 0.0; - out_2180245129448668184[147] = 0.0; - out_2180245129448668184[148] = 0.0; - out_2180245129448668184[149] = 0.0; - out_2180245129448668184[150] = 0.0; - out_2180245129448668184[151] = 0.0; - out_2180245129448668184[152] = 1.0; - out_2180245129448668184[153] = 0.0; - out_2180245129448668184[154] = 0.0; - out_2180245129448668184[155] = 0.0; - out_2180245129448668184[156] = 0.0; - out_2180245129448668184[157] = 0.0; - out_2180245129448668184[158] = 0.0; - out_2180245129448668184[159] = 0.0; - out_2180245129448668184[160] = 0.0; - out_2180245129448668184[161] = 0.0; - out_2180245129448668184[162] = 0.0; - out_2180245129448668184[163] = 0.0; - out_2180245129448668184[164] = 0.0; - out_2180245129448668184[165] = 0.0; - out_2180245129448668184[166] = 0.0; - out_2180245129448668184[167] = 0.0; - out_2180245129448668184[168] = 0.0; - out_2180245129448668184[169] = 0.0; - out_2180245129448668184[170] = 0.0; - out_2180245129448668184[171] = 1.0; - out_2180245129448668184[172] = 0.0; - out_2180245129448668184[173] = 0.0; - out_2180245129448668184[174] = 0.0; - out_2180245129448668184[175] = 0.0; - out_2180245129448668184[176] = 0.0; - out_2180245129448668184[177] = 0.0; - out_2180245129448668184[178] = 0.0; - out_2180245129448668184[179] = 0.0; - out_2180245129448668184[180] = 0.0; - out_2180245129448668184[181] = 0.0; - out_2180245129448668184[182] = 0.0; - out_2180245129448668184[183] = 0.0; - out_2180245129448668184[184] = 0.0; - out_2180245129448668184[185] = 0.0; - out_2180245129448668184[186] = 0.0; - out_2180245129448668184[187] = 0.0; - out_2180245129448668184[188] = 0.0; - out_2180245129448668184[189] = 0.0; - out_2180245129448668184[190] = 1.0; - out_2180245129448668184[191] = 0.0; - out_2180245129448668184[192] = 0.0; - out_2180245129448668184[193] = 0.0; - out_2180245129448668184[194] = 0.0; - out_2180245129448668184[195] = 0.0; - out_2180245129448668184[196] = 0.0; - out_2180245129448668184[197] = 0.0; - out_2180245129448668184[198] = 0.0; - out_2180245129448668184[199] = 0.0; - out_2180245129448668184[200] = 0.0; - out_2180245129448668184[201] = 0.0; - out_2180245129448668184[202] = 0.0; - out_2180245129448668184[203] = 0.0; - out_2180245129448668184[204] = 0.0; - out_2180245129448668184[205] = 0.0; - out_2180245129448668184[206] = 0.0; - out_2180245129448668184[207] = 0.0; - out_2180245129448668184[208] = 0.0; - out_2180245129448668184[209] = 1.0; - out_2180245129448668184[210] = 0.0; - out_2180245129448668184[211] = 0.0; - out_2180245129448668184[212] = 0.0; - out_2180245129448668184[213] = 0.0; - out_2180245129448668184[214] = 0.0; - out_2180245129448668184[215] = 0.0; - out_2180245129448668184[216] = 0.0; - out_2180245129448668184[217] = 0.0; - out_2180245129448668184[218] = 0.0; - out_2180245129448668184[219] = 0.0; - out_2180245129448668184[220] = 0.0; - out_2180245129448668184[221] = 0.0; - out_2180245129448668184[222] = 0.0; - out_2180245129448668184[223] = 0.0; - out_2180245129448668184[224] = 0.0; - out_2180245129448668184[225] = 0.0; - out_2180245129448668184[226] = 0.0; - out_2180245129448668184[227] = 0.0; - out_2180245129448668184[228] = 1.0; - out_2180245129448668184[229] = 0.0; - out_2180245129448668184[230] = 0.0; - out_2180245129448668184[231] = 0.0; - out_2180245129448668184[232] = 0.0; - out_2180245129448668184[233] = 0.0; - out_2180245129448668184[234] = 0.0; - out_2180245129448668184[235] = 0.0; - out_2180245129448668184[236] = 0.0; - out_2180245129448668184[237] = 0.0; - out_2180245129448668184[238] = 0.0; - out_2180245129448668184[239] = 0.0; - out_2180245129448668184[240] = 0.0; - out_2180245129448668184[241] = 0.0; - out_2180245129448668184[242] = 0.0; - out_2180245129448668184[243] = 0.0; - out_2180245129448668184[244] = 0.0; - out_2180245129448668184[245] = 0.0; - out_2180245129448668184[246] = 0.0; - out_2180245129448668184[247] = 1.0; - out_2180245129448668184[248] = 0.0; - out_2180245129448668184[249] = 0.0; - out_2180245129448668184[250] = 0.0; - out_2180245129448668184[251] = 0.0; - out_2180245129448668184[252] = 0.0; - out_2180245129448668184[253] = 0.0; - out_2180245129448668184[254] = 0.0; - out_2180245129448668184[255] = 0.0; - out_2180245129448668184[256] = 0.0; - out_2180245129448668184[257] = 0.0; - out_2180245129448668184[258] = 0.0; - out_2180245129448668184[259] = 0.0; - out_2180245129448668184[260] = 0.0; - out_2180245129448668184[261] = 0.0; - out_2180245129448668184[262] = 0.0; - out_2180245129448668184[263] = 0.0; - out_2180245129448668184[264] = 0.0; - out_2180245129448668184[265] = 0.0; - out_2180245129448668184[266] = 1.0; - out_2180245129448668184[267] = 0.0; - out_2180245129448668184[268] = 0.0; - out_2180245129448668184[269] = 0.0; - out_2180245129448668184[270] = 0.0; - out_2180245129448668184[271] = 0.0; - out_2180245129448668184[272] = 0.0; - out_2180245129448668184[273] = 0.0; - out_2180245129448668184[274] = 0.0; - out_2180245129448668184[275] = 0.0; - out_2180245129448668184[276] = 0.0; - out_2180245129448668184[277] = 0.0; - out_2180245129448668184[278] = 0.0; - out_2180245129448668184[279] = 0.0; - out_2180245129448668184[280] = 0.0; - out_2180245129448668184[281] = 0.0; - out_2180245129448668184[282] = 0.0; - out_2180245129448668184[283] = 0.0; - out_2180245129448668184[284] = 0.0; - out_2180245129448668184[285] = 1.0; - out_2180245129448668184[286] = 0.0; - out_2180245129448668184[287] = 0.0; - out_2180245129448668184[288] = 0.0; - out_2180245129448668184[289] = 0.0; - out_2180245129448668184[290] = 0.0; - out_2180245129448668184[291] = 0.0; - out_2180245129448668184[292] = 0.0; - out_2180245129448668184[293] = 0.0; - out_2180245129448668184[294] = 0.0; - out_2180245129448668184[295] = 0.0; - out_2180245129448668184[296] = 0.0; - out_2180245129448668184[297] = 0.0; - out_2180245129448668184[298] = 0.0; - out_2180245129448668184[299] = 0.0; - out_2180245129448668184[300] = 0.0; - out_2180245129448668184[301] = 0.0; - out_2180245129448668184[302] = 0.0; - out_2180245129448668184[303] = 0.0; - out_2180245129448668184[304] = 1.0; - out_2180245129448668184[305] = 0.0; - out_2180245129448668184[306] = 0.0; - out_2180245129448668184[307] = 0.0; - out_2180245129448668184[308] = 0.0; - out_2180245129448668184[309] = 0.0; - out_2180245129448668184[310] = 0.0; - out_2180245129448668184[311] = 0.0; - out_2180245129448668184[312] = 0.0; - out_2180245129448668184[313] = 0.0; - out_2180245129448668184[314] = 0.0; - out_2180245129448668184[315] = 0.0; - out_2180245129448668184[316] = 0.0; - out_2180245129448668184[317] = 0.0; - out_2180245129448668184[318] = 0.0; - out_2180245129448668184[319] = 0.0; - out_2180245129448668184[320] = 0.0; - out_2180245129448668184[321] = 0.0; - out_2180245129448668184[322] = 0.0; - out_2180245129448668184[323] = 1.0; +void H_mod_fun(double *state, double *out_5270168453751144292) { + out_5270168453751144292[0] = 1.0; + out_5270168453751144292[1] = 0.0; + out_5270168453751144292[2] = 0.0; + out_5270168453751144292[3] = 0.0; + out_5270168453751144292[4] = 0.0; + out_5270168453751144292[5] = 0.0; + out_5270168453751144292[6] = 0.0; + out_5270168453751144292[7] = 0.0; + out_5270168453751144292[8] = 0.0; + out_5270168453751144292[9] = 0.0; + out_5270168453751144292[10] = 0.0; + out_5270168453751144292[11] = 0.0; + out_5270168453751144292[12] = 0.0; + out_5270168453751144292[13] = 0.0; + out_5270168453751144292[14] = 0.0; + out_5270168453751144292[15] = 0.0; + out_5270168453751144292[16] = 0.0; + out_5270168453751144292[17] = 0.0; + out_5270168453751144292[18] = 0.0; + out_5270168453751144292[19] = 1.0; + out_5270168453751144292[20] = 0.0; + out_5270168453751144292[21] = 0.0; + out_5270168453751144292[22] = 0.0; + out_5270168453751144292[23] = 0.0; + out_5270168453751144292[24] = 0.0; + out_5270168453751144292[25] = 0.0; + out_5270168453751144292[26] = 0.0; + out_5270168453751144292[27] = 0.0; + out_5270168453751144292[28] = 0.0; + out_5270168453751144292[29] = 0.0; + out_5270168453751144292[30] = 0.0; + out_5270168453751144292[31] = 0.0; + out_5270168453751144292[32] = 0.0; + out_5270168453751144292[33] = 0.0; + out_5270168453751144292[34] = 0.0; + out_5270168453751144292[35] = 0.0; + out_5270168453751144292[36] = 0.0; + out_5270168453751144292[37] = 0.0; + out_5270168453751144292[38] = 1.0; + out_5270168453751144292[39] = 0.0; + out_5270168453751144292[40] = 0.0; + out_5270168453751144292[41] = 0.0; + out_5270168453751144292[42] = 0.0; + out_5270168453751144292[43] = 0.0; + out_5270168453751144292[44] = 0.0; + out_5270168453751144292[45] = 0.0; + out_5270168453751144292[46] = 0.0; + out_5270168453751144292[47] = 0.0; + out_5270168453751144292[48] = 0.0; + out_5270168453751144292[49] = 0.0; + out_5270168453751144292[50] = 0.0; + out_5270168453751144292[51] = 0.0; + out_5270168453751144292[52] = 0.0; + out_5270168453751144292[53] = 0.0; + out_5270168453751144292[54] = 0.0; + out_5270168453751144292[55] = 0.0; + out_5270168453751144292[56] = 0.0; + out_5270168453751144292[57] = 1.0; + out_5270168453751144292[58] = 0.0; + out_5270168453751144292[59] = 0.0; + out_5270168453751144292[60] = 0.0; + out_5270168453751144292[61] = 0.0; + out_5270168453751144292[62] = 0.0; + out_5270168453751144292[63] = 0.0; + out_5270168453751144292[64] = 0.0; + out_5270168453751144292[65] = 0.0; + out_5270168453751144292[66] = 0.0; + out_5270168453751144292[67] = 0.0; + out_5270168453751144292[68] = 0.0; + out_5270168453751144292[69] = 0.0; + out_5270168453751144292[70] = 0.0; + out_5270168453751144292[71] = 0.0; + out_5270168453751144292[72] = 0.0; + out_5270168453751144292[73] = 0.0; + out_5270168453751144292[74] = 0.0; + out_5270168453751144292[75] = 0.0; + out_5270168453751144292[76] = 1.0; + out_5270168453751144292[77] = 0.0; + out_5270168453751144292[78] = 0.0; + out_5270168453751144292[79] = 0.0; + out_5270168453751144292[80] = 0.0; + out_5270168453751144292[81] = 0.0; + out_5270168453751144292[82] = 0.0; + out_5270168453751144292[83] = 0.0; + out_5270168453751144292[84] = 0.0; + out_5270168453751144292[85] = 0.0; + out_5270168453751144292[86] = 0.0; + out_5270168453751144292[87] = 0.0; + out_5270168453751144292[88] = 0.0; + out_5270168453751144292[89] = 0.0; + out_5270168453751144292[90] = 0.0; + out_5270168453751144292[91] = 0.0; + out_5270168453751144292[92] = 0.0; + out_5270168453751144292[93] = 0.0; + out_5270168453751144292[94] = 0.0; + out_5270168453751144292[95] = 1.0; + out_5270168453751144292[96] = 0.0; + out_5270168453751144292[97] = 0.0; + out_5270168453751144292[98] = 0.0; + out_5270168453751144292[99] = 0.0; + out_5270168453751144292[100] = 0.0; + out_5270168453751144292[101] = 0.0; + out_5270168453751144292[102] = 0.0; + out_5270168453751144292[103] = 0.0; + out_5270168453751144292[104] = 0.0; + out_5270168453751144292[105] = 0.0; + out_5270168453751144292[106] = 0.0; + out_5270168453751144292[107] = 0.0; + out_5270168453751144292[108] = 0.0; + out_5270168453751144292[109] = 0.0; + out_5270168453751144292[110] = 0.0; + out_5270168453751144292[111] = 0.0; + out_5270168453751144292[112] = 0.0; + out_5270168453751144292[113] = 0.0; + out_5270168453751144292[114] = 1.0; + out_5270168453751144292[115] = 0.0; + out_5270168453751144292[116] = 0.0; + out_5270168453751144292[117] = 0.0; + out_5270168453751144292[118] = 0.0; + out_5270168453751144292[119] = 0.0; + out_5270168453751144292[120] = 0.0; + out_5270168453751144292[121] = 0.0; + out_5270168453751144292[122] = 0.0; + out_5270168453751144292[123] = 0.0; + out_5270168453751144292[124] = 0.0; + out_5270168453751144292[125] = 0.0; + out_5270168453751144292[126] = 0.0; + out_5270168453751144292[127] = 0.0; + out_5270168453751144292[128] = 0.0; + out_5270168453751144292[129] = 0.0; + out_5270168453751144292[130] = 0.0; + out_5270168453751144292[131] = 0.0; + out_5270168453751144292[132] = 0.0; + out_5270168453751144292[133] = 1.0; + out_5270168453751144292[134] = 0.0; + out_5270168453751144292[135] = 0.0; + out_5270168453751144292[136] = 0.0; + out_5270168453751144292[137] = 0.0; + out_5270168453751144292[138] = 0.0; + out_5270168453751144292[139] = 0.0; + out_5270168453751144292[140] = 0.0; + out_5270168453751144292[141] = 0.0; + out_5270168453751144292[142] = 0.0; + out_5270168453751144292[143] = 0.0; + out_5270168453751144292[144] = 0.0; + out_5270168453751144292[145] = 0.0; + out_5270168453751144292[146] = 0.0; + out_5270168453751144292[147] = 0.0; + out_5270168453751144292[148] = 0.0; + out_5270168453751144292[149] = 0.0; + out_5270168453751144292[150] = 0.0; + out_5270168453751144292[151] = 0.0; + out_5270168453751144292[152] = 1.0; + out_5270168453751144292[153] = 0.0; + out_5270168453751144292[154] = 0.0; + out_5270168453751144292[155] = 0.0; + out_5270168453751144292[156] = 0.0; + out_5270168453751144292[157] = 0.0; + out_5270168453751144292[158] = 0.0; + out_5270168453751144292[159] = 0.0; + out_5270168453751144292[160] = 0.0; + out_5270168453751144292[161] = 0.0; + out_5270168453751144292[162] = 0.0; + out_5270168453751144292[163] = 0.0; + out_5270168453751144292[164] = 0.0; + out_5270168453751144292[165] = 0.0; + out_5270168453751144292[166] = 0.0; + out_5270168453751144292[167] = 0.0; + out_5270168453751144292[168] = 0.0; + out_5270168453751144292[169] = 0.0; + out_5270168453751144292[170] = 0.0; + out_5270168453751144292[171] = 1.0; + out_5270168453751144292[172] = 0.0; + out_5270168453751144292[173] = 0.0; + out_5270168453751144292[174] = 0.0; + out_5270168453751144292[175] = 0.0; + out_5270168453751144292[176] = 0.0; + out_5270168453751144292[177] = 0.0; + out_5270168453751144292[178] = 0.0; + out_5270168453751144292[179] = 0.0; + out_5270168453751144292[180] = 0.0; + out_5270168453751144292[181] = 0.0; + out_5270168453751144292[182] = 0.0; + out_5270168453751144292[183] = 0.0; + out_5270168453751144292[184] = 0.0; + out_5270168453751144292[185] = 0.0; + out_5270168453751144292[186] = 0.0; + out_5270168453751144292[187] = 0.0; + out_5270168453751144292[188] = 0.0; + out_5270168453751144292[189] = 0.0; + out_5270168453751144292[190] = 1.0; + out_5270168453751144292[191] = 0.0; + out_5270168453751144292[192] = 0.0; + out_5270168453751144292[193] = 0.0; + out_5270168453751144292[194] = 0.0; + out_5270168453751144292[195] = 0.0; + out_5270168453751144292[196] = 0.0; + out_5270168453751144292[197] = 0.0; + out_5270168453751144292[198] = 0.0; + out_5270168453751144292[199] = 0.0; + out_5270168453751144292[200] = 0.0; + out_5270168453751144292[201] = 0.0; + out_5270168453751144292[202] = 0.0; + out_5270168453751144292[203] = 0.0; + out_5270168453751144292[204] = 0.0; + out_5270168453751144292[205] = 0.0; + out_5270168453751144292[206] = 0.0; + out_5270168453751144292[207] = 0.0; + out_5270168453751144292[208] = 0.0; + out_5270168453751144292[209] = 1.0; + out_5270168453751144292[210] = 0.0; + out_5270168453751144292[211] = 0.0; + out_5270168453751144292[212] = 0.0; + out_5270168453751144292[213] = 0.0; + out_5270168453751144292[214] = 0.0; + out_5270168453751144292[215] = 0.0; + out_5270168453751144292[216] = 0.0; + out_5270168453751144292[217] = 0.0; + out_5270168453751144292[218] = 0.0; + out_5270168453751144292[219] = 0.0; + out_5270168453751144292[220] = 0.0; + out_5270168453751144292[221] = 0.0; + out_5270168453751144292[222] = 0.0; + out_5270168453751144292[223] = 0.0; + out_5270168453751144292[224] = 0.0; + out_5270168453751144292[225] = 0.0; + out_5270168453751144292[226] = 0.0; + out_5270168453751144292[227] = 0.0; + out_5270168453751144292[228] = 1.0; + out_5270168453751144292[229] = 0.0; + out_5270168453751144292[230] = 0.0; + out_5270168453751144292[231] = 0.0; + out_5270168453751144292[232] = 0.0; + out_5270168453751144292[233] = 0.0; + out_5270168453751144292[234] = 0.0; + out_5270168453751144292[235] = 0.0; + out_5270168453751144292[236] = 0.0; + out_5270168453751144292[237] = 0.0; + out_5270168453751144292[238] = 0.0; + out_5270168453751144292[239] = 0.0; + out_5270168453751144292[240] = 0.0; + out_5270168453751144292[241] = 0.0; + out_5270168453751144292[242] = 0.0; + out_5270168453751144292[243] = 0.0; + out_5270168453751144292[244] = 0.0; + out_5270168453751144292[245] = 0.0; + out_5270168453751144292[246] = 0.0; + out_5270168453751144292[247] = 1.0; + out_5270168453751144292[248] = 0.0; + out_5270168453751144292[249] = 0.0; + out_5270168453751144292[250] = 0.0; + out_5270168453751144292[251] = 0.0; + out_5270168453751144292[252] = 0.0; + out_5270168453751144292[253] = 0.0; + out_5270168453751144292[254] = 0.0; + out_5270168453751144292[255] = 0.0; + out_5270168453751144292[256] = 0.0; + out_5270168453751144292[257] = 0.0; + out_5270168453751144292[258] = 0.0; + out_5270168453751144292[259] = 0.0; + out_5270168453751144292[260] = 0.0; + out_5270168453751144292[261] = 0.0; + out_5270168453751144292[262] = 0.0; + out_5270168453751144292[263] = 0.0; + out_5270168453751144292[264] = 0.0; + out_5270168453751144292[265] = 0.0; + out_5270168453751144292[266] = 1.0; + out_5270168453751144292[267] = 0.0; + out_5270168453751144292[268] = 0.0; + out_5270168453751144292[269] = 0.0; + out_5270168453751144292[270] = 0.0; + out_5270168453751144292[271] = 0.0; + out_5270168453751144292[272] = 0.0; + out_5270168453751144292[273] = 0.0; + out_5270168453751144292[274] = 0.0; + out_5270168453751144292[275] = 0.0; + out_5270168453751144292[276] = 0.0; + out_5270168453751144292[277] = 0.0; + out_5270168453751144292[278] = 0.0; + out_5270168453751144292[279] = 0.0; + out_5270168453751144292[280] = 0.0; + out_5270168453751144292[281] = 0.0; + out_5270168453751144292[282] = 0.0; + out_5270168453751144292[283] = 0.0; + out_5270168453751144292[284] = 0.0; + out_5270168453751144292[285] = 1.0; + out_5270168453751144292[286] = 0.0; + out_5270168453751144292[287] = 0.0; + out_5270168453751144292[288] = 0.0; + out_5270168453751144292[289] = 0.0; + out_5270168453751144292[290] = 0.0; + out_5270168453751144292[291] = 0.0; + out_5270168453751144292[292] = 0.0; + out_5270168453751144292[293] = 0.0; + out_5270168453751144292[294] = 0.0; + out_5270168453751144292[295] = 0.0; + out_5270168453751144292[296] = 0.0; + out_5270168453751144292[297] = 0.0; + out_5270168453751144292[298] = 0.0; + out_5270168453751144292[299] = 0.0; + out_5270168453751144292[300] = 0.0; + out_5270168453751144292[301] = 0.0; + out_5270168453751144292[302] = 0.0; + out_5270168453751144292[303] = 0.0; + out_5270168453751144292[304] = 1.0; + out_5270168453751144292[305] = 0.0; + out_5270168453751144292[306] = 0.0; + out_5270168453751144292[307] = 0.0; + out_5270168453751144292[308] = 0.0; + out_5270168453751144292[309] = 0.0; + out_5270168453751144292[310] = 0.0; + out_5270168453751144292[311] = 0.0; + out_5270168453751144292[312] = 0.0; + out_5270168453751144292[313] = 0.0; + out_5270168453751144292[314] = 0.0; + out_5270168453751144292[315] = 0.0; + out_5270168453751144292[316] = 0.0; + out_5270168453751144292[317] = 0.0; + out_5270168453751144292[318] = 0.0; + out_5270168453751144292[319] = 0.0; + out_5270168453751144292[320] = 0.0; + out_5270168453751144292[321] = 0.0; + out_5270168453751144292[322] = 0.0; + out_5270168453751144292[323] = 1.0; } -void f_fun(double *state, double dt, double *out_1359935913624561504) { - out_1359935913624561504[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_1359935913624561504[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_1359935913624561504[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_1359935913624561504[3] = dt*state[12] + state[3]; - out_1359935913624561504[4] = dt*state[13] + state[4]; - out_1359935913624561504[5] = dt*state[14] + state[5]; - out_1359935913624561504[6] = state[6]; - out_1359935913624561504[7] = state[7]; - out_1359935913624561504[8] = state[8]; - out_1359935913624561504[9] = state[9]; - out_1359935913624561504[10] = state[10]; - out_1359935913624561504[11] = state[11]; - out_1359935913624561504[12] = state[12]; - out_1359935913624561504[13] = state[13]; - out_1359935913624561504[14] = state[14]; - out_1359935913624561504[15] = state[15]; - out_1359935913624561504[16] = state[16]; - out_1359935913624561504[17] = state[17]; +void f_fun(double *state, double dt, double *out_5870118476100851027) { + out_5870118476100851027[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_5870118476100851027[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_5870118476100851027[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_5870118476100851027[3] = dt*state[12] + state[3]; + out_5870118476100851027[4] = dt*state[13] + state[4]; + out_5870118476100851027[5] = dt*state[14] + state[5]; + out_5870118476100851027[6] = state[6]; + out_5870118476100851027[7] = state[7]; + out_5870118476100851027[8] = state[8]; + out_5870118476100851027[9] = state[9]; + out_5870118476100851027[10] = state[10]; + out_5870118476100851027[11] = state[11]; + out_5870118476100851027[12] = state[12]; + out_5870118476100851027[13] = state[13]; + out_5870118476100851027[14] = state[14]; + out_5870118476100851027[15] = state[15]; + out_5870118476100851027[16] = state[16]; + out_5870118476100851027[17] = state[17]; } -void F_fun(double *state, double dt, double *out_3401373591342797666) { - out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[2] = 0; - out_3401373591342797666[3] = 0; - out_3401373591342797666[4] = 0; - out_3401373591342797666[5] = 0; - out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[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_3401373591342797666[9] = 0; - out_3401373591342797666[10] = 0; - out_3401373591342797666[11] = 0; - out_3401373591342797666[12] = 0; - out_3401373591342797666[13] = 0; - out_3401373591342797666[14] = 0; - out_3401373591342797666[15] = 0; - out_3401373591342797666[16] = 0; - out_3401373591342797666[17] = 0; - out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[20] = 0; - out_3401373591342797666[21] = 0; - out_3401373591342797666[22] = 0; - out_3401373591342797666[23] = 0; - out_3401373591342797666[24] = 0; - out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[27] = 0; - out_3401373591342797666[28] = 0; - out_3401373591342797666[29] = 0; - out_3401373591342797666[30] = 0; - out_3401373591342797666[31] = 0; - out_3401373591342797666[32] = 0; - out_3401373591342797666[33] = 0; - out_3401373591342797666[34] = 0; - out_3401373591342797666[35] = 0; - out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[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_3401373591342797666[39] = 0; - out_3401373591342797666[40] = 0; - out_3401373591342797666[41] = 0; - out_3401373591342797666[42] = 0; - out_3401373591342797666[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_3401373591342797666[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_3401373591342797666[45] = 0; - out_3401373591342797666[46] = 0; - out_3401373591342797666[47] = 0; - out_3401373591342797666[48] = 0; - out_3401373591342797666[49] = 0; - out_3401373591342797666[50] = 0; - out_3401373591342797666[51] = 0; - out_3401373591342797666[52] = 0; - out_3401373591342797666[53] = 0; - out_3401373591342797666[54] = 0; - out_3401373591342797666[55] = 0; - out_3401373591342797666[56] = 0; - out_3401373591342797666[57] = 1; - out_3401373591342797666[58] = 0; - out_3401373591342797666[59] = 0; - out_3401373591342797666[60] = 0; - out_3401373591342797666[61] = 0; - out_3401373591342797666[62] = 0; - out_3401373591342797666[63] = 0; - out_3401373591342797666[64] = 0; - out_3401373591342797666[65] = 0; - out_3401373591342797666[66] = dt; - out_3401373591342797666[67] = 0; - out_3401373591342797666[68] = 0; - out_3401373591342797666[69] = 0; - out_3401373591342797666[70] = 0; - out_3401373591342797666[71] = 0; - out_3401373591342797666[72] = 0; - out_3401373591342797666[73] = 0; - out_3401373591342797666[74] = 0; - out_3401373591342797666[75] = 0; - out_3401373591342797666[76] = 1; - out_3401373591342797666[77] = 0; - out_3401373591342797666[78] = 0; - out_3401373591342797666[79] = 0; - out_3401373591342797666[80] = 0; - out_3401373591342797666[81] = 0; - out_3401373591342797666[82] = 0; - out_3401373591342797666[83] = 0; - out_3401373591342797666[84] = 0; - out_3401373591342797666[85] = dt; - out_3401373591342797666[86] = 0; - out_3401373591342797666[87] = 0; - out_3401373591342797666[88] = 0; - out_3401373591342797666[89] = 0; - out_3401373591342797666[90] = 0; - out_3401373591342797666[91] = 0; - out_3401373591342797666[92] = 0; - out_3401373591342797666[93] = 0; - out_3401373591342797666[94] = 0; - out_3401373591342797666[95] = 1; - out_3401373591342797666[96] = 0; - out_3401373591342797666[97] = 0; - out_3401373591342797666[98] = 0; - out_3401373591342797666[99] = 0; - out_3401373591342797666[100] = 0; - out_3401373591342797666[101] = 0; - out_3401373591342797666[102] = 0; - out_3401373591342797666[103] = 0; - out_3401373591342797666[104] = dt; - out_3401373591342797666[105] = 0; - out_3401373591342797666[106] = 0; - out_3401373591342797666[107] = 0; - out_3401373591342797666[108] = 0; - out_3401373591342797666[109] = 0; - out_3401373591342797666[110] = 0; - out_3401373591342797666[111] = 0; - out_3401373591342797666[112] = 0; - out_3401373591342797666[113] = 0; - out_3401373591342797666[114] = 1; - out_3401373591342797666[115] = 0; - out_3401373591342797666[116] = 0; - out_3401373591342797666[117] = 0; - out_3401373591342797666[118] = 0; - out_3401373591342797666[119] = 0; - out_3401373591342797666[120] = 0; - out_3401373591342797666[121] = 0; - out_3401373591342797666[122] = 0; - out_3401373591342797666[123] = 0; - out_3401373591342797666[124] = 0; - out_3401373591342797666[125] = 0; - out_3401373591342797666[126] = 0; - out_3401373591342797666[127] = 0; - out_3401373591342797666[128] = 0; - out_3401373591342797666[129] = 0; - out_3401373591342797666[130] = 0; - out_3401373591342797666[131] = 0; - out_3401373591342797666[132] = 0; - out_3401373591342797666[133] = 1; - out_3401373591342797666[134] = 0; - out_3401373591342797666[135] = 0; - out_3401373591342797666[136] = 0; - out_3401373591342797666[137] = 0; - out_3401373591342797666[138] = 0; - out_3401373591342797666[139] = 0; - out_3401373591342797666[140] = 0; - out_3401373591342797666[141] = 0; - out_3401373591342797666[142] = 0; - out_3401373591342797666[143] = 0; - out_3401373591342797666[144] = 0; - out_3401373591342797666[145] = 0; - out_3401373591342797666[146] = 0; - out_3401373591342797666[147] = 0; - out_3401373591342797666[148] = 0; - out_3401373591342797666[149] = 0; - out_3401373591342797666[150] = 0; - out_3401373591342797666[151] = 0; - out_3401373591342797666[152] = 1; - out_3401373591342797666[153] = 0; - out_3401373591342797666[154] = 0; - out_3401373591342797666[155] = 0; - out_3401373591342797666[156] = 0; - out_3401373591342797666[157] = 0; - out_3401373591342797666[158] = 0; - out_3401373591342797666[159] = 0; - out_3401373591342797666[160] = 0; - out_3401373591342797666[161] = 0; - out_3401373591342797666[162] = 0; - out_3401373591342797666[163] = 0; - out_3401373591342797666[164] = 0; - out_3401373591342797666[165] = 0; - out_3401373591342797666[166] = 0; - out_3401373591342797666[167] = 0; - out_3401373591342797666[168] = 0; - out_3401373591342797666[169] = 0; - out_3401373591342797666[170] = 0; - out_3401373591342797666[171] = 1; - out_3401373591342797666[172] = 0; - out_3401373591342797666[173] = 0; - out_3401373591342797666[174] = 0; - out_3401373591342797666[175] = 0; - out_3401373591342797666[176] = 0; - out_3401373591342797666[177] = 0; - out_3401373591342797666[178] = 0; - out_3401373591342797666[179] = 0; - out_3401373591342797666[180] = 0; - out_3401373591342797666[181] = 0; - out_3401373591342797666[182] = 0; - out_3401373591342797666[183] = 0; - out_3401373591342797666[184] = 0; - out_3401373591342797666[185] = 0; - out_3401373591342797666[186] = 0; - out_3401373591342797666[187] = 0; - out_3401373591342797666[188] = 0; - out_3401373591342797666[189] = 0; - out_3401373591342797666[190] = 1; - out_3401373591342797666[191] = 0; - out_3401373591342797666[192] = 0; - out_3401373591342797666[193] = 0; - out_3401373591342797666[194] = 0; - out_3401373591342797666[195] = 0; - out_3401373591342797666[196] = 0; - out_3401373591342797666[197] = 0; - out_3401373591342797666[198] = 0; - out_3401373591342797666[199] = 0; - out_3401373591342797666[200] = 0; - out_3401373591342797666[201] = 0; - out_3401373591342797666[202] = 0; - out_3401373591342797666[203] = 0; - out_3401373591342797666[204] = 0; - out_3401373591342797666[205] = 0; - out_3401373591342797666[206] = 0; - out_3401373591342797666[207] = 0; - out_3401373591342797666[208] = 0; - out_3401373591342797666[209] = 1; - out_3401373591342797666[210] = 0; - out_3401373591342797666[211] = 0; - out_3401373591342797666[212] = 0; - out_3401373591342797666[213] = 0; - out_3401373591342797666[214] = 0; - out_3401373591342797666[215] = 0; - out_3401373591342797666[216] = 0; - out_3401373591342797666[217] = 0; - out_3401373591342797666[218] = 0; - out_3401373591342797666[219] = 0; - out_3401373591342797666[220] = 0; - out_3401373591342797666[221] = 0; - out_3401373591342797666[222] = 0; - out_3401373591342797666[223] = 0; - out_3401373591342797666[224] = 0; - out_3401373591342797666[225] = 0; - out_3401373591342797666[226] = 0; - out_3401373591342797666[227] = 0; - out_3401373591342797666[228] = 1; - out_3401373591342797666[229] = 0; - out_3401373591342797666[230] = 0; - out_3401373591342797666[231] = 0; - out_3401373591342797666[232] = 0; - out_3401373591342797666[233] = 0; - out_3401373591342797666[234] = 0; - out_3401373591342797666[235] = 0; - out_3401373591342797666[236] = 0; - out_3401373591342797666[237] = 0; - out_3401373591342797666[238] = 0; - out_3401373591342797666[239] = 0; - out_3401373591342797666[240] = 0; - out_3401373591342797666[241] = 0; - out_3401373591342797666[242] = 0; - out_3401373591342797666[243] = 0; - out_3401373591342797666[244] = 0; - out_3401373591342797666[245] = 0; - out_3401373591342797666[246] = 0; - out_3401373591342797666[247] = 1; - out_3401373591342797666[248] = 0; - out_3401373591342797666[249] = 0; - out_3401373591342797666[250] = 0; - out_3401373591342797666[251] = 0; - out_3401373591342797666[252] = 0; - out_3401373591342797666[253] = 0; - out_3401373591342797666[254] = 0; - out_3401373591342797666[255] = 0; - out_3401373591342797666[256] = 0; - out_3401373591342797666[257] = 0; - out_3401373591342797666[258] = 0; - out_3401373591342797666[259] = 0; - out_3401373591342797666[260] = 0; - out_3401373591342797666[261] = 0; - out_3401373591342797666[262] = 0; - out_3401373591342797666[263] = 0; - out_3401373591342797666[264] = 0; - out_3401373591342797666[265] = 0; - out_3401373591342797666[266] = 1; - out_3401373591342797666[267] = 0; - out_3401373591342797666[268] = 0; - out_3401373591342797666[269] = 0; - out_3401373591342797666[270] = 0; - out_3401373591342797666[271] = 0; - out_3401373591342797666[272] = 0; - out_3401373591342797666[273] = 0; - out_3401373591342797666[274] = 0; - out_3401373591342797666[275] = 0; - out_3401373591342797666[276] = 0; - out_3401373591342797666[277] = 0; - out_3401373591342797666[278] = 0; - out_3401373591342797666[279] = 0; - out_3401373591342797666[280] = 0; - out_3401373591342797666[281] = 0; - out_3401373591342797666[282] = 0; - out_3401373591342797666[283] = 0; - out_3401373591342797666[284] = 0; - out_3401373591342797666[285] = 1; - out_3401373591342797666[286] = 0; - out_3401373591342797666[287] = 0; - out_3401373591342797666[288] = 0; - out_3401373591342797666[289] = 0; - out_3401373591342797666[290] = 0; - out_3401373591342797666[291] = 0; - out_3401373591342797666[292] = 0; - out_3401373591342797666[293] = 0; - out_3401373591342797666[294] = 0; - out_3401373591342797666[295] = 0; - out_3401373591342797666[296] = 0; - out_3401373591342797666[297] = 0; - out_3401373591342797666[298] = 0; - out_3401373591342797666[299] = 0; - out_3401373591342797666[300] = 0; - out_3401373591342797666[301] = 0; - out_3401373591342797666[302] = 0; - out_3401373591342797666[303] = 0; - out_3401373591342797666[304] = 1; - out_3401373591342797666[305] = 0; - out_3401373591342797666[306] = 0; - out_3401373591342797666[307] = 0; - out_3401373591342797666[308] = 0; - out_3401373591342797666[309] = 0; - out_3401373591342797666[310] = 0; - out_3401373591342797666[311] = 0; - out_3401373591342797666[312] = 0; - out_3401373591342797666[313] = 0; - out_3401373591342797666[314] = 0; - out_3401373591342797666[315] = 0; - out_3401373591342797666[316] = 0; - out_3401373591342797666[317] = 0; - out_3401373591342797666[318] = 0; - out_3401373591342797666[319] = 0; - out_3401373591342797666[320] = 0; - out_3401373591342797666[321] = 0; - out_3401373591342797666[322] = 0; - out_3401373591342797666[323] = 1; +void F_fun(double *state, double dt, double *out_6643036112755764466) { + out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[2] = 0; + out_6643036112755764466[3] = 0; + out_6643036112755764466[4] = 0; + out_6643036112755764466[5] = 0; + out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[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_6643036112755764466[9] = 0; + out_6643036112755764466[10] = 0; + out_6643036112755764466[11] = 0; + out_6643036112755764466[12] = 0; + out_6643036112755764466[13] = 0; + out_6643036112755764466[14] = 0; + out_6643036112755764466[15] = 0; + out_6643036112755764466[16] = 0; + out_6643036112755764466[17] = 0; + out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[20] = 0; + out_6643036112755764466[21] = 0; + out_6643036112755764466[22] = 0; + out_6643036112755764466[23] = 0; + out_6643036112755764466[24] = 0; + out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[27] = 0; + out_6643036112755764466[28] = 0; + out_6643036112755764466[29] = 0; + out_6643036112755764466[30] = 0; + out_6643036112755764466[31] = 0; + out_6643036112755764466[32] = 0; + out_6643036112755764466[33] = 0; + out_6643036112755764466[34] = 0; + out_6643036112755764466[35] = 0; + out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[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_6643036112755764466[39] = 0; + out_6643036112755764466[40] = 0; + out_6643036112755764466[41] = 0; + out_6643036112755764466[42] = 0; + out_6643036112755764466[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_6643036112755764466[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_6643036112755764466[45] = 0; + out_6643036112755764466[46] = 0; + out_6643036112755764466[47] = 0; + out_6643036112755764466[48] = 0; + out_6643036112755764466[49] = 0; + out_6643036112755764466[50] = 0; + out_6643036112755764466[51] = 0; + out_6643036112755764466[52] = 0; + out_6643036112755764466[53] = 0; + out_6643036112755764466[54] = 0; + out_6643036112755764466[55] = 0; + out_6643036112755764466[56] = 0; + out_6643036112755764466[57] = 1; + out_6643036112755764466[58] = 0; + out_6643036112755764466[59] = 0; + out_6643036112755764466[60] = 0; + out_6643036112755764466[61] = 0; + out_6643036112755764466[62] = 0; + out_6643036112755764466[63] = 0; + out_6643036112755764466[64] = 0; + out_6643036112755764466[65] = 0; + out_6643036112755764466[66] = dt; + out_6643036112755764466[67] = 0; + out_6643036112755764466[68] = 0; + out_6643036112755764466[69] = 0; + out_6643036112755764466[70] = 0; + out_6643036112755764466[71] = 0; + out_6643036112755764466[72] = 0; + out_6643036112755764466[73] = 0; + out_6643036112755764466[74] = 0; + out_6643036112755764466[75] = 0; + out_6643036112755764466[76] = 1; + out_6643036112755764466[77] = 0; + out_6643036112755764466[78] = 0; + out_6643036112755764466[79] = 0; + out_6643036112755764466[80] = 0; + out_6643036112755764466[81] = 0; + out_6643036112755764466[82] = 0; + out_6643036112755764466[83] = 0; + out_6643036112755764466[84] = 0; + out_6643036112755764466[85] = dt; + out_6643036112755764466[86] = 0; + out_6643036112755764466[87] = 0; + out_6643036112755764466[88] = 0; + out_6643036112755764466[89] = 0; + out_6643036112755764466[90] = 0; + out_6643036112755764466[91] = 0; + out_6643036112755764466[92] = 0; + out_6643036112755764466[93] = 0; + out_6643036112755764466[94] = 0; + out_6643036112755764466[95] = 1; + out_6643036112755764466[96] = 0; + out_6643036112755764466[97] = 0; + out_6643036112755764466[98] = 0; + out_6643036112755764466[99] = 0; + out_6643036112755764466[100] = 0; + out_6643036112755764466[101] = 0; + out_6643036112755764466[102] = 0; + out_6643036112755764466[103] = 0; + out_6643036112755764466[104] = dt; + out_6643036112755764466[105] = 0; + out_6643036112755764466[106] = 0; + out_6643036112755764466[107] = 0; + out_6643036112755764466[108] = 0; + out_6643036112755764466[109] = 0; + out_6643036112755764466[110] = 0; + out_6643036112755764466[111] = 0; + out_6643036112755764466[112] = 0; + out_6643036112755764466[113] = 0; + out_6643036112755764466[114] = 1; + out_6643036112755764466[115] = 0; + out_6643036112755764466[116] = 0; + out_6643036112755764466[117] = 0; + out_6643036112755764466[118] = 0; + out_6643036112755764466[119] = 0; + out_6643036112755764466[120] = 0; + out_6643036112755764466[121] = 0; + out_6643036112755764466[122] = 0; + out_6643036112755764466[123] = 0; + out_6643036112755764466[124] = 0; + out_6643036112755764466[125] = 0; + out_6643036112755764466[126] = 0; + out_6643036112755764466[127] = 0; + out_6643036112755764466[128] = 0; + out_6643036112755764466[129] = 0; + out_6643036112755764466[130] = 0; + out_6643036112755764466[131] = 0; + out_6643036112755764466[132] = 0; + out_6643036112755764466[133] = 1; + out_6643036112755764466[134] = 0; + out_6643036112755764466[135] = 0; + out_6643036112755764466[136] = 0; + out_6643036112755764466[137] = 0; + out_6643036112755764466[138] = 0; + out_6643036112755764466[139] = 0; + out_6643036112755764466[140] = 0; + out_6643036112755764466[141] = 0; + out_6643036112755764466[142] = 0; + out_6643036112755764466[143] = 0; + out_6643036112755764466[144] = 0; + out_6643036112755764466[145] = 0; + out_6643036112755764466[146] = 0; + out_6643036112755764466[147] = 0; + out_6643036112755764466[148] = 0; + out_6643036112755764466[149] = 0; + out_6643036112755764466[150] = 0; + out_6643036112755764466[151] = 0; + out_6643036112755764466[152] = 1; + out_6643036112755764466[153] = 0; + out_6643036112755764466[154] = 0; + out_6643036112755764466[155] = 0; + out_6643036112755764466[156] = 0; + out_6643036112755764466[157] = 0; + out_6643036112755764466[158] = 0; + out_6643036112755764466[159] = 0; + out_6643036112755764466[160] = 0; + out_6643036112755764466[161] = 0; + out_6643036112755764466[162] = 0; + out_6643036112755764466[163] = 0; + out_6643036112755764466[164] = 0; + out_6643036112755764466[165] = 0; + out_6643036112755764466[166] = 0; + out_6643036112755764466[167] = 0; + out_6643036112755764466[168] = 0; + out_6643036112755764466[169] = 0; + out_6643036112755764466[170] = 0; + out_6643036112755764466[171] = 1; + out_6643036112755764466[172] = 0; + out_6643036112755764466[173] = 0; + out_6643036112755764466[174] = 0; + out_6643036112755764466[175] = 0; + out_6643036112755764466[176] = 0; + out_6643036112755764466[177] = 0; + out_6643036112755764466[178] = 0; + out_6643036112755764466[179] = 0; + out_6643036112755764466[180] = 0; + out_6643036112755764466[181] = 0; + out_6643036112755764466[182] = 0; + out_6643036112755764466[183] = 0; + out_6643036112755764466[184] = 0; + out_6643036112755764466[185] = 0; + out_6643036112755764466[186] = 0; + out_6643036112755764466[187] = 0; + out_6643036112755764466[188] = 0; + out_6643036112755764466[189] = 0; + out_6643036112755764466[190] = 1; + out_6643036112755764466[191] = 0; + out_6643036112755764466[192] = 0; + out_6643036112755764466[193] = 0; + out_6643036112755764466[194] = 0; + out_6643036112755764466[195] = 0; + out_6643036112755764466[196] = 0; + out_6643036112755764466[197] = 0; + out_6643036112755764466[198] = 0; + out_6643036112755764466[199] = 0; + out_6643036112755764466[200] = 0; + out_6643036112755764466[201] = 0; + out_6643036112755764466[202] = 0; + out_6643036112755764466[203] = 0; + out_6643036112755764466[204] = 0; + out_6643036112755764466[205] = 0; + out_6643036112755764466[206] = 0; + out_6643036112755764466[207] = 0; + out_6643036112755764466[208] = 0; + out_6643036112755764466[209] = 1; + out_6643036112755764466[210] = 0; + out_6643036112755764466[211] = 0; + out_6643036112755764466[212] = 0; + out_6643036112755764466[213] = 0; + out_6643036112755764466[214] = 0; + out_6643036112755764466[215] = 0; + out_6643036112755764466[216] = 0; + out_6643036112755764466[217] = 0; + out_6643036112755764466[218] = 0; + out_6643036112755764466[219] = 0; + out_6643036112755764466[220] = 0; + out_6643036112755764466[221] = 0; + out_6643036112755764466[222] = 0; + out_6643036112755764466[223] = 0; + out_6643036112755764466[224] = 0; + out_6643036112755764466[225] = 0; + out_6643036112755764466[226] = 0; + out_6643036112755764466[227] = 0; + out_6643036112755764466[228] = 1; + out_6643036112755764466[229] = 0; + out_6643036112755764466[230] = 0; + out_6643036112755764466[231] = 0; + out_6643036112755764466[232] = 0; + out_6643036112755764466[233] = 0; + out_6643036112755764466[234] = 0; + out_6643036112755764466[235] = 0; + out_6643036112755764466[236] = 0; + out_6643036112755764466[237] = 0; + out_6643036112755764466[238] = 0; + out_6643036112755764466[239] = 0; + out_6643036112755764466[240] = 0; + out_6643036112755764466[241] = 0; + out_6643036112755764466[242] = 0; + out_6643036112755764466[243] = 0; + out_6643036112755764466[244] = 0; + out_6643036112755764466[245] = 0; + out_6643036112755764466[246] = 0; + out_6643036112755764466[247] = 1; + out_6643036112755764466[248] = 0; + out_6643036112755764466[249] = 0; + out_6643036112755764466[250] = 0; + out_6643036112755764466[251] = 0; + out_6643036112755764466[252] = 0; + out_6643036112755764466[253] = 0; + out_6643036112755764466[254] = 0; + out_6643036112755764466[255] = 0; + out_6643036112755764466[256] = 0; + out_6643036112755764466[257] = 0; + out_6643036112755764466[258] = 0; + out_6643036112755764466[259] = 0; + out_6643036112755764466[260] = 0; + out_6643036112755764466[261] = 0; + out_6643036112755764466[262] = 0; + out_6643036112755764466[263] = 0; + out_6643036112755764466[264] = 0; + out_6643036112755764466[265] = 0; + out_6643036112755764466[266] = 1; + out_6643036112755764466[267] = 0; + out_6643036112755764466[268] = 0; + out_6643036112755764466[269] = 0; + out_6643036112755764466[270] = 0; + out_6643036112755764466[271] = 0; + out_6643036112755764466[272] = 0; + out_6643036112755764466[273] = 0; + out_6643036112755764466[274] = 0; + out_6643036112755764466[275] = 0; + out_6643036112755764466[276] = 0; + out_6643036112755764466[277] = 0; + out_6643036112755764466[278] = 0; + out_6643036112755764466[279] = 0; + out_6643036112755764466[280] = 0; + out_6643036112755764466[281] = 0; + out_6643036112755764466[282] = 0; + out_6643036112755764466[283] = 0; + out_6643036112755764466[284] = 0; + out_6643036112755764466[285] = 1; + out_6643036112755764466[286] = 0; + out_6643036112755764466[287] = 0; + out_6643036112755764466[288] = 0; + out_6643036112755764466[289] = 0; + out_6643036112755764466[290] = 0; + out_6643036112755764466[291] = 0; + out_6643036112755764466[292] = 0; + out_6643036112755764466[293] = 0; + out_6643036112755764466[294] = 0; + out_6643036112755764466[295] = 0; + out_6643036112755764466[296] = 0; + out_6643036112755764466[297] = 0; + out_6643036112755764466[298] = 0; + out_6643036112755764466[299] = 0; + out_6643036112755764466[300] = 0; + out_6643036112755764466[301] = 0; + out_6643036112755764466[302] = 0; + out_6643036112755764466[303] = 0; + out_6643036112755764466[304] = 1; + out_6643036112755764466[305] = 0; + out_6643036112755764466[306] = 0; + out_6643036112755764466[307] = 0; + out_6643036112755764466[308] = 0; + out_6643036112755764466[309] = 0; + out_6643036112755764466[310] = 0; + out_6643036112755764466[311] = 0; + out_6643036112755764466[312] = 0; + out_6643036112755764466[313] = 0; + out_6643036112755764466[314] = 0; + out_6643036112755764466[315] = 0; + out_6643036112755764466[316] = 0; + out_6643036112755764466[317] = 0; + out_6643036112755764466[318] = 0; + out_6643036112755764466[319] = 0; + out_6643036112755764466[320] = 0; + out_6643036112755764466[321] = 0; + out_6643036112755764466[322] = 0; + out_6643036112755764466[323] = 1; } -void h_4(double *state, double *unused, double *out_2493922315843674136) { - out_2493922315843674136[0] = state[6] + state[9]; - out_2493922315843674136[1] = state[7] + state[10]; - out_2493922315843674136[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_3825151169355500496) { + out_3825151169355500496[0] = state[6] + state[9]; + out_3825151169355500496[1] = state[7] + state[10]; + out_3825151169355500496[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_1093976949342281069) { - out_1093976949342281069[0] = 0; - out_1093976949342281069[1] = 0; - out_1093976949342281069[2] = 0; - out_1093976949342281069[3] = 0; - out_1093976949342281069[4] = 0; - out_1093976949342281069[5] = 0; - out_1093976949342281069[6] = 1; - out_1093976949342281069[7] = 0; - out_1093976949342281069[8] = 0; - out_1093976949342281069[9] = 1; - out_1093976949342281069[10] = 0; - out_1093976949342281069[11] = 0; - out_1093976949342281069[12] = 0; - out_1093976949342281069[13] = 0; - out_1093976949342281069[14] = 0; - out_1093976949342281069[15] = 0; - out_1093976949342281069[16] = 0; - out_1093976949342281069[17] = 0; - out_1093976949342281069[18] = 0; - out_1093976949342281069[19] = 0; - out_1093976949342281069[20] = 0; - out_1093976949342281069[21] = 0; - out_1093976949342281069[22] = 0; - out_1093976949342281069[23] = 0; - out_1093976949342281069[24] = 0; - out_1093976949342281069[25] = 1; - out_1093976949342281069[26] = 0; - out_1093976949342281069[27] = 0; - out_1093976949342281069[28] = 1; - out_1093976949342281069[29] = 0; - out_1093976949342281069[30] = 0; - out_1093976949342281069[31] = 0; - out_1093976949342281069[32] = 0; - out_1093976949342281069[33] = 0; - out_1093976949342281069[34] = 0; - out_1093976949342281069[35] = 0; - out_1093976949342281069[36] = 0; - out_1093976949342281069[37] = 0; - out_1093976949342281069[38] = 0; - out_1093976949342281069[39] = 0; - out_1093976949342281069[40] = 0; - out_1093976949342281069[41] = 0; - out_1093976949342281069[42] = 0; - out_1093976949342281069[43] = 0; - out_1093976949342281069[44] = 1; - out_1093976949342281069[45] = 0; - out_1093976949342281069[46] = 0; - out_1093976949342281069[47] = 1; - out_1093976949342281069[48] = 0; - out_1093976949342281069[49] = 0; - out_1093976949342281069[50] = 0; - out_1093976949342281069[51] = 0; - out_1093976949342281069[52] = 0; - out_1093976949342281069[53] = 0; +void H_4(double *state, double *unused, double *out_4438379387875418186) { + out_4438379387875418186[0] = 0; + out_4438379387875418186[1] = 0; + out_4438379387875418186[2] = 0; + out_4438379387875418186[3] = 0; + out_4438379387875418186[4] = 0; + out_4438379387875418186[5] = 0; + out_4438379387875418186[6] = 1; + out_4438379387875418186[7] = 0; + out_4438379387875418186[8] = 0; + out_4438379387875418186[9] = 1; + out_4438379387875418186[10] = 0; + out_4438379387875418186[11] = 0; + out_4438379387875418186[12] = 0; + out_4438379387875418186[13] = 0; + out_4438379387875418186[14] = 0; + out_4438379387875418186[15] = 0; + out_4438379387875418186[16] = 0; + out_4438379387875418186[17] = 0; + out_4438379387875418186[18] = 0; + out_4438379387875418186[19] = 0; + out_4438379387875418186[20] = 0; + out_4438379387875418186[21] = 0; + out_4438379387875418186[22] = 0; + out_4438379387875418186[23] = 0; + out_4438379387875418186[24] = 0; + out_4438379387875418186[25] = 1; + out_4438379387875418186[26] = 0; + out_4438379387875418186[27] = 0; + out_4438379387875418186[28] = 1; + out_4438379387875418186[29] = 0; + out_4438379387875418186[30] = 0; + out_4438379387875418186[31] = 0; + out_4438379387875418186[32] = 0; + out_4438379387875418186[33] = 0; + out_4438379387875418186[34] = 0; + out_4438379387875418186[35] = 0; + out_4438379387875418186[36] = 0; + out_4438379387875418186[37] = 0; + out_4438379387875418186[38] = 0; + out_4438379387875418186[39] = 0; + out_4438379387875418186[40] = 0; + out_4438379387875418186[41] = 0; + out_4438379387875418186[42] = 0; + out_4438379387875418186[43] = 0; + out_4438379387875418186[44] = 1; + out_4438379387875418186[45] = 0; + out_4438379387875418186[46] = 0; + out_4438379387875418186[47] = 1; + out_4438379387875418186[48] = 0; + out_4438379387875418186[49] = 0; + out_4438379387875418186[50] = 0; + out_4438379387875418186[51] = 0; + out_4438379387875418186[52] = 0; + out_4438379387875418186[53] = 0; } -void h_10(double *state, double *unused, double *out_288808481596522343) { - out_288808481596522343[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_288808481596522343[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_288808481596522343[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_6180343716098328505) { + out_6180343716098328505[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_6180343716098328505[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_6180343716098328505[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_4474711072663792050) { - out_4474711072663792050[0] = 0; - out_4474711072663792050[1] = 9.8100000000000005*cos(state[1]); - out_4474711072663792050[2] = 0; - out_4474711072663792050[3] = 0; - out_4474711072663792050[4] = -state[8]; - out_4474711072663792050[5] = state[7]; - out_4474711072663792050[6] = 0; - out_4474711072663792050[7] = state[5]; - out_4474711072663792050[8] = -state[4]; - out_4474711072663792050[9] = 0; - out_4474711072663792050[10] = 0; - out_4474711072663792050[11] = 0; - out_4474711072663792050[12] = 1; - out_4474711072663792050[13] = 0; - out_4474711072663792050[14] = 0; - out_4474711072663792050[15] = 1; - out_4474711072663792050[16] = 0; - out_4474711072663792050[17] = 0; - out_4474711072663792050[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_4474711072663792050[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_4474711072663792050[20] = 0; - out_4474711072663792050[21] = state[8]; - out_4474711072663792050[22] = 0; - out_4474711072663792050[23] = -state[6]; - out_4474711072663792050[24] = -state[5]; - out_4474711072663792050[25] = 0; - out_4474711072663792050[26] = state[3]; - out_4474711072663792050[27] = 0; - out_4474711072663792050[28] = 0; - out_4474711072663792050[29] = 0; - out_4474711072663792050[30] = 0; - out_4474711072663792050[31] = 1; - out_4474711072663792050[32] = 0; - out_4474711072663792050[33] = 0; - out_4474711072663792050[34] = 1; - out_4474711072663792050[35] = 0; - out_4474711072663792050[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_4474711072663792050[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_4474711072663792050[38] = 0; - out_4474711072663792050[39] = -state[7]; - out_4474711072663792050[40] = state[6]; - out_4474711072663792050[41] = 0; - out_4474711072663792050[42] = state[4]; - out_4474711072663792050[43] = -state[3]; - out_4474711072663792050[44] = 0; - out_4474711072663792050[45] = 0; - out_4474711072663792050[46] = 0; - out_4474711072663792050[47] = 0; - out_4474711072663792050[48] = 0; - out_4474711072663792050[49] = 0; - out_4474711072663792050[50] = 1; - out_4474711072663792050[51] = 0; - out_4474711072663792050[52] = 0; - out_4474711072663792050[53] = 1; +void H_10(double *state, double *unused, double *out_6436902649839043686) { + out_6436902649839043686[0] = 0; + out_6436902649839043686[1] = 9.8100000000000005*cos(state[1]); + out_6436902649839043686[2] = 0; + out_6436902649839043686[3] = 0; + out_6436902649839043686[4] = -state[8]; + out_6436902649839043686[5] = state[7]; + out_6436902649839043686[6] = 0; + out_6436902649839043686[7] = state[5]; + out_6436902649839043686[8] = -state[4]; + out_6436902649839043686[9] = 0; + out_6436902649839043686[10] = 0; + out_6436902649839043686[11] = 0; + out_6436902649839043686[12] = 1; + out_6436902649839043686[13] = 0; + out_6436902649839043686[14] = 0; + out_6436902649839043686[15] = 1; + out_6436902649839043686[16] = 0; + out_6436902649839043686[17] = 0; + out_6436902649839043686[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_6436902649839043686[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_6436902649839043686[20] = 0; + out_6436902649839043686[21] = state[8]; + out_6436902649839043686[22] = 0; + out_6436902649839043686[23] = -state[6]; + out_6436902649839043686[24] = -state[5]; + out_6436902649839043686[25] = 0; + out_6436902649839043686[26] = state[3]; + out_6436902649839043686[27] = 0; + out_6436902649839043686[28] = 0; + out_6436902649839043686[29] = 0; + out_6436902649839043686[30] = 0; + out_6436902649839043686[31] = 1; + out_6436902649839043686[32] = 0; + out_6436902649839043686[33] = 0; + out_6436902649839043686[34] = 1; + out_6436902649839043686[35] = 0; + out_6436902649839043686[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_6436902649839043686[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_6436902649839043686[38] = 0; + out_6436902649839043686[39] = -state[7]; + out_6436902649839043686[40] = state[6]; + out_6436902649839043686[41] = 0; + out_6436902649839043686[42] = state[4]; + out_6436902649839043686[43] = -state[3]; + out_6436902649839043686[44] = 0; + out_6436902649839043686[45] = 0; + out_6436902649839043686[46] = 0; + out_6436902649839043686[47] = 0; + out_6436902649839043686[48] = 0; + out_6436902649839043686[49] = 0; + out_6436902649839043686[50] = 1; + out_6436902649839043686[51] = 0; + out_6436902649839043686[52] = 0; + out_6436902649839043686[53] = 1; } -void h_13(double *state, double *unused, double *out_5108938482451502751) { - out_5108938482451502751[0] = state[3]; - out_5108938482451502751[1] = state[4]; - out_5108938482451502751[2] = state[5]; +void h_13(double *state, double *unused, double *out_5615804281251360017) { + out_5615804281251360017[0] = state[3]; + out_5615804281251360017[1] = state[4]; + out_5615804281251360017[2] = state[5]; } -void H_13(double *state, double *unused, double *out_2118296875990051732) { - out_2118296875990051732[0] = 0; - out_2118296875990051732[1] = 0; - out_2118296875990051732[2] = 0; - out_2118296875990051732[3] = 1; - out_2118296875990051732[4] = 0; - out_2118296875990051732[5] = 0; - out_2118296875990051732[6] = 0; - out_2118296875990051732[7] = 0; - out_2118296875990051732[8] = 0; - out_2118296875990051732[9] = 0; - out_2118296875990051732[10] = 0; - out_2118296875990051732[11] = 0; - out_2118296875990051732[12] = 0; - out_2118296875990051732[13] = 0; - out_2118296875990051732[14] = 0; - out_2118296875990051732[15] = 0; - out_2118296875990051732[16] = 0; - out_2118296875990051732[17] = 0; - out_2118296875990051732[18] = 0; - out_2118296875990051732[19] = 0; - out_2118296875990051732[20] = 0; - out_2118296875990051732[21] = 0; - out_2118296875990051732[22] = 1; - out_2118296875990051732[23] = 0; - out_2118296875990051732[24] = 0; - out_2118296875990051732[25] = 0; - out_2118296875990051732[26] = 0; - out_2118296875990051732[27] = 0; - out_2118296875990051732[28] = 0; - out_2118296875990051732[29] = 0; - out_2118296875990051732[30] = 0; - out_2118296875990051732[31] = 0; - out_2118296875990051732[32] = 0; - out_2118296875990051732[33] = 0; - out_2118296875990051732[34] = 0; - out_2118296875990051732[35] = 0; - out_2118296875990051732[36] = 0; - out_2118296875990051732[37] = 0; - out_2118296875990051732[38] = 0; - out_2118296875990051732[39] = 0; - out_2118296875990051732[40] = 0; - out_2118296875990051732[41] = 1; - out_2118296875990051732[42] = 0; - out_2118296875990051732[43] = 0; - out_2118296875990051732[44] = 0; - out_2118296875990051732[45] = 0; - out_2118296875990051732[46] = 0; - out_2118296875990051732[47] = 0; - out_2118296875990051732[48] = 0; - out_2118296875990051732[49] = 0; - out_2118296875990051732[50] = 0; - out_2118296875990051732[51] = 0; - out_2118296875990051732[52] = 0; - out_2118296875990051732[53] = 0; +void H_13(double *state, double *unused, double *out_8272134851177942210) { + out_8272134851177942210[0] = 0; + out_8272134851177942210[1] = 0; + out_8272134851177942210[2] = 0; + out_8272134851177942210[3] = 1; + out_8272134851177942210[4] = 0; + out_8272134851177942210[5] = 0; + out_8272134851177942210[6] = 0; + out_8272134851177942210[7] = 0; + out_8272134851177942210[8] = 0; + out_8272134851177942210[9] = 0; + out_8272134851177942210[10] = 0; + out_8272134851177942210[11] = 0; + out_8272134851177942210[12] = 0; + out_8272134851177942210[13] = 0; + out_8272134851177942210[14] = 0; + out_8272134851177942210[15] = 0; + out_8272134851177942210[16] = 0; + out_8272134851177942210[17] = 0; + out_8272134851177942210[18] = 0; + out_8272134851177942210[19] = 0; + out_8272134851177942210[20] = 0; + out_8272134851177942210[21] = 0; + out_8272134851177942210[22] = 1; + out_8272134851177942210[23] = 0; + out_8272134851177942210[24] = 0; + out_8272134851177942210[25] = 0; + out_8272134851177942210[26] = 0; + out_8272134851177942210[27] = 0; + out_8272134851177942210[28] = 0; + out_8272134851177942210[29] = 0; + out_8272134851177942210[30] = 0; + out_8272134851177942210[31] = 0; + out_8272134851177942210[32] = 0; + out_8272134851177942210[33] = 0; + out_8272134851177942210[34] = 0; + out_8272134851177942210[35] = 0; + out_8272134851177942210[36] = 0; + out_8272134851177942210[37] = 0; + out_8272134851177942210[38] = 0; + out_8272134851177942210[39] = 0; + out_8272134851177942210[40] = 0; + out_8272134851177942210[41] = 1; + out_8272134851177942210[42] = 0; + out_8272134851177942210[43] = 0; + out_8272134851177942210[44] = 0; + out_8272134851177942210[45] = 0; + out_8272134851177942210[46] = 0; + out_8272134851177942210[47] = 0; + out_8272134851177942210[48] = 0; + out_8272134851177942210[49] = 0; + out_8272134851177942210[50] = 0; + out_8272134851177942210[51] = 0; + out_8272134851177942210[52] = 0; + out_8272134851177942210[53] = 0; } -void h_14(double *state, double *unused, double *out_5863792870354861000) { - out_5863792870354861000[0] = state[6]; - out_5863792870354861000[1] = state[7]; - out_5863792870354861000[2] = state[8]; +void h_14(double *state, double *unused, double *out_1282457731118130678) { + out_1282457731118130678[0] = state[6]; + out_1282457731118130678[1] = state[7]; + out_1282457731118130678[2] = state[8]; } -void H_14(double *state, double *unused, double *out_2869263906997203460) { - out_2869263906997203460[0] = 0; - out_2869263906997203460[1] = 0; - out_2869263906997203460[2] = 0; - out_2869263906997203460[3] = 0; - out_2869263906997203460[4] = 0; - out_2869263906997203460[5] = 0; - out_2869263906997203460[6] = 1; - out_2869263906997203460[7] = 0; - out_2869263906997203460[8] = 0; - out_2869263906997203460[9] = 0; - out_2869263906997203460[10] = 0; - out_2869263906997203460[11] = 0; - out_2869263906997203460[12] = 0; - out_2869263906997203460[13] = 0; - out_2869263906997203460[14] = 0; - out_2869263906997203460[15] = 0; - out_2869263906997203460[16] = 0; - out_2869263906997203460[17] = 0; - out_2869263906997203460[18] = 0; - out_2869263906997203460[19] = 0; - out_2869263906997203460[20] = 0; - out_2869263906997203460[21] = 0; - out_2869263906997203460[22] = 0; - out_2869263906997203460[23] = 0; - out_2869263906997203460[24] = 0; - out_2869263906997203460[25] = 1; - out_2869263906997203460[26] = 0; - out_2869263906997203460[27] = 0; - out_2869263906997203460[28] = 0; - out_2869263906997203460[29] = 0; - out_2869263906997203460[30] = 0; - out_2869263906997203460[31] = 0; - out_2869263906997203460[32] = 0; - out_2869263906997203460[33] = 0; - out_2869263906997203460[34] = 0; - out_2869263906997203460[35] = 0; - out_2869263906997203460[36] = 0; - out_2869263906997203460[37] = 0; - out_2869263906997203460[38] = 0; - out_2869263906997203460[39] = 0; - out_2869263906997203460[40] = 0; - out_2869263906997203460[41] = 0; - out_2869263906997203460[42] = 0; - out_2869263906997203460[43] = 0; - out_2869263906997203460[44] = 1; - out_2869263906997203460[45] = 0; - out_2869263906997203460[46] = 0; - out_2869263906997203460[47] = 0; - out_2869263906997203460[48] = 0; - out_2869263906997203460[49] = 0; - out_2869263906997203460[50] = 0; - out_2869263906997203460[51] = 0; - out_2869263906997203460[52] = 0; - out_2869263906997203460[53] = 0; +void H_14(double *state, double *unused, double *out_7521167820170790482) { + out_7521167820170790482[0] = 0; + out_7521167820170790482[1] = 0; + out_7521167820170790482[2] = 0; + out_7521167820170790482[3] = 0; + out_7521167820170790482[4] = 0; + out_7521167820170790482[5] = 0; + out_7521167820170790482[6] = 1; + out_7521167820170790482[7] = 0; + out_7521167820170790482[8] = 0; + out_7521167820170790482[9] = 0; + out_7521167820170790482[10] = 0; + out_7521167820170790482[11] = 0; + out_7521167820170790482[12] = 0; + out_7521167820170790482[13] = 0; + out_7521167820170790482[14] = 0; + out_7521167820170790482[15] = 0; + out_7521167820170790482[16] = 0; + out_7521167820170790482[17] = 0; + out_7521167820170790482[18] = 0; + out_7521167820170790482[19] = 0; + out_7521167820170790482[20] = 0; + out_7521167820170790482[21] = 0; + out_7521167820170790482[22] = 0; + out_7521167820170790482[23] = 0; + out_7521167820170790482[24] = 0; + out_7521167820170790482[25] = 1; + out_7521167820170790482[26] = 0; + out_7521167820170790482[27] = 0; + out_7521167820170790482[28] = 0; + out_7521167820170790482[29] = 0; + out_7521167820170790482[30] = 0; + out_7521167820170790482[31] = 0; + out_7521167820170790482[32] = 0; + out_7521167820170790482[33] = 0; + out_7521167820170790482[34] = 0; + out_7521167820170790482[35] = 0; + out_7521167820170790482[36] = 0; + out_7521167820170790482[37] = 0; + out_7521167820170790482[38] = 0; + out_7521167820170790482[39] = 0; + out_7521167820170790482[40] = 0; + out_7521167820170790482[41] = 0; + out_7521167820170790482[42] = 0; + out_7521167820170790482[43] = 0; + out_7521167820170790482[44] = 1; + out_7521167820170790482[45] = 0; + out_7521167820170790482[46] = 0; + out_7521167820170790482[47] = 0; + out_7521167820170790482[48] = 0; + out_7521167820170790482[49] = 0; + out_7521167820170790482[50] = 0; + out_7521167820170790482[51] = 0; + out_7521167820170790482[52] = 0; + out_7521167820170790482[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_6009088715125097596) { - err_fun(nom_x, delta_x, out_6009088715125097596); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2383575066684203465) { + err_fun(nom_x, delta_x, out_2383575066684203465); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5460684038605246490) { - inv_err_fun(nom_x, true_x, out_5460684038605246490); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2531792213831641268) { + inv_err_fun(nom_x, true_x, out_2531792213831641268); } -void pose_H_mod_fun(double *state, double *out_2180245129448668184) { - H_mod_fun(state, out_2180245129448668184); +void pose_H_mod_fun(double *state, double *out_5270168453751144292) { + H_mod_fun(state, out_5270168453751144292); } -void pose_f_fun(double *state, double dt, double *out_1359935913624561504) { - f_fun(state, dt, out_1359935913624561504); +void pose_f_fun(double *state, double dt, double *out_5870118476100851027) { + f_fun(state, dt, out_5870118476100851027); } -void pose_F_fun(double *state, double dt, double *out_3401373591342797666) { - F_fun(state, dt, out_3401373591342797666); +void pose_F_fun(double *state, double dt, double *out_6643036112755764466) { + F_fun(state, dt, out_6643036112755764466); } -void pose_h_4(double *state, double *unused, double *out_2493922315843674136) { - h_4(state, unused, out_2493922315843674136); +void pose_h_4(double *state, double *unused, double *out_3825151169355500496) { + h_4(state, unused, out_3825151169355500496); } -void pose_H_4(double *state, double *unused, double *out_1093976949342281069) { - H_4(state, unused, out_1093976949342281069); +void pose_H_4(double *state, double *unused, double *out_4438379387875418186) { + H_4(state, unused, out_4438379387875418186); } -void pose_h_10(double *state, double *unused, double *out_288808481596522343) { - h_10(state, unused, out_288808481596522343); +void pose_h_10(double *state, double *unused, double *out_6180343716098328505) { + h_10(state, unused, out_6180343716098328505); } -void pose_H_10(double *state, double *unused, double *out_4474711072663792050) { - H_10(state, unused, out_4474711072663792050); +void pose_H_10(double *state, double *unused, double *out_6436902649839043686) { + H_10(state, unused, out_6436902649839043686); } -void pose_h_13(double *state, double *unused, double *out_5108938482451502751) { - h_13(state, unused, out_5108938482451502751); +void pose_h_13(double *state, double *unused, double *out_5615804281251360017) { + h_13(state, unused, out_5615804281251360017); } -void pose_H_13(double *state, double *unused, double *out_2118296875990051732) { - H_13(state, unused, out_2118296875990051732); +void pose_H_13(double *state, double *unused, double *out_8272134851177942210) { + H_13(state, unused, out_8272134851177942210); } -void pose_h_14(double *state, double *unused, double *out_5863792870354861000) { - h_14(state, unused, out_5863792870354861000); +void pose_h_14(double *state, double *unused, double *out_1282457731118130678) { + h_14(state, unused, out_1282457731118130678); } -void pose_H_14(double *state, double *unused, double *out_2869263906997203460) { - H_14(state, unused, out_2869263906997203460); +void pose_H_14(double *state, double *unused, double *out_7521167820170790482) { + H_14(state, unused, out_7521167820170790482); } 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 fb3aeec79..5dadeaf60 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_6009088715125097596); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5460684038605246490); -void pose_H_mod_fun(double *state, double *out_2180245129448668184); -void pose_f_fun(double *state, double dt, double *out_1359935913624561504); -void pose_F_fun(double *state, double dt, double *out_3401373591342797666); -void pose_h_4(double *state, double *unused, double *out_2493922315843674136); -void pose_H_4(double *state, double *unused, double *out_1093976949342281069); -void pose_h_10(double *state, double *unused, double *out_288808481596522343); -void pose_H_10(double *state, double *unused, double *out_4474711072663792050); -void pose_h_13(double *state, double *unused, double *out_5108938482451502751); -void pose_H_13(double *state, double *unused, double *out_2118296875990051732); -void pose_h_14(double *state, double *unused, double *out_5863792870354861000); -void pose_H_14(double *state, double *unused, double *out_2869263906997203460); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2383575066684203465); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2531792213831641268); +void pose_H_mod_fun(double *state, double *out_5270168453751144292); +void pose_f_fun(double *state, double dt, double *out_5870118476100851027); +void pose_F_fun(double *state, double dt, double *out_6643036112755764466); +void pose_h_4(double *state, double *unused, double *out_3825151169355500496); +void pose_H_4(double *state, double *unused, double *out_4438379387875418186); +void pose_h_10(double *state, double *unused, double *out_6180343716098328505); +void pose_H_10(double *state, double *unused, double *out_6436902649839043686); +void pose_h_13(double *state, double *unused, double *out_5615804281251360017); +void pose_H_13(double *state, double *unused, double *out_8272134851177942210); +void pose_h_14(double *state, double *unused, double *out_1282457731118130678); +void pose_H_14(double *state, double *unused, double *out_7521167820170790482); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index d168c8528..7ff571faf 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ