diff --git a/common/libcommon.a b/common/libcommon.a index daaf9e0b9..79a984d38 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 c63af44bc..5e97323a4 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 c0fe5575d..733fa2710 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 3ab259f47..95cfa785d 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 ec7bba7ca..58bcb21b1 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 fde8ca60a..045c92224 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 6d06b10e4..36ad99c97 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 3b3670cae..5225b82f9 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 6c75311da..09c1e9cf5 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 6c75311da..09c1e9cf5 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 fd0f9ef5e..fb11c0434 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 3b3670cae..5225b82f9 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 3d7e852b7..1ce3a787e 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-47c2cc99-DEBUG"; +const uint8_t gitversion[19] = "DEV-e5cc7460-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 1131eb781..1023134df 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 e5ab600d8..dbd53064c 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 da9e972f0..bfc239ed5 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 418fefc18..2936260bc 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 b2b6725a1..8907aef0e 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 da905ab08..c1c1dae5e 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 9c5548685..8047a27da 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 796e24bb3..744190b3b 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 7faaac18d..4ba1eb791 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 3bd8a9196..59ff4ccfb 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 fd67759d2..13e46a3df 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 509abeef1..3d2df1174 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 8cdfa9df6..be4a4d972 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 5705980b1..5b7ebbf5f 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 b1ed508aa..c228035c3 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 6885b65d3..0e82a2dc6 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 b173864c4..e0b25f7cf 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 2b5232496..e4ccbadb9 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 fa0909228..f787ea248 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 5b8be3acd..f0c8f4a22 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 5c5d90c79..7a7d0a5d1 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-47c2cc99-DEBUG \ No newline at end of file +DEV-e5cc7460-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 9edda8b3b..9c144a3ac 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_2057434223379957831) { - out_2057434223379957831[0] = delta_x[0] + nom_x[0]; - out_2057434223379957831[1] = delta_x[1] + nom_x[1]; - out_2057434223379957831[2] = delta_x[2] + nom_x[2]; - out_2057434223379957831[3] = delta_x[3] + nom_x[3]; - out_2057434223379957831[4] = delta_x[4] + nom_x[4]; - out_2057434223379957831[5] = delta_x[5] + nom_x[5]; - out_2057434223379957831[6] = delta_x[6] + nom_x[6]; - out_2057434223379957831[7] = delta_x[7] + nom_x[7]; - out_2057434223379957831[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_2765937094321424100) { + out_2765937094321424100[0] = delta_x[0] + nom_x[0]; + out_2765937094321424100[1] = delta_x[1] + nom_x[1]; + out_2765937094321424100[2] = delta_x[2] + nom_x[2]; + out_2765937094321424100[3] = delta_x[3] + nom_x[3]; + out_2765937094321424100[4] = delta_x[4] + nom_x[4]; + out_2765937094321424100[5] = delta_x[5] + nom_x[5]; + out_2765937094321424100[6] = delta_x[6] + nom_x[6]; + out_2765937094321424100[7] = delta_x[7] + nom_x[7]; + out_2765937094321424100[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4924816816161575884) { - out_4924816816161575884[0] = -nom_x[0] + true_x[0]; - out_4924816816161575884[1] = -nom_x[1] + true_x[1]; - out_4924816816161575884[2] = -nom_x[2] + true_x[2]; - out_4924816816161575884[3] = -nom_x[3] + true_x[3]; - out_4924816816161575884[4] = -nom_x[4] + true_x[4]; - out_4924816816161575884[5] = -nom_x[5] + true_x[5]; - out_4924816816161575884[6] = -nom_x[6] + true_x[6]; - out_4924816816161575884[7] = -nom_x[7] + true_x[7]; - out_4924816816161575884[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1382255689110899984) { + out_1382255689110899984[0] = -nom_x[0] + true_x[0]; + out_1382255689110899984[1] = -nom_x[1] + true_x[1]; + out_1382255689110899984[2] = -nom_x[2] + true_x[2]; + out_1382255689110899984[3] = -nom_x[3] + true_x[3]; + out_1382255689110899984[4] = -nom_x[4] + true_x[4]; + out_1382255689110899984[5] = -nom_x[5] + true_x[5]; + out_1382255689110899984[6] = -nom_x[6] + true_x[6]; + out_1382255689110899984[7] = -nom_x[7] + true_x[7]; + out_1382255689110899984[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_8615316232539034437) { - out_8615316232539034437[0] = 1.0; - out_8615316232539034437[1] = 0.0; - out_8615316232539034437[2] = 0.0; - out_8615316232539034437[3] = 0.0; - out_8615316232539034437[4] = 0.0; - out_8615316232539034437[5] = 0.0; - out_8615316232539034437[6] = 0.0; - out_8615316232539034437[7] = 0.0; - out_8615316232539034437[8] = 0.0; - out_8615316232539034437[9] = 0.0; - out_8615316232539034437[10] = 1.0; - out_8615316232539034437[11] = 0.0; - out_8615316232539034437[12] = 0.0; - out_8615316232539034437[13] = 0.0; - out_8615316232539034437[14] = 0.0; - out_8615316232539034437[15] = 0.0; - out_8615316232539034437[16] = 0.0; - out_8615316232539034437[17] = 0.0; - out_8615316232539034437[18] = 0.0; - out_8615316232539034437[19] = 0.0; - out_8615316232539034437[20] = 1.0; - out_8615316232539034437[21] = 0.0; - out_8615316232539034437[22] = 0.0; - out_8615316232539034437[23] = 0.0; - out_8615316232539034437[24] = 0.0; - out_8615316232539034437[25] = 0.0; - out_8615316232539034437[26] = 0.0; - out_8615316232539034437[27] = 0.0; - out_8615316232539034437[28] = 0.0; - out_8615316232539034437[29] = 0.0; - out_8615316232539034437[30] = 1.0; - out_8615316232539034437[31] = 0.0; - out_8615316232539034437[32] = 0.0; - out_8615316232539034437[33] = 0.0; - out_8615316232539034437[34] = 0.0; - out_8615316232539034437[35] = 0.0; - out_8615316232539034437[36] = 0.0; - out_8615316232539034437[37] = 0.0; - out_8615316232539034437[38] = 0.0; - out_8615316232539034437[39] = 0.0; - out_8615316232539034437[40] = 1.0; - out_8615316232539034437[41] = 0.0; - out_8615316232539034437[42] = 0.0; - out_8615316232539034437[43] = 0.0; - out_8615316232539034437[44] = 0.0; - out_8615316232539034437[45] = 0.0; - out_8615316232539034437[46] = 0.0; - out_8615316232539034437[47] = 0.0; - out_8615316232539034437[48] = 0.0; - out_8615316232539034437[49] = 0.0; - out_8615316232539034437[50] = 1.0; - out_8615316232539034437[51] = 0.0; - out_8615316232539034437[52] = 0.0; - out_8615316232539034437[53] = 0.0; - out_8615316232539034437[54] = 0.0; - out_8615316232539034437[55] = 0.0; - out_8615316232539034437[56] = 0.0; - out_8615316232539034437[57] = 0.0; - out_8615316232539034437[58] = 0.0; - out_8615316232539034437[59] = 0.0; - out_8615316232539034437[60] = 1.0; - out_8615316232539034437[61] = 0.0; - out_8615316232539034437[62] = 0.0; - out_8615316232539034437[63] = 0.0; - out_8615316232539034437[64] = 0.0; - out_8615316232539034437[65] = 0.0; - out_8615316232539034437[66] = 0.0; - out_8615316232539034437[67] = 0.0; - out_8615316232539034437[68] = 0.0; - out_8615316232539034437[69] = 0.0; - out_8615316232539034437[70] = 1.0; - out_8615316232539034437[71] = 0.0; - out_8615316232539034437[72] = 0.0; - out_8615316232539034437[73] = 0.0; - out_8615316232539034437[74] = 0.0; - out_8615316232539034437[75] = 0.0; - out_8615316232539034437[76] = 0.0; - out_8615316232539034437[77] = 0.0; - out_8615316232539034437[78] = 0.0; - out_8615316232539034437[79] = 0.0; - out_8615316232539034437[80] = 1.0; +void H_mod_fun(double *state, double *out_6175643942212596402) { + out_6175643942212596402[0] = 1.0; + out_6175643942212596402[1] = 0.0; + out_6175643942212596402[2] = 0.0; + out_6175643942212596402[3] = 0.0; + out_6175643942212596402[4] = 0.0; + out_6175643942212596402[5] = 0.0; + out_6175643942212596402[6] = 0.0; + out_6175643942212596402[7] = 0.0; + out_6175643942212596402[8] = 0.0; + out_6175643942212596402[9] = 0.0; + out_6175643942212596402[10] = 1.0; + out_6175643942212596402[11] = 0.0; + out_6175643942212596402[12] = 0.0; + out_6175643942212596402[13] = 0.0; + out_6175643942212596402[14] = 0.0; + out_6175643942212596402[15] = 0.0; + out_6175643942212596402[16] = 0.0; + out_6175643942212596402[17] = 0.0; + out_6175643942212596402[18] = 0.0; + out_6175643942212596402[19] = 0.0; + out_6175643942212596402[20] = 1.0; + out_6175643942212596402[21] = 0.0; + out_6175643942212596402[22] = 0.0; + out_6175643942212596402[23] = 0.0; + out_6175643942212596402[24] = 0.0; + out_6175643942212596402[25] = 0.0; + out_6175643942212596402[26] = 0.0; + out_6175643942212596402[27] = 0.0; + out_6175643942212596402[28] = 0.0; + out_6175643942212596402[29] = 0.0; + out_6175643942212596402[30] = 1.0; + out_6175643942212596402[31] = 0.0; + out_6175643942212596402[32] = 0.0; + out_6175643942212596402[33] = 0.0; + out_6175643942212596402[34] = 0.0; + out_6175643942212596402[35] = 0.0; + out_6175643942212596402[36] = 0.0; + out_6175643942212596402[37] = 0.0; + out_6175643942212596402[38] = 0.0; + out_6175643942212596402[39] = 0.0; + out_6175643942212596402[40] = 1.0; + out_6175643942212596402[41] = 0.0; + out_6175643942212596402[42] = 0.0; + out_6175643942212596402[43] = 0.0; + out_6175643942212596402[44] = 0.0; + out_6175643942212596402[45] = 0.0; + out_6175643942212596402[46] = 0.0; + out_6175643942212596402[47] = 0.0; + out_6175643942212596402[48] = 0.0; + out_6175643942212596402[49] = 0.0; + out_6175643942212596402[50] = 1.0; + out_6175643942212596402[51] = 0.0; + out_6175643942212596402[52] = 0.0; + out_6175643942212596402[53] = 0.0; + out_6175643942212596402[54] = 0.0; + out_6175643942212596402[55] = 0.0; + out_6175643942212596402[56] = 0.0; + out_6175643942212596402[57] = 0.0; + out_6175643942212596402[58] = 0.0; + out_6175643942212596402[59] = 0.0; + out_6175643942212596402[60] = 1.0; + out_6175643942212596402[61] = 0.0; + out_6175643942212596402[62] = 0.0; + out_6175643942212596402[63] = 0.0; + out_6175643942212596402[64] = 0.0; + out_6175643942212596402[65] = 0.0; + out_6175643942212596402[66] = 0.0; + out_6175643942212596402[67] = 0.0; + out_6175643942212596402[68] = 0.0; + out_6175643942212596402[69] = 0.0; + out_6175643942212596402[70] = 1.0; + out_6175643942212596402[71] = 0.0; + out_6175643942212596402[72] = 0.0; + out_6175643942212596402[73] = 0.0; + out_6175643942212596402[74] = 0.0; + out_6175643942212596402[75] = 0.0; + out_6175643942212596402[76] = 0.0; + out_6175643942212596402[77] = 0.0; + out_6175643942212596402[78] = 0.0; + out_6175643942212596402[79] = 0.0; + out_6175643942212596402[80] = 1.0; } -void f_fun(double *state, double dt, double *out_8159180570289535402) { - out_8159180570289535402[0] = state[0]; - out_8159180570289535402[1] = state[1]; - out_8159180570289535402[2] = state[2]; - out_8159180570289535402[3] = state[3]; - out_8159180570289535402[4] = state[4]; - out_8159180570289535402[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_8159180570289535402[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_8159180570289535402[7] = state[7]; - out_8159180570289535402[8] = state[8]; +void f_fun(double *state, double dt, double *out_2437881867604815540) { + out_2437881867604815540[0] = state[0]; + out_2437881867604815540[1] = state[1]; + out_2437881867604815540[2] = state[2]; + out_2437881867604815540[3] = state[3]; + out_2437881867604815540[4] = state[4]; + out_2437881867604815540[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_2437881867604815540[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_2437881867604815540[7] = state[7]; + out_2437881867604815540[8] = state[8]; } -void F_fun(double *state, double dt, double *out_8855850861951736689) { - out_8855850861951736689[0] = 1; - out_8855850861951736689[1] = 0; - out_8855850861951736689[2] = 0; - out_8855850861951736689[3] = 0; - out_8855850861951736689[4] = 0; - out_8855850861951736689[5] = 0; - out_8855850861951736689[6] = 0; - out_8855850861951736689[7] = 0; - out_8855850861951736689[8] = 0; - out_8855850861951736689[9] = 0; - out_8855850861951736689[10] = 1; - out_8855850861951736689[11] = 0; - out_8855850861951736689[12] = 0; - out_8855850861951736689[13] = 0; - out_8855850861951736689[14] = 0; - out_8855850861951736689[15] = 0; - out_8855850861951736689[16] = 0; - out_8855850861951736689[17] = 0; - out_8855850861951736689[18] = 0; - out_8855850861951736689[19] = 0; - out_8855850861951736689[20] = 1; - out_8855850861951736689[21] = 0; - out_8855850861951736689[22] = 0; - out_8855850861951736689[23] = 0; - out_8855850861951736689[24] = 0; - out_8855850861951736689[25] = 0; - out_8855850861951736689[26] = 0; - out_8855850861951736689[27] = 0; - out_8855850861951736689[28] = 0; - out_8855850861951736689[29] = 0; - out_8855850861951736689[30] = 1; - out_8855850861951736689[31] = 0; - out_8855850861951736689[32] = 0; - out_8855850861951736689[33] = 0; - out_8855850861951736689[34] = 0; - out_8855850861951736689[35] = 0; - out_8855850861951736689[36] = 0; - out_8855850861951736689[37] = 0; - out_8855850861951736689[38] = 0; - out_8855850861951736689[39] = 0; - out_8855850861951736689[40] = 1; - out_8855850861951736689[41] = 0; - out_8855850861951736689[42] = 0; - out_8855850861951736689[43] = 0; - out_8855850861951736689[44] = 0; - out_8855850861951736689[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_8855850861951736689[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_8855850861951736689[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8855850861951736689[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8855850861951736689[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_8855850861951736689[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_8855850861951736689[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_8855850861951736689[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_8855850861951736689[53] = -9.8100000000000005*dt; - out_8855850861951736689[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_8855850861951736689[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_8855850861951736689[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8855850861951736689[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8855850861951736689[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_8855850861951736689[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_8855850861951736689[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_8855850861951736689[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8855850861951736689[62] = 0; - out_8855850861951736689[63] = 0; - out_8855850861951736689[64] = 0; - out_8855850861951736689[65] = 0; - out_8855850861951736689[66] = 0; - out_8855850861951736689[67] = 0; - out_8855850861951736689[68] = 0; - out_8855850861951736689[69] = 0; - out_8855850861951736689[70] = 1; - out_8855850861951736689[71] = 0; - out_8855850861951736689[72] = 0; - out_8855850861951736689[73] = 0; - out_8855850861951736689[74] = 0; - out_8855850861951736689[75] = 0; - out_8855850861951736689[76] = 0; - out_8855850861951736689[77] = 0; - out_8855850861951736689[78] = 0; - out_8855850861951736689[79] = 0; - out_8855850861951736689[80] = 1; +void F_fun(double *state, double dt, double *out_3645925778664752399) { + out_3645925778664752399[0] = 1; + out_3645925778664752399[1] = 0; + out_3645925778664752399[2] = 0; + out_3645925778664752399[3] = 0; + out_3645925778664752399[4] = 0; + out_3645925778664752399[5] = 0; + out_3645925778664752399[6] = 0; + out_3645925778664752399[7] = 0; + out_3645925778664752399[8] = 0; + out_3645925778664752399[9] = 0; + out_3645925778664752399[10] = 1; + out_3645925778664752399[11] = 0; + out_3645925778664752399[12] = 0; + out_3645925778664752399[13] = 0; + out_3645925778664752399[14] = 0; + out_3645925778664752399[15] = 0; + out_3645925778664752399[16] = 0; + out_3645925778664752399[17] = 0; + out_3645925778664752399[18] = 0; + out_3645925778664752399[19] = 0; + out_3645925778664752399[20] = 1; + out_3645925778664752399[21] = 0; + out_3645925778664752399[22] = 0; + out_3645925778664752399[23] = 0; + out_3645925778664752399[24] = 0; + out_3645925778664752399[25] = 0; + out_3645925778664752399[26] = 0; + out_3645925778664752399[27] = 0; + out_3645925778664752399[28] = 0; + out_3645925778664752399[29] = 0; + out_3645925778664752399[30] = 1; + out_3645925778664752399[31] = 0; + out_3645925778664752399[32] = 0; + out_3645925778664752399[33] = 0; + out_3645925778664752399[34] = 0; + out_3645925778664752399[35] = 0; + out_3645925778664752399[36] = 0; + out_3645925778664752399[37] = 0; + out_3645925778664752399[38] = 0; + out_3645925778664752399[39] = 0; + out_3645925778664752399[40] = 1; + out_3645925778664752399[41] = 0; + out_3645925778664752399[42] = 0; + out_3645925778664752399[43] = 0; + out_3645925778664752399[44] = 0; + out_3645925778664752399[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_3645925778664752399[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_3645925778664752399[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3645925778664752399[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3645925778664752399[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_3645925778664752399[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_3645925778664752399[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_3645925778664752399[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_3645925778664752399[53] = -9.8100000000000005*dt; + out_3645925778664752399[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_3645925778664752399[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_3645925778664752399[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3645925778664752399[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3645925778664752399[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_3645925778664752399[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_3645925778664752399[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_3645925778664752399[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3645925778664752399[62] = 0; + out_3645925778664752399[63] = 0; + out_3645925778664752399[64] = 0; + out_3645925778664752399[65] = 0; + out_3645925778664752399[66] = 0; + out_3645925778664752399[67] = 0; + out_3645925778664752399[68] = 0; + out_3645925778664752399[69] = 0; + out_3645925778664752399[70] = 1; + out_3645925778664752399[71] = 0; + out_3645925778664752399[72] = 0; + out_3645925778664752399[73] = 0; + out_3645925778664752399[74] = 0; + out_3645925778664752399[75] = 0; + out_3645925778664752399[76] = 0; + out_3645925778664752399[77] = 0; + out_3645925778664752399[78] = 0; + out_3645925778664752399[79] = 0; + out_3645925778664752399[80] = 1; } -void h_25(double *state, double *unused, double *out_7057179500116168670) { - out_7057179500116168670[0] = state[6]; +void h_25(double *state, double *unused, double *out_1347552076965832871) { + out_1347552076965832871[0] = state[6]; } -void H_25(double *state, double *unused, double *out_8109981790338737725) { - out_8109981790338737725[0] = 0; - out_8109981790338737725[1] = 0; - out_8109981790338737725[2] = 0; - out_8109981790338737725[3] = 0; - out_8109981790338737725[4] = 0; - out_8109981790338737725[5] = 0; - out_8109981790338737725[6] = 1; - out_8109981790338737725[7] = 0; - out_8109981790338737725[8] = 0; +void H_25(double *state, double *unused, double *out_6035190491488835800) { + out_6035190491488835800[0] = 0; + out_6035190491488835800[1] = 0; + out_6035190491488835800[2] = 0; + out_6035190491488835800[3] = 0; + out_6035190491488835800[4] = 0; + out_6035190491488835800[5] = 0; + out_6035190491488835800[6] = 1; + out_6035190491488835800[7] = 0; + out_6035190491488835800[8] = 0; } -void h_24(double *state, double *unused, double *out_6854320894517861807) { - out_6854320894517861807[0] = state[4]; - out_6854320894517861807[1] = state[5]; +void h_24(double *state, double *unused, double *out_7176999251413506765) { + out_7176999251413506765[0] = state[4]; + out_7176999251413506765[1] = state[5]; } -void H_24(double *state, double *unused, double *out_8607899164193499822) { - out_8607899164193499822[0] = 0; - out_8607899164193499822[1] = 0; - out_8607899164193499822[2] = 0; - out_8607899164193499822[3] = 0; - out_8607899164193499822[4] = 1; - out_8607899164193499822[5] = 0; - out_8607899164193499822[6] = 0; - out_8607899164193499822[7] = 0; - out_8607899164193499822[8] = 0; - out_8607899164193499822[9] = 0; - out_8607899164193499822[10] = 0; - out_8607899164193499822[11] = 0; - out_8607899164193499822[12] = 0; - out_8607899164193499822[13] = 0; - out_8607899164193499822[14] = 1; - out_8607899164193499822[15] = 0; - out_8607899164193499822[16] = 0; - out_8607899164193499822[17] = 0; +void H_24(double *state, double *unused, double *out_6210520902369554390) { + out_6210520902369554390[0] = 0; + out_6210520902369554390[1] = 0; + out_6210520902369554390[2] = 0; + out_6210520902369554390[3] = 0; + out_6210520902369554390[4] = 1; + out_6210520902369554390[5] = 0; + out_6210520902369554390[6] = 0; + out_6210520902369554390[7] = 0; + out_6210520902369554390[8] = 0; + out_6210520902369554390[9] = 0; + out_6210520902369554390[10] = 0; + out_6210520902369554390[11] = 0; + out_6210520902369554390[12] = 0; + out_6210520902369554390[13] = 0; + out_6210520902369554390[14] = 1; + out_6210520902369554390[15] = 0; + out_6210520902369554390[16] = 0; + out_6210520902369554390[17] = 0; } -void h_30(double *state, double *unused, double *out_3929005809344494621) { - out_3929005809344494621[0] = state[4]; +void h_30(double *state, double *unused, double *out_2798183469594716038) { + out_2798183469594716038[0] = state[4]; } -void H_30(double *state, double *unused, double *out_1193291448847120970) { - out_1193291448847120970[0] = 0; - out_1193291448847120970[1] = 0; - out_1193291448847120970[2] = 0; - out_1193291448847120970[3] = 0; - out_1193291448847120970[4] = 1; - out_1193291448847120970[5] = 0; - out_1193291448847120970[6] = 0; - out_1193291448847120970[7] = 0; - out_1193291448847120970[8] = 0; +void H_30(double *state, double *unused, double *out_881499850002780955) { + out_881499850002780955[0] = 0; + out_881499850002780955[1] = 0; + out_881499850002780955[2] = 0; + out_881499850002780955[3] = 0; + out_881499850002780955[4] = 1; + out_881499850002780955[5] = 0; + out_881499850002780955[6] = 0; + out_881499850002780955[7] = 0; + out_881499850002780955[8] = 0; } -void h_26(double *state, double *unused, double *out_6807016989944022268) { - out_6807016989944022268[0] = state[7]; +void h_26(double *state, double *unused, double *out_7428563492211452550) { + out_7428563492211452550[0] = state[7]; } -void H_26(double *state, double *unused, double *out_6595258964496757667) { - out_6595258964496757667[0] = 0; - out_6595258964496757667[1] = 0; - out_6595258964496757667[2] = 0; - out_6595258964496757667[3] = 0; - out_6595258964496757667[4] = 0; - out_6595258964496757667[5] = 0; - out_6595258964496757667[6] = 0; - out_6595258964496757667[7] = 1; - out_6595258964496757667[8] = 0; +void H_26(double *state, double *unused, double *out_8670050263346659592) { + out_8670050263346659592[0] = 0; + out_8670050263346659592[1] = 0; + out_8670050263346659592[2] = 0; + out_8670050263346659592[3] = 0; + out_8670050263346659592[4] = 0; + out_8670050263346659592[5] = 0; + out_8670050263346659592[6] = 0; + out_8670050263346659592[7] = 1; + out_8670050263346659592[8] = 0; } -void h_27(double *state, double *unused, double *out_2055506232455323025) { - out_2055506232455323025[0] = state[3]; +void h_27(double *state, double *unused, double *out_3654121190695012774) { + out_3654121190695012774[0] = state[3]; } -void H_27(double *state, double *unused, double *out_3368054760647545881) { - out_3368054760647545881[0] = 0; - out_3368054760647545881[1] = 0; - out_3368054760647545881[2] = 0; - out_3368054760647545881[3] = 1; - out_3368054760647545881[4] = 0; - out_3368054760647545881[5] = 0; - out_3368054760647545881[6] = 0; - out_3368054760647545881[7] = 0; - out_3368054760647545881[8] = 0; +void H_27(double *state, double *unused, double *out_8339292750432500781) { + out_8339292750432500781[0] = 0; + out_8339292750432500781[1] = 0; + out_8339292750432500781[2] = 0; + out_8339292750432500781[3] = 1; + out_8339292750432500781[4] = 0; + out_8339292750432500781[5] = 0; + out_8339292750432500781[6] = 0; + out_8339292750432500781[7] = 0; + out_8339292750432500781[8] = 0; } -void h_29(double *state, double *unused, double *out_7581563669006346138) { - out_7581563669006346138[0] = state[1]; +void h_29(double *state, double *unused, double *out_2203489798066129607) { + out_2203489798066129607[0] = state[1]; } -void H_29(double *state, double *unused, double *out_683060104532728786) { - out_683060104532728786[0] = 0; - out_683060104532728786[1] = 1; - out_683060104532728786[2] = 0; - out_683060104532728786[3] = 0; - out_683060104532728786[4] = 0; - out_683060104532728786[5] = 0; - out_683060104532728786[6] = 0; - out_683060104532728786[7] = 0; - out_683060104532728786[8] = 0; +void H_29(double *state, double *unused, double *out_5654298094317683686) { + out_5654298094317683686[0] = 0; + out_5654298094317683686[1] = 1; + out_5654298094317683686[2] = 0; + out_5654298094317683686[3] = 0; + out_5654298094317683686[4] = 0; + out_5654298094317683686[5] = 0; + out_5654298094317683686[6] = 0; + out_5654298094317683686[7] = 0; + out_5654298094317683686[8] = 0; } -void h_28(double *state, double *unused, double *out_7985581021610001938) { - out_7985581021610001938[0] = state[0]; +void h_28(double *state, double *unused, double *out_3781022243733688039) { + out_3781022243733688039[0] = state[0]; } -void H_28(double *state, double *unused, double *out_5765459121602259360) { - out_5765459121602259360[0] = 1; - out_5765459121602259360[1] = 0; - out_5765459121602259360[2] = 0; - out_5765459121602259360[3] = 0; - out_5765459121602259360[4] = 0; - out_5765459121602259360[5] = 0; - out_5765459121602259360[6] = 0; - out_5765459121602259360[7] = 0; - out_5765459121602259360[8] = 0; +void H_28(double *state, double *unused, double *out_3690667822752357435) { + out_3690667822752357435[0] = 1; + out_3690667822752357435[1] = 0; + out_3690667822752357435[2] = 0; + out_3690667822752357435[3] = 0; + out_3690667822752357435[4] = 0; + out_3690667822752357435[5] = 0; + out_3690667822752357435[6] = 0; + out_3690667822752357435[7] = 0; + out_3690667822752357435[8] = 0; } -void h_31(double *state, double *unused, double *out_706085756704503771) { - out_706085756704503771[0] = state[8]; +void h_31(double *state, double *unused, double *out_5423283149384518065) { + out_5423283149384518065[0] = state[8]; } -void H_31(double *state, double *unused, double *out_8079335828461777297) { - out_8079335828461777297[0] = 0; - out_8079335828461777297[1] = 0; - out_8079335828461777297[2] = 0; - out_8079335828461777297[3] = 0; - out_8079335828461777297[4] = 0; - out_8079335828461777297[5] = 0; - out_8079335828461777297[6] = 0; - out_8079335828461777297[7] = 0; - out_8079335828461777297[8] = 1; +void H_31(double *state, double *unused, double *out_6004544529611875372) { + out_6004544529611875372[0] = 0; + out_6004544529611875372[1] = 0; + out_6004544529611875372[2] = 0; + out_6004544529611875372[3] = 0; + out_6004544529611875372[4] = 0; + out_6004544529611875372[5] = 0; + out_6004544529611875372[6] = 0; + out_6004544529611875372[7] = 0; + out_6004544529611875372[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_2057434223379957831) { - err_fun(nom_x, delta_x, out_2057434223379957831); +void car_err_fun(double *nom_x, double *delta_x, double *out_2765937094321424100) { + err_fun(nom_x, delta_x, out_2765937094321424100); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4924816816161575884) { - inv_err_fun(nom_x, true_x, out_4924816816161575884); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1382255689110899984) { + inv_err_fun(nom_x, true_x, out_1382255689110899984); } -void car_H_mod_fun(double *state, double *out_8615316232539034437) { - H_mod_fun(state, out_8615316232539034437); +void car_H_mod_fun(double *state, double *out_6175643942212596402) { + H_mod_fun(state, out_6175643942212596402); } -void car_f_fun(double *state, double dt, double *out_8159180570289535402) { - f_fun(state, dt, out_8159180570289535402); +void car_f_fun(double *state, double dt, double *out_2437881867604815540) { + f_fun(state, dt, out_2437881867604815540); } -void car_F_fun(double *state, double dt, double *out_8855850861951736689) { - F_fun(state, dt, out_8855850861951736689); +void car_F_fun(double *state, double dt, double *out_3645925778664752399) { + F_fun(state, dt, out_3645925778664752399); } -void car_h_25(double *state, double *unused, double *out_7057179500116168670) { - h_25(state, unused, out_7057179500116168670); +void car_h_25(double *state, double *unused, double *out_1347552076965832871) { + h_25(state, unused, out_1347552076965832871); } -void car_H_25(double *state, double *unused, double *out_8109981790338737725) { - H_25(state, unused, out_8109981790338737725); +void car_H_25(double *state, double *unused, double *out_6035190491488835800) { + H_25(state, unused, out_6035190491488835800); } -void car_h_24(double *state, double *unused, double *out_6854320894517861807) { - h_24(state, unused, out_6854320894517861807); +void car_h_24(double *state, double *unused, double *out_7176999251413506765) { + h_24(state, unused, out_7176999251413506765); } -void car_H_24(double *state, double *unused, double *out_8607899164193499822) { - H_24(state, unused, out_8607899164193499822); +void car_H_24(double *state, double *unused, double *out_6210520902369554390) { + H_24(state, unused, out_6210520902369554390); } -void car_h_30(double *state, double *unused, double *out_3929005809344494621) { - h_30(state, unused, out_3929005809344494621); +void car_h_30(double *state, double *unused, double *out_2798183469594716038) { + h_30(state, unused, out_2798183469594716038); } -void car_H_30(double *state, double *unused, double *out_1193291448847120970) { - H_30(state, unused, out_1193291448847120970); +void car_H_30(double *state, double *unused, double *out_881499850002780955) { + H_30(state, unused, out_881499850002780955); } -void car_h_26(double *state, double *unused, double *out_6807016989944022268) { - h_26(state, unused, out_6807016989944022268); +void car_h_26(double *state, double *unused, double *out_7428563492211452550) { + h_26(state, unused, out_7428563492211452550); } -void car_H_26(double *state, double *unused, double *out_6595258964496757667) { - H_26(state, unused, out_6595258964496757667); +void car_H_26(double *state, double *unused, double *out_8670050263346659592) { + H_26(state, unused, out_8670050263346659592); } -void car_h_27(double *state, double *unused, double *out_2055506232455323025) { - h_27(state, unused, out_2055506232455323025); +void car_h_27(double *state, double *unused, double *out_3654121190695012774) { + h_27(state, unused, out_3654121190695012774); } -void car_H_27(double *state, double *unused, double *out_3368054760647545881) { - H_27(state, unused, out_3368054760647545881); +void car_H_27(double *state, double *unused, double *out_8339292750432500781) { + H_27(state, unused, out_8339292750432500781); } -void car_h_29(double *state, double *unused, double *out_7581563669006346138) { - h_29(state, unused, out_7581563669006346138); +void car_h_29(double *state, double *unused, double *out_2203489798066129607) { + h_29(state, unused, out_2203489798066129607); } -void car_H_29(double *state, double *unused, double *out_683060104532728786) { - H_29(state, unused, out_683060104532728786); +void car_H_29(double *state, double *unused, double *out_5654298094317683686) { + H_29(state, unused, out_5654298094317683686); } -void car_h_28(double *state, double *unused, double *out_7985581021610001938) { - h_28(state, unused, out_7985581021610001938); +void car_h_28(double *state, double *unused, double *out_3781022243733688039) { + h_28(state, unused, out_3781022243733688039); } -void car_H_28(double *state, double *unused, double *out_5765459121602259360) { - H_28(state, unused, out_5765459121602259360); +void car_H_28(double *state, double *unused, double *out_3690667822752357435) { + H_28(state, unused, out_3690667822752357435); } -void car_h_31(double *state, double *unused, double *out_706085756704503771) { - h_31(state, unused, out_706085756704503771); +void car_h_31(double *state, double *unused, double *out_5423283149384518065) { + h_31(state, unused, out_5423283149384518065); } -void car_H_31(double *state, double *unused, double *out_8079335828461777297) { - H_31(state, unused, out_8079335828461777297); +void car_H_31(double *state, double *unused, double *out_6004544529611875372) { + H_31(state, unused, out_6004544529611875372); } 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 0c68b398d..627c90f74 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_2057434223379957831); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4924816816161575884); -void car_H_mod_fun(double *state, double *out_8615316232539034437); -void car_f_fun(double *state, double dt, double *out_8159180570289535402); -void car_F_fun(double *state, double dt, double *out_8855850861951736689); -void car_h_25(double *state, double *unused, double *out_7057179500116168670); -void car_H_25(double *state, double *unused, double *out_8109981790338737725); -void car_h_24(double *state, double *unused, double *out_6854320894517861807); -void car_H_24(double *state, double *unused, double *out_8607899164193499822); -void car_h_30(double *state, double *unused, double *out_3929005809344494621); -void car_H_30(double *state, double *unused, double *out_1193291448847120970); -void car_h_26(double *state, double *unused, double *out_6807016989944022268); -void car_H_26(double *state, double *unused, double *out_6595258964496757667); -void car_h_27(double *state, double *unused, double *out_2055506232455323025); -void car_H_27(double *state, double *unused, double *out_3368054760647545881); -void car_h_29(double *state, double *unused, double *out_7581563669006346138); -void car_H_29(double *state, double *unused, double *out_683060104532728786); -void car_h_28(double *state, double *unused, double *out_7985581021610001938); -void car_H_28(double *state, double *unused, double *out_5765459121602259360); -void car_h_31(double *state, double *unused, double *out_706085756704503771); -void car_H_31(double *state, double *unused, double *out_8079335828461777297); +void car_err_fun(double *nom_x, double *delta_x, double *out_2765937094321424100); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_1382255689110899984); +void car_H_mod_fun(double *state, double *out_6175643942212596402); +void car_f_fun(double *state, double dt, double *out_2437881867604815540); +void car_F_fun(double *state, double dt, double *out_3645925778664752399); +void car_h_25(double *state, double *unused, double *out_1347552076965832871); +void car_H_25(double *state, double *unused, double *out_6035190491488835800); +void car_h_24(double *state, double *unused, double *out_7176999251413506765); +void car_H_24(double *state, double *unused, double *out_6210520902369554390); +void car_h_30(double *state, double *unused, double *out_2798183469594716038); +void car_H_30(double *state, double *unused, double *out_881499850002780955); +void car_h_26(double *state, double *unused, double *out_7428563492211452550); +void car_H_26(double *state, double *unused, double *out_8670050263346659592); +void car_h_27(double *state, double *unused, double *out_3654121190695012774); +void car_H_27(double *state, double *unused, double *out_8339292750432500781); +void car_h_29(double *state, double *unused, double *out_2203489798066129607); +void car_H_29(double *state, double *unused, double *out_5654298094317683686); +void car_h_28(double *state, double *unused, double *out_3781022243733688039); +void car_H_28(double *state, double *unused, double *out_3690667822752357435); +void car_h_31(double *state, double *unused, double *out_5423283149384518065); +void car_H_31(double *state, double *unused, double *out_6004544529611875372); 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 c83cc8fd8..1366d16bb 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_2423125642091007723) { - out_2423125642091007723[0] = delta_x[0] + nom_x[0]; - out_2423125642091007723[1] = delta_x[1] + nom_x[1]; - out_2423125642091007723[2] = delta_x[2] + nom_x[2]; - out_2423125642091007723[3] = delta_x[3] + nom_x[3]; - out_2423125642091007723[4] = delta_x[4] + nom_x[4]; - out_2423125642091007723[5] = delta_x[5] + nom_x[5]; - out_2423125642091007723[6] = delta_x[6] + nom_x[6]; - out_2423125642091007723[7] = delta_x[7] + nom_x[7]; - out_2423125642091007723[8] = delta_x[8] + nom_x[8]; - out_2423125642091007723[9] = delta_x[9] + nom_x[9]; - out_2423125642091007723[10] = delta_x[10] + nom_x[10]; - out_2423125642091007723[11] = delta_x[11] + nom_x[11]; - out_2423125642091007723[12] = delta_x[12] + nom_x[12]; - out_2423125642091007723[13] = delta_x[13] + nom_x[13]; - out_2423125642091007723[14] = delta_x[14] + nom_x[14]; - out_2423125642091007723[15] = delta_x[15] + nom_x[15]; - out_2423125642091007723[16] = delta_x[16] + nom_x[16]; - out_2423125642091007723[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_2822233615381903085) { + out_2822233615381903085[0] = delta_x[0] + nom_x[0]; + out_2822233615381903085[1] = delta_x[1] + nom_x[1]; + out_2822233615381903085[2] = delta_x[2] + nom_x[2]; + out_2822233615381903085[3] = delta_x[3] + nom_x[3]; + out_2822233615381903085[4] = delta_x[4] + nom_x[4]; + out_2822233615381903085[5] = delta_x[5] + nom_x[5]; + out_2822233615381903085[6] = delta_x[6] + nom_x[6]; + out_2822233615381903085[7] = delta_x[7] + nom_x[7]; + out_2822233615381903085[8] = delta_x[8] + nom_x[8]; + out_2822233615381903085[9] = delta_x[9] + nom_x[9]; + out_2822233615381903085[10] = delta_x[10] + nom_x[10]; + out_2822233615381903085[11] = delta_x[11] + nom_x[11]; + out_2822233615381903085[12] = delta_x[12] + nom_x[12]; + out_2822233615381903085[13] = delta_x[13] + nom_x[13]; + out_2822233615381903085[14] = delta_x[14] + nom_x[14]; + out_2822233615381903085[15] = delta_x[15] + nom_x[15]; + out_2822233615381903085[16] = delta_x[16] + nom_x[16]; + out_2822233615381903085[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1501585610038659930) { - out_1501585610038659930[0] = -nom_x[0] + true_x[0]; - out_1501585610038659930[1] = -nom_x[1] + true_x[1]; - out_1501585610038659930[2] = -nom_x[2] + true_x[2]; - out_1501585610038659930[3] = -nom_x[3] + true_x[3]; - out_1501585610038659930[4] = -nom_x[4] + true_x[4]; - out_1501585610038659930[5] = -nom_x[5] + true_x[5]; - out_1501585610038659930[6] = -nom_x[6] + true_x[6]; - out_1501585610038659930[7] = -nom_x[7] + true_x[7]; - out_1501585610038659930[8] = -nom_x[8] + true_x[8]; - out_1501585610038659930[9] = -nom_x[9] + true_x[9]; - out_1501585610038659930[10] = -nom_x[10] + true_x[10]; - out_1501585610038659930[11] = -nom_x[11] + true_x[11]; - out_1501585610038659930[12] = -nom_x[12] + true_x[12]; - out_1501585610038659930[13] = -nom_x[13] + true_x[13]; - out_1501585610038659930[14] = -nom_x[14] + true_x[14]; - out_1501585610038659930[15] = -nom_x[15] + true_x[15]; - out_1501585610038659930[16] = -nom_x[16] + true_x[16]; - out_1501585610038659930[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3556797172859207138) { + out_3556797172859207138[0] = -nom_x[0] + true_x[0]; + out_3556797172859207138[1] = -nom_x[1] + true_x[1]; + out_3556797172859207138[2] = -nom_x[2] + true_x[2]; + out_3556797172859207138[3] = -nom_x[3] + true_x[3]; + out_3556797172859207138[4] = -nom_x[4] + true_x[4]; + out_3556797172859207138[5] = -nom_x[5] + true_x[5]; + out_3556797172859207138[6] = -nom_x[6] + true_x[6]; + out_3556797172859207138[7] = -nom_x[7] + true_x[7]; + out_3556797172859207138[8] = -nom_x[8] + true_x[8]; + out_3556797172859207138[9] = -nom_x[9] + true_x[9]; + out_3556797172859207138[10] = -nom_x[10] + true_x[10]; + out_3556797172859207138[11] = -nom_x[11] + true_x[11]; + out_3556797172859207138[12] = -nom_x[12] + true_x[12]; + out_3556797172859207138[13] = -nom_x[13] + true_x[13]; + out_3556797172859207138[14] = -nom_x[14] + true_x[14]; + out_3556797172859207138[15] = -nom_x[15] + true_x[15]; + out_3556797172859207138[16] = -nom_x[16] + true_x[16]; + out_3556797172859207138[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_313504174478215792) { - out_313504174478215792[0] = 1.0; - out_313504174478215792[1] = 0.0; - out_313504174478215792[2] = 0.0; - out_313504174478215792[3] = 0.0; - out_313504174478215792[4] = 0.0; - out_313504174478215792[5] = 0.0; - out_313504174478215792[6] = 0.0; - out_313504174478215792[7] = 0.0; - out_313504174478215792[8] = 0.0; - out_313504174478215792[9] = 0.0; - out_313504174478215792[10] = 0.0; - out_313504174478215792[11] = 0.0; - out_313504174478215792[12] = 0.0; - out_313504174478215792[13] = 0.0; - out_313504174478215792[14] = 0.0; - out_313504174478215792[15] = 0.0; - out_313504174478215792[16] = 0.0; - out_313504174478215792[17] = 0.0; - out_313504174478215792[18] = 0.0; - out_313504174478215792[19] = 1.0; - out_313504174478215792[20] = 0.0; - out_313504174478215792[21] = 0.0; - out_313504174478215792[22] = 0.0; - out_313504174478215792[23] = 0.0; - out_313504174478215792[24] = 0.0; - out_313504174478215792[25] = 0.0; - out_313504174478215792[26] = 0.0; - out_313504174478215792[27] = 0.0; - out_313504174478215792[28] = 0.0; - out_313504174478215792[29] = 0.0; - out_313504174478215792[30] = 0.0; - out_313504174478215792[31] = 0.0; - out_313504174478215792[32] = 0.0; - out_313504174478215792[33] = 0.0; - out_313504174478215792[34] = 0.0; - out_313504174478215792[35] = 0.0; - out_313504174478215792[36] = 0.0; - out_313504174478215792[37] = 0.0; - out_313504174478215792[38] = 1.0; - out_313504174478215792[39] = 0.0; - out_313504174478215792[40] = 0.0; - out_313504174478215792[41] = 0.0; - out_313504174478215792[42] = 0.0; - out_313504174478215792[43] = 0.0; - out_313504174478215792[44] = 0.0; - out_313504174478215792[45] = 0.0; - out_313504174478215792[46] = 0.0; - out_313504174478215792[47] = 0.0; - out_313504174478215792[48] = 0.0; - out_313504174478215792[49] = 0.0; - out_313504174478215792[50] = 0.0; - out_313504174478215792[51] = 0.0; - out_313504174478215792[52] = 0.0; - out_313504174478215792[53] = 0.0; - out_313504174478215792[54] = 0.0; - out_313504174478215792[55] = 0.0; - out_313504174478215792[56] = 0.0; - out_313504174478215792[57] = 1.0; - out_313504174478215792[58] = 0.0; - out_313504174478215792[59] = 0.0; - out_313504174478215792[60] = 0.0; - out_313504174478215792[61] = 0.0; - out_313504174478215792[62] = 0.0; - out_313504174478215792[63] = 0.0; - out_313504174478215792[64] = 0.0; - out_313504174478215792[65] = 0.0; - out_313504174478215792[66] = 0.0; - out_313504174478215792[67] = 0.0; - out_313504174478215792[68] = 0.0; - out_313504174478215792[69] = 0.0; - out_313504174478215792[70] = 0.0; - out_313504174478215792[71] = 0.0; - out_313504174478215792[72] = 0.0; - out_313504174478215792[73] = 0.0; - out_313504174478215792[74] = 0.0; - out_313504174478215792[75] = 0.0; - out_313504174478215792[76] = 1.0; - out_313504174478215792[77] = 0.0; - out_313504174478215792[78] = 0.0; - out_313504174478215792[79] = 0.0; - out_313504174478215792[80] = 0.0; - out_313504174478215792[81] = 0.0; - out_313504174478215792[82] = 0.0; - out_313504174478215792[83] = 0.0; - out_313504174478215792[84] = 0.0; - out_313504174478215792[85] = 0.0; - out_313504174478215792[86] = 0.0; - out_313504174478215792[87] = 0.0; - out_313504174478215792[88] = 0.0; - out_313504174478215792[89] = 0.0; - out_313504174478215792[90] = 0.0; - out_313504174478215792[91] = 0.0; - out_313504174478215792[92] = 0.0; - out_313504174478215792[93] = 0.0; - out_313504174478215792[94] = 0.0; - out_313504174478215792[95] = 1.0; - out_313504174478215792[96] = 0.0; - out_313504174478215792[97] = 0.0; - out_313504174478215792[98] = 0.0; - out_313504174478215792[99] = 0.0; - out_313504174478215792[100] = 0.0; - out_313504174478215792[101] = 0.0; - out_313504174478215792[102] = 0.0; - out_313504174478215792[103] = 0.0; - out_313504174478215792[104] = 0.0; - out_313504174478215792[105] = 0.0; - out_313504174478215792[106] = 0.0; - out_313504174478215792[107] = 0.0; - out_313504174478215792[108] = 0.0; - out_313504174478215792[109] = 0.0; - out_313504174478215792[110] = 0.0; - out_313504174478215792[111] = 0.0; - out_313504174478215792[112] = 0.0; - out_313504174478215792[113] = 0.0; - out_313504174478215792[114] = 1.0; - out_313504174478215792[115] = 0.0; - out_313504174478215792[116] = 0.0; - out_313504174478215792[117] = 0.0; - out_313504174478215792[118] = 0.0; - out_313504174478215792[119] = 0.0; - out_313504174478215792[120] = 0.0; - out_313504174478215792[121] = 0.0; - out_313504174478215792[122] = 0.0; - out_313504174478215792[123] = 0.0; - out_313504174478215792[124] = 0.0; - out_313504174478215792[125] = 0.0; - out_313504174478215792[126] = 0.0; - out_313504174478215792[127] = 0.0; - out_313504174478215792[128] = 0.0; - out_313504174478215792[129] = 0.0; - out_313504174478215792[130] = 0.0; - out_313504174478215792[131] = 0.0; - out_313504174478215792[132] = 0.0; - out_313504174478215792[133] = 1.0; - out_313504174478215792[134] = 0.0; - out_313504174478215792[135] = 0.0; - out_313504174478215792[136] = 0.0; - out_313504174478215792[137] = 0.0; - out_313504174478215792[138] = 0.0; - out_313504174478215792[139] = 0.0; - out_313504174478215792[140] = 0.0; - out_313504174478215792[141] = 0.0; - out_313504174478215792[142] = 0.0; - out_313504174478215792[143] = 0.0; - out_313504174478215792[144] = 0.0; - out_313504174478215792[145] = 0.0; - out_313504174478215792[146] = 0.0; - out_313504174478215792[147] = 0.0; - out_313504174478215792[148] = 0.0; - out_313504174478215792[149] = 0.0; - out_313504174478215792[150] = 0.0; - out_313504174478215792[151] = 0.0; - out_313504174478215792[152] = 1.0; - out_313504174478215792[153] = 0.0; - out_313504174478215792[154] = 0.0; - out_313504174478215792[155] = 0.0; - out_313504174478215792[156] = 0.0; - out_313504174478215792[157] = 0.0; - out_313504174478215792[158] = 0.0; - out_313504174478215792[159] = 0.0; - out_313504174478215792[160] = 0.0; - out_313504174478215792[161] = 0.0; - out_313504174478215792[162] = 0.0; - out_313504174478215792[163] = 0.0; - out_313504174478215792[164] = 0.0; - out_313504174478215792[165] = 0.0; - out_313504174478215792[166] = 0.0; - out_313504174478215792[167] = 0.0; - out_313504174478215792[168] = 0.0; - out_313504174478215792[169] = 0.0; - out_313504174478215792[170] = 0.0; - out_313504174478215792[171] = 1.0; - out_313504174478215792[172] = 0.0; - out_313504174478215792[173] = 0.0; - out_313504174478215792[174] = 0.0; - out_313504174478215792[175] = 0.0; - out_313504174478215792[176] = 0.0; - out_313504174478215792[177] = 0.0; - out_313504174478215792[178] = 0.0; - out_313504174478215792[179] = 0.0; - out_313504174478215792[180] = 0.0; - out_313504174478215792[181] = 0.0; - out_313504174478215792[182] = 0.0; - out_313504174478215792[183] = 0.0; - out_313504174478215792[184] = 0.0; - out_313504174478215792[185] = 0.0; - out_313504174478215792[186] = 0.0; - out_313504174478215792[187] = 0.0; - out_313504174478215792[188] = 0.0; - out_313504174478215792[189] = 0.0; - out_313504174478215792[190] = 1.0; - out_313504174478215792[191] = 0.0; - out_313504174478215792[192] = 0.0; - out_313504174478215792[193] = 0.0; - out_313504174478215792[194] = 0.0; - out_313504174478215792[195] = 0.0; - out_313504174478215792[196] = 0.0; - out_313504174478215792[197] = 0.0; - out_313504174478215792[198] = 0.0; - out_313504174478215792[199] = 0.0; - out_313504174478215792[200] = 0.0; - out_313504174478215792[201] = 0.0; - out_313504174478215792[202] = 0.0; - out_313504174478215792[203] = 0.0; - out_313504174478215792[204] = 0.0; - out_313504174478215792[205] = 0.0; - out_313504174478215792[206] = 0.0; - out_313504174478215792[207] = 0.0; - out_313504174478215792[208] = 0.0; - out_313504174478215792[209] = 1.0; - out_313504174478215792[210] = 0.0; - out_313504174478215792[211] = 0.0; - out_313504174478215792[212] = 0.0; - out_313504174478215792[213] = 0.0; - out_313504174478215792[214] = 0.0; - out_313504174478215792[215] = 0.0; - out_313504174478215792[216] = 0.0; - out_313504174478215792[217] = 0.0; - out_313504174478215792[218] = 0.0; - out_313504174478215792[219] = 0.0; - out_313504174478215792[220] = 0.0; - out_313504174478215792[221] = 0.0; - out_313504174478215792[222] = 0.0; - out_313504174478215792[223] = 0.0; - out_313504174478215792[224] = 0.0; - out_313504174478215792[225] = 0.0; - out_313504174478215792[226] = 0.0; - out_313504174478215792[227] = 0.0; - out_313504174478215792[228] = 1.0; - out_313504174478215792[229] = 0.0; - out_313504174478215792[230] = 0.0; - out_313504174478215792[231] = 0.0; - out_313504174478215792[232] = 0.0; - out_313504174478215792[233] = 0.0; - out_313504174478215792[234] = 0.0; - out_313504174478215792[235] = 0.0; - out_313504174478215792[236] = 0.0; - out_313504174478215792[237] = 0.0; - out_313504174478215792[238] = 0.0; - out_313504174478215792[239] = 0.0; - out_313504174478215792[240] = 0.0; - out_313504174478215792[241] = 0.0; - out_313504174478215792[242] = 0.0; - out_313504174478215792[243] = 0.0; - out_313504174478215792[244] = 0.0; - out_313504174478215792[245] = 0.0; - out_313504174478215792[246] = 0.0; - out_313504174478215792[247] = 1.0; - out_313504174478215792[248] = 0.0; - out_313504174478215792[249] = 0.0; - out_313504174478215792[250] = 0.0; - out_313504174478215792[251] = 0.0; - out_313504174478215792[252] = 0.0; - out_313504174478215792[253] = 0.0; - out_313504174478215792[254] = 0.0; - out_313504174478215792[255] = 0.0; - out_313504174478215792[256] = 0.0; - out_313504174478215792[257] = 0.0; - out_313504174478215792[258] = 0.0; - out_313504174478215792[259] = 0.0; - out_313504174478215792[260] = 0.0; - out_313504174478215792[261] = 0.0; - out_313504174478215792[262] = 0.0; - out_313504174478215792[263] = 0.0; - out_313504174478215792[264] = 0.0; - out_313504174478215792[265] = 0.0; - out_313504174478215792[266] = 1.0; - out_313504174478215792[267] = 0.0; - out_313504174478215792[268] = 0.0; - out_313504174478215792[269] = 0.0; - out_313504174478215792[270] = 0.0; - out_313504174478215792[271] = 0.0; - out_313504174478215792[272] = 0.0; - out_313504174478215792[273] = 0.0; - out_313504174478215792[274] = 0.0; - out_313504174478215792[275] = 0.0; - out_313504174478215792[276] = 0.0; - out_313504174478215792[277] = 0.0; - out_313504174478215792[278] = 0.0; - out_313504174478215792[279] = 0.0; - out_313504174478215792[280] = 0.0; - out_313504174478215792[281] = 0.0; - out_313504174478215792[282] = 0.0; - out_313504174478215792[283] = 0.0; - out_313504174478215792[284] = 0.0; - out_313504174478215792[285] = 1.0; - out_313504174478215792[286] = 0.0; - out_313504174478215792[287] = 0.0; - out_313504174478215792[288] = 0.0; - out_313504174478215792[289] = 0.0; - out_313504174478215792[290] = 0.0; - out_313504174478215792[291] = 0.0; - out_313504174478215792[292] = 0.0; - out_313504174478215792[293] = 0.0; - out_313504174478215792[294] = 0.0; - out_313504174478215792[295] = 0.0; - out_313504174478215792[296] = 0.0; - out_313504174478215792[297] = 0.0; - out_313504174478215792[298] = 0.0; - out_313504174478215792[299] = 0.0; - out_313504174478215792[300] = 0.0; - out_313504174478215792[301] = 0.0; - out_313504174478215792[302] = 0.0; - out_313504174478215792[303] = 0.0; - out_313504174478215792[304] = 1.0; - out_313504174478215792[305] = 0.0; - out_313504174478215792[306] = 0.0; - out_313504174478215792[307] = 0.0; - out_313504174478215792[308] = 0.0; - out_313504174478215792[309] = 0.0; - out_313504174478215792[310] = 0.0; - out_313504174478215792[311] = 0.0; - out_313504174478215792[312] = 0.0; - out_313504174478215792[313] = 0.0; - out_313504174478215792[314] = 0.0; - out_313504174478215792[315] = 0.0; - out_313504174478215792[316] = 0.0; - out_313504174478215792[317] = 0.0; - out_313504174478215792[318] = 0.0; - out_313504174478215792[319] = 0.0; - out_313504174478215792[320] = 0.0; - out_313504174478215792[321] = 0.0; - out_313504174478215792[322] = 0.0; - out_313504174478215792[323] = 1.0; +void H_mod_fun(double *state, double *out_4158952900173024917) { + out_4158952900173024917[0] = 1.0; + out_4158952900173024917[1] = 0.0; + out_4158952900173024917[2] = 0.0; + out_4158952900173024917[3] = 0.0; + out_4158952900173024917[4] = 0.0; + out_4158952900173024917[5] = 0.0; + out_4158952900173024917[6] = 0.0; + out_4158952900173024917[7] = 0.0; + out_4158952900173024917[8] = 0.0; + out_4158952900173024917[9] = 0.0; + out_4158952900173024917[10] = 0.0; + out_4158952900173024917[11] = 0.0; + out_4158952900173024917[12] = 0.0; + out_4158952900173024917[13] = 0.0; + out_4158952900173024917[14] = 0.0; + out_4158952900173024917[15] = 0.0; + out_4158952900173024917[16] = 0.0; + out_4158952900173024917[17] = 0.0; + out_4158952900173024917[18] = 0.0; + out_4158952900173024917[19] = 1.0; + out_4158952900173024917[20] = 0.0; + out_4158952900173024917[21] = 0.0; + out_4158952900173024917[22] = 0.0; + out_4158952900173024917[23] = 0.0; + out_4158952900173024917[24] = 0.0; + out_4158952900173024917[25] = 0.0; + out_4158952900173024917[26] = 0.0; + out_4158952900173024917[27] = 0.0; + out_4158952900173024917[28] = 0.0; + out_4158952900173024917[29] = 0.0; + out_4158952900173024917[30] = 0.0; + out_4158952900173024917[31] = 0.0; + out_4158952900173024917[32] = 0.0; + out_4158952900173024917[33] = 0.0; + out_4158952900173024917[34] = 0.0; + out_4158952900173024917[35] = 0.0; + out_4158952900173024917[36] = 0.0; + out_4158952900173024917[37] = 0.0; + out_4158952900173024917[38] = 1.0; + out_4158952900173024917[39] = 0.0; + out_4158952900173024917[40] = 0.0; + out_4158952900173024917[41] = 0.0; + out_4158952900173024917[42] = 0.0; + out_4158952900173024917[43] = 0.0; + out_4158952900173024917[44] = 0.0; + out_4158952900173024917[45] = 0.0; + out_4158952900173024917[46] = 0.0; + out_4158952900173024917[47] = 0.0; + out_4158952900173024917[48] = 0.0; + out_4158952900173024917[49] = 0.0; + out_4158952900173024917[50] = 0.0; + out_4158952900173024917[51] = 0.0; + out_4158952900173024917[52] = 0.0; + out_4158952900173024917[53] = 0.0; + out_4158952900173024917[54] = 0.0; + out_4158952900173024917[55] = 0.0; + out_4158952900173024917[56] = 0.0; + out_4158952900173024917[57] = 1.0; + out_4158952900173024917[58] = 0.0; + out_4158952900173024917[59] = 0.0; + out_4158952900173024917[60] = 0.0; + out_4158952900173024917[61] = 0.0; + out_4158952900173024917[62] = 0.0; + out_4158952900173024917[63] = 0.0; + out_4158952900173024917[64] = 0.0; + out_4158952900173024917[65] = 0.0; + out_4158952900173024917[66] = 0.0; + out_4158952900173024917[67] = 0.0; + out_4158952900173024917[68] = 0.0; + out_4158952900173024917[69] = 0.0; + out_4158952900173024917[70] = 0.0; + out_4158952900173024917[71] = 0.0; + out_4158952900173024917[72] = 0.0; + out_4158952900173024917[73] = 0.0; + out_4158952900173024917[74] = 0.0; + out_4158952900173024917[75] = 0.0; + out_4158952900173024917[76] = 1.0; + out_4158952900173024917[77] = 0.0; + out_4158952900173024917[78] = 0.0; + out_4158952900173024917[79] = 0.0; + out_4158952900173024917[80] = 0.0; + out_4158952900173024917[81] = 0.0; + out_4158952900173024917[82] = 0.0; + out_4158952900173024917[83] = 0.0; + out_4158952900173024917[84] = 0.0; + out_4158952900173024917[85] = 0.0; + out_4158952900173024917[86] = 0.0; + out_4158952900173024917[87] = 0.0; + out_4158952900173024917[88] = 0.0; + out_4158952900173024917[89] = 0.0; + out_4158952900173024917[90] = 0.0; + out_4158952900173024917[91] = 0.0; + out_4158952900173024917[92] = 0.0; + out_4158952900173024917[93] = 0.0; + out_4158952900173024917[94] = 0.0; + out_4158952900173024917[95] = 1.0; + out_4158952900173024917[96] = 0.0; + out_4158952900173024917[97] = 0.0; + out_4158952900173024917[98] = 0.0; + out_4158952900173024917[99] = 0.0; + out_4158952900173024917[100] = 0.0; + out_4158952900173024917[101] = 0.0; + out_4158952900173024917[102] = 0.0; + out_4158952900173024917[103] = 0.0; + out_4158952900173024917[104] = 0.0; + out_4158952900173024917[105] = 0.0; + out_4158952900173024917[106] = 0.0; + out_4158952900173024917[107] = 0.0; + out_4158952900173024917[108] = 0.0; + out_4158952900173024917[109] = 0.0; + out_4158952900173024917[110] = 0.0; + out_4158952900173024917[111] = 0.0; + out_4158952900173024917[112] = 0.0; + out_4158952900173024917[113] = 0.0; + out_4158952900173024917[114] = 1.0; + out_4158952900173024917[115] = 0.0; + out_4158952900173024917[116] = 0.0; + out_4158952900173024917[117] = 0.0; + out_4158952900173024917[118] = 0.0; + out_4158952900173024917[119] = 0.0; + out_4158952900173024917[120] = 0.0; + out_4158952900173024917[121] = 0.0; + out_4158952900173024917[122] = 0.0; + out_4158952900173024917[123] = 0.0; + out_4158952900173024917[124] = 0.0; + out_4158952900173024917[125] = 0.0; + out_4158952900173024917[126] = 0.0; + out_4158952900173024917[127] = 0.0; + out_4158952900173024917[128] = 0.0; + out_4158952900173024917[129] = 0.0; + out_4158952900173024917[130] = 0.0; + out_4158952900173024917[131] = 0.0; + out_4158952900173024917[132] = 0.0; + out_4158952900173024917[133] = 1.0; + out_4158952900173024917[134] = 0.0; + out_4158952900173024917[135] = 0.0; + out_4158952900173024917[136] = 0.0; + out_4158952900173024917[137] = 0.0; + out_4158952900173024917[138] = 0.0; + out_4158952900173024917[139] = 0.0; + out_4158952900173024917[140] = 0.0; + out_4158952900173024917[141] = 0.0; + out_4158952900173024917[142] = 0.0; + out_4158952900173024917[143] = 0.0; + out_4158952900173024917[144] = 0.0; + out_4158952900173024917[145] = 0.0; + out_4158952900173024917[146] = 0.0; + out_4158952900173024917[147] = 0.0; + out_4158952900173024917[148] = 0.0; + out_4158952900173024917[149] = 0.0; + out_4158952900173024917[150] = 0.0; + out_4158952900173024917[151] = 0.0; + out_4158952900173024917[152] = 1.0; + out_4158952900173024917[153] = 0.0; + out_4158952900173024917[154] = 0.0; + out_4158952900173024917[155] = 0.0; + out_4158952900173024917[156] = 0.0; + out_4158952900173024917[157] = 0.0; + out_4158952900173024917[158] = 0.0; + out_4158952900173024917[159] = 0.0; + out_4158952900173024917[160] = 0.0; + out_4158952900173024917[161] = 0.0; + out_4158952900173024917[162] = 0.0; + out_4158952900173024917[163] = 0.0; + out_4158952900173024917[164] = 0.0; + out_4158952900173024917[165] = 0.0; + out_4158952900173024917[166] = 0.0; + out_4158952900173024917[167] = 0.0; + out_4158952900173024917[168] = 0.0; + out_4158952900173024917[169] = 0.0; + out_4158952900173024917[170] = 0.0; + out_4158952900173024917[171] = 1.0; + out_4158952900173024917[172] = 0.0; + out_4158952900173024917[173] = 0.0; + out_4158952900173024917[174] = 0.0; + out_4158952900173024917[175] = 0.0; + out_4158952900173024917[176] = 0.0; + out_4158952900173024917[177] = 0.0; + out_4158952900173024917[178] = 0.0; + out_4158952900173024917[179] = 0.0; + out_4158952900173024917[180] = 0.0; + out_4158952900173024917[181] = 0.0; + out_4158952900173024917[182] = 0.0; + out_4158952900173024917[183] = 0.0; + out_4158952900173024917[184] = 0.0; + out_4158952900173024917[185] = 0.0; + out_4158952900173024917[186] = 0.0; + out_4158952900173024917[187] = 0.0; + out_4158952900173024917[188] = 0.0; + out_4158952900173024917[189] = 0.0; + out_4158952900173024917[190] = 1.0; + out_4158952900173024917[191] = 0.0; + out_4158952900173024917[192] = 0.0; + out_4158952900173024917[193] = 0.0; + out_4158952900173024917[194] = 0.0; + out_4158952900173024917[195] = 0.0; + out_4158952900173024917[196] = 0.0; + out_4158952900173024917[197] = 0.0; + out_4158952900173024917[198] = 0.0; + out_4158952900173024917[199] = 0.0; + out_4158952900173024917[200] = 0.0; + out_4158952900173024917[201] = 0.0; + out_4158952900173024917[202] = 0.0; + out_4158952900173024917[203] = 0.0; + out_4158952900173024917[204] = 0.0; + out_4158952900173024917[205] = 0.0; + out_4158952900173024917[206] = 0.0; + out_4158952900173024917[207] = 0.0; + out_4158952900173024917[208] = 0.0; + out_4158952900173024917[209] = 1.0; + out_4158952900173024917[210] = 0.0; + out_4158952900173024917[211] = 0.0; + out_4158952900173024917[212] = 0.0; + out_4158952900173024917[213] = 0.0; + out_4158952900173024917[214] = 0.0; + out_4158952900173024917[215] = 0.0; + out_4158952900173024917[216] = 0.0; + out_4158952900173024917[217] = 0.0; + out_4158952900173024917[218] = 0.0; + out_4158952900173024917[219] = 0.0; + out_4158952900173024917[220] = 0.0; + out_4158952900173024917[221] = 0.0; + out_4158952900173024917[222] = 0.0; + out_4158952900173024917[223] = 0.0; + out_4158952900173024917[224] = 0.0; + out_4158952900173024917[225] = 0.0; + out_4158952900173024917[226] = 0.0; + out_4158952900173024917[227] = 0.0; + out_4158952900173024917[228] = 1.0; + out_4158952900173024917[229] = 0.0; + out_4158952900173024917[230] = 0.0; + out_4158952900173024917[231] = 0.0; + out_4158952900173024917[232] = 0.0; + out_4158952900173024917[233] = 0.0; + out_4158952900173024917[234] = 0.0; + out_4158952900173024917[235] = 0.0; + out_4158952900173024917[236] = 0.0; + out_4158952900173024917[237] = 0.0; + out_4158952900173024917[238] = 0.0; + out_4158952900173024917[239] = 0.0; + out_4158952900173024917[240] = 0.0; + out_4158952900173024917[241] = 0.0; + out_4158952900173024917[242] = 0.0; + out_4158952900173024917[243] = 0.0; + out_4158952900173024917[244] = 0.0; + out_4158952900173024917[245] = 0.0; + out_4158952900173024917[246] = 0.0; + out_4158952900173024917[247] = 1.0; + out_4158952900173024917[248] = 0.0; + out_4158952900173024917[249] = 0.0; + out_4158952900173024917[250] = 0.0; + out_4158952900173024917[251] = 0.0; + out_4158952900173024917[252] = 0.0; + out_4158952900173024917[253] = 0.0; + out_4158952900173024917[254] = 0.0; + out_4158952900173024917[255] = 0.0; + out_4158952900173024917[256] = 0.0; + out_4158952900173024917[257] = 0.0; + out_4158952900173024917[258] = 0.0; + out_4158952900173024917[259] = 0.0; + out_4158952900173024917[260] = 0.0; + out_4158952900173024917[261] = 0.0; + out_4158952900173024917[262] = 0.0; + out_4158952900173024917[263] = 0.0; + out_4158952900173024917[264] = 0.0; + out_4158952900173024917[265] = 0.0; + out_4158952900173024917[266] = 1.0; + out_4158952900173024917[267] = 0.0; + out_4158952900173024917[268] = 0.0; + out_4158952900173024917[269] = 0.0; + out_4158952900173024917[270] = 0.0; + out_4158952900173024917[271] = 0.0; + out_4158952900173024917[272] = 0.0; + out_4158952900173024917[273] = 0.0; + out_4158952900173024917[274] = 0.0; + out_4158952900173024917[275] = 0.0; + out_4158952900173024917[276] = 0.0; + out_4158952900173024917[277] = 0.0; + out_4158952900173024917[278] = 0.0; + out_4158952900173024917[279] = 0.0; + out_4158952900173024917[280] = 0.0; + out_4158952900173024917[281] = 0.0; + out_4158952900173024917[282] = 0.0; + out_4158952900173024917[283] = 0.0; + out_4158952900173024917[284] = 0.0; + out_4158952900173024917[285] = 1.0; + out_4158952900173024917[286] = 0.0; + out_4158952900173024917[287] = 0.0; + out_4158952900173024917[288] = 0.0; + out_4158952900173024917[289] = 0.0; + out_4158952900173024917[290] = 0.0; + out_4158952900173024917[291] = 0.0; + out_4158952900173024917[292] = 0.0; + out_4158952900173024917[293] = 0.0; + out_4158952900173024917[294] = 0.0; + out_4158952900173024917[295] = 0.0; + out_4158952900173024917[296] = 0.0; + out_4158952900173024917[297] = 0.0; + out_4158952900173024917[298] = 0.0; + out_4158952900173024917[299] = 0.0; + out_4158952900173024917[300] = 0.0; + out_4158952900173024917[301] = 0.0; + out_4158952900173024917[302] = 0.0; + out_4158952900173024917[303] = 0.0; + out_4158952900173024917[304] = 1.0; + out_4158952900173024917[305] = 0.0; + out_4158952900173024917[306] = 0.0; + out_4158952900173024917[307] = 0.0; + out_4158952900173024917[308] = 0.0; + out_4158952900173024917[309] = 0.0; + out_4158952900173024917[310] = 0.0; + out_4158952900173024917[311] = 0.0; + out_4158952900173024917[312] = 0.0; + out_4158952900173024917[313] = 0.0; + out_4158952900173024917[314] = 0.0; + out_4158952900173024917[315] = 0.0; + out_4158952900173024917[316] = 0.0; + out_4158952900173024917[317] = 0.0; + out_4158952900173024917[318] = 0.0; + out_4158952900173024917[319] = 0.0; + out_4158952900173024917[320] = 0.0; + out_4158952900173024917[321] = 0.0; + out_4158952900173024917[322] = 0.0; + out_4158952900173024917[323] = 1.0; } -void f_fun(double *state, double dt, double *out_8978916703249748052) { - out_8978916703249748052[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_8978916703249748052[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_8978916703249748052[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_8978916703249748052[3] = dt*state[12] + state[3]; - out_8978916703249748052[4] = dt*state[13] + state[4]; - out_8978916703249748052[5] = dt*state[14] + state[5]; - out_8978916703249748052[6] = state[6]; - out_8978916703249748052[7] = state[7]; - out_8978916703249748052[8] = state[8]; - out_8978916703249748052[9] = state[9]; - out_8978916703249748052[10] = state[10]; - out_8978916703249748052[11] = state[11]; - out_8978916703249748052[12] = state[12]; - out_8978916703249748052[13] = state[13]; - out_8978916703249748052[14] = state[14]; - out_8978916703249748052[15] = state[15]; - out_8978916703249748052[16] = state[16]; - out_8978916703249748052[17] = state[17]; +void f_fun(double *state, double dt, double *out_4480310224533440205) { + out_4480310224533440205[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_4480310224533440205[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_4480310224533440205[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_4480310224533440205[3] = dt*state[12] + state[3]; + out_4480310224533440205[4] = dt*state[13] + state[4]; + out_4480310224533440205[5] = dt*state[14] + state[5]; + out_4480310224533440205[6] = state[6]; + out_4480310224533440205[7] = state[7]; + out_4480310224533440205[8] = state[8]; + out_4480310224533440205[9] = state[9]; + out_4480310224533440205[10] = state[10]; + out_4480310224533440205[11] = state[11]; + out_4480310224533440205[12] = state[12]; + out_4480310224533440205[13] = state[13]; + out_4480310224533440205[14] = state[14]; + out_4480310224533440205[15] = state[15]; + out_4480310224533440205[16] = state[16]; + out_4480310224533440205[17] = state[17]; } -void F_fun(double *state, double dt, double *out_3060487946531646253) { - out_3060487946531646253[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_3060487946531646253[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_3060487946531646253[2] = 0; - out_3060487946531646253[3] = 0; - out_3060487946531646253[4] = 0; - out_3060487946531646253[5] = 0; - out_3060487946531646253[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_3060487946531646253[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_3060487946531646253[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_3060487946531646253[9] = 0; - out_3060487946531646253[10] = 0; - out_3060487946531646253[11] = 0; - out_3060487946531646253[12] = 0; - out_3060487946531646253[13] = 0; - out_3060487946531646253[14] = 0; - out_3060487946531646253[15] = 0; - out_3060487946531646253[16] = 0; - out_3060487946531646253[17] = 0; - out_3060487946531646253[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_3060487946531646253[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_3060487946531646253[20] = 0; - out_3060487946531646253[21] = 0; - out_3060487946531646253[22] = 0; - out_3060487946531646253[23] = 0; - out_3060487946531646253[24] = 0; - out_3060487946531646253[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_3060487946531646253[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_3060487946531646253[27] = 0; - out_3060487946531646253[28] = 0; - out_3060487946531646253[29] = 0; - out_3060487946531646253[30] = 0; - out_3060487946531646253[31] = 0; - out_3060487946531646253[32] = 0; - out_3060487946531646253[33] = 0; - out_3060487946531646253[34] = 0; - out_3060487946531646253[35] = 0; - out_3060487946531646253[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_3060487946531646253[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_3060487946531646253[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_3060487946531646253[39] = 0; - out_3060487946531646253[40] = 0; - out_3060487946531646253[41] = 0; - out_3060487946531646253[42] = 0; - out_3060487946531646253[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_3060487946531646253[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_3060487946531646253[45] = 0; - out_3060487946531646253[46] = 0; - out_3060487946531646253[47] = 0; - out_3060487946531646253[48] = 0; - out_3060487946531646253[49] = 0; - out_3060487946531646253[50] = 0; - out_3060487946531646253[51] = 0; - out_3060487946531646253[52] = 0; - out_3060487946531646253[53] = 0; - out_3060487946531646253[54] = 0; - out_3060487946531646253[55] = 0; - out_3060487946531646253[56] = 0; - out_3060487946531646253[57] = 1; - out_3060487946531646253[58] = 0; - out_3060487946531646253[59] = 0; - out_3060487946531646253[60] = 0; - out_3060487946531646253[61] = 0; - out_3060487946531646253[62] = 0; - out_3060487946531646253[63] = 0; - out_3060487946531646253[64] = 0; - out_3060487946531646253[65] = 0; - out_3060487946531646253[66] = dt; - out_3060487946531646253[67] = 0; - out_3060487946531646253[68] = 0; - out_3060487946531646253[69] = 0; - out_3060487946531646253[70] = 0; - out_3060487946531646253[71] = 0; - out_3060487946531646253[72] = 0; - out_3060487946531646253[73] = 0; - out_3060487946531646253[74] = 0; - out_3060487946531646253[75] = 0; - out_3060487946531646253[76] = 1; - out_3060487946531646253[77] = 0; - out_3060487946531646253[78] = 0; - out_3060487946531646253[79] = 0; - out_3060487946531646253[80] = 0; - out_3060487946531646253[81] = 0; - out_3060487946531646253[82] = 0; - out_3060487946531646253[83] = 0; - out_3060487946531646253[84] = 0; - out_3060487946531646253[85] = dt; - out_3060487946531646253[86] = 0; - out_3060487946531646253[87] = 0; - out_3060487946531646253[88] = 0; - out_3060487946531646253[89] = 0; - out_3060487946531646253[90] = 0; - out_3060487946531646253[91] = 0; - out_3060487946531646253[92] = 0; - out_3060487946531646253[93] = 0; - out_3060487946531646253[94] = 0; - out_3060487946531646253[95] = 1; - out_3060487946531646253[96] = 0; - out_3060487946531646253[97] = 0; - out_3060487946531646253[98] = 0; - out_3060487946531646253[99] = 0; - out_3060487946531646253[100] = 0; - out_3060487946531646253[101] = 0; - out_3060487946531646253[102] = 0; - out_3060487946531646253[103] = 0; - out_3060487946531646253[104] = dt; - out_3060487946531646253[105] = 0; - out_3060487946531646253[106] = 0; - out_3060487946531646253[107] = 0; - out_3060487946531646253[108] = 0; - out_3060487946531646253[109] = 0; - out_3060487946531646253[110] = 0; - out_3060487946531646253[111] = 0; - out_3060487946531646253[112] = 0; - out_3060487946531646253[113] = 0; - out_3060487946531646253[114] = 1; - out_3060487946531646253[115] = 0; - out_3060487946531646253[116] = 0; - out_3060487946531646253[117] = 0; - out_3060487946531646253[118] = 0; - out_3060487946531646253[119] = 0; - out_3060487946531646253[120] = 0; - out_3060487946531646253[121] = 0; - out_3060487946531646253[122] = 0; - out_3060487946531646253[123] = 0; - out_3060487946531646253[124] = 0; - out_3060487946531646253[125] = 0; - out_3060487946531646253[126] = 0; - out_3060487946531646253[127] = 0; - out_3060487946531646253[128] = 0; - out_3060487946531646253[129] = 0; - out_3060487946531646253[130] = 0; - out_3060487946531646253[131] = 0; - out_3060487946531646253[132] = 0; - out_3060487946531646253[133] = 1; - out_3060487946531646253[134] = 0; - out_3060487946531646253[135] = 0; - out_3060487946531646253[136] = 0; - out_3060487946531646253[137] = 0; - out_3060487946531646253[138] = 0; - out_3060487946531646253[139] = 0; - out_3060487946531646253[140] = 0; - out_3060487946531646253[141] = 0; - out_3060487946531646253[142] = 0; - out_3060487946531646253[143] = 0; - out_3060487946531646253[144] = 0; - out_3060487946531646253[145] = 0; - out_3060487946531646253[146] = 0; - out_3060487946531646253[147] = 0; - out_3060487946531646253[148] = 0; - out_3060487946531646253[149] = 0; - out_3060487946531646253[150] = 0; - out_3060487946531646253[151] = 0; - out_3060487946531646253[152] = 1; - out_3060487946531646253[153] = 0; - out_3060487946531646253[154] = 0; - out_3060487946531646253[155] = 0; - out_3060487946531646253[156] = 0; - out_3060487946531646253[157] = 0; - out_3060487946531646253[158] = 0; - out_3060487946531646253[159] = 0; - out_3060487946531646253[160] = 0; - out_3060487946531646253[161] = 0; - out_3060487946531646253[162] = 0; - out_3060487946531646253[163] = 0; - out_3060487946531646253[164] = 0; - out_3060487946531646253[165] = 0; - out_3060487946531646253[166] = 0; - out_3060487946531646253[167] = 0; - out_3060487946531646253[168] = 0; - out_3060487946531646253[169] = 0; - out_3060487946531646253[170] = 0; - out_3060487946531646253[171] = 1; - out_3060487946531646253[172] = 0; - out_3060487946531646253[173] = 0; - out_3060487946531646253[174] = 0; - out_3060487946531646253[175] = 0; - out_3060487946531646253[176] = 0; - out_3060487946531646253[177] = 0; - out_3060487946531646253[178] = 0; - out_3060487946531646253[179] = 0; - out_3060487946531646253[180] = 0; - out_3060487946531646253[181] = 0; - out_3060487946531646253[182] = 0; - out_3060487946531646253[183] = 0; - out_3060487946531646253[184] = 0; - out_3060487946531646253[185] = 0; - out_3060487946531646253[186] = 0; - out_3060487946531646253[187] = 0; - out_3060487946531646253[188] = 0; - out_3060487946531646253[189] = 0; - out_3060487946531646253[190] = 1; - out_3060487946531646253[191] = 0; - out_3060487946531646253[192] = 0; - out_3060487946531646253[193] = 0; - out_3060487946531646253[194] = 0; - out_3060487946531646253[195] = 0; - out_3060487946531646253[196] = 0; - out_3060487946531646253[197] = 0; - out_3060487946531646253[198] = 0; - out_3060487946531646253[199] = 0; - out_3060487946531646253[200] = 0; - out_3060487946531646253[201] = 0; - out_3060487946531646253[202] = 0; - out_3060487946531646253[203] = 0; - out_3060487946531646253[204] = 0; - out_3060487946531646253[205] = 0; - out_3060487946531646253[206] = 0; - out_3060487946531646253[207] = 0; - out_3060487946531646253[208] = 0; - out_3060487946531646253[209] = 1; - out_3060487946531646253[210] = 0; - out_3060487946531646253[211] = 0; - out_3060487946531646253[212] = 0; - out_3060487946531646253[213] = 0; - out_3060487946531646253[214] = 0; - out_3060487946531646253[215] = 0; - out_3060487946531646253[216] = 0; - out_3060487946531646253[217] = 0; - out_3060487946531646253[218] = 0; - out_3060487946531646253[219] = 0; - out_3060487946531646253[220] = 0; - out_3060487946531646253[221] = 0; - out_3060487946531646253[222] = 0; - out_3060487946531646253[223] = 0; - out_3060487946531646253[224] = 0; - out_3060487946531646253[225] = 0; - out_3060487946531646253[226] = 0; - out_3060487946531646253[227] = 0; - out_3060487946531646253[228] = 1; - out_3060487946531646253[229] = 0; - out_3060487946531646253[230] = 0; - out_3060487946531646253[231] = 0; - out_3060487946531646253[232] = 0; - out_3060487946531646253[233] = 0; - out_3060487946531646253[234] = 0; - out_3060487946531646253[235] = 0; - out_3060487946531646253[236] = 0; - out_3060487946531646253[237] = 0; - out_3060487946531646253[238] = 0; - out_3060487946531646253[239] = 0; - out_3060487946531646253[240] = 0; - out_3060487946531646253[241] = 0; - out_3060487946531646253[242] = 0; - out_3060487946531646253[243] = 0; - out_3060487946531646253[244] = 0; - out_3060487946531646253[245] = 0; - out_3060487946531646253[246] = 0; - out_3060487946531646253[247] = 1; - out_3060487946531646253[248] = 0; - out_3060487946531646253[249] = 0; - out_3060487946531646253[250] = 0; - out_3060487946531646253[251] = 0; - out_3060487946531646253[252] = 0; - out_3060487946531646253[253] = 0; - out_3060487946531646253[254] = 0; - out_3060487946531646253[255] = 0; - out_3060487946531646253[256] = 0; - out_3060487946531646253[257] = 0; - out_3060487946531646253[258] = 0; - out_3060487946531646253[259] = 0; - out_3060487946531646253[260] = 0; - out_3060487946531646253[261] = 0; - out_3060487946531646253[262] = 0; - out_3060487946531646253[263] = 0; - out_3060487946531646253[264] = 0; - out_3060487946531646253[265] = 0; - out_3060487946531646253[266] = 1; - out_3060487946531646253[267] = 0; - out_3060487946531646253[268] = 0; - out_3060487946531646253[269] = 0; - out_3060487946531646253[270] = 0; - out_3060487946531646253[271] = 0; - out_3060487946531646253[272] = 0; - out_3060487946531646253[273] = 0; - out_3060487946531646253[274] = 0; - out_3060487946531646253[275] = 0; - out_3060487946531646253[276] = 0; - out_3060487946531646253[277] = 0; - out_3060487946531646253[278] = 0; - out_3060487946531646253[279] = 0; - out_3060487946531646253[280] = 0; - out_3060487946531646253[281] = 0; - out_3060487946531646253[282] = 0; - out_3060487946531646253[283] = 0; - out_3060487946531646253[284] = 0; - out_3060487946531646253[285] = 1; - out_3060487946531646253[286] = 0; - out_3060487946531646253[287] = 0; - out_3060487946531646253[288] = 0; - out_3060487946531646253[289] = 0; - out_3060487946531646253[290] = 0; - out_3060487946531646253[291] = 0; - out_3060487946531646253[292] = 0; - out_3060487946531646253[293] = 0; - out_3060487946531646253[294] = 0; - out_3060487946531646253[295] = 0; - out_3060487946531646253[296] = 0; - out_3060487946531646253[297] = 0; - out_3060487946531646253[298] = 0; - out_3060487946531646253[299] = 0; - out_3060487946531646253[300] = 0; - out_3060487946531646253[301] = 0; - out_3060487946531646253[302] = 0; - out_3060487946531646253[303] = 0; - out_3060487946531646253[304] = 1; - out_3060487946531646253[305] = 0; - out_3060487946531646253[306] = 0; - out_3060487946531646253[307] = 0; - out_3060487946531646253[308] = 0; - out_3060487946531646253[309] = 0; - out_3060487946531646253[310] = 0; - out_3060487946531646253[311] = 0; - out_3060487946531646253[312] = 0; - out_3060487946531646253[313] = 0; - out_3060487946531646253[314] = 0; - out_3060487946531646253[315] = 0; - out_3060487946531646253[316] = 0; - out_3060487946531646253[317] = 0; - out_3060487946531646253[318] = 0; - out_3060487946531646253[319] = 0; - out_3060487946531646253[320] = 0; - out_3060487946531646253[321] = 0; - out_3060487946531646253[322] = 0; - out_3060487946531646253[323] = 1; +void F_fun(double *state, double dt, double *out_2252133042369597057) { + out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[2] = 0; + out_2252133042369597057[3] = 0; + out_2252133042369597057[4] = 0; + out_2252133042369597057[5] = 0; + out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[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_2252133042369597057[9] = 0; + out_2252133042369597057[10] = 0; + out_2252133042369597057[11] = 0; + out_2252133042369597057[12] = 0; + out_2252133042369597057[13] = 0; + out_2252133042369597057[14] = 0; + out_2252133042369597057[15] = 0; + out_2252133042369597057[16] = 0; + out_2252133042369597057[17] = 0; + out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[20] = 0; + out_2252133042369597057[21] = 0; + out_2252133042369597057[22] = 0; + out_2252133042369597057[23] = 0; + out_2252133042369597057[24] = 0; + out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[27] = 0; + out_2252133042369597057[28] = 0; + out_2252133042369597057[29] = 0; + out_2252133042369597057[30] = 0; + out_2252133042369597057[31] = 0; + out_2252133042369597057[32] = 0; + out_2252133042369597057[33] = 0; + out_2252133042369597057[34] = 0; + out_2252133042369597057[35] = 0; + out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[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_2252133042369597057[39] = 0; + out_2252133042369597057[40] = 0; + out_2252133042369597057[41] = 0; + out_2252133042369597057[42] = 0; + out_2252133042369597057[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_2252133042369597057[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_2252133042369597057[45] = 0; + out_2252133042369597057[46] = 0; + out_2252133042369597057[47] = 0; + out_2252133042369597057[48] = 0; + out_2252133042369597057[49] = 0; + out_2252133042369597057[50] = 0; + out_2252133042369597057[51] = 0; + out_2252133042369597057[52] = 0; + out_2252133042369597057[53] = 0; + out_2252133042369597057[54] = 0; + out_2252133042369597057[55] = 0; + out_2252133042369597057[56] = 0; + out_2252133042369597057[57] = 1; + out_2252133042369597057[58] = 0; + out_2252133042369597057[59] = 0; + out_2252133042369597057[60] = 0; + out_2252133042369597057[61] = 0; + out_2252133042369597057[62] = 0; + out_2252133042369597057[63] = 0; + out_2252133042369597057[64] = 0; + out_2252133042369597057[65] = 0; + out_2252133042369597057[66] = dt; + out_2252133042369597057[67] = 0; + out_2252133042369597057[68] = 0; + out_2252133042369597057[69] = 0; + out_2252133042369597057[70] = 0; + out_2252133042369597057[71] = 0; + out_2252133042369597057[72] = 0; + out_2252133042369597057[73] = 0; + out_2252133042369597057[74] = 0; + out_2252133042369597057[75] = 0; + out_2252133042369597057[76] = 1; + out_2252133042369597057[77] = 0; + out_2252133042369597057[78] = 0; + out_2252133042369597057[79] = 0; + out_2252133042369597057[80] = 0; + out_2252133042369597057[81] = 0; + out_2252133042369597057[82] = 0; + out_2252133042369597057[83] = 0; + out_2252133042369597057[84] = 0; + out_2252133042369597057[85] = dt; + out_2252133042369597057[86] = 0; + out_2252133042369597057[87] = 0; + out_2252133042369597057[88] = 0; + out_2252133042369597057[89] = 0; + out_2252133042369597057[90] = 0; + out_2252133042369597057[91] = 0; + out_2252133042369597057[92] = 0; + out_2252133042369597057[93] = 0; + out_2252133042369597057[94] = 0; + out_2252133042369597057[95] = 1; + out_2252133042369597057[96] = 0; + out_2252133042369597057[97] = 0; + out_2252133042369597057[98] = 0; + out_2252133042369597057[99] = 0; + out_2252133042369597057[100] = 0; + out_2252133042369597057[101] = 0; + out_2252133042369597057[102] = 0; + out_2252133042369597057[103] = 0; + out_2252133042369597057[104] = dt; + out_2252133042369597057[105] = 0; + out_2252133042369597057[106] = 0; + out_2252133042369597057[107] = 0; + out_2252133042369597057[108] = 0; + out_2252133042369597057[109] = 0; + out_2252133042369597057[110] = 0; + out_2252133042369597057[111] = 0; + out_2252133042369597057[112] = 0; + out_2252133042369597057[113] = 0; + out_2252133042369597057[114] = 1; + out_2252133042369597057[115] = 0; + out_2252133042369597057[116] = 0; + out_2252133042369597057[117] = 0; + out_2252133042369597057[118] = 0; + out_2252133042369597057[119] = 0; + out_2252133042369597057[120] = 0; + out_2252133042369597057[121] = 0; + out_2252133042369597057[122] = 0; + out_2252133042369597057[123] = 0; + out_2252133042369597057[124] = 0; + out_2252133042369597057[125] = 0; + out_2252133042369597057[126] = 0; + out_2252133042369597057[127] = 0; + out_2252133042369597057[128] = 0; + out_2252133042369597057[129] = 0; + out_2252133042369597057[130] = 0; + out_2252133042369597057[131] = 0; + out_2252133042369597057[132] = 0; + out_2252133042369597057[133] = 1; + out_2252133042369597057[134] = 0; + out_2252133042369597057[135] = 0; + out_2252133042369597057[136] = 0; + out_2252133042369597057[137] = 0; + out_2252133042369597057[138] = 0; + out_2252133042369597057[139] = 0; + out_2252133042369597057[140] = 0; + out_2252133042369597057[141] = 0; + out_2252133042369597057[142] = 0; + out_2252133042369597057[143] = 0; + out_2252133042369597057[144] = 0; + out_2252133042369597057[145] = 0; + out_2252133042369597057[146] = 0; + out_2252133042369597057[147] = 0; + out_2252133042369597057[148] = 0; + out_2252133042369597057[149] = 0; + out_2252133042369597057[150] = 0; + out_2252133042369597057[151] = 0; + out_2252133042369597057[152] = 1; + out_2252133042369597057[153] = 0; + out_2252133042369597057[154] = 0; + out_2252133042369597057[155] = 0; + out_2252133042369597057[156] = 0; + out_2252133042369597057[157] = 0; + out_2252133042369597057[158] = 0; + out_2252133042369597057[159] = 0; + out_2252133042369597057[160] = 0; + out_2252133042369597057[161] = 0; + out_2252133042369597057[162] = 0; + out_2252133042369597057[163] = 0; + out_2252133042369597057[164] = 0; + out_2252133042369597057[165] = 0; + out_2252133042369597057[166] = 0; + out_2252133042369597057[167] = 0; + out_2252133042369597057[168] = 0; + out_2252133042369597057[169] = 0; + out_2252133042369597057[170] = 0; + out_2252133042369597057[171] = 1; + out_2252133042369597057[172] = 0; + out_2252133042369597057[173] = 0; + out_2252133042369597057[174] = 0; + out_2252133042369597057[175] = 0; + out_2252133042369597057[176] = 0; + out_2252133042369597057[177] = 0; + out_2252133042369597057[178] = 0; + out_2252133042369597057[179] = 0; + out_2252133042369597057[180] = 0; + out_2252133042369597057[181] = 0; + out_2252133042369597057[182] = 0; + out_2252133042369597057[183] = 0; + out_2252133042369597057[184] = 0; + out_2252133042369597057[185] = 0; + out_2252133042369597057[186] = 0; + out_2252133042369597057[187] = 0; + out_2252133042369597057[188] = 0; + out_2252133042369597057[189] = 0; + out_2252133042369597057[190] = 1; + out_2252133042369597057[191] = 0; + out_2252133042369597057[192] = 0; + out_2252133042369597057[193] = 0; + out_2252133042369597057[194] = 0; + out_2252133042369597057[195] = 0; + out_2252133042369597057[196] = 0; + out_2252133042369597057[197] = 0; + out_2252133042369597057[198] = 0; + out_2252133042369597057[199] = 0; + out_2252133042369597057[200] = 0; + out_2252133042369597057[201] = 0; + out_2252133042369597057[202] = 0; + out_2252133042369597057[203] = 0; + out_2252133042369597057[204] = 0; + out_2252133042369597057[205] = 0; + out_2252133042369597057[206] = 0; + out_2252133042369597057[207] = 0; + out_2252133042369597057[208] = 0; + out_2252133042369597057[209] = 1; + out_2252133042369597057[210] = 0; + out_2252133042369597057[211] = 0; + out_2252133042369597057[212] = 0; + out_2252133042369597057[213] = 0; + out_2252133042369597057[214] = 0; + out_2252133042369597057[215] = 0; + out_2252133042369597057[216] = 0; + out_2252133042369597057[217] = 0; + out_2252133042369597057[218] = 0; + out_2252133042369597057[219] = 0; + out_2252133042369597057[220] = 0; + out_2252133042369597057[221] = 0; + out_2252133042369597057[222] = 0; + out_2252133042369597057[223] = 0; + out_2252133042369597057[224] = 0; + out_2252133042369597057[225] = 0; + out_2252133042369597057[226] = 0; + out_2252133042369597057[227] = 0; + out_2252133042369597057[228] = 1; + out_2252133042369597057[229] = 0; + out_2252133042369597057[230] = 0; + out_2252133042369597057[231] = 0; + out_2252133042369597057[232] = 0; + out_2252133042369597057[233] = 0; + out_2252133042369597057[234] = 0; + out_2252133042369597057[235] = 0; + out_2252133042369597057[236] = 0; + out_2252133042369597057[237] = 0; + out_2252133042369597057[238] = 0; + out_2252133042369597057[239] = 0; + out_2252133042369597057[240] = 0; + out_2252133042369597057[241] = 0; + out_2252133042369597057[242] = 0; + out_2252133042369597057[243] = 0; + out_2252133042369597057[244] = 0; + out_2252133042369597057[245] = 0; + out_2252133042369597057[246] = 0; + out_2252133042369597057[247] = 1; + out_2252133042369597057[248] = 0; + out_2252133042369597057[249] = 0; + out_2252133042369597057[250] = 0; + out_2252133042369597057[251] = 0; + out_2252133042369597057[252] = 0; + out_2252133042369597057[253] = 0; + out_2252133042369597057[254] = 0; + out_2252133042369597057[255] = 0; + out_2252133042369597057[256] = 0; + out_2252133042369597057[257] = 0; + out_2252133042369597057[258] = 0; + out_2252133042369597057[259] = 0; + out_2252133042369597057[260] = 0; + out_2252133042369597057[261] = 0; + out_2252133042369597057[262] = 0; + out_2252133042369597057[263] = 0; + out_2252133042369597057[264] = 0; + out_2252133042369597057[265] = 0; + out_2252133042369597057[266] = 1; + out_2252133042369597057[267] = 0; + out_2252133042369597057[268] = 0; + out_2252133042369597057[269] = 0; + out_2252133042369597057[270] = 0; + out_2252133042369597057[271] = 0; + out_2252133042369597057[272] = 0; + out_2252133042369597057[273] = 0; + out_2252133042369597057[274] = 0; + out_2252133042369597057[275] = 0; + out_2252133042369597057[276] = 0; + out_2252133042369597057[277] = 0; + out_2252133042369597057[278] = 0; + out_2252133042369597057[279] = 0; + out_2252133042369597057[280] = 0; + out_2252133042369597057[281] = 0; + out_2252133042369597057[282] = 0; + out_2252133042369597057[283] = 0; + out_2252133042369597057[284] = 0; + out_2252133042369597057[285] = 1; + out_2252133042369597057[286] = 0; + out_2252133042369597057[287] = 0; + out_2252133042369597057[288] = 0; + out_2252133042369597057[289] = 0; + out_2252133042369597057[290] = 0; + out_2252133042369597057[291] = 0; + out_2252133042369597057[292] = 0; + out_2252133042369597057[293] = 0; + out_2252133042369597057[294] = 0; + out_2252133042369597057[295] = 0; + out_2252133042369597057[296] = 0; + out_2252133042369597057[297] = 0; + out_2252133042369597057[298] = 0; + out_2252133042369597057[299] = 0; + out_2252133042369597057[300] = 0; + out_2252133042369597057[301] = 0; + out_2252133042369597057[302] = 0; + out_2252133042369597057[303] = 0; + out_2252133042369597057[304] = 1; + out_2252133042369597057[305] = 0; + out_2252133042369597057[306] = 0; + out_2252133042369597057[307] = 0; + out_2252133042369597057[308] = 0; + out_2252133042369597057[309] = 0; + out_2252133042369597057[310] = 0; + out_2252133042369597057[311] = 0; + out_2252133042369597057[312] = 0; + out_2252133042369597057[313] = 0; + out_2252133042369597057[314] = 0; + out_2252133042369597057[315] = 0; + out_2252133042369597057[316] = 0; + out_2252133042369597057[317] = 0; + out_2252133042369597057[318] = 0; + out_2252133042369597057[319] = 0; + out_2252133042369597057[320] = 0; + out_2252133042369597057[321] = 0; + out_2252133042369597057[322] = 0; + out_2252133042369597057[323] = 1; } -void h_4(double *state, double *unused, double *out_2072690755108230087) { - out_2072690755108230087[0] = state[6] + state[9]; - out_2072690755108230087[1] = state[7] + state[10]; - out_2072690755108230087[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_1225931617160521679) { + out_1225931617160521679[0] = state[6] + state[9]; + out_1225931617160521679[1] = state[7] + state[10]; + out_1225931617160521679[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_440656827576496815) { - out_440656827576496815[0] = 0; - out_440656827576496815[1] = 0; - out_440656827576496815[2] = 0; - out_440656827576496815[3] = 0; - out_440656827576496815[4] = 0; - out_440656827576496815[5] = 0; - out_440656827576496815[6] = 1; - out_440656827576496815[7] = 0; - out_440656827576496815[8] = 0; - out_440656827576496815[9] = 1; - out_440656827576496815[10] = 0; - out_440656827576496815[11] = 0; - out_440656827576496815[12] = 0; - out_440656827576496815[13] = 0; - out_440656827576496815[14] = 0; - out_440656827576496815[15] = 0; - out_440656827576496815[16] = 0; - out_440656827576496815[17] = 0; - out_440656827576496815[18] = 0; - out_440656827576496815[19] = 0; - out_440656827576496815[20] = 0; - out_440656827576496815[21] = 0; - out_440656827576496815[22] = 0; - out_440656827576496815[23] = 0; - out_440656827576496815[24] = 0; - out_440656827576496815[25] = 1; - out_440656827576496815[26] = 0; - out_440656827576496815[27] = 0; - out_440656827576496815[28] = 1; - out_440656827576496815[29] = 0; - out_440656827576496815[30] = 0; - out_440656827576496815[31] = 0; - out_440656827576496815[32] = 0; - out_440656827576496815[33] = 0; - out_440656827576496815[34] = 0; - out_440656827576496815[35] = 0; - out_440656827576496815[36] = 0; - out_440656827576496815[37] = 0; - out_440656827576496815[38] = 0; - out_440656827576496815[39] = 0; - out_440656827576496815[40] = 0; - out_440656827576496815[41] = 0; - out_440656827576496815[42] = 0; - out_440656827576496815[43] = 0; - out_440656827576496815[44] = 1; - out_440656827576496815[45] = 0; - out_440656827576496815[46] = 0; - out_440656827576496815[47] = 1; - out_440656827576496815[48] = 0; - out_440656827576496815[49] = 0; - out_440656827576496815[50] = 0; - out_440656827576496815[51] = 0; - out_440656827576496815[52] = 0; - out_440656827576496815[53] = 0; +void H_4(double *state, double *unused, double *out_3404791898118312310) { + out_3404791898118312310[0] = 0; + out_3404791898118312310[1] = 0; + out_3404791898118312310[2] = 0; + out_3404791898118312310[3] = 0; + out_3404791898118312310[4] = 0; + out_3404791898118312310[5] = 0; + out_3404791898118312310[6] = 1; + out_3404791898118312310[7] = 0; + out_3404791898118312310[8] = 0; + out_3404791898118312310[9] = 1; + out_3404791898118312310[10] = 0; + out_3404791898118312310[11] = 0; + out_3404791898118312310[12] = 0; + out_3404791898118312310[13] = 0; + out_3404791898118312310[14] = 0; + out_3404791898118312310[15] = 0; + out_3404791898118312310[16] = 0; + out_3404791898118312310[17] = 0; + out_3404791898118312310[18] = 0; + out_3404791898118312310[19] = 0; + out_3404791898118312310[20] = 0; + out_3404791898118312310[21] = 0; + out_3404791898118312310[22] = 0; + out_3404791898118312310[23] = 0; + out_3404791898118312310[24] = 0; + out_3404791898118312310[25] = 1; + out_3404791898118312310[26] = 0; + out_3404791898118312310[27] = 0; + out_3404791898118312310[28] = 1; + out_3404791898118312310[29] = 0; + out_3404791898118312310[30] = 0; + out_3404791898118312310[31] = 0; + out_3404791898118312310[32] = 0; + out_3404791898118312310[33] = 0; + out_3404791898118312310[34] = 0; + out_3404791898118312310[35] = 0; + out_3404791898118312310[36] = 0; + out_3404791898118312310[37] = 0; + out_3404791898118312310[38] = 0; + out_3404791898118312310[39] = 0; + out_3404791898118312310[40] = 0; + out_3404791898118312310[41] = 0; + out_3404791898118312310[42] = 0; + out_3404791898118312310[43] = 0; + out_3404791898118312310[44] = 1; + out_3404791898118312310[45] = 0; + out_3404791898118312310[46] = 0; + out_3404791898118312310[47] = 1; + out_3404791898118312310[48] = 0; + out_3404791898118312310[49] = 0; + out_3404791898118312310[50] = 0; + out_3404791898118312310[51] = 0; + out_3404791898118312310[52] = 0; + out_3404791898118312310[53] = 0; } -void h_10(double *state, double *unused, double *out_3685903144892211871) { - out_3685903144892211871[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_3685903144892211871[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_3685903144892211871[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_8314558572170779017) { + out_8314558572170779017[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_8314558572170779017[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_8314558572170779017[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_7274933657249705055) { - out_7274933657249705055[0] = 0; - out_7274933657249705055[1] = 9.8100000000000005*cos(state[1]); - out_7274933657249705055[2] = 0; - out_7274933657249705055[3] = 0; - out_7274933657249705055[4] = -state[8]; - out_7274933657249705055[5] = state[7]; - out_7274933657249705055[6] = 0; - out_7274933657249705055[7] = state[5]; - out_7274933657249705055[8] = -state[4]; - out_7274933657249705055[9] = 0; - out_7274933657249705055[10] = 0; - out_7274933657249705055[11] = 0; - out_7274933657249705055[12] = 1; - out_7274933657249705055[13] = 0; - out_7274933657249705055[14] = 0; - out_7274933657249705055[15] = 1; - out_7274933657249705055[16] = 0; - out_7274933657249705055[17] = 0; - out_7274933657249705055[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_7274933657249705055[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_7274933657249705055[20] = 0; - out_7274933657249705055[21] = state[8]; - out_7274933657249705055[22] = 0; - out_7274933657249705055[23] = -state[6]; - out_7274933657249705055[24] = -state[5]; - out_7274933657249705055[25] = 0; - out_7274933657249705055[26] = state[3]; - out_7274933657249705055[27] = 0; - out_7274933657249705055[28] = 0; - out_7274933657249705055[29] = 0; - out_7274933657249705055[30] = 0; - out_7274933657249705055[31] = 1; - out_7274933657249705055[32] = 0; - out_7274933657249705055[33] = 0; - out_7274933657249705055[34] = 1; - out_7274933657249705055[35] = 0; - out_7274933657249705055[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_7274933657249705055[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_7274933657249705055[38] = 0; - out_7274933657249705055[39] = -state[7]; - out_7274933657249705055[40] = state[6]; - out_7274933657249705055[41] = 0; - out_7274933657249705055[42] = state[4]; - out_7274933657249705055[43] = -state[3]; - out_7274933657249705055[44] = 0; - out_7274933657249705055[45] = 0; - out_7274933657249705055[46] = 0; - out_7274933657249705055[47] = 0; - out_7274933657249705055[48] = 0; - out_7274933657249705055[49] = 0; - out_7274933657249705055[50] = 1; - out_7274933657249705055[51] = 0; - out_7274933657249705055[52] = 0; - out_7274933657249705055[53] = 1; +void H_10(double *state, double *unused, double *out_3543553484029179274) { + out_3543553484029179274[0] = 0; + out_3543553484029179274[1] = 9.8100000000000005*cos(state[1]); + out_3543553484029179274[2] = 0; + out_3543553484029179274[3] = 0; + out_3543553484029179274[4] = -state[8]; + out_3543553484029179274[5] = state[7]; + out_3543553484029179274[6] = 0; + out_3543553484029179274[7] = state[5]; + out_3543553484029179274[8] = -state[4]; + out_3543553484029179274[9] = 0; + out_3543553484029179274[10] = 0; + out_3543553484029179274[11] = 0; + out_3543553484029179274[12] = 1; + out_3543553484029179274[13] = 0; + out_3543553484029179274[14] = 0; + out_3543553484029179274[15] = 1; + out_3543553484029179274[16] = 0; + out_3543553484029179274[17] = 0; + out_3543553484029179274[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_3543553484029179274[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_3543553484029179274[20] = 0; + out_3543553484029179274[21] = state[8]; + out_3543553484029179274[22] = 0; + out_3543553484029179274[23] = -state[6]; + out_3543553484029179274[24] = -state[5]; + out_3543553484029179274[25] = 0; + out_3543553484029179274[26] = state[3]; + out_3543553484029179274[27] = 0; + out_3543553484029179274[28] = 0; + out_3543553484029179274[29] = 0; + out_3543553484029179274[30] = 0; + out_3543553484029179274[31] = 1; + out_3543553484029179274[32] = 0; + out_3543553484029179274[33] = 0; + out_3543553484029179274[34] = 1; + out_3543553484029179274[35] = 0; + out_3543553484029179274[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_3543553484029179274[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_3543553484029179274[38] = 0; + out_3543553484029179274[39] = -state[7]; + out_3543553484029179274[40] = state[6]; + out_3543553484029179274[41] = 0; + out_3543553484029179274[42] = state[4]; + out_3543553484029179274[43] = -state[3]; + out_3543553484029179274[44] = 0; + out_3543553484029179274[45] = 0; + out_3543553484029179274[46] = 0; + out_3543553484029179274[47] = 0; + out_3543553484029179274[48] = 0; + out_3543553484029179274[49] = 0; + out_3543553484029179274[50] = 1; + out_3543553484029179274[51] = 0; + out_3543553484029179274[52] = 0; + out_3543553484029179274[53] = 1; } -void h_13(double *state, double *unused, double *out_7605941513603236476) { - out_7605941513603236476[0] = state[3]; - out_7605941513603236476[1] = state[4]; - out_7605941513603236476[2] = state[5]; +void h_13(double *state, double *unused, double *out_7492025966781743409) { + out_7492025966781743409[0] = state[3]; + out_7492025966781743409[1] = state[4]; + out_7492025966781743409[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3652930652908829616) { - out_3652930652908829616[0] = 0; - out_3652930652908829616[1] = 0; - out_3652930652908829616[2] = 0; - out_3652930652908829616[3] = 1; - out_3652930652908829616[4] = 0; - out_3652930652908829616[5] = 0; - out_3652930652908829616[6] = 0; - out_3652930652908829616[7] = 0; - out_3652930652908829616[8] = 0; - out_3652930652908829616[9] = 0; - out_3652930652908829616[10] = 0; - out_3652930652908829616[11] = 0; - out_3652930652908829616[12] = 0; - out_3652930652908829616[13] = 0; - out_3652930652908829616[14] = 0; - out_3652930652908829616[15] = 0; - out_3652930652908829616[16] = 0; - out_3652930652908829616[17] = 0; - out_3652930652908829616[18] = 0; - out_3652930652908829616[19] = 0; - out_3652930652908829616[20] = 0; - out_3652930652908829616[21] = 0; - out_3652930652908829616[22] = 1; - out_3652930652908829616[23] = 0; - out_3652930652908829616[24] = 0; - out_3652930652908829616[25] = 0; - out_3652930652908829616[26] = 0; - out_3652930652908829616[27] = 0; - out_3652930652908829616[28] = 0; - out_3652930652908829616[29] = 0; - out_3652930652908829616[30] = 0; - out_3652930652908829616[31] = 0; - out_3652930652908829616[32] = 0; - out_3652930652908829616[33] = 0; - out_3652930652908829616[34] = 0; - out_3652930652908829616[35] = 0; - out_3652930652908829616[36] = 0; - out_3652930652908829616[37] = 0; - out_3652930652908829616[38] = 0; - out_3652930652908829616[39] = 0; - out_3652930652908829616[40] = 0; - out_3652930652908829616[41] = 1; - out_3652930652908829616[42] = 0; - out_3652930652908829616[43] = 0; - out_3652930652908829616[44] = 0; - out_3652930652908829616[45] = 0; - out_3652930652908829616[46] = 0; - out_3652930652908829616[47] = 0; - out_3652930652908829616[48] = 0; - out_3652930652908829616[49] = 0; - out_3652930652908829616[50] = 0; - out_3652930652908829616[51] = 0; - out_3652930652908829616[52] = 0; - out_3652930652908829616[53] = 0; +void H_13(double *state, double *unused, double *out_4205839310198388619) { + out_4205839310198388619[0] = 0; + out_4205839310198388619[1] = 0; + out_4205839310198388619[2] = 0; + out_4205839310198388619[3] = 1; + out_4205839310198388619[4] = 0; + out_4205839310198388619[5] = 0; + out_4205839310198388619[6] = 0; + out_4205839310198388619[7] = 0; + out_4205839310198388619[8] = 0; + out_4205839310198388619[9] = 0; + out_4205839310198388619[10] = 0; + out_4205839310198388619[11] = 0; + out_4205839310198388619[12] = 0; + out_4205839310198388619[13] = 0; + out_4205839310198388619[14] = 0; + out_4205839310198388619[15] = 0; + out_4205839310198388619[16] = 0; + out_4205839310198388619[17] = 0; + out_4205839310198388619[18] = 0; + out_4205839310198388619[19] = 0; + out_4205839310198388619[20] = 0; + out_4205839310198388619[21] = 0; + out_4205839310198388619[22] = 1; + out_4205839310198388619[23] = 0; + out_4205839310198388619[24] = 0; + out_4205839310198388619[25] = 0; + out_4205839310198388619[26] = 0; + out_4205839310198388619[27] = 0; + out_4205839310198388619[28] = 0; + out_4205839310198388619[29] = 0; + out_4205839310198388619[30] = 0; + out_4205839310198388619[31] = 0; + out_4205839310198388619[32] = 0; + out_4205839310198388619[33] = 0; + out_4205839310198388619[34] = 0; + out_4205839310198388619[35] = 0; + out_4205839310198388619[36] = 0; + out_4205839310198388619[37] = 0; + out_4205839310198388619[38] = 0; + out_4205839310198388619[39] = 0; + out_4205839310198388619[40] = 0; + out_4205839310198388619[41] = 1; + out_4205839310198388619[42] = 0; + out_4205839310198388619[43] = 0; + out_4205839310198388619[44] = 0; + out_4205839310198388619[45] = 0; + out_4205839310198388619[46] = 0; + out_4205839310198388619[47] = 0; + out_4205839310198388619[48] = 0; + out_4205839310198388619[49] = 0; + out_4205839310198388619[50] = 0; + out_4205839310198388619[51] = 0; + out_4205839310198388619[52] = 0; + out_4205839310198388619[53] = 0; } -void h_14(double *state, double *unused, double *out_732635666688386653) { - out_732635666688386653[0] = state[6]; - out_732635666688386653[1] = state[7]; - out_732635666688386653[2] = state[8]; +void h_14(double *state, double *unused, double *out_8444019918627598165) { + out_8444019918627598165[0] = state[6]; + out_8444019918627598165[1] = state[7]; + out_8444019918627598165[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4403897683915981344) { - out_4403897683915981344[0] = 0; - out_4403897683915981344[1] = 0; - out_4403897683915981344[2] = 0; - out_4403897683915981344[3] = 0; - out_4403897683915981344[4] = 0; - out_4403897683915981344[5] = 0; - out_4403897683915981344[6] = 1; - out_4403897683915981344[7] = 0; - out_4403897683915981344[8] = 0; - out_4403897683915981344[9] = 0; - out_4403897683915981344[10] = 0; - out_4403897683915981344[11] = 0; - out_4403897683915981344[12] = 0; - out_4403897683915981344[13] = 0; - out_4403897683915981344[14] = 0; - out_4403897683915981344[15] = 0; - out_4403897683915981344[16] = 0; - out_4403897683915981344[17] = 0; - out_4403897683915981344[18] = 0; - out_4403897683915981344[19] = 0; - out_4403897683915981344[20] = 0; - out_4403897683915981344[21] = 0; - out_4403897683915981344[22] = 0; - out_4403897683915981344[23] = 0; - out_4403897683915981344[24] = 0; - out_4403897683915981344[25] = 1; - out_4403897683915981344[26] = 0; - out_4403897683915981344[27] = 0; - out_4403897683915981344[28] = 0; - out_4403897683915981344[29] = 0; - out_4403897683915981344[30] = 0; - out_4403897683915981344[31] = 0; - out_4403897683915981344[32] = 0; - out_4403897683915981344[33] = 0; - out_4403897683915981344[34] = 0; - out_4403897683915981344[35] = 0; - out_4403897683915981344[36] = 0; - out_4403897683915981344[37] = 0; - out_4403897683915981344[38] = 0; - out_4403897683915981344[39] = 0; - out_4403897683915981344[40] = 0; - out_4403897683915981344[41] = 0; - out_4403897683915981344[42] = 0; - out_4403897683915981344[43] = 0; - out_4403897683915981344[44] = 1; - out_4403897683915981344[45] = 0; - out_4403897683915981344[46] = 0; - out_4403897683915981344[47] = 0; - out_4403897683915981344[48] = 0; - out_4403897683915981344[49] = 0; - out_4403897683915981344[50] = 0; - out_4403897683915981344[51] = 0; - out_4403897683915981344[52] = 0; - out_4403897683915981344[53] = 0; +void H_14(double *state, double *unused, double *out_6487580330413684606) { + out_6487580330413684606[0] = 0; + out_6487580330413684606[1] = 0; + out_6487580330413684606[2] = 0; + out_6487580330413684606[3] = 0; + out_6487580330413684606[4] = 0; + out_6487580330413684606[5] = 0; + out_6487580330413684606[6] = 1; + out_6487580330413684606[7] = 0; + out_6487580330413684606[8] = 0; + out_6487580330413684606[9] = 0; + out_6487580330413684606[10] = 0; + out_6487580330413684606[11] = 0; + out_6487580330413684606[12] = 0; + out_6487580330413684606[13] = 0; + out_6487580330413684606[14] = 0; + out_6487580330413684606[15] = 0; + out_6487580330413684606[16] = 0; + out_6487580330413684606[17] = 0; + out_6487580330413684606[18] = 0; + out_6487580330413684606[19] = 0; + out_6487580330413684606[20] = 0; + out_6487580330413684606[21] = 0; + out_6487580330413684606[22] = 0; + out_6487580330413684606[23] = 0; + out_6487580330413684606[24] = 0; + out_6487580330413684606[25] = 1; + out_6487580330413684606[26] = 0; + out_6487580330413684606[27] = 0; + out_6487580330413684606[28] = 0; + out_6487580330413684606[29] = 0; + out_6487580330413684606[30] = 0; + out_6487580330413684606[31] = 0; + out_6487580330413684606[32] = 0; + out_6487580330413684606[33] = 0; + out_6487580330413684606[34] = 0; + out_6487580330413684606[35] = 0; + out_6487580330413684606[36] = 0; + out_6487580330413684606[37] = 0; + out_6487580330413684606[38] = 0; + out_6487580330413684606[39] = 0; + out_6487580330413684606[40] = 0; + out_6487580330413684606[41] = 0; + out_6487580330413684606[42] = 0; + out_6487580330413684606[43] = 0; + out_6487580330413684606[44] = 1; + out_6487580330413684606[45] = 0; + out_6487580330413684606[46] = 0; + out_6487580330413684606[47] = 0; + out_6487580330413684606[48] = 0; + out_6487580330413684606[49] = 0; + out_6487580330413684606[50] = 0; + out_6487580330413684606[51] = 0; + out_6487580330413684606[52] = 0; + out_6487580330413684606[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_2423125642091007723) { - err_fun(nom_x, delta_x, out_2423125642091007723); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2822233615381903085) { + err_fun(nom_x, delta_x, out_2822233615381903085); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1501585610038659930) { - inv_err_fun(nom_x, true_x, out_1501585610038659930); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3556797172859207138) { + inv_err_fun(nom_x, true_x, out_3556797172859207138); } -void pose_H_mod_fun(double *state, double *out_313504174478215792) { - H_mod_fun(state, out_313504174478215792); +void pose_H_mod_fun(double *state, double *out_4158952900173024917) { + H_mod_fun(state, out_4158952900173024917); } -void pose_f_fun(double *state, double dt, double *out_8978916703249748052) { - f_fun(state, dt, out_8978916703249748052); +void pose_f_fun(double *state, double dt, double *out_4480310224533440205) { + f_fun(state, dt, out_4480310224533440205); } -void pose_F_fun(double *state, double dt, double *out_3060487946531646253) { - F_fun(state, dt, out_3060487946531646253); +void pose_F_fun(double *state, double dt, double *out_2252133042369597057) { + F_fun(state, dt, out_2252133042369597057); } -void pose_h_4(double *state, double *unused, double *out_2072690755108230087) { - h_4(state, unused, out_2072690755108230087); +void pose_h_4(double *state, double *unused, double *out_1225931617160521679) { + h_4(state, unused, out_1225931617160521679); } -void pose_H_4(double *state, double *unused, double *out_440656827576496815) { - H_4(state, unused, out_440656827576496815); +void pose_H_4(double *state, double *unused, double *out_3404791898118312310) { + H_4(state, unused, out_3404791898118312310); } -void pose_h_10(double *state, double *unused, double *out_3685903144892211871) { - h_10(state, unused, out_3685903144892211871); +void pose_h_10(double *state, double *unused, double *out_8314558572170779017) { + h_10(state, unused, out_8314558572170779017); } -void pose_H_10(double *state, double *unused, double *out_7274933657249705055) { - H_10(state, unused, out_7274933657249705055); +void pose_H_10(double *state, double *unused, double *out_3543553484029179274) { + H_10(state, unused, out_3543553484029179274); } -void pose_h_13(double *state, double *unused, double *out_7605941513603236476) { - h_13(state, unused, out_7605941513603236476); +void pose_h_13(double *state, double *unused, double *out_7492025966781743409) { + h_13(state, unused, out_7492025966781743409); } -void pose_H_13(double *state, double *unused, double *out_3652930652908829616) { - H_13(state, unused, out_3652930652908829616); +void pose_H_13(double *state, double *unused, double *out_4205839310198388619) { + H_13(state, unused, out_4205839310198388619); } -void pose_h_14(double *state, double *unused, double *out_732635666688386653) { - h_14(state, unused, out_732635666688386653); +void pose_h_14(double *state, double *unused, double *out_8444019918627598165) { + h_14(state, unused, out_8444019918627598165); } -void pose_H_14(double *state, double *unused, double *out_4403897683915981344) { - H_14(state, unused, out_4403897683915981344); +void pose_H_14(double *state, double *unused, double *out_6487580330413684606) { + H_14(state, unused, out_6487580330413684606); } 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 ddfc7f12b..b7e1330e1 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_2423125642091007723); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1501585610038659930); -void pose_H_mod_fun(double *state, double *out_313504174478215792); -void pose_f_fun(double *state, double dt, double *out_8978916703249748052); -void pose_F_fun(double *state, double dt, double *out_3060487946531646253); -void pose_h_4(double *state, double *unused, double *out_2072690755108230087); -void pose_H_4(double *state, double *unused, double *out_440656827576496815); -void pose_h_10(double *state, double *unused, double *out_3685903144892211871); -void pose_H_10(double *state, double *unused, double *out_7274933657249705055); -void pose_h_13(double *state, double *unused, double *out_7605941513603236476); -void pose_H_13(double *state, double *unused, double *out_3652930652908829616); -void pose_h_14(double *state, double *unused, double *out_732635666688386653); -void pose_H_14(double *state, double *unused, double *out_4403897683915981344); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2822233615381903085); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3556797172859207138); +void pose_H_mod_fun(double *state, double *out_4158952900173024917); +void pose_f_fun(double *state, double dt, double *out_4480310224533440205); +void pose_F_fun(double *state, double dt, double *out_2252133042369597057); +void pose_h_4(double *state, double *unused, double *out_1225931617160521679); +void pose_H_4(double *state, double *unused, double *out_3404791898118312310); +void pose_h_10(double *state, double *unused, double *out_8314558572170779017); +void pose_H_10(double *state, double *unused, double *out_3543553484029179274); +void pose_h_13(double *state, double *unused, double *out_7492025966781743409); +void pose_H_13(double *state, double *unused, double *out_4205839310198388619); +void pose_h_14(double *state, double *unused, double *out_8444019918627598165); +void pose_H_14(double *state, double *unused, double *out_6487580330413684606); 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 d7b6274de..3310e7f0d 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index fa04ca10a..6cae17eb6 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index e142f44c7..54bfdddfc 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 5c6c0e72c..8a8884ab0 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index c25aa7d92..4f3ff4ef8 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 8b218c2a7..9893bb0f2 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ