mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-03 11:09:55 +08:00
Update Device 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_4191231468621882354) {
|
||||
out_4191231468621882354[0] = delta_x[0] + nom_x[0];
|
||||
out_4191231468621882354[1] = delta_x[1] + nom_x[1];
|
||||
out_4191231468621882354[2] = delta_x[2] + nom_x[2];
|
||||
out_4191231468621882354[3] = delta_x[3] + nom_x[3];
|
||||
out_4191231468621882354[4] = delta_x[4] + nom_x[4];
|
||||
out_4191231468621882354[5] = delta_x[5] + nom_x[5];
|
||||
out_4191231468621882354[6] = delta_x[6] + nom_x[6];
|
||||
out_4191231468621882354[7] = delta_x[7] + nom_x[7];
|
||||
out_4191231468621882354[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_8532074412831680005) {
|
||||
out_8532074412831680005[0] = delta_x[0] + nom_x[0];
|
||||
out_8532074412831680005[1] = delta_x[1] + nom_x[1];
|
||||
out_8532074412831680005[2] = delta_x[2] + nom_x[2];
|
||||
out_8532074412831680005[3] = delta_x[3] + nom_x[3];
|
||||
out_8532074412831680005[4] = delta_x[4] + nom_x[4];
|
||||
out_8532074412831680005[5] = delta_x[5] + nom_x[5];
|
||||
out_8532074412831680005[6] = delta_x[6] + nom_x[6];
|
||||
out_8532074412831680005[7] = delta_x[7] + nom_x[7];
|
||||
out_8532074412831680005[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_4409511713992133464) {
|
||||
out_4409511713992133464[0] = -nom_x[0] + true_x[0];
|
||||
out_4409511713992133464[1] = -nom_x[1] + true_x[1];
|
||||
out_4409511713992133464[2] = -nom_x[2] + true_x[2];
|
||||
out_4409511713992133464[3] = -nom_x[3] + true_x[3];
|
||||
out_4409511713992133464[4] = -nom_x[4] + true_x[4];
|
||||
out_4409511713992133464[5] = -nom_x[5] + true_x[5];
|
||||
out_4409511713992133464[6] = -nom_x[6] + true_x[6];
|
||||
out_4409511713992133464[7] = -nom_x[7] + true_x[7];
|
||||
out_4409511713992133464[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8887769849182574222) {
|
||||
out_8887769849182574222[0] = -nom_x[0] + true_x[0];
|
||||
out_8887769849182574222[1] = -nom_x[1] + true_x[1];
|
||||
out_8887769849182574222[2] = -nom_x[2] + true_x[2];
|
||||
out_8887769849182574222[3] = -nom_x[3] + true_x[3];
|
||||
out_8887769849182574222[4] = -nom_x[4] + true_x[4];
|
||||
out_8887769849182574222[5] = -nom_x[5] + true_x[5];
|
||||
out_8887769849182574222[6] = -nom_x[6] + true_x[6];
|
||||
out_8887769849182574222[7] = -nom_x[7] + true_x[7];
|
||||
out_8887769849182574222[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_7704822479144562897) {
|
||||
out_7704822479144562897[0] = 1.0;
|
||||
out_7704822479144562897[1] = 0.0;
|
||||
out_7704822479144562897[2] = 0.0;
|
||||
out_7704822479144562897[3] = 0.0;
|
||||
out_7704822479144562897[4] = 0.0;
|
||||
out_7704822479144562897[5] = 0.0;
|
||||
out_7704822479144562897[6] = 0.0;
|
||||
out_7704822479144562897[7] = 0.0;
|
||||
out_7704822479144562897[8] = 0.0;
|
||||
out_7704822479144562897[9] = 0.0;
|
||||
out_7704822479144562897[10] = 1.0;
|
||||
out_7704822479144562897[11] = 0.0;
|
||||
out_7704822479144562897[12] = 0.0;
|
||||
out_7704822479144562897[13] = 0.0;
|
||||
out_7704822479144562897[14] = 0.0;
|
||||
out_7704822479144562897[15] = 0.0;
|
||||
out_7704822479144562897[16] = 0.0;
|
||||
out_7704822479144562897[17] = 0.0;
|
||||
out_7704822479144562897[18] = 0.0;
|
||||
out_7704822479144562897[19] = 0.0;
|
||||
out_7704822479144562897[20] = 1.0;
|
||||
out_7704822479144562897[21] = 0.0;
|
||||
out_7704822479144562897[22] = 0.0;
|
||||
out_7704822479144562897[23] = 0.0;
|
||||
out_7704822479144562897[24] = 0.0;
|
||||
out_7704822479144562897[25] = 0.0;
|
||||
out_7704822479144562897[26] = 0.0;
|
||||
out_7704822479144562897[27] = 0.0;
|
||||
out_7704822479144562897[28] = 0.0;
|
||||
out_7704822479144562897[29] = 0.0;
|
||||
out_7704822479144562897[30] = 1.0;
|
||||
out_7704822479144562897[31] = 0.0;
|
||||
out_7704822479144562897[32] = 0.0;
|
||||
out_7704822479144562897[33] = 0.0;
|
||||
out_7704822479144562897[34] = 0.0;
|
||||
out_7704822479144562897[35] = 0.0;
|
||||
out_7704822479144562897[36] = 0.0;
|
||||
out_7704822479144562897[37] = 0.0;
|
||||
out_7704822479144562897[38] = 0.0;
|
||||
out_7704822479144562897[39] = 0.0;
|
||||
out_7704822479144562897[40] = 1.0;
|
||||
out_7704822479144562897[41] = 0.0;
|
||||
out_7704822479144562897[42] = 0.0;
|
||||
out_7704822479144562897[43] = 0.0;
|
||||
out_7704822479144562897[44] = 0.0;
|
||||
out_7704822479144562897[45] = 0.0;
|
||||
out_7704822479144562897[46] = 0.0;
|
||||
out_7704822479144562897[47] = 0.0;
|
||||
out_7704822479144562897[48] = 0.0;
|
||||
out_7704822479144562897[49] = 0.0;
|
||||
out_7704822479144562897[50] = 1.0;
|
||||
out_7704822479144562897[51] = 0.0;
|
||||
out_7704822479144562897[52] = 0.0;
|
||||
out_7704822479144562897[53] = 0.0;
|
||||
out_7704822479144562897[54] = 0.0;
|
||||
out_7704822479144562897[55] = 0.0;
|
||||
out_7704822479144562897[56] = 0.0;
|
||||
out_7704822479144562897[57] = 0.0;
|
||||
out_7704822479144562897[58] = 0.0;
|
||||
out_7704822479144562897[59] = 0.0;
|
||||
out_7704822479144562897[60] = 1.0;
|
||||
out_7704822479144562897[61] = 0.0;
|
||||
out_7704822479144562897[62] = 0.0;
|
||||
out_7704822479144562897[63] = 0.0;
|
||||
out_7704822479144562897[64] = 0.0;
|
||||
out_7704822479144562897[65] = 0.0;
|
||||
out_7704822479144562897[66] = 0.0;
|
||||
out_7704822479144562897[67] = 0.0;
|
||||
out_7704822479144562897[68] = 0.0;
|
||||
out_7704822479144562897[69] = 0.0;
|
||||
out_7704822479144562897[70] = 1.0;
|
||||
out_7704822479144562897[71] = 0.0;
|
||||
out_7704822479144562897[72] = 0.0;
|
||||
out_7704822479144562897[73] = 0.0;
|
||||
out_7704822479144562897[74] = 0.0;
|
||||
out_7704822479144562897[75] = 0.0;
|
||||
out_7704822479144562897[76] = 0.0;
|
||||
out_7704822479144562897[77] = 0.0;
|
||||
out_7704822479144562897[78] = 0.0;
|
||||
out_7704822479144562897[79] = 0.0;
|
||||
out_7704822479144562897[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_2434822093009109592) {
|
||||
out_2434822093009109592[0] = 1.0;
|
||||
out_2434822093009109592[1] = 0.0;
|
||||
out_2434822093009109592[2] = 0.0;
|
||||
out_2434822093009109592[3] = 0.0;
|
||||
out_2434822093009109592[4] = 0.0;
|
||||
out_2434822093009109592[5] = 0.0;
|
||||
out_2434822093009109592[6] = 0.0;
|
||||
out_2434822093009109592[7] = 0.0;
|
||||
out_2434822093009109592[8] = 0.0;
|
||||
out_2434822093009109592[9] = 0.0;
|
||||
out_2434822093009109592[10] = 1.0;
|
||||
out_2434822093009109592[11] = 0.0;
|
||||
out_2434822093009109592[12] = 0.0;
|
||||
out_2434822093009109592[13] = 0.0;
|
||||
out_2434822093009109592[14] = 0.0;
|
||||
out_2434822093009109592[15] = 0.0;
|
||||
out_2434822093009109592[16] = 0.0;
|
||||
out_2434822093009109592[17] = 0.0;
|
||||
out_2434822093009109592[18] = 0.0;
|
||||
out_2434822093009109592[19] = 0.0;
|
||||
out_2434822093009109592[20] = 1.0;
|
||||
out_2434822093009109592[21] = 0.0;
|
||||
out_2434822093009109592[22] = 0.0;
|
||||
out_2434822093009109592[23] = 0.0;
|
||||
out_2434822093009109592[24] = 0.0;
|
||||
out_2434822093009109592[25] = 0.0;
|
||||
out_2434822093009109592[26] = 0.0;
|
||||
out_2434822093009109592[27] = 0.0;
|
||||
out_2434822093009109592[28] = 0.0;
|
||||
out_2434822093009109592[29] = 0.0;
|
||||
out_2434822093009109592[30] = 1.0;
|
||||
out_2434822093009109592[31] = 0.0;
|
||||
out_2434822093009109592[32] = 0.0;
|
||||
out_2434822093009109592[33] = 0.0;
|
||||
out_2434822093009109592[34] = 0.0;
|
||||
out_2434822093009109592[35] = 0.0;
|
||||
out_2434822093009109592[36] = 0.0;
|
||||
out_2434822093009109592[37] = 0.0;
|
||||
out_2434822093009109592[38] = 0.0;
|
||||
out_2434822093009109592[39] = 0.0;
|
||||
out_2434822093009109592[40] = 1.0;
|
||||
out_2434822093009109592[41] = 0.0;
|
||||
out_2434822093009109592[42] = 0.0;
|
||||
out_2434822093009109592[43] = 0.0;
|
||||
out_2434822093009109592[44] = 0.0;
|
||||
out_2434822093009109592[45] = 0.0;
|
||||
out_2434822093009109592[46] = 0.0;
|
||||
out_2434822093009109592[47] = 0.0;
|
||||
out_2434822093009109592[48] = 0.0;
|
||||
out_2434822093009109592[49] = 0.0;
|
||||
out_2434822093009109592[50] = 1.0;
|
||||
out_2434822093009109592[51] = 0.0;
|
||||
out_2434822093009109592[52] = 0.0;
|
||||
out_2434822093009109592[53] = 0.0;
|
||||
out_2434822093009109592[54] = 0.0;
|
||||
out_2434822093009109592[55] = 0.0;
|
||||
out_2434822093009109592[56] = 0.0;
|
||||
out_2434822093009109592[57] = 0.0;
|
||||
out_2434822093009109592[58] = 0.0;
|
||||
out_2434822093009109592[59] = 0.0;
|
||||
out_2434822093009109592[60] = 1.0;
|
||||
out_2434822093009109592[61] = 0.0;
|
||||
out_2434822093009109592[62] = 0.0;
|
||||
out_2434822093009109592[63] = 0.0;
|
||||
out_2434822093009109592[64] = 0.0;
|
||||
out_2434822093009109592[65] = 0.0;
|
||||
out_2434822093009109592[66] = 0.0;
|
||||
out_2434822093009109592[67] = 0.0;
|
||||
out_2434822093009109592[68] = 0.0;
|
||||
out_2434822093009109592[69] = 0.0;
|
||||
out_2434822093009109592[70] = 1.0;
|
||||
out_2434822093009109592[71] = 0.0;
|
||||
out_2434822093009109592[72] = 0.0;
|
||||
out_2434822093009109592[73] = 0.0;
|
||||
out_2434822093009109592[74] = 0.0;
|
||||
out_2434822093009109592[75] = 0.0;
|
||||
out_2434822093009109592[76] = 0.0;
|
||||
out_2434822093009109592[77] = 0.0;
|
||||
out_2434822093009109592[78] = 0.0;
|
||||
out_2434822093009109592[79] = 0.0;
|
||||
out_2434822093009109592[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_2423168834336223124) {
|
||||
out_2423168834336223124[0] = state[0];
|
||||
out_2423168834336223124[1] = state[1];
|
||||
out_2423168834336223124[2] = state[2];
|
||||
out_2423168834336223124[3] = state[3];
|
||||
out_2423168834336223124[4] = state[4];
|
||||
out_2423168834336223124[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_2423168834336223124[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_2423168834336223124[7] = state[7];
|
||||
out_2423168834336223124[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_7489300736878445586) {
|
||||
out_7489300736878445586[0] = state[0];
|
||||
out_7489300736878445586[1] = state[1];
|
||||
out_7489300736878445586[2] = state[2];
|
||||
out_7489300736878445586[3] = state[3];
|
||||
out_7489300736878445586[4] = state[4];
|
||||
out_7489300736878445586[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_7489300736878445586[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_7489300736878445586[7] = state[7];
|
||||
out_7489300736878445586[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_7639588574330807467) {
|
||||
out_7639588574330807467[0] = 1;
|
||||
out_7639588574330807467[1] = 0;
|
||||
out_7639588574330807467[2] = 0;
|
||||
out_7639588574330807467[3] = 0;
|
||||
out_7639588574330807467[4] = 0;
|
||||
out_7639588574330807467[5] = 0;
|
||||
out_7639588574330807467[6] = 0;
|
||||
out_7639588574330807467[7] = 0;
|
||||
out_7639588574330807467[8] = 0;
|
||||
out_7639588574330807467[9] = 0;
|
||||
out_7639588574330807467[10] = 1;
|
||||
out_7639588574330807467[11] = 0;
|
||||
out_7639588574330807467[12] = 0;
|
||||
out_7639588574330807467[13] = 0;
|
||||
out_7639588574330807467[14] = 0;
|
||||
out_7639588574330807467[15] = 0;
|
||||
out_7639588574330807467[16] = 0;
|
||||
out_7639588574330807467[17] = 0;
|
||||
out_7639588574330807467[18] = 0;
|
||||
out_7639588574330807467[19] = 0;
|
||||
out_7639588574330807467[20] = 1;
|
||||
out_7639588574330807467[21] = 0;
|
||||
out_7639588574330807467[22] = 0;
|
||||
out_7639588574330807467[23] = 0;
|
||||
out_7639588574330807467[24] = 0;
|
||||
out_7639588574330807467[25] = 0;
|
||||
out_7639588574330807467[26] = 0;
|
||||
out_7639588574330807467[27] = 0;
|
||||
out_7639588574330807467[28] = 0;
|
||||
out_7639588574330807467[29] = 0;
|
||||
out_7639588574330807467[30] = 1;
|
||||
out_7639588574330807467[31] = 0;
|
||||
out_7639588574330807467[32] = 0;
|
||||
out_7639588574330807467[33] = 0;
|
||||
out_7639588574330807467[34] = 0;
|
||||
out_7639588574330807467[35] = 0;
|
||||
out_7639588574330807467[36] = 0;
|
||||
out_7639588574330807467[37] = 0;
|
||||
out_7639588574330807467[38] = 0;
|
||||
out_7639588574330807467[39] = 0;
|
||||
out_7639588574330807467[40] = 1;
|
||||
out_7639588574330807467[41] = 0;
|
||||
out_7639588574330807467[42] = 0;
|
||||
out_7639588574330807467[43] = 0;
|
||||
out_7639588574330807467[44] = 0;
|
||||
out_7639588574330807467[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_7639588574330807467[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_7639588574330807467[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7639588574330807467[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7639588574330807467[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_7639588574330807467[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_7639588574330807467[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_7639588574330807467[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7639588574330807467[53] = -9.8100000000000005*dt;
|
||||
out_7639588574330807467[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_7639588574330807467[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_7639588574330807467[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7639588574330807467[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7639588574330807467[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_7639588574330807467[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_7639588574330807467[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_7639588574330807467[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7639588574330807467[62] = 0;
|
||||
out_7639588574330807467[63] = 0;
|
||||
out_7639588574330807467[64] = 0;
|
||||
out_7639588574330807467[65] = 0;
|
||||
out_7639588574330807467[66] = 0;
|
||||
out_7639588574330807467[67] = 0;
|
||||
out_7639588574330807467[68] = 0;
|
||||
out_7639588574330807467[69] = 0;
|
||||
out_7639588574330807467[70] = 1;
|
||||
out_7639588574330807467[71] = 0;
|
||||
out_7639588574330807467[72] = 0;
|
||||
out_7639588574330807467[73] = 0;
|
||||
out_7639588574330807467[74] = 0;
|
||||
out_7639588574330807467[75] = 0;
|
||||
out_7639588574330807467[76] = 0;
|
||||
out_7639588574330807467[77] = 0;
|
||||
out_7639588574330807467[78] = 0;
|
||||
out_7639588574330807467[79] = 0;
|
||||
out_7639588574330807467[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_5688594807383263913) {
|
||||
out_5688594807383263913[0] = 1;
|
||||
out_5688594807383263913[1] = 0;
|
||||
out_5688594807383263913[2] = 0;
|
||||
out_5688594807383263913[3] = 0;
|
||||
out_5688594807383263913[4] = 0;
|
||||
out_5688594807383263913[5] = 0;
|
||||
out_5688594807383263913[6] = 0;
|
||||
out_5688594807383263913[7] = 0;
|
||||
out_5688594807383263913[8] = 0;
|
||||
out_5688594807383263913[9] = 0;
|
||||
out_5688594807383263913[10] = 1;
|
||||
out_5688594807383263913[11] = 0;
|
||||
out_5688594807383263913[12] = 0;
|
||||
out_5688594807383263913[13] = 0;
|
||||
out_5688594807383263913[14] = 0;
|
||||
out_5688594807383263913[15] = 0;
|
||||
out_5688594807383263913[16] = 0;
|
||||
out_5688594807383263913[17] = 0;
|
||||
out_5688594807383263913[18] = 0;
|
||||
out_5688594807383263913[19] = 0;
|
||||
out_5688594807383263913[20] = 1;
|
||||
out_5688594807383263913[21] = 0;
|
||||
out_5688594807383263913[22] = 0;
|
||||
out_5688594807383263913[23] = 0;
|
||||
out_5688594807383263913[24] = 0;
|
||||
out_5688594807383263913[25] = 0;
|
||||
out_5688594807383263913[26] = 0;
|
||||
out_5688594807383263913[27] = 0;
|
||||
out_5688594807383263913[28] = 0;
|
||||
out_5688594807383263913[29] = 0;
|
||||
out_5688594807383263913[30] = 1;
|
||||
out_5688594807383263913[31] = 0;
|
||||
out_5688594807383263913[32] = 0;
|
||||
out_5688594807383263913[33] = 0;
|
||||
out_5688594807383263913[34] = 0;
|
||||
out_5688594807383263913[35] = 0;
|
||||
out_5688594807383263913[36] = 0;
|
||||
out_5688594807383263913[37] = 0;
|
||||
out_5688594807383263913[38] = 0;
|
||||
out_5688594807383263913[39] = 0;
|
||||
out_5688594807383263913[40] = 1;
|
||||
out_5688594807383263913[41] = 0;
|
||||
out_5688594807383263913[42] = 0;
|
||||
out_5688594807383263913[43] = 0;
|
||||
out_5688594807383263913[44] = 0;
|
||||
out_5688594807383263913[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_5688594807383263913[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_5688594807383263913[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5688594807383263913[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5688594807383263913[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_5688594807383263913[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_5688594807383263913[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_5688594807383263913[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5688594807383263913[53] = -9.8100000000000005*dt;
|
||||
out_5688594807383263913[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_5688594807383263913[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_5688594807383263913[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5688594807383263913[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5688594807383263913[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_5688594807383263913[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_5688594807383263913[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_5688594807383263913[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5688594807383263913[62] = 0;
|
||||
out_5688594807383263913[63] = 0;
|
||||
out_5688594807383263913[64] = 0;
|
||||
out_5688594807383263913[65] = 0;
|
||||
out_5688594807383263913[66] = 0;
|
||||
out_5688594807383263913[67] = 0;
|
||||
out_5688594807383263913[68] = 0;
|
||||
out_5688594807383263913[69] = 0;
|
||||
out_5688594807383263913[70] = 1;
|
||||
out_5688594807383263913[71] = 0;
|
||||
out_5688594807383263913[72] = 0;
|
||||
out_5688594807383263913[73] = 0;
|
||||
out_5688594807383263913[74] = 0;
|
||||
out_5688594807383263913[75] = 0;
|
||||
out_5688594807383263913[76] = 0;
|
||||
out_5688594807383263913[77] = 0;
|
||||
out_5688594807383263913[78] = 0;
|
||||
out_5688594807383263913[79] = 0;
|
||||
out_5688594807383263913[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_4973933158632202337) {
|
||||
out_4973933158632202337[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_4593071790268116288) {
|
||||
out_4593071790268116288[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_1488365746767674340) {
|
||||
out_1488365746767674340[0] = 0;
|
||||
out_1488365746767674340[1] = 0;
|
||||
out_1488365746767674340[2] = 0;
|
||||
out_1488365746767674340[3] = 0;
|
||||
out_1488365746767674340[4] = 0;
|
||||
out_1488365746767674340[5] = 0;
|
||||
out_1488365746767674340[6] = 1;
|
||||
out_1488365746767674340[7] = 0;
|
||||
out_1488365746767674340[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_3739161203201081212) {
|
||||
out_3739161203201081212[0] = 0;
|
||||
out_3739161203201081212[1] = 0;
|
||||
out_3739161203201081212[2] = 0;
|
||||
out_3739161203201081212[3] = 0;
|
||||
out_3739161203201081212[4] = 0;
|
||||
out_3739161203201081212[5] = 0;
|
||||
out_3739161203201081212[6] = 1;
|
||||
out_3739161203201081212[7] = 0;
|
||||
out_3739161203201081212[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_8075656837058392290) {
|
||||
out_8075656837058392290[0] = state[4];
|
||||
out_8075656837058392290[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_8003921618768497979) {
|
||||
out_8003921618768497979[0] = state[4];
|
||||
out_8003921618768497979[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_688848676839475633) {
|
||||
out_688848676839475633[0] = 0;
|
||||
out_688848676839475633[1] = 0;
|
||||
out_688848676839475633[2] = 0;
|
||||
out_688848676839475633[3] = 0;
|
||||
out_688848676839475633[4] = 1;
|
||||
out_688848676839475633[5] = 0;
|
||||
out_688848676839475633[6] = 0;
|
||||
out_688848676839475633[7] = 0;
|
||||
out_688848676839475633[8] = 0;
|
||||
out_688848676839475633[9] = 0;
|
||||
out_688848676839475633[10] = 0;
|
||||
out_688848676839475633[11] = 0;
|
||||
out_688848676839475633[12] = 0;
|
||||
out_688848676839475633[13] = 0;
|
||||
out_688848676839475633[14] = 1;
|
||||
out_688848676839475633[15] = 0;
|
||||
out_688848676839475633[16] = 0;
|
||||
out_688848676839475633[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_4646084155782705221) {
|
||||
out_4646084155782705221[0] = 0;
|
||||
out_4646084155782705221[1] = 0;
|
||||
out_4646084155782705221[2] = 0;
|
||||
out_4646084155782705221[3] = 0;
|
||||
out_4646084155782705221[4] = 1;
|
||||
out_4646084155782705221[5] = 0;
|
||||
out_4646084155782705221[6] = 0;
|
||||
out_4646084155782705221[7] = 0;
|
||||
out_4646084155782705221[8] = 0;
|
||||
out_4646084155782705221[9] = 0;
|
||||
out_4646084155782705221[10] = 0;
|
||||
out_4646084155782705221[11] = 0;
|
||||
out_4646084155782705221[12] = 0;
|
||||
out_4646084155782705221[13] = 0;
|
||||
out_4646084155782705221[14] = 1;
|
||||
out_4646084155782705221[15] = 0;
|
||||
out_4646084155782705221[16] = 0;
|
||||
out_4646084155782705221[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_4816746490281073192) {
|
||||
out_4816746490281073192[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_7633295100594421359) {
|
||||
out_7633295100594421359[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_1029967211739574287) {
|
||||
out_1029967211739574287[0] = 0;
|
||||
out_1029967211739574287[1] = 0;
|
||||
out_1029967211739574287[2] = 0;
|
||||
out_1029967211739574287[3] = 0;
|
||||
out_1029967211739574287[4] = 1;
|
||||
out_1029967211739574287[5] = 0;
|
||||
out_1029967211739574287[6] = 0;
|
||||
out_1029967211739574287[7] = 0;
|
||||
out_1029967211739574287[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_3177529138290535543) {
|
||||
out_3177529138290535543[0] = 0;
|
||||
out_3177529138290535543[1] = 0;
|
||||
out_3177529138290535543[2] = 0;
|
||||
out_3177529138290535543[3] = 0;
|
||||
out_3177529138290535543[4] = 1;
|
||||
out_3177529138290535543[5] = 0;
|
||||
out_3177529138290535543[6] = 0;
|
||||
out_3177529138290535543[7] = 0;
|
||||
out_3177529138290535543[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_2119428105077637665) {
|
||||
out_2119428105077637665[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_5811775705454284612) {
|
||||
out_5811775705454284612[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_5229869065641730564) {
|
||||
out_5229869065641730564[0] = 0;
|
||||
out_5229869065641730564[1] = 0;
|
||||
out_5229869065641730564[2] = 0;
|
||||
out_5229869065641730564[3] = 0;
|
||||
out_5229869065641730564[4] = 0;
|
||||
out_5229869065641730564[5] = 0;
|
||||
out_5229869065641730564[6] = 0;
|
||||
out_5229869065641730564[7] = 1;
|
||||
out_5229869065641730564[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_7480664522075137436) {
|
||||
out_7480664522075137436[0] = 0;
|
||||
out_7480664522075137436[1] = 0;
|
||||
out_7480664522075137436[2] = 0;
|
||||
out_7480664522075137436[3] = 0;
|
||||
out_7480664522075137436[4] = 0;
|
||||
out_7480664522075137436[5] = 0;
|
||||
out_7480664522075137436[6] = 0;
|
||||
out_7480664522075137436[7] = 1;
|
||||
out_7480664522075137436[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_2161381055362348954) {
|
||||
out_2161381055362348954[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_8851999015780589683) {
|
||||
out_8851999015780589683[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_3253561282923517504) {
|
||||
out_3253561282923517504[0] = 0;
|
||||
out_3253561282923517504[1] = 0;
|
||||
out_3253561282923517504[2] = 0;
|
||||
out_3253561282923517504[3] = 1;
|
||||
out_3253561282923517504[4] = 0;
|
||||
out_3253561282923517504[5] = 0;
|
||||
out_3253561282923517504[6] = 0;
|
||||
out_3253561282923517504[7] = 0;
|
||||
out_3253561282923517504[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_6043263462144746193) {
|
||||
out_6043263462144746193[0] = 0;
|
||||
out_6043263462144746193[1] = 0;
|
||||
out_6043263462144746193[2] = 0;
|
||||
out_6043263462144746193[3] = 1;
|
||||
out_6043263462144746193[4] = 0;
|
||||
out_6043263462144746193[5] = 0;
|
||||
out_6043263462144746193[6] = 0;
|
||||
out_6043263462144746193[7] = 0;
|
||||
out_6043263462144746193[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_2436575117646854843) {
|
||||
out_2436575117646854843[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_1831479801108289492) {
|
||||
out_1831479801108289492[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_1540198556053966471) {
|
||||
out_1540198556053966471[0] = 0;
|
||||
out_1540198556053966471[1] = 1;
|
||||
out_1540198556053966471[2] = 0;
|
||||
out_1540198556053966471[3] = 0;
|
||||
out_1540198556053966471[4] = 0;
|
||||
out_1540198556053966471[5] = 0;
|
||||
out_1540198556053966471[6] = 0;
|
||||
out_1540198556053966471[7] = 0;
|
||||
out_1540198556053966471[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_7756626189014297226) {
|
||||
out_7756626189014297226[0] = 0;
|
||||
out_7756626189014297226[1] = 1;
|
||||
out_7756626189014297226[2] = 0;
|
||||
out_7756626189014297226[3] = 0;
|
||||
out_7756626189014297226[4] = 0;
|
||||
out_7756626189014297226[5] = 0;
|
||||
out_7756626189014297226[6] = 0;
|
||||
out_7756626189014297226[7] = 0;
|
||||
out_7756626189014297226[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_7163054323023194599) {
|
||||
out_7163054323023194599[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_6557959006484629248) {
|
||||
out_6557959006484629248[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_3542200461015564103) {
|
||||
out_3542200461015564103[0] = 1;
|
||||
out_3542200461015564103[1] = 0;
|
||||
out_3542200461015564103[2] = 0;
|
||||
out_3542200461015564103[3] = 0;
|
||||
out_3542200461015564103[4] = 0;
|
||||
out_3542200461015564103[5] = 0;
|
||||
out_3542200461015564103[6] = 0;
|
||||
out_3542200461015564103[7] = 0;
|
||||
out_3542200461015564103[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_5792995917448970975) {
|
||||
out_5792995917448970975[0] = 1;
|
||||
out_5792995917448970975[1] = 0;
|
||||
out_5792995917448970975[2] = 0;
|
||||
out_5792995917448970975[3] = 0;
|
||||
out_5792995917448970975[4] = 0;
|
||||
out_5792995917448970975[5] = 0;
|
||||
out_5792995917448970975[6] = 0;
|
||||
out_5792995917448970975[7] = 0;
|
||||
out_5792995917448970975[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_1389660819669613524) {
|
||||
out_1389660819669613524[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_5357011602177695336) {
|
||||
out_5357011602177695336[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_1457719784890713912) {
|
||||
out_1457719784890713912[0] = 0;
|
||||
out_1457719784890713912[1] = 0;
|
||||
out_1457719784890713912[2] = 0;
|
||||
out_1457719784890713912[3] = 0;
|
||||
out_1457719784890713912[4] = 0;
|
||||
out_1457719784890713912[5] = 0;
|
||||
out_1457719784890713912[6] = 0;
|
||||
out_1457719784890713912[7] = 0;
|
||||
out_1457719784890713912[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_3708515241324120784) {
|
||||
out_3708515241324120784[0] = 0;
|
||||
out_3708515241324120784[1] = 0;
|
||||
out_3708515241324120784[2] = 0;
|
||||
out_3708515241324120784[3] = 0;
|
||||
out_3708515241324120784[4] = 0;
|
||||
out_3708515241324120784[5] = 0;
|
||||
out_3708515241324120784[6] = 0;
|
||||
out_3708515241324120784[7] = 0;
|
||||
out_3708515241324120784[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_4191231468621882354) {
|
||||
err_fun(nom_x, delta_x, out_4191231468621882354);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_8532074412831680005) {
|
||||
err_fun(nom_x, delta_x, out_8532074412831680005);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4409511713992133464) {
|
||||
inv_err_fun(nom_x, true_x, out_4409511713992133464);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8887769849182574222) {
|
||||
inv_err_fun(nom_x, true_x, out_8887769849182574222);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_7704822479144562897) {
|
||||
H_mod_fun(state, out_7704822479144562897);
|
||||
void car_H_mod_fun(double *state, double *out_2434822093009109592) {
|
||||
H_mod_fun(state, out_2434822093009109592);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_2423168834336223124) {
|
||||
f_fun(state, dt, out_2423168834336223124);
|
||||
void car_f_fun(double *state, double dt, double *out_7489300736878445586) {
|
||||
f_fun(state, dt, out_7489300736878445586);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_7639588574330807467) {
|
||||
F_fun(state, dt, out_7639588574330807467);
|
||||
void car_F_fun(double *state, double dt, double *out_5688594807383263913) {
|
||||
F_fun(state, dt, out_5688594807383263913);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_4973933158632202337) {
|
||||
h_25(state, unused, out_4973933158632202337);
|
||||
void car_h_25(double *state, double *unused, double *out_4593071790268116288) {
|
||||
h_25(state, unused, out_4593071790268116288);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_1488365746767674340) {
|
||||
H_25(state, unused, out_1488365746767674340);
|
||||
void car_H_25(double *state, double *unused, double *out_3739161203201081212) {
|
||||
H_25(state, unused, out_3739161203201081212);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_8075656837058392290) {
|
||||
h_24(state, unused, out_8075656837058392290);
|
||||
void car_h_24(double *state, double *unused, double *out_8003921618768497979) {
|
||||
h_24(state, unused, out_8003921618768497979);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_688848676839475633) {
|
||||
H_24(state, unused, out_688848676839475633);
|
||||
void car_H_24(double *state, double *unused, double *out_4646084155782705221) {
|
||||
H_24(state, unused, out_4646084155782705221);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_4816746490281073192) {
|
||||
h_30(state, unused, out_4816746490281073192);
|
||||
void car_h_30(double *state, double *unused, double *out_7633295100594421359) {
|
||||
h_30(state, unused, out_7633295100594421359);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_1029967211739574287) {
|
||||
H_30(state, unused, out_1029967211739574287);
|
||||
void car_H_30(double *state, double *unused, double *out_3177529138290535543) {
|
||||
H_30(state, unused, out_3177529138290535543);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_2119428105077637665) {
|
||||
h_26(state, unused, out_2119428105077637665);
|
||||
void car_h_26(double *state, double *unused, double *out_5811775705454284612) {
|
||||
h_26(state, unused, out_5811775705454284612);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_5229869065641730564) {
|
||||
H_26(state, unused, out_5229869065641730564);
|
||||
void car_H_26(double *state, double *unused, double *out_7480664522075137436) {
|
||||
H_26(state, unused, out_7480664522075137436);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_2161381055362348954) {
|
||||
h_27(state, unused, out_2161381055362348954);
|
||||
void car_h_27(double *state, double *unused, double *out_8851999015780589683) {
|
||||
h_27(state, unused, out_8851999015780589683);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_3253561282923517504) {
|
||||
H_27(state, unused, out_3253561282923517504);
|
||||
void car_H_27(double *state, double *unused, double *out_6043263462144746193) {
|
||||
H_27(state, unused, out_6043263462144746193);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_2436575117646854843) {
|
||||
h_29(state, unused, out_2436575117646854843);
|
||||
void car_h_29(double *state, double *unused, double *out_1831479801108289492) {
|
||||
h_29(state, unused, out_1831479801108289492);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_1540198556053966471) {
|
||||
H_29(state, unused, out_1540198556053966471);
|
||||
void car_H_29(double *state, double *unused, double *out_7756626189014297226) {
|
||||
H_29(state, unused, out_7756626189014297226);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_7163054323023194599) {
|
||||
h_28(state, unused, out_7163054323023194599);
|
||||
void car_h_28(double *state, double *unused, double *out_6557959006484629248) {
|
||||
h_28(state, unused, out_6557959006484629248);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_3542200461015564103) {
|
||||
H_28(state, unused, out_3542200461015564103);
|
||||
void car_H_28(double *state, double *unused, double *out_5792995917448970975) {
|
||||
H_28(state, unused, out_5792995917448970975);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_1389660819669613524) {
|
||||
h_31(state, unused, out_1389660819669613524);
|
||||
void car_h_31(double *state, double *unused, double *out_5357011602177695336) {
|
||||
h_31(state, unused, out_5357011602177695336);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_1457719784890713912) {
|
||||
H_31(state, unused, out_1457719784890713912);
|
||||
void car_H_31(double *state, double *unused, double *out_3708515241324120784) {
|
||||
H_31(state, unused, out_3708515241324120784);
|
||||
}
|
||||
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_4191231468621882354);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4409511713992133464);
|
||||
void car_H_mod_fun(double *state, double *out_7704822479144562897);
|
||||
void car_f_fun(double *state, double dt, double *out_2423168834336223124);
|
||||
void car_F_fun(double *state, double dt, double *out_7639588574330807467);
|
||||
void car_h_25(double *state, double *unused, double *out_4973933158632202337);
|
||||
void car_H_25(double *state, double *unused, double *out_1488365746767674340);
|
||||
void car_h_24(double *state, double *unused, double *out_8075656837058392290);
|
||||
void car_H_24(double *state, double *unused, double *out_688848676839475633);
|
||||
void car_h_30(double *state, double *unused, double *out_4816746490281073192);
|
||||
void car_H_30(double *state, double *unused, double *out_1029967211739574287);
|
||||
void car_h_26(double *state, double *unused, double *out_2119428105077637665);
|
||||
void car_H_26(double *state, double *unused, double *out_5229869065641730564);
|
||||
void car_h_27(double *state, double *unused, double *out_2161381055362348954);
|
||||
void car_H_27(double *state, double *unused, double *out_3253561282923517504);
|
||||
void car_h_29(double *state, double *unused, double *out_2436575117646854843);
|
||||
void car_H_29(double *state, double *unused, double *out_1540198556053966471);
|
||||
void car_h_28(double *state, double *unused, double *out_7163054323023194599);
|
||||
void car_H_28(double *state, double *unused, double *out_3542200461015564103);
|
||||
void car_h_31(double *state, double *unused, double *out_1389660819669613524);
|
||||
void car_H_31(double *state, double *unused, double *out_1457719784890713912);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_8532074412831680005);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8887769849182574222);
|
||||
void car_H_mod_fun(double *state, double *out_2434822093009109592);
|
||||
void car_f_fun(double *state, double dt, double *out_7489300736878445586);
|
||||
void car_F_fun(double *state, double dt, double *out_5688594807383263913);
|
||||
void car_h_25(double *state, double *unused, double *out_4593071790268116288);
|
||||
void car_H_25(double *state, double *unused, double *out_3739161203201081212);
|
||||
void car_h_24(double *state, double *unused, double *out_8003921618768497979);
|
||||
void car_H_24(double *state, double *unused, double *out_4646084155782705221);
|
||||
void car_h_30(double *state, double *unused, double *out_7633295100594421359);
|
||||
void car_H_30(double *state, double *unused, double *out_3177529138290535543);
|
||||
void car_h_26(double *state, double *unused, double *out_5811775705454284612);
|
||||
void car_H_26(double *state, double *unused, double *out_7480664522075137436);
|
||||
void car_h_27(double *state, double *unused, double *out_8851999015780589683);
|
||||
void car_H_27(double *state, double *unused, double *out_6043263462144746193);
|
||||
void car_h_29(double *state, double *unused, double *out_1831479801108289492);
|
||||
void car_H_29(double *state, double *unused, double *out_7756626189014297226);
|
||||
void car_h_28(double *state, double *unused, double *out_6557959006484629248);
|
||||
void car_H_28(double *state, double *unused, double *out_5792995917448970975);
|
||||
void car_h_31(double *state, double *unused, double *out_5357011602177695336);
|
||||
void car_H_31(double *state, double *unused, double *out_3708515241324120784);
|
||||
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_7083554587370275014);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6548503546481104197);
|
||||
void pose_H_mod_fun(double *state, double *out_8484867830328864601);
|
||||
void pose_f_fun(double *state, double dt, double *out_6058628880177520660);
|
||||
void pose_F_fun(double *state, double dt, double *out_5197906072130444471);
|
||||
void pose_h_4(double *state, double *unused, double *out_8442663857645814160);
|
||||
void pose_H_4(double *state, double *unused, double *out_1223680011297697877);
|
||||
void pose_h_10(double *state, double *unused, double *out_3964974043036968705);
|
||||
void pose_H_10(double *state, double *unused, double *out_5914479273564375913);
|
||||
void pose_h_13(double *state, double *unused, double *out_8876471781075959631);
|
||||
void pose_H_13(double *state, double *unused, double *out_1988593814034634924);
|
||||
void pose_h_14(double *state, double *unused, double *out_8405881405530134324);
|
||||
void pose_H_14(double *state, double *unused, double *out_1658796537942581476);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_3388086590141056177);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1633734341039361389);
|
||||
void pose_H_mod_fun(double *state, double *out_3284972142892485625);
|
||||
void pose_f_fun(double *state, double dt, double *out_7294513523227393093);
|
||||
void pose_F_fun(double *state, double dt, double *out_1315688866139806167);
|
||||
void pose_h_4(double *state, double *unused, double *out_6849640214971072753);
|
||||
void pose_H_4(double *state, double *unused, double *out_6217410264759244649);
|
||||
void pose_h_10(double *state, double *unused, double *out_2538664534589622554);
|
||||
void pose_H_10(double *state, double *unused, double *out_5499394725381253526);
|
||||
void pose_h_13(double *state, double *unused, double *out_228516151720916839);
|
||||
void pose_H_13(double *state, double *unused, double *out_9017059983617974166);
|
||||
void pose_h_14(double *state, double *unused, double *out_4519675338605033391);
|
||||
void pose_H_14(double *state, double *unused, double *out_3134621832463872353);
|
||||
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.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
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-6b39253b-P92eb5651-DEBUG";
|
||||
const uint8_t gitversion[29] = "DEV-100b077d-P92eb5651-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
DEV-6b39253b-P92eb5651-DEBUG
|
||||
DEV-100b077d-P92eb5651-DEBUG
|
||||
+1
-1
@@ -1 +1 @@
|
||||
{"protected_inventory_sha256": "03ace866590c4fce8ef8b93f6fe1bbd4e30385a11ddfcf85e4542df4f54d1b7b", "schema_version": 1}
|
||||
{"protected_inventory_sha256": "a6517218441bdbebab61ec25dfeef503bde1af33a83c21d1e1589153184d9410", "schema_version": 1}
|
||||
|
||||
Reference in New Issue
Block a user