diff --git a/openpilot/selfdrive/locationd/models/generated/car.cpp b/openpilot/selfdrive/locationd/models/generated/car.cpp index e1b65039d7..1586f62f9d 100644 --- a/openpilot/selfdrive/locationd/models/generated/car.cpp +++ b/openpilot/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_7295323435102138985) { - out_7295323435102138985[0] = delta_x[0] + nom_x[0]; - out_7295323435102138985[1] = delta_x[1] + nom_x[1]; - out_7295323435102138985[2] = delta_x[2] + nom_x[2]; - out_7295323435102138985[3] = delta_x[3] + nom_x[3]; - out_7295323435102138985[4] = delta_x[4] + nom_x[4]; - out_7295323435102138985[5] = delta_x[5] + nom_x[5]; - out_7295323435102138985[6] = delta_x[6] + nom_x[6]; - out_7295323435102138985[7] = delta_x[7] + nom_x[7]; - out_7295323435102138985[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_6226893299276424900) { + out_6226893299276424900[0] = delta_x[0] + nom_x[0]; + out_6226893299276424900[1] = delta_x[1] + nom_x[1]; + out_6226893299276424900[2] = delta_x[2] + nom_x[2]; + out_6226893299276424900[3] = delta_x[3] + nom_x[3]; + out_6226893299276424900[4] = delta_x[4] + nom_x[4]; + out_6226893299276424900[5] = delta_x[5] + nom_x[5]; + out_6226893299276424900[6] = delta_x[6] + nom_x[6]; + out_6226893299276424900[7] = delta_x[7] + nom_x[7]; + out_6226893299276424900[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5740214235924271464) { - out_5740214235924271464[0] = -nom_x[0] + true_x[0]; - out_5740214235924271464[1] = -nom_x[1] + true_x[1]; - out_5740214235924271464[2] = -nom_x[2] + true_x[2]; - out_5740214235924271464[3] = -nom_x[3] + true_x[3]; - out_5740214235924271464[4] = -nom_x[4] + true_x[4]; - out_5740214235924271464[5] = -nom_x[5] + true_x[5]; - out_5740214235924271464[6] = -nom_x[6] + true_x[6]; - out_5740214235924271464[7] = -nom_x[7] + true_x[7]; - out_5740214235924271464[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3929429382952821600) { + out_3929429382952821600[0] = -nom_x[0] + true_x[0]; + out_3929429382952821600[1] = -nom_x[1] + true_x[1]; + out_3929429382952821600[2] = -nom_x[2] + true_x[2]; + out_3929429382952821600[3] = -nom_x[3] + true_x[3]; + out_3929429382952821600[4] = -nom_x[4] + true_x[4]; + out_3929429382952821600[5] = -nom_x[5] + true_x[5]; + out_3929429382952821600[6] = -nom_x[6] + true_x[6]; + out_3929429382952821600[7] = -nom_x[7] + true_x[7]; + out_3929429382952821600[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_703782495897250555) { - out_703782495897250555[0] = 1.0; - out_703782495897250555[1] = 0.0; - out_703782495897250555[2] = 0.0; - out_703782495897250555[3] = 0.0; - out_703782495897250555[4] = 0.0; - out_703782495897250555[5] = 0.0; - out_703782495897250555[6] = 0.0; - out_703782495897250555[7] = 0.0; - out_703782495897250555[8] = 0.0; - out_703782495897250555[9] = 0.0; - out_703782495897250555[10] = 1.0; - out_703782495897250555[11] = 0.0; - out_703782495897250555[12] = 0.0; - out_703782495897250555[13] = 0.0; - out_703782495897250555[14] = 0.0; - out_703782495897250555[15] = 0.0; - out_703782495897250555[16] = 0.0; - out_703782495897250555[17] = 0.0; - out_703782495897250555[18] = 0.0; - out_703782495897250555[19] = 0.0; - out_703782495897250555[20] = 1.0; - out_703782495897250555[21] = 0.0; - out_703782495897250555[22] = 0.0; - out_703782495897250555[23] = 0.0; - out_703782495897250555[24] = 0.0; - out_703782495897250555[25] = 0.0; - out_703782495897250555[26] = 0.0; - out_703782495897250555[27] = 0.0; - out_703782495897250555[28] = 0.0; - out_703782495897250555[29] = 0.0; - out_703782495897250555[30] = 1.0; - out_703782495897250555[31] = 0.0; - out_703782495897250555[32] = 0.0; - out_703782495897250555[33] = 0.0; - out_703782495897250555[34] = 0.0; - out_703782495897250555[35] = 0.0; - out_703782495897250555[36] = 0.0; - out_703782495897250555[37] = 0.0; - out_703782495897250555[38] = 0.0; - out_703782495897250555[39] = 0.0; - out_703782495897250555[40] = 1.0; - out_703782495897250555[41] = 0.0; - out_703782495897250555[42] = 0.0; - out_703782495897250555[43] = 0.0; - out_703782495897250555[44] = 0.0; - out_703782495897250555[45] = 0.0; - out_703782495897250555[46] = 0.0; - out_703782495897250555[47] = 0.0; - out_703782495897250555[48] = 0.0; - out_703782495897250555[49] = 0.0; - out_703782495897250555[50] = 1.0; - out_703782495897250555[51] = 0.0; - out_703782495897250555[52] = 0.0; - out_703782495897250555[53] = 0.0; - out_703782495897250555[54] = 0.0; - out_703782495897250555[55] = 0.0; - out_703782495897250555[56] = 0.0; - out_703782495897250555[57] = 0.0; - out_703782495897250555[58] = 0.0; - out_703782495897250555[59] = 0.0; - out_703782495897250555[60] = 1.0; - out_703782495897250555[61] = 0.0; - out_703782495897250555[62] = 0.0; - out_703782495897250555[63] = 0.0; - out_703782495897250555[64] = 0.0; - out_703782495897250555[65] = 0.0; - out_703782495897250555[66] = 0.0; - out_703782495897250555[67] = 0.0; - out_703782495897250555[68] = 0.0; - out_703782495897250555[69] = 0.0; - out_703782495897250555[70] = 1.0; - out_703782495897250555[71] = 0.0; - out_703782495897250555[72] = 0.0; - out_703782495897250555[73] = 0.0; - out_703782495897250555[74] = 0.0; - out_703782495897250555[75] = 0.0; - out_703782495897250555[76] = 0.0; - out_703782495897250555[77] = 0.0; - out_703782495897250555[78] = 0.0; - out_703782495897250555[79] = 0.0; - out_703782495897250555[80] = 1.0; +void H_mod_fun(double *state, double *out_1271629825232555869) { + out_1271629825232555869[0] = 1.0; + out_1271629825232555869[1] = 0.0; + out_1271629825232555869[2] = 0.0; + out_1271629825232555869[3] = 0.0; + out_1271629825232555869[4] = 0.0; + out_1271629825232555869[5] = 0.0; + out_1271629825232555869[6] = 0.0; + out_1271629825232555869[7] = 0.0; + out_1271629825232555869[8] = 0.0; + out_1271629825232555869[9] = 0.0; + out_1271629825232555869[10] = 1.0; + out_1271629825232555869[11] = 0.0; + out_1271629825232555869[12] = 0.0; + out_1271629825232555869[13] = 0.0; + out_1271629825232555869[14] = 0.0; + out_1271629825232555869[15] = 0.0; + out_1271629825232555869[16] = 0.0; + out_1271629825232555869[17] = 0.0; + out_1271629825232555869[18] = 0.0; + out_1271629825232555869[19] = 0.0; + out_1271629825232555869[20] = 1.0; + out_1271629825232555869[21] = 0.0; + out_1271629825232555869[22] = 0.0; + out_1271629825232555869[23] = 0.0; + out_1271629825232555869[24] = 0.0; + out_1271629825232555869[25] = 0.0; + out_1271629825232555869[26] = 0.0; + out_1271629825232555869[27] = 0.0; + out_1271629825232555869[28] = 0.0; + out_1271629825232555869[29] = 0.0; + out_1271629825232555869[30] = 1.0; + out_1271629825232555869[31] = 0.0; + out_1271629825232555869[32] = 0.0; + out_1271629825232555869[33] = 0.0; + out_1271629825232555869[34] = 0.0; + out_1271629825232555869[35] = 0.0; + out_1271629825232555869[36] = 0.0; + out_1271629825232555869[37] = 0.0; + out_1271629825232555869[38] = 0.0; + out_1271629825232555869[39] = 0.0; + out_1271629825232555869[40] = 1.0; + out_1271629825232555869[41] = 0.0; + out_1271629825232555869[42] = 0.0; + out_1271629825232555869[43] = 0.0; + out_1271629825232555869[44] = 0.0; + out_1271629825232555869[45] = 0.0; + out_1271629825232555869[46] = 0.0; + out_1271629825232555869[47] = 0.0; + out_1271629825232555869[48] = 0.0; + out_1271629825232555869[49] = 0.0; + out_1271629825232555869[50] = 1.0; + out_1271629825232555869[51] = 0.0; + out_1271629825232555869[52] = 0.0; + out_1271629825232555869[53] = 0.0; + out_1271629825232555869[54] = 0.0; + out_1271629825232555869[55] = 0.0; + out_1271629825232555869[56] = 0.0; + out_1271629825232555869[57] = 0.0; + out_1271629825232555869[58] = 0.0; + out_1271629825232555869[59] = 0.0; + out_1271629825232555869[60] = 1.0; + out_1271629825232555869[61] = 0.0; + out_1271629825232555869[62] = 0.0; + out_1271629825232555869[63] = 0.0; + out_1271629825232555869[64] = 0.0; + out_1271629825232555869[65] = 0.0; + out_1271629825232555869[66] = 0.0; + out_1271629825232555869[67] = 0.0; + out_1271629825232555869[68] = 0.0; + out_1271629825232555869[69] = 0.0; + out_1271629825232555869[70] = 1.0; + out_1271629825232555869[71] = 0.0; + out_1271629825232555869[72] = 0.0; + out_1271629825232555869[73] = 0.0; + out_1271629825232555869[74] = 0.0; + out_1271629825232555869[75] = 0.0; + out_1271629825232555869[76] = 0.0; + out_1271629825232555869[77] = 0.0; + out_1271629825232555869[78] = 0.0; + out_1271629825232555869[79] = 0.0; + out_1271629825232555869[80] = 1.0; } -void f_fun(double *state, double dt, double *out_4026170021074555431) { - out_4026170021074555431[0] = state[0]; - out_4026170021074555431[1] = state[1]; - out_4026170021074555431[2] = state[2]; - out_4026170021074555431[3] = state[3]; - out_4026170021074555431[4] = state[4]; - out_4026170021074555431[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_4026170021074555431[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_4026170021074555431[7] = state[7]; - out_4026170021074555431[8] = state[8]; +void f_fun(double *state, double dt, double *out_6661690039012274114) { + out_6661690039012274114[0] = state[0]; + out_6661690039012274114[1] = state[1]; + out_6661690039012274114[2] = state[2]; + out_6661690039012274114[3] = state[3]; + out_6661690039012274114[4] = state[4]; + out_6661690039012274114[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_6661690039012274114[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_6661690039012274114[7] = state[7]; + out_6661690039012274114[8] = state[8]; } -void F_fun(double *state, double dt, double *out_4652814377482513796) { - out_4652814377482513796[0] = 1; - out_4652814377482513796[1] = 0; - out_4652814377482513796[2] = 0; - out_4652814377482513796[3] = 0; - out_4652814377482513796[4] = 0; - out_4652814377482513796[5] = 0; - out_4652814377482513796[6] = 0; - out_4652814377482513796[7] = 0; - out_4652814377482513796[8] = 0; - out_4652814377482513796[9] = 0; - out_4652814377482513796[10] = 1; - out_4652814377482513796[11] = 0; - out_4652814377482513796[12] = 0; - out_4652814377482513796[13] = 0; - out_4652814377482513796[14] = 0; - out_4652814377482513796[15] = 0; - out_4652814377482513796[16] = 0; - out_4652814377482513796[17] = 0; - out_4652814377482513796[18] = 0; - out_4652814377482513796[19] = 0; - out_4652814377482513796[20] = 1; - out_4652814377482513796[21] = 0; - out_4652814377482513796[22] = 0; - out_4652814377482513796[23] = 0; - out_4652814377482513796[24] = 0; - out_4652814377482513796[25] = 0; - out_4652814377482513796[26] = 0; - out_4652814377482513796[27] = 0; - out_4652814377482513796[28] = 0; - out_4652814377482513796[29] = 0; - out_4652814377482513796[30] = 1; - out_4652814377482513796[31] = 0; - out_4652814377482513796[32] = 0; - out_4652814377482513796[33] = 0; - out_4652814377482513796[34] = 0; - out_4652814377482513796[35] = 0; - out_4652814377482513796[36] = 0; - out_4652814377482513796[37] = 0; - out_4652814377482513796[38] = 0; - out_4652814377482513796[39] = 0; - out_4652814377482513796[40] = 1; - out_4652814377482513796[41] = 0; - out_4652814377482513796[42] = 0; - out_4652814377482513796[43] = 0; - out_4652814377482513796[44] = 0; - out_4652814377482513796[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_4652814377482513796[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_4652814377482513796[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4652814377482513796[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4652814377482513796[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_4652814377482513796[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_4652814377482513796[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_4652814377482513796[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_4652814377482513796[53] = -9.8100000000000005*dt; - out_4652814377482513796[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_4652814377482513796[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_4652814377482513796[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4652814377482513796[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4652814377482513796[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_4652814377482513796[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_4652814377482513796[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_4652814377482513796[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4652814377482513796[62] = 0; - out_4652814377482513796[63] = 0; - out_4652814377482513796[64] = 0; - out_4652814377482513796[65] = 0; - out_4652814377482513796[66] = 0; - out_4652814377482513796[67] = 0; - out_4652814377482513796[68] = 0; - out_4652814377482513796[69] = 0; - out_4652814377482513796[70] = 1; - out_4652814377482513796[71] = 0; - out_4652814377482513796[72] = 0; - out_4652814377482513796[73] = 0; - out_4652814377482513796[74] = 0; - out_4652814377482513796[75] = 0; - out_4652814377482513796[76] = 0; - out_4652814377482513796[77] = 0; - out_4652814377482513796[78] = 0; - out_4652814377482513796[79] = 0; - out_4652814377482513796[80] = 1; +void F_fun(double *state, double dt, double *out_1834203406066241770) { + out_1834203406066241770[0] = 1; + out_1834203406066241770[1] = 0; + out_1834203406066241770[2] = 0; + out_1834203406066241770[3] = 0; + out_1834203406066241770[4] = 0; + out_1834203406066241770[5] = 0; + out_1834203406066241770[6] = 0; + out_1834203406066241770[7] = 0; + out_1834203406066241770[8] = 0; + out_1834203406066241770[9] = 0; + out_1834203406066241770[10] = 1; + out_1834203406066241770[11] = 0; + out_1834203406066241770[12] = 0; + out_1834203406066241770[13] = 0; + out_1834203406066241770[14] = 0; + out_1834203406066241770[15] = 0; + out_1834203406066241770[16] = 0; + out_1834203406066241770[17] = 0; + out_1834203406066241770[18] = 0; + out_1834203406066241770[19] = 0; + out_1834203406066241770[20] = 1; + out_1834203406066241770[21] = 0; + out_1834203406066241770[22] = 0; + out_1834203406066241770[23] = 0; + out_1834203406066241770[24] = 0; + out_1834203406066241770[25] = 0; + out_1834203406066241770[26] = 0; + out_1834203406066241770[27] = 0; + out_1834203406066241770[28] = 0; + out_1834203406066241770[29] = 0; + out_1834203406066241770[30] = 1; + out_1834203406066241770[31] = 0; + out_1834203406066241770[32] = 0; + out_1834203406066241770[33] = 0; + out_1834203406066241770[34] = 0; + out_1834203406066241770[35] = 0; + out_1834203406066241770[36] = 0; + out_1834203406066241770[37] = 0; + out_1834203406066241770[38] = 0; + out_1834203406066241770[39] = 0; + out_1834203406066241770[40] = 1; + out_1834203406066241770[41] = 0; + out_1834203406066241770[42] = 0; + out_1834203406066241770[43] = 0; + out_1834203406066241770[44] = 0; + out_1834203406066241770[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_1834203406066241770[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_1834203406066241770[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_1834203406066241770[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_1834203406066241770[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_1834203406066241770[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_1834203406066241770[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_1834203406066241770[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_1834203406066241770[53] = -9.8100000000000005*dt; + out_1834203406066241770[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_1834203406066241770[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_1834203406066241770[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1834203406066241770[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1834203406066241770[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_1834203406066241770[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_1834203406066241770[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_1834203406066241770[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1834203406066241770[62] = 0; + out_1834203406066241770[63] = 0; + out_1834203406066241770[64] = 0; + out_1834203406066241770[65] = 0; + out_1834203406066241770[66] = 0; + out_1834203406066241770[67] = 0; + out_1834203406066241770[68] = 0; + out_1834203406066241770[69] = 0; + out_1834203406066241770[70] = 1; + out_1834203406066241770[71] = 0; + out_1834203406066241770[72] = 0; + out_1834203406066241770[73] = 0; + out_1834203406066241770[74] = 0; + out_1834203406066241770[75] = 0; + out_1834203406066241770[76] = 0; + out_1834203406066241770[77] = 0; + out_1834203406066241770[78] = 0; + out_1834203406066241770[79] = 0; + out_1834203406066241770[80] = 1; } -void h_25(double *state, double *unused, double *out_4039743270013067154) { - out_4039743270013067154[0] = state[6]; +void h_25(double *state, double *unused, double *out_1060401645907473267) { + out_1060401645907473267[0] = state[6]; } -void H_25(double *state, double *unused, double *out_4226763145886742232) { - out_4226763145886742232[0] = 0; - out_4226763145886742232[1] = 0; - out_4226763145886742232[2] = 0; - out_4226763145886742232[3] = 0; - out_4226763145886742232[4] = 0; - out_4226763145886742232[5] = 0; - out_4226763145886742232[6] = 1; - out_4226763145886742232[7] = 0; - out_4226763145886742232[8] = 0; +void H_25(double *state, double *unused, double *out_7492528034826712728) { + out_7492528034826712728[0] = 0; + out_7492528034826712728[1] = 0; + out_7492528034826712728[2] = 0; + out_7492528034826712728[3] = 0; + out_7492528034826712728[4] = 0; + out_7492528034826712728[5] = 0; + out_7492528034826712728[6] = 1; + out_7492528034826712728[7] = 0; + out_7492528034826712728[8] = 0; } -void h_24(double *state, double *unused, double *out_3606421098967363994) { - out_3606421098967363994[0] = state[4]; - out_3606421098967363994[1] = state[5]; +void h_24(double *state, double *unused, double *out_8639369439479545960) { + out_8639369439479545960[0] = state[4]; + out_8639369439479545960[1] = state[5]; } -void H_24(double *state, double *unused, double *out_6403977569493892205) { - out_6403977569493892205[0] = 0; - out_6403977569493892205[1] = 0; - out_6403977569493892205[2] = 0; - out_6403977569493892205[3] = 0; - out_6403977569493892205[4] = 1; - out_6403977569493892205[5] = 0; - out_6403977569493892205[6] = 0; - out_6403977569493892205[7] = 0; - out_6403977569493892205[8] = 0; - out_6403977569493892205[9] = 0; - out_6403977569493892205[10] = 0; - out_6403977569493892205[11] = 0; - out_6403977569493892205[12] = 0; - out_6403977569493892205[13] = 0; - out_6403977569493892205[14] = 1; - out_6403977569493892205[15] = 0; - out_6403977569493892205[16] = 0; - out_6403977569493892205[17] = 0; +void H_24(double *state, double *unused, double *out_8239110267165924544) { + out_8239110267165924544[0] = 0; + out_8239110267165924544[1] = 0; + out_8239110267165924544[2] = 0; + out_8239110267165924544[3] = 0; + out_8239110267165924544[4] = 1; + out_8239110267165924544[5] = 0; + out_8239110267165924544[6] = 0; + out_8239110267165924544[7] = 0; + out_8239110267165924544[8] = 0; + out_8239110267165924544[9] = 0; + out_8239110267165924544[10] = 0; + out_8239110267165924544[11] = 0; + out_8239110267165924544[12] = 0; + out_8239110267165924544[13] = 0; + out_8239110267165924544[14] = 1; + out_8239110267165924544[15] = 0; + out_8239110267165924544[16] = 0; + out_8239110267165924544[17] = 0; } -void h_30(double *state, double *unused, double *out_4600134206705474730) { - out_4600134206705474730[0] = state[4]; +void h_30(double *state, double *unused, double *out_7831236872257824203) { + out_7831236872257824203[0] = state[4]; } -void H_30(double *state, double *unused, double *out_6745096104393990859) { - out_6745096104393990859[0] = 0; - out_6745096104393990859[1] = 0; - out_6745096104393990859[2] = 0; - out_6745096104393990859[3] = 0; - out_6745096104393990859[4] = 1; - out_6745096104393990859[5] = 0; - out_6745096104393990859[6] = 0; - out_6745096104393990859[7] = 0; - out_6745096104393990859[8] = 0; +void H_30(double *state, double *unused, double *out_4037525697391222133) { + out_4037525697391222133[0] = 0; + out_4037525697391222133[1] = 0; + out_4037525697391222133[2] = 0; + out_4037525697391222133[3] = 0; + out_4037525697391222133[4] = 1; + out_4037525697391222133[5] = 0; + out_4037525697391222133[6] = 0; + out_4037525697391222133[7] = 0; + out_4037525697391222133[8] = 0; } -void h_26(double *state, double *unused, double *out_1787582103435621347) { - out_1787582103435621347[0] = state[7]; +void h_26(double *state, double *unused, double *out_8434552756934301720) { + out_8434552756934301720[0] = state[7]; } -void H_26(double *state, double *unused, double *out_485259827012686008) { - out_485259827012686008[0] = 0; - out_485259827012686008[1] = 0; - out_485259827012686008[2] = 0; - out_485259827012686008[3] = 0; - out_485259827012686008[4] = 0; - out_485259827012686008[5] = 0; - out_485259827012686008[6] = 0; - out_485259827012686008[7] = 1; - out_485259827012686008[8] = 0; +void H_26(double *state, double *unused, double *out_7649690069122038287) { + out_7649690069122038287[0] = 0; + out_7649690069122038287[1] = 0; + out_7649690069122038287[2] = 0; + out_7649690069122038287[3] = 0; + out_7649690069122038287[4] = 0; + out_7649690069122038287[5] = 0; + out_7649690069122038287[6] = 0; + out_7649690069122038287[7] = 1; + out_7649690069122038287[8] = 0; } -void h_27(double *state, double *unused, double *out_9041416537673912799) { - out_9041416537673912799[0] = state[3]; +void h_27(double *state, double *unused, double *out_3941271621753372378) { + out_3941271621753372378[0] = state[3]; } -void H_27(double *state, double *unused, double *out_8968690175577934076) { - out_8968690175577934076[0] = 0; - out_8968690175577934076[1] = 0; - out_8968690175577934076[2] = 0; - out_8968690175577934076[3] = 1; - out_8968690175577934076[4] = 0; - out_8968690175577934076[5] = 0; - out_8968690175577934076[6] = 0; - out_8968690175577934076[7] = 0; - out_8968690175577934076[8] = 0; +void H_27(double *state, double *unused, double *out_6212289009191647044) { + out_6212289009191647044[0] = 0; + out_6212289009191647044[1] = 0; + out_6212289009191647044[2] = 0; + out_6212289009191647044[3] = 1; + out_6212289009191647044[4] = 0; + out_6212289009191647044[5] = 0; + out_6212289009191647044[6] = 0; + out_6212289009191647044[7] = 0; + out_6212289009191647044[8] = 0; } -void h_29(double *state, double *unused, double *out_5767878007781616467) { - out_5767878007781616467[0] = state[1]; +void h_29(double *state, double *unused, double *out_7578721150007394728) { + out_7578721150007394728[0] = state[1]; } -void H_29(double *state, double *unused, double *out_7255327448708383043) { - out_7255327448708383043[0] = 0; - out_7255327448708383043[1] = 1; - out_7255327448708383043[2] = 0; - out_7255327448708383043[3] = 0; - out_7255327448708383043[4] = 0; - out_7255327448708383043[5] = 0; - out_7255327448708383043[6] = 0; - out_7255327448708383043[7] = 0; - out_7255327448708383043[8] = 0; +void H_29(double *state, double *unused, double *out_7925651736061198077) { + out_7925651736061198077[0] = 0; + out_7925651736061198077[1] = 1; + out_7925651736061198077[2] = 0; + out_7925651736061198077[3] = 0; + out_7925651736061198077[4] = 0; + out_7925651736061198077[5] = 0; + out_7925651736061198077[6] = 0; + out_7925651736061198077[7] = 0; + out_7925651736061198077[8] = 0; } -void h_28(double *state, double *unused, double *out_5400909299850363481) { - out_5400909299850363481[0] = state[0]; +void h_28(double *state, double *unused, double *out_2997761768851709943) { + out_2997761768851709943[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2172928431638852469) { - out_2172928431638852469[0] = 1; - out_2172928431638852469[1] = 0; - out_2172928431638852469[2] = 0; - out_2172928431638852469[3] = 0; - out_2172928431638852469[4] = 0; - out_2172928431638852469[5] = 0; - out_2172928431638852469[6] = 0; - out_2172928431638852469[7] = 0; - out_2172928431638852469[8] = 0; +void H_28(double *state, double *unused, double *out_5438693320578822965) { + out_5438693320578822965[0] = 1; + out_5438693320578822965[1] = 0; + out_5438693320578822965[2] = 0; + out_5438693320578822965[3] = 0; + out_5438693320578822965[4] = 0; + out_5438693320578822965[5] = 0; + out_5438693320578822965[6] = 0; + out_5438693320578822965[7] = 0; + out_5438693320578822965[8] = 0; } -void h_31(double *state, double *unused, double *out_3764549207728561265) { - out_3764549207728561265[0] = state[8]; +void h_31(double *state, double *unused, double *out_390229746721409900) { + out_390229746721409900[0] = state[8]; } -void H_31(double *state, double *unused, double *out_4257409107763702660) { - out_4257409107763702660[0] = 0; - out_4257409107763702660[1] = 0; - out_4257409107763702660[2] = 0; - out_4257409107763702660[3] = 0; - out_4257409107763702660[4] = 0; - out_4257409107763702660[5] = 0; - out_4257409107763702660[6] = 0; - out_4257409107763702660[7] = 0; - out_4257409107763702660[8] = 1; +void H_31(double *state, double *unused, double *out_7523173996703673156) { + out_7523173996703673156[0] = 0; + out_7523173996703673156[1] = 0; + out_7523173996703673156[2] = 0; + out_7523173996703673156[3] = 0; + out_7523173996703673156[4] = 0; + out_7523173996703673156[5] = 0; + out_7523173996703673156[6] = 0; + out_7523173996703673156[7] = 0; + out_7523173996703673156[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_7295323435102138985) { - err_fun(nom_x, delta_x, out_7295323435102138985); +void car_err_fun(double *nom_x, double *delta_x, double *out_6226893299276424900) { + err_fun(nom_x, delta_x, out_6226893299276424900); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_5740214235924271464) { - inv_err_fun(nom_x, true_x, out_5740214235924271464); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3929429382952821600) { + inv_err_fun(nom_x, true_x, out_3929429382952821600); } -void car_H_mod_fun(double *state, double *out_703782495897250555) { - H_mod_fun(state, out_703782495897250555); +void car_H_mod_fun(double *state, double *out_1271629825232555869) { + H_mod_fun(state, out_1271629825232555869); } -void car_f_fun(double *state, double dt, double *out_4026170021074555431) { - f_fun(state, dt, out_4026170021074555431); +void car_f_fun(double *state, double dt, double *out_6661690039012274114) { + f_fun(state, dt, out_6661690039012274114); } -void car_F_fun(double *state, double dt, double *out_4652814377482513796) { - F_fun(state, dt, out_4652814377482513796); +void car_F_fun(double *state, double dt, double *out_1834203406066241770) { + F_fun(state, dt, out_1834203406066241770); } -void car_h_25(double *state, double *unused, double *out_4039743270013067154) { - h_25(state, unused, out_4039743270013067154); +void car_h_25(double *state, double *unused, double *out_1060401645907473267) { + h_25(state, unused, out_1060401645907473267); } -void car_H_25(double *state, double *unused, double *out_4226763145886742232) { - H_25(state, unused, out_4226763145886742232); +void car_H_25(double *state, double *unused, double *out_7492528034826712728) { + H_25(state, unused, out_7492528034826712728); } -void car_h_24(double *state, double *unused, double *out_3606421098967363994) { - h_24(state, unused, out_3606421098967363994); +void car_h_24(double *state, double *unused, double *out_8639369439479545960) { + h_24(state, unused, out_8639369439479545960); } -void car_H_24(double *state, double *unused, double *out_6403977569493892205) { - H_24(state, unused, out_6403977569493892205); +void car_H_24(double *state, double *unused, double *out_8239110267165924544) { + H_24(state, unused, out_8239110267165924544); } -void car_h_30(double *state, double *unused, double *out_4600134206705474730) { - h_30(state, unused, out_4600134206705474730); +void car_h_30(double *state, double *unused, double *out_7831236872257824203) { + h_30(state, unused, out_7831236872257824203); } -void car_H_30(double *state, double *unused, double *out_6745096104393990859) { - H_30(state, unused, out_6745096104393990859); +void car_H_30(double *state, double *unused, double *out_4037525697391222133) { + H_30(state, unused, out_4037525697391222133); } -void car_h_26(double *state, double *unused, double *out_1787582103435621347) { - h_26(state, unused, out_1787582103435621347); +void car_h_26(double *state, double *unused, double *out_8434552756934301720) { + h_26(state, unused, out_8434552756934301720); } -void car_H_26(double *state, double *unused, double *out_485259827012686008) { - H_26(state, unused, out_485259827012686008); +void car_H_26(double *state, double *unused, double *out_7649690069122038287) { + H_26(state, unused, out_7649690069122038287); } -void car_h_27(double *state, double *unused, double *out_9041416537673912799) { - h_27(state, unused, out_9041416537673912799); +void car_h_27(double *state, double *unused, double *out_3941271621753372378) { + h_27(state, unused, out_3941271621753372378); } -void car_H_27(double *state, double *unused, double *out_8968690175577934076) { - H_27(state, unused, out_8968690175577934076); +void car_H_27(double *state, double *unused, double *out_6212289009191647044) { + H_27(state, unused, out_6212289009191647044); } -void car_h_29(double *state, double *unused, double *out_5767878007781616467) { - h_29(state, unused, out_5767878007781616467); +void car_h_29(double *state, double *unused, double *out_7578721150007394728) { + h_29(state, unused, out_7578721150007394728); } -void car_H_29(double *state, double *unused, double *out_7255327448708383043) { - H_29(state, unused, out_7255327448708383043); +void car_H_29(double *state, double *unused, double *out_7925651736061198077) { + H_29(state, unused, out_7925651736061198077); } -void car_h_28(double *state, double *unused, double *out_5400909299850363481) { - h_28(state, unused, out_5400909299850363481); +void car_h_28(double *state, double *unused, double *out_2997761768851709943) { + h_28(state, unused, out_2997761768851709943); } -void car_H_28(double *state, double *unused, double *out_2172928431638852469) { - H_28(state, unused, out_2172928431638852469); +void car_H_28(double *state, double *unused, double *out_5438693320578822965) { + H_28(state, unused, out_5438693320578822965); } -void car_h_31(double *state, double *unused, double *out_3764549207728561265) { - h_31(state, unused, out_3764549207728561265); +void car_h_31(double *state, double *unused, double *out_390229746721409900) { + h_31(state, unused, out_390229746721409900); } -void car_H_31(double *state, double *unused, double *out_4257409107763702660) { - H_31(state, unused, out_4257409107763702660); +void car_H_31(double *state, double *unused, double *out_7523173996703673156) { + H_31(state, unused, out_7523173996703673156); } 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/openpilot/selfdrive/locationd/models/generated/car.h b/openpilot/selfdrive/locationd/models/generated/car.h index 3b9c045a69..8c1241d4a8 100644 --- a/openpilot/selfdrive/locationd/models/generated/car.h +++ b/openpilot/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_7295323435102138985); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_5740214235924271464); -void car_H_mod_fun(double *state, double *out_703782495897250555); -void car_f_fun(double *state, double dt, double *out_4026170021074555431); -void car_F_fun(double *state, double dt, double *out_4652814377482513796); -void car_h_25(double *state, double *unused, double *out_4039743270013067154); -void car_H_25(double *state, double *unused, double *out_4226763145886742232); -void car_h_24(double *state, double *unused, double *out_3606421098967363994); -void car_H_24(double *state, double *unused, double *out_6403977569493892205); -void car_h_30(double *state, double *unused, double *out_4600134206705474730); -void car_H_30(double *state, double *unused, double *out_6745096104393990859); -void car_h_26(double *state, double *unused, double *out_1787582103435621347); -void car_H_26(double *state, double *unused, double *out_485259827012686008); -void car_h_27(double *state, double *unused, double *out_9041416537673912799); -void car_H_27(double *state, double *unused, double *out_8968690175577934076); -void car_h_29(double *state, double *unused, double *out_5767878007781616467); -void car_H_29(double *state, double *unused, double *out_7255327448708383043); -void car_h_28(double *state, double *unused, double *out_5400909299850363481); -void car_H_28(double *state, double *unused, double *out_2172928431638852469); -void car_h_31(double *state, double *unused, double *out_3764549207728561265); -void car_H_31(double *state, double *unused, double *out_4257409107763702660); +void car_err_fun(double *nom_x, double *delta_x, double *out_6226893299276424900); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3929429382952821600); +void car_H_mod_fun(double *state, double *out_1271629825232555869); +void car_f_fun(double *state, double dt, double *out_6661690039012274114); +void car_F_fun(double *state, double dt, double *out_1834203406066241770); +void car_h_25(double *state, double *unused, double *out_1060401645907473267); +void car_H_25(double *state, double *unused, double *out_7492528034826712728); +void car_h_24(double *state, double *unused, double *out_8639369439479545960); +void car_H_24(double *state, double *unused, double *out_8239110267165924544); +void car_h_30(double *state, double *unused, double *out_7831236872257824203); +void car_H_30(double *state, double *unused, double *out_4037525697391222133); +void car_h_26(double *state, double *unused, double *out_8434552756934301720); +void car_H_26(double *state, double *unused, double *out_7649690069122038287); +void car_h_27(double *state, double *unused, double *out_3941271621753372378); +void car_H_27(double *state, double *unused, double *out_6212289009191647044); +void car_h_29(double *state, double *unused, double *out_7578721150007394728); +void car_H_29(double *state, double *unused, double *out_7925651736061198077); +void car_h_28(double *state, double *unused, double *out_2997761768851709943); +void car_H_28(double *state, double *unused, double *out_5438693320578822965); +void car_h_31(double *state, double *unused, double *out_390229746721409900); +void car_H_31(double *state, double *unused, double *out_7523173996703673156); 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/openpilot/selfdrive/locationd/models/generated/libcar.so b/openpilot/selfdrive/locationd/models/generated/libcar.so index 4670561662..6f572f78b9 100755 Binary files a/openpilot/selfdrive/locationd/models/generated/libcar.so and b/openpilot/selfdrive/locationd/models/generated/libcar.so differ diff --git a/openpilot/selfdrive/locationd/models/generated/libpose.so b/openpilot/selfdrive/locationd/models/generated/libpose.so index e410861817..04ec1feea5 100755 Binary files a/openpilot/selfdrive/locationd/models/generated/libpose.so and b/openpilot/selfdrive/locationd/models/generated/libpose.so differ diff --git a/openpilot/selfdrive/locationd/models/generated/pose.cpp b/openpilot/selfdrive/locationd/models/generated/pose.cpp index e39761059f..d020745d2a 100644 --- a/openpilot/selfdrive/locationd/models/generated/pose.cpp +++ b/openpilot/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_3094993718265680010) { - out_3094993718265680010[0] = delta_x[0] + nom_x[0]; - out_3094993718265680010[1] = delta_x[1] + nom_x[1]; - out_3094993718265680010[2] = delta_x[2] + nom_x[2]; - out_3094993718265680010[3] = delta_x[3] + nom_x[3]; - out_3094993718265680010[4] = delta_x[4] + nom_x[4]; - out_3094993718265680010[5] = delta_x[5] + nom_x[5]; - out_3094993718265680010[6] = delta_x[6] + nom_x[6]; - out_3094993718265680010[7] = delta_x[7] + nom_x[7]; - out_3094993718265680010[8] = delta_x[8] + nom_x[8]; - out_3094993718265680010[9] = delta_x[9] + nom_x[9]; - out_3094993718265680010[10] = delta_x[10] + nom_x[10]; - out_3094993718265680010[11] = delta_x[11] + nom_x[11]; - out_3094993718265680010[12] = delta_x[12] + nom_x[12]; - out_3094993718265680010[13] = delta_x[13] + nom_x[13]; - out_3094993718265680010[14] = delta_x[14] + nom_x[14]; - out_3094993718265680010[15] = delta_x[15] + nom_x[15]; - out_3094993718265680010[16] = delta_x[16] + nom_x[16]; - out_3094993718265680010[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_1002597543094682529) { + out_1002597543094682529[0] = delta_x[0] + nom_x[0]; + out_1002597543094682529[1] = delta_x[1] + nom_x[1]; + out_1002597543094682529[2] = delta_x[2] + nom_x[2]; + out_1002597543094682529[3] = delta_x[3] + nom_x[3]; + out_1002597543094682529[4] = delta_x[4] + nom_x[4]; + out_1002597543094682529[5] = delta_x[5] + nom_x[5]; + out_1002597543094682529[6] = delta_x[6] + nom_x[6]; + out_1002597543094682529[7] = delta_x[7] + nom_x[7]; + out_1002597543094682529[8] = delta_x[8] + nom_x[8]; + out_1002597543094682529[9] = delta_x[9] + nom_x[9]; + out_1002597543094682529[10] = delta_x[10] + nom_x[10]; + out_1002597543094682529[11] = delta_x[11] + nom_x[11]; + out_1002597543094682529[12] = delta_x[12] + nom_x[12]; + out_1002597543094682529[13] = delta_x[13] + nom_x[13]; + out_1002597543094682529[14] = delta_x[14] + nom_x[14]; + out_1002597543094682529[15] = delta_x[15] + nom_x[15]; + out_1002597543094682529[16] = delta_x[16] + nom_x[16]; + out_1002597543094682529[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_446507105452282601) { - out_446507105452282601[0] = -nom_x[0] + true_x[0]; - out_446507105452282601[1] = -nom_x[1] + true_x[1]; - out_446507105452282601[2] = -nom_x[2] + true_x[2]; - out_446507105452282601[3] = -nom_x[3] + true_x[3]; - out_446507105452282601[4] = -nom_x[4] + true_x[4]; - out_446507105452282601[5] = -nom_x[5] + true_x[5]; - out_446507105452282601[6] = -nom_x[6] + true_x[6]; - out_446507105452282601[7] = -nom_x[7] + true_x[7]; - out_446507105452282601[8] = -nom_x[8] + true_x[8]; - out_446507105452282601[9] = -nom_x[9] + true_x[9]; - out_446507105452282601[10] = -nom_x[10] + true_x[10]; - out_446507105452282601[11] = -nom_x[11] + true_x[11]; - out_446507105452282601[12] = -nom_x[12] + true_x[12]; - out_446507105452282601[13] = -nom_x[13] + true_x[13]; - out_446507105452282601[14] = -nom_x[14] + true_x[14]; - out_446507105452282601[15] = -nom_x[15] + true_x[15]; - out_446507105452282601[16] = -nom_x[16] + true_x[16]; - out_446507105452282601[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6790221310632921126) { + out_6790221310632921126[0] = -nom_x[0] + true_x[0]; + out_6790221310632921126[1] = -nom_x[1] + true_x[1]; + out_6790221310632921126[2] = -nom_x[2] + true_x[2]; + out_6790221310632921126[3] = -nom_x[3] + true_x[3]; + out_6790221310632921126[4] = -nom_x[4] + true_x[4]; + out_6790221310632921126[5] = -nom_x[5] + true_x[5]; + out_6790221310632921126[6] = -nom_x[6] + true_x[6]; + out_6790221310632921126[7] = -nom_x[7] + true_x[7]; + out_6790221310632921126[8] = -nom_x[8] + true_x[8]; + out_6790221310632921126[9] = -nom_x[9] + true_x[9]; + out_6790221310632921126[10] = -nom_x[10] + true_x[10]; + out_6790221310632921126[11] = -nom_x[11] + true_x[11]; + out_6790221310632921126[12] = -nom_x[12] + true_x[12]; + out_6790221310632921126[13] = -nom_x[13] + true_x[13]; + out_6790221310632921126[14] = -nom_x[14] + true_x[14]; + out_6790221310632921126[15] = -nom_x[15] + true_x[15]; + out_6790221310632921126[16] = -nom_x[16] + true_x[16]; + out_6790221310632921126[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_96002479082188485) { - out_96002479082188485[0] = 1.0; - out_96002479082188485[1] = 0.0; - out_96002479082188485[2] = 0.0; - out_96002479082188485[3] = 0.0; - out_96002479082188485[4] = 0.0; - out_96002479082188485[5] = 0.0; - out_96002479082188485[6] = 0.0; - out_96002479082188485[7] = 0.0; - out_96002479082188485[8] = 0.0; - out_96002479082188485[9] = 0.0; - out_96002479082188485[10] = 0.0; - out_96002479082188485[11] = 0.0; - out_96002479082188485[12] = 0.0; - out_96002479082188485[13] = 0.0; - out_96002479082188485[14] = 0.0; - out_96002479082188485[15] = 0.0; - out_96002479082188485[16] = 0.0; - out_96002479082188485[17] = 0.0; - out_96002479082188485[18] = 0.0; - out_96002479082188485[19] = 1.0; - out_96002479082188485[20] = 0.0; - out_96002479082188485[21] = 0.0; - out_96002479082188485[22] = 0.0; - out_96002479082188485[23] = 0.0; - out_96002479082188485[24] = 0.0; - out_96002479082188485[25] = 0.0; - out_96002479082188485[26] = 0.0; - out_96002479082188485[27] = 0.0; - out_96002479082188485[28] = 0.0; - out_96002479082188485[29] = 0.0; - out_96002479082188485[30] = 0.0; - out_96002479082188485[31] = 0.0; - out_96002479082188485[32] = 0.0; - out_96002479082188485[33] = 0.0; - out_96002479082188485[34] = 0.0; - out_96002479082188485[35] = 0.0; - out_96002479082188485[36] = 0.0; - out_96002479082188485[37] = 0.0; - out_96002479082188485[38] = 1.0; - out_96002479082188485[39] = 0.0; - out_96002479082188485[40] = 0.0; - out_96002479082188485[41] = 0.0; - out_96002479082188485[42] = 0.0; - out_96002479082188485[43] = 0.0; - out_96002479082188485[44] = 0.0; - out_96002479082188485[45] = 0.0; - out_96002479082188485[46] = 0.0; - out_96002479082188485[47] = 0.0; - out_96002479082188485[48] = 0.0; - out_96002479082188485[49] = 0.0; - out_96002479082188485[50] = 0.0; - out_96002479082188485[51] = 0.0; - out_96002479082188485[52] = 0.0; - out_96002479082188485[53] = 0.0; - out_96002479082188485[54] = 0.0; - out_96002479082188485[55] = 0.0; - out_96002479082188485[56] = 0.0; - out_96002479082188485[57] = 1.0; - out_96002479082188485[58] = 0.0; - out_96002479082188485[59] = 0.0; - out_96002479082188485[60] = 0.0; - out_96002479082188485[61] = 0.0; - out_96002479082188485[62] = 0.0; - out_96002479082188485[63] = 0.0; - out_96002479082188485[64] = 0.0; - out_96002479082188485[65] = 0.0; - out_96002479082188485[66] = 0.0; - out_96002479082188485[67] = 0.0; - out_96002479082188485[68] = 0.0; - out_96002479082188485[69] = 0.0; - out_96002479082188485[70] = 0.0; - out_96002479082188485[71] = 0.0; - out_96002479082188485[72] = 0.0; - out_96002479082188485[73] = 0.0; - out_96002479082188485[74] = 0.0; - out_96002479082188485[75] = 0.0; - out_96002479082188485[76] = 1.0; - out_96002479082188485[77] = 0.0; - out_96002479082188485[78] = 0.0; - out_96002479082188485[79] = 0.0; - out_96002479082188485[80] = 0.0; - out_96002479082188485[81] = 0.0; - out_96002479082188485[82] = 0.0; - out_96002479082188485[83] = 0.0; - out_96002479082188485[84] = 0.0; - out_96002479082188485[85] = 0.0; - out_96002479082188485[86] = 0.0; - out_96002479082188485[87] = 0.0; - out_96002479082188485[88] = 0.0; - out_96002479082188485[89] = 0.0; - out_96002479082188485[90] = 0.0; - out_96002479082188485[91] = 0.0; - out_96002479082188485[92] = 0.0; - out_96002479082188485[93] = 0.0; - out_96002479082188485[94] = 0.0; - out_96002479082188485[95] = 1.0; - out_96002479082188485[96] = 0.0; - out_96002479082188485[97] = 0.0; - out_96002479082188485[98] = 0.0; - out_96002479082188485[99] = 0.0; - out_96002479082188485[100] = 0.0; - out_96002479082188485[101] = 0.0; - out_96002479082188485[102] = 0.0; - out_96002479082188485[103] = 0.0; - out_96002479082188485[104] = 0.0; - out_96002479082188485[105] = 0.0; - out_96002479082188485[106] = 0.0; - out_96002479082188485[107] = 0.0; - out_96002479082188485[108] = 0.0; - out_96002479082188485[109] = 0.0; - out_96002479082188485[110] = 0.0; - out_96002479082188485[111] = 0.0; - out_96002479082188485[112] = 0.0; - out_96002479082188485[113] = 0.0; - out_96002479082188485[114] = 1.0; - out_96002479082188485[115] = 0.0; - out_96002479082188485[116] = 0.0; - out_96002479082188485[117] = 0.0; - out_96002479082188485[118] = 0.0; - out_96002479082188485[119] = 0.0; - out_96002479082188485[120] = 0.0; - out_96002479082188485[121] = 0.0; - out_96002479082188485[122] = 0.0; - out_96002479082188485[123] = 0.0; - out_96002479082188485[124] = 0.0; - out_96002479082188485[125] = 0.0; - out_96002479082188485[126] = 0.0; - out_96002479082188485[127] = 0.0; - out_96002479082188485[128] = 0.0; - out_96002479082188485[129] = 0.0; - out_96002479082188485[130] = 0.0; - out_96002479082188485[131] = 0.0; - out_96002479082188485[132] = 0.0; - out_96002479082188485[133] = 1.0; - out_96002479082188485[134] = 0.0; - out_96002479082188485[135] = 0.0; - out_96002479082188485[136] = 0.0; - out_96002479082188485[137] = 0.0; - out_96002479082188485[138] = 0.0; - out_96002479082188485[139] = 0.0; - out_96002479082188485[140] = 0.0; - out_96002479082188485[141] = 0.0; - out_96002479082188485[142] = 0.0; - out_96002479082188485[143] = 0.0; - out_96002479082188485[144] = 0.0; - out_96002479082188485[145] = 0.0; - out_96002479082188485[146] = 0.0; - out_96002479082188485[147] = 0.0; - out_96002479082188485[148] = 0.0; - out_96002479082188485[149] = 0.0; - out_96002479082188485[150] = 0.0; - out_96002479082188485[151] = 0.0; - out_96002479082188485[152] = 1.0; - out_96002479082188485[153] = 0.0; - out_96002479082188485[154] = 0.0; - out_96002479082188485[155] = 0.0; - out_96002479082188485[156] = 0.0; - out_96002479082188485[157] = 0.0; - out_96002479082188485[158] = 0.0; - out_96002479082188485[159] = 0.0; - out_96002479082188485[160] = 0.0; - out_96002479082188485[161] = 0.0; - out_96002479082188485[162] = 0.0; - out_96002479082188485[163] = 0.0; - out_96002479082188485[164] = 0.0; - out_96002479082188485[165] = 0.0; - out_96002479082188485[166] = 0.0; - out_96002479082188485[167] = 0.0; - out_96002479082188485[168] = 0.0; - out_96002479082188485[169] = 0.0; - out_96002479082188485[170] = 0.0; - out_96002479082188485[171] = 1.0; - out_96002479082188485[172] = 0.0; - out_96002479082188485[173] = 0.0; - out_96002479082188485[174] = 0.0; - out_96002479082188485[175] = 0.0; - out_96002479082188485[176] = 0.0; - out_96002479082188485[177] = 0.0; - out_96002479082188485[178] = 0.0; - out_96002479082188485[179] = 0.0; - out_96002479082188485[180] = 0.0; - out_96002479082188485[181] = 0.0; - out_96002479082188485[182] = 0.0; - out_96002479082188485[183] = 0.0; - out_96002479082188485[184] = 0.0; - out_96002479082188485[185] = 0.0; - out_96002479082188485[186] = 0.0; - out_96002479082188485[187] = 0.0; - out_96002479082188485[188] = 0.0; - out_96002479082188485[189] = 0.0; - out_96002479082188485[190] = 1.0; - out_96002479082188485[191] = 0.0; - out_96002479082188485[192] = 0.0; - out_96002479082188485[193] = 0.0; - out_96002479082188485[194] = 0.0; - out_96002479082188485[195] = 0.0; - out_96002479082188485[196] = 0.0; - out_96002479082188485[197] = 0.0; - out_96002479082188485[198] = 0.0; - out_96002479082188485[199] = 0.0; - out_96002479082188485[200] = 0.0; - out_96002479082188485[201] = 0.0; - out_96002479082188485[202] = 0.0; - out_96002479082188485[203] = 0.0; - out_96002479082188485[204] = 0.0; - out_96002479082188485[205] = 0.0; - out_96002479082188485[206] = 0.0; - out_96002479082188485[207] = 0.0; - out_96002479082188485[208] = 0.0; - out_96002479082188485[209] = 1.0; - out_96002479082188485[210] = 0.0; - out_96002479082188485[211] = 0.0; - out_96002479082188485[212] = 0.0; - out_96002479082188485[213] = 0.0; - out_96002479082188485[214] = 0.0; - out_96002479082188485[215] = 0.0; - out_96002479082188485[216] = 0.0; - out_96002479082188485[217] = 0.0; - out_96002479082188485[218] = 0.0; - out_96002479082188485[219] = 0.0; - out_96002479082188485[220] = 0.0; - out_96002479082188485[221] = 0.0; - out_96002479082188485[222] = 0.0; - out_96002479082188485[223] = 0.0; - out_96002479082188485[224] = 0.0; - out_96002479082188485[225] = 0.0; - out_96002479082188485[226] = 0.0; - out_96002479082188485[227] = 0.0; - out_96002479082188485[228] = 1.0; - out_96002479082188485[229] = 0.0; - out_96002479082188485[230] = 0.0; - out_96002479082188485[231] = 0.0; - out_96002479082188485[232] = 0.0; - out_96002479082188485[233] = 0.0; - out_96002479082188485[234] = 0.0; - out_96002479082188485[235] = 0.0; - out_96002479082188485[236] = 0.0; - out_96002479082188485[237] = 0.0; - out_96002479082188485[238] = 0.0; - out_96002479082188485[239] = 0.0; - out_96002479082188485[240] = 0.0; - out_96002479082188485[241] = 0.0; - out_96002479082188485[242] = 0.0; - out_96002479082188485[243] = 0.0; - out_96002479082188485[244] = 0.0; - out_96002479082188485[245] = 0.0; - out_96002479082188485[246] = 0.0; - out_96002479082188485[247] = 1.0; - out_96002479082188485[248] = 0.0; - out_96002479082188485[249] = 0.0; - out_96002479082188485[250] = 0.0; - out_96002479082188485[251] = 0.0; - out_96002479082188485[252] = 0.0; - out_96002479082188485[253] = 0.0; - out_96002479082188485[254] = 0.0; - out_96002479082188485[255] = 0.0; - out_96002479082188485[256] = 0.0; - out_96002479082188485[257] = 0.0; - out_96002479082188485[258] = 0.0; - out_96002479082188485[259] = 0.0; - out_96002479082188485[260] = 0.0; - out_96002479082188485[261] = 0.0; - out_96002479082188485[262] = 0.0; - out_96002479082188485[263] = 0.0; - out_96002479082188485[264] = 0.0; - out_96002479082188485[265] = 0.0; - out_96002479082188485[266] = 1.0; - out_96002479082188485[267] = 0.0; - out_96002479082188485[268] = 0.0; - out_96002479082188485[269] = 0.0; - out_96002479082188485[270] = 0.0; - out_96002479082188485[271] = 0.0; - out_96002479082188485[272] = 0.0; - out_96002479082188485[273] = 0.0; - out_96002479082188485[274] = 0.0; - out_96002479082188485[275] = 0.0; - out_96002479082188485[276] = 0.0; - out_96002479082188485[277] = 0.0; - out_96002479082188485[278] = 0.0; - out_96002479082188485[279] = 0.0; - out_96002479082188485[280] = 0.0; - out_96002479082188485[281] = 0.0; - out_96002479082188485[282] = 0.0; - out_96002479082188485[283] = 0.0; - out_96002479082188485[284] = 0.0; - out_96002479082188485[285] = 1.0; - out_96002479082188485[286] = 0.0; - out_96002479082188485[287] = 0.0; - out_96002479082188485[288] = 0.0; - out_96002479082188485[289] = 0.0; - out_96002479082188485[290] = 0.0; - out_96002479082188485[291] = 0.0; - out_96002479082188485[292] = 0.0; - out_96002479082188485[293] = 0.0; - out_96002479082188485[294] = 0.0; - out_96002479082188485[295] = 0.0; - out_96002479082188485[296] = 0.0; - out_96002479082188485[297] = 0.0; - out_96002479082188485[298] = 0.0; - out_96002479082188485[299] = 0.0; - out_96002479082188485[300] = 0.0; - out_96002479082188485[301] = 0.0; - out_96002479082188485[302] = 0.0; - out_96002479082188485[303] = 0.0; - out_96002479082188485[304] = 1.0; - out_96002479082188485[305] = 0.0; - out_96002479082188485[306] = 0.0; - out_96002479082188485[307] = 0.0; - out_96002479082188485[308] = 0.0; - out_96002479082188485[309] = 0.0; - out_96002479082188485[310] = 0.0; - out_96002479082188485[311] = 0.0; - out_96002479082188485[312] = 0.0; - out_96002479082188485[313] = 0.0; - out_96002479082188485[314] = 0.0; - out_96002479082188485[315] = 0.0; - out_96002479082188485[316] = 0.0; - out_96002479082188485[317] = 0.0; - out_96002479082188485[318] = 0.0; - out_96002479082188485[319] = 0.0; - out_96002479082188485[320] = 0.0; - out_96002479082188485[321] = 0.0; - out_96002479082188485[322] = 0.0; - out_96002479082188485[323] = 1.0; +void H_mod_fun(double *state, double *out_1061940044083541431) { + out_1061940044083541431[0] = 1.0; + out_1061940044083541431[1] = 0.0; + out_1061940044083541431[2] = 0.0; + out_1061940044083541431[3] = 0.0; + out_1061940044083541431[4] = 0.0; + out_1061940044083541431[5] = 0.0; + out_1061940044083541431[6] = 0.0; + out_1061940044083541431[7] = 0.0; + out_1061940044083541431[8] = 0.0; + out_1061940044083541431[9] = 0.0; + out_1061940044083541431[10] = 0.0; + out_1061940044083541431[11] = 0.0; + out_1061940044083541431[12] = 0.0; + out_1061940044083541431[13] = 0.0; + out_1061940044083541431[14] = 0.0; + out_1061940044083541431[15] = 0.0; + out_1061940044083541431[16] = 0.0; + out_1061940044083541431[17] = 0.0; + out_1061940044083541431[18] = 0.0; + out_1061940044083541431[19] = 1.0; + out_1061940044083541431[20] = 0.0; + out_1061940044083541431[21] = 0.0; + out_1061940044083541431[22] = 0.0; + out_1061940044083541431[23] = 0.0; + out_1061940044083541431[24] = 0.0; + out_1061940044083541431[25] = 0.0; + out_1061940044083541431[26] = 0.0; + out_1061940044083541431[27] = 0.0; + out_1061940044083541431[28] = 0.0; + out_1061940044083541431[29] = 0.0; + out_1061940044083541431[30] = 0.0; + out_1061940044083541431[31] = 0.0; + out_1061940044083541431[32] = 0.0; + out_1061940044083541431[33] = 0.0; + out_1061940044083541431[34] = 0.0; + out_1061940044083541431[35] = 0.0; + out_1061940044083541431[36] = 0.0; + out_1061940044083541431[37] = 0.0; + out_1061940044083541431[38] = 1.0; + out_1061940044083541431[39] = 0.0; + out_1061940044083541431[40] = 0.0; + out_1061940044083541431[41] = 0.0; + out_1061940044083541431[42] = 0.0; + out_1061940044083541431[43] = 0.0; + out_1061940044083541431[44] = 0.0; + out_1061940044083541431[45] = 0.0; + out_1061940044083541431[46] = 0.0; + out_1061940044083541431[47] = 0.0; + out_1061940044083541431[48] = 0.0; + out_1061940044083541431[49] = 0.0; + out_1061940044083541431[50] = 0.0; + out_1061940044083541431[51] = 0.0; + out_1061940044083541431[52] = 0.0; + out_1061940044083541431[53] = 0.0; + out_1061940044083541431[54] = 0.0; + out_1061940044083541431[55] = 0.0; + out_1061940044083541431[56] = 0.0; + out_1061940044083541431[57] = 1.0; + out_1061940044083541431[58] = 0.0; + out_1061940044083541431[59] = 0.0; + out_1061940044083541431[60] = 0.0; + out_1061940044083541431[61] = 0.0; + out_1061940044083541431[62] = 0.0; + out_1061940044083541431[63] = 0.0; + out_1061940044083541431[64] = 0.0; + out_1061940044083541431[65] = 0.0; + out_1061940044083541431[66] = 0.0; + out_1061940044083541431[67] = 0.0; + out_1061940044083541431[68] = 0.0; + out_1061940044083541431[69] = 0.0; + out_1061940044083541431[70] = 0.0; + out_1061940044083541431[71] = 0.0; + out_1061940044083541431[72] = 0.0; + out_1061940044083541431[73] = 0.0; + out_1061940044083541431[74] = 0.0; + out_1061940044083541431[75] = 0.0; + out_1061940044083541431[76] = 1.0; + out_1061940044083541431[77] = 0.0; + out_1061940044083541431[78] = 0.0; + out_1061940044083541431[79] = 0.0; + out_1061940044083541431[80] = 0.0; + out_1061940044083541431[81] = 0.0; + out_1061940044083541431[82] = 0.0; + out_1061940044083541431[83] = 0.0; + out_1061940044083541431[84] = 0.0; + out_1061940044083541431[85] = 0.0; + out_1061940044083541431[86] = 0.0; + out_1061940044083541431[87] = 0.0; + out_1061940044083541431[88] = 0.0; + out_1061940044083541431[89] = 0.0; + out_1061940044083541431[90] = 0.0; + out_1061940044083541431[91] = 0.0; + out_1061940044083541431[92] = 0.0; + out_1061940044083541431[93] = 0.0; + out_1061940044083541431[94] = 0.0; + out_1061940044083541431[95] = 1.0; + out_1061940044083541431[96] = 0.0; + out_1061940044083541431[97] = 0.0; + out_1061940044083541431[98] = 0.0; + out_1061940044083541431[99] = 0.0; + out_1061940044083541431[100] = 0.0; + out_1061940044083541431[101] = 0.0; + out_1061940044083541431[102] = 0.0; + out_1061940044083541431[103] = 0.0; + out_1061940044083541431[104] = 0.0; + out_1061940044083541431[105] = 0.0; + out_1061940044083541431[106] = 0.0; + out_1061940044083541431[107] = 0.0; + out_1061940044083541431[108] = 0.0; + out_1061940044083541431[109] = 0.0; + out_1061940044083541431[110] = 0.0; + out_1061940044083541431[111] = 0.0; + out_1061940044083541431[112] = 0.0; + out_1061940044083541431[113] = 0.0; + out_1061940044083541431[114] = 1.0; + out_1061940044083541431[115] = 0.0; + out_1061940044083541431[116] = 0.0; + out_1061940044083541431[117] = 0.0; + out_1061940044083541431[118] = 0.0; + out_1061940044083541431[119] = 0.0; + out_1061940044083541431[120] = 0.0; + out_1061940044083541431[121] = 0.0; + out_1061940044083541431[122] = 0.0; + out_1061940044083541431[123] = 0.0; + out_1061940044083541431[124] = 0.0; + out_1061940044083541431[125] = 0.0; + out_1061940044083541431[126] = 0.0; + out_1061940044083541431[127] = 0.0; + out_1061940044083541431[128] = 0.0; + out_1061940044083541431[129] = 0.0; + out_1061940044083541431[130] = 0.0; + out_1061940044083541431[131] = 0.0; + out_1061940044083541431[132] = 0.0; + out_1061940044083541431[133] = 1.0; + out_1061940044083541431[134] = 0.0; + out_1061940044083541431[135] = 0.0; + out_1061940044083541431[136] = 0.0; + out_1061940044083541431[137] = 0.0; + out_1061940044083541431[138] = 0.0; + out_1061940044083541431[139] = 0.0; + out_1061940044083541431[140] = 0.0; + out_1061940044083541431[141] = 0.0; + out_1061940044083541431[142] = 0.0; + out_1061940044083541431[143] = 0.0; + out_1061940044083541431[144] = 0.0; + out_1061940044083541431[145] = 0.0; + out_1061940044083541431[146] = 0.0; + out_1061940044083541431[147] = 0.0; + out_1061940044083541431[148] = 0.0; + out_1061940044083541431[149] = 0.0; + out_1061940044083541431[150] = 0.0; + out_1061940044083541431[151] = 0.0; + out_1061940044083541431[152] = 1.0; + out_1061940044083541431[153] = 0.0; + out_1061940044083541431[154] = 0.0; + out_1061940044083541431[155] = 0.0; + out_1061940044083541431[156] = 0.0; + out_1061940044083541431[157] = 0.0; + out_1061940044083541431[158] = 0.0; + out_1061940044083541431[159] = 0.0; + out_1061940044083541431[160] = 0.0; + out_1061940044083541431[161] = 0.0; + out_1061940044083541431[162] = 0.0; + out_1061940044083541431[163] = 0.0; + out_1061940044083541431[164] = 0.0; + out_1061940044083541431[165] = 0.0; + out_1061940044083541431[166] = 0.0; + out_1061940044083541431[167] = 0.0; + out_1061940044083541431[168] = 0.0; + out_1061940044083541431[169] = 0.0; + out_1061940044083541431[170] = 0.0; + out_1061940044083541431[171] = 1.0; + out_1061940044083541431[172] = 0.0; + out_1061940044083541431[173] = 0.0; + out_1061940044083541431[174] = 0.0; + out_1061940044083541431[175] = 0.0; + out_1061940044083541431[176] = 0.0; + out_1061940044083541431[177] = 0.0; + out_1061940044083541431[178] = 0.0; + out_1061940044083541431[179] = 0.0; + out_1061940044083541431[180] = 0.0; + out_1061940044083541431[181] = 0.0; + out_1061940044083541431[182] = 0.0; + out_1061940044083541431[183] = 0.0; + out_1061940044083541431[184] = 0.0; + out_1061940044083541431[185] = 0.0; + out_1061940044083541431[186] = 0.0; + out_1061940044083541431[187] = 0.0; + out_1061940044083541431[188] = 0.0; + out_1061940044083541431[189] = 0.0; + out_1061940044083541431[190] = 1.0; + out_1061940044083541431[191] = 0.0; + out_1061940044083541431[192] = 0.0; + out_1061940044083541431[193] = 0.0; + out_1061940044083541431[194] = 0.0; + out_1061940044083541431[195] = 0.0; + out_1061940044083541431[196] = 0.0; + out_1061940044083541431[197] = 0.0; + out_1061940044083541431[198] = 0.0; + out_1061940044083541431[199] = 0.0; + out_1061940044083541431[200] = 0.0; + out_1061940044083541431[201] = 0.0; + out_1061940044083541431[202] = 0.0; + out_1061940044083541431[203] = 0.0; + out_1061940044083541431[204] = 0.0; + out_1061940044083541431[205] = 0.0; + out_1061940044083541431[206] = 0.0; + out_1061940044083541431[207] = 0.0; + out_1061940044083541431[208] = 0.0; + out_1061940044083541431[209] = 1.0; + out_1061940044083541431[210] = 0.0; + out_1061940044083541431[211] = 0.0; + out_1061940044083541431[212] = 0.0; + out_1061940044083541431[213] = 0.0; + out_1061940044083541431[214] = 0.0; + out_1061940044083541431[215] = 0.0; + out_1061940044083541431[216] = 0.0; + out_1061940044083541431[217] = 0.0; + out_1061940044083541431[218] = 0.0; + out_1061940044083541431[219] = 0.0; + out_1061940044083541431[220] = 0.0; + out_1061940044083541431[221] = 0.0; + out_1061940044083541431[222] = 0.0; + out_1061940044083541431[223] = 0.0; + out_1061940044083541431[224] = 0.0; + out_1061940044083541431[225] = 0.0; + out_1061940044083541431[226] = 0.0; + out_1061940044083541431[227] = 0.0; + out_1061940044083541431[228] = 1.0; + out_1061940044083541431[229] = 0.0; + out_1061940044083541431[230] = 0.0; + out_1061940044083541431[231] = 0.0; + out_1061940044083541431[232] = 0.0; + out_1061940044083541431[233] = 0.0; + out_1061940044083541431[234] = 0.0; + out_1061940044083541431[235] = 0.0; + out_1061940044083541431[236] = 0.0; + out_1061940044083541431[237] = 0.0; + out_1061940044083541431[238] = 0.0; + out_1061940044083541431[239] = 0.0; + out_1061940044083541431[240] = 0.0; + out_1061940044083541431[241] = 0.0; + out_1061940044083541431[242] = 0.0; + out_1061940044083541431[243] = 0.0; + out_1061940044083541431[244] = 0.0; + out_1061940044083541431[245] = 0.0; + out_1061940044083541431[246] = 0.0; + out_1061940044083541431[247] = 1.0; + out_1061940044083541431[248] = 0.0; + out_1061940044083541431[249] = 0.0; + out_1061940044083541431[250] = 0.0; + out_1061940044083541431[251] = 0.0; + out_1061940044083541431[252] = 0.0; + out_1061940044083541431[253] = 0.0; + out_1061940044083541431[254] = 0.0; + out_1061940044083541431[255] = 0.0; + out_1061940044083541431[256] = 0.0; + out_1061940044083541431[257] = 0.0; + out_1061940044083541431[258] = 0.0; + out_1061940044083541431[259] = 0.0; + out_1061940044083541431[260] = 0.0; + out_1061940044083541431[261] = 0.0; + out_1061940044083541431[262] = 0.0; + out_1061940044083541431[263] = 0.0; + out_1061940044083541431[264] = 0.0; + out_1061940044083541431[265] = 0.0; + out_1061940044083541431[266] = 1.0; + out_1061940044083541431[267] = 0.0; + out_1061940044083541431[268] = 0.0; + out_1061940044083541431[269] = 0.0; + out_1061940044083541431[270] = 0.0; + out_1061940044083541431[271] = 0.0; + out_1061940044083541431[272] = 0.0; + out_1061940044083541431[273] = 0.0; + out_1061940044083541431[274] = 0.0; + out_1061940044083541431[275] = 0.0; + out_1061940044083541431[276] = 0.0; + out_1061940044083541431[277] = 0.0; + out_1061940044083541431[278] = 0.0; + out_1061940044083541431[279] = 0.0; + out_1061940044083541431[280] = 0.0; + out_1061940044083541431[281] = 0.0; + out_1061940044083541431[282] = 0.0; + out_1061940044083541431[283] = 0.0; + out_1061940044083541431[284] = 0.0; + out_1061940044083541431[285] = 1.0; + out_1061940044083541431[286] = 0.0; + out_1061940044083541431[287] = 0.0; + out_1061940044083541431[288] = 0.0; + out_1061940044083541431[289] = 0.0; + out_1061940044083541431[290] = 0.0; + out_1061940044083541431[291] = 0.0; + out_1061940044083541431[292] = 0.0; + out_1061940044083541431[293] = 0.0; + out_1061940044083541431[294] = 0.0; + out_1061940044083541431[295] = 0.0; + out_1061940044083541431[296] = 0.0; + out_1061940044083541431[297] = 0.0; + out_1061940044083541431[298] = 0.0; + out_1061940044083541431[299] = 0.0; + out_1061940044083541431[300] = 0.0; + out_1061940044083541431[301] = 0.0; + out_1061940044083541431[302] = 0.0; + out_1061940044083541431[303] = 0.0; + out_1061940044083541431[304] = 1.0; + out_1061940044083541431[305] = 0.0; + out_1061940044083541431[306] = 0.0; + out_1061940044083541431[307] = 0.0; + out_1061940044083541431[308] = 0.0; + out_1061940044083541431[309] = 0.0; + out_1061940044083541431[310] = 0.0; + out_1061940044083541431[311] = 0.0; + out_1061940044083541431[312] = 0.0; + out_1061940044083541431[313] = 0.0; + out_1061940044083541431[314] = 0.0; + out_1061940044083541431[315] = 0.0; + out_1061940044083541431[316] = 0.0; + out_1061940044083541431[317] = 0.0; + out_1061940044083541431[318] = 0.0; + out_1061940044083541431[319] = 0.0; + out_1061940044083541431[320] = 0.0; + out_1061940044083541431[321] = 0.0; + out_1061940044083541431[322] = 0.0; + out_1061940044083541431[323] = 1.0; } -void f_fun(double *state, double dt, double *out_8888553933023554192) { - out_8888553933023554192[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_8888553933023554192[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_8888553933023554192[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_8888553933023554192[3] = dt*state[12] + state[3]; - out_8888553933023554192[4] = dt*state[13] + state[4]; - out_8888553933023554192[5] = dt*state[14] + state[5]; - out_8888553933023554192[6] = state[6]; - out_8888553933023554192[7] = state[7]; - out_8888553933023554192[8] = state[8]; - out_8888553933023554192[9] = state[9]; - out_8888553933023554192[10] = state[10]; - out_8888553933023554192[11] = state[11]; - out_8888553933023554192[12] = state[12]; - out_8888553933023554192[13] = state[13]; - out_8888553933023554192[14] = state[14]; - out_8888553933023554192[15] = state[15]; - out_8888553933023554192[16] = state[16]; - out_8888553933023554192[17] = state[17]; +void f_fun(double *state, double dt, double *out_3240563005197080216) { + out_3240563005197080216[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_3240563005197080216[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_3240563005197080216[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_3240563005197080216[3] = dt*state[12] + state[3]; + out_3240563005197080216[4] = dt*state[13] + state[4]; + out_3240563005197080216[5] = dt*state[14] + state[5]; + out_3240563005197080216[6] = state[6]; + out_3240563005197080216[7] = state[7]; + out_3240563005197080216[8] = state[8]; + out_3240563005197080216[9] = state[9]; + out_3240563005197080216[10] = state[10]; + out_3240563005197080216[11] = state[11]; + out_3240563005197080216[12] = state[12]; + out_3240563005197080216[13] = state[13]; + out_3240563005197080216[14] = state[14]; + out_3240563005197080216[15] = state[15]; + out_3240563005197080216[16] = state[16]; + out_3240563005197080216[17] = state[17]; } -void F_fun(double *state, double dt, double *out_3793856272932959588) { - out_3793856272932959588[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_3793856272932959588[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_3793856272932959588[2] = 0; - out_3793856272932959588[3] = 0; - out_3793856272932959588[4] = 0; - out_3793856272932959588[5] = 0; - out_3793856272932959588[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_3793856272932959588[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_3793856272932959588[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_3793856272932959588[9] = 0; - out_3793856272932959588[10] = 0; - out_3793856272932959588[11] = 0; - out_3793856272932959588[12] = 0; - out_3793856272932959588[13] = 0; - out_3793856272932959588[14] = 0; - out_3793856272932959588[15] = 0; - out_3793856272932959588[16] = 0; - out_3793856272932959588[17] = 0; - out_3793856272932959588[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_3793856272932959588[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_3793856272932959588[20] = 0; - out_3793856272932959588[21] = 0; - out_3793856272932959588[22] = 0; - out_3793856272932959588[23] = 0; - out_3793856272932959588[24] = 0; - out_3793856272932959588[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_3793856272932959588[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_3793856272932959588[27] = 0; - out_3793856272932959588[28] = 0; - out_3793856272932959588[29] = 0; - out_3793856272932959588[30] = 0; - out_3793856272932959588[31] = 0; - out_3793856272932959588[32] = 0; - out_3793856272932959588[33] = 0; - out_3793856272932959588[34] = 0; - out_3793856272932959588[35] = 0; - out_3793856272932959588[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_3793856272932959588[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_3793856272932959588[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_3793856272932959588[39] = 0; - out_3793856272932959588[40] = 0; - out_3793856272932959588[41] = 0; - out_3793856272932959588[42] = 0; - out_3793856272932959588[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_3793856272932959588[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_3793856272932959588[45] = 0; - out_3793856272932959588[46] = 0; - out_3793856272932959588[47] = 0; - out_3793856272932959588[48] = 0; - out_3793856272932959588[49] = 0; - out_3793856272932959588[50] = 0; - out_3793856272932959588[51] = 0; - out_3793856272932959588[52] = 0; - out_3793856272932959588[53] = 0; - out_3793856272932959588[54] = 0; - out_3793856272932959588[55] = 0; - out_3793856272932959588[56] = 0; - out_3793856272932959588[57] = 1; - out_3793856272932959588[58] = 0; - out_3793856272932959588[59] = 0; - out_3793856272932959588[60] = 0; - out_3793856272932959588[61] = 0; - out_3793856272932959588[62] = 0; - out_3793856272932959588[63] = 0; - out_3793856272932959588[64] = 0; - out_3793856272932959588[65] = 0; - out_3793856272932959588[66] = dt; - out_3793856272932959588[67] = 0; - out_3793856272932959588[68] = 0; - out_3793856272932959588[69] = 0; - out_3793856272932959588[70] = 0; - out_3793856272932959588[71] = 0; - out_3793856272932959588[72] = 0; - out_3793856272932959588[73] = 0; - out_3793856272932959588[74] = 0; - out_3793856272932959588[75] = 0; - out_3793856272932959588[76] = 1; - out_3793856272932959588[77] = 0; - out_3793856272932959588[78] = 0; - out_3793856272932959588[79] = 0; - out_3793856272932959588[80] = 0; - out_3793856272932959588[81] = 0; - out_3793856272932959588[82] = 0; - out_3793856272932959588[83] = 0; - out_3793856272932959588[84] = 0; - out_3793856272932959588[85] = dt; - out_3793856272932959588[86] = 0; - out_3793856272932959588[87] = 0; - out_3793856272932959588[88] = 0; - out_3793856272932959588[89] = 0; - out_3793856272932959588[90] = 0; - out_3793856272932959588[91] = 0; - out_3793856272932959588[92] = 0; - out_3793856272932959588[93] = 0; - out_3793856272932959588[94] = 0; - out_3793856272932959588[95] = 1; - out_3793856272932959588[96] = 0; - out_3793856272932959588[97] = 0; - out_3793856272932959588[98] = 0; - out_3793856272932959588[99] = 0; - out_3793856272932959588[100] = 0; - out_3793856272932959588[101] = 0; - out_3793856272932959588[102] = 0; - out_3793856272932959588[103] = 0; - out_3793856272932959588[104] = dt; - out_3793856272932959588[105] = 0; - out_3793856272932959588[106] = 0; - out_3793856272932959588[107] = 0; - out_3793856272932959588[108] = 0; - out_3793856272932959588[109] = 0; - out_3793856272932959588[110] = 0; - out_3793856272932959588[111] = 0; - out_3793856272932959588[112] = 0; - out_3793856272932959588[113] = 0; - out_3793856272932959588[114] = 1; - out_3793856272932959588[115] = 0; - out_3793856272932959588[116] = 0; - out_3793856272932959588[117] = 0; - out_3793856272932959588[118] = 0; - out_3793856272932959588[119] = 0; - out_3793856272932959588[120] = 0; - out_3793856272932959588[121] = 0; - out_3793856272932959588[122] = 0; - out_3793856272932959588[123] = 0; - out_3793856272932959588[124] = 0; - out_3793856272932959588[125] = 0; - out_3793856272932959588[126] = 0; - out_3793856272932959588[127] = 0; - out_3793856272932959588[128] = 0; - out_3793856272932959588[129] = 0; - out_3793856272932959588[130] = 0; - out_3793856272932959588[131] = 0; - out_3793856272932959588[132] = 0; - out_3793856272932959588[133] = 1; - out_3793856272932959588[134] = 0; - out_3793856272932959588[135] = 0; - out_3793856272932959588[136] = 0; - out_3793856272932959588[137] = 0; - out_3793856272932959588[138] = 0; - out_3793856272932959588[139] = 0; - out_3793856272932959588[140] = 0; - out_3793856272932959588[141] = 0; - out_3793856272932959588[142] = 0; - out_3793856272932959588[143] = 0; - out_3793856272932959588[144] = 0; - out_3793856272932959588[145] = 0; - out_3793856272932959588[146] = 0; - out_3793856272932959588[147] = 0; - out_3793856272932959588[148] = 0; - out_3793856272932959588[149] = 0; - out_3793856272932959588[150] = 0; - out_3793856272932959588[151] = 0; - out_3793856272932959588[152] = 1; - out_3793856272932959588[153] = 0; - out_3793856272932959588[154] = 0; - out_3793856272932959588[155] = 0; - out_3793856272932959588[156] = 0; - out_3793856272932959588[157] = 0; - out_3793856272932959588[158] = 0; - out_3793856272932959588[159] = 0; - out_3793856272932959588[160] = 0; - out_3793856272932959588[161] = 0; - out_3793856272932959588[162] = 0; - out_3793856272932959588[163] = 0; - out_3793856272932959588[164] = 0; - out_3793856272932959588[165] = 0; - out_3793856272932959588[166] = 0; - out_3793856272932959588[167] = 0; - out_3793856272932959588[168] = 0; - out_3793856272932959588[169] = 0; - out_3793856272932959588[170] = 0; - out_3793856272932959588[171] = 1; - out_3793856272932959588[172] = 0; - out_3793856272932959588[173] = 0; - out_3793856272932959588[174] = 0; - out_3793856272932959588[175] = 0; - out_3793856272932959588[176] = 0; - out_3793856272932959588[177] = 0; - out_3793856272932959588[178] = 0; - out_3793856272932959588[179] = 0; - out_3793856272932959588[180] = 0; - out_3793856272932959588[181] = 0; - out_3793856272932959588[182] = 0; - out_3793856272932959588[183] = 0; - out_3793856272932959588[184] = 0; - out_3793856272932959588[185] = 0; - out_3793856272932959588[186] = 0; - out_3793856272932959588[187] = 0; - out_3793856272932959588[188] = 0; - out_3793856272932959588[189] = 0; - out_3793856272932959588[190] = 1; - out_3793856272932959588[191] = 0; - out_3793856272932959588[192] = 0; - out_3793856272932959588[193] = 0; - out_3793856272932959588[194] = 0; - out_3793856272932959588[195] = 0; - out_3793856272932959588[196] = 0; - out_3793856272932959588[197] = 0; - out_3793856272932959588[198] = 0; - out_3793856272932959588[199] = 0; - out_3793856272932959588[200] = 0; - out_3793856272932959588[201] = 0; - out_3793856272932959588[202] = 0; - out_3793856272932959588[203] = 0; - out_3793856272932959588[204] = 0; - out_3793856272932959588[205] = 0; - out_3793856272932959588[206] = 0; - out_3793856272932959588[207] = 0; - out_3793856272932959588[208] = 0; - out_3793856272932959588[209] = 1; - out_3793856272932959588[210] = 0; - out_3793856272932959588[211] = 0; - out_3793856272932959588[212] = 0; - out_3793856272932959588[213] = 0; - out_3793856272932959588[214] = 0; - out_3793856272932959588[215] = 0; - out_3793856272932959588[216] = 0; - out_3793856272932959588[217] = 0; - out_3793856272932959588[218] = 0; - out_3793856272932959588[219] = 0; - out_3793856272932959588[220] = 0; - out_3793856272932959588[221] = 0; - out_3793856272932959588[222] = 0; - out_3793856272932959588[223] = 0; - out_3793856272932959588[224] = 0; - out_3793856272932959588[225] = 0; - out_3793856272932959588[226] = 0; - out_3793856272932959588[227] = 0; - out_3793856272932959588[228] = 1; - out_3793856272932959588[229] = 0; - out_3793856272932959588[230] = 0; - out_3793856272932959588[231] = 0; - out_3793856272932959588[232] = 0; - out_3793856272932959588[233] = 0; - out_3793856272932959588[234] = 0; - out_3793856272932959588[235] = 0; - out_3793856272932959588[236] = 0; - out_3793856272932959588[237] = 0; - out_3793856272932959588[238] = 0; - out_3793856272932959588[239] = 0; - out_3793856272932959588[240] = 0; - out_3793856272932959588[241] = 0; - out_3793856272932959588[242] = 0; - out_3793856272932959588[243] = 0; - out_3793856272932959588[244] = 0; - out_3793856272932959588[245] = 0; - out_3793856272932959588[246] = 0; - out_3793856272932959588[247] = 1; - out_3793856272932959588[248] = 0; - out_3793856272932959588[249] = 0; - out_3793856272932959588[250] = 0; - out_3793856272932959588[251] = 0; - out_3793856272932959588[252] = 0; - out_3793856272932959588[253] = 0; - out_3793856272932959588[254] = 0; - out_3793856272932959588[255] = 0; - out_3793856272932959588[256] = 0; - out_3793856272932959588[257] = 0; - out_3793856272932959588[258] = 0; - out_3793856272932959588[259] = 0; - out_3793856272932959588[260] = 0; - out_3793856272932959588[261] = 0; - out_3793856272932959588[262] = 0; - out_3793856272932959588[263] = 0; - out_3793856272932959588[264] = 0; - out_3793856272932959588[265] = 0; - out_3793856272932959588[266] = 1; - out_3793856272932959588[267] = 0; - out_3793856272932959588[268] = 0; - out_3793856272932959588[269] = 0; - out_3793856272932959588[270] = 0; - out_3793856272932959588[271] = 0; - out_3793856272932959588[272] = 0; - out_3793856272932959588[273] = 0; - out_3793856272932959588[274] = 0; - out_3793856272932959588[275] = 0; - out_3793856272932959588[276] = 0; - out_3793856272932959588[277] = 0; - out_3793856272932959588[278] = 0; - out_3793856272932959588[279] = 0; - out_3793856272932959588[280] = 0; - out_3793856272932959588[281] = 0; - out_3793856272932959588[282] = 0; - out_3793856272932959588[283] = 0; - out_3793856272932959588[284] = 0; - out_3793856272932959588[285] = 1; - out_3793856272932959588[286] = 0; - out_3793856272932959588[287] = 0; - out_3793856272932959588[288] = 0; - out_3793856272932959588[289] = 0; - out_3793856272932959588[290] = 0; - out_3793856272932959588[291] = 0; - out_3793856272932959588[292] = 0; - out_3793856272932959588[293] = 0; - out_3793856272932959588[294] = 0; - out_3793856272932959588[295] = 0; - out_3793856272932959588[296] = 0; - out_3793856272932959588[297] = 0; - out_3793856272932959588[298] = 0; - out_3793856272932959588[299] = 0; - out_3793856272932959588[300] = 0; - out_3793856272932959588[301] = 0; - out_3793856272932959588[302] = 0; - out_3793856272932959588[303] = 0; - out_3793856272932959588[304] = 1; - out_3793856272932959588[305] = 0; - out_3793856272932959588[306] = 0; - out_3793856272932959588[307] = 0; - out_3793856272932959588[308] = 0; - out_3793856272932959588[309] = 0; - out_3793856272932959588[310] = 0; - out_3793856272932959588[311] = 0; - out_3793856272932959588[312] = 0; - out_3793856272932959588[313] = 0; - out_3793856272932959588[314] = 0; - out_3793856272932959588[315] = 0; - out_3793856272932959588[316] = 0; - out_3793856272932959588[317] = 0; - out_3793856272932959588[318] = 0; - out_3793856272932959588[319] = 0; - out_3793856272932959588[320] = 0; - out_3793856272932959588[321] = 0; - out_3793856272932959588[322] = 0; - out_3793856272932959588[323] = 1; +void F_fun(double *state, double dt, double *out_5982976038096974601) { + out_5982976038096974601[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_5982976038096974601[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_5982976038096974601[2] = 0; + out_5982976038096974601[3] = 0; + out_5982976038096974601[4] = 0; + out_5982976038096974601[5] = 0; + out_5982976038096974601[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_5982976038096974601[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_5982976038096974601[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_5982976038096974601[9] = 0; + out_5982976038096974601[10] = 0; + out_5982976038096974601[11] = 0; + out_5982976038096974601[12] = 0; + out_5982976038096974601[13] = 0; + out_5982976038096974601[14] = 0; + out_5982976038096974601[15] = 0; + out_5982976038096974601[16] = 0; + out_5982976038096974601[17] = 0; + out_5982976038096974601[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_5982976038096974601[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_5982976038096974601[20] = 0; + out_5982976038096974601[21] = 0; + out_5982976038096974601[22] = 0; + out_5982976038096974601[23] = 0; + out_5982976038096974601[24] = 0; + out_5982976038096974601[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_5982976038096974601[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_5982976038096974601[27] = 0; + out_5982976038096974601[28] = 0; + out_5982976038096974601[29] = 0; + out_5982976038096974601[30] = 0; + out_5982976038096974601[31] = 0; + out_5982976038096974601[32] = 0; + out_5982976038096974601[33] = 0; + out_5982976038096974601[34] = 0; + out_5982976038096974601[35] = 0; + out_5982976038096974601[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_5982976038096974601[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_5982976038096974601[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_5982976038096974601[39] = 0; + out_5982976038096974601[40] = 0; + out_5982976038096974601[41] = 0; + out_5982976038096974601[42] = 0; + out_5982976038096974601[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_5982976038096974601[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_5982976038096974601[45] = 0; + out_5982976038096974601[46] = 0; + out_5982976038096974601[47] = 0; + out_5982976038096974601[48] = 0; + out_5982976038096974601[49] = 0; + out_5982976038096974601[50] = 0; + out_5982976038096974601[51] = 0; + out_5982976038096974601[52] = 0; + out_5982976038096974601[53] = 0; + out_5982976038096974601[54] = 0; + out_5982976038096974601[55] = 0; + out_5982976038096974601[56] = 0; + out_5982976038096974601[57] = 1; + out_5982976038096974601[58] = 0; + out_5982976038096974601[59] = 0; + out_5982976038096974601[60] = 0; + out_5982976038096974601[61] = 0; + out_5982976038096974601[62] = 0; + out_5982976038096974601[63] = 0; + out_5982976038096974601[64] = 0; + out_5982976038096974601[65] = 0; + out_5982976038096974601[66] = dt; + out_5982976038096974601[67] = 0; + out_5982976038096974601[68] = 0; + out_5982976038096974601[69] = 0; + out_5982976038096974601[70] = 0; + out_5982976038096974601[71] = 0; + out_5982976038096974601[72] = 0; + out_5982976038096974601[73] = 0; + out_5982976038096974601[74] = 0; + out_5982976038096974601[75] = 0; + out_5982976038096974601[76] = 1; + out_5982976038096974601[77] = 0; + out_5982976038096974601[78] = 0; + out_5982976038096974601[79] = 0; + out_5982976038096974601[80] = 0; + out_5982976038096974601[81] = 0; + out_5982976038096974601[82] = 0; + out_5982976038096974601[83] = 0; + out_5982976038096974601[84] = 0; + out_5982976038096974601[85] = dt; + out_5982976038096974601[86] = 0; + out_5982976038096974601[87] = 0; + out_5982976038096974601[88] = 0; + out_5982976038096974601[89] = 0; + out_5982976038096974601[90] = 0; + out_5982976038096974601[91] = 0; + out_5982976038096974601[92] = 0; + out_5982976038096974601[93] = 0; + out_5982976038096974601[94] = 0; + out_5982976038096974601[95] = 1; + out_5982976038096974601[96] = 0; + out_5982976038096974601[97] = 0; + out_5982976038096974601[98] = 0; + out_5982976038096974601[99] = 0; + out_5982976038096974601[100] = 0; + out_5982976038096974601[101] = 0; + out_5982976038096974601[102] = 0; + out_5982976038096974601[103] = 0; + out_5982976038096974601[104] = dt; + out_5982976038096974601[105] = 0; + out_5982976038096974601[106] = 0; + out_5982976038096974601[107] = 0; + out_5982976038096974601[108] = 0; + out_5982976038096974601[109] = 0; + out_5982976038096974601[110] = 0; + out_5982976038096974601[111] = 0; + out_5982976038096974601[112] = 0; + out_5982976038096974601[113] = 0; + out_5982976038096974601[114] = 1; + out_5982976038096974601[115] = 0; + out_5982976038096974601[116] = 0; + out_5982976038096974601[117] = 0; + out_5982976038096974601[118] = 0; + out_5982976038096974601[119] = 0; + out_5982976038096974601[120] = 0; + out_5982976038096974601[121] = 0; + out_5982976038096974601[122] = 0; + out_5982976038096974601[123] = 0; + out_5982976038096974601[124] = 0; + out_5982976038096974601[125] = 0; + out_5982976038096974601[126] = 0; + out_5982976038096974601[127] = 0; + out_5982976038096974601[128] = 0; + out_5982976038096974601[129] = 0; + out_5982976038096974601[130] = 0; + out_5982976038096974601[131] = 0; + out_5982976038096974601[132] = 0; + out_5982976038096974601[133] = 1; + out_5982976038096974601[134] = 0; + out_5982976038096974601[135] = 0; + out_5982976038096974601[136] = 0; + out_5982976038096974601[137] = 0; + out_5982976038096974601[138] = 0; + out_5982976038096974601[139] = 0; + out_5982976038096974601[140] = 0; + out_5982976038096974601[141] = 0; + out_5982976038096974601[142] = 0; + out_5982976038096974601[143] = 0; + out_5982976038096974601[144] = 0; + out_5982976038096974601[145] = 0; + out_5982976038096974601[146] = 0; + out_5982976038096974601[147] = 0; + out_5982976038096974601[148] = 0; + out_5982976038096974601[149] = 0; + out_5982976038096974601[150] = 0; + out_5982976038096974601[151] = 0; + out_5982976038096974601[152] = 1; + out_5982976038096974601[153] = 0; + out_5982976038096974601[154] = 0; + out_5982976038096974601[155] = 0; + out_5982976038096974601[156] = 0; + out_5982976038096974601[157] = 0; + out_5982976038096974601[158] = 0; + out_5982976038096974601[159] = 0; + out_5982976038096974601[160] = 0; + out_5982976038096974601[161] = 0; + out_5982976038096974601[162] = 0; + out_5982976038096974601[163] = 0; + out_5982976038096974601[164] = 0; + out_5982976038096974601[165] = 0; + out_5982976038096974601[166] = 0; + out_5982976038096974601[167] = 0; + out_5982976038096974601[168] = 0; + out_5982976038096974601[169] = 0; + out_5982976038096974601[170] = 0; + out_5982976038096974601[171] = 1; + out_5982976038096974601[172] = 0; + out_5982976038096974601[173] = 0; + out_5982976038096974601[174] = 0; + out_5982976038096974601[175] = 0; + out_5982976038096974601[176] = 0; + out_5982976038096974601[177] = 0; + out_5982976038096974601[178] = 0; + out_5982976038096974601[179] = 0; + out_5982976038096974601[180] = 0; + out_5982976038096974601[181] = 0; + out_5982976038096974601[182] = 0; + out_5982976038096974601[183] = 0; + out_5982976038096974601[184] = 0; + out_5982976038096974601[185] = 0; + out_5982976038096974601[186] = 0; + out_5982976038096974601[187] = 0; + out_5982976038096974601[188] = 0; + out_5982976038096974601[189] = 0; + out_5982976038096974601[190] = 1; + out_5982976038096974601[191] = 0; + out_5982976038096974601[192] = 0; + out_5982976038096974601[193] = 0; + out_5982976038096974601[194] = 0; + out_5982976038096974601[195] = 0; + out_5982976038096974601[196] = 0; + out_5982976038096974601[197] = 0; + out_5982976038096974601[198] = 0; + out_5982976038096974601[199] = 0; + out_5982976038096974601[200] = 0; + out_5982976038096974601[201] = 0; + out_5982976038096974601[202] = 0; + out_5982976038096974601[203] = 0; + out_5982976038096974601[204] = 0; + out_5982976038096974601[205] = 0; + out_5982976038096974601[206] = 0; + out_5982976038096974601[207] = 0; + out_5982976038096974601[208] = 0; + out_5982976038096974601[209] = 1; + out_5982976038096974601[210] = 0; + out_5982976038096974601[211] = 0; + out_5982976038096974601[212] = 0; + out_5982976038096974601[213] = 0; + out_5982976038096974601[214] = 0; + out_5982976038096974601[215] = 0; + out_5982976038096974601[216] = 0; + out_5982976038096974601[217] = 0; + out_5982976038096974601[218] = 0; + out_5982976038096974601[219] = 0; + out_5982976038096974601[220] = 0; + out_5982976038096974601[221] = 0; + out_5982976038096974601[222] = 0; + out_5982976038096974601[223] = 0; + out_5982976038096974601[224] = 0; + out_5982976038096974601[225] = 0; + out_5982976038096974601[226] = 0; + out_5982976038096974601[227] = 0; + out_5982976038096974601[228] = 1; + out_5982976038096974601[229] = 0; + out_5982976038096974601[230] = 0; + out_5982976038096974601[231] = 0; + out_5982976038096974601[232] = 0; + out_5982976038096974601[233] = 0; + out_5982976038096974601[234] = 0; + out_5982976038096974601[235] = 0; + out_5982976038096974601[236] = 0; + out_5982976038096974601[237] = 0; + out_5982976038096974601[238] = 0; + out_5982976038096974601[239] = 0; + out_5982976038096974601[240] = 0; + out_5982976038096974601[241] = 0; + out_5982976038096974601[242] = 0; + out_5982976038096974601[243] = 0; + out_5982976038096974601[244] = 0; + out_5982976038096974601[245] = 0; + out_5982976038096974601[246] = 0; + out_5982976038096974601[247] = 1; + out_5982976038096974601[248] = 0; + out_5982976038096974601[249] = 0; + out_5982976038096974601[250] = 0; + out_5982976038096974601[251] = 0; + out_5982976038096974601[252] = 0; + out_5982976038096974601[253] = 0; + out_5982976038096974601[254] = 0; + out_5982976038096974601[255] = 0; + out_5982976038096974601[256] = 0; + out_5982976038096974601[257] = 0; + out_5982976038096974601[258] = 0; + out_5982976038096974601[259] = 0; + out_5982976038096974601[260] = 0; + out_5982976038096974601[261] = 0; + out_5982976038096974601[262] = 0; + out_5982976038096974601[263] = 0; + out_5982976038096974601[264] = 0; + out_5982976038096974601[265] = 0; + out_5982976038096974601[266] = 1; + out_5982976038096974601[267] = 0; + out_5982976038096974601[268] = 0; + out_5982976038096974601[269] = 0; + out_5982976038096974601[270] = 0; + out_5982976038096974601[271] = 0; + out_5982976038096974601[272] = 0; + out_5982976038096974601[273] = 0; + out_5982976038096974601[274] = 0; + out_5982976038096974601[275] = 0; + out_5982976038096974601[276] = 0; + out_5982976038096974601[277] = 0; + out_5982976038096974601[278] = 0; + out_5982976038096974601[279] = 0; + out_5982976038096974601[280] = 0; + out_5982976038096974601[281] = 0; + out_5982976038096974601[282] = 0; + out_5982976038096974601[283] = 0; + out_5982976038096974601[284] = 0; + out_5982976038096974601[285] = 1; + out_5982976038096974601[286] = 0; + out_5982976038096974601[287] = 0; + out_5982976038096974601[288] = 0; + out_5982976038096974601[289] = 0; + out_5982976038096974601[290] = 0; + out_5982976038096974601[291] = 0; + out_5982976038096974601[292] = 0; + out_5982976038096974601[293] = 0; + out_5982976038096974601[294] = 0; + out_5982976038096974601[295] = 0; + out_5982976038096974601[296] = 0; + out_5982976038096974601[297] = 0; + out_5982976038096974601[298] = 0; + out_5982976038096974601[299] = 0; + out_5982976038096974601[300] = 0; + out_5982976038096974601[301] = 0; + out_5982976038096974601[302] = 0; + out_5982976038096974601[303] = 0; + out_5982976038096974601[304] = 1; + out_5982976038096974601[305] = 0; + out_5982976038096974601[306] = 0; + out_5982976038096974601[307] = 0; + out_5982976038096974601[308] = 0; + out_5982976038096974601[309] = 0; + out_5982976038096974601[310] = 0; + out_5982976038096974601[311] = 0; + out_5982976038096974601[312] = 0; + out_5982976038096974601[313] = 0; + out_5982976038096974601[314] = 0; + out_5982976038096974601[315] = 0; + out_5982976038096974601[316] = 0; + out_5982976038096974601[317] = 0; + out_5982976038096974601[318] = 0; + out_5982976038096974601[319] = 0; + out_5982976038096974601[320] = 0; + out_5982976038096974601[321] = 0; + out_5982976038096974601[322] = 0; + out_5982976038096974601[323] = 1; } -void h_4(double *state, double *unused, double *out_10924150640940683) { - out_10924150640940683[0] = state[6] + state[9]; - out_10924150640940683[1] = state[7] + state[10]; - out_10924150640940683[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_3015297743882558799) { + out_3015297743882558799[0] = state[6] + state[9]; + out_3015297743882558799[1] = state[7] + state[10]; + out_3015297743882558799[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_5752079327669770236) { - out_5752079327669770236[0] = 0; - out_5752079327669770236[1] = 0; - out_5752079327669770236[2] = 0; - out_5752079327669770236[3] = 0; - out_5752079327669770236[4] = 0; - out_5752079327669770236[5] = 0; - out_5752079327669770236[6] = 1; - out_5752079327669770236[7] = 0; - out_5752079327669770236[8] = 0; - out_5752079327669770236[9] = 1; - out_5752079327669770236[10] = 0; - out_5752079327669770236[11] = 0; - out_5752079327669770236[12] = 0; - out_5752079327669770236[13] = 0; - out_5752079327669770236[14] = 0; - out_5752079327669770236[15] = 0; - out_5752079327669770236[16] = 0; - out_5752079327669770236[17] = 0; - out_5752079327669770236[18] = 0; - out_5752079327669770236[19] = 0; - out_5752079327669770236[20] = 0; - out_5752079327669770236[21] = 0; - out_5752079327669770236[22] = 0; - out_5752079327669770236[23] = 0; - out_5752079327669770236[24] = 0; - out_5752079327669770236[25] = 1; - out_5752079327669770236[26] = 0; - out_5752079327669770236[27] = 0; - out_5752079327669770236[28] = 1; - out_5752079327669770236[29] = 0; - out_5752079327669770236[30] = 0; - out_5752079327669770236[31] = 0; - out_5752079327669770236[32] = 0; - out_5752079327669770236[33] = 0; - out_5752079327669770236[34] = 0; - out_5752079327669770236[35] = 0; - out_5752079327669770236[36] = 0; - out_5752079327669770236[37] = 0; - out_5752079327669770236[38] = 0; - out_5752079327669770236[39] = 0; - out_5752079327669770236[40] = 0; - out_5752079327669770236[41] = 0; - out_5752079327669770236[42] = 0; - out_5752079327669770236[43] = 0; - out_5752079327669770236[44] = 1; - out_5752079327669770236[45] = 0; - out_5752079327669770236[46] = 0; - out_5752079327669770236[47] = 1; - out_5752079327669770236[48] = 0; - out_5752079327669770236[49] = 0; - out_5752079327669770236[50] = 0; - out_5752079327669770236[51] = 0; - out_5752079327669770236[52] = 0; - out_5752079327669770236[53] = 0; +void H_4(double *state, double *unused, double *out_8440442363568188843) { + out_8440442363568188843[0] = 0; + out_8440442363568188843[1] = 0; + out_8440442363568188843[2] = 0; + out_8440442363568188843[3] = 0; + out_8440442363568188843[4] = 0; + out_8440442363568188843[5] = 0; + out_8440442363568188843[6] = 1; + out_8440442363568188843[7] = 0; + out_8440442363568188843[8] = 0; + out_8440442363568188843[9] = 1; + out_8440442363568188843[10] = 0; + out_8440442363568188843[11] = 0; + out_8440442363568188843[12] = 0; + out_8440442363568188843[13] = 0; + out_8440442363568188843[14] = 0; + out_8440442363568188843[15] = 0; + out_8440442363568188843[16] = 0; + out_8440442363568188843[17] = 0; + out_8440442363568188843[18] = 0; + out_8440442363568188843[19] = 0; + out_8440442363568188843[20] = 0; + out_8440442363568188843[21] = 0; + out_8440442363568188843[22] = 0; + out_8440442363568188843[23] = 0; + out_8440442363568188843[24] = 0; + out_8440442363568188843[25] = 1; + out_8440442363568188843[26] = 0; + out_8440442363568188843[27] = 0; + out_8440442363568188843[28] = 1; + out_8440442363568188843[29] = 0; + out_8440442363568188843[30] = 0; + out_8440442363568188843[31] = 0; + out_8440442363568188843[32] = 0; + out_8440442363568188843[33] = 0; + out_8440442363568188843[34] = 0; + out_8440442363568188843[35] = 0; + out_8440442363568188843[36] = 0; + out_8440442363568188843[37] = 0; + out_8440442363568188843[38] = 0; + out_8440442363568188843[39] = 0; + out_8440442363568188843[40] = 0; + out_8440442363568188843[41] = 0; + out_8440442363568188843[42] = 0; + out_8440442363568188843[43] = 0; + out_8440442363568188843[44] = 1; + out_8440442363568188843[45] = 0; + out_8440442363568188843[46] = 0; + out_8440442363568188843[47] = 1; + out_8440442363568188843[48] = 0; + out_8440442363568188843[49] = 0; + out_8440442363568188843[50] = 0; + out_8440442363568188843[51] = 0; + out_8440442363568188843[52] = 0; + out_8440442363568188843[53] = 0; } -void h_10(double *state, double *unused, double *out_3400315615895606338) { - out_3400315615895606338[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_3400315615895606338[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_3400315615895606338[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_4712013892887729118) { + out_4712013892887729118[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_4712013892887729118[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_4712013892887729118[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_6603039364318987837) { - out_6603039364318987837[0] = 0; - out_6603039364318987837[1] = 9.8100000000000005*cos(state[1]); - out_6603039364318987837[2] = 0; - out_6603039364318987837[3] = 0; - out_6603039364318987837[4] = -state[8]; - out_6603039364318987837[5] = state[7]; - out_6603039364318987837[6] = 0; - out_6603039364318987837[7] = state[5]; - out_6603039364318987837[8] = -state[4]; - out_6603039364318987837[9] = 0; - out_6603039364318987837[10] = 0; - out_6603039364318987837[11] = 0; - out_6603039364318987837[12] = 1; - out_6603039364318987837[13] = 0; - out_6603039364318987837[14] = 0; - out_6603039364318987837[15] = 1; - out_6603039364318987837[16] = 0; - out_6603039364318987837[17] = 0; - out_6603039364318987837[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_6603039364318987837[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_6603039364318987837[20] = 0; - out_6603039364318987837[21] = state[8]; - out_6603039364318987837[22] = 0; - out_6603039364318987837[23] = -state[6]; - out_6603039364318987837[24] = -state[5]; - out_6603039364318987837[25] = 0; - out_6603039364318987837[26] = state[3]; - out_6603039364318987837[27] = 0; - out_6603039364318987837[28] = 0; - out_6603039364318987837[29] = 0; - out_6603039364318987837[30] = 0; - out_6603039364318987837[31] = 1; - out_6603039364318987837[32] = 0; - out_6603039364318987837[33] = 0; - out_6603039364318987837[34] = 1; - out_6603039364318987837[35] = 0; - out_6603039364318987837[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_6603039364318987837[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_6603039364318987837[38] = 0; - out_6603039364318987837[39] = -state[7]; - out_6603039364318987837[40] = state[6]; - out_6603039364318987837[41] = 0; - out_6603039364318987837[42] = state[4]; - out_6603039364318987837[43] = -state[3]; - out_6603039364318987837[44] = 0; - out_6603039364318987837[45] = 0; - out_6603039364318987837[46] = 0; - out_6603039364318987837[47] = 0; - out_6603039364318987837[48] = 0; - out_6603039364318987837[49] = 0; - out_6603039364318987837[50] = 1; - out_6603039364318987837[51] = 0; - out_6603039364318987837[52] = 0; - out_6603039364318987837[53] = 1; +void H_10(double *state, double *unused, double *out_747450665488565157) { + out_747450665488565157[0] = 0; + out_747450665488565157[1] = 9.8100000000000005*cos(state[1]); + out_747450665488565157[2] = 0; + out_747450665488565157[3] = 0; + out_747450665488565157[4] = -state[8]; + out_747450665488565157[5] = state[7]; + out_747450665488565157[6] = 0; + out_747450665488565157[7] = state[5]; + out_747450665488565157[8] = -state[4]; + out_747450665488565157[9] = 0; + out_747450665488565157[10] = 0; + out_747450665488565157[11] = 0; + out_747450665488565157[12] = 1; + out_747450665488565157[13] = 0; + out_747450665488565157[14] = 0; + out_747450665488565157[15] = 1; + out_747450665488565157[16] = 0; + out_747450665488565157[17] = 0; + out_747450665488565157[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_747450665488565157[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_747450665488565157[20] = 0; + out_747450665488565157[21] = state[8]; + out_747450665488565157[22] = 0; + out_747450665488565157[23] = -state[6]; + out_747450665488565157[24] = -state[5]; + out_747450665488565157[25] = 0; + out_747450665488565157[26] = state[3]; + out_747450665488565157[27] = 0; + out_747450665488565157[28] = 0; + out_747450665488565157[29] = 0; + out_747450665488565157[30] = 0; + out_747450665488565157[31] = 1; + out_747450665488565157[32] = 0; + out_747450665488565157[33] = 0; + out_747450665488565157[34] = 1; + out_747450665488565157[35] = 0; + out_747450665488565157[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_747450665488565157[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_747450665488565157[38] = 0; + out_747450665488565157[39] = -state[7]; + out_747450665488565157[40] = state[6]; + out_747450665488565157[41] = 0; + out_747450665488565157[42] = state[4]; + out_747450665488565157[43] = -state[3]; + out_747450665488565157[44] = 0; + out_747450665488565157[45] = 0; + out_747450665488565157[46] = 0; + out_747450665488565157[47] = 0; + out_747450665488565157[48] = 0; + out_747450665488565157[49] = 0; + out_747450665488565157[50] = 1; + out_747450665488565157[51] = 0; + out_747450665488565157[52] = 0; + out_747450665488565157[53] = 1; } -void h_13(double *state, double *unused, double *out_5162106749790850325) { - out_5162106749790850325[0] = state[3]; - out_5162106749790850325[1] = state[4]; - out_5162106749790850325[2] = state[5]; +void h_13(double *state, double *unused, double *out_413147883148967273) { + out_413147883148967273[0] = state[3]; + out_413147883148967273[1] = state[4]; + out_413147883148967273[2] = state[5]; } -void H_13(double *state, double *unused, double *out_2539805502337437435) { - out_2539805502337437435[0] = 0; - out_2539805502337437435[1] = 0; - out_2539805502337437435[2] = 0; - out_2539805502337437435[3] = 1; - out_2539805502337437435[4] = 0; - out_2539805502337437435[5] = 0; - out_2539805502337437435[6] = 0; - out_2539805502337437435[7] = 0; - out_2539805502337437435[8] = 0; - out_2539805502337437435[9] = 0; - out_2539805502337437435[10] = 0; - out_2539805502337437435[11] = 0; - out_2539805502337437435[12] = 0; - out_2539805502337437435[13] = 0; - out_2539805502337437435[14] = 0; - out_2539805502337437435[15] = 0; - out_2539805502337437435[16] = 0; - out_2539805502337437435[17] = 0; - out_2539805502337437435[18] = 0; - out_2539805502337437435[19] = 0; - out_2539805502337437435[20] = 0; - out_2539805502337437435[21] = 0; - out_2539805502337437435[22] = 1; - out_2539805502337437435[23] = 0; - out_2539805502337437435[24] = 0; - out_2539805502337437435[25] = 0; - out_2539805502337437435[26] = 0; - out_2539805502337437435[27] = 0; - out_2539805502337437435[28] = 0; - out_2539805502337437435[29] = 0; - out_2539805502337437435[30] = 0; - out_2539805502337437435[31] = 0; - out_2539805502337437435[32] = 0; - out_2539805502337437435[33] = 0; - out_2539805502337437435[34] = 0; - out_2539805502337437435[35] = 0; - out_2539805502337437435[36] = 0; - out_2539805502337437435[37] = 0; - out_2539805502337437435[38] = 0; - out_2539805502337437435[39] = 0; - out_2539805502337437435[40] = 0; - out_2539805502337437435[41] = 1; - out_2539805502337437435[42] = 0; - out_2539805502337437435[43] = 0; - out_2539805502337437435[44] = 0; - out_2539805502337437435[45] = 0; - out_2539805502337437435[46] = 0; - out_2539805502337437435[47] = 0; - out_2539805502337437435[48] = 0; - out_2539805502337437435[49] = 0; - out_2539805502337437435[50] = 0; - out_2539805502337437435[51] = 0; - out_2539805502337437435[52] = 0; - out_2539805502337437435[53] = 0; +void H_13(double *state, double *unused, double *out_6794027884809029972) { + out_6794027884809029972[0] = 0; + out_6794027884809029972[1] = 0; + out_6794027884809029972[2] = 0; + out_6794027884809029972[3] = 1; + out_6794027884809029972[4] = 0; + out_6794027884809029972[5] = 0; + out_6794027884809029972[6] = 0; + out_6794027884809029972[7] = 0; + out_6794027884809029972[8] = 0; + out_6794027884809029972[9] = 0; + out_6794027884809029972[10] = 0; + out_6794027884809029972[11] = 0; + out_6794027884809029972[12] = 0; + out_6794027884809029972[13] = 0; + out_6794027884809029972[14] = 0; + out_6794027884809029972[15] = 0; + out_6794027884809029972[16] = 0; + out_6794027884809029972[17] = 0; + out_6794027884809029972[18] = 0; + out_6794027884809029972[19] = 0; + out_6794027884809029972[20] = 0; + out_6794027884809029972[21] = 0; + out_6794027884809029972[22] = 1; + out_6794027884809029972[23] = 0; + out_6794027884809029972[24] = 0; + out_6794027884809029972[25] = 0; + out_6794027884809029972[26] = 0; + out_6794027884809029972[27] = 0; + out_6794027884809029972[28] = 0; + out_6794027884809029972[29] = 0; + out_6794027884809029972[30] = 0; + out_6794027884809029972[31] = 0; + out_6794027884809029972[32] = 0; + out_6794027884809029972[33] = 0; + out_6794027884809029972[34] = 0; + out_6794027884809029972[35] = 0; + out_6794027884809029972[36] = 0; + out_6794027884809029972[37] = 0; + out_6794027884809029972[38] = 0; + out_6794027884809029972[39] = 0; + out_6794027884809029972[40] = 0; + out_6794027884809029972[41] = 1; + out_6794027884809029972[42] = 0; + out_6794027884809029972[43] = 0; + out_6794027884809029972[44] = 0; + out_6794027884809029972[45] = 0; + out_6794027884809029972[46] = 0; + out_6794027884809029972[47] = 0; + out_6794027884809029972[48] = 0; + out_6794027884809029972[49] = 0; + out_6794027884809029972[50] = 0; + out_6794027884809029972[51] = 0; + out_6794027884809029972[52] = 0; + out_6794027884809029972[53] = 0; } -void h_14(double *state, double *unused, double *out_8034891048701220434) { - out_8034891048701220434[0] = state[6]; - out_8034891048701220434[1] = state[7]; - out_8034891048701220434[2] = state[8]; +void h_14(double *state, double *unused, double *out_2902963862180291092) { + out_2902963862180291092[0] = state[6]; + out_2902963862180291092[1] = state[7]; + out_2902963862180291092[2] = state[8]; } -void H_14(double *state, double *unused, double *out_1788838471330285707) { - out_1788838471330285707[0] = 0; - out_1788838471330285707[1] = 0; - out_1788838471330285707[2] = 0; - out_1788838471330285707[3] = 0; - out_1788838471330285707[4] = 0; - out_1788838471330285707[5] = 0; - out_1788838471330285707[6] = 1; - out_1788838471330285707[7] = 0; - out_1788838471330285707[8] = 0; - out_1788838471330285707[9] = 0; - out_1788838471330285707[10] = 0; - out_1788838471330285707[11] = 0; - out_1788838471330285707[12] = 0; - out_1788838471330285707[13] = 0; - out_1788838471330285707[14] = 0; - out_1788838471330285707[15] = 0; - out_1788838471330285707[16] = 0; - out_1788838471330285707[17] = 0; - out_1788838471330285707[18] = 0; - out_1788838471330285707[19] = 0; - out_1788838471330285707[20] = 0; - out_1788838471330285707[21] = 0; - out_1788838471330285707[22] = 0; - out_1788838471330285707[23] = 0; - out_1788838471330285707[24] = 0; - out_1788838471330285707[25] = 1; - out_1788838471330285707[26] = 0; - out_1788838471330285707[27] = 0; - out_1788838471330285707[28] = 0; - out_1788838471330285707[29] = 0; - out_1788838471330285707[30] = 0; - out_1788838471330285707[31] = 0; - out_1788838471330285707[32] = 0; - out_1788838471330285707[33] = 0; - out_1788838471330285707[34] = 0; - out_1788838471330285707[35] = 0; - out_1788838471330285707[36] = 0; - out_1788838471330285707[37] = 0; - out_1788838471330285707[38] = 0; - out_1788838471330285707[39] = 0; - out_1788838471330285707[40] = 0; - out_1788838471330285707[41] = 0; - out_1788838471330285707[42] = 0; - out_1788838471330285707[43] = 0; - out_1788838471330285707[44] = 1; - out_1788838471330285707[45] = 0; - out_1788838471330285707[46] = 0; - out_1788838471330285707[47] = 0; - out_1788838471330285707[48] = 0; - out_1788838471330285707[49] = 0; - out_1788838471330285707[50] = 0; - out_1788838471330285707[51] = 0; - out_1788838471330285707[52] = 0; - out_1788838471330285707[53] = 0; +void H_14(double *state, double *unused, double *out_5357653931272816547) { + out_5357653931272816547[0] = 0; + out_5357653931272816547[1] = 0; + out_5357653931272816547[2] = 0; + out_5357653931272816547[3] = 0; + out_5357653931272816547[4] = 0; + out_5357653931272816547[5] = 0; + out_5357653931272816547[6] = 1; + out_5357653931272816547[7] = 0; + out_5357653931272816547[8] = 0; + out_5357653931272816547[9] = 0; + out_5357653931272816547[10] = 0; + out_5357653931272816547[11] = 0; + out_5357653931272816547[12] = 0; + out_5357653931272816547[13] = 0; + out_5357653931272816547[14] = 0; + out_5357653931272816547[15] = 0; + out_5357653931272816547[16] = 0; + out_5357653931272816547[17] = 0; + out_5357653931272816547[18] = 0; + out_5357653931272816547[19] = 0; + out_5357653931272816547[20] = 0; + out_5357653931272816547[21] = 0; + out_5357653931272816547[22] = 0; + out_5357653931272816547[23] = 0; + out_5357653931272816547[24] = 0; + out_5357653931272816547[25] = 1; + out_5357653931272816547[26] = 0; + out_5357653931272816547[27] = 0; + out_5357653931272816547[28] = 0; + out_5357653931272816547[29] = 0; + out_5357653931272816547[30] = 0; + out_5357653931272816547[31] = 0; + out_5357653931272816547[32] = 0; + out_5357653931272816547[33] = 0; + out_5357653931272816547[34] = 0; + out_5357653931272816547[35] = 0; + out_5357653931272816547[36] = 0; + out_5357653931272816547[37] = 0; + out_5357653931272816547[38] = 0; + out_5357653931272816547[39] = 0; + out_5357653931272816547[40] = 0; + out_5357653931272816547[41] = 0; + out_5357653931272816547[42] = 0; + out_5357653931272816547[43] = 0; + out_5357653931272816547[44] = 1; + out_5357653931272816547[45] = 0; + out_5357653931272816547[46] = 0; + out_5357653931272816547[47] = 0; + out_5357653931272816547[48] = 0; + out_5357653931272816547[49] = 0; + out_5357653931272816547[50] = 0; + out_5357653931272816547[51] = 0; + out_5357653931272816547[52] = 0; + out_5357653931272816547[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_3094993718265680010) { - err_fun(nom_x, delta_x, out_3094993718265680010); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1002597543094682529) { + err_fun(nom_x, delta_x, out_1002597543094682529); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_446507105452282601) { - inv_err_fun(nom_x, true_x, out_446507105452282601); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6790221310632921126) { + inv_err_fun(nom_x, true_x, out_6790221310632921126); } -void pose_H_mod_fun(double *state, double *out_96002479082188485) { - H_mod_fun(state, out_96002479082188485); +void pose_H_mod_fun(double *state, double *out_1061940044083541431) { + H_mod_fun(state, out_1061940044083541431); } -void pose_f_fun(double *state, double dt, double *out_8888553933023554192) { - f_fun(state, dt, out_8888553933023554192); +void pose_f_fun(double *state, double dt, double *out_3240563005197080216) { + f_fun(state, dt, out_3240563005197080216); } -void pose_F_fun(double *state, double dt, double *out_3793856272932959588) { - F_fun(state, dt, out_3793856272932959588); +void pose_F_fun(double *state, double dt, double *out_5982976038096974601) { + F_fun(state, dt, out_5982976038096974601); } -void pose_h_4(double *state, double *unused, double *out_10924150640940683) { - h_4(state, unused, out_10924150640940683); +void pose_h_4(double *state, double *unused, double *out_3015297743882558799) { + h_4(state, unused, out_3015297743882558799); } -void pose_H_4(double *state, double *unused, double *out_5752079327669770236) { - H_4(state, unused, out_5752079327669770236); +void pose_H_4(double *state, double *unused, double *out_8440442363568188843) { + H_4(state, unused, out_8440442363568188843); } -void pose_h_10(double *state, double *unused, double *out_3400315615895606338) { - h_10(state, unused, out_3400315615895606338); +void pose_h_10(double *state, double *unused, double *out_4712013892887729118) { + h_10(state, unused, out_4712013892887729118); } -void pose_H_10(double *state, double *unused, double *out_6603039364318987837) { - H_10(state, unused, out_6603039364318987837); +void pose_H_10(double *state, double *unused, double *out_747450665488565157) { + H_10(state, unused, out_747450665488565157); } -void pose_h_13(double *state, double *unused, double *out_5162106749790850325) { - h_13(state, unused, out_5162106749790850325); +void pose_h_13(double *state, double *unused, double *out_413147883148967273) { + h_13(state, unused, out_413147883148967273); } -void pose_H_13(double *state, double *unused, double *out_2539805502337437435) { - H_13(state, unused, out_2539805502337437435); +void pose_H_13(double *state, double *unused, double *out_6794027884809029972) { + H_13(state, unused, out_6794027884809029972); } -void pose_h_14(double *state, double *unused, double *out_8034891048701220434) { - h_14(state, unused, out_8034891048701220434); +void pose_h_14(double *state, double *unused, double *out_2902963862180291092) { + h_14(state, unused, out_2902963862180291092); } -void pose_H_14(double *state, double *unused, double *out_1788838471330285707) { - H_14(state, unused, out_1788838471330285707); +void pose_H_14(double *state, double *unused, double *out_5357653931272816547) { + H_14(state, unused, out_5357653931272816547); } 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/openpilot/selfdrive/locationd/models/generated/pose.h b/openpilot/selfdrive/locationd/models/generated/pose.h index cf56a29c25..9b90cd398e 100644 --- a/openpilot/selfdrive/locationd/models/generated/pose.h +++ b/openpilot/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_3094993718265680010); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_446507105452282601); -void pose_H_mod_fun(double *state, double *out_96002479082188485); -void pose_f_fun(double *state, double dt, double *out_8888553933023554192); -void pose_F_fun(double *state, double dt, double *out_3793856272932959588); -void pose_h_4(double *state, double *unused, double *out_10924150640940683); -void pose_H_4(double *state, double *unused, double *out_5752079327669770236); -void pose_h_10(double *state, double *unused, double *out_3400315615895606338); -void pose_H_10(double *state, double *unused, double *out_6603039364318987837); -void pose_h_13(double *state, double *unused, double *out_5162106749790850325); -void pose_H_13(double *state, double *unused, double *out_2539805502337437435); -void pose_h_14(double *state, double *unused, double *out_8034891048701220434); -void pose_H_14(double *state, double *unused, double *out_1788838471330285707); +void pose_err_fun(double *nom_x, double *delta_x, double *out_1002597543094682529); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6790221310632921126); +void pose_H_mod_fun(double *state, double *out_1061940044083541431); +void pose_f_fun(double *state, double dt, double *out_3240563005197080216); +void pose_F_fun(double *state, double dt, double *out_5982976038096974601); +void pose_h_4(double *state, double *unused, double *out_3015297743882558799); +void pose_H_4(double *state, double *unused, double *out_8440442363568188843); +void pose_h_10(double *state, double *unused, double *out_4712013892887729118); +void pose_H_10(double *state, double *unused, double *out_747450665488565157); +void pose_h_13(double *state, double *unused, double *out_413147883148967273); +void pose_H_13(double *state, double *unused, double *out_6794027884809029972); +void pose_h_14(double *state, double *unused, double *out_2902963862180291092); +void pose_H_14(double *state, double *unused, double *out_5357653931272816547); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index e274eab261..8beb9bb497 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/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index fbf63f3f6e..b8b718853d 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 32b6c06f92..a278ef5aba 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 3d021bc9bd..0c88f7f8fc 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/gitversion.h b/panda/board/obj/gitversion.h index 8c1164a649..98fa03ae0c 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,4 +1,4 @@ #pragma once extern const uint8_t gitversion[29]; -const uint8_t gitversion[29] = "DEV-14a796e5-P92eb5651-DEBUG"; +const uint8_t gitversion[29] = "DEV-6e0183dc-P92eb5651-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 3efa6c91dc..a22a13924a 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_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index 093b46a163..f32580e76a 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/version b/panda/board/obj/version index 1e0d8f1c95..16f997091a 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-14a796e5-P92eb5651-DEBUG \ No newline at end of file +DEV-6e0183dc-P92eb5651-DEBUG \ No newline at end of file diff --git a/prebuilt.json b/prebuilt.json index e3e3146877..47f61106ba 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "d5fba5e558d15835d6ea4034f05472d78dd76fd1775f2a94284f668289ba1ab3", "schema_version": 1} +{"protected_inventory_sha256": "ee4fd4896b142c1c30faca2cfc7ce3cdec4a989c08052d113d05d44f70b7c6b3", "schema_version": 1}