mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-08-05 08:16:06 +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.
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-3d0c3a8a-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-0f8142a1-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.
@@ -1 +1 @@
|
||||
DEV-3d0c3a8a-DEBUG
|
||||
DEV-0f8142a1-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_1263767819732456823) {
|
||||
out_1263767819732456823[0] = delta_x[0] + nom_x[0];
|
||||
out_1263767819732456823[1] = delta_x[1] + nom_x[1];
|
||||
out_1263767819732456823[2] = delta_x[2] + nom_x[2];
|
||||
out_1263767819732456823[3] = delta_x[3] + nom_x[3];
|
||||
out_1263767819732456823[4] = delta_x[4] + nom_x[4];
|
||||
out_1263767819732456823[5] = delta_x[5] + nom_x[5];
|
||||
out_1263767819732456823[6] = delta_x[6] + nom_x[6];
|
||||
out_1263767819732456823[7] = delta_x[7] + nom_x[7];
|
||||
out_1263767819732456823[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_7019737380087612192) {
|
||||
out_7019737380087612192[0] = delta_x[0] + nom_x[0];
|
||||
out_7019737380087612192[1] = delta_x[1] + nom_x[1];
|
||||
out_7019737380087612192[2] = delta_x[2] + nom_x[2];
|
||||
out_7019737380087612192[3] = delta_x[3] + nom_x[3];
|
||||
out_7019737380087612192[4] = delta_x[4] + nom_x[4];
|
||||
out_7019737380087612192[5] = delta_x[5] + nom_x[5];
|
||||
out_7019737380087612192[6] = delta_x[6] + nom_x[6];
|
||||
out_7019737380087612192[7] = delta_x[7] + nom_x[7];
|
||||
out_7019737380087612192[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_3529295667634364975) {
|
||||
out_3529295667634364975[0] = -nom_x[0] + true_x[0];
|
||||
out_3529295667634364975[1] = -nom_x[1] + true_x[1];
|
||||
out_3529295667634364975[2] = -nom_x[2] + true_x[2];
|
||||
out_3529295667634364975[3] = -nom_x[3] + true_x[3];
|
||||
out_3529295667634364975[4] = -nom_x[4] + true_x[4];
|
||||
out_3529295667634364975[5] = -nom_x[5] + true_x[5];
|
||||
out_3529295667634364975[6] = -nom_x[6] + true_x[6];
|
||||
out_3529295667634364975[7] = -nom_x[7] + true_x[7];
|
||||
out_3529295667634364975[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_5837591879726777447) {
|
||||
out_5837591879726777447[0] = -nom_x[0] + true_x[0];
|
||||
out_5837591879726777447[1] = -nom_x[1] + true_x[1];
|
||||
out_5837591879726777447[2] = -nom_x[2] + true_x[2];
|
||||
out_5837591879726777447[3] = -nom_x[3] + true_x[3];
|
||||
out_5837591879726777447[4] = -nom_x[4] + true_x[4];
|
||||
out_5837591879726777447[5] = -nom_x[5] + true_x[5];
|
||||
out_5837591879726777447[6] = -nom_x[6] + true_x[6];
|
||||
out_5837591879726777447[7] = -nom_x[7] + true_x[7];
|
||||
out_5837591879726777447[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_5031874340838454569) {
|
||||
out_5031874340838454569[0] = 1.0;
|
||||
out_5031874340838454569[1] = 0.0;
|
||||
out_5031874340838454569[2] = 0.0;
|
||||
out_5031874340838454569[3] = 0.0;
|
||||
out_5031874340838454569[4] = 0.0;
|
||||
out_5031874340838454569[5] = 0.0;
|
||||
out_5031874340838454569[6] = 0.0;
|
||||
out_5031874340838454569[7] = 0.0;
|
||||
out_5031874340838454569[8] = 0.0;
|
||||
out_5031874340838454569[9] = 0.0;
|
||||
out_5031874340838454569[10] = 1.0;
|
||||
out_5031874340838454569[11] = 0.0;
|
||||
out_5031874340838454569[12] = 0.0;
|
||||
out_5031874340838454569[13] = 0.0;
|
||||
out_5031874340838454569[14] = 0.0;
|
||||
out_5031874340838454569[15] = 0.0;
|
||||
out_5031874340838454569[16] = 0.0;
|
||||
out_5031874340838454569[17] = 0.0;
|
||||
out_5031874340838454569[18] = 0.0;
|
||||
out_5031874340838454569[19] = 0.0;
|
||||
out_5031874340838454569[20] = 1.0;
|
||||
out_5031874340838454569[21] = 0.0;
|
||||
out_5031874340838454569[22] = 0.0;
|
||||
out_5031874340838454569[23] = 0.0;
|
||||
out_5031874340838454569[24] = 0.0;
|
||||
out_5031874340838454569[25] = 0.0;
|
||||
out_5031874340838454569[26] = 0.0;
|
||||
out_5031874340838454569[27] = 0.0;
|
||||
out_5031874340838454569[28] = 0.0;
|
||||
out_5031874340838454569[29] = 0.0;
|
||||
out_5031874340838454569[30] = 1.0;
|
||||
out_5031874340838454569[31] = 0.0;
|
||||
out_5031874340838454569[32] = 0.0;
|
||||
out_5031874340838454569[33] = 0.0;
|
||||
out_5031874340838454569[34] = 0.0;
|
||||
out_5031874340838454569[35] = 0.0;
|
||||
out_5031874340838454569[36] = 0.0;
|
||||
out_5031874340838454569[37] = 0.0;
|
||||
out_5031874340838454569[38] = 0.0;
|
||||
out_5031874340838454569[39] = 0.0;
|
||||
out_5031874340838454569[40] = 1.0;
|
||||
out_5031874340838454569[41] = 0.0;
|
||||
out_5031874340838454569[42] = 0.0;
|
||||
out_5031874340838454569[43] = 0.0;
|
||||
out_5031874340838454569[44] = 0.0;
|
||||
out_5031874340838454569[45] = 0.0;
|
||||
out_5031874340838454569[46] = 0.0;
|
||||
out_5031874340838454569[47] = 0.0;
|
||||
out_5031874340838454569[48] = 0.0;
|
||||
out_5031874340838454569[49] = 0.0;
|
||||
out_5031874340838454569[50] = 1.0;
|
||||
out_5031874340838454569[51] = 0.0;
|
||||
out_5031874340838454569[52] = 0.0;
|
||||
out_5031874340838454569[53] = 0.0;
|
||||
out_5031874340838454569[54] = 0.0;
|
||||
out_5031874340838454569[55] = 0.0;
|
||||
out_5031874340838454569[56] = 0.0;
|
||||
out_5031874340838454569[57] = 0.0;
|
||||
out_5031874340838454569[58] = 0.0;
|
||||
out_5031874340838454569[59] = 0.0;
|
||||
out_5031874340838454569[60] = 1.0;
|
||||
out_5031874340838454569[61] = 0.0;
|
||||
out_5031874340838454569[62] = 0.0;
|
||||
out_5031874340838454569[63] = 0.0;
|
||||
out_5031874340838454569[64] = 0.0;
|
||||
out_5031874340838454569[65] = 0.0;
|
||||
out_5031874340838454569[66] = 0.0;
|
||||
out_5031874340838454569[67] = 0.0;
|
||||
out_5031874340838454569[68] = 0.0;
|
||||
out_5031874340838454569[69] = 0.0;
|
||||
out_5031874340838454569[70] = 1.0;
|
||||
out_5031874340838454569[71] = 0.0;
|
||||
out_5031874340838454569[72] = 0.0;
|
||||
out_5031874340838454569[73] = 0.0;
|
||||
out_5031874340838454569[74] = 0.0;
|
||||
out_5031874340838454569[75] = 0.0;
|
||||
out_5031874340838454569[76] = 0.0;
|
||||
out_5031874340838454569[77] = 0.0;
|
||||
out_5031874340838454569[78] = 0.0;
|
||||
out_5031874340838454569[79] = 0.0;
|
||||
out_5031874340838454569[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_7212341887433844410) {
|
||||
out_7212341887433844410[0] = 1.0;
|
||||
out_7212341887433844410[1] = 0.0;
|
||||
out_7212341887433844410[2] = 0.0;
|
||||
out_7212341887433844410[3] = 0.0;
|
||||
out_7212341887433844410[4] = 0.0;
|
||||
out_7212341887433844410[5] = 0.0;
|
||||
out_7212341887433844410[6] = 0.0;
|
||||
out_7212341887433844410[7] = 0.0;
|
||||
out_7212341887433844410[8] = 0.0;
|
||||
out_7212341887433844410[9] = 0.0;
|
||||
out_7212341887433844410[10] = 1.0;
|
||||
out_7212341887433844410[11] = 0.0;
|
||||
out_7212341887433844410[12] = 0.0;
|
||||
out_7212341887433844410[13] = 0.0;
|
||||
out_7212341887433844410[14] = 0.0;
|
||||
out_7212341887433844410[15] = 0.0;
|
||||
out_7212341887433844410[16] = 0.0;
|
||||
out_7212341887433844410[17] = 0.0;
|
||||
out_7212341887433844410[18] = 0.0;
|
||||
out_7212341887433844410[19] = 0.0;
|
||||
out_7212341887433844410[20] = 1.0;
|
||||
out_7212341887433844410[21] = 0.0;
|
||||
out_7212341887433844410[22] = 0.0;
|
||||
out_7212341887433844410[23] = 0.0;
|
||||
out_7212341887433844410[24] = 0.0;
|
||||
out_7212341887433844410[25] = 0.0;
|
||||
out_7212341887433844410[26] = 0.0;
|
||||
out_7212341887433844410[27] = 0.0;
|
||||
out_7212341887433844410[28] = 0.0;
|
||||
out_7212341887433844410[29] = 0.0;
|
||||
out_7212341887433844410[30] = 1.0;
|
||||
out_7212341887433844410[31] = 0.0;
|
||||
out_7212341887433844410[32] = 0.0;
|
||||
out_7212341887433844410[33] = 0.0;
|
||||
out_7212341887433844410[34] = 0.0;
|
||||
out_7212341887433844410[35] = 0.0;
|
||||
out_7212341887433844410[36] = 0.0;
|
||||
out_7212341887433844410[37] = 0.0;
|
||||
out_7212341887433844410[38] = 0.0;
|
||||
out_7212341887433844410[39] = 0.0;
|
||||
out_7212341887433844410[40] = 1.0;
|
||||
out_7212341887433844410[41] = 0.0;
|
||||
out_7212341887433844410[42] = 0.0;
|
||||
out_7212341887433844410[43] = 0.0;
|
||||
out_7212341887433844410[44] = 0.0;
|
||||
out_7212341887433844410[45] = 0.0;
|
||||
out_7212341887433844410[46] = 0.0;
|
||||
out_7212341887433844410[47] = 0.0;
|
||||
out_7212341887433844410[48] = 0.0;
|
||||
out_7212341887433844410[49] = 0.0;
|
||||
out_7212341887433844410[50] = 1.0;
|
||||
out_7212341887433844410[51] = 0.0;
|
||||
out_7212341887433844410[52] = 0.0;
|
||||
out_7212341887433844410[53] = 0.0;
|
||||
out_7212341887433844410[54] = 0.0;
|
||||
out_7212341887433844410[55] = 0.0;
|
||||
out_7212341887433844410[56] = 0.0;
|
||||
out_7212341887433844410[57] = 0.0;
|
||||
out_7212341887433844410[58] = 0.0;
|
||||
out_7212341887433844410[59] = 0.0;
|
||||
out_7212341887433844410[60] = 1.0;
|
||||
out_7212341887433844410[61] = 0.0;
|
||||
out_7212341887433844410[62] = 0.0;
|
||||
out_7212341887433844410[63] = 0.0;
|
||||
out_7212341887433844410[64] = 0.0;
|
||||
out_7212341887433844410[65] = 0.0;
|
||||
out_7212341887433844410[66] = 0.0;
|
||||
out_7212341887433844410[67] = 0.0;
|
||||
out_7212341887433844410[68] = 0.0;
|
||||
out_7212341887433844410[69] = 0.0;
|
||||
out_7212341887433844410[70] = 1.0;
|
||||
out_7212341887433844410[71] = 0.0;
|
||||
out_7212341887433844410[72] = 0.0;
|
||||
out_7212341887433844410[73] = 0.0;
|
||||
out_7212341887433844410[74] = 0.0;
|
||||
out_7212341887433844410[75] = 0.0;
|
||||
out_7212341887433844410[76] = 0.0;
|
||||
out_7212341887433844410[77] = 0.0;
|
||||
out_7212341887433844410[78] = 0.0;
|
||||
out_7212341887433844410[79] = 0.0;
|
||||
out_7212341887433844410[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_1529561136835295758) {
|
||||
out_1529561136835295758[0] = state[0];
|
||||
out_1529561136835295758[1] = state[1];
|
||||
out_1529561136835295758[2] = state[2];
|
||||
out_1529561136835295758[3] = state[3];
|
||||
out_1529561136835295758[4] = state[4];
|
||||
out_1529561136835295758[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_1529561136835295758[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_1529561136835295758[7] = state[7];
|
||||
out_1529561136835295758[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_884422573903963140) {
|
||||
out_884422573903963140[0] = state[0];
|
||||
out_884422573903963140[1] = state[1];
|
||||
out_884422573903963140[2] = state[2];
|
||||
out_884422573903963140[3] = state[3];
|
||||
out_884422573903963140[4] = state[4];
|
||||
out_884422573903963140[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_884422573903963140[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_884422573903963140[7] = state[7];
|
||||
out_884422573903963140[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_665605705060988729) {
|
||||
out_665605705060988729[0] = 1;
|
||||
out_665605705060988729[1] = 0;
|
||||
out_665605705060988729[2] = 0;
|
||||
out_665605705060988729[3] = 0;
|
||||
out_665605705060988729[4] = 0;
|
||||
out_665605705060988729[5] = 0;
|
||||
out_665605705060988729[6] = 0;
|
||||
out_665605705060988729[7] = 0;
|
||||
out_665605705060988729[8] = 0;
|
||||
out_665605705060988729[9] = 0;
|
||||
out_665605705060988729[10] = 1;
|
||||
out_665605705060988729[11] = 0;
|
||||
out_665605705060988729[12] = 0;
|
||||
out_665605705060988729[13] = 0;
|
||||
out_665605705060988729[14] = 0;
|
||||
out_665605705060988729[15] = 0;
|
||||
out_665605705060988729[16] = 0;
|
||||
out_665605705060988729[17] = 0;
|
||||
out_665605705060988729[18] = 0;
|
||||
out_665605705060988729[19] = 0;
|
||||
out_665605705060988729[20] = 1;
|
||||
out_665605705060988729[21] = 0;
|
||||
out_665605705060988729[22] = 0;
|
||||
out_665605705060988729[23] = 0;
|
||||
out_665605705060988729[24] = 0;
|
||||
out_665605705060988729[25] = 0;
|
||||
out_665605705060988729[26] = 0;
|
||||
out_665605705060988729[27] = 0;
|
||||
out_665605705060988729[28] = 0;
|
||||
out_665605705060988729[29] = 0;
|
||||
out_665605705060988729[30] = 1;
|
||||
out_665605705060988729[31] = 0;
|
||||
out_665605705060988729[32] = 0;
|
||||
out_665605705060988729[33] = 0;
|
||||
out_665605705060988729[34] = 0;
|
||||
out_665605705060988729[35] = 0;
|
||||
out_665605705060988729[36] = 0;
|
||||
out_665605705060988729[37] = 0;
|
||||
out_665605705060988729[38] = 0;
|
||||
out_665605705060988729[39] = 0;
|
||||
out_665605705060988729[40] = 1;
|
||||
out_665605705060988729[41] = 0;
|
||||
out_665605705060988729[42] = 0;
|
||||
out_665605705060988729[43] = 0;
|
||||
out_665605705060988729[44] = 0;
|
||||
out_665605705060988729[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_665605705060988729[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_665605705060988729[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_665605705060988729[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_665605705060988729[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_665605705060988729[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_665605705060988729[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_665605705060988729[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_665605705060988729[53] = -9.8100000000000005*dt;
|
||||
out_665605705060988729[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_665605705060988729[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_665605705060988729[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_665605705060988729[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_665605705060988729[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_665605705060988729[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_665605705060988729[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_665605705060988729[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_665605705060988729[62] = 0;
|
||||
out_665605705060988729[63] = 0;
|
||||
out_665605705060988729[64] = 0;
|
||||
out_665605705060988729[65] = 0;
|
||||
out_665605705060988729[66] = 0;
|
||||
out_665605705060988729[67] = 0;
|
||||
out_665605705060988729[68] = 0;
|
||||
out_665605705060988729[69] = 0;
|
||||
out_665605705060988729[70] = 1;
|
||||
out_665605705060988729[71] = 0;
|
||||
out_665605705060988729[72] = 0;
|
||||
out_665605705060988729[73] = 0;
|
||||
out_665605705060988729[74] = 0;
|
||||
out_665605705060988729[75] = 0;
|
||||
out_665605705060988729[76] = 0;
|
||||
out_665605705060988729[77] = 0;
|
||||
out_665605705060988729[78] = 0;
|
||||
out_665605705060988729[79] = 0;
|
||||
out_665605705060988729[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_7930257990036637257) {
|
||||
out_7930257990036637257[0] = 1;
|
||||
out_7930257990036637257[1] = 0;
|
||||
out_7930257990036637257[2] = 0;
|
||||
out_7930257990036637257[3] = 0;
|
||||
out_7930257990036637257[4] = 0;
|
||||
out_7930257990036637257[5] = 0;
|
||||
out_7930257990036637257[6] = 0;
|
||||
out_7930257990036637257[7] = 0;
|
||||
out_7930257990036637257[8] = 0;
|
||||
out_7930257990036637257[9] = 0;
|
||||
out_7930257990036637257[10] = 1;
|
||||
out_7930257990036637257[11] = 0;
|
||||
out_7930257990036637257[12] = 0;
|
||||
out_7930257990036637257[13] = 0;
|
||||
out_7930257990036637257[14] = 0;
|
||||
out_7930257990036637257[15] = 0;
|
||||
out_7930257990036637257[16] = 0;
|
||||
out_7930257990036637257[17] = 0;
|
||||
out_7930257990036637257[18] = 0;
|
||||
out_7930257990036637257[19] = 0;
|
||||
out_7930257990036637257[20] = 1;
|
||||
out_7930257990036637257[21] = 0;
|
||||
out_7930257990036637257[22] = 0;
|
||||
out_7930257990036637257[23] = 0;
|
||||
out_7930257990036637257[24] = 0;
|
||||
out_7930257990036637257[25] = 0;
|
||||
out_7930257990036637257[26] = 0;
|
||||
out_7930257990036637257[27] = 0;
|
||||
out_7930257990036637257[28] = 0;
|
||||
out_7930257990036637257[29] = 0;
|
||||
out_7930257990036637257[30] = 1;
|
||||
out_7930257990036637257[31] = 0;
|
||||
out_7930257990036637257[32] = 0;
|
||||
out_7930257990036637257[33] = 0;
|
||||
out_7930257990036637257[34] = 0;
|
||||
out_7930257990036637257[35] = 0;
|
||||
out_7930257990036637257[36] = 0;
|
||||
out_7930257990036637257[37] = 0;
|
||||
out_7930257990036637257[38] = 0;
|
||||
out_7930257990036637257[39] = 0;
|
||||
out_7930257990036637257[40] = 1;
|
||||
out_7930257990036637257[41] = 0;
|
||||
out_7930257990036637257[42] = 0;
|
||||
out_7930257990036637257[43] = 0;
|
||||
out_7930257990036637257[44] = 0;
|
||||
out_7930257990036637257[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_7930257990036637257[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_7930257990036637257[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7930257990036637257[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7930257990036637257[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_7930257990036637257[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_7930257990036637257[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_7930257990036637257[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7930257990036637257[53] = -9.8100000000000005*dt;
|
||||
out_7930257990036637257[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_7930257990036637257[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_7930257990036637257[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7930257990036637257[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7930257990036637257[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_7930257990036637257[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_7930257990036637257[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_7930257990036637257[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7930257990036637257[62] = 0;
|
||||
out_7930257990036637257[63] = 0;
|
||||
out_7930257990036637257[64] = 0;
|
||||
out_7930257990036637257[65] = 0;
|
||||
out_7930257990036637257[66] = 0;
|
||||
out_7930257990036637257[67] = 0;
|
||||
out_7930257990036637257[68] = 0;
|
||||
out_7930257990036637257[69] = 0;
|
||||
out_7930257990036637257[70] = 1;
|
||||
out_7930257990036637257[71] = 0;
|
||||
out_7930257990036637257[72] = 0;
|
||||
out_7930257990036637257[73] = 0;
|
||||
out_7930257990036637257[74] = 0;
|
||||
out_7930257990036637257[75] = 0;
|
||||
out_7930257990036637257[76] = 0;
|
||||
out_7930257990036637257[77] = 0;
|
||||
out_7930257990036637257[78] = 0;
|
||||
out_7930257990036637257[79] = 0;
|
||||
out_7930257990036637257[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_2720961962516974745) {
|
||||
out_2720961962516974745[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_7492620826943930634) {
|
||||
out_7492620826943930634[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_5863506002035943510) {
|
||||
out_5863506002035943510[0] = 0;
|
||||
out_5863506002035943510[1] = 0;
|
||||
out_5863506002035943510[2] = 0;
|
||||
out_5863506002035943510[3] = 0;
|
||||
out_5863506002035943510[4] = 0;
|
||||
out_5863506002035943510[5] = 0;
|
||||
out_5863506002035943510[6] = 1;
|
||||
out_5863506002035943510[7] = 0;
|
||||
out_5863506002035943510[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_6439571343347838097) {
|
||||
out_6439571343347838097[0] = 0;
|
||||
out_6439571343347838097[1] = 0;
|
||||
out_6439571343347838097[2] = 0;
|
||||
out_6439571343347838097[3] = 0;
|
||||
out_6439571343347838097[4] = 0;
|
||||
out_6439571343347838097[5] = 0;
|
||||
out_6439571343347838097[6] = 1;
|
||||
out_6439571343347838097[7] = 0;
|
||||
out_6439571343347838097[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_1485787421513289501) {
|
||||
out_1485787421513289501[0] = state[4];
|
||||
out_1485787421513289501[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_4985338018763469281) {
|
||||
out_4985338018763469281[0] = state[4];
|
||||
out_4985338018763469281[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_1554885631640983383) {
|
||||
out_1554885631640983383[0] = 0;
|
||||
out_1554885631640983383[1] = 0;
|
||||
out_1554885631640983383[2] = 0;
|
||||
out_1554885631640983383[3] = 0;
|
||||
out_1554885631640983383[4] = 1;
|
||||
out_1554885631640983383[5] = 0;
|
||||
out_1554885631640983383[6] = 0;
|
||||
out_1554885631640983383[7] = 0;
|
||||
out_1554885631640983383[8] = 0;
|
||||
out_1554885631640983383[9] = 0;
|
||||
out_1554885631640983383[10] = 0;
|
||||
out_1554885631640983383[11] = 0;
|
||||
out_1554885631640983383[12] = 0;
|
||||
out_1554885631640983383[13] = 0;
|
||||
out_1554885631640983383[14] = 1;
|
||||
out_1554885631640983383[15] = 0;
|
||||
out_1554885631640983383[16] = 0;
|
||||
out_1554885631640983383[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_7852599591386246934) {
|
||||
out_7852599591386246934[0] = 0;
|
||||
out_7852599591386246934[1] = 0;
|
||||
out_7852599591386246934[2] = 0;
|
||||
out_7852599591386246934[3] = 0;
|
||||
out_7852599591386246934[4] = 1;
|
||||
out_7852599591386246934[5] = 0;
|
||||
out_7852599591386246934[6] = 0;
|
||||
out_7852599591386246934[7] = 0;
|
||||
out_7852599591386246934[8] = 0;
|
||||
out_7852599591386246934[9] = 0;
|
||||
out_7852599591386246934[10] = 0;
|
||||
out_7852599591386246934[11] = 0;
|
||||
out_7852599591386246934[12] = 0;
|
||||
out_7852599591386246934[13] = 0;
|
||||
out_7852599591386246934[14] = 1;
|
||||
out_7852599591386246934[15] = 0;
|
||||
out_7852599591386246934[16] = 0;
|
||||
out_7852599591386246934[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_2878148630868103890) {
|
||||
out_2878148630868103890[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_3190857989944948541) {
|
||||
out_3190857989944948541[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_5992844949179183580) {
|
||||
out_5992844949179183580[0] = 0;
|
||||
out_5992844949179183580[1] = 0;
|
||||
out_5992844949179183580[2] = 0;
|
||||
out_5992844949179183580[3] = 0;
|
||||
out_5992844949179183580[4] = 1;
|
||||
out_5992844949179183580[5] = 0;
|
||||
out_5992844949179183580[6] = 0;
|
||||
out_5992844949179183580[7] = 0;
|
||||
out_5992844949179183580[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_8957904301855086724) {
|
||||
out_8957904301855086724[0] = 0;
|
||||
out_8957904301855086724[1] = 0;
|
||||
out_8957904301855086724[2] = 0;
|
||||
out_8957904301855086724[3] = 0;
|
||||
out_8957904301855086724[4] = 1;
|
||||
out_8957904301855086724[5] = 0;
|
||||
out_8957904301855086724[6] = 0;
|
||||
out_8957904301855086724[7] = 0;
|
||||
out_8957904301855086724[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_158637420576228795) {
|
||||
out_158637420576228795[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_1810815277715897104) {
|
||||
out_1810815277715897104[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_8841734752799551882) {
|
||||
out_8841734752799551882[0] = 0;
|
||||
out_8841734752799551882[1] = 0;
|
||||
out_8841734752799551882[2] = 0;
|
||||
out_8841734752799551882[3] = 0;
|
||||
out_8841734752799551882[4] = 0;
|
||||
out_8841734752799551882[5] = 0;
|
||||
out_8841734752799551882[6] = 0;
|
||||
out_8841734752799551882[7] = 1;
|
||||
out_8841734752799551882[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_2698068024473781873) {
|
||||
out_2698068024473781873[0] = 0;
|
||||
out_2698068024473781873[1] = 0;
|
||||
out_2698068024473781873[2] = 0;
|
||||
out_2698068024473781873[3] = 0;
|
||||
out_2698068024473781873[4] = 0;
|
||||
out_2698068024473781873[5] = 0;
|
||||
out_2698068024473781873[6] = 0;
|
||||
out_2698068024473781873[7] = 1;
|
||||
out_2698068024473781873[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_2280711305143870900) {
|
||||
out_2280711305143870900[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_1225216092924761692) {
|
||||
out_1225216092924761692[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_8167608260979608491) {
|
||||
out_8167608260979608491[0] = 0;
|
||||
out_8167608260979608491[1] = 0;
|
||||
out_8167608260979608491[2] = 0;
|
||||
out_8167608260979608491[3] = 1;
|
||||
out_8167608260979608491[4] = 0;
|
||||
out_8167608260979608491[5] = 0;
|
||||
out_8167608260979608491[6] = 0;
|
||||
out_8167608260979608491[7] = 0;
|
||||
out_8167608260979608491[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_6783140990054661813) {
|
||||
out_6783140990054661813[0] = 0;
|
||||
out_6783140990054661813[1] = 0;
|
||||
out_6783140990054661813[2] = 0;
|
||||
out_6783140990054661813[3] = 1;
|
||||
out_6783140990054661813[4] = 0;
|
||||
out_6783140990054661813[5] = 0;
|
||||
out_6783140990054661813[6] = 0;
|
||||
out_6783140990054661813[7] = 0;
|
||||
out_6783140990054661813[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_6142394781572372552) {
|
||||
out_6142394781572372552[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_8192531257605794186) {
|
||||
out_8192531257605794186[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_5482613604864791396) {
|
||||
out_5482613604864791396[0] = 0;
|
||||
out_5482613604864791396[1] = 1;
|
||||
out_5482613604864791396[2] = 0;
|
||||
out_5482613604864791396[3] = 0;
|
||||
out_5482613604864791396[4] = 0;
|
||||
out_5482613604864791396[5] = 0;
|
||||
out_5482613604864791396[6] = 0;
|
||||
out_5482613604864791396[7] = 0;
|
||||
out_5482613604864791396[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_8978608427540072708) {
|
||||
out_8978608427540072708[0] = 0;
|
||||
out_8978608427540072708[1] = 1;
|
||||
out_8978608427540072708[2] = 0;
|
||||
out_8978608427540072708[3] = 0;
|
||||
out_8978608427540072708[4] = 0;
|
||||
out_8978608427540072708[5] = 0;
|
||||
out_8978608427540072708[6] = 0;
|
||||
out_8978608427540072708[7] = 0;
|
||||
out_8978608427540072708[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_6701257866862969865) {
|
||||
out_6701257866862969865[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_6973827342419625862) {
|
||||
out_6973827342419625862[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_7881731451775229646) {
|
||||
out_7881731451775229646[0] = 1;
|
||||
out_7881731451775229646[1] = 0;
|
||||
out_7881731451775229646[2] = 0;
|
||||
out_7881731451775229646[3] = 0;
|
||||
out_7881731451775229646[4] = 0;
|
||||
out_7881731451775229646[5] = 0;
|
||||
out_7881731451775229646[6] = 0;
|
||||
out_7881731451775229646[7] = 0;
|
||||
out_7881731451775229646[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_4385736629099948334) {
|
||||
out_4385736629099948334[0] = 1;
|
||||
out_4385736629099948334[1] = 0;
|
||||
out_4385736629099948334[2] = 0;
|
||||
out_4385736629099948334[3] = 0;
|
||||
out_4385736629099948334[4] = 0;
|
||||
out_4385736629099948334[5] = 0;
|
||||
out_4385736629099948334[6] = 0;
|
||||
out_4385736629099948334[7] = 0;
|
||||
out_4385736629099948334[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_4049873263833376191) {
|
||||
out_4049873263833376191[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_7638140651164585605) {
|
||||
out_7638140651164585605[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_5832860040158983082) {
|
||||
out_5832860040158983082[0] = 0;
|
||||
out_5832860040158983082[1] = 0;
|
||||
out_5832860040158983082[2] = 0;
|
||||
out_5832860040158983082[3] = 0;
|
||||
out_5832860040158983082[4] = 0;
|
||||
out_5832860040158983082[5] = 0;
|
||||
out_5832860040158983082[6] = 0;
|
||||
out_5832860040158983082[7] = 0;
|
||||
out_5832860040158983082[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_2071859922240430397) {
|
||||
out_2071859922240430397[0] = 0;
|
||||
out_2071859922240430397[1] = 0;
|
||||
out_2071859922240430397[2] = 0;
|
||||
out_2071859922240430397[3] = 0;
|
||||
out_2071859922240430397[4] = 0;
|
||||
out_2071859922240430397[5] = 0;
|
||||
out_2071859922240430397[6] = 0;
|
||||
out_2071859922240430397[7] = 0;
|
||||
out_2071859922240430397[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_1263767819732456823) {
|
||||
err_fun(nom_x, delta_x, out_1263767819732456823);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_7019737380087612192) {
|
||||
err_fun(nom_x, delta_x, out_7019737380087612192);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3529295667634364975) {
|
||||
inv_err_fun(nom_x, true_x, out_3529295667634364975);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5837591879726777447) {
|
||||
inv_err_fun(nom_x, true_x, out_5837591879726777447);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_5031874340838454569) {
|
||||
H_mod_fun(state, out_5031874340838454569);
|
||||
void car_H_mod_fun(double *state, double *out_7212341887433844410) {
|
||||
H_mod_fun(state, out_7212341887433844410);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_1529561136835295758) {
|
||||
f_fun(state, dt, out_1529561136835295758);
|
||||
void car_f_fun(double *state, double dt, double *out_884422573903963140) {
|
||||
f_fun(state, dt, out_884422573903963140);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_665605705060988729) {
|
||||
F_fun(state, dt, out_665605705060988729);
|
||||
void car_F_fun(double *state, double dt, double *out_7930257990036637257) {
|
||||
F_fun(state, dt, out_7930257990036637257);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_2720961962516974745) {
|
||||
h_25(state, unused, out_2720961962516974745);
|
||||
void car_h_25(double *state, double *unused, double *out_7492620826943930634) {
|
||||
h_25(state, unused, out_7492620826943930634);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_5863506002035943510) {
|
||||
H_25(state, unused, out_5863506002035943510);
|
||||
void car_H_25(double *state, double *unused, double *out_6439571343347838097) {
|
||||
H_25(state, unused, out_6439571343347838097);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_1485787421513289501) {
|
||||
h_24(state, unused, out_1485787421513289501);
|
||||
void car_h_24(double *state, double *unused, double *out_4985338018763469281) {
|
||||
h_24(state, unused, out_4985338018763469281);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_1554885631640983383) {
|
||||
H_24(state, unused, out_1554885631640983383);
|
||||
void car_H_24(double *state, double *unused, double *out_7852599591386246934) {
|
||||
H_24(state, unused, out_7852599591386246934);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_2878148630868103890) {
|
||||
h_30(state, unused, out_2878148630868103890);
|
||||
void car_h_30(double *state, double *unused, double *out_3190857989944948541) {
|
||||
h_30(state, unused, out_3190857989944948541);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_5992844949179183580) {
|
||||
H_30(state, unused, out_5992844949179183580);
|
||||
void car_H_30(double *state, double *unused, double *out_8957904301855086724) {
|
||||
H_30(state, unused, out_8957904301855086724);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_158637420576228795) {
|
||||
h_26(state, unused, out_158637420576228795);
|
||||
void car_h_26(double *state, double *unused, double *out_1810815277715897104) {
|
||||
h_26(state, unused, out_1810815277715897104);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_8841734752799551882) {
|
||||
H_26(state, unused, out_8841734752799551882);
|
||||
void car_H_26(double *state, double *unused, double *out_2698068024473781873) {
|
||||
H_26(state, unused, out_2698068024473781873);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_2280711305143870900) {
|
||||
h_27(state, unused, out_2280711305143870900);
|
||||
void car_h_27(double *state, double *unused, double *out_1225216092924761692) {
|
||||
h_27(state, unused, out_1225216092924761692);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_8167608260979608491) {
|
||||
H_27(state, unused, out_8167608260979608491);
|
||||
void car_H_27(double *state, double *unused, double *out_6783140990054661813) {
|
||||
H_27(state, unused, out_6783140990054661813);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_6142394781572372552) {
|
||||
h_29(state, unused, out_6142394781572372552);
|
||||
void car_h_29(double *state, double *unused, double *out_8192531257605794186) {
|
||||
h_29(state, unused, out_8192531257605794186);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_5482613604864791396) {
|
||||
H_29(state, unused, out_5482613604864791396);
|
||||
void car_H_29(double *state, double *unused, double *out_8978608427540072708) {
|
||||
H_29(state, unused, out_8978608427540072708);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_6701257866862969865) {
|
||||
h_28(state, unused, out_6701257866862969865);
|
||||
void car_h_28(double *state, double *unused, double *out_6973827342419625862) {
|
||||
h_28(state, unused, out_6973827342419625862);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_7881731451775229646) {
|
||||
H_28(state, unused, out_7881731451775229646);
|
||||
void car_H_28(double *state, double *unused, double *out_4385736629099948334) {
|
||||
H_28(state, unused, out_4385736629099948334);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_4049873263833376191) {
|
||||
h_31(state, unused, out_4049873263833376191);
|
||||
void car_h_31(double *state, double *unused, double *out_7638140651164585605) {
|
||||
h_31(state, unused, out_7638140651164585605);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_5832860040158983082) {
|
||||
H_31(state, unused, out_5832860040158983082);
|
||||
void car_H_31(double *state, double *unused, double *out_2071859922240430397) {
|
||||
H_31(state, unused, out_2071859922240430397);
|
||||
}
|
||||
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_1263767819732456823);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3529295667634364975);
|
||||
void car_H_mod_fun(double *state, double *out_5031874340838454569);
|
||||
void car_f_fun(double *state, double dt, double *out_1529561136835295758);
|
||||
void car_F_fun(double *state, double dt, double *out_665605705060988729);
|
||||
void car_h_25(double *state, double *unused, double *out_2720961962516974745);
|
||||
void car_H_25(double *state, double *unused, double *out_5863506002035943510);
|
||||
void car_h_24(double *state, double *unused, double *out_1485787421513289501);
|
||||
void car_H_24(double *state, double *unused, double *out_1554885631640983383);
|
||||
void car_h_30(double *state, double *unused, double *out_2878148630868103890);
|
||||
void car_H_30(double *state, double *unused, double *out_5992844949179183580);
|
||||
void car_h_26(double *state, double *unused, double *out_158637420576228795);
|
||||
void car_H_26(double *state, double *unused, double *out_8841734752799551882);
|
||||
void car_h_27(double *state, double *unused, double *out_2280711305143870900);
|
||||
void car_H_27(double *state, double *unused, double *out_8167608260979608491);
|
||||
void car_h_29(double *state, double *unused, double *out_6142394781572372552);
|
||||
void car_H_29(double *state, double *unused, double *out_5482613604864791396);
|
||||
void car_h_28(double *state, double *unused, double *out_6701257866862969865);
|
||||
void car_H_28(double *state, double *unused, double *out_7881731451775229646);
|
||||
void car_h_31(double *state, double *unused, double *out_4049873263833376191);
|
||||
void car_H_31(double *state, double *unused, double *out_5832860040158983082);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_7019737380087612192);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5837591879726777447);
|
||||
void car_H_mod_fun(double *state, double *out_7212341887433844410);
|
||||
void car_f_fun(double *state, double dt, double *out_884422573903963140);
|
||||
void car_F_fun(double *state, double dt, double *out_7930257990036637257);
|
||||
void car_h_25(double *state, double *unused, double *out_7492620826943930634);
|
||||
void car_H_25(double *state, double *unused, double *out_6439571343347838097);
|
||||
void car_h_24(double *state, double *unused, double *out_4985338018763469281);
|
||||
void car_H_24(double *state, double *unused, double *out_7852599591386246934);
|
||||
void car_h_30(double *state, double *unused, double *out_3190857989944948541);
|
||||
void car_H_30(double *state, double *unused, double *out_8957904301855086724);
|
||||
void car_h_26(double *state, double *unused, double *out_1810815277715897104);
|
||||
void car_H_26(double *state, double *unused, double *out_2698068024473781873);
|
||||
void car_h_27(double *state, double *unused, double *out_1225216092924761692);
|
||||
void car_H_27(double *state, double *unused, double *out_6783140990054661813);
|
||||
void car_h_29(double *state, double *unused, double *out_8192531257605794186);
|
||||
void car_H_29(double *state, double *unused, double *out_8978608427540072708);
|
||||
void car_h_28(double *state, double *unused, double *out_6973827342419625862);
|
||||
void car_H_28(double *state, double *unused, double *out_4385736629099948334);
|
||||
void car_h_31(double *state, double *unused, double *out_7638140651164585605);
|
||||
void car_H_31(double *state, double *unused, double *out_2071859922240430397);
|
||||
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_8550489212526755506);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2197167056715094306);
|
||||
void pose_H_mod_fun(double *state, double *out_1273572215837011735);
|
||||
void pose_f_fun(double *state, double dt, double *out_58843541046312828);
|
||||
void pose_F_fun(double *state, double dt, double *out_4952539587807327652);
|
||||
void pose_h_4(double *state, double *unused, double *out_105029621528743882);
|
||||
void pose_H_4(double *state, double *unused, double *out_7121654022588970456);
|
||||
void pose_h_10(double *state, double *unused, double *out_7355867801776776353);
|
||||
void pose_H_10(double *state, double *unused, double *out_6146846088544597929);
|
||||
void pose_h_13(double *state, double *unused, double *out_2344100091734301077);
|
||||
void pose_H_13(double *state, double *unused, double *out_3909380197256637655);
|
||||
void pose_h_14(double *state, double *unused, double *out_589344124113930337);
|
||||
void pose_H_14(double *state, double *unused, double *out_3158413166249485927);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_937271495952840532);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_712402659692030222);
|
||||
void pose_H_mod_fun(double *state, double *out_4304163029800659692);
|
||||
void pose_f_fun(double *state, double dt, double *out_7809694961382882469);
|
||||
void pose_F_fun(double *state, double dt, double *out_4402047608279739822);
|
||||
void pose_h_4(double *state, double *unused, double *out_1761018668225147827);
|
||||
void pose_H_4(double *state, double *unused, double *out_6123861755706956553);
|
||||
void pose_h_10(double *state, double *unused, double *out_5906261859060682989);
|
||||
void pose_H_10(double *state, double *unused, double *out_8717187080084033317);
|
||||
void pose_h_13(double *state, double *unused, double *out_5437403947989237748);
|
||||
void pose_H_13(double *state, double *unused, double *out_8489126854700071039);
|
||||
void pose_h_14(double *state, double *unused, double *out_2973157686650314108);
|
||||
void pose_H_14(double *state, double *unused, double *out_9206650188002328849);
|
||||
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.
Reference in New Issue
Block a user