mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-12 12:52:13 +08:00
build
This commit is contained in:
firestar5683
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,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-57a36f06-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-891c1eb8-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.
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.
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 +1 @@
|
||||
DEV-57a36f06-DEBUG
|
||||
DEV-891c1eb8-DEBUG
|
||||
@@ -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_1054824998219177113) {
|
||||
out_1054824998219177113[0] = delta_x[0] + nom_x[0];
|
||||
out_1054824998219177113[1] = delta_x[1] + nom_x[1];
|
||||
out_1054824998219177113[2] = delta_x[2] + nom_x[2];
|
||||
out_1054824998219177113[3] = delta_x[3] + nom_x[3];
|
||||
out_1054824998219177113[4] = delta_x[4] + nom_x[4];
|
||||
out_1054824998219177113[5] = delta_x[5] + nom_x[5];
|
||||
out_1054824998219177113[6] = delta_x[6] + nom_x[6];
|
||||
out_1054824998219177113[7] = delta_x[7] + nom_x[7];
|
||||
out_1054824998219177113[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_3577007229449845283) {
|
||||
out_3577007229449845283[0] = delta_x[0] + nom_x[0];
|
||||
out_3577007229449845283[1] = delta_x[1] + nom_x[1];
|
||||
out_3577007229449845283[2] = delta_x[2] + nom_x[2];
|
||||
out_3577007229449845283[3] = delta_x[3] + nom_x[3];
|
||||
out_3577007229449845283[4] = delta_x[4] + nom_x[4];
|
||||
out_3577007229449845283[5] = delta_x[5] + nom_x[5];
|
||||
out_3577007229449845283[6] = delta_x[6] + nom_x[6];
|
||||
out_3577007229449845283[7] = delta_x[7] + nom_x[7];
|
||||
out_3577007229449845283[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8618844445394053899) {
|
||||
out_8618844445394053899[0] = -nom_x[0] + true_x[0];
|
||||
out_8618844445394053899[1] = -nom_x[1] + true_x[1];
|
||||
out_8618844445394053899[2] = -nom_x[2] + true_x[2];
|
||||
out_8618844445394053899[3] = -nom_x[3] + true_x[3];
|
||||
out_8618844445394053899[4] = -nom_x[4] + true_x[4];
|
||||
out_8618844445394053899[5] = -nom_x[5] + true_x[5];
|
||||
out_8618844445394053899[6] = -nom_x[6] + true_x[6];
|
||||
out_8618844445394053899[7] = -nom_x[7] + true_x[7];
|
||||
out_8618844445394053899[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_6399722403094813471) {
|
||||
out_6399722403094813471[0] = -nom_x[0] + true_x[0];
|
||||
out_6399722403094813471[1] = -nom_x[1] + true_x[1];
|
||||
out_6399722403094813471[2] = -nom_x[2] + true_x[2];
|
||||
out_6399722403094813471[3] = -nom_x[3] + true_x[3];
|
||||
out_6399722403094813471[4] = -nom_x[4] + true_x[4];
|
||||
out_6399722403094813471[5] = -nom_x[5] + true_x[5];
|
||||
out_6399722403094813471[6] = -nom_x[6] + true_x[6];
|
||||
out_6399722403094813471[7] = -nom_x[7] + true_x[7];
|
||||
out_6399722403094813471[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_9083000739620874267) {
|
||||
out_9083000739620874267[0] = 1.0;
|
||||
out_9083000739620874267[1] = 0.0;
|
||||
out_9083000739620874267[2] = 0.0;
|
||||
out_9083000739620874267[3] = 0.0;
|
||||
out_9083000739620874267[4] = 0.0;
|
||||
out_9083000739620874267[5] = 0.0;
|
||||
out_9083000739620874267[6] = 0.0;
|
||||
out_9083000739620874267[7] = 0.0;
|
||||
out_9083000739620874267[8] = 0.0;
|
||||
out_9083000739620874267[9] = 0.0;
|
||||
out_9083000739620874267[10] = 1.0;
|
||||
out_9083000739620874267[11] = 0.0;
|
||||
out_9083000739620874267[12] = 0.0;
|
||||
out_9083000739620874267[13] = 0.0;
|
||||
out_9083000739620874267[14] = 0.0;
|
||||
out_9083000739620874267[15] = 0.0;
|
||||
out_9083000739620874267[16] = 0.0;
|
||||
out_9083000739620874267[17] = 0.0;
|
||||
out_9083000739620874267[18] = 0.0;
|
||||
out_9083000739620874267[19] = 0.0;
|
||||
out_9083000739620874267[20] = 1.0;
|
||||
out_9083000739620874267[21] = 0.0;
|
||||
out_9083000739620874267[22] = 0.0;
|
||||
out_9083000739620874267[23] = 0.0;
|
||||
out_9083000739620874267[24] = 0.0;
|
||||
out_9083000739620874267[25] = 0.0;
|
||||
out_9083000739620874267[26] = 0.0;
|
||||
out_9083000739620874267[27] = 0.0;
|
||||
out_9083000739620874267[28] = 0.0;
|
||||
out_9083000739620874267[29] = 0.0;
|
||||
out_9083000739620874267[30] = 1.0;
|
||||
out_9083000739620874267[31] = 0.0;
|
||||
out_9083000739620874267[32] = 0.0;
|
||||
out_9083000739620874267[33] = 0.0;
|
||||
out_9083000739620874267[34] = 0.0;
|
||||
out_9083000739620874267[35] = 0.0;
|
||||
out_9083000739620874267[36] = 0.0;
|
||||
out_9083000739620874267[37] = 0.0;
|
||||
out_9083000739620874267[38] = 0.0;
|
||||
out_9083000739620874267[39] = 0.0;
|
||||
out_9083000739620874267[40] = 1.0;
|
||||
out_9083000739620874267[41] = 0.0;
|
||||
out_9083000739620874267[42] = 0.0;
|
||||
out_9083000739620874267[43] = 0.0;
|
||||
out_9083000739620874267[44] = 0.0;
|
||||
out_9083000739620874267[45] = 0.0;
|
||||
out_9083000739620874267[46] = 0.0;
|
||||
out_9083000739620874267[47] = 0.0;
|
||||
out_9083000739620874267[48] = 0.0;
|
||||
out_9083000739620874267[49] = 0.0;
|
||||
out_9083000739620874267[50] = 1.0;
|
||||
out_9083000739620874267[51] = 0.0;
|
||||
out_9083000739620874267[52] = 0.0;
|
||||
out_9083000739620874267[53] = 0.0;
|
||||
out_9083000739620874267[54] = 0.0;
|
||||
out_9083000739620874267[55] = 0.0;
|
||||
out_9083000739620874267[56] = 0.0;
|
||||
out_9083000739620874267[57] = 0.0;
|
||||
out_9083000739620874267[58] = 0.0;
|
||||
out_9083000739620874267[59] = 0.0;
|
||||
out_9083000739620874267[60] = 1.0;
|
||||
out_9083000739620874267[61] = 0.0;
|
||||
out_9083000739620874267[62] = 0.0;
|
||||
out_9083000739620874267[63] = 0.0;
|
||||
out_9083000739620874267[64] = 0.0;
|
||||
out_9083000739620874267[65] = 0.0;
|
||||
out_9083000739620874267[66] = 0.0;
|
||||
out_9083000739620874267[67] = 0.0;
|
||||
out_9083000739620874267[68] = 0.0;
|
||||
out_9083000739620874267[69] = 0.0;
|
||||
out_9083000739620874267[70] = 1.0;
|
||||
out_9083000739620874267[71] = 0.0;
|
||||
out_9083000739620874267[72] = 0.0;
|
||||
out_9083000739620874267[73] = 0.0;
|
||||
out_9083000739620874267[74] = 0.0;
|
||||
out_9083000739620874267[75] = 0.0;
|
||||
out_9083000739620874267[76] = 0.0;
|
||||
out_9083000739620874267[77] = 0.0;
|
||||
out_9083000739620874267[78] = 0.0;
|
||||
out_9083000739620874267[79] = 0.0;
|
||||
out_9083000739620874267[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_7904303228569973439) {
|
||||
out_7904303228569973439[0] = 1.0;
|
||||
out_7904303228569973439[1] = 0.0;
|
||||
out_7904303228569973439[2] = 0.0;
|
||||
out_7904303228569973439[3] = 0.0;
|
||||
out_7904303228569973439[4] = 0.0;
|
||||
out_7904303228569973439[5] = 0.0;
|
||||
out_7904303228569973439[6] = 0.0;
|
||||
out_7904303228569973439[7] = 0.0;
|
||||
out_7904303228569973439[8] = 0.0;
|
||||
out_7904303228569973439[9] = 0.0;
|
||||
out_7904303228569973439[10] = 1.0;
|
||||
out_7904303228569973439[11] = 0.0;
|
||||
out_7904303228569973439[12] = 0.0;
|
||||
out_7904303228569973439[13] = 0.0;
|
||||
out_7904303228569973439[14] = 0.0;
|
||||
out_7904303228569973439[15] = 0.0;
|
||||
out_7904303228569973439[16] = 0.0;
|
||||
out_7904303228569973439[17] = 0.0;
|
||||
out_7904303228569973439[18] = 0.0;
|
||||
out_7904303228569973439[19] = 0.0;
|
||||
out_7904303228569973439[20] = 1.0;
|
||||
out_7904303228569973439[21] = 0.0;
|
||||
out_7904303228569973439[22] = 0.0;
|
||||
out_7904303228569973439[23] = 0.0;
|
||||
out_7904303228569973439[24] = 0.0;
|
||||
out_7904303228569973439[25] = 0.0;
|
||||
out_7904303228569973439[26] = 0.0;
|
||||
out_7904303228569973439[27] = 0.0;
|
||||
out_7904303228569973439[28] = 0.0;
|
||||
out_7904303228569973439[29] = 0.0;
|
||||
out_7904303228569973439[30] = 1.0;
|
||||
out_7904303228569973439[31] = 0.0;
|
||||
out_7904303228569973439[32] = 0.0;
|
||||
out_7904303228569973439[33] = 0.0;
|
||||
out_7904303228569973439[34] = 0.0;
|
||||
out_7904303228569973439[35] = 0.0;
|
||||
out_7904303228569973439[36] = 0.0;
|
||||
out_7904303228569973439[37] = 0.0;
|
||||
out_7904303228569973439[38] = 0.0;
|
||||
out_7904303228569973439[39] = 0.0;
|
||||
out_7904303228569973439[40] = 1.0;
|
||||
out_7904303228569973439[41] = 0.0;
|
||||
out_7904303228569973439[42] = 0.0;
|
||||
out_7904303228569973439[43] = 0.0;
|
||||
out_7904303228569973439[44] = 0.0;
|
||||
out_7904303228569973439[45] = 0.0;
|
||||
out_7904303228569973439[46] = 0.0;
|
||||
out_7904303228569973439[47] = 0.0;
|
||||
out_7904303228569973439[48] = 0.0;
|
||||
out_7904303228569973439[49] = 0.0;
|
||||
out_7904303228569973439[50] = 1.0;
|
||||
out_7904303228569973439[51] = 0.0;
|
||||
out_7904303228569973439[52] = 0.0;
|
||||
out_7904303228569973439[53] = 0.0;
|
||||
out_7904303228569973439[54] = 0.0;
|
||||
out_7904303228569973439[55] = 0.0;
|
||||
out_7904303228569973439[56] = 0.0;
|
||||
out_7904303228569973439[57] = 0.0;
|
||||
out_7904303228569973439[58] = 0.0;
|
||||
out_7904303228569973439[59] = 0.0;
|
||||
out_7904303228569973439[60] = 1.0;
|
||||
out_7904303228569973439[61] = 0.0;
|
||||
out_7904303228569973439[62] = 0.0;
|
||||
out_7904303228569973439[63] = 0.0;
|
||||
out_7904303228569973439[64] = 0.0;
|
||||
out_7904303228569973439[65] = 0.0;
|
||||
out_7904303228569973439[66] = 0.0;
|
||||
out_7904303228569973439[67] = 0.0;
|
||||
out_7904303228569973439[68] = 0.0;
|
||||
out_7904303228569973439[69] = 0.0;
|
||||
out_7904303228569973439[70] = 1.0;
|
||||
out_7904303228569973439[71] = 0.0;
|
||||
out_7904303228569973439[72] = 0.0;
|
||||
out_7904303228569973439[73] = 0.0;
|
||||
out_7904303228569973439[74] = 0.0;
|
||||
out_7904303228569973439[75] = 0.0;
|
||||
out_7904303228569973439[76] = 0.0;
|
||||
out_7904303228569973439[77] = 0.0;
|
||||
out_7904303228569973439[78] = 0.0;
|
||||
out_7904303228569973439[79] = 0.0;
|
||||
out_7904303228569973439[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_2939712332855985662) {
|
||||
out_2939712332855985662[0] = state[0];
|
||||
out_2939712332855985662[1] = state[1];
|
||||
out_2939712332855985662[2] = state[2];
|
||||
out_2939712332855985662[3] = state[3];
|
||||
out_2939712332855985662[4] = state[4];
|
||||
out_2939712332855985662[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_2939712332855985662[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_2939712332855985662[7] = state[7];
|
||||
out_2939712332855985662[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_2229936979988599717) {
|
||||
out_2229936979988599717[0] = state[0];
|
||||
out_2229936979988599717[1] = state[1];
|
||||
out_2229936979988599717[2] = state[2];
|
||||
out_2229936979988599717[3] = state[3];
|
||||
out_2229936979988599717[4] = state[4];
|
||||
out_2229936979988599717[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_2229936979988599717[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_2229936979988599717[7] = state[7];
|
||||
out_2229936979988599717[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_9197210553723469698) {
|
||||
out_9197210553723469698[0] = 1;
|
||||
out_9197210553723469698[1] = 0;
|
||||
out_9197210553723469698[2] = 0;
|
||||
out_9197210553723469698[3] = 0;
|
||||
out_9197210553723469698[4] = 0;
|
||||
out_9197210553723469698[5] = 0;
|
||||
out_9197210553723469698[6] = 0;
|
||||
out_9197210553723469698[7] = 0;
|
||||
out_9197210553723469698[8] = 0;
|
||||
out_9197210553723469698[9] = 0;
|
||||
out_9197210553723469698[10] = 1;
|
||||
out_9197210553723469698[11] = 0;
|
||||
out_9197210553723469698[12] = 0;
|
||||
out_9197210553723469698[13] = 0;
|
||||
out_9197210553723469698[14] = 0;
|
||||
out_9197210553723469698[15] = 0;
|
||||
out_9197210553723469698[16] = 0;
|
||||
out_9197210553723469698[17] = 0;
|
||||
out_9197210553723469698[18] = 0;
|
||||
out_9197210553723469698[19] = 0;
|
||||
out_9197210553723469698[20] = 1;
|
||||
out_9197210553723469698[21] = 0;
|
||||
out_9197210553723469698[22] = 0;
|
||||
out_9197210553723469698[23] = 0;
|
||||
out_9197210553723469698[24] = 0;
|
||||
out_9197210553723469698[25] = 0;
|
||||
out_9197210553723469698[26] = 0;
|
||||
out_9197210553723469698[27] = 0;
|
||||
out_9197210553723469698[28] = 0;
|
||||
out_9197210553723469698[29] = 0;
|
||||
out_9197210553723469698[30] = 1;
|
||||
out_9197210553723469698[31] = 0;
|
||||
out_9197210553723469698[32] = 0;
|
||||
out_9197210553723469698[33] = 0;
|
||||
out_9197210553723469698[34] = 0;
|
||||
out_9197210553723469698[35] = 0;
|
||||
out_9197210553723469698[36] = 0;
|
||||
out_9197210553723469698[37] = 0;
|
||||
out_9197210553723469698[38] = 0;
|
||||
out_9197210553723469698[39] = 0;
|
||||
out_9197210553723469698[40] = 1;
|
||||
out_9197210553723469698[41] = 0;
|
||||
out_9197210553723469698[42] = 0;
|
||||
out_9197210553723469698[43] = 0;
|
||||
out_9197210553723469698[44] = 0;
|
||||
out_9197210553723469698[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_9197210553723469698[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_9197210553723469698[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_9197210553723469698[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_9197210553723469698[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_9197210553723469698[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_9197210553723469698[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_9197210553723469698[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_9197210553723469698[53] = -9.8100000000000005*dt;
|
||||
out_9197210553723469698[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_9197210553723469698[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_9197210553723469698[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_9197210553723469698[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_9197210553723469698[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_9197210553723469698[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_9197210553723469698[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_9197210553723469698[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_9197210553723469698[62] = 0;
|
||||
out_9197210553723469698[63] = 0;
|
||||
out_9197210553723469698[64] = 0;
|
||||
out_9197210553723469698[65] = 0;
|
||||
out_9197210553723469698[66] = 0;
|
||||
out_9197210553723469698[67] = 0;
|
||||
out_9197210553723469698[68] = 0;
|
||||
out_9197210553723469698[69] = 0;
|
||||
out_9197210553723469698[70] = 1;
|
||||
out_9197210553723469698[71] = 0;
|
||||
out_9197210553723469698[72] = 0;
|
||||
out_9197210553723469698[73] = 0;
|
||||
out_9197210553723469698[74] = 0;
|
||||
out_9197210553723469698[75] = 0;
|
||||
out_9197210553723469698[76] = 0;
|
||||
out_9197210553723469698[77] = 0;
|
||||
out_9197210553723469698[78] = 0;
|
||||
out_9197210553723469698[79] = 0;
|
||||
out_9197210553723469698[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_265299588973257661) {
|
||||
out_265299588973257661[0] = 1;
|
||||
out_265299588973257661[1] = 0;
|
||||
out_265299588973257661[2] = 0;
|
||||
out_265299588973257661[3] = 0;
|
||||
out_265299588973257661[4] = 0;
|
||||
out_265299588973257661[5] = 0;
|
||||
out_265299588973257661[6] = 0;
|
||||
out_265299588973257661[7] = 0;
|
||||
out_265299588973257661[8] = 0;
|
||||
out_265299588973257661[9] = 0;
|
||||
out_265299588973257661[10] = 1;
|
||||
out_265299588973257661[11] = 0;
|
||||
out_265299588973257661[12] = 0;
|
||||
out_265299588973257661[13] = 0;
|
||||
out_265299588973257661[14] = 0;
|
||||
out_265299588973257661[15] = 0;
|
||||
out_265299588973257661[16] = 0;
|
||||
out_265299588973257661[17] = 0;
|
||||
out_265299588973257661[18] = 0;
|
||||
out_265299588973257661[19] = 0;
|
||||
out_265299588973257661[20] = 1;
|
||||
out_265299588973257661[21] = 0;
|
||||
out_265299588973257661[22] = 0;
|
||||
out_265299588973257661[23] = 0;
|
||||
out_265299588973257661[24] = 0;
|
||||
out_265299588973257661[25] = 0;
|
||||
out_265299588973257661[26] = 0;
|
||||
out_265299588973257661[27] = 0;
|
||||
out_265299588973257661[28] = 0;
|
||||
out_265299588973257661[29] = 0;
|
||||
out_265299588973257661[30] = 1;
|
||||
out_265299588973257661[31] = 0;
|
||||
out_265299588973257661[32] = 0;
|
||||
out_265299588973257661[33] = 0;
|
||||
out_265299588973257661[34] = 0;
|
||||
out_265299588973257661[35] = 0;
|
||||
out_265299588973257661[36] = 0;
|
||||
out_265299588973257661[37] = 0;
|
||||
out_265299588973257661[38] = 0;
|
||||
out_265299588973257661[39] = 0;
|
||||
out_265299588973257661[40] = 1;
|
||||
out_265299588973257661[41] = 0;
|
||||
out_265299588973257661[42] = 0;
|
||||
out_265299588973257661[43] = 0;
|
||||
out_265299588973257661[44] = 0;
|
||||
out_265299588973257661[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_265299588973257661[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_265299588973257661[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_265299588973257661[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_265299588973257661[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_265299588973257661[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_265299588973257661[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_265299588973257661[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_265299588973257661[53] = -9.8100000000000005*dt;
|
||||
out_265299588973257661[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_265299588973257661[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_265299588973257661[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_265299588973257661[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_265299588973257661[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_265299588973257661[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_265299588973257661[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_265299588973257661[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_265299588973257661[62] = 0;
|
||||
out_265299588973257661[63] = 0;
|
||||
out_265299588973257661[64] = 0;
|
||||
out_265299588973257661[65] = 0;
|
||||
out_265299588973257661[66] = 0;
|
||||
out_265299588973257661[67] = 0;
|
||||
out_265299588973257661[68] = 0;
|
||||
out_265299588973257661[69] = 0;
|
||||
out_265299588973257661[70] = 1;
|
||||
out_265299588973257661[71] = 0;
|
||||
out_265299588973257661[72] = 0;
|
||||
out_265299588973257661[73] = 0;
|
||||
out_265299588973257661[74] = 0;
|
||||
out_265299588973257661[75] = 0;
|
||||
out_265299588973257661[76] = 0;
|
||||
out_265299588973257661[77] = 0;
|
||||
out_265299588973257661[78] = 0;
|
||||
out_265299588973257661[79] = 0;
|
||||
out_265299588973257661[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_7502443551709492094) {
|
||||
out_7502443551709492094[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_6366176548286495673) {
|
||||
out_6366176548286495673[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_2585977779730844316) {
|
||||
out_2585977779730844316[0] = 0;
|
||||
out_2585977779730844316[1] = 0;
|
||||
out_2585977779730844316[2] = 0;
|
||||
out_2585977779730844316[3] = 0;
|
||||
out_2585977779730844316[4] = 0;
|
||||
out_2585977779730844316[5] = 0;
|
||||
out_2585977779730844316[6] = 1;
|
||||
out_2585977779730844316[7] = 0;
|
||||
out_2585977779730844316[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_2729963794422514103) {
|
||||
out_2729963794422514103[0] = 0;
|
||||
out_2729963794422514103[1] = 0;
|
||||
out_2729963794422514103[2] = 0;
|
||||
out_2729963794422514103[3] = 0;
|
||||
out_2729963794422514103[4] = 0;
|
||||
out_2729963794422514103[5] = 0;
|
||||
out_2729963794422514103[6] = 1;
|
||||
out_2729963794422514103[7] = 0;
|
||||
out_2729963794422514103[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_4099652252284200208) {
|
||||
out_4099652252284200208[0] = state[4];
|
||||
out_4099652252284200208[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_3288800728819921756) {
|
||||
out_3288800728819921756[0] = state[4];
|
||||
out_3288800728819921756[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_2115520297687505592) {
|
||||
out_2115520297687505592[0] = 0;
|
||||
out_2115520297687505592[1] = 0;
|
||||
out_2115520297687505592[2] = 0;
|
||||
out_2115520297687505592[3] = 0;
|
||||
out_2115520297687505592[4] = 1;
|
||||
out_2115520297687505592[5] = 0;
|
||||
out_2115520297687505592[6] = 0;
|
||||
out_2115520297687505592[7] = 0;
|
||||
out_2115520297687505592[8] = 0;
|
||||
out_2115520297687505592[9] = 0;
|
||||
out_2115520297687505592[10] = 0;
|
||||
out_2115520297687505592[11] = 0;
|
||||
out_2115520297687505592[12] = 0;
|
||||
out_2115520297687505592[13] = 0;
|
||||
out_2115520297687505592[14] = 1;
|
||||
out_2115520297687505592[15] = 0;
|
||||
out_2115520297687505592[16] = 0;
|
||||
out_2115520297687505592[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_3471068885428647323) {
|
||||
out_3471068885428647323[0] = 0;
|
||||
out_3471068885428647323[1] = 0;
|
||||
out_3471068885428647323[2] = 0;
|
||||
out_3471068885428647323[3] = 0;
|
||||
out_3471068885428647323[4] = 1;
|
||||
out_3471068885428647323[5] = 0;
|
||||
out_3471068885428647323[6] = 0;
|
||||
out_3471068885428647323[7] = 0;
|
||||
out_3471068885428647323[8] = 0;
|
||||
out_3471068885428647323[9] = 0;
|
||||
out_3471068885428647323[10] = 0;
|
||||
out_3471068885428647323[11] = 0;
|
||||
out_3471068885428647323[12] = 0;
|
||||
out_3471068885428647323[13] = 0;
|
||||
out_3471068885428647323[14] = 1;
|
||||
out_3471068885428647323[15] = 0;
|
||||
out_3471068885428647323[16] = 0;
|
||||
out_3471068885428647323[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_7659630220060621239) {
|
||||
out_7659630220060621239[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_4915545155657612506) {
|
||||
out_4915545155657612506[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_2456638832587604246) {
|
||||
out_2456638832587604246[0] = 0;
|
||||
out_2456638832587604246[1] = 0;
|
||||
out_2456638832587604246[2] = 0;
|
||||
out_2456638832587604246[3] = 0;
|
||||
out_2456638832587604246[4] = 1;
|
||||
out_2456638832587604246[5] = 0;
|
||||
out_2456638832587604246[6] = 0;
|
||||
out_2456638832587604246[7] = 0;
|
||||
out_2456638832587604246[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_5248296752929762730) {
|
||||
out_5248296752929762730[0] = 0;
|
||||
out_5248296752929762730[1] = 0;
|
||||
out_5248296752929762730[2] = 0;
|
||||
out_5248296752929762730[3] = 0;
|
||||
out_5248296752929762730[4] = 1;
|
||||
out_5248296752929762730[5] = 0;
|
||||
out_5248296752929762730[6] = 0;
|
||||
out_5248296752929762730[7] = 0;
|
||||
out_5248296752929762730[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_2198951171844088969) {
|
||||
out_2198951171844088969[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_4312229270981134989) {
|
||||
out_4312229270981134989[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_1155525539143211908) {
|
||||
out_1155525539143211908[0] = 0;
|
||||
out_1155525539143211908[1] = 0;
|
||||
out_1155525539143211908[2] = 0;
|
||||
out_1155525539143211908[3] = 0;
|
||||
out_1155525539143211908[4] = 0;
|
||||
out_1155525539143211908[5] = 0;
|
||||
out_1155525539143211908[6] = 0;
|
||||
out_1155525539143211908[7] = 1;
|
||||
out_1155525539143211908[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_1011539524451542121) {
|
||||
out_1011539524451542121[0] = 0;
|
||||
out_1011539524451542121[1] = 0;
|
||||
out_1011539524451542121[2] = 0;
|
||||
out_1011539524451542121[3] = 0;
|
||||
out_1011539524451542121[4] = 0;
|
||||
out_1011539524451542121[5] = 0;
|
||||
out_1011539524451542121[6] = 0;
|
||||
out_1011539524451542121[7] = 1;
|
||||
out_1011539524451542121[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_5981920524318766290) {
|
||||
out_5981920524318766290[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_2035945772564408966) {
|
||||
out_2035945772564408966[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_281875520787179335) {
|
||||
out_281875520787179335[0] = 0;
|
||||
out_281875520787179335[1] = 0;
|
||||
out_281875520787179335[2] = 0;
|
||||
out_281875520787179335[3] = 1;
|
||||
out_281875520787179335[4] = 0;
|
||||
out_281875520787179335[5] = 0;
|
||||
out_281875520787179335[6] = 0;
|
||||
out_281875520787179335[7] = 0;
|
||||
out_281875520787179335[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_425861535478849122) {
|
||||
out_425861535478849122[0] = 0;
|
||||
out_425861535478849122[1] = 0;
|
||||
out_425861535478849122[2] = 0;
|
||||
out_425861535478849122[3] = 1;
|
||||
out_425861535478849122[4] = 0;
|
||||
out_425861535478849122[5] = 0;
|
||||
out_425861535478849122[6] = 0;
|
||||
out_425861535478849122[7] = 0;
|
||||
out_425861535478849122[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_1781344732501906151) {
|
||||
out_1781344732501906151[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_8529525650391093465) {
|
||||
out_8529525650391093465[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_1431487206082371698) {
|
||||
out_1431487206082371698[0] = 0;
|
||||
out_1431487206082371698[1] = 1;
|
||||
out_1431487206082371698[2] = 0;
|
||||
out_1431487206082371698[3] = 0;
|
||||
out_1431487206082371698[4] = 0;
|
||||
out_1431487206082371698[5] = 0;
|
||||
out_1431487206082371698[6] = 0;
|
||||
out_1431487206082371698[7] = 0;
|
||||
out_1431487206082371698[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_1287501191390701911) {
|
||||
out_1287501191390701911[0] = 0;
|
||||
out_1287501191390701911[1] = 1;
|
||||
out_1287501191390701911[2] = 0;
|
||||
out_1287501191390701911[3] = 0;
|
||||
out_1287501191390701911[4] = 0;
|
||||
out_1287501191390701911[5] = 0;
|
||||
out_1287501191390701911[6] = 0;
|
||||
out_1287501191390701911[7] = 0;
|
||||
out_1287501191390701911[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_2565647234669696274) {
|
||||
out_2565647234669696274[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_9014834141134194238) {
|
||||
out_9014834141134194238[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_6513886223151902272) {
|
||||
out_6513886223151902272[0] = 1;
|
||||
out_6513886223151902272[1] = 0;
|
||||
out_6513886223151902272[2] = 0;
|
||||
out_6513886223151902272[3] = 0;
|
||||
out_6513886223151902272[4] = 0;
|
||||
out_6513886223151902272[5] = 0;
|
||||
out_6513886223151902272[6] = 0;
|
||||
out_6513886223151902272[7] = 0;
|
||||
out_6513886223151902272[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_676129080174624340) {
|
||||
out_676129080174624340[0] = 1;
|
||||
out_676129080174624340[1] = 0;
|
||||
out_676129080174624340[2] = 0;
|
||||
out_676129080174624340[3] = 0;
|
||||
out_676129080174624340[4] = 0;
|
||||
out_676129080174624340[5] = 0;
|
||||
out_676129080174624340[6] = 0;
|
||||
out_676129080174624340[7] = 0;
|
||||
out_676129080174624340[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_7751082483008271581) {
|
||||
out_7751082483008271581[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_1515096102467553512) {
|
||||
out_1515096102467553512[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_2616623741607804744) {
|
||||
out_2616623741607804744[0] = 0;
|
||||
out_2616623741607804744[1] = 0;
|
||||
out_2616623741607804744[2] = 0;
|
||||
out_2616623741607804744[3] = 0;
|
||||
out_2616623741607804744[4] = 0;
|
||||
out_2616623741607804744[5] = 0;
|
||||
out_2616623741607804744[6] = 0;
|
||||
out_2616623741607804744[7] = 0;
|
||||
out_2616623741607804744[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_2760609756299474531) {
|
||||
out_2760609756299474531[0] = 0;
|
||||
out_2760609756299474531[1] = 0;
|
||||
out_2760609756299474531[2] = 0;
|
||||
out_2760609756299474531[3] = 0;
|
||||
out_2760609756299474531[4] = 0;
|
||||
out_2760609756299474531[5] = 0;
|
||||
out_2760609756299474531[6] = 0;
|
||||
out_2760609756299474531[7] = 0;
|
||||
out_2760609756299474531[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_1054824998219177113) {
|
||||
err_fun(nom_x, delta_x, out_1054824998219177113);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_3577007229449845283) {
|
||||
err_fun(nom_x, delta_x, out_3577007229449845283);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8618844445394053899) {
|
||||
inv_err_fun(nom_x, true_x, out_8618844445394053899);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_6399722403094813471) {
|
||||
inv_err_fun(nom_x, true_x, out_6399722403094813471);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_9083000739620874267) {
|
||||
H_mod_fun(state, out_9083000739620874267);
|
||||
void car_H_mod_fun(double *state, double *out_7904303228569973439) {
|
||||
H_mod_fun(state, out_7904303228569973439);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_2939712332855985662) {
|
||||
f_fun(state, dt, out_2939712332855985662);
|
||||
void car_f_fun(double *state, double dt, double *out_2229936979988599717) {
|
||||
f_fun(state, dt, out_2229936979988599717);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_9197210553723469698) {
|
||||
F_fun(state, dt, out_9197210553723469698);
|
||||
void car_F_fun(double *state, double dt, double *out_265299588973257661) {
|
||||
F_fun(state, dt, out_265299588973257661);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_7502443551709492094) {
|
||||
h_25(state, unused, out_7502443551709492094);
|
||||
void car_h_25(double *state, double *unused, double *out_6366176548286495673) {
|
||||
h_25(state, unused, out_6366176548286495673);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_2585977779730844316) {
|
||||
H_25(state, unused, out_2585977779730844316);
|
||||
void car_H_25(double *state, double *unused, double *out_2729963794422514103) {
|
||||
H_25(state, unused, out_2729963794422514103);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_4099652252284200208) {
|
||||
h_24(state, unused, out_4099652252284200208);
|
||||
void car_h_24(double *state, double *unused, double *out_3288800728819921756) {
|
||||
h_24(state, unused, out_3288800728819921756);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_2115520297687505592) {
|
||||
H_24(state, unused, out_2115520297687505592);
|
||||
void car_H_24(double *state, double *unused, double *out_3471068885428647323) {
|
||||
H_24(state, unused, out_3471068885428647323);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_7659630220060621239) {
|
||||
h_30(state, unused, out_7659630220060621239);
|
||||
void car_h_30(double *state, double *unused, double *out_4915545155657612506) {
|
||||
h_30(state, unused, out_4915545155657612506);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_2456638832587604246) {
|
||||
H_30(state, unused, out_2456638832587604246);
|
||||
void car_H_30(double *state, double *unused, double *out_5248296752929762730) {
|
||||
H_30(state, unused, out_5248296752929762730);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_2198951171844088969) {
|
||||
h_26(state, unused, out_2198951171844088969);
|
||||
void car_h_26(double *state, double *unused, double *out_4312229270981134989) {
|
||||
h_26(state, unused, out_4312229270981134989);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_1155525539143211908) {
|
||||
H_26(state, unused, out_1155525539143211908);
|
||||
void car_H_26(double *state, double *unused, double *out_1011539524451542121) {
|
||||
H_26(state, unused, out_1011539524451542121);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_5981920524318766290) {
|
||||
h_27(state, unused, out_5981920524318766290);
|
||||
void car_h_27(double *state, double *unused, double *out_2035945772564408966) {
|
||||
h_27(state, unused, out_2035945772564408966);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_281875520787179335) {
|
||||
H_27(state, unused, out_281875520787179335);
|
||||
void car_H_27(double *state, double *unused, double *out_425861535478849122) {
|
||||
H_27(state, unused, out_425861535478849122);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_1781344732501906151) {
|
||||
h_29(state, unused, out_1781344732501906151);
|
||||
void car_h_29(double *state, double *unused, double *out_8529525650391093465) {
|
||||
h_29(state, unused, out_8529525650391093465);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_1431487206082371698) {
|
||||
H_29(state, unused, out_1431487206082371698);
|
||||
void car_H_29(double *state, double *unused, double *out_1287501191390701911) {
|
||||
H_29(state, unused, out_1287501191390701911);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_2565647234669696274) {
|
||||
h_28(state, unused, out_2565647234669696274);
|
||||
void car_h_28(double *state, double *unused, double *out_9014834141134194238) {
|
||||
h_28(state, unused, out_9014834141134194238);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_6513886223151902272) {
|
||||
H_28(state, unused, out_6513886223151902272);
|
||||
void car_H_28(double *state, double *unused, double *out_676129080174624340) {
|
||||
H_28(state, unused, out_676129080174624340);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_7751082483008271581) {
|
||||
h_31(state, unused, out_7751082483008271581);
|
||||
void car_h_31(double *state, double *unused, double *out_1515096102467553512) {
|
||||
h_31(state, unused, out_1515096102467553512);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_2616623741607804744) {
|
||||
H_31(state, unused, out_2616623741607804744);
|
||||
void car_H_31(double *state, double *unused, double *out_2760609756299474531) {
|
||||
H_31(state, unused, out_2760609756299474531);
|
||||
}
|
||||
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_1054824998219177113);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8618844445394053899);
|
||||
void car_H_mod_fun(double *state, double *out_9083000739620874267);
|
||||
void car_f_fun(double *state, double dt, double *out_2939712332855985662);
|
||||
void car_F_fun(double *state, double dt, double *out_9197210553723469698);
|
||||
void car_h_25(double *state, double *unused, double *out_7502443551709492094);
|
||||
void car_H_25(double *state, double *unused, double *out_2585977779730844316);
|
||||
void car_h_24(double *state, double *unused, double *out_4099652252284200208);
|
||||
void car_H_24(double *state, double *unused, double *out_2115520297687505592);
|
||||
void car_h_30(double *state, double *unused, double *out_7659630220060621239);
|
||||
void car_H_30(double *state, double *unused, double *out_2456638832587604246);
|
||||
void car_h_26(double *state, double *unused, double *out_2198951171844088969);
|
||||
void car_H_26(double *state, double *unused, double *out_1155525539143211908);
|
||||
void car_h_27(double *state, double *unused, double *out_5981920524318766290);
|
||||
void car_H_27(double *state, double *unused, double *out_281875520787179335);
|
||||
void car_h_29(double *state, double *unused, double *out_1781344732501906151);
|
||||
void car_H_29(double *state, double *unused, double *out_1431487206082371698);
|
||||
void car_h_28(double *state, double *unused, double *out_2565647234669696274);
|
||||
void car_H_28(double *state, double *unused, double *out_6513886223151902272);
|
||||
void car_h_31(double *state, double *unused, double *out_7751082483008271581);
|
||||
void car_H_31(double *state, double *unused, double *out_2616623741607804744);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_3577007229449845283);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_6399722403094813471);
|
||||
void car_H_mod_fun(double *state, double *out_7904303228569973439);
|
||||
void car_f_fun(double *state, double dt, double *out_2229936979988599717);
|
||||
void car_F_fun(double *state, double dt, double *out_265299588973257661);
|
||||
void car_h_25(double *state, double *unused, double *out_6366176548286495673);
|
||||
void car_H_25(double *state, double *unused, double *out_2729963794422514103);
|
||||
void car_h_24(double *state, double *unused, double *out_3288800728819921756);
|
||||
void car_H_24(double *state, double *unused, double *out_3471068885428647323);
|
||||
void car_h_30(double *state, double *unused, double *out_4915545155657612506);
|
||||
void car_H_30(double *state, double *unused, double *out_5248296752929762730);
|
||||
void car_h_26(double *state, double *unused, double *out_4312229270981134989);
|
||||
void car_H_26(double *state, double *unused, double *out_1011539524451542121);
|
||||
void car_h_27(double *state, double *unused, double *out_2035945772564408966);
|
||||
void car_H_27(double *state, double *unused, double *out_425861535478849122);
|
||||
void car_h_29(double *state, double *unused, double *out_8529525650391093465);
|
||||
void car_H_29(double *state, double *unused, double *out_1287501191390701911);
|
||||
void car_h_28(double *state, double *unused, double *out_9014834141134194238);
|
||||
void car_H_28(double *state, double *unused, double *out_676129080174624340);
|
||||
void car_h_31(double *state, double *unused, double *out_1515096102467553512);
|
||||
void car_H_31(double *state, double *unused, double *out_2760609756299474531);
|
||||
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);
|
||||
|
||||
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_3926294361318359623);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5231104082209692226);
|
||||
void pose_H_mod_fun(double *state, double *out_5646756432622182646);
|
||||
void pose_f_fun(double *state, double dt, double *out_1137320570887410122);
|
||||
void pose_F_fun(double *state, double dt, double *out_2697011094328554799);
|
||||
void pose_h_4(double *state, double *unused, double *out_1053451418042427012);
|
||||
void pose_H_4(double *state, double *unused, double *out_7540267336217958736);
|
||||
void pose_h_10(double *state, double *unused, double *out_3684376753725260505);
|
||||
void pose_H_10(double *state, double *unused, double *out_5090252371717073425);
|
||||
void pose_h_13(double *state, double *unused, double *out_5749954067458841209);
|
||||
void pose_H_13(double *state, double *unused, double *out_4327993510885625935);
|
||||
void pose_h_14(double *state, double *unused, double *out_9151671114827748167);
|
||||
void pose_H_14(double *state, double *unused, double *out_3577026479878474207);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_6464539647254934838);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6486962015996802201);
|
||||
void pose_H_mod_fun(double *state, double *out_6072805508798864108);
|
||||
void pose_f_fun(double *state, double dt, double *out_5954902381465487783);
|
||||
void pose_F_fun(double *state, double dt, double *out_3606916503598787696);
|
||||
void pose_h_4(double *state, double *unused, double *out_5987237266806342619);
|
||||
void pose_H_4(double *state, double *unused, double *out_2798583430007914855);
|
||||
void pose_h_10(double *state, double *unused, double *out_3025597584677695987);
|
||||
void pose_H_10(double *state, double *unused, double *out_1745173860280422879);
|
||||
void pose_h_13(double *state, double *unused, double *out_8599707273883010977);
|
||||
void pose_H_13(double *state, double *unused, double *out_8037529435384935832);
|
||||
void pose_h_14(double *state, double *unused, double *out_3899158584107184147);
|
||||
void pose_H_14(double *state, double *unused, double *out_6761824286347399384);
|
||||
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.
Reference in New Issue
Block a user