diff --git a/common/libcommon.a b/common/libcommon.a index dfcfa9374..a7970a361 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index 76e123f8e..4ac4c8f31 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index a31f4fa0b..001e406e3 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 54dc56d49..b81a8223e 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 b505335b6..8976513a9 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 ff0007bdd..aee4a357c 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 8f396165c..a8cffc805 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 19d81659f..7607bc3e9 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 16486dcf8..79b9a36df 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 16486dcf8..79b9a36df 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 626678479..811c1eb46 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 19d81659f..7607bc3e9 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 b2b78fb13..34f245225 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-7a17c21f-DEBUG"; +const uint8_t gitversion[19] = "DEV-88c259a6-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 6375b7e77..4f55ed142 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 b26999f28..03814e91b 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 a094d7062..b2a36a024 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 78773d207..dc5b949df 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 cd11af645..bdd85acfc 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 bef3a4ce5..2d95638fb 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 4bb20a4bf..f47329187 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 66ef0ac1d..e4d7a5c8e 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 6ffdcffc0..2a7e1d0c2 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 1915a5573..d90699525 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 b0e96a4fc..a2e0cb8b3 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 11c2e5e23..cb8f0b680 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 3e26a367f..b25958a13 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 48297d538..c8b083e64 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 5b8567ae8..b12e2a27f 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 84d40c9c8..3dd886c58 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 e5eeac046..15baff923 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 460a16596..d319318fb 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 a9f75f76a..d9e9f6a9c 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 af121604e..fcfc6b7cd 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 ffc04fd7f..e2931a4d9 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-7a17c21f-DEBUG \ No newline at end of file +DEV-88c259a6-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 153f1a384..e0890f46b 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_4271848112759039807) { - out_4271848112759039807[0] = delta_x[0] + nom_x[0]; - out_4271848112759039807[1] = delta_x[1] + nom_x[1]; - out_4271848112759039807[2] = delta_x[2] + nom_x[2]; - out_4271848112759039807[3] = delta_x[3] + nom_x[3]; - out_4271848112759039807[4] = delta_x[4] + nom_x[4]; - out_4271848112759039807[5] = delta_x[5] + nom_x[5]; - out_4271848112759039807[6] = delta_x[6] + nom_x[6]; - out_4271848112759039807[7] = delta_x[7] + nom_x[7]; - out_4271848112759039807[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_3281150270443704084) { + out_3281150270443704084[0] = delta_x[0] + nom_x[0]; + out_3281150270443704084[1] = delta_x[1] + nom_x[1]; + out_3281150270443704084[2] = delta_x[2] + nom_x[2]; + out_3281150270443704084[3] = delta_x[3] + nom_x[3]; + out_3281150270443704084[4] = delta_x[4] + nom_x[4]; + out_3281150270443704084[5] = delta_x[5] + nom_x[5]; + out_3281150270443704084[6] = delta_x[6] + nom_x[6]; + out_3281150270443704084[7] = delta_x[7] + nom_x[7]; + out_3281150270443704084[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_2031572598342150124) { - out_2031572598342150124[0] = -nom_x[0] + true_x[0]; - out_2031572598342150124[1] = -nom_x[1] + true_x[1]; - out_2031572598342150124[2] = -nom_x[2] + true_x[2]; - out_2031572598342150124[3] = -nom_x[3] + true_x[3]; - out_2031572598342150124[4] = -nom_x[4] + true_x[4]; - out_2031572598342150124[5] = -nom_x[5] + true_x[5]; - out_2031572598342150124[6] = -nom_x[6] + true_x[6]; - out_2031572598342150124[7] = -nom_x[7] + true_x[7]; - out_2031572598342150124[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3048073845424101937) { + out_3048073845424101937[0] = -nom_x[0] + true_x[0]; + out_3048073845424101937[1] = -nom_x[1] + true_x[1]; + out_3048073845424101937[2] = -nom_x[2] + true_x[2]; + out_3048073845424101937[3] = -nom_x[3] + true_x[3]; + out_3048073845424101937[4] = -nom_x[4] + true_x[4]; + out_3048073845424101937[5] = -nom_x[5] + true_x[5]; + out_3048073845424101937[6] = -nom_x[6] + true_x[6]; + out_3048073845424101937[7] = -nom_x[7] + true_x[7]; + out_3048073845424101937[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_177684180657030354) { - out_177684180657030354[0] = 1.0; - out_177684180657030354[1] = 0.0; - out_177684180657030354[2] = 0.0; - out_177684180657030354[3] = 0.0; - out_177684180657030354[4] = 0.0; - out_177684180657030354[5] = 0.0; - out_177684180657030354[6] = 0.0; - out_177684180657030354[7] = 0.0; - out_177684180657030354[8] = 0.0; - out_177684180657030354[9] = 0.0; - out_177684180657030354[10] = 1.0; - out_177684180657030354[11] = 0.0; - out_177684180657030354[12] = 0.0; - out_177684180657030354[13] = 0.0; - out_177684180657030354[14] = 0.0; - out_177684180657030354[15] = 0.0; - out_177684180657030354[16] = 0.0; - out_177684180657030354[17] = 0.0; - out_177684180657030354[18] = 0.0; - out_177684180657030354[19] = 0.0; - out_177684180657030354[20] = 1.0; - out_177684180657030354[21] = 0.0; - out_177684180657030354[22] = 0.0; - out_177684180657030354[23] = 0.0; - out_177684180657030354[24] = 0.0; - out_177684180657030354[25] = 0.0; - out_177684180657030354[26] = 0.0; - out_177684180657030354[27] = 0.0; - out_177684180657030354[28] = 0.0; - out_177684180657030354[29] = 0.0; - out_177684180657030354[30] = 1.0; - out_177684180657030354[31] = 0.0; - out_177684180657030354[32] = 0.0; - out_177684180657030354[33] = 0.0; - out_177684180657030354[34] = 0.0; - out_177684180657030354[35] = 0.0; - out_177684180657030354[36] = 0.0; - out_177684180657030354[37] = 0.0; - out_177684180657030354[38] = 0.0; - out_177684180657030354[39] = 0.0; - out_177684180657030354[40] = 1.0; - out_177684180657030354[41] = 0.0; - out_177684180657030354[42] = 0.0; - out_177684180657030354[43] = 0.0; - out_177684180657030354[44] = 0.0; - out_177684180657030354[45] = 0.0; - out_177684180657030354[46] = 0.0; - out_177684180657030354[47] = 0.0; - out_177684180657030354[48] = 0.0; - out_177684180657030354[49] = 0.0; - out_177684180657030354[50] = 1.0; - out_177684180657030354[51] = 0.0; - out_177684180657030354[52] = 0.0; - out_177684180657030354[53] = 0.0; - out_177684180657030354[54] = 0.0; - out_177684180657030354[55] = 0.0; - out_177684180657030354[56] = 0.0; - out_177684180657030354[57] = 0.0; - out_177684180657030354[58] = 0.0; - out_177684180657030354[59] = 0.0; - out_177684180657030354[60] = 1.0; - out_177684180657030354[61] = 0.0; - out_177684180657030354[62] = 0.0; - out_177684180657030354[63] = 0.0; - out_177684180657030354[64] = 0.0; - out_177684180657030354[65] = 0.0; - out_177684180657030354[66] = 0.0; - out_177684180657030354[67] = 0.0; - out_177684180657030354[68] = 0.0; - out_177684180657030354[69] = 0.0; - out_177684180657030354[70] = 1.0; - out_177684180657030354[71] = 0.0; - out_177684180657030354[72] = 0.0; - out_177684180657030354[73] = 0.0; - out_177684180657030354[74] = 0.0; - out_177684180657030354[75] = 0.0; - out_177684180657030354[76] = 0.0; - out_177684180657030354[77] = 0.0; - out_177684180657030354[78] = 0.0; - out_177684180657030354[79] = 0.0; - out_177684180657030354[80] = 1.0; +void H_mod_fun(double *state, double *out_5861733525303608227) { + out_5861733525303608227[0] = 1.0; + out_5861733525303608227[1] = 0.0; + out_5861733525303608227[2] = 0.0; + out_5861733525303608227[3] = 0.0; + out_5861733525303608227[4] = 0.0; + out_5861733525303608227[5] = 0.0; + out_5861733525303608227[6] = 0.0; + out_5861733525303608227[7] = 0.0; + out_5861733525303608227[8] = 0.0; + out_5861733525303608227[9] = 0.0; + out_5861733525303608227[10] = 1.0; + out_5861733525303608227[11] = 0.0; + out_5861733525303608227[12] = 0.0; + out_5861733525303608227[13] = 0.0; + out_5861733525303608227[14] = 0.0; + out_5861733525303608227[15] = 0.0; + out_5861733525303608227[16] = 0.0; + out_5861733525303608227[17] = 0.0; + out_5861733525303608227[18] = 0.0; + out_5861733525303608227[19] = 0.0; + out_5861733525303608227[20] = 1.0; + out_5861733525303608227[21] = 0.0; + out_5861733525303608227[22] = 0.0; + out_5861733525303608227[23] = 0.0; + out_5861733525303608227[24] = 0.0; + out_5861733525303608227[25] = 0.0; + out_5861733525303608227[26] = 0.0; + out_5861733525303608227[27] = 0.0; + out_5861733525303608227[28] = 0.0; + out_5861733525303608227[29] = 0.0; + out_5861733525303608227[30] = 1.0; + out_5861733525303608227[31] = 0.0; + out_5861733525303608227[32] = 0.0; + out_5861733525303608227[33] = 0.0; + out_5861733525303608227[34] = 0.0; + out_5861733525303608227[35] = 0.0; + out_5861733525303608227[36] = 0.0; + out_5861733525303608227[37] = 0.0; + out_5861733525303608227[38] = 0.0; + out_5861733525303608227[39] = 0.0; + out_5861733525303608227[40] = 1.0; + out_5861733525303608227[41] = 0.0; + out_5861733525303608227[42] = 0.0; + out_5861733525303608227[43] = 0.0; + out_5861733525303608227[44] = 0.0; + out_5861733525303608227[45] = 0.0; + out_5861733525303608227[46] = 0.0; + out_5861733525303608227[47] = 0.0; + out_5861733525303608227[48] = 0.0; + out_5861733525303608227[49] = 0.0; + out_5861733525303608227[50] = 1.0; + out_5861733525303608227[51] = 0.0; + out_5861733525303608227[52] = 0.0; + out_5861733525303608227[53] = 0.0; + out_5861733525303608227[54] = 0.0; + out_5861733525303608227[55] = 0.0; + out_5861733525303608227[56] = 0.0; + out_5861733525303608227[57] = 0.0; + out_5861733525303608227[58] = 0.0; + out_5861733525303608227[59] = 0.0; + out_5861733525303608227[60] = 1.0; + out_5861733525303608227[61] = 0.0; + out_5861733525303608227[62] = 0.0; + out_5861733525303608227[63] = 0.0; + out_5861733525303608227[64] = 0.0; + out_5861733525303608227[65] = 0.0; + out_5861733525303608227[66] = 0.0; + out_5861733525303608227[67] = 0.0; + out_5861733525303608227[68] = 0.0; + out_5861733525303608227[69] = 0.0; + out_5861733525303608227[70] = 1.0; + out_5861733525303608227[71] = 0.0; + out_5861733525303608227[72] = 0.0; + out_5861733525303608227[73] = 0.0; + out_5861733525303608227[74] = 0.0; + out_5861733525303608227[75] = 0.0; + out_5861733525303608227[76] = 0.0; + out_5861733525303608227[77] = 0.0; + out_5861733525303608227[78] = 0.0; + out_5861733525303608227[79] = 0.0; + out_5861733525303608227[80] = 1.0; } -void f_fun(double *state, double dt, double *out_6521764996120993087) { - out_6521764996120993087[0] = state[0]; - out_6521764996120993087[1] = state[1]; - out_6521764996120993087[2] = state[2]; - out_6521764996120993087[3] = state[3]; - out_6521764996120993087[4] = state[4]; - out_6521764996120993087[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_6521764996120993087[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_6521764996120993087[7] = state[7]; - out_6521764996120993087[8] = state[8]; +void f_fun(double *state, double dt, double *out_6520674902136446798) { + out_6520674902136446798[0] = state[0]; + out_6520674902136446798[1] = state[1]; + out_6520674902136446798[2] = state[2]; + out_6520674902136446798[3] = state[3]; + out_6520674902136446798[4] = state[4]; + out_6520674902136446798[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_6520674902136446798[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_6520674902136446798[7] = state[7]; + out_6520674902136446798[8] = state[8]; } -void F_fun(double *state, double dt, double *out_4624398414228863854) { - out_4624398414228863854[0] = 1; - out_4624398414228863854[1] = 0; - out_4624398414228863854[2] = 0; - out_4624398414228863854[3] = 0; - out_4624398414228863854[4] = 0; - out_4624398414228863854[5] = 0; - out_4624398414228863854[6] = 0; - out_4624398414228863854[7] = 0; - out_4624398414228863854[8] = 0; - out_4624398414228863854[9] = 0; - out_4624398414228863854[10] = 1; - out_4624398414228863854[11] = 0; - out_4624398414228863854[12] = 0; - out_4624398414228863854[13] = 0; - out_4624398414228863854[14] = 0; - out_4624398414228863854[15] = 0; - out_4624398414228863854[16] = 0; - out_4624398414228863854[17] = 0; - out_4624398414228863854[18] = 0; - out_4624398414228863854[19] = 0; - out_4624398414228863854[20] = 1; - out_4624398414228863854[21] = 0; - out_4624398414228863854[22] = 0; - out_4624398414228863854[23] = 0; - out_4624398414228863854[24] = 0; - out_4624398414228863854[25] = 0; - out_4624398414228863854[26] = 0; - out_4624398414228863854[27] = 0; - out_4624398414228863854[28] = 0; - out_4624398414228863854[29] = 0; - out_4624398414228863854[30] = 1; - out_4624398414228863854[31] = 0; - out_4624398414228863854[32] = 0; - out_4624398414228863854[33] = 0; - out_4624398414228863854[34] = 0; - out_4624398414228863854[35] = 0; - out_4624398414228863854[36] = 0; - out_4624398414228863854[37] = 0; - out_4624398414228863854[38] = 0; - out_4624398414228863854[39] = 0; - out_4624398414228863854[40] = 1; - out_4624398414228863854[41] = 0; - out_4624398414228863854[42] = 0; - out_4624398414228863854[43] = 0; - out_4624398414228863854[44] = 0; - out_4624398414228863854[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_4624398414228863854[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_4624398414228863854[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4624398414228863854[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4624398414228863854[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_4624398414228863854[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_4624398414228863854[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_4624398414228863854[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_4624398414228863854[53] = -9.8100000000000005*dt; - out_4624398414228863854[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_4624398414228863854[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_4624398414228863854[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4624398414228863854[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4624398414228863854[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_4624398414228863854[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_4624398414228863854[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_4624398414228863854[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4624398414228863854[62] = 0; - out_4624398414228863854[63] = 0; - out_4624398414228863854[64] = 0; - out_4624398414228863854[65] = 0; - out_4624398414228863854[66] = 0; - out_4624398414228863854[67] = 0; - out_4624398414228863854[68] = 0; - out_4624398414228863854[69] = 0; - out_4624398414228863854[70] = 1; - out_4624398414228863854[71] = 0; - out_4624398414228863854[72] = 0; - out_4624398414228863854[73] = 0; - out_4624398414228863854[74] = 0; - out_4624398414228863854[75] = 0; - out_4624398414228863854[76] = 0; - out_4624398414228863854[77] = 0; - out_4624398414228863854[78] = 0; - out_4624398414228863854[79] = 0; - out_4624398414228863854[80] = 1; +void F_fun(double *state, double dt, double *out_2036702211381872187) { + out_2036702211381872187[0] = 1; + out_2036702211381872187[1] = 0; + out_2036702211381872187[2] = 0; + out_2036702211381872187[3] = 0; + out_2036702211381872187[4] = 0; + out_2036702211381872187[5] = 0; + out_2036702211381872187[6] = 0; + out_2036702211381872187[7] = 0; + out_2036702211381872187[8] = 0; + out_2036702211381872187[9] = 0; + out_2036702211381872187[10] = 1; + out_2036702211381872187[11] = 0; + out_2036702211381872187[12] = 0; + out_2036702211381872187[13] = 0; + out_2036702211381872187[14] = 0; + out_2036702211381872187[15] = 0; + out_2036702211381872187[16] = 0; + out_2036702211381872187[17] = 0; + out_2036702211381872187[18] = 0; + out_2036702211381872187[19] = 0; + out_2036702211381872187[20] = 1; + out_2036702211381872187[21] = 0; + out_2036702211381872187[22] = 0; + out_2036702211381872187[23] = 0; + out_2036702211381872187[24] = 0; + out_2036702211381872187[25] = 0; + out_2036702211381872187[26] = 0; + out_2036702211381872187[27] = 0; + out_2036702211381872187[28] = 0; + out_2036702211381872187[29] = 0; + out_2036702211381872187[30] = 1; + out_2036702211381872187[31] = 0; + out_2036702211381872187[32] = 0; + out_2036702211381872187[33] = 0; + out_2036702211381872187[34] = 0; + out_2036702211381872187[35] = 0; + out_2036702211381872187[36] = 0; + out_2036702211381872187[37] = 0; + out_2036702211381872187[38] = 0; + out_2036702211381872187[39] = 0; + out_2036702211381872187[40] = 1; + out_2036702211381872187[41] = 0; + out_2036702211381872187[42] = 0; + out_2036702211381872187[43] = 0; + out_2036702211381872187[44] = 0; + out_2036702211381872187[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_2036702211381872187[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2036702211381872187[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2036702211381872187[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2036702211381872187[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_2036702211381872187[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2036702211381872187[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2036702211381872187[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2036702211381872187[53] = -9.8100000000000005*dt; + out_2036702211381872187[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_2036702211381872187[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2036702211381872187[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2036702211381872187[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2036702211381872187[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_2036702211381872187[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2036702211381872187[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_2036702211381872187[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2036702211381872187[62] = 0; + out_2036702211381872187[63] = 0; + out_2036702211381872187[64] = 0; + out_2036702211381872187[65] = 0; + out_2036702211381872187[66] = 0; + out_2036702211381872187[67] = 0; + out_2036702211381872187[68] = 0; + out_2036702211381872187[69] = 0; + out_2036702211381872187[70] = 1; + out_2036702211381872187[71] = 0; + out_2036702211381872187[72] = 0; + out_2036702211381872187[73] = 0; + out_2036702211381872187[74] = 0; + out_2036702211381872187[75] = 0; + out_2036702211381872187[76] = 0; + out_2036702211381872187[77] = 0; + out_2036702211381872187[78] = 0; + out_2036702211381872187[79] = 0; + out_2036702211381872187[80] = 1; } -void h_25(double *state, double *unused, double *out_5192262937518193843) { - out_5192262937518193843[0] = state[6]; +void h_25(double *state, double *unused, double *out_7733856010300149046) { + out_7733856010300149046[0] = state[6]; } -void H_25(double *state, double *unused, double *out_6857380300493335265) { - out_6857380300493335265[0] = 0; - out_6857380300493335265[1] = 0; - out_6857380300493335265[2] = 0; - out_6857380300493335265[3] = 0; - out_6857380300493335265[4] = 0; - out_6857380300493335265[5] = 0; - out_6857380300493335265[6] = 1; - out_6857380300493335265[7] = 0; - out_6857380300493335265[8] = 0; +void H_25(double *state, double *unused, double *out_6810854447332090436) { + out_6810854447332090436[0] = 0; + out_6810854447332090436[1] = 0; + out_6810854447332090436[2] = 0; + out_6810854447332090436[3] = 0; + out_6810854447332090436[4] = 0; + out_6810854447332090436[5] = 0; + out_6810854447332090436[6] = 1; + out_6810854447332090436[7] = 0; + out_6810854447332090436[8] = 0; } -void h_24(double *state, double *unused, double *out_6066922532331985124) { - out_6066922532331985124[0] = state[4]; - out_6066922532331985124[1] = state[5]; +void h_24(double *state, double *unused, double *out_8048847696560640687) { + out_8048847696560640687[0] = state[4]; + out_8048847696560640687[1] = state[5]; } -void H_24(double *state, double *unused, double *out_4684730701487835699) { - out_4684730701487835699[0] = 0; - out_4684730701487835699[1] = 0; - out_4684730701487835699[2] = 0; - out_4684730701487835699[3] = 0; - out_4684730701487835699[4] = 1; - out_4684730701487835699[5] = 0; - out_4684730701487835699[6] = 0; - out_4684730701487835699[7] = 0; - out_4684730701487835699[8] = 0; - out_4684730701487835699[9] = 0; - out_4684730701487835699[10] = 0; - out_4684730701487835699[11] = 0; - out_4684730701487835699[12] = 0; - out_4684730701487835699[13] = 0; - out_4684730701487835699[14] = 1; - out_4684730701487835699[15] = 0; - out_4684730701487835699[16] = 0; - out_4684730701487835699[17] = 0; +void H_24(double *state, double *unused, double *out_2407824440308265955) { + out_2407824440308265955[0] = 0; + out_2407824440308265955[1] = 0; + out_2407824440308265955[2] = 0; + out_2407824440308265955[3] = 0; + out_2407824440308265955[4] = 1; + out_2407824440308265955[5] = 0; + out_2407824440308265955[6] = 0; + out_2407824440308265955[7] = 0; + out_2407824440308265955[8] = 0; + out_2407824440308265955[9] = 0; + out_2407824440308265955[10] = 0; + out_2407824440308265955[11] = 0; + out_2407824440308265955[12] = 0; + out_2407824440308265955[13] = 0; + out_2407824440308265955[14] = 1; + out_2407824440308265955[15] = 0; + out_2407824440308265955[16] = 0; + out_2407824440308265955[17] = 0; } -void h_30(double *state, double *unused, double *out_3741631544889310676) { - out_3741631544889310676[0] = state[4]; +void h_30(double *state, double *unused, double *out_7458661948015643157) { + out_7458661948015643157[0] = state[4]; } -void H_30(double *state, double *unused, double *out_9071030814708967724) { - out_9071030814708967724[0] = 0; - out_9071030814708967724[1] = 0; - out_9071030814708967724[2] = 0; - out_9071030814708967724[3] = 0; - out_9071030814708967724[4] = 1; - out_9071030814708967724[5] = 0; - out_9071030814708967724[6] = 0; - out_9071030814708967724[7] = 0; - out_9071030814708967724[8] = 0; +void H_30(double *state, double *unused, double *out_2283158117204482238) { + out_2283158117204482238[0] = 0; + out_2283158117204482238[1] = 0; + out_2283158117204482238[2] = 0; + out_2283158117204482238[3] = 0; + out_2283158117204482238[4] = 1; + out_2283158117204482238[5] = 0; + out_2283158117204482238[6] = 0; + out_2283158117204482238[7] = 0; + out_2283158117204482238[8] = 0; } -void h_26(double *state, double *unused, double *out_7702349825027175161) { - out_7702349825027175161[0] = state[7]; +void h_26(double *state, double *unused, double *out_5109796638135193371) { + out_5109796638135193371[0] = state[7]; } -void H_26(double *state, double *unused, double *out_3115876981619279041) { - out_3115876981619279041[0] = 0; - out_3115876981619279041[1] = 0; - out_3115876981619279041[2] = 0; - out_3115876981619279041[3] = 0; - out_3115876981619279041[4] = 0; - out_3115876981619279041[5] = 0; - out_3115876981619279041[6] = 0; - out_3115876981619279041[7] = 1; - out_3115876981619279041[8] = 0; +void H_26(double *state, double *unused, double *out_3069351128458034212) { + out_3069351128458034212[0] = 0; + out_3069351128458034212[1] = 0; + out_3069351128458034212[2] = 0; + out_3069351128458034212[3] = 0; + out_3069351128458034212[4] = 0; + out_3069351128458034212[5] = 0; + out_3069351128458034212[6] = 0; + out_3069351128458034212[7] = 1; + out_3069351128458034212[8] = 0; } -void h_27(double *state, double *unused, double *out_2155476886073861158) { - out_2155476886073861158[0] = state[3]; +void h_27(double *state, double *unused, double *out_2732182742639303401) { + out_2732182742639303401[0] = state[3]; } -void H_27(double *state, double *unused, double *out_7200949947200158981) { - out_7200949947200158981[0] = 0; - out_7200949947200158981[1] = 0; - out_7200949947200158981[2] = 0; - out_7200949947200158981[3] = 1; - out_7200949947200158981[4] = 0; - out_7200949947200158981[5] = 0; - out_7200949947200158981[6] = 0; - out_7200949947200158981[7] = 0; - out_7200949947200158981[8] = 0; +void H_27(double *state, double *unused, double *out_108394805404057327) { + out_108394805404057327[0] = 0; + out_108394805404057327[1] = 0; + out_108394805404057327[2] = 0; + out_108394805404057327[3] = 1; + out_108394805404057327[4] = 0; + out_108394805404057327[5] = 0; + out_108394805404057327[6] = 0; + out_108394805404057327[7] = 0; + out_108394805404057327[8] = 0; } -void h_29(double *state, double *unused, double *out_8743304812550156321) { - out_8743304812550156321[0] = state[1]; +void h_29(double *state, double *unused, double *out_1129500733789198251) { + out_1129500733789198251[0] = state[1]; } -void H_29(double *state, double *unused, double *out_8560799470394575540) { - out_8560799470394575540[0] = 0; - out_8560799470394575540[1] = 1; - out_8560799470394575540[2] = 0; - out_8560799470394575540[3] = 0; - out_8560799470394575540[4] = 0; - out_8560799470394575540[5] = 0; - out_8560799470394575540[6] = 0; - out_8560799470394575540[7] = 0; - out_8560799470394575540[8] = 0; +void H_29(double *state, double *unused, double *out_2793389461518874422) { + out_2793389461518874422[0] = 0; + out_2793389461518874422[1] = 1; + out_2793389461518874422[2] = 0; + out_2793389461518874422[3] = 0; + out_2793389461518874422[4] = 0; + out_2793389461518874422[5] = 0; + out_2793389461518874422[6] = 0; + out_2793389461518874422[7] = 0; + out_2793389461518874422[8] = 0; } -void h_28(double *state, double *unused, double *out_1211967033172198723) { - out_1211967033172198723[0] = state[0]; +void h_28(double *state, double *unused, double *out_4422630577374984361) { + out_4422630577374984361[0] = state[0]; } -void H_28(double *state, double *unused, double *out_4803545586245445502) { - out_4803545586245445502[0] = 1; - out_4803545586245445502[1] = 0; - out_4803545586245445502[2] = 0; - out_4803545586245445502[3] = 0; - out_4803545586245445502[4] = 0; - out_4803545586245445502[5] = 0; - out_4803545586245445502[6] = 0; - out_4803545586245445502[7] = 0; - out_4803545586245445502[8] = 0; +void H_28(double *state, double *unused, double *out_2289009555550656152) { + out_2289009555550656152[0] = 1; + out_2289009555550656152[1] = 0; + out_2289009555550656152[2] = 0; + out_2289009555550656152[3] = 0; + out_2289009555550656152[4] = 0; + out_2289009555550656152[5] = 0; + out_2289009555550656152[6] = 0; + out_2289009555550656152[7] = 0; + out_2289009555550656152[8] = 0; } -void h_31(double *state, double *unused, double *out_6483645909841006837) { - out_6483645909841006837[0] = state[8]; +void h_31(double *state, double *unused, double *out_7765162073053917609) { + out_7765162073053917609[0] = state[8]; } -void H_31(double *state, double *unused, double *out_2489668879385927565) { - out_2489668879385927565[0] = 0; - out_2489668879385927565[1] = 0; - out_2489668879385927565[2] = 0; - out_2489668879385927565[3] = 0; - out_2489668879385927565[4] = 0; - out_2489668879385927565[5] = 0; - out_2489668879385927565[6] = 0; - out_2489668879385927565[7] = 0; - out_2489668879385927565[8] = 1; +void H_31(double *state, double *unused, double *out_2443143026224682736) { + out_2443143026224682736[0] = 0; + out_2443143026224682736[1] = 0; + out_2443143026224682736[2] = 0; + out_2443143026224682736[3] = 0; + out_2443143026224682736[4] = 0; + out_2443143026224682736[5] = 0; + out_2443143026224682736[6] = 0; + out_2443143026224682736[7] = 0; + out_2443143026224682736[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_4271848112759039807) { - err_fun(nom_x, delta_x, out_4271848112759039807); +void car_err_fun(double *nom_x, double *delta_x, double *out_3281150270443704084) { + err_fun(nom_x, delta_x, out_3281150270443704084); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_2031572598342150124) { - inv_err_fun(nom_x, true_x, out_2031572598342150124); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3048073845424101937) { + inv_err_fun(nom_x, true_x, out_3048073845424101937); } -void car_H_mod_fun(double *state, double *out_177684180657030354) { - H_mod_fun(state, out_177684180657030354); +void car_H_mod_fun(double *state, double *out_5861733525303608227) { + H_mod_fun(state, out_5861733525303608227); } -void car_f_fun(double *state, double dt, double *out_6521764996120993087) { - f_fun(state, dt, out_6521764996120993087); +void car_f_fun(double *state, double dt, double *out_6520674902136446798) { + f_fun(state, dt, out_6520674902136446798); } -void car_F_fun(double *state, double dt, double *out_4624398414228863854) { - F_fun(state, dt, out_4624398414228863854); +void car_F_fun(double *state, double dt, double *out_2036702211381872187) { + F_fun(state, dt, out_2036702211381872187); } -void car_h_25(double *state, double *unused, double *out_5192262937518193843) { - h_25(state, unused, out_5192262937518193843); +void car_h_25(double *state, double *unused, double *out_7733856010300149046) { + h_25(state, unused, out_7733856010300149046); } -void car_H_25(double *state, double *unused, double *out_6857380300493335265) { - H_25(state, unused, out_6857380300493335265); +void car_H_25(double *state, double *unused, double *out_6810854447332090436) { + H_25(state, unused, out_6810854447332090436); } -void car_h_24(double *state, double *unused, double *out_6066922532331985124) { - h_24(state, unused, out_6066922532331985124); +void car_h_24(double *state, double *unused, double *out_8048847696560640687) { + h_24(state, unused, out_8048847696560640687); } -void car_H_24(double *state, double *unused, double *out_4684730701487835699) { - H_24(state, unused, out_4684730701487835699); +void car_H_24(double *state, double *unused, double *out_2407824440308265955) { + H_24(state, unused, out_2407824440308265955); } -void car_h_30(double *state, double *unused, double *out_3741631544889310676) { - h_30(state, unused, out_3741631544889310676); +void car_h_30(double *state, double *unused, double *out_7458661948015643157) { + h_30(state, unused, out_7458661948015643157); } -void car_H_30(double *state, double *unused, double *out_9071030814708967724) { - H_30(state, unused, out_9071030814708967724); +void car_H_30(double *state, double *unused, double *out_2283158117204482238) { + H_30(state, unused, out_2283158117204482238); } -void car_h_26(double *state, double *unused, double *out_7702349825027175161) { - h_26(state, unused, out_7702349825027175161); +void car_h_26(double *state, double *unused, double *out_5109796638135193371) { + h_26(state, unused, out_5109796638135193371); } -void car_H_26(double *state, double *unused, double *out_3115876981619279041) { - H_26(state, unused, out_3115876981619279041); +void car_H_26(double *state, double *unused, double *out_3069351128458034212) { + H_26(state, unused, out_3069351128458034212); } -void car_h_27(double *state, double *unused, double *out_2155476886073861158) { - h_27(state, unused, out_2155476886073861158); +void car_h_27(double *state, double *unused, double *out_2732182742639303401) { + h_27(state, unused, out_2732182742639303401); } -void car_H_27(double *state, double *unused, double *out_7200949947200158981) { - H_27(state, unused, out_7200949947200158981); +void car_H_27(double *state, double *unused, double *out_108394805404057327) { + H_27(state, unused, out_108394805404057327); } -void car_h_29(double *state, double *unused, double *out_8743304812550156321) { - h_29(state, unused, out_8743304812550156321); +void car_h_29(double *state, double *unused, double *out_1129500733789198251) { + h_29(state, unused, out_1129500733789198251); } -void car_H_29(double *state, double *unused, double *out_8560799470394575540) { - H_29(state, unused, out_8560799470394575540); +void car_H_29(double *state, double *unused, double *out_2793389461518874422) { + H_29(state, unused, out_2793389461518874422); } -void car_h_28(double *state, double *unused, double *out_1211967033172198723) { - h_28(state, unused, out_1211967033172198723); +void car_h_28(double *state, double *unused, double *out_4422630577374984361) { + h_28(state, unused, out_4422630577374984361); } -void car_H_28(double *state, double *unused, double *out_4803545586245445502) { - H_28(state, unused, out_4803545586245445502); +void car_H_28(double *state, double *unused, double *out_2289009555550656152) { + H_28(state, unused, out_2289009555550656152); } -void car_h_31(double *state, double *unused, double *out_6483645909841006837) { - h_31(state, unused, out_6483645909841006837); +void car_h_31(double *state, double *unused, double *out_7765162073053917609) { + h_31(state, unused, out_7765162073053917609); } -void car_H_31(double *state, double *unused, double *out_2489668879385927565) { - H_31(state, unused, out_2489668879385927565); +void car_H_31(double *state, double *unused, double *out_2443143026224682736) { + H_31(state, unused, out_2443143026224682736); } 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 9aa00f291..455985ab6 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_4271848112759039807); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_2031572598342150124); -void car_H_mod_fun(double *state, double *out_177684180657030354); -void car_f_fun(double *state, double dt, double *out_6521764996120993087); -void car_F_fun(double *state, double dt, double *out_4624398414228863854); -void car_h_25(double *state, double *unused, double *out_5192262937518193843); -void car_H_25(double *state, double *unused, double *out_6857380300493335265); -void car_h_24(double *state, double *unused, double *out_6066922532331985124); -void car_H_24(double *state, double *unused, double *out_4684730701487835699); -void car_h_30(double *state, double *unused, double *out_3741631544889310676); -void car_H_30(double *state, double *unused, double *out_9071030814708967724); -void car_h_26(double *state, double *unused, double *out_7702349825027175161); -void car_H_26(double *state, double *unused, double *out_3115876981619279041); -void car_h_27(double *state, double *unused, double *out_2155476886073861158); -void car_H_27(double *state, double *unused, double *out_7200949947200158981); -void car_h_29(double *state, double *unused, double *out_8743304812550156321); -void car_H_29(double *state, double *unused, double *out_8560799470394575540); -void car_h_28(double *state, double *unused, double *out_1211967033172198723); -void car_H_28(double *state, double *unused, double *out_4803545586245445502); -void car_h_31(double *state, double *unused, double *out_6483645909841006837); -void car_H_31(double *state, double *unused, double *out_2489668879385927565); +void car_err_fun(double *nom_x, double *delta_x, double *out_3281150270443704084); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3048073845424101937); +void car_H_mod_fun(double *state, double *out_5861733525303608227); +void car_f_fun(double *state, double dt, double *out_6520674902136446798); +void car_F_fun(double *state, double dt, double *out_2036702211381872187); +void car_h_25(double *state, double *unused, double *out_7733856010300149046); +void car_H_25(double *state, double *unused, double *out_6810854447332090436); +void car_h_24(double *state, double *unused, double *out_8048847696560640687); +void car_H_24(double *state, double *unused, double *out_2407824440308265955); +void car_h_30(double *state, double *unused, double *out_7458661948015643157); +void car_H_30(double *state, double *unused, double *out_2283158117204482238); +void car_h_26(double *state, double *unused, double *out_5109796638135193371); +void car_H_26(double *state, double *unused, double *out_3069351128458034212); +void car_h_27(double *state, double *unused, double *out_2732182742639303401); +void car_H_27(double *state, double *unused, double *out_108394805404057327); +void car_h_29(double *state, double *unused, double *out_1129500733789198251); +void car_H_29(double *state, double *unused, double *out_2793389461518874422); +void car_h_28(double *state, double *unused, double *out_4422630577374984361); +void car_H_28(double *state, double *unused, double *out_2289009555550656152); +void car_h_31(double *state, double *unused, double *out_7765162073053917609); +void car_H_31(double *state, double *unused, double *out_2443143026224682736); 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 49a2289d2..e1ede770a 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_6891956137394554809) { - out_6891956137394554809[0] = delta_x[0] + nom_x[0]; - out_6891956137394554809[1] = delta_x[1] + nom_x[1]; - out_6891956137394554809[2] = delta_x[2] + nom_x[2]; - out_6891956137394554809[3] = delta_x[3] + nom_x[3]; - out_6891956137394554809[4] = delta_x[4] + nom_x[4]; - out_6891956137394554809[5] = delta_x[5] + nom_x[5]; - out_6891956137394554809[6] = delta_x[6] + nom_x[6]; - out_6891956137394554809[7] = delta_x[7] + nom_x[7]; - out_6891956137394554809[8] = delta_x[8] + nom_x[8]; - out_6891956137394554809[9] = delta_x[9] + nom_x[9]; - out_6891956137394554809[10] = delta_x[10] + nom_x[10]; - out_6891956137394554809[11] = delta_x[11] + nom_x[11]; - out_6891956137394554809[12] = delta_x[12] + nom_x[12]; - out_6891956137394554809[13] = delta_x[13] + nom_x[13]; - out_6891956137394554809[14] = delta_x[14] + nom_x[14]; - out_6891956137394554809[15] = delta_x[15] + nom_x[15]; - out_6891956137394554809[16] = delta_x[16] + nom_x[16]; - out_6891956137394554809[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_6531280423659383110) { + out_6531280423659383110[0] = delta_x[0] + nom_x[0]; + out_6531280423659383110[1] = delta_x[1] + nom_x[1]; + out_6531280423659383110[2] = delta_x[2] + nom_x[2]; + out_6531280423659383110[3] = delta_x[3] + nom_x[3]; + out_6531280423659383110[4] = delta_x[4] + nom_x[4]; + out_6531280423659383110[5] = delta_x[5] + nom_x[5]; + out_6531280423659383110[6] = delta_x[6] + nom_x[6]; + out_6531280423659383110[7] = delta_x[7] + nom_x[7]; + out_6531280423659383110[8] = delta_x[8] + nom_x[8]; + out_6531280423659383110[9] = delta_x[9] + nom_x[9]; + out_6531280423659383110[10] = delta_x[10] + nom_x[10]; + out_6531280423659383110[11] = delta_x[11] + nom_x[11]; + out_6531280423659383110[12] = delta_x[12] + nom_x[12]; + out_6531280423659383110[13] = delta_x[13] + nom_x[13]; + out_6531280423659383110[14] = delta_x[14] + nom_x[14]; + out_6531280423659383110[15] = delta_x[15] + nom_x[15]; + out_6531280423659383110[16] = delta_x[16] + nom_x[16]; + out_6531280423659383110[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_352074767866048570) { - out_352074767866048570[0] = -nom_x[0] + true_x[0]; - out_352074767866048570[1] = -nom_x[1] + true_x[1]; - out_352074767866048570[2] = -nom_x[2] + true_x[2]; - out_352074767866048570[3] = -nom_x[3] + true_x[3]; - out_352074767866048570[4] = -nom_x[4] + true_x[4]; - out_352074767866048570[5] = -nom_x[5] + true_x[5]; - out_352074767866048570[6] = -nom_x[6] + true_x[6]; - out_352074767866048570[7] = -nom_x[7] + true_x[7]; - out_352074767866048570[8] = -nom_x[8] + true_x[8]; - out_352074767866048570[9] = -nom_x[9] + true_x[9]; - out_352074767866048570[10] = -nom_x[10] + true_x[10]; - out_352074767866048570[11] = -nom_x[11] + true_x[11]; - out_352074767866048570[12] = -nom_x[12] + true_x[12]; - out_352074767866048570[13] = -nom_x[13] + true_x[13]; - out_352074767866048570[14] = -nom_x[14] + true_x[14]; - out_352074767866048570[15] = -nom_x[15] + true_x[15]; - out_352074767866048570[16] = -nom_x[16] + true_x[16]; - out_352074767866048570[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2847042605170284069) { + out_2847042605170284069[0] = -nom_x[0] + true_x[0]; + out_2847042605170284069[1] = -nom_x[1] + true_x[1]; + out_2847042605170284069[2] = -nom_x[2] + true_x[2]; + out_2847042605170284069[3] = -nom_x[3] + true_x[3]; + out_2847042605170284069[4] = -nom_x[4] + true_x[4]; + out_2847042605170284069[5] = -nom_x[5] + true_x[5]; + out_2847042605170284069[6] = -nom_x[6] + true_x[6]; + out_2847042605170284069[7] = -nom_x[7] + true_x[7]; + out_2847042605170284069[8] = -nom_x[8] + true_x[8]; + out_2847042605170284069[9] = -nom_x[9] + true_x[9]; + out_2847042605170284069[10] = -nom_x[10] + true_x[10]; + out_2847042605170284069[11] = -nom_x[11] + true_x[11]; + out_2847042605170284069[12] = -nom_x[12] + true_x[12]; + out_2847042605170284069[13] = -nom_x[13] + true_x[13]; + out_2847042605170284069[14] = -nom_x[14] + true_x[14]; + out_2847042605170284069[15] = -nom_x[15] + true_x[15]; + out_2847042605170284069[16] = -nom_x[16] + true_x[16]; + out_2847042605170284069[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_57344037476075931) { - out_57344037476075931[0] = 1.0; - out_57344037476075931[1] = 0.0; - out_57344037476075931[2] = 0.0; - out_57344037476075931[3] = 0.0; - out_57344037476075931[4] = 0.0; - out_57344037476075931[5] = 0.0; - out_57344037476075931[6] = 0.0; - out_57344037476075931[7] = 0.0; - out_57344037476075931[8] = 0.0; - out_57344037476075931[9] = 0.0; - out_57344037476075931[10] = 0.0; - out_57344037476075931[11] = 0.0; - out_57344037476075931[12] = 0.0; - out_57344037476075931[13] = 0.0; - out_57344037476075931[14] = 0.0; - out_57344037476075931[15] = 0.0; - out_57344037476075931[16] = 0.0; - out_57344037476075931[17] = 0.0; - out_57344037476075931[18] = 0.0; - out_57344037476075931[19] = 1.0; - out_57344037476075931[20] = 0.0; - out_57344037476075931[21] = 0.0; - out_57344037476075931[22] = 0.0; - out_57344037476075931[23] = 0.0; - out_57344037476075931[24] = 0.0; - out_57344037476075931[25] = 0.0; - out_57344037476075931[26] = 0.0; - out_57344037476075931[27] = 0.0; - out_57344037476075931[28] = 0.0; - out_57344037476075931[29] = 0.0; - out_57344037476075931[30] = 0.0; - out_57344037476075931[31] = 0.0; - out_57344037476075931[32] = 0.0; - out_57344037476075931[33] = 0.0; - out_57344037476075931[34] = 0.0; - out_57344037476075931[35] = 0.0; - out_57344037476075931[36] = 0.0; - out_57344037476075931[37] = 0.0; - out_57344037476075931[38] = 1.0; - out_57344037476075931[39] = 0.0; - out_57344037476075931[40] = 0.0; - out_57344037476075931[41] = 0.0; - out_57344037476075931[42] = 0.0; - out_57344037476075931[43] = 0.0; - out_57344037476075931[44] = 0.0; - out_57344037476075931[45] = 0.0; - out_57344037476075931[46] = 0.0; - out_57344037476075931[47] = 0.0; - out_57344037476075931[48] = 0.0; - out_57344037476075931[49] = 0.0; - out_57344037476075931[50] = 0.0; - out_57344037476075931[51] = 0.0; - out_57344037476075931[52] = 0.0; - out_57344037476075931[53] = 0.0; - out_57344037476075931[54] = 0.0; - out_57344037476075931[55] = 0.0; - out_57344037476075931[56] = 0.0; - out_57344037476075931[57] = 1.0; - out_57344037476075931[58] = 0.0; - out_57344037476075931[59] = 0.0; - out_57344037476075931[60] = 0.0; - out_57344037476075931[61] = 0.0; - out_57344037476075931[62] = 0.0; - out_57344037476075931[63] = 0.0; - out_57344037476075931[64] = 0.0; - out_57344037476075931[65] = 0.0; - out_57344037476075931[66] = 0.0; - out_57344037476075931[67] = 0.0; - out_57344037476075931[68] = 0.0; - out_57344037476075931[69] = 0.0; - out_57344037476075931[70] = 0.0; - out_57344037476075931[71] = 0.0; - out_57344037476075931[72] = 0.0; - out_57344037476075931[73] = 0.0; - out_57344037476075931[74] = 0.0; - out_57344037476075931[75] = 0.0; - out_57344037476075931[76] = 1.0; - out_57344037476075931[77] = 0.0; - out_57344037476075931[78] = 0.0; - out_57344037476075931[79] = 0.0; - out_57344037476075931[80] = 0.0; - out_57344037476075931[81] = 0.0; - out_57344037476075931[82] = 0.0; - out_57344037476075931[83] = 0.0; - out_57344037476075931[84] = 0.0; - out_57344037476075931[85] = 0.0; - out_57344037476075931[86] = 0.0; - out_57344037476075931[87] = 0.0; - out_57344037476075931[88] = 0.0; - out_57344037476075931[89] = 0.0; - out_57344037476075931[90] = 0.0; - out_57344037476075931[91] = 0.0; - out_57344037476075931[92] = 0.0; - out_57344037476075931[93] = 0.0; - out_57344037476075931[94] = 0.0; - out_57344037476075931[95] = 1.0; - out_57344037476075931[96] = 0.0; - out_57344037476075931[97] = 0.0; - out_57344037476075931[98] = 0.0; - out_57344037476075931[99] = 0.0; - out_57344037476075931[100] = 0.0; - out_57344037476075931[101] = 0.0; - out_57344037476075931[102] = 0.0; - out_57344037476075931[103] = 0.0; - out_57344037476075931[104] = 0.0; - out_57344037476075931[105] = 0.0; - out_57344037476075931[106] = 0.0; - out_57344037476075931[107] = 0.0; - out_57344037476075931[108] = 0.0; - out_57344037476075931[109] = 0.0; - out_57344037476075931[110] = 0.0; - out_57344037476075931[111] = 0.0; - out_57344037476075931[112] = 0.0; - out_57344037476075931[113] = 0.0; - out_57344037476075931[114] = 1.0; - out_57344037476075931[115] = 0.0; - out_57344037476075931[116] = 0.0; - out_57344037476075931[117] = 0.0; - out_57344037476075931[118] = 0.0; - out_57344037476075931[119] = 0.0; - out_57344037476075931[120] = 0.0; - out_57344037476075931[121] = 0.0; - out_57344037476075931[122] = 0.0; - out_57344037476075931[123] = 0.0; - out_57344037476075931[124] = 0.0; - out_57344037476075931[125] = 0.0; - out_57344037476075931[126] = 0.0; - out_57344037476075931[127] = 0.0; - out_57344037476075931[128] = 0.0; - out_57344037476075931[129] = 0.0; - out_57344037476075931[130] = 0.0; - out_57344037476075931[131] = 0.0; - out_57344037476075931[132] = 0.0; - out_57344037476075931[133] = 1.0; - out_57344037476075931[134] = 0.0; - out_57344037476075931[135] = 0.0; - out_57344037476075931[136] = 0.0; - out_57344037476075931[137] = 0.0; - out_57344037476075931[138] = 0.0; - out_57344037476075931[139] = 0.0; - out_57344037476075931[140] = 0.0; - out_57344037476075931[141] = 0.0; - out_57344037476075931[142] = 0.0; - out_57344037476075931[143] = 0.0; - out_57344037476075931[144] = 0.0; - out_57344037476075931[145] = 0.0; - out_57344037476075931[146] = 0.0; - out_57344037476075931[147] = 0.0; - out_57344037476075931[148] = 0.0; - out_57344037476075931[149] = 0.0; - out_57344037476075931[150] = 0.0; - out_57344037476075931[151] = 0.0; - out_57344037476075931[152] = 1.0; - out_57344037476075931[153] = 0.0; - out_57344037476075931[154] = 0.0; - out_57344037476075931[155] = 0.0; - out_57344037476075931[156] = 0.0; - out_57344037476075931[157] = 0.0; - out_57344037476075931[158] = 0.0; - out_57344037476075931[159] = 0.0; - out_57344037476075931[160] = 0.0; - out_57344037476075931[161] = 0.0; - out_57344037476075931[162] = 0.0; - out_57344037476075931[163] = 0.0; - out_57344037476075931[164] = 0.0; - out_57344037476075931[165] = 0.0; - out_57344037476075931[166] = 0.0; - out_57344037476075931[167] = 0.0; - out_57344037476075931[168] = 0.0; - out_57344037476075931[169] = 0.0; - out_57344037476075931[170] = 0.0; - out_57344037476075931[171] = 1.0; - out_57344037476075931[172] = 0.0; - out_57344037476075931[173] = 0.0; - out_57344037476075931[174] = 0.0; - out_57344037476075931[175] = 0.0; - out_57344037476075931[176] = 0.0; - out_57344037476075931[177] = 0.0; - out_57344037476075931[178] = 0.0; - out_57344037476075931[179] = 0.0; - out_57344037476075931[180] = 0.0; - out_57344037476075931[181] = 0.0; - out_57344037476075931[182] = 0.0; - out_57344037476075931[183] = 0.0; - out_57344037476075931[184] = 0.0; - out_57344037476075931[185] = 0.0; - out_57344037476075931[186] = 0.0; - out_57344037476075931[187] = 0.0; - out_57344037476075931[188] = 0.0; - out_57344037476075931[189] = 0.0; - out_57344037476075931[190] = 1.0; - out_57344037476075931[191] = 0.0; - out_57344037476075931[192] = 0.0; - out_57344037476075931[193] = 0.0; - out_57344037476075931[194] = 0.0; - out_57344037476075931[195] = 0.0; - out_57344037476075931[196] = 0.0; - out_57344037476075931[197] = 0.0; - out_57344037476075931[198] = 0.0; - out_57344037476075931[199] = 0.0; - out_57344037476075931[200] = 0.0; - out_57344037476075931[201] = 0.0; - out_57344037476075931[202] = 0.0; - out_57344037476075931[203] = 0.0; - out_57344037476075931[204] = 0.0; - out_57344037476075931[205] = 0.0; - out_57344037476075931[206] = 0.0; - out_57344037476075931[207] = 0.0; - out_57344037476075931[208] = 0.0; - out_57344037476075931[209] = 1.0; - out_57344037476075931[210] = 0.0; - out_57344037476075931[211] = 0.0; - out_57344037476075931[212] = 0.0; - out_57344037476075931[213] = 0.0; - out_57344037476075931[214] = 0.0; - out_57344037476075931[215] = 0.0; - out_57344037476075931[216] = 0.0; - out_57344037476075931[217] = 0.0; - out_57344037476075931[218] = 0.0; - out_57344037476075931[219] = 0.0; - out_57344037476075931[220] = 0.0; - out_57344037476075931[221] = 0.0; - out_57344037476075931[222] = 0.0; - out_57344037476075931[223] = 0.0; - out_57344037476075931[224] = 0.0; - out_57344037476075931[225] = 0.0; - out_57344037476075931[226] = 0.0; - out_57344037476075931[227] = 0.0; - out_57344037476075931[228] = 1.0; - out_57344037476075931[229] = 0.0; - out_57344037476075931[230] = 0.0; - out_57344037476075931[231] = 0.0; - out_57344037476075931[232] = 0.0; - out_57344037476075931[233] = 0.0; - out_57344037476075931[234] = 0.0; - out_57344037476075931[235] = 0.0; - out_57344037476075931[236] = 0.0; - out_57344037476075931[237] = 0.0; - out_57344037476075931[238] = 0.0; - out_57344037476075931[239] = 0.0; - out_57344037476075931[240] = 0.0; - out_57344037476075931[241] = 0.0; - out_57344037476075931[242] = 0.0; - out_57344037476075931[243] = 0.0; - out_57344037476075931[244] = 0.0; - out_57344037476075931[245] = 0.0; - out_57344037476075931[246] = 0.0; - out_57344037476075931[247] = 1.0; - out_57344037476075931[248] = 0.0; - out_57344037476075931[249] = 0.0; - out_57344037476075931[250] = 0.0; - out_57344037476075931[251] = 0.0; - out_57344037476075931[252] = 0.0; - out_57344037476075931[253] = 0.0; - out_57344037476075931[254] = 0.0; - out_57344037476075931[255] = 0.0; - out_57344037476075931[256] = 0.0; - out_57344037476075931[257] = 0.0; - out_57344037476075931[258] = 0.0; - out_57344037476075931[259] = 0.0; - out_57344037476075931[260] = 0.0; - out_57344037476075931[261] = 0.0; - out_57344037476075931[262] = 0.0; - out_57344037476075931[263] = 0.0; - out_57344037476075931[264] = 0.0; - out_57344037476075931[265] = 0.0; - out_57344037476075931[266] = 1.0; - out_57344037476075931[267] = 0.0; - out_57344037476075931[268] = 0.0; - out_57344037476075931[269] = 0.0; - out_57344037476075931[270] = 0.0; - out_57344037476075931[271] = 0.0; - out_57344037476075931[272] = 0.0; - out_57344037476075931[273] = 0.0; - out_57344037476075931[274] = 0.0; - out_57344037476075931[275] = 0.0; - out_57344037476075931[276] = 0.0; - out_57344037476075931[277] = 0.0; - out_57344037476075931[278] = 0.0; - out_57344037476075931[279] = 0.0; - out_57344037476075931[280] = 0.0; - out_57344037476075931[281] = 0.0; - out_57344037476075931[282] = 0.0; - out_57344037476075931[283] = 0.0; - out_57344037476075931[284] = 0.0; - out_57344037476075931[285] = 1.0; - out_57344037476075931[286] = 0.0; - out_57344037476075931[287] = 0.0; - out_57344037476075931[288] = 0.0; - out_57344037476075931[289] = 0.0; - out_57344037476075931[290] = 0.0; - out_57344037476075931[291] = 0.0; - out_57344037476075931[292] = 0.0; - out_57344037476075931[293] = 0.0; - out_57344037476075931[294] = 0.0; - out_57344037476075931[295] = 0.0; - out_57344037476075931[296] = 0.0; - out_57344037476075931[297] = 0.0; - out_57344037476075931[298] = 0.0; - out_57344037476075931[299] = 0.0; - out_57344037476075931[300] = 0.0; - out_57344037476075931[301] = 0.0; - out_57344037476075931[302] = 0.0; - out_57344037476075931[303] = 0.0; - out_57344037476075931[304] = 1.0; - out_57344037476075931[305] = 0.0; - out_57344037476075931[306] = 0.0; - out_57344037476075931[307] = 0.0; - out_57344037476075931[308] = 0.0; - out_57344037476075931[309] = 0.0; - out_57344037476075931[310] = 0.0; - out_57344037476075931[311] = 0.0; - out_57344037476075931[312] = 0.0; - out_57344037476075931[313] = 0.0; - out_57344037476075931[314] = 0.0; - out_57344037476075931[315] = 0.0; - out_57344037476075931[316] = 0.0; - out_57344037476075931[317] = 0.0; - out_57344037476075931[318] = 0.0; - out_57344037476075931[319] = 0.0; - out_57344037476075931[320] = 0.0; - out_57344037476075931[321] = 0.0; - out_57344037476075931[322] = 0.0; - out_57344037476075931[323] = 1.0; +void H_mod_fun(double *state, double *out_1201664963462517165) { + out_1201664963462517165[0] = 1.0; + out_1201664963462517165[1] = 0.0; + out_1201664963462517165[2] = 0.0; + out_1201664963462517165[3] = 0.0; + out_1201664963462517165[4] = 0.0; + out_1201664963462517165[5] = 0.0; + out_1201664963462517165[6] = 0.0; + out_1201664963462517165[7] = 0.0; + out_1201664963462517165[8] = 0.0; + out_1201664963462517165[9] = 0.0; + out_1201664963462517165[10] = 0.0; + out_1201664963462517165[11] = 0.0; + out_1201664963462517165[12] = 0.0; + out_1201664963462517165[13] = 0.0; + out_1201664963462517165[14] = 0.0; + out_1201664963462517165[15] = 0.0; + out_1201664963462517165[16] = 0.0; + out_1201664963462517165[17] = 0.0; + out_1201664963462517165[18] = 0.0; + out_1201664963462517165[19] = 1.0; + out_1201664963462517165[20] = 0.0; + out_1201664963462517165[21] = 0.0; + out_1201664963462517165[22] = 0.0; + out_1201664963462517165[23] = 0.0; + out_1201664963462517165[24] = 0.0; + out_1201664963462517165[25] = 0.0; + out_1201664963462517165[26] = 0.0; + out_1201664963462517165[27] = 0.0; + out_1201664963462517165[28] = 0.0; + out_1201664963462517165[29] = 0.0; + out_1201664963462517165[30] = 0.0; + out_1201664963462517165[31] = 0.0; + out_1201664963462517165[32] = 0.0; + out_1201664963462517165[33] = 0.0; + out_1201664963462517165[34] = 0.0; + out_1201664963462517165[35] = 0.0; + out_1201664963462517165[36] = 0.0; + out_1201664963462517165[37] = 0.0; + out_1201664963462517165[38] = 1.0; + out_1201664963462517165[39] = 0.0; + out_1201664963462517165[40] = 0.0; + out_1201664963462517165[41] = 0.0; + out_1201664963462517165[42] = 0.0; + out_1201664963462517165[43] = 0.0; + out_1201664963462517165[44] = 0.0; + out_1201664963462517165[45] = 0.0; + out_1201664963462517165[46] = 0.0; + out_1201664963462517165[47] = 0.0; + out_1201664963462517165[48] = 0.0; + out_1201664963462517165[49] = 0.0; + out_1201664963462517165[50] = 0.0; + out_1201664963462517165[51] = 0.0; + out_1201664963462517165[52] = 0.0; + out_1201664963462517165[53] = 0.0; + out_1201664963462517165[54] = 0.0; + out_1201664963462517165[55] = 0.0; + out_1201664963462517165[56] = 0.0; + out_1201664963462517165[57] = 1.0; + out_1201664963462517165[58] = 0.0; + out_1201664963462517165[59] = 0.0; + out_1201664963462517165[60] = 0.0; + out_1201664963462517165[61] = 0.0; + out_1201664963462517165[62] = 0.0; + out_1201664963462517165[63] = 0.0; + out_1201664963462517165[64] = 0.0; + out_1201664963462517165[65] = 0.0; + out_1201664963462517165[66] = 0.0; + out_1201664963462517165[67] = 0.0; + out_1201664963462517165[68] = 0.0; + out_1201664963462517165[69] = 0.0; + out_1201664963462517165[70] = 0.0; + out_1201664963462517165[71] = 0.0; + out_1201664963462517165[72] = 0.0; + out_1201664963462517165[73] = 0.0; + out_1201664963462517165[74] = 0.0; + out_1201664963462517165[75] = 0.0; + out_1201664963462517165[76] = 1.0; + out_1201664963462517165[77] = 0.0; + out_1201664963462517165[78] = 0.0; + out_1201664963462517165[79] = 0.0; + out_1201664963462517165[80] = 0.0; + out_1201664963462517165[81] = 0.0; + out_1201664963462517165[82] = 0.0; + out_1201664963462517165[83] = 0.0; + out_1201664963462517165[84] = 0.0; + out_1201664963462517165[85] = 0.0; + out_1201664963462517165[86] = 0.0; + out_1201664963462517165[87] = 0.0; + out_1201664963462517165[88] = 0.0; + out_1201664963462517165[89] = 0.0; + out_1201664963462517165[90] = 0.0; + out_1201664963462517165[91] = 0.0; + out_1201664963462517165[92] = 0.0; + out_1201664963462517165[93] = 0.0; + out_1201664963462517165[94] = 0.0; + out_1201664963462517165[95] = 1.0; + out_1201664963462517165[96] = 0.0; + out_1201664963462517165[97] = 0.0; + out_1201664963462517165[98] = 0.0; + out_1201664963462517165[99] = 0.0; + out_1201664963462517165[100] = 0.0; + out_1201664963462517165[101] = 0.0; + out_1201664963462517165[102] = 0.0; + out_1201664963462517165[103] = 0.0; + out_1201664963462517165[104] = 0.0; + out_1201664963462517165[105] = 0.0; + out_1201664963462517165[106] = 0.0; + out_1201664963462517165[107] = 0.0; + out_1201664963462517165[108] = 0.0; + out_1201664963462517165[109] = 0.0; + out_1201664963462517165[110] = 0.0; + out_1201664963462517165[111] = 0.0; + out_1201664963462517165[112] = 0.0; + out_1201664963462517165[113] = 0.0; + out_1201664963462517165[114] = 1.0; + out_1201664963462517165[115] = 0.0; + out_1201664963462517165[116] = 0.0; + out_1201664963462517165[117] = 0.0; + out_1201664963462517165[118] = 0.0; + out_1201664963462517165[119] = 0.0; + out_1201664963462517165[120] = 0.0; + out_1201664963462517165[121] = 0.0; + out_1201664963462517165[122] = 0.0; + out_1201664963462517165[123] = 0.0; + out_1201664963462517165[124] = 0.0; + out_1201664963462517165[125] = 0.0; + out_1201664963462517165[126] = 0.0; + out_1201664963462517165[127] = 0.0; + out_1201664963462517165[128] = 0.0; + out_1201664963462517165[129] = 0.0; + out_1201664963462517165[130] = 0.0; + out_1201664963462517165[131] = 0.0; + out_1201664963462517165[132] = 0.0; + out_1201664963462517165[133] = 1.0; + out_1201664963462517165[134] = 0.0; + out_1201664963462517165[135] = 0.0; + out_1201664963462517165[136] = 0.0; + out_1201664963462517165[137] = 0.0; + out_1201664963462517165[138] = 0.0; + out_1201664963462517165[139] = 0.0; + out_1201664963462517165[140] = 0.0; + out_1201664963462517165[141] = 0.0; + out_1201664963462517165[142] = 0.0; + out_1201664963462517165[143] = 0.0; + out_1201664963462517165[144] = 0.0; + out_1201664963462517165[145] = 0.0; + out_1201664963462517165[146] = 0.0; + out_1201664963462517165[147] = 0.0; + out_1201664963462517165[148] = 0.0; + out_1201664963462517165[149] = 0.0; + out_1201664963462517165[150] = 0.0; + out_1201664963462517165[151] = 0.0; + out_1201664963462517165[152] = 1.0; + out_1201664963462517165[153] = 0.0; + out_1201664963462517165[154] = 0.0; + out_1201664963462517165[155] = 0.0; + out_1201664963462517165[156] = 0.0; + out_1201664963462517165[157] = 0.0; + out_1201664963462517165[158] = 0.0; + out_1201664963462517165[159] = 0.0; + out_1201664963462517165[160] = 0.0; + out_1201664963462517165[161] = 0.0; + out_1201664963462517165[162] = 0.0; + out_1201664963462517165[163] = 0.0; + out_1201664963462517165[164] = 0.0; + out_1201664963462517165[165] = 0.0; + out_1201664963462517165[166] = 0.0; + out_1201664963462517165[167] = 0.0; + out_1201664963462517165[168] = 0.0; + out_1201664963462517165[169] = 0.0; + out_1201664963462517165[170] = 0.0; + out_1201664963462517165[171] = 1.0; + out_1201664963462517165[172] = 0.0; + out_1201664963462517165[173] = 0.0; + out_1201664963462517165[174] = 0.0; + out_1201664963462517165[175] = 0.0; + out_1201664963462517165[176] = 0.0; + out_1201664963462517165[177] = 0.0; + out_1201664963462517165[178] = 0.0; + out_1201664963462517165[179] = 0.0; + out_1201664963462517165[180] = 0.0; + out_1201664963462517165[181] = 0.0; + out_1201664963462517165[182] = 0.0; + out_1201664963462517165[183] = 0.0; + out_1201664963462517165[184] = 0.0; + out_1201664963462517165[185] = 0.0; + out_1201664963462517165[186] = 0.0; + out_1201664963462517165[187] = 0.0; + out_1201664963462517165[188] = 0.0; + out_1201664963462517165[189] = 0.0; + out_1201664963462517165[190] = 1.0; + out_1201664963462517165[191] = 0.0; + out_1201664963462517165[192] = 0.0; + out_1201664963462517165[193] = 0.0; + out_1201664963462517165[194] = 0.0; + out_1201664963462517165[195] = 0.0; + out_1201664963462517165[196] = 0.0; + out_1201664963462517165[197] = 0.0; + out_1201664963462517165[198] = 0.0; + out_1201664963462517165[199] = 0.0; + out_1201664963462517165[200] = 0.0; + out_1201664963462517165[201] = 0.0; + out_1201664963462517165[202] = 0.0; + out_1201664963462517165[203] = 0.0; + out_1201664963462517165[204] = 0.0; + out_1201664963462517165[205] = 0.0; + out_1201664963462517165[206] = 0.0; + out_1201664963462517165[207] = 0.0; + out_1201664963462517165[208] = 0.0; + out_1201664963462517165[209] = 1.0; + out_1201664963462517165[210] = 0.0; + out_1201664963462517165[211] = 0.0; + out_1201664963462517165[212] = 0.0; + out_1201664963462517165[213] = 0.0; + out_1201664963462517165[214] = 0.0; + out_1201664963462517165[215] = 0.0; + out_1201664963462517165[216] = 0.0; + out_1201664963462517165[217] = 0.0; + out_1201664963462517165[218] = 0.0; + out_1201664963462517165[219] = 0.0; + out_1201664963462517165[220] = 0.0; + out_1201664963462517165[221] = 0.0; + out_1201664963462517165[222] = 0.0; + out_1201664963462517165[223] = 0.0; + out_1201664963462517165[224] = 0.0; + out_1201664963462517165[225] = 0.0; + out_1201664963462517165[226] = 0.0; + out_1201664963462517165[227] = 0.0; + out_1201664963462517165[228] = 1.0; + out_1201664963462517165[229] = 0.0; + out_1201664963462517165[230] = 0.0; + out_1201664963462517165[231] = 0.0; + out_1201664963462517165[232] = 0.0; + out_1201664963462517165[233] = 0.0; + out_1201664963462517165[234] = 0.0; + out_1201664963462517165[235] = 0.0; + out_1201664963462517165[236] = 0.0; + out_1201664963462517165[237] = 0.0; + out_1201664963462517165[238] = 0.0; + out_1201664963462517165[239] = 0.0; + out_1201664963462517165[240] = 0.0; + out_1201664963462517165[241] = 0.0; + out_1201664963462517165[242] = 0.0; + out_1201664963462517165[243] = 0.0; + out_1201664963462517165[244] = 0.0; + out_1201664963462517165[245] = 0.0; + out_1201664963462517165[246] = 0.0; + out_1201664963462517165[247] = 1.0; + out_1201664963462517165[248] = 0.0; + out_1201664963462517165[249] = 0.0; + out_1201664963462517165[250] = 0.0; + out_1201664963462517165[251] = 0.0; + out_1201664963462517165[252] = 0.0; + out_1201664963462517165[253] = 0.0; + out_1201664963462517165[254] = 0.0; + out_1201664963462517165[255] = 0.0; + out_1201664963462517165[256] = 0.0; + out_1201664963462517165[257] = 0.0; + out_1201664963462517165[258] = 0.0; + out_1201664963462517165[259] = 0.0; + out_1201664963462517165[260] = 0.0; + out_1201664963462517165[261] = 0.0; + out_1201664963462517165[262] = 0.0; + out_1201664963462517165[263] = 0.0; + out_1201664963462517165[264] = 0.0; + out_1201664963462517165[265] = 0.0; + out_1201664963462517165[266] = 1.0; + out_1201664963462517165[267] = 0.0; + out_1201664963462517165[268] = 0.0; + out_1201664963462517165[269] = 0.0; + out_1201664963462517165[270] = 0.0; + out_1201664963462517165[271] = 0.0; + out_1201664963462517165[272] = 0.0; + out_1201664963462517165[273] = 0.0; + out_1201664963462517165[274] = 0.0; + out_1201664963462517165[275] = 0.0; + out_1201664963462517165[276] = 0.0; + out_1201664963462517165[277] = 0.0; + out_1201664963462517165[278] = 0.0; + out_1201664963462517165[279] = 0.0; + out_1201664963462517165[280] = 0.0; + out_1201664963462517165[281] = 0.0; + out_1201664963462517165[282] = 0.0; + out_1201664963462517165[283] = 0.0; + out_1201664963462517165[284] = 0.0; + out_1201664963462517165[285] = 1.0; + out_1201664963462517165[286] = 0.0; + out_1201664963462517165[287] = 0.0; + out_1201664963462517165[288] = 0.0; + out_1201664963462517165[289] = 0.0; + out_1201664963462517165[290] = 0.0; + out_1201664963462517165[291] = 0.0; + out_1201664963462517165[292] = 0.0; + out_1201664963462517165[293] = 0.0; + out_1201664963462517165[294] = 0.0; + out_1201664963462517165[295] = 0.0; + out_1201664963462517165[296] = 0.0; + out_1201664963462517165[297] = 0.0; + out_1201664963462517165[298] = 0.0; + out_1201664963462517165[299] = 0.0; + out_1201664963462517165[300] = 0.0; + out_1201664963462517165[301] = 0.0; + out_1201664963462517165[302] = 0.0; + out_1201664963462517165[303] = 0.0; + out_1201664963462517165[304] = 1.0; + out_1201664963462517165[305] = 0.0; + out_1201664963462517165[306] = 0.0; + out_1201664963462517165[307] = 0.0; + out_1201664963462517165[308] = 0.0; + out_1201664963462517165[309] = 0.0; + out_1201664963462517165[310] = 0.0; + out_1201664963462517165[311] = 0.0; + out_1201664963462517165[312] = 0.0; + out_1201664963462517165[313] = 0.0; + out_1201664963462517165[314] = 0.0; + out_1201664963462517165[315] = 0.0; + out_1201664963462517165[316] = 0.0; + out_1201664963462517165[317] = 0.0; + out_1201664963462517165[318] = 0.0; + out_1201664963462517165[319] = 0.0; + out_1201664963462517165[320] = 0.0; + out_1201664963462517165[321] = 0.0; + out_1201664963462517165[322] = 0.0; + out_1201664963462517165[323] = 1.0; } -void f_fun(double *state, double dt, double *out_8328845329683204295) { - out_8328845329683204295[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_8328845329683204295[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_8328845329683204295[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_8328845329683204295[3] = dt*state[12] + state[3]; - out_8328845329683204295[4] = dt*state[13] + state[4]; - out_8328845329683204295[5] = dt*state[14] + state[5]; - out_8328845329683204295[6] = state[6]; - out_8328845329683204295[7] = state[7]; - out_8328845329683204295[8] = state[8]; - out_8328845329683204295[9] = state[9]; - out_8328845329683204295[10] = state[10]; - out_8328845329683204295[11] = state[11]; - out_8328845329683204295[12] = state[12]; - out_8328845329683204295[13] = state[13]; - out_8328845329683204295[14] = state[14]; - out_8328845329683204295[15] = state[15]; - out_8328845329683204295[16] = state[16]; - out_8328845329683204295[17] = state[17]; +void f_fun(double *state, double dt, double *out_4925442091395050631) { + out_4925442091395050631[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_4925442091395050631[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_4925442091395050631[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_4925442091395050631[3] = dt*state[12] + state[3]; + out_4925442091395050631[4] = dt*state[13] + state[4]; + out_4925442091395050631[5] = dt*state[14] + state[5]; + out_4925442091395050631[6] = state[6]; + out_4925442091395050631[7] = state[7]; + out_4925442091395050631[8] = state[8]; + out_4925442091395050631[9] = state[9]; + out_4925442091395050631[10] = state[10]; + out_4925442091395050631[11] = state[11]; + out_4925442091395050631[12] = state[12]; + out_4925442091395050631[13] = state[13]; + out_4925442091395050631[14] = state[14]; + out_4925442091395050631[15] = state[15]; + out_4925442091395050631[16] = state[16]; + out_4925442091395050631[17] = state[17]; } -void F_fun(double *state, double dt, double *out_6465495001911852221) { - out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[2] = 0; - out_6465495001911852221[3] = 0; - out_6465495001911852221[4] = 0; - out_6465495001911852221[5] = 0; - out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[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_6465495001911852221[9] = 0; - out_6465495001911852221[10] = 0; - out_6465495001911852221[11] = 0; - out_6465495001911852221[12] = 0; - out_6465495001911852221[13] = 0; - out_6465495001911852221[14] = 0; - out_6465495001911852221[15] = 0; - out_6465495001911852221[16] = 0; - out_6465495001911852221[17] = 0; - out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[20] = 0; - out_6465495001911852221[21] = 0; - out_6465495001911852221[22] = 0; - out_6465495001911852221[23] = 0; - out_6465495001911852221[24] = 0; - out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[27] = 0; - out_6465495001911852221[28] = 0; - out_6465495001911852221[29] = 0; - out_6465495001911852221[30] = 0; - out_6465495001911852221[31] = 0; - out_6465495001911852221[32] = 0; - out_6465495001911852221[33] = 0; - out_6465495001911852221[34] = 0; - out_6465495001911852221[35] = 0; - out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[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_6465495001911852221[39] = 0; - out_6465495001911852221[40] = 0; - out_6465495001911852221[41] = 0; - out_6465495001911852221[42] = 0; - out_6465495001911852221[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_6465495001911852221[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_6465495001911852221[45] = 0; - out_6465495001911852221[46] = 0; - out_6465495001911852221[47] = 0; - out_6465495001911852221[48] = 0; - out_6465495001911852221[49] = 0; - out_6465495001911852221[50] = 0; - out_6465495001911852221[51] = 0; - out_6465495001911852221[52] = 0; - out_6465495001911852221[53] = 0; - out_6465495001911852221[54] = 0; - out_6465495001911852221[55] = 0; - out_6465495001911852221[56] = 0; - out_6465495001911852221[57] = 1; - out_6465495001911852221[58] = 0; - out_6465495001911852221[59] = 0; - out_6465495001911852221[60] = 0; - out_6465495001911852221[61] = 0; - out_6465495001911852221[62] = 0; - out_6465495001911852221[63] = 0; - out_6465495001911852221[64] = 0; - out_6465495001911852221[65] = 0; - out_6465495001911852221[66] = dt; - out_6465495001911852221[67] = 0; - out_6465495001911852221[68] = 0; - out_6465495001911852221[69] = 0; - out_6465495001911852221[70] = 0; - out_6465495001911852221[71] = 0; - out_6465495001911852221[72] = 0; - out_6465495001911852221[73] = 0; - out_6465495001911852221[74] = 0; - out_6465495001911852221[75] = 0; - out_6465495001911852221[76] = 1; - out_6465495001911852221[77] = 0; - out_6465495001911852221[78] = 0; - out_6465495001911852221[79] = 0; - out_6465495001911852221[80] = 0; - out_6465495001911852221[81] = 0; - out_6465495001911852221[82] = 0; - out_6465495001911852221[83] = 0; - out_6465495001911852221[84] = 0; - out_6465495001911852221[85] = dt; - out_6465495001911852221[86] = 0; - out_6465495001911852221[87] = 0; - out_6465495001911852221[88] = 0; - out_6465495001911852221[89] = 0; - out_6465495001911852221[90] = 0; - out_6465495001911852221[91] = 0; - out_6465495001911852221[92] = 0; - out_6465495001911852221[93] = 0; - out_6465495001911852221[94] = 0; - out_6465495001911852221[95] = 1; - out_6465495001911852221[96] = 0; - out_6465495001911852221[97] = 0; - out_6465495001911852221[98] = 0; - out_6465495001911852221[99] = 0; - out_6465495001911852221[100] = 0; - out_6465495001911852221[101] = 0; - out_6465495001911852221[102] = 0; - out_6465495001911852221[103] = 0; - out_6465495001911852221[104] = dt; - out_6465495001911852221[105] = 0; - out_6465495001911852221[106] = 0; - out_6465495001911852221[107] = 0; - out_6465495001911852221[108] = 0; - out_6465495001911852221[109] = 0; - out_6465495001911852221[110] = 0; - out_6465495001911852221[111] = 0; - out_6465495001911852221[112] = 0; - out_6465495001911852221[113] = 0; - out_6465495001911852221[114] = 1; - out_6465495001911852221[115] = 0; - out_6465495001911852221[116] = 0; - out_6465495001911852221[117] = 0; - out_6465495001911852221[118] = 0; - out_6465495001911852221[119] = 0; - out_6465495001911852221[120] = 0; - out_6465495001911852221[121] = 0; - out_6465495001911852221[122] = 0; - out_6465495001911852221[123] = 0; - out_6465495001911852221[124] = 0; - out_6465495001911852221[125] = 0; - out_6465495001911852221[126] = 0; - out_6465495001911852221[127] = 0; - out_6465495001911852221[128] = 0; - out_6465495001911852221[129] = 0; - out_6465495001911852221[130] = 0; - out_6465495001911852221[131] = 0; - out_6465495001911852221[132] = 0; - out_6465495001911852221[133] = 1; - out_6465495001911852221[134] = 0; - out_6465495001911852221[135] = 0; - out_6465495001911852221[136] = 0; - out_6465495001911852221[137] = 0; - out_6465495001911852221[138] = 0; - out_6465495001911852221[139] = 0; - out_6465495001911852221[140] = 0; - out_6465495001911852221[141] = 0; - out_6465495001911852221[142] = 0; - out_6465495001911852221[143] = 0; - out_6465495001911852221[144] = 0; - out_6465495001911852221[145] = 0; - out_6465495001911852221[146] = 0; - out_6465495001911852221[147] = 0; - out_6465495001911852221[148] = 0; - out_6465495001911852221[149] = 0; - out_6465495001911852221[150] = 0; - out_6465495001911852221[151] = 0; - out_6465495001911852221[152] = 1; - out_6465495001911852221[153] = 0; - out_6465495001911852221[154] = 0; - out_6465495001911852221[155] = 0; - out_6465495001911852221[156] = 0; - out_6465495001911852221[157] = 0; - out_6465495001911852221[158] = 0; - out_6465495001911852221[159] = 0; - out_6465495001911852221[160] = 0; - out_6465495001911852221[161] = 0; - out_6465495001911852221[162] = 0; - out_6465495001911852221[163] = 0; - out_6465495001911852221[164] = 0; - out_6465495001911852221[165] = 0; - out_6465495001911852221[166] = 0; - out_6465495001911852221[167] = 0; - out_6465495001911852221[168] = 0; - out_6465495001911852221[169] = 0; - out_6465495001911852221[170] = 0; - out_6465495001911852221[171] = 1; - out_6465495001911852221[172] = 0; - out_6465495001911852221[173] = 0; - out_6465495001911852221[174] = 0; - out_6465495001911852221[175] = 0; - out_6465495001911852221[176] = 0; - out_6465495001911852221[177] = 0; - out_6465495001911852221[178] = 0; - out_6465495001911852221[179] = 0; - out_6465495001911852221[180] = 0; - out_6465495001911852221[181] = 0; - out_6465495001911852221[182] = 0; - out_6465495001911852221[183] = 0; - out_6465495001911852221[184] = 0; - out_6465495001911852221[185] = 0; - out_6465495001911852221[186] = 0; - out_6465495001911852221[187] = 0; - out_6465495001911852221[188] = 0; - out_6465495001911852221[189] = 0; - out_6465495001911852221[190] = 1; - out_6465495001911852221[191] = 0; - out_6465495001911852221[192] = 0; - out_6465495001911852221[193] = 0; - out_6465495001911852221[194] = 0; - out_6465495001911852221[195] = 0; - out_6465495001911852221[196] = 0; - out_6465495001911852221[197] = 0; - out_6465495001911852221[198] = 0; - out_6465495001911852221[199] = 0; - out_6465495001911852221[200] = 0; - out_6465495001911852221[201] = 0; - out_6465495001911852221[202] = 0; - out_6465495001911852221[203] = 0; - out_6465495001911852221[204] = 0; - out_6465495001911852221[205] = 0; - out_6465495001911852221[206] = 0; - out_6465495001911852221[207] = 0; - out_6465495001911852221[208] = 0; - out_6465495001911852221[209] = 1; - out_6465495001911852221[210] = 0; - out_6465495001911852221[211] = 0; - out_6465495001911852221[212] = 0; - out_6465495001911852221[213] = 0; - out_6465495001911852221[214] = 0; - out_6465495001911852221[215] = 0; - out_6465495001911852221[216] = 0; - out_6465495001911852221[217] = 0; - out_6465495001911852221[218] = 0; - out_6465495001911852221[219] = 0; - out_6465495001911852221[220] = 0; - out_6465495001911852221[221] = 0; - out_6465495001911852221[222] = 0; - out_6465495001911852221[223] = 0; - out_6465495001911852221[224] = 0; - out_6465495001911852221[225] = 0; - out_6465495001911852221[226] = 0; - out_6465495001911852221[227] = 0; - out_6465495001911852221[228] = 1; - out_6465495001911852221[229] = 0; - out_6465495001911852221[230] = 0; - out_6465495001911852221[231] = 0; - out_6465495001911852221[232] = 0; - out_6465495001911852221[233] = 0; - out_6465495001911852221[234] = 0; - out_6465495001911852221[235] = 0; - out_6465495001911852221[236] = 0; - out_6465495001911852221[237] = 0; - out_6465495001911852221[238] = 0; - out_6465495001911852221[239] = 0; - out_6465495001911852221[240] = 0; - out_6465495001911852221[241] = 0; - out_6465495001911852221[242] = 0; - out_6465495001911852221[243] = 0; - out_6465495001911852221[244] = 0; - out_6465495001911852221[245] = 0; - out_6465495001911852221[246] = 0; - out_6465495001911852221[247] = 1; - out_6465495001911852221[248] = 0; - out_6465495001911852221[249] = 0; - out_6465495001911852221[250] = 0; - out_6465495001911852221[251] = 0; - out_6465495001911852221[252] = 0; - out_6465495001911852221[253] = 0; - out_6465495001911852221[254] = 0; - out_6465495001911852221[255] = 0; - out_6465495001911852221[256] = 0; - out_6465495001911852221[257] = 0; - out_6465495001911852221[258] = 0; - out_6465495001911852221[259] = 0; - out_6465495001911852221[260] = 0; - out_6465495001911852221[261] = 0; - out_6465495001911852221[262] = 0; - out_6465495001911852221[263] = 0; - out_6465495001911852221[264] = 0; - out_6465495001911852221[265] = 0; - out_6465495001911852221[266] = 1; - out_6465495001911852221[267] = 0; - out_6465495001911852221[268] = 0; - out_6465495001911852221[269] = 0; - out_6465495001911852221[270] = 0; - out_6465495001911852221[271] = 0; - out_6465495001911852221[272] = 0; - out_6465495001911852221[273] = 0; - out_6465495001911852221[274] = 0; - out_6465495001911852221[275] = 0; - out_6465495001911852221[276] = 0; - out_6465495001911852221[277] = 0; - out_6465495001911852221[278] = 0; - out_6465495001911852221[279] = 0; - out_6465495001911852221[280] = 0; - out_6465495001911852221[281] = 0; - out_6465495001911852221[282] = 0; - out_6465495001911852221[283] = 0; - out_6465495001911852221[284] = 0; - out_6465495001911852221[285] = 1; - out_6465495001911852221[286] = 0; - out_6465495001911852221[287] = 0; - out_6465495001911852221[288] = 0; - out_6465495001911852221[289] = 0; - out_6465495001911852221[290] = 0; - out_6465495001911852221[291] = 0; - out_6465495001911852221[292] = 0; - out_6465495001911852221[293] = 0; - out_6465495001911852221[294] = 0; - out_6465495001911852221[295] = 0; - out_6465495001911852221[296] = 0; - out_6465495001911852221[297] = 0; - out_6465495001911852221[298] = 0; - out_6465495001911852221[299] = 0; - out_6465495001911852221[300] = 0; - out_6465495001911852221[301] = 0; - out_6465495001911852221[302] = 0; - out_6465495001911852221[303] = 0; - out_6465495001911852221[304] = 1; - out_6465495001911852221[305] = 0; - out_6465495001911852221[306] = 0; - out_6465495001911852221[307] = 0; - out_6465495001911852221[308] = 0; - out_6465495001911852221[309] = 0; - out_6465495001911852221[310] = 0; - out_6465495001911852221[311] = 0; - out_6465495001911852221[312] = 0; - out_6465495001911852221[313] = 0; - out_6465495001911852221[314] = 0; - out_6465495001911852221[315] = 0; - out_6465495001911852221[316] = 0; - out_6465495001911852221[317] = 0; - out_6465495001911852221[318] = 0; - out_6465495001911852221[319] = 0; - out_6465495001911852221[320] = 0; - out_6465495001911852221[321] = 0; - out_6465495001911852221[322] = 0; - out_6465495001911852221[323] = 1; +void F_fun(double *state, double dt, double *out_3854196706556040023) { + out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[2] = 0; + out_3854196706556040023[3] = 0; + out_3854196706556040023[4] = 0; + out_3854196706556040023[5] = 0; + out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[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_3854196706556040023[9] = 0; + out_3854196706556040023[10] = 0; + out_3854196706556040023[11] = 0; + out_3854196706556040023[12] = 0; + out_3854196706556040023[13] = 0; + out_3854196706556040023[14] = 0; + out_3854196706556040023[15] = 0; + out_3854196706556040023[16] = 0; + out_3854196706556040023[17] = 0; + out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[20] = 0; + out_3854196706556040023[21] = 0; + out_3854196706556040023[22] = 0; + out_3854196706556040023[23] = 0; + out_3854196706556040023[24] = 0; + out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[27] = 0; + out_3854196706556040023[28] = 0; + out_3854196706556040023[29] = 0; + out_3854196706556040023[30] = 0; + out_3854196706556040023[31] = 0; + out_3854196706556040023[32] = 0; + out_3854196706556040023[33] = 0; + out_3854196706556040023[34] = 0; + out_3854196706556040023[35] = 0; + out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[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_3854196706556040023[39] = 0; + out_3854196706556040023[40] = 0; + out_3854196706556040023[41] = 0; + out_3854196706556040023[42] = 0; + out_3854196706556040023[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_3854196706556040023[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_3854196706556040023[45] = 0; + out_3854196706556040023[46] = 0; + out_3854196706556040023[47] = 0; + out_3854196706556040023[48] = 0; + out_3854196706556040023[49] = 0; + out_3854196706556040023[50] = 0; + out_3854196706556040023[51] = 0; + out_3854196706556040023[52] = 0; + out_3854196706556040023[53] = 0; + out_3854196706556040023[54] = 0; + out_3854196706556040023[55] = 0; + out_3854196706556040023[56] = 0; + out_3854196706556040023[57] = 1; + out_3854196706556040023[58] = 0; + out_3854196706556040023[59] = 0; + out_3854196706556040023[60] = 0; + out_3854196706556040023[61] = 0; + out_3854196706556040023[62] = 0; + out_3854196706556040023[63] = 0; + out_3854196706556040023[64] = 0; + out_3854196706556040023[65] = 0; + out_3854196706556040023[66] = dt; + out_3854196706556040023[67] = 0; + out_3854196706556040023[68] = 0; + out_3854196706556040023[69] = 0; + out_3854196706556040023[70] = 0; + out_3854196706556040023[71] = 0; + out_3854196706556040023[72] = 0; + out_3854196706556040023[73] = 0; + out_3854196706556040023[74] = 0; + out_3854196706556040023[75] = 0; + out_3854196706556040023[76] = 1; + out_3854196706556040023[77] = 0; + out_3854196706556040023[78] = 0; + out_3854196706556040023[79] = 0; + out_3854196706556040023[80] = 0; + out_3854196706556040023[81] = 0; + out_3854196706556040023[82] = 0; + out_3854196706556040023[83] = 0; + out_3854196706556040023[84] = 0; + out_3854196706556040023[85] = dt; + out_3854196706556040023[86] = 0; + out_3854196706556040023[87] = 0; + out_3854196706556040023[88] = 0; + out_3854196706556040023[89] = 0; + out_3854196706556040023[90] = 0; + out_3854196706556040023[91] = 0; + out_3854196706556040023[92] = 0; + out_3854196706556040023[93] = 0; + out_3854196706556040023[94] = 0; + out_3854196706556040023[95] = 1; + out_3854196706556040023[96] = 0; + out_3854196706556040023[97] = 0; + out_3854196706556040023[98] = 0; + out_3854196706556040023[99] = 0; + out_3854196706556040023[100] = 0; + out_3854196706556040023[101] = 0; + out_3854196706556040023[102] = 0; + out_3854196706556040023[103] = 0; + out_3854196706556040023[104] = dt; + out_3854196706556040023[105] = 0; + out_3854196706556040023[106] = 0; + out_3854196706556040023[107] = 0; + out_3854196706556040023[108] = 0; + out_3854196706556040023[109] = 0; + out_3854196706556040023[110] = 0; + out_3854196706556040023[111] = 0; + out_3854196706556040023[112] = 0; + out_3854196706556040023[113] = 0; + out_3854196706556040023[114] = 1; + out_3854196706556040023[115] = 0; + out_3854196706556040023[116] = 0; + out_3854196706556040023[117] = 0; + out_3854196706556040023[118] = 0; + out_3854196706556040023[119] = 0; + out_3854196706556040023[120] = 0; + out_3854196706556040023[121] = 0; + out_3854196706556040023[122] = 0; + out_3854196706556040023[123] = 0; + out_3854196706556040023[124] = 0; + out_3854196706556040023[125] = 0; + out_3854196706556040023[126] = 0; + out_3854196706556040023[127] = 0; + out_3854196706556040023[128] = 0; + out_3854196706556040023[129] = 0; + out_3854196706556040023[130] = 0; + out_3854196706556040023[131] = 0; + out_3854196706556040023[132] = 0; + out_3854196706556040023[133] = 1; + out_3854196706556040023[134] = 0; + out_3854196706556040023[135] = 0; + out_3854196706556040023[136] = 0; + out_3854196706556040023[137] = 0; + out_3854196706556040023[138] = 0; + out_3854196706556040023[139] = 0; + out_3854196706556040023[140] = 0; + out_3854196706556040023[141] = 0; + out_3854196706556040023[142] = 0; + out_3854196706556040023[143] = 0; + out_3854196706556040023[144] = 0; + out_3854196706556040023[145] = 0; + out_3854196706556040023[146] = 0; + out_3854196706556040023[147] = 0; + out_3854196706556040023[148] = 0; + out_3854196706556040023[149] = 0; + out_3854196706556040023[150] = 0; + out_3854196706556040023[151] = 0; + out_3854196706556040023[152] = 1; + out_3854196706556040023[153] = 0; + out_3854196706556040023[154] = 0; + out_3854196706556040023[155] = 0; + out_3854196706556040023[156] = 0; + out_3854196706556040023[157] = 0; + out_3854196706556040023[158] = 0; + out_3854196706556040023[159] = 0; + out_3854196706556040023[160] = 0; + out_3854196706556040023[161] = 0; + out_3854196706556040023[162] = 0; + out_3854196706556040023[163] = 0; + out_3854196706556040023[164] = 0; + out_3854196706556040023[165] = 0; + out_3854196706556040023[166] = 0; + out_3854196706556040023[167] = 0; + out_3854196706556040023[168] = 0; + out_3854196706556040023[169] = 0; + out_3854196706556040023[170] = 0; + out_3854196706556040023[171] = 1; + out_3854196706556040023[172] = 0; + out_3854196706556040023[173] = 0; + out_3854196706556040023[174] = 0; + out_3854196706556040023[175] = 0; + out_3854196706556040023[176] = 0; + out_3854196706556040023[177] = 0; + out_3854196706556040023[178] = 0; + out_3854196706556040023[179] = 0; + out_3854196706556040023[180] = 0; + out_3854196706556040023[181] = 0; + out_3854196706556040023[182] = 0; + out_3854196706556040023[183] = 0; + out_3854196706556040023[184] = 0; + out_3854196706556040023[185] = 0; + out_3854196706556040023[186] = 0; + out_3854196706556040023[187] = 0; + out_3854196706556040023[188] = 0; + out_3854196706556040023[189] = 0; + out_3854196706556040023[190] = 1; + out_3854196706556040023[191] = 0; + out_3854196706556040023[192] = 0; + out_3854196706556040023[193] = 0; + out_3854196706556040023[194] = 0; + out_3854196706556040023[195] = 0; + out_3854196706556040023[196] = 0; + out_3854196706556040023[197] = 0; + out_3854196706556040023[198] = 0; + out_3854196706556040023[199] = 0; + out_3854196706556040023[200] = 0; + out_3854196706556040023[201] = 0; + out_3854196706556040023[202] = 0; + out_3854196706556040023[203] = 0; + out_3854196706556040023[204] = 0; + out_3854196706556040023[205] = 0; + out_3854196706556040023[206] = 0; + out_3854196706556040023[207] = 0; + out_3854196706556040023[208] = 0; + out_3854196706556040023[209] = 1; + out_3854196706556040023[210] = 0; + out_3854196706556040023[211] = 0; + out_3854196706556040023[212] = 0; + out_3854196706556040023[213] = 0; + out_3854196706556040023[214] = 0; + out_3854196706556040023[215] = 0; + out_3854196706556040023[216] = 0; + out_3854196706556040023[217] = 0; + out_3854196706556040023[218] = 0; + out_3854196706556040023[219] = 0; + out_3854196706556040023[220] = 0; + out_3854196706556040023[221] = 0; + out_3854196706556040023[222] = 0; + out_3854196706556040023[223] = 0; + out_3854196706556040023[224] = 0; + out_3854196706556040023[225] = 0; + out_3854196706556040023[226] = 0; + out_3854196706556040023[227] = 0; + out_3854196706556040023[228] = 1; + out_3854196706556040023[229] = 0; + out_3854196706556040023[230] = 0; + out_3854196706556040023[231] = 0; + out_3854196706556040023[232] = 0; + out_3854196706556040023[233] = 0; + out_3854196706556040023[234] = 0; + out_3854196706556040023[235] = 0; + out_3854196706556040023[236] = 0; + out_3854196706556040023[237] = 0; + out_3854196706556040023[238] = 0; + out_3854196706556040023[239] = 0; + out_3854196706556040023[240] = 0; + out_3854196706556040023[241] = 0; + out_3854196706556040023[242] = 0; + out_3854196706556040023[243] = 0; + out_3854196706556040023[244] = 0; + out_3854196706556040023[245] = 0; + out_3854196706556040023[246] = 0; + out_3854196706556040023[247] = 1; + out_3854196706556040023[248] = 0; + out_3854196706556040023[249] = 0; + out_3854196706556040023[250] = 0; + out_3854196706556040023[251] = 0; + out_3854196706556040023[252] = 0; + out_3854196706556040023[253] = 0; + out_3854196706556040023[254] = 0; + out_3854196706556040023[255] = 0; + out_3854196706556040023[256] = 0; + out_3854196706556040023[257] = 0; + out_3854196706556040023[258] = 0; + out_3854196706556040023[259] = 0; + out_3854196706556040023[260] = 0; + out_3854196706556040023[261] = 0; + out_3854196706556040023[262] = 0; + out_3854196706556040023[263] = 0; + out_3854196706556040023[264] = 0; + out_3854196706556040023[265] = 0; + out_3854196706556040023[266] = 1; + out_3854196706556040023[267] = 0; + out_3854196706556040023[268] = 0; + out_3854196706556040023[269] = 0; + out_3854196706556040023[270] = 0; + out_3854196706556040023[271] = 0; + out_3854196706556040023[272] = 0; + out_3854196706556040023[273] = 0; + out_3854196706556040023[274] = 0; + out_3854196706556040023[275] = 0; + out_3854196706556040023[276] = 0; + out_3854196706556040023[277] = 0; + out_3854196706556040023[278] = 0; + out_3854196706556040023[279] = 0; + out_3854196706556040023[280] = 0; + out_3854196706556040023[281] = 0; + out_3854196706556040023[282] = 0; + out_3854196706556040023[283] = 0; + out_3854196706556040023[284] = 0; + out_3854196706556040023[285] = 1; + out_3854196706556040023[286] = 0; + out_3854196706556040023[287] = 0; + out_3854196706556040023[288] = 0; + out_3854196706556040023[289] = 0; + out_3854196706556040023[290] = 0; + out_3854196706556040023[291] = 0; + out_3854196706556040023[292] = 0; + out_3854196706556040023[293] = 0; + out_3854196706556040023[294] = 0; + out_3854196706556040023[295] = 0; + out_3854196706556040023[296] = 0; + out_3854196706556040023[297] = 0; + out_3854196706556040023[298] = 0; + out_3854196706556040023[299] = 0; + out_3854196706556040023[300] = 0; + out_3854196706556040023[301] = 0; + out_3854196706556040023[302] = 0; + out_3854196706556040023[303] = 0; + out_3854196706556040023[304] = 1; + out_3854196706556040023[305] = 0; + out_3854196706556040023[306] = 0; + out_3854196706556040023[307] = 0; + out_3854196706556040023[308] = 0; + out_3854196706556040023[309] = 0; + out_3854196706556040023[310] = 0; + out_3854196706556040023[311] = 0; + out_3854196706556040023[312] = 0; + out_3854196706556040023[313] = 0; + out_3854196706556040023[314] = 0; + out_3854196706556040023[315] = 0; + out_3854196706556040023[316] = 0; + out_3854196706556040023[317] = 0; + out_3854196706556040023[318] = 0; + out_3854196706556040023[319] = 0; + out_3854196706556040023[320] = 0; + out_3854196706556040023[321] = 0; + out_3854196706556040023[322] = 0; + out_3854196706556040023[323] = 1; } -void h_4(double *state, double *unused, double *out_7442631847161232120) { - out_7442631847161232120[0] = state[6] + state[9]; - out_7442631847161232120[1] = state[7] + state[10]; - out_7442631847161232120[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_6044371799568265897) { + out_6044371799568265897[0] = state[6] + state[9]; + out_6044371799568265897[1] = state[7] + state[10]; + out_6044371799568265897[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_8142960846497148836) { - out_8142960846497148836[0] = 0; - out_8142960846497148836[1] = 0; - out_8142960846497148836[2] = 0; - out_8142960846497148836[3] = 0; - out_8142960846497148836[4] = 0; - out_8142960846497148836[5] = 0; - out_8142960846497148836[6] = 1; - out_8142960846497148836[7] = 0; - out_8142960846497148836[8] = 0; - out_8142960846497148836[9] = 1; - out_8142960846497148836[10] = 0; - out_8142960846497148836[11] = 0; - out_8142960846497148836[12] = 0; - out_8142960846497148836[13] = 0; - out_8142960846497148836[14] = 0; - out_8142960846497148836[15] = 0; - out_8142960846497148836[16] = 0; - out_8142960846497148836[17] = 0; - out_8142960846497148836[18] = 0; - out_8142960846497148836[19] = 0; - out_8142960846497148836[20] = 0; - out_8142960846497148836[21] = 0; - out_8142960846497148836[22] = 0; - out_8142960846497148836[23] = 0; - out_8142960846497148836[24] = 0; - out_8142960846497148836[25] = 1; - out_8142960846497148836[26] = 0; - out_8142960846497148836[27] = 0; - out_8142960846497148836[28] = 1; - out_8142960846497148836[29] = 0; - out_8142960846497148836[30] = 0; - out_8142960846497148836[31] = 0; - out_8142960846497148836[32] = 0; - out_8142960846497148836[33] = 0; - out_8142960846497148836[34] = 0; - out_8142960846497148836[35] = 0; - out_8142960846497148836[36] = 0; - out_8142960846497148836[37] = 0; - out_8142960846497148836[38] = 0; - out_8142960846497148836[39] = 0; - out_8142960846497148836[40] = 0; - out_8142960846497148836[41] = 0; - out_8142960846497148836[42] = 0; - out_8142960846497148836[43] = 0; - out_8142960846497148836[44] = 1; - out_8142960846497148836[45] = 0; - out_8142960846497148836[46] = 0; - out_8142960846497148836[47] = 1; - out_8142960846497148836[48] = 0; - out_8142960846497148836[49] = 0; - out_8142960846497148836[50] = 0; - out_8142960846497148836[51] = 0; - out_8142960846497148836[52] = 0; - out_8142960846497148836[53] = 0; +void H_4(double *state, double *unused, double *out_8751938887176636728) { + out_8751938887176636728[0] = 0; + out_8751938887176636728[1] = 0; + out_8751938887176636728[2] = 0; + out_8751938887176636728[3] = 0; + out_8751938887176636728[4] = 0; + out_8751938887176636728[5] = 0; + out_8751938887176636728[6] = 1; + out_8751938887176636728[7] = 0; + out_8751938887176636728[8] = 0; + out_8751938887176636728[9] = 1; + out_8751938887176636728[10] = 0; + out_8751938887176636728[11] = 0; + out_8751938887176636728[12] = 0; + out_8751938887176636728[13] = 0; + out_8751938887176636728[14] = 0; + out_8751938887176636728[15] = 0; + out_8751938887176636728[16] = 0; + out_8751938887176636728[17] = 0; + out_8751938887176636728[18] = 0; + out_8751938887176636728[19] = 0; + out_8751938887176636728[20] = 0; + out_8751938887176636728[21] = 0; + out_8751938887176636728[22] = 0; + out_8751938887176636728[23] = 0; + out_8751938887176636728[24] = 0; + out_8751938887176636728[25] = 1; + out_8751938887176636728[26] = 0; + out_8751938887176636728[27] = 0; + out_8751938887176636728[28] = 1; + out_8751938887176636728[29] = 0; + out_8751938887176636728[30] = 0; + out_8751938887176636728[31] = 0; + out_8751938887176636728[32] = 0; + out_8751938887176636728[33] = 0; + out_8751938887176636728[34] = 0; + out_8751938887176636728[35] = 0; + out_8751938887176636728[36] = 0; + out_8751938887176636728[37] = 0; + out_8751938887176636728[38] = 0; + out_8751938887176636728[39] = 0; + out_8751938887176636728[40] = 0; + out_8751938887176636728[41] = 0; + out_8751938887176636728[42] = 0; + out_8751938887176636728[43] = 0; + out_8751938887176636728[44] = 1; + out_8751938887176636728[45] = 0; + out_8751938887176636728[46] = 0; + out_8751938887176636728[47] = 1; + out_8751938887176636728[48] = 0; + out_8751938887176636728[49] = 0; + out_8751938887176636728[50] = 0; + out_8751938887176636728[51] = 0; + out_8751938887176636728[52] = 0; + out_8751938887176636728[53] = 0; } -void h_10(double *state, double *unused, double *out_6865183365695080269) { - out_6865183365695080269[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_6865183365695080269[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_6865183365695080269[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_9126782512040865980) { + out_9126782512040865980[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_9126782512040865980[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_9126782512040865980[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_456103805919410821) { - out_456103805919410821[0] = 0; - out_456103805919410821[1] = 9.8100000000000005*cos(state[1]); - out_456103805919410821[2] = 0; - out_456103805919410821[3] = 0; - out_456103805919410821[4] = -state[8]; - out_456103805919410821[5] = state[7]; - out_456103805919410821[6] = 0; - out_456103805919410821[7] = state[5]; - out_456103805919410821[8] = -state[4]; - out_456103805919410821[9] = 0; - out_456103805919410821[10] = 0; - out_456103805919410821[11] = 0; - out_456103805919410821[12] = 1; - out_456103805919410821[13] = 0; - out_456103805919410821[14] = 0; - out_456103805919410821[15] = 1; - out_456103805919410821[16] = 0; - out_456103805919410821[17] = 0; - out_456103805919410821[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_456103805919410821[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_456103805919410821[20] = 0; - out_456103805919410821[21] = state[8]; - out_456103805919410821[22] = 0; - out_456103805919410821[23] = -state[6]; - out_456103805919410821[24] = -state[5]; - out_456103805919410821[25] = 0; - out_456103805919410821[26] = state[3]; - out_456103805919410821[27] = 0; - out_456103805919410821[28] = 0; - out_456103805919410821[29] = 0; - out_456103805919410821[30] = 0; - out_456103805919410821[31] = 1; - out_456103805919410821[32] = 0; - out_456103805919410821[33] = 0; - out_456103805919410821[34] = 1; - out_456103805919410821[35] = 0; - out_456103805919410821[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_456103805919410821[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_456103805919410821[38] = 0; - out_456103805919410821[39] = -state[7]; - out_456103805919410821[40] = state[6]; - out_456103805919410821[41] = 0; - out_456103805919410821[42] = state[4]; - out_456103805919410821[43] = -state[3]; - out_456103805919410821[44] = 0; - out_456103805919410821[45] = 0; - out_456103805919410821[46] = 0; - out_456103805919410821[47] = 0; - out_456103805919410821[48] = 0; - out_456103805919410821[49] = 0; - out_456103805919410821[50] = 1; - out_456103805919410821[51] = 0; - out_456103805919410821[52] = 0; - out_456103805919410821[53] = 1; +void H_10(double *state, double *unused, double *out_2728048388349900962) { + out_2728048388349900962[0] = 0; + out_2728048388349900962[1] = 9.8100000000000005*cos(state[1]); + out_2728048388349900962[2] = 0; + out_2728048388349900962[3] = 0; + out_2728048388349900962[4] = -state[8]; + out_2728048388349900962[5] = state[7]; + out_2728048388349900962[6] = 0; + out_2728048388349900962[7] = state[5]; + out_2728048388349900962[8] = -state[4]; + out_2728048388349900962[9] = 0; + out_2728048388349900962[10] = 0; + out_2728048388349900962[11] = 0; + out_2728048388349900962[12] = 1; + out_2728048388349900962[13] = 0; + out_2728048388349900962[14] = 0; + out_2728048388349900962[15] = 1; + out_2728048388349900962[16] = 0; + out_2728048388349900962[17] = 0; + out_2728048388349900962[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_2728048388349900962[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_2728048388349900962[20] = 0; + out_2728048388349900962[21] = state[8]; + out_2728048388349900962[22] = 0; + out_2728048388349900962[23] = -state[6]; + out_2728048388349900962[24] = -state[5]; + out_2728048388349900962[25] = 0; + out_2728048388349900962[26] = state[3]; + out_2728048388349900962[27] = 0; + out_2728048388349900962[28] = 0; + out_2728048388349900962[29] = 0; + out_2728048388349900962[30] = 0; + out_2728048388349900962[31] = 1; + out_2728048388349900962[32] = 0; + out_2728048388349900962[33] = 0; + out_2728048388349900962[34] = 1; + out_2728048388349900962[35] = 0; + out_2728048388349900962[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_2728048388349900962[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_2728048388349900962[38] = 0; + out_2728048388349900962[39] = -state[7]; + out_2728048388349900962[40] = state[6]; + out_2728048388349900962[41] = 0; + out_2728048388349900962[42] = state[4]; + out_2728048388349900962[43] = -state[3]; + out_2728048388349900962[44] = 0; + out_2728048388349900962[45] = 0; + out_2728048388349900962[46] = 0; + out_2728048388349900962[47] = 0; + out_2728048388349900962[48] = 0; + out_2728048388349900962[49] = 0; + out_2728048388349900962[50] = 1; + out_2728048388349900962[51] = 0; + out_2728048388349900962[52] = 0; + out_2728048388349900962[53] = 1; } -void h_13(double *state, double *unused, double *out_2130091715364414233) { - out_2130091715364414233[0] = state[3]; - out_2130091715364414233[1] = state[4]; - out_2130091715364414233[2] = state[5]; +void h_13(double *state, double *unused, double *out_6648711810487729870) { + out_6648711810487729870[0] = state[3]; + out_6648711810487729870[1] = state[4]; + out_6648711810487729870[2] = state[5]; } -void H_13(double *state, double *unused, double *out_2693152018895701851) { - out_2693152018895701851[0] = 0; - out_2693152018895701851[1] = 0; - out_2693152018895701851[2] = 0; - out_2693152018895701851[3] = 1; - out_2693152018895701851[4] = 0; - out_2693152018895701851[5] = 0; - out_2693152018895701851[6] = 0; - out_2693152018895701851[7] = 0; - out_2693152018895701851[8] = 0; - out_2693152018895701851[9] = 0; - out_2693152018895701851[10] = 0; - out_2693152018895701851[11] = 0; - out_2693152018895701851[12] = 0; - out_2693152018895701851[13] = 0; - out_2693152018895701851[14] = 0; - out_2693152018895701851[15] = 0; - out_2693152018895701851[16] = 0; - out_2693152018895701851[17] = 0; - out_2693152018895701851[18] = 0; - out_2693152018895701851[19] = 0; - out_2693152018895701851[20] = 0; - out_2693152018895701851[21] = 0; - out_2693152018895701851[22] = 1; - out_2693152018895701851[23] = 0; - out_2693152018895701851[24] = 0; - out_2693152018895701851[25] = 0; - out_2693152018895701851[26] = 0; - out_2693152018895701851[27] = 0; - out_2693152018895701851[28] = 0; - out_2693152018895701851[29] = 0; - out_2693152018895701851[30] = 0; - out_2693152018895701851[31] = 0; - out_2693152018895701851[32] = 0; - out_2693152018895701851[33] = 0; - out_2693152018895701851[34] = 0; - out_2693152018895701851[35] = 0; - out_2693152018895701851[36] = 0; - out_2693152018895701851[37] = 0; - out_2693152018895701851[38] = 0; - out_2693152018895701851[39] = 0; - out_2693152018895701851[40] = 0; - out_2693152018895701851[41] = 1; - out_2693152018895701851[42] = 0; - out_2693152018895701851[43] = 0; - out_2693152018895701851[44] = 0; - out_2693152018895701851[45] = 0; - out_2693152018895701851[46] = 0; - out_2693152018895701851[47] = 0; - out_2693152018895701851[48] = 0; - out_2693152018895701851[49] = 0; - out_2693152018895701851[50] = 0; - out_2693152018895701851[51] = 0; - out_2693152018895701851[52] = 0; - out_2693152018895701851[53] = 0; +void H_13(double *state, double *unused, double *out_1141307678859935799) { + out_1141307678859935799[0] = 0; + out_1141307678859935799[1] = 0; + out_1141307678859935799[2] = 0; + out_1141307678859935799[3] = 1; + out_1141307678859935799[4] = 0; + out_1141307678859935799[5] = 0; + out_1141307678859935799[6] = 0; + out_1141307678859935799[7] = 0; + out_1141307678859935799[8] = 0; + out_1141307678859935799[9] = 0; + out_1141307678859935799[10] = 0; + out_1141307678859935799[11] = 0; + out_1141307678859935799[12] = 0; + out_1141307678859935799[13] = 0; + out_1141307678859935799[14] = 0; + out_1141307678859935799[15] = 0; + out_1141307678859935799[16] = 0; + out_1141307678859935799[17] = 0; + out_1141307678859935799[18] = 0; + out_1141307678859935799[19] = 0; + out_1141307678859935799[20] = 0; + out_1141307678859935799[21] = 0; + out_1141307678859935799[22] = 1; + out_1141307678859935799[23] = 0; + out_1141307678859935799[24] = 0; + out_1141307678859935799[25] = 0; + out_1141307678859935799[26] = 0; + out_1141307678859935799[27] = 0; + out_1141307678859935799[28] = 0; + out_1141307678859935799[29] = 0; + out_1141307678859935799[30] = 0; + out_1141307678859935799[31] = 0; + out_1141307678859935799[32] = 0; + out_1141307678859935799[33] = 0; + out_1141307678859935799[34] = 0; + out_1141307678859935799[35] = 0; + out_1141307678859935799[36] = 0; + out_1141307678859935799[37] = 0; + out_1141307678859935799[38] = 0; + out_1141307678859935799[39] = 0; + out_1141307678859935799[40] = 0; + out_1141307678859935799[41] = 1; + out_1141307678859935799[42] = 0; + out_1141307678859935799[43] = 0; + out_1141307678859935799[44] = 0; + out_1141307678859935799[45] = 0; + out_1141307678859935799[46] = 0; + out_1141307678859935799[47] = 0; + out_1141307678859935799[48] = 0; + out_1141307678859935799[49] = 0; + out_1141307678859935799[50] = 0; + out_1141307678859935799[51] = 0; + out_1141307678859935799[52] = 0; + out_1141307678859935799[53] = 0; } -void h_14(double *state, double *unused, double *out_2314638522999502046) { - out_2314638522999502046[0] = state[6]; - out_2314638522999502046[1] = state[7]; - out_2314638522999502046[2] = state[8]; +void h_14(double *state, double *unused, double *out_6901319929397273887) { + out_6901319929397273887[0] = state[6]; + out_6901319929397273887[1] = state[7]; + out_6901319929397273887[2] = state[8]; } -void H_14(double *state, double *unused, double *out_6340542370872918251) { - out_6340542370872918251[0] = 0; - out_6340542370872918251[1] = 0; - out_6340542370872918251[2] = 0; - out_6340542370872918251[3] = 0; - out_6340542370872918251[4] = 0; - out_6340542370872918251[5] = 0; - out_6340542370872918251[6] = 1; - out_6340542370872918251[7] = 0; - out_6340542370872918251[8] = 0; - out_6340542370872918251[9] = 0; - out_6340542370872918251[10] = 0; - out_6340542370872918251[11] = 0; - out_6340542370872918251[12] = 0; - out_6340542370872918251[13] = 0; - out_6340542370872918251[14] = 0; - out_6340542370872918251[15] = 0; - out_6340542370872918251[16] = 0; - out_6340542370872918251[17] = 0; - out_6340542370872918251[18] = 0; - out_6340542370872918251[19] = 0; - out_6340542370872918251[20] = 0; - out_6340542370872918251[21] = 0; - out_6340542370872918251[22] = 0; - out_6340542370872918251[23] = 0; - out_6340542370872918251[24] = 0; - out_6340542370872918251[25] = 1; - out_6340542370872918251[26] = 0; - out_6340542370872918251[27] = 0; - out_6340542370872918251[28] = 0; - out_6340542370872918251[29] = 0; - out_6340542370872918251[30] = 0; - out_6340542370872918251[31] = 0; - out_6340542370872918251[32] = 0; - out_6340542370872918251[33] = 0; - out_6340542370872918251[34] = 0; - out_6340542370872918251[35] = 0; - out_6340542370872918251[36] = 0; - out_6340542370872918251[37] = 0; - out_6340542370872918251[38] = 0; - out_6340542370872918251[39] = 0; - out_6340542370872918251[40] = 0; - out_6340542370872918251[41] = 0; - out_6340542370872918251[42] = 0; - out_6340542370872918251[43] = 0; - out_6340542370872918251[44] = 1; - out_6340542370872918251[45] = 0; - out_6340542370872918251[46] = 0; - out_6340542370872918251[47] = 0; - out_6340542370872918251[48] = 0; - out_6340542370872918251[49] = 0; - out_6340542370872918251[50] = 0; - out_6340542370872918251[51] = 0; - out_6340542370872918251[52] = 0; - out_6340542370872918251[53] = 0; +void H_14(double *state, double *unused, double *out_4788698030837152199) { + out_4788698030837152199[0] = 0; + out_4788698030837152199[1] = 0; + out_4788698030837152199[2] = 0; + out_4788698030837152199[3] = 0; + out_4788698030837152199[4] = 0; + out_4788698030837152199[5] = 0; + out_4788698030837152199[6] = 1; + out_4788698030837152199[7] = 0; + out_4788698030837152199[8] = 0; + out_4788698030837152199[9] = 0; + out_4788698030837152199[10] = 0; + out_4788698030837152199[11] = 0; + out_4788698030837152199[12] = 0; + out_4788698030837152199[13] = 0; + out_4788698030837152199[14] = 0; + out_4788698030837152199[15] = 0; + out_4788698030837152199[16] = 0; + out_4788698030837152199[17] = 0; + out_4788698030837152199[18] = 0; + out_4788698030837152199[19] = 0; + out_4788698030837152199[20] = 0; + out_4788698030837152199[21] = 0; + out_4788698030837152199[22] = 0; + out_4788698030837152199[23] = 0; + out_4788698030837152199[24] = 0; + out_4788698030837152199[25] = 1; + out_4788698030837152199[26] = 0; + out_4788698030837152199[27] = 0; + out_4788698030837152199[28] = 0; + out_4788698030837152199[29] = 0; + out_4788698030837152199[30] = 0; + out_4788698030837152199[31] = 0; + out_4788698030837152199[32] = 0; + out_4788698030837152199[33] = 0; + out_4788698030837152199[34] = 0; + out_4788698030837152199[35] = 0; + out_4788698030837152199[36] = 0; + out_4788698030837152199[37] = 0; + out_4788698030837152199[38] = 0; + out_4788698030837152199[39] = 0; + out_4788698030837152199[40] = 0; + out_4788698030837152199[41] = 0; + out_4788698030837152199[42] = 0; + out_4788698030837152199[43] = 0; + out_4788698030837152199[44] = 1; + out_4788698030837152199[45] = 0; + out_4788698030837152199[46] = 0; + out_4788698030837152199[47] = 0; + out_4788698030837152199[48] = 0; + out_4788698030837152199[49] = 0; + out_4788698030837152199[50] = 0; + out_4788698030837152199[51] = 0; + out_4788698030837152199[52] = 0; + out_4788698030837152199[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_6891956137394554809) { - err_fun(nom_x, delta_x, out_6891956137394554809); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6531280423659383110) { + err_fun(nom_x, delta_x, out_6531280423659383110); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_352074767866048570) { - inv_err_fun(nom_x, true_x, out_352074767866048570); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2847042605170284069) { + inv_err_fun(nom_x, true_x, out_2847042605170284069); } -void pose_H_mod_fun(double *state, double *out_57344037476075931) { - H_mod_fun(state, out_57344037476075931); +void pose_H_mod_fun(double *state, double *out_1201664963462517165) { + H_mod_fun(state, out_1201664963462517165); } -void pose_f_fun(double *state, double dt, double *out_8328845329683204295) { - f_fun(state, dt, out_8328845329683204295); +void pose_f_fun(double *state, double dt, double *out_4925442091395050631) { + f_fun(state, dt, out_4925442091395050631); } -void pose_F_fun(double *state, double dt, double *out_6465495001911852221) { - F_fun(state, dt, out_6465495001911852221); +void pose_F_fun(double *state, double dt, double *out_3854196706556040023) { + F_fun(state, dt, out_3854196706556040023); } -void pose_h_4(double *state, double *unused, double *out_7442631847161232120) { - h_4(state, unused, out_7442631847161232120); +void pose_h_4(double *state, double *unused, double *out_6044371799568265897) { + h_4(state, unused, out_6044371799568265897); } -void pose_H_4(double *state, double *unused, double *out_8142960846497148836) { - H_4(state, unused, out_8142960846497148836); +void pose_H_4(double *state, double *unused, double *out_8751938887176636728) { + H_4(state, unused, out_8751938887176636728); } -void pose_h_10(double *state, double *unused, double *out_6865183365695080269) { - h_10(state, unused, out_6865183365695080269); +void pose_h_10(double *state, double *unused, double *out_9126782512040865980) { + h_10(state, unused, out_9126782512040865980); } -void pose_H_10(double *state, double *unused, double *out_456103805919410821) { - H_10(state, unused, out_456103805919410821); +void pose_H_10(double *state, double *unused, double *out_2728048388349900962) { + H_10(state, unused, out_2728048388349900962); } -void pose_h_13(double *state, double *unused, double *out_2130091715364414233) { - h_13(state, unused, out_2130091715364414233); +void pose_h_13(double *state, double *unused, double *out_6648711810487729870) { + h_13(state, unused, out_6648711810487729870); } -void pose_H_13(double *state, double *unused, double *out_2693152018895701851) { - H_13(state, unused, out_2693152018895701851); +void pose_H_13(double *state, double *unused, double *out_1141307678859935799) { + H_13(state, unused, out_1141307678859935799); } -void pose_h_14(double *state, double *unused, double *out_2314638522999502046) { - h_14(state, unused, out_2314638522999502046); +void pose_h_14(double *state, double *unused, double *out_6901319929397273887) { + h_14(state, unused, out_6901319929397273887); } -void pose_H_14(double *state, double *unused, double *out_6340542370872918251) { - H_14(state, unused, out_6340542370872918251); +void pose_H_14(double *state, double *unused, double *out_4788698030837152199) { + H_14(state, unused, out_4788698030837152199); } 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 cd6119db3..bc3ed9a96 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_6891956137394554809); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_352074767866048570); -void pose_H_mod_fun(double *state, double *out_57344037476075931); -void pose_f_fun(double *state, double dt, double *out_8328845329683204295); -void pose_F_fun(double *state, double dt, double *out_6465495001911852221); -void pose_h_4(double *state, double *unused, double *out_7442631847161232120); -void pose_H_4(double *state, double *unused, double *out_8142960846497148836); -void pose_h_10(double *state, double *unused, double *out_6865183365695080269); -void pose_H_10(double *state, double *unused, double *out_456103805919410821); -void pose_h_13(double *state, double *unused, double *out_2130091715364414233); -void pose_H_13(double *state, double *unused, double *out_2693152018895701851); -void pose_h_14(double *state, double *unused, double *out_2314638522999502046); -void pose_H_14(double *state, double *unused, double *out_6340542370872918251); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6531280423659383110); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2847042605170284069); +void pose_H_mod_fun(double *state, double *out_1201664963462517165); +void pose_f_fun(double *state, double dt, double *out_4925442091395050631); +void pose_F_fun(double *state, double dt, double *out_3854196706556040023); +void pose_h_4(double *state, double *unused, double *out_6044371799568265897); +void pose_H_4(double *state, double *unused, double *out_8751938887176636728); +void pose_h_10(double *state, double *unused, double *out_9126782512040865980); +void pose_H_10(double *state, double *unused, double *out_2728048388349900962); +void pose_h_13(double *state, double *unused, double *out_6648711810487729870); +void pose_H_13(double *state, double *unused, double *out_1141307678859935799); +void pose_h_14(double *state, double *unused, double *out_6901319929397273887); +void pose_H_14(double *state, double *unused, double *out_4788698030837152199); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index d25bb5325..1b95da792 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index f497e7e3f..cf0519349 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 435247293..1d46a13ad 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 09f431ed0..15f5104f0 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 401f7d44e..29e1ba83e 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index da108f5b4..b8011cc33 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ