mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-09 22:13:43 +08:00
Build
This commit is contained in:
firestar5683
@@ -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_191040875433637227) {
|
||||
out_191040875433637227[0] = delta_x[0] + nom_x[0];
|
||||
out_191040875433637227[1] = delta_x[1] + nom_x[1];
|
||||
out_191040875433637227[2] = delta_x[2] + nom_x[2];
|
||||
out_191040875433637227[3] = delta_x[3] + nom_x[3];
|
||||
out_191040875433637227[4] = delta_x[4] + nom_x[4];
|
||||
out_191040875433637227[5] = delta_x[5] + nom_x[5];
|
||||
out_191040875433637227[6] = delta_x[6] + nom_x[6];
|
||||
out_191040875433637227[7] = delta_x[7] + nom_x[7];
|
||||
out_191040875433637227[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_2204053027494663001) {
|
||||
out_2204053027494663001[0] = delta_x[0] + nom_x[0];
|
||||
out_2204053027494663001[1] = delta_x[1] + nom_x[1];
|
||||
out_2204053027494663001[2] = delta_x[2] + nom_x[2];
|
||||
out_2204053027494663001[3] = delta_x[3] + nom_x[3];
|
||||
out_2204053027494663001[4] = delta_x[4] + nom_x[4];
|
||||
out_2204053027494663001[5] = delta_x[5] + nom_x[5];
|
||||
out_2204053027494663001[6] = delta_x[6] + nom_x[6];
|
||||
out_2204053027494663001[7] = delta_x[7] + nom_x[7];
|
||||
out_2204053027494663001[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_449478399145670770) {
|
||||
out_449478399145670770[0] = -nom_x[0] + true_x[0];
|
||||
out_449478399145670770[1] = -nom_x[1] + true_x[1];
|
||||
out_449478399145670770[2] = -nom_x[2] + true_x[2];
|
||||
out_449478399145670770[3] = -nom_x[3] + true_x[3];
|
||||
out_449478399145670770[4] = -nom_x[4] + true_x[4];
|
||||
out_449478399145670770[5] = -nom_x[5] + true_x[5];
|
||||
out_449478399145670770[6] = -nom_x[6] + true_x[6];
|
||||
out_449478399145670770[7] = -nom_x[7] + true_x[7];
|
||||
out_449478399145670770[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_2041279983635365065) {
|
||||
out_2041279983635365065[0] = -nom_x[0] + true_x[0];
|
||||
out_2041279983635365065[1] = -nom_x[1] + true_x[1];
|
||||
out_2041279983635365065[2] = -nom_x[2] + true_x[2];
|
||||
out_2041279983635365065[3] = -nom_x[3] + true_x[3];
|
||||
out_2041279983635365065[4] = -nom_x[4] + true_x[4];
|
||||
out_2041279983635365065[5] = -nom_x[5] + true_x[5];
|
||||
out_2041279983635365065[6] = -nom_x[6] + true_x[6];
|
||||
out_2041279983635365065[7] = -nom_x[7] + true_x[7];
|
||||
out_2041279983635365065[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_2217112053847292599) {
|
||||
out_2217112053847292599[0] = 1.0;
|
||||
out_2217112053847292599[1] = 0.0;
|
||||
out_2217112053847292599[2] = 0.0;
|
||||
out_2217112053847292599[3] = 0.0;
|
||||
out_2217112053847292599[4] = 0.0;
|
||||
out_2217112053847292599[5] = 0.0;
|
||||
out_2217112053847292599[6] = 0.0;
|
||||
out_2217112053847292599[7] = 0.0;
|
||||
out_2217112053847292599[8] = 0.0;
|
||||
out_2217112053847292599[9] = 0.0;
|
||||
out_2217112053847292599[10] = 1.0;
|
||||
out_2217112053847292599[11] = 0.0;
|
||||
out_2217112053847292599[12] = 0.0;
|
||||
out_2217112053847292599[13] = 0.0;
|
||||
out_2217112053847292599[14] = 0.0;
|
||||
out_2217112053847292599[15] = 0.0;
|
||||
out_2217112053847292599[16] = 0.0;
|
||||
out_2217112053847292599[17] = 0.0;
|
||||
out_2217112053847292599[18] = 0.0;
|
||||
out_2217112053847292599[19] = 0.0;
|
||||
out_2217112053847292599[20] = 1.0;
|
||||
out_2217112053847292599[21] = 0.0;
|
||||
out_2217112053847292599[22] = 0.0;
|
||||
out_2217112053847292599[23] = 0.0;
|
||||
out_2217112053847292599[24] = 0.0;
|
||||
out_2217112053847292599[25] = 0.0;
|
||||
out_2217112053847292599[26] = 0.0;
|
||||
out_2217112053847292599[27] = 0.0;
|
||||
out_2217112053847292599[28] = 0.0;
|
||||
out_2217112053847292599[29] = 0.0;
|
||||
out_2217112053847292599[30] = 1.0;
|
||||
out_2217112053847292599[31] = 0.0;
|
||||
out_2217112053847292599[32] = 0.0;
|
||||
out_2217112053847292599[33] = 0.0;
|
||||
out_2217112053847292599[34] = 0.0;
|
||||
out_2217112053847292599[35] = 0.0;
|
||||
out_2217112053847292599[36] = 0.0;
|
||||
out_2217112053847292599[37] = 0.0;
|
||||
out_2217112053847292599[38] = 0.0;
|
||||
out_2217112053847292599[39] = 0.0;
|
||||
out_2217112053847292599[40] = 1.0;
|
||||
out_2217112053847292599[41] = 0.0;
|
||||
out_2217112053847292599[42] = 0.0;
|
||||
out_2217112053847292599[43] = 0.0;
|
||||
out_2217112053847292599[44] = 0.0;
|
||||
out_2217112053847292599[45] = 0.0;
|
||||
out_2217112053847292599[46] = 0.0;
|
||||
out_2217112053847292599[47] = 0.0;
|
||||
out_2217112053847292599[48] = 0.0;
|
||||
out_2217112053847292599[49] = 0.0;
|
||||
out_2217112053847292599[50] = 1.0;
|
||||
out_2217112053847292599[51] = 0.0;
|
||||
out_2217112053847292599[52] = 0.0;
|
||||
out_2217112053847292599[53] = 0.0;
|
||||
out_2217112053847292599[54] = 0.0;
|
||||
out_2217112053847292599[55] = 0.0;
|
||||
out_2217112053847292599[56] = 0.0;
|
||||
out_2217112053847292599[57] = 0.0;
|
||||
out_2217112053847292599[58] = 0.0;
|
||||
out_2217112053847292599[59] = 0.0;
|
||||
out_2217112053847292599[60] = 1.0;
|
||||
out_2217112053847292599[61] = 0.0;
|
||||
out_2217112053847292599[62] = 0.0;
|
||||
out_2217112053847292599[63] = 0.0;
|
||||
out_2217112053847292599[64] = 0.0;
|
||||
out_2217112053847292599[65] = 0.0;
|
||||
out_2217112053847292599[66] = 0.0;
|
||||
out_2217112053847292599[67] = 0.0;
|
||||
out_2217112053847292599[68] = 0.0;
|
||||
out_2217112053847292599[69] = 0.0;
|
||||
out_2217112053847292599[70] = 1.0;
|
||||
out_2217112053847292599[71] = 0.0;
|
||||
out_2217112053847292599[72] = 0.0;
|
||||
out_2217112053847292599[73] = 0.0;
|
||||
out_2217112053847292599[74] = 0.0;
|
||||
out_2217112053847292599[75] = 0.0;
|
||||
out_2217112053847292599[76] = 0.0;
|
||||
out_2217112053847292599[77] = 0.0;
|
||||
out_2217112053847292599[78] = 0.0;
|
||||
out_2217112053847292599[79] = 0.0;
|
||||
out_2217112053847292599[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_7031372799249316450) {
|
||||
out_7031372799249316450[0] = 1.0;
|
||||
out_7031372799249316450[1] = 0.0;
|
||||
out_7031372799249316450[2] = 0.0;
|
||||
out_7031372799249316450[3] = 0.0;
|
||||
out_7031372799249316450[4] = 0.0;
|
||||
out_7031372799249316450[5] = 0.0;
|
||||
out_7031372799249316450[6] = 0.0;
|
||||
out_7031372799249316450[7] = 0.0;
|
||||
out_7031372799249316450[8] = 0.0;
|
||||
out_7031372799249316450[9] = 0.0;
|
||||
out_7031372799249316450[10] = 1.0;
|
||||
out_7031372799249316450[11] = 0.0;
|
||||
out_7031372799249316450[12] = 0.0;
|
||||
out_7031372799249316450[13] = 0.0;
|
||||
out_7031372799249316450[14] = 0.0;
|
||||
out_7031372799249316450[15] = 0.0;
|
||||
out_7031372799249316450[16] = 0.0;
|
||||
out_7031372799249316450[17] = 0.0;
|
||||
out_7031372799249316450[18] = 0.0;
|
||||
out_7031372799249316450[19] = 0.0;
|
||||
out_7031372799249316450[20] = 1.0;
|
||||
out_7031372799249316450[21] = 0.0;
|
||||
out_7031372799249316450[22] = 0.0;
|
||||
out_7031372799249316450[23] = 0.0;
|
||||
out_7031372799249316450[24] = 0.0;
|
||||
out_7031372799249316450[25] = 0.0;
|
||||
out_7031372799249316450[26] = 0.0;
|
||||
out_7031372799249316450[27] = 0.0;
|
||||
out_7031372799249316450[28] = 0.0;
|
||||
out_7031372799249316450[29] = 0.0;
|
||||
out_7031372799249316450[30] = 1.0;
|
||||
out_7031372799249316450[31] = 0.0;
|
||||
out_7031372799249316450[32] = 0.0;
|
||||
out_7031372799249316450[33] = 0.0;
|
||||
out_7031372799249316450[34] = 0.0;
|
||||
out_7031372799249316450[35] = 0.0;
|
||||
out_7031372799249316450[36] = 0.0;
|
||||
out_7031372799249316450[37] = 0.0;
|
||||
out_7031372799249316450[38] = 0.0;
|
||||
out_7031372799249316450[39] = 0.0;
|
||||
out_7031372799249316450[40] = 1.0;
|
||||
out_7031372799249316450[41] = 0.0;
|
||||
out_7031372799249316450[42] = 0.0;
|
||||
out_7031372799249316450[43] = 0.0;
|
||||
out_7031372799249316450[44] = 0.0;
|
||||
out_7031372799249316450[45] = 0.0;
|
||||
out_7031372799249316450[46] = 0.0;
|
||||
out_7031372799249316450[47] = 0.0;
|
||||
out_7031372799249316450[48] = 0.0;
|
||||
out_7031372799249316450[49] = 0.0;
|
||||
out_7031372799249316450[50] = 1.0;
|
||||
out_7031372799249316450[51] = 0.0;
|
||||
out_7031372799249316450[52] = 0.0;
|
||||
out_7031372799249316450[53] = 0.0;
|
||||
out_7031372799249316450[54] = 0.0;
|
||||
out_7031372799249316450[55] = 0.0;
|
||||
out_7031372799249316450[56] = 0.0;
|
||||
out_7031372799249316450[57] = 0.0;
|
||||
out_7031372799249316450[58] = 0.0;
|
||||
out_7031372799249316450[59] = 0.0;
|
||||
out_7031372799249316450[60] = 1.0;
|
||||
out_7031372799249316450[61] = 0.0;
|
||||
out_7031372799249316450[62] = 0.0;
|
||||
out_7031372799249316450[63] = 0.0;
|
||||
out_7031372799249316450[64] = 0.0;
|
||||
out_7031372799249316450[65] = 0.0;
|
||||
out_7031372799249316450[66] = 0.0;
|
||||
out_7031372799249316450[67] = 0.0;
|
||||
out_7031372799249316450[68] = 0.0;
|
||||
out_7031372799249316450[69] = 0.0;
|
||||
out_7031372799249316450[70] = 1.0;
|
||||
out_7031372799249316450[71] = 0.0;
|
||||
out_7031372799249316450[72] = 0.0;
|
||||
out_7031372799249316450[73] = 0.0;
|
||||
out_7031372799249316450[74] = 0.0;
|
||||
out_7031372799249316450[75] = 0.0;
|
||||
out_7031372799249316450[76] = 0.0;
|
||||
out_7031372799249316450[77] = 0.0;
|
||||
out_7031372799249316450[78] = 0.0;
|
||||
out_7031372799249316450[79] = 0.0;
|
||||
out_7031372799249316450[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_5425352922863285243) {
|
||||
out_5425352922863285243[0] = state[0];
|
||||
out_5425352922863285243[1] = state[1];
|
||||
out_5425352922863285243[2] = state[2];
|
||||
out_5425352922863285243[3] = state[3];
|
||||
out_5425352922863285243[4] = state[4];
|
||||
out_5425352922863285243[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_5425352922863285243[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_5425352922863285243[7] = state[7];
|
||||
out_5425352922863285243[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_1116747486686820994) {
|
||||
out_1116747486686820994[0] = state[0];
|
||||
out_1116747486686820994[1] = state[1];
|
||||
out_1116747486686820994[2] = state[2];
|
||||
out_1116747486686820994[3] = state[3];
|
||||
out_1116747486686820994[4] = state[4];
|
||||
out_1116747486686820994[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_1116747486686820994[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_1116747486686820994[7] = state[7];
|
||||
out_1116747486686820994[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_5183523293737892740) {
|
||||
out_5183523293737892740[0] = 1;
|
||||
out_5183523293737892740[1] = 0;
|
||||
out_5183523293737892740[2] = 0;
|
||||
out_5183523293737892740[3] = 0;
|
||||
out_5183523293737892740[4] = 0;
|
||||
out_5183523293737892740[5] = 0;
|
||||
out_5183523293737892740[6] = 0;
|
||||
out_5183523293737892740[7] = 0;
|
||||
out_5183523293737892740[8] = 0;
|
||||
out_5183523293737892740[9] = 0;
|
||||
out_5183523293737892740[10] = 1;
|
||||
out_5183523293737892740[11] = 0;
|
||||
out_5183523293737892740[12] = 0;
|
||||
out_5183523293737892740[13] = 0;
|
||||
out_5183523293737892740[14] = 0;
|
||||
out_5183523293737892740[15] = 0;
|
||||
out_5183523293737892740[16] = 0;
|
||||
out_5183523293737892740[17] = 0;
|
||||
out_5183523293737892740[18] = 0;
|
||||
out_5183523293737892740[19] = 0;
|
||||
out_5183523293737892740[20] = 1;
|
||||
out_5183523293737892740[21] = 0;
|
||||
out_5183523293737892740[22] = 0;
|
||||
out_5183523293737892740[23] = 0;
|
||||
out_5183523293737892740[24] = 0;
|
||||
out_5183523293737892740[25] = 0;
|
||||
out_5183523293737892740[26] = 0;
|
||||
out_5183523293737892740[27] = 0;
|
||||
out_5183523293737892740[28] = 0;
|
||||
out_5183523293737892740[29] = 0;
|
||||
out_5183523293737892740[30] = 1;
|
||||
out_5183523293737892740[31] = 0;
|
||||
out_5183523293737892740[32] = 0;
|
||||
out_5183523293737892740[33] = 0;
|
||||
out_5183523293737892740[34] = 0;
|
||||
out_5183523293737892740[35] = 0;
|
||||
out_5183523293737892740[36] = 0;
|
||||
out_5183523293737892740[37] = 0;
|
||||
out_5183523293737892740[38] = 0;
|
||||
out_5183523293737892740[39] = 0;
|
||||
out_5183523293737892740[40] = 1;
|
||||
out_5183523293737892740[41] = 0;
|
||||
out_5183523293737892740[42] = 0;
|
||||
out_5183523293737892740[43] = 0;
|
||||
out_5183523293737892740[44] = 0;
|
||||
out_5183523293737892740[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_5183523293737892740[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_5183523293737892740[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5183523293737892740[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5183523293737892740[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_5183523293737892740[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_5183523293737892740[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_5183523293737892740[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5183523293737892740[53] = -9.8100000000000005*dt;
|
||||
out_5183523293737892740[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_5183523293737892740[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_5183523293737892740[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5183523293737892740[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5183523293737892740[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_5183523293737892740[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_5183523293737892740[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_5183523293737892740[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5183523293737892740[62] = 0;
|
||||
out_5183523293737892740[63] = 0;
|
||||
out_5183523293737892740[64] = 0;
|
||||
out_5183523293737892740[65] = 0;
|
||||
out_5183523293737892740[66] = 0;
|
||||
out_5183523293737892740[67] = 0;
|
||||
out_5183523293737892740[68] = 0;
|
||||
out_5183523293737892740[69] = 0;
|
||||
out_5183523293737892740[70] = 1;
|
||||
out_5183523293737892740[71] = 0;
|
||||
out_5183523293737892740[72] = 0;
|
||||
out_5183523293737892740[73] = 0;
|
||||
out_5183523293737892740[74] = 0;
|
||||
out_5183523293737892740[75] = 0;
|
||||
out_5183523293737892740[76] = 0;
|
||||
out_5183523293737892740[77] = 0;
|
||||
out_5183523293737892740[78] = 0;
|
||||
out_5183523293737892740[79] = 0;
|
||||
out_5183523293737892740[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_4929085835885003775) {
|
||||
out_4929085835885003775[0] = 1;
|
||||
out_4929085835885003775[1] = 0;
|
||||
out_4929085835885003775[2] = 0;
|
||||
out_4929085835885003775[3] = 0;
|
||||
out_4929085835885003775[4] = 0;
|
||||
out_4929085835885003775[5] = 0;
|
||||
out_4929085835885003775[6] = 0;
|
||||
out_4929085835885003775[7] = 0;
|
||||
out_4929085835885003775[8] = 0;
|
||||
out_4929085835885003775[9] = 0;
|
||||
out_4929085835885003775[10] = 1;
|
||||
out_4929085835885003775[11] = 0;
|
||||
out_4929085835885003775[12] = 0;
|
||||
out_4929085835885003775[13] = 0;
|
||||
out_4929085835885003775[14] = 0;
|
||||
out_4929085835885003775[15] = 0;
|
||||
out_4929085835885003775[16] = 0;
|
||||
out_4929085835885003775[17] = 0;
|
||||
out_4929085835885003775[18] = 0;
|
||||
out_4929085835885003775[19] = 0;
|
||||
out_4929085835885003775[20] = 1;
|
||||
out_4929085835885003775[21] = 0;
|
||||
out_4929085835885003775[22] = 0;
|
||||
out_4929085835885003775[23] = 0;
|
||||
out_4929085835885003775[24] = 0;
|
||||
out_4929085835885003775[25] = 0;
|
||||
out_4929085835885003775[26] = 0;
|
||||
out_4929085835885003775[27] = 0;
|
||||
out_4929085835885003775[28] = 0;
|
||||
out_4929085835885003775[29] = 0;
|
||||
out_4929085835885003775[30] = 1;
|
||||
out_4929085835885003775[31] = 0;
|
||||
out_4929085835885003775[32] = 0;
|
||||
out_4929085835885003775[33] = 0;
|
||||
out_4929085835885003775[34] = 0;
|
||||
out_4929085835885003775[35] = 0;
|
||||
out_4929085835885003775[36] = 0;
|
||||
out_4929085835885003775[37] = 0;
|
||||
out_4929085835885003775[38] = 0;
|
||||
out_4929085835885003775[39] = 0;
|
||||
out_4929085835885003775[40] = 1;
|
||||
out_4929085835885003775[41] = 0;
|
||||
out_4929085835885003775[42] = 0;
|
||||
out_4929085835885003775[43] = 0;
|
||||
out_4929085835885003775[44] = 0;
|
||||
out_4929085835885003775[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_4929085835885003775[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_4929085835885003775[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_4929085835885003775[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_4929085835885003775[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_4929085835885003775[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_4929085835885003775[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_4929085835885003775[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_4929085835885003775[53] = -9.8100000000000005*dt;
|
||||
out_4929085835885003775[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_4929085835885003775[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_4929085835885003775[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_4929085835885003775[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_4929085835885003775[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_4929085835885003775[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_4929085835885003775[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_4929085835885003775[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_4929085835885003775[62] = 0;
|
||||
out_4929085835885003775[63] = 0;
|
||||
out_4929085835885003775[64] = 0;
|
||||
out_4929085835885003775[65] = 0;
|
||||
out_4929085835885003775[66] = 0;
|
||||
out_4929085835885003775[67] = 0;
|
||||
out_4929085835885003775[68] = 0;
|
||||
out_4929085835885003775[69] = 0;
|
||||
out_4929085835885003775[70] = 1;
|
||||
out_4929085835885003775[71] = 0;
|
||||
out_4929085835885003775[72] = 0;
|
||||
out_4929085835885003775[73] = 0;
|
||||
out_4929085835885003775[74] = 0;
|
||||
out_4929085835885003775[75] = 0;
|
||||
out_4929085835885003775[76] = 0;
|
||||
out_4929085835885003775[77] = 0;
|
||||
out_4929085835885003775[78] = 0;
|
||||
out_4929085835885003775[79] = 0;
|
||||
out_4929085835885003775[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_1323322140775861569) {
|
||||
out_1323322140775861569[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_3988698768583113995) {
|
||||
out_3988698768583113995[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_2722446496047589311) {
|
||||
out_2722446496047589311[0] = 0;
|
||||
out_2722446496047589311[1] = 0;
|
||||
out_2722446496047589311[2] = 0;
|
||||
out_2722446496047589311[3] = 0;
|
||||
out_2722446496047589311[4] = 0;
|
||||
out_2722446496047589311[5] = 0;
|
||||
out_2722446496047589311[6] = 1;
|
||||
out_2722446496047589311[7] = 0;
|
||||
out_2722446496047589311[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_3508392149259824773) {
|
||||
out_3508392149259824773[0] = 0;
|
||||
out_3508392149259824773[1] = 0;
|
||||
out_3508392149259824773[2] = 0;
|
||||
out_3508392149259824773[3] = 0;
|
||||
out_3508392149259824773[4] = 0;
|
||||
out_3508392149259824773[5] = 0;
|
||||
out_3508392149259824773[6] = 1;
|
||||
out_3508392149259824773[7] = 0;
|
||||
out_3508392149259824773[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_9087948729282653086) {
|
||||
out_9087948729282653086[0] = state[4];
|
||||
out_9087948729282653086[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_4767470784617198679) {
|
||||
out_4767470784617198679[0] = state[4];
|
||||
out_4767470784617198679[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_3567443104831284710) {
|
||||
out_3567443104831284710[0] = 0;
|
||||
out_3567443104831284710[1] = 0;
|
||||
out_3567443104831284710[2] = 0;
|
||||
out_3567443104831284710[3] = 0;
|
||||
out_3567443104831284710[4] = 1;
|
||||
out_3567443104831284710[5] = 0;
|
||||
out_3567443104831284710[6] = 0;
|
||||
out_3567443104831284710[7] = 0;
|
||||
out_3567443104831284710[8] = 0;
|
||||
out_3567443104831284710[9] = 0;
|
||||
out_3567443104831284710[10] = 0;
|
||||
out_3567443104831284710[11] = 0;
|
||||
out_3567443104831284710[12] = 0;
|
||||
out_3567443104831284710[13] = 0;
|
||||
out_3567443104831284710[14] = 1;
|
||||
out_3567443104831284710[15] = 0;
|
||||
out_3567443104831284710[16] = 0;
|
||||
out_3567443104831284710[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_8254901476226333807) {
|
||||
out_8254901476226333807[0] = 0;
|
||||
out_8254901476226333807[1] = 0;
|
||||
out_8254901476226333807[2] = 0;
|
||||
out_8254901476226333807[3] = 0;
|
||||
out_8254901476226333807[4] = 1;
|
||||
out_8254901476226333807[5] = 0;
|
||||
out_8254901476226333807[6] = 0;
|
||||
out_8254901476226333807[7] = 0;
|
||||
out_8254901476226333807[8] = 0;
|
||||
out_8254901476226333807[9] = 0;
|
||||
out_8254901476226333807[10] = 0;
|
||||
out_8254901476226333807[11] = 0;
|
||||
out_8254901476226333807[12] = 0;
|
||||
out_8254901476226333807[13] = 0;
|
||||
out_8254901476226333807[14] = 1;
|
||||
out_8254901476226333807[15] = 0;
|
||||
out_8254901476226333807[16] = 0;
|
||||
out_8254901476226333807[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_1374282254885888156) {
|
||||
out_1374282254885888156[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_7626148296837136345) {
|
||||
out_7626148296837136345[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_8807607236170345550) {
|
||||
out_8807607236170345550[0] = 0;
|
||||
out_8807607236170345550[1] = 0;
|
||||
out_8807607236170345550[2] = 0;
|
||||
out_8807607236170345550[3] = 0;
|
||||
out_8807607236170345550[4] = 1;
|
||||
out_8807607236170345550[5] = 0;
|
||||
out_8807607236170345550[6] = 0;
|
||||
out_8807607236170345550[7] = 0;
|
||||
out_8807607236170345550[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_3408298192231791982) {
|
||||
out_3408298192231791982[0] = 0;
|
||||
out_3408298192231791982[1] = 0;
|
||||
out_3408298192231791982[2] = 0;
|
||||
out_3408298192231791982[3] = 0;
|
||||
out_3408298192231791982[4] = 1;
|
||||
out_3408298192231791982[5] = 0;
|
||||
out_3408298192231791982[6] = 0;
|
||||
out_3408298192231791982[7] = 0;
|
||||
out_3408298192231791982[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_2542026055962029893) {
|
||||
out_2542026055962029893[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_2624475029176290700) {
|
||||
out_2624475029176290700[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_1019056822826466913) {
|
||||
out_1019056822826466913[0] = 0;
|
||||
out_1019056822826466913[1] = 0;
|
||||
out_1019056822826466913[2] = 0;
|
||||
out_1019056822826466913[3] = 0;
|
||||
out_1019056822826466913[4] = 0;
|
||||
out_1019056822826466913[5] = 0;
|
||||
out_1019056822826466913[6] = 0;
|
||||
out_1019056822826466913[7] = 1;
|
||||
out_1019056822826466913[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_7249895468133880997) {
|
||||
out_7249895468133880997[0] = 0;
|
||||
out_7249895468133880997[1] = 0;
|
||||
out_7249895468133880997[2] = 0;
|
||||
out_7249895468133880997[3] = 0;
|
||||
out_7249895468133880997[4] = 0;
|
||||
out_7249895468133880997[5] = 0;
|
||||
out_7249895468133880997[6] = 0;
|
||||
out_7249895468133880997[7] = 1;
|
||||
out_7249895468133880997[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_1281083180032836082) {
|
||||
out_1281083180032836082[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_572425478934043979) {
|
||||
out_572425478934043979[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_7464373525738781155) {
|
||||
out_7464373525738781155[0] = 0;
|
||||
out_7464373525738781155[1] = 0;
|
||||
out_7464373525738781155[2] = 0;
|
||||
out_7464373525738781155[3] = 1;
|
||||
out_7464373525738781155[4] = 0;
|
||||
out_7464373525738781155[5] = 0;
|
||||
out_7464373525738781155[6] = 0;
|
||||
out_7464373525738781155[7] = 0;
|
||||
out_7464373525738781155[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_1233534880431367071) {
|
||||
out_1233534880431367071[0] = 0;
|
||||
out_1233534880431367071[1] = 0;
|
||||
out_1233534880431367071[2] = 0;
|
||||
out_1233534880431367071[3] = 1;
|
||||
out_1233534880431367071[4] = 0;
|
||||
out_1233534880431367071[5] = 0;
|
||||
out_1233534880431367071[6] = 0;
|
||||
out_1233534880431367071[7] = 0;
|
||||
out_1233534880431367071[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_1438269848383965227) {
|
||||
out_1438269848383965227[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_5818922509211569626) {
|
||||
out_5818922509211569626[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_8297375891855953366) {
|
||||
out_8297375891855953366[0] = 0;
|
||||
out_8297375891855953366[1] = 1;
|
||||
out_8297375891855953366[2] = 0;
|
||||
out_8297375891855953366[3] = 0;
|
||||
out_8297375891855953366[4] = 0;
|
||||
out_8297375891855953366[5] = 0;
|
||||
out_8297375891855953366[6] = 0;
|
||||
out_8297375891855953366[7] = 0;
|
||||
out_8297375891855953366[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_3918529536546184166) {
|
||||
out_3918529536546184166[0] = 0;
|
||||
out_3918529536546184166[1] = 1;
|
||||
out_3918529536546184166[2] = 0;
|
||||
out_3918529536546184166[3] = 0;
|
||||
out_3918529536546184166[4] = 0;
|
||||
out_3918529536546184166[5] = 0;
|
||||
out_3918529536546184166[6] = 0;
|
||||
out_3918529536546184166[7] = 0;
|
||||
out_3918529536546184166[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_677767295356358565) {
|
||||
out_677767295356358565[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_4389954518747249253) {
|
||||
out_4389954518747249253[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_668611781799699548) {
|
||||
out_668611781799699548[0] = 1;
|
||||
out_668611781799699548[1] = 0;
|
||||
out_668611781799699548[2] = 0;
|
||||
out_668611781799699548[3] = 0;
|
||||
out_668611781799699548[4] = 0;
|
||||
out_668611781799699548[5] = 0;
|
||||
out_668611781799699548[6] = 0;
|
||||
out_668611781799699548[7] = 0;
|
||||
out_668611781799699548[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_5562226863507714536) {
|
||||
out_5562226863507714536[0] = 1;
|
||||
out_5562226863507714536[1] = 0;
|
||||
out_5562226863507714536[2] = 0;
|
||||
out_5562226863507714536[3] = 0;
|
||||
out_5562226863507714536[4] = 0;
|
||||
out_5562226863507714536[5] = 0;
|
||||
out_5562226863507714536[6] = 0;
|
||||
out_5562226863507714536[7] = 0;
|
||||
out_5562226863507714536[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_2538361335652640083) {
|
||||
out_2538361335652640083[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_3713504706298608106) {
|
||||
out_3713504706298608106[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_2753092457924549739) {
|
||||
out_2753092457924549739[0] = 0;
|
||||
out_2753092457924549739[1] = 0;
|
||||
out_2753092457924549739[2] = 0;
|
||||
out_2753092457924549739[3] = 0;
|
||||
out_2753092457924549739[4] = 0;
|
||||
out_2753092457924549739[5] = 0;
|
||||
out_2753092457924549739[6] = 0;
|
||||
out_2753092457924549739[7] = 0;
|
||||
out_2753092457924549739[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_3477746187382864345) {
|
||||
out_3477746187382864345[0] = 0;
|
||||
out_3477746187382864345[1] = 0;
|
||||
out_3477746187382864345[2] = 0;
|
||||
out_3477746187382864345[3] = 0;
|
||||
out_3477746187382864345[4] = 0;
|
||||
out_3477746187382864345[5] = 0;
|
||||
out_3477746187382864345[6] = 0;
|
||||
out_3477746187382864345[7] = 0;
|
||||
out_3477746187382864345[8] = 1;
|
||||
}
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
@@ -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_191040875433637227) {
|
||||
err_fun(nom_x, delta_x, out_191040875433637227);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_2204053027494663001) {
|
||||
err_fun(nom_x, delta_x, out_2204053027494663001);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_449478399145670770) {
|
||||
inv_err_fun(nom_x, true_x, out_449478399145670770);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_2041279983635365065) {
|
||||
inv_err_fun(nom_x, true_x, out_2041279983635365065);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_2217112053847292599) {
|
||||
H_mod_fun(state, out_2217112053847292599);
|
||||
void car_H_mod_fun(double *state, double *out_7031372799249316450) {
|
||||
H_mod_fun(state, out_7031372799249316450);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_5425352922863285243) {
|
||||
f_fun(state, dt, out_5425352922863285243);
|
||||
void car_f_fun(double *state, double dt, double *out_1116747486686820994) {
|
||||
f_fun(state, dt, out_1116747486686820994);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_5183523293737892740) {
|
||||
F_fun(state, dt, out_5183523293737892740);
|
||||
void car_F_fun(double *state, double dt, double *out_4929085835885003775) {
|
||||
F_fun(state, dt, out_4929085835885003775);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_1323322140775861569) {
|
||||
h_25(state, unused, out_1323322140775861569);
|
||||
void car_h_25(double *state, double *unused, double *out_3988698768583113995) {
|
||||
h_25(state, unused, out_3988698768583113995);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_2722446496047589311) {
|
||||
H_25(state, unused, out_2722446496047589311);
|
||||
void car_H_25(double *state, double *unused, double *out_3508392149259824773) {
|
||||
H_25(state, unused, out_3508392149259824773);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_9087948729282653086) {
|
||||
h_24(state, unused, out_9087948729282653086);
|
||||
void car_h_24(double *state, double *unused, double *out_4767470784617198679) {
|
||||
h_24(state, unused, out_4767470784617198679);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_3567443104831284710) {
|
||||
H_24(state, unused, out_3567443104831284710);
|
||||
void car_H_24(double *state, double *unused, double *out_8254901476226333807) {
|
||||
H_24(state, unused, out_8254901476226333807);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_1374282254885888156) {
|
||||
h_30(state, unused, out_1374282254885888156);
|
||||
void car_h_30(double *state, double *unused, double *out_7626148296837136345) {
|
||||
h_30(state, unused, out_7626148296837136345);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_8807607236170345550) {
|
||||
H_30(state, unused, out_8807607236170345550);
|
||||
void car_H_30(double *state, double *unused, double *out_3408298192231791982) {
|
||||
H_30(state, unused, out_3408298192231791982);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_2542026055962029893) {
|
||||
h_26(state, unused, out_2542026055962029893);
|
||||
void car_h_26(double *state, double *unused, double *out_2624475029176290700) {
|
||||
h_26(state, unused, out_2624475029176290700);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_1019056822826466913) {
|
||||
H_26(state, unused, out_1019056822826466913);
|
||||
void car_H_26(double *state, double *unused, double *out_7249895468133880997) {
|
||||
H_26(state, unused, out_7249895468133880997);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_1281083180032836082) {
|
||||
h_27(state, unused, out_1281083180032836082);
|
||||
void car_h_27(double *state, double *unused, double *out_572425478934043979) {
|
||||
h_27(state, unused, out_572425478934043979);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_7464373525738781155) {
|
||||
H_27(state, unused, out_7464373525738781155);
|
||||
void car_H_27(double *state, double *unused, double *out_1233534880431367071) {
|
||||
H_27(state, unused, out_1233534880431367071);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_1438269848383965227) {
|
||||
h_29(state, unused, out_1438269848383965227);
|
||||
void car_h_29(double *state, double *unused, double *out_5818922509211569626) {
|
||||
h_29(state, unused, out_5818922509211569626);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_8297375891855953366) {
|
||||
H_29(state, unused, out_8297375891855953366);
|
||||
void car_H_29(double *state, double *unused, double *out_3918529536546184166) {
|
||||
H_29(state, unused, out_3918529536546184166);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_677767295356358565) {
|
||||
h_28(state, unused, out_677767295356358565);
|
||||
void car_h_28(double *state, double *unused, double *out_4389954518747249253) {
|
||||
h_28(state, unused, out_4389954518747249253);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_668611781799699548) {
|
||||
H_28(state, unused, out_668611781799699548);
|
||||
void car_H_28(double *state, double *unused, double *out_5562226863507714536) {
|
||||
H_28(state, unused, out_5562226863507714536);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_2538361335652640083) {
|
||||
h_31(state, unused, out_2538361335652640083);
|
||||
void car_h_31(double *state, double *unused, double *out_3713504706298608106) {
|
||||
h_31(state, unused, out_3713504706298608106);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_2753092457924549739) {
|
||||
H_31(state, unused, out_2753092457924549739);
|
||||
void car_H_31(double *state, double *unused, double *out_3477746187382864345) {
|
||||
H_31(state, unused, out_3477746187382864345);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -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_191040875433637227);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_449478399145670770);
|
||||
void car_H_mod_fun(double *state, double *out_2217112053847292599);
|
||||
void car_f_fun(double *state, double dt, double *out_5425352922863285243);
|
||||
void car_F_fun(double *state, double dt, double *out_5183523293737892740);
|
||||
void car_h_25(double *state, double *unused, double *out_1323322140775861569);
|
||||
void car_H_25(double *state, double *unused, double *out_2722446496047589311);
|
||||
void car_h_24(double *state, double *unused, double *out_9087948729282653086);
|
||||
void car_H_24(double *state, double *unused, double *out_3567443104831284710);
|
||||
void car_h_30(double *state, double *unused, double *out_1374282254885888156);
|
||||
void car_H_30(double *state, double *unused, double *out_8807607236170345550);
|
||||
void car_h_26(double *state, double *unused, double *out_2542026055962029893);
|
||||
void car_H_26(double *state, double *unused, double *out_1019056822826466913);
|
||||
void car_h_27(double *state, double *unused, double *out_1281083180032836082);
|
||||
void car_H_27(double *state, double *unused, double *out_7464373525738781155);
|
||||
void car_h_29(double *state, double *unused, double *out_1438269848383965227);
|
||||
void car_H_29(double *state, double *unused, double *out_8297375891855953366);
|
||||
void car_h_28(double *state, double *unused, double *out_677767295356358565);
|
||||
void car_H_28(double *state, double *unused, double *out_668611781799699548);
|
||||
void car_h_31(double *state, double *unused, double *out_2538361335652640083);
|
||||
void car_H_31(double *state, double *unused, double *out_2753092457924549739);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_2204053027494663001);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_2041279983635365065);
|
||||
void car_H_mod_fun(double *state, double *out_7031372799249316450);
|
||||
void car_f_fun(double *state, double dt, double *out_1116747486686820994);
|
||||
void car_F_fun(double *state, double dt, double *out_4929085835885003775);
|
||||
void car_h_25(double *state, double *unused, double *out_3988698768583113995);
|
||||
void car_H_25(double *state, double *unused, double *out_3508392149259824773);
|
||||
void car_h_24(double *state, double *unused, double *out_4767470784617198679);
|
||||
void car_H_24(double *state, double *unused, double *out_8254901476226333807);
|
||||
void car_h_30(double *state, double *unused, double *out_7626148296837136345);
|
||||
void car_H_30(double *state, double *unused, double *out_3408298192231791982);
|
||||
void car_h_26(double *state, double *unused, double *out_2624475029176290700);
|
||||
void car_H_26(double *state, double *unused, double *out_7249895468133880997);
|
||||
void car_h_27(double *state, double *unused, double *out_572425478934043979);
|
||||
void car_H_27(double *state, double *unused, double *out_1233534880431367071);
|
||||
void car_h_29(double *state, double *unused, double *out_5818922509211569626);
|
||||
void car_H_29(double *state, double *unused, double *out_3918529536546184166);
|
||||
void car_h_28(double *state, double *unused, double *out_4389954518747249253);
|
||||
void car_H_28(double *state, double *unused, double *out_5562226863507714536);
|
||||
void car_h_31(double *state, double *unused, double *out_3713504706298608106);
|
||||
void car_H_31(double *state, double *unused, double *out_3477746187382864345);
|
||||
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);
|
||||
|
||||
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -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_8386835163844872263);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5826204590882848019);
|
||||
void pose_H_mod_fun(double *state, double *out_7521593534267777079);
|
||||
void pose_f_fun(double *state, double dt, double *out_8190127916652503129);
|
||||
void pose_F_fun(double *state, double dt, double *out_8302830792992532360);
|
||||
void pose_h_4(double *state, double *unused, double *out_6664200415740758039);
|
||||
void pose_H_4(double *state, double *unused, double *out_5772632154402693802);
|
||||
void pose_h_10(double *state, double *unused, double *out_2232580687525306632);
|
||||
void pose_H_10(double *state, double *unused, double *out_1994505413045679835);
|
||||
void pose_h_13(double *state, double *unused, double *out_1739439411373720736);
|
||||
void pose_H_13(double *state, double *unused, double *out_2560358329070361001);
|
||||
void pose_h_14(double *state, double *unused, double *out_2162487226046509469);
|
||||
void pose_H_14(double *state, double *unused, double *out_1809391298063209273);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_7675941765646048454);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6578985106349638079);
|
||||
void pose_H_mod_fun(double *state, double *out_8249219982215908455);
|
||||
void pose_f_fun(double *state, double dt, double *out_741196485394149917);
|
||||
void pose_F_fun(double *state, double dt, double *out_914245573923806659);
|
||||
void pose_h_4(double *state, double *unused, double *out_1661592600804563205);
|
||||
void pose_H_4(double *state, double *unused, double *out_3634592945286592091);
|
||||
void pose_h_10(double *state, double *unused, double *out_1540829144479980375);
|
||||
void pose_H_10(double *state, double *unused, double *out_3859318150895892325);
|
||||
void pose_h_13(double *state, double *unused, double *out_1494548337638663963);
|
||||
void pose_H_13(double *state, double *unused, double *out_422319119954259290);
|
||||
void pose_h_14(double *state, double *unused, double *out_4998597365434596125);
|
||||
void pose_H_14(double *state, double *unused, double *out_328647911052892438);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,4 +1,4 @@
|
||||
#pragma once
|
||||
|
||||
extern const uint8_t gitversion[29];
|
||||
const uint8_t gitversion[29] = "DEV-a2602aa3-P92eb5651-DEBUG";
|
||||
const uint8_t gitversion[29] = "DEV-44048a30-P92eb5651-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
DEV-a2602aa3-P92eb5651-DEBUG
|
||||
DEV-44048a30-P92eb5651-DEBUG
|
||||
+1
-1
@@ -1 +1 @@
|
||||
{"protected_inventory_sha256": "c2e4e536170d6b29ca3772814021cac69f5e5cf68514fc1a4c6eb513fe34a671", "schema_version": 1}
|
||||
{"protected_inventory_sha256": "b4e686661d5f6afd9c25218e0374c35fc155aca5019ba5f579cd5f71341dda93", "schema_version": 1}
|
||||
|
||||
Reference in New Issue
Block a user