Compiled on Mar.26,2024
This commit is contained in:
Comma Device
@@ -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_7226686656405340864) {
|
||||
out_7226686656405340864[0] = delta_x[0] + nom_x[0];
|
||||
out_7226686656405340864[1] = delta_x[1] + nom_x[1];
|
||||
out_7226686656405340864[2] = delta_x[2] + nom_x[2];
|
||||
out_7226686656405340864[3] = delta_x[3] + nom_x[3];
|
||||
out_7226686656405340864[4] = delta_x[4] + nom_x[4];
|
||||
out_7226686656405340864[5] = delta_x[5] + nom_x[5];
|
||||
out_7226686656405340864[6] = delta_x[6] + nom_x[6];
|
||||
out_7226686656405340864[7] = delta_x[7] + nom_x[7];
|
||||
out_7226686656405340864[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_7418791751555442547) {
|
||||
out_7418791751555442547[0] = delta_x[0] + nom_x[0];
|
||||
out_7418791751555442547[1] = delta_x[1] + nom_x[1];
|
||||
out_7418791751555442547[2] = delta_x[2] + nom_x[2];
|
||||
out_7418791751555442547[3] = delta_x[3] + nom_x[3];
|
||||
out_7418791751555442547[4] = delta_x[4] + nom_x[4];
|
||||
out_7418791751555442547[5] = delta_x[5] + nom_x[5];
|
||||
out_7418791751555442547[6] = delta_x[6] + nom_x[6];
|
||||
out_7418791751555442547[7] = delta_x[7] + nom_x[7];
|
||||
out_7418791751555442547[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_7867434532787478357) {
|
||||
out_7867434532787478357[0] = -nom_x[0] + true_x[0];
|
||||
out_7867434532787478357[1] = -nom_x[1] + true_x[1];
|
||||
out_7867434532787478357[2] = -nom_x[2] + true_x[2];
|
||||
out_7867434532787478357[3] = -nom_x[3] + true_x[3];
|
||||
out_7867434532787478357[4] = -nom_x[4] + true_x[4];
|
||||
out_7867434532787478357[5] = -nom_x[5] + true_x[5];
|
||||
out_7867434532787478357[6] = -nom_x[6] + true_x[6];
|
||||
out_7867434532787478357[7] = -nom_x[7] + true_x[7];
|
||||
out_7867434532787478357[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_4675497126535007434) {
|
||||
out_4675497126535007434[0] = -nom_x[0] + true_x[0];
|
||||
out_4675497126535007434[1] = -nom_x[1] + true_x[1];
|
||||
out_4675497126535007434[2] = -nom_x[2] + true_x[2];
|
||||
out_4675497126535007434[3] = -nom_x[3] + true_x[3];
|
||||
out_4675497126535007434[4] = -nom_x[4] + true_x[4];
|
||||
out_4675497126535007434[5] = -nom_x[5] + true_x[5];
|
||||
out_4675497126535007434[6] = -nom_x[6] + true_x[6];
|
||||
out_4675497126535007434[7] = -nom_x[7] + true_x[7];
|
||||
out_4675497126535007434[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_3288405267109084725) {
|
||||
out_3288405267109084725[0] = 1.0;
|
||||
out_3288405267109084725[1] = 0;
|
||||
out_3288405267109084725[2] = 0;
|
||||
out_3288405267109084725[3] = 0;
|
||||
out_3288405267109084725[4] = 0;
|
||||
out_3288405267109084725[5] = 0;
|
||||
out_3288405267109084725[6] = 0;
|
||||
out_3288405267109084725[7] = 0;
|
||||
out_3288405267109084725[8] = 0;
|
||||
out_3288405267109084725[9] = 0;
|
||||
out_3288405267109084725[10] = 1.0;
|
||||
out_3288405267109084725[11] = 0;
|
||||
out_3288405267109084725[12] = 0;
|
||||
out_3288405267109084725[13] = 0;
|
||||
out_3288405267109084725[14] = 0;
|
||||
out_3288405267109084725[15] = 0;
|
||||
out_3288405267109084725[16] = 0;
|
||||
out_3288405267109084725[17] = 0;
|
||||
out_3288405267109084725[18] = 0;
|
||||
out_3288405267109084725[19] = 0;
|
||||
out_3288405267109084725[20] = 1.0;
|
||||
out_3288405267109084725[21] = 0;
|
||||
out_3288405267109084725[22] = 0;
|
||||
out_3288405267109084725[23] = 0;
|
||||
out_3288405267109084725[24] = 0;
|
||||
out_3288405267109084725[25] = 0;
|
||||
out_3288405267109084725[26] = 0;
|
||||
out_3288405267109084725[27] = 0;
|
||||
out_3288405267109084725[28] = 0;
|
||||
out_3288405267109084725[29] = 0;
|
||||
out_3288405267109084725[30] = 1.0;
|
||||
out_3288405267109084725[31] = 0;
|
||||
out_3288405267109084725[32] = 0;
|
||||
out_3288405267109084725[33] = 0;
|
||||
out_3288405267109084725[34] = 0;
|
||||
out_3288405267109084725[35] = 0;
|
||||
out_3288405267109084725[36] = 0;
|
||||
out_3288405267109084725[37] = 0;
|
||||
out_3288405267109084725[38] = 0;
|
||||
out_3288405267109084725[39] = 0;
|
||||
out_3288405267109084725[40] = 1.0;
|
||||
out_3288405267109084725[41] = 0;
|
||||
out_3288405267109084725[42] = 0;
|
||||
out_3288405267109084725[43] = 0;
|
||||
out_3288405267109084725[44] = 0;
|
||||
out_3288405267109084725[45] = 0;
|
||||
out_3288405267109084725[46] = 0;
|
||||
out_3288405267109084725[47] = 0;
|
||||
out_3288405267109084725[48] = 0;
|
||||
out_3288405267109084725[49] = 0;
|
||||
out_3288405267109084725[50] = 1.0;
|
||||
out_3288405267109084725[51] = 0;
|
||||
out_3288405267109084725[52] = 0;
|
||||
out_3288405267109084725[53] = 0;
|
||||
out_3288405267109084725[54] = 0;
|
||||
out_3288405267109084725[55] = 0;
|
||||
out_3288405267109084725[56] = 0;
|
||||
out_3288405267109084725[57] = 0;
|
||||
out_3288405267109084725[58] = 0;
|
||||
out_3288405267109084725[59] = 0;
|
||||
out_3288405267109084725[60] = 1.0;
|
||||
out_3288405267109084725[61] = 0;
|
||||
out_3288405267109084725[62] = 0;
|
||||
out_3288405267109084725[63] = 0;
|
||||
out_3288405267109084725[64] = 0;
|
||||
out_3288405267109084725[65] = 0;
|
||||
out_3288405267109084725[66] = 0;
|
||||
out_3288405267109084725[67] = 0;
|
||||
out_3288405267109084725[68] = 0;
|
||||
out_3288405267109084725[69] = 0;
|
||||
out_3288405267109084725[70] = 1.0;
|
||||
out_3288405267109084725[71] = 0;
|
||||
out_3288405267109084725[72] = 0;
|
||||
out_3288405267109084725[73] = 0;
|
||||
out_3288405267109084725[74] = 0;
|
||||
out_3288405267109084725[75] = 0;
|
||||
out_3288405267109084725[76] = 0;
|
||||
out_3288405267109084725[77] = 0;
|
||||
out_3288405267109084725[78] = 0;
|
||||
out_3288405267109084725[79] = 0;
|
||||
out_3288405267109084725[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_4383613899688875359) {
|
||||
out_4383613899688875359[0] = 1.0;
|
||||
out_4383613899688875359[1] = 0;
|
||||
out_4383613899688875359[2] = 0;
|
||||
out_4383613899688875359[3] = 0;
|
||||
out_4383613899688875359[4] = 0;
|
||||
out_4383613899688875359[5] = 0;
|
||||
out_4383613899688875359[6] = 0;
|
||||
out_4383613899688875359[7] = 0;
|
||||
out_4383613899688875359[8] = 0;
|
||||
out_4383613899688875359[9] = 0;
|
||||
out_4383613899688875359[10] = 1.0;
|
||||
out_4383613899688875359[11] = 0;
|
||||
out_4383613899688875359[12] = 0;
|
||||
out_4383613899688875359[13] = 0;
|
||||
out_4383613899688875359[14] = 0;
|
||||
out_4383613899688875359[15] = 0;
|
||||
out_4383613899688875359[16] = 0;
|
||||
out_4383613899688875359[17] = 0;
|
||||
out_4383613899688875359[18] = 0;
|
||||
out_4383613899688875359[19] = 0;
|
||||
out_4383613899688875359[20] = 1.0;
|
||||
out_4383613899688875359[21] = 0;
|
||||
out_4383613899688875359[22] = 0;
|
||||
out_4383613899688875359[23] = 0;
|
||||
out_4383613899688875359[24] = 0;
|
||||
out_4383613899688875359[25] = 0;
|
||||
out_4383613899688875359[26] = 0;
|
||||
out_4383613899688875359[27] = 0;
|
||||
out_4383613899688875359[28] = 0;
|
||||
out_4383613899688875359[29] = 0;
|
||||
out_4383613899688875359[30] = 1.0;
|
||||
out_4383613899688875359[31] = 0;
|
||||
out_4383613899688875359[32] = 0;
|
||||
out_4383613899688875359[33] = 0;
|
||||
out_4383613899688875359[34] = 0;
|
||||
out_4383613899688875359[35] = 0;
|
||||
out_4383613899688875359[36] = 0;
|
||||
out_4383613899688875359[37] = 0;
|
||||
out_4383613899688875359[38] = 0;
|
||||
out_4383613899688875359[39] = 0;
|
||||
out_4383613899688875359[40] = 1.0;
|
||||
out_4383613899688875359[41] = 0;
|
||||
out_4383613899688875359[42] = 0;
|
||||
out_4383613899688875359[43] = 0;
|
||||
out_4383613899688875359[44] = 0;
|
||||
out_4383613899688875359[45] = 0;
|
||||
out_4383613899688875359[46] = 0;
|
||||
out_4383613899688875359[47] = 0;
|
||||
out_4383613899688875359[48] = 0;
|
||||
out_4383613899688875359[49] = 0;
|
||||
out_4383613899688875359[50] = 1.0;
|
||||
out_4383613899688875359[51] = 0;
|
||||
out_4383613899688875359[52] = 0;
|
||||
out_4383613899688875359[53] = 0;
|
||||
out_4383613899688875359[54] = 0;
|
||||
out_4383613899688875359[55] = 0;
|
||||
out_4383613899688875359[56] = 0;
|
||||
out_4383613899688875359[57] = 0;
|
||||
out_4383613899688875359[58] = 0;
|
||||
out_4383613899688875359[59] = 0;
|
||||
out_4383613899688875359[60] = 1.0;
|
||||
out_4383613899688875359[61] = 0;
|
||||
out_4383613899688875359[62] = 0;
|
||||
out_4383613899688875359[63] = 0;
|
||||
out_4383613899688875359[64] = 0;
|
||||
out_4383613899688875359[65] = 0;
|
||||
out_4383613899688875359[66] = 0;
|
||||
out_4383613899688875359[67] = 0;
|
||||
out_4383613899688875359[68] = 0;
|
||||
out_4383613899688875359[69] = 0;
|
||||
out_4383613899688875359[70] = 1.0;
|
||||
out_4383613899688875359[71] = 0;
|
||||
out_4383613899688875359[72] = 0;
|
||||
out_4383613899688875359[73] = 0;
|
||||
out_4383613899688875359[74] = 0;
|
||||
out_4383613899688875359[75] = 0;
|
||||
out_4383613899688875359[76] = 0;
|
||||
out_4383613899688875359[77] = 0;
|
||||
out_4383613899688875359[78] = 0;
|
||||
out_4383613899688875359[79] = 0;
|
||||
out_4383613899688875359[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_8974995752329095985) {
|
||||
out_8974995752329095985[0] = state[0];
|
||||
out_8974995752329095985[1] = state[1];
|
||||
out_8974995752329095985[2] = state[2];
|
||||
out_8974995752329095985[3] = state[3];
|
||||
out_8974995752329095985[4] = state[4];
|
||||
out_8974995752329095985[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*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_8974995752329095985[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_8974995752329095985[7] = state[7];
|
||||
out_8974995752329095985[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_2672769777354396612) {
|
||||
out_2672769777354396612[0] = state[0];
|
||||
out_2672769777354396612[1] = state[1];
|
||||
out_2672769777354396612[2] = state[2];
|
||||
out_2672769777354396612[3] = state[3];
|
||||
out_2672769777354396612[4] = state[4];
|
||||
out_2672769777354396612[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*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_2672769777354396612[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_2672769777354396612[7] = state[7];
|
||||
out_2672769777354396612[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_2278957145959582876) {
|
||||
out_2278957145959582876[0] = 1;
|
||||
out_2278957145959582876[1] = 0;
|
||||
out_2278957145959582876[2] = 0;
|
||||
out_2278957145959582876[3] = 0;
|
||||
out_2278957145959582876[4] = 0;
|
||||
out_2278957145959582876[5] = 0;
|
||||
out_2278957145959582876[6] = 0;
|
||||
out_2278957145959582876[7] = 0;
|
||||
out_2278957145959582876[8] = 0;
|
||||
out_2278957145959582876[9] = 0;
|
||||
out_2278957145959582876[10] = 1;
|
||||
out_2278957145959582876[11] = 0;
|
||||
out_2278957145959582876[12] = 0;
|
||||
out_2278957145959582876[13] = 0;
|
||||
out_2278957145959582876[14] = 0;
|
||||
out_2278957145959582876[15] = 0;
|
||||
out_2278957145959582876[16] = 0;
|
||||
out_2278957145959582876[17] = 0;
|
||||
out_2278957145959582876[18] = 0;
|
||||
out_2278957145959582876[19] = 0;
|
||||
out_2278957145959582876[20] = 1;
|
||||
out_2278957145959582876[21] = 0;
|
||||
out_2278957145959582876[22] = 0;
|
||||
out_2278957145959582876[23] = 0;
|
||||
out_2278957145959582876[24] = 0;
|
||||
out_2278957145959582876[25] = 0;
|
||||
out_2278957145959582876[26] = 0;
|
||||
out_2278957145959582876[27] = 0;
|
||||
out_2278957145959582876[28] = 0;
|
||||
out_2278957145959582876[29] = 0;
|
||||
out_2278957145959582876[30] = 1;
|
||||
out_2278957145959582876[31] = 0;
|
||||
out_2278957145959582876[32] = 0;
|
||||
out_2278957145959582876[33] = 0;
|
||||
out_2278957145959582876[34] = 0;
|
||||
out_2278957145959582876[35] = 0;
|
||||
out_2278957145959582876[36] = 0;
|
||||
out_2278957145959582876[37] = 0;
|
||||
out_2278957145959582876[38] = 0;
|
||||
out_2278957145959582876[39] = 0;
|
||||
out_2278957145959582876[40] = 1;
|
||||
out_2278957145959582876[41] = 0;
|
||||
out_2278957145959582876[42] = 0;
|
||||
out_2278957145959582876[43] = 0;
|
||||
out_2278957145959582876[44] = 0;
|
||||
out_2278957145959582876[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_2278957145959582876[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_2278957145959582876[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2278957145959582876[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2278957145959582876[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_2278957145959582876[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_2278957145959582876[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_2278957145959582876[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2278957145959582876[53] = -9.8000000000000007*dt;
|
||||
out_2278957145959582876[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_2278957145959582876[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_2278957145959582876[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2278957145959582876[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2278957145959582876[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_2278957145959582876[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_2278957145959582876[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_2278957145959582876[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2278957145959582876[62] = 0;
|
||||
out_2278957145959582876[63] = 0;
|
||||
out_2278957145959582876[64] = 0;
|
||||
out_2278957145959582876[65] = 0;
|
||||
out_2278957145959582876[66] = 0;
|
||||
out_2278957145959582876[67] = 0;
|
||||
out_2278957145959582876[68] = 0;
|
||||
out_2278957145959582876[69] = 0;
|
||||
out_2278957145959582876[70] = 1;
|
||||
out_2278957145959582876[71] = 0;
|
||||
out_2278957145959582876[72] = 0;
|
||||
out_2278957145959582876[73] = 0;
|
||||
out_2278957145959582876[74] = 0;
|
||||
out_2278957145959582876[75] = 0;
|
||||
out_2278957145959582876[76] = 0;
|
||||
out_2278957145959582876[77] = 0;
|
||||
out_2278957145959582876[78] = 0;
|
||||
out_2278957145959582876[79] = 0;
|
||||
out_2278957145959582876[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_8981334264948615307) {
|
||||
out_8981334264948615307[0] = 1;
|
||||
out_8981334264948615307[1] = 0;
|
||||
out_8981334264948615307[2] = 0;
|
||||
out_8981334264948615307[3] = 0;
|
||||
out_8981334264948615307[4] = 0;
|
||||
out_8981334264948615307[5] = 0;
|
||||
out_8981334264948615307[6] = 0;
|
||||
out_8981334264948615307[7] = 0;
|
||||
out_8981334264948615307[8] = 0;
|
||||
out_8981334264948615307[9] = 0;
|
||||
out_8981334264948615307[10] = 1;
|
||||
out_8981334264948615307[11] = 0;
|
||||
out_8981334264948615307[12] = 0;
|
||||
out_8981334264948615307[13] = 0;
|
||||
out_8981334264948615307[14] = 0;
|
||||
out_8981334264948615307[15] = 0;
|
||||
out_8981334264948615307[16] = 0;
|
||||
out_8981334264948615307[17] = 0;
|
||||
out_8981334264948615307[18] = 0;
|
||||
out_8981334264948615307[19] = 0;
|
||||
out_8981334264948615307[20] = 1;
|
||||
out_8981334264948615307[21] = 0;
|
||||
out_8981334264948615307[22] = 0;
|
||||
out_8981334264948615307[23] = 0;
|
||||
out_8981334264948615307[24] = 0;
|
||||
out_8981334264948615307[25] = 0;
|
||||
out_8981334264948615307[26] = 0;
|
||||
out_8981334264948615307[27] = 0;
|
||||
out_8981334264948615307[28] = 0;
|
||||
out_8981334264948615307[29] = 0;
|
||||
out_8981334264948615307[30] = 1;
|
||||
out_8981334264948615307[31] = 0;
|
||||
out_8981334264948615307[32] = 0;
|
||||
out_8981334264948615307[33] = 0;
|
||||
out_8981334264948615307[34] = 0;
|
||||
out_8981334264948615307[35] = 0;
|
||||
out_8981334264948615307[36] = 0;
|
||||
out_8981334264948615307[37] = 0;
|
||||
out_8981334264948615307[38] = 0;
|
||||
out_8981334264948615307[39] = 0;
|
||||
out_8981334264948615307[40] = 1;
|
||||
out_8981334264948615307[41] = 0;
|
||||
out_8981334264948615307[42] = 0;
|
||||
out_8981334264948615307[43] = 0;
|
||||
out_8981334264948615307[44] = 0;
|
||||
out_8981334264948615307[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_8981334264948615307[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_8981334264948615307[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_8981334264948615307[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_8981334264948615307[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_8981334264948615307[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_8981334264948615307[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_8981334264948615307[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_8981334264948615307[53] = -9.8000000000000007*dt;
|
||||
out_8981334264948615307[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_8981334264948615307[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_8981334264948615307[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_8981334264948615307[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_8981334264948615307[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_8981334264948615307[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_8981334264948615307[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_8981334264948615307[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_8981334264948615307[62] = 0;
|
||||
out_8981334264948615307[63] = 0;
|
||||
out_8981334264948615307[64] = 0;
|
||||
out_8981334264948615307[65] = 0;
|
||||
out_8981334264948615307[66] = 0;
|
||||
out_8981334264948615307[67] = 0;
|
||||
out_8981334264948615307[68] = 0;
|
||||
out_8981334264948615307[69] = 0;
|
||||
out_8981334264948615307[70] = 1;
|
||||
out_8981334264948615307[71] = 0;
|
||||
out_8981334264948615307[72] = 0;
|
||||
out_8981334264948615307[73] = 0;
|
||||
out_8981334264948615307[74] = 0;
|
||||
out_8981334264948615307[75] = 0;
|
||||
out_8981334264948615307[76] = 0;
|
||||
out_8981334264948615307[77] = 0;
|
||||
out_8981334264948615307[78] = 0;
|
||||
out_8981334264948615307[79] = 0;
|
||||
out_8981334264948615307[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_6677369844743091603) {
|
||||
out_6677369844743091603[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_2323891550179960131) {
|
||||
out_2323891550179960131[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_2625808263069423248) {
|
||||
out_2625808263069423248[0] = 0;
|
||||
out_2625808263069423248[1] = 0;
|
||||
out_2625808263069423248[2] = 0;
|
||||
out_2625808263069423248[3] = 0;
|
||||
out_2625808263069423248[4] = 0;
|
||||
out_2625808263069423248[5] = 0;
|
||||
out_2625808263069423248[6] = 1;
|
||||
out_2625808263069423248[7] = 0;
|
||||
out_2625808263069423248[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_3130628365570768536) {
|
||||
out_3130628365570768536[0] = 0;
|
||||
out_3130628365570768536[1] = 0;
|
||||
out_3130628365570768536[2] = 0;
|
||||
out_3130628365570768536[3] = 0;
|
||||
out_3130628365570768536[4] = 0;
|
||||
out_3130628365570768536[5] = 0;
|
||||
out_3130628365570768536[6] = 1;
|
||||
out_3130628365570768536[7] = 0;
|
||||
out_3130628365570768536[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_4367008819681309639) {
|
||||
out_4367008819681309639[0] = state[4];
|
||||
out_4367008819681309639[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_3959618100859615665) {
|
||||
out_3959618100859615665[0] = state[4];
|
||||
out_3959618100859615665[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_4798457862074922814) {
|
||||
out_4798457862074922814[0] = 0;
|
||||
out_4798457862074922814[1] = 0;
|
||||
out_4798457862074922814[2] = 0;
|
||||
out_4798457862074922814[3] = 0;
|
||||
out_4798457862074922814[4] = 1;
|
||||
out_4798457862074922814[5] = 0;
|
||||
out_4798457862074922814[6] = 0;
|
||||
out_4798457862074922814[7] = 0;
|
||||
out_4798457862074922814[8] = 0;
|
||||
out_4798457862074922814[9] = 0;
|
||||
out_4798457862074922814[10] = 0;
|
||||
out_4798457862074922814[11] = 0;
|
||||
out_4798457862074922814[12] = 0;
|
||||
out_4798457862074922814[13] = 0;
|
||||
out_4798457862074922814[14] = 1;
|
||||
out_4798457862074922814[15] = 0;
|
||||
out_4798457862074922814[16] = 0;
|
||||
out_4798457862074922814[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_3527273669924628031) {
|
||||
out_3527273669924628031[0] = 0;
|
||||
out_3527273669924628031[1] = 0;
|
||||
out_3527273669924628031[2] = 0;
|
||||
out_3527273669924628031[3] = 0;
|
||||
out_3527273669924628031[4] = 1;
|
||||
out_3527273669924628031[5] = 0;
|
||||
out_3527273669924628031[6] = 0;
|
||||
out_3527273669924628031[7] = 0;
|
||||
out_3527273669924628031[8] = 0;
|
||||
out_3527273669924628031[9] = 0;
|
||||
out_3527273669924628031[10] = 0;
|
||||
out_3527273669924628031[11] = 0;
|
||||
out_3527273669924628031[12] = 0;
|
||||
out_3527273669924628031[13] = 0;
|
||||
out_3527273669924628031[14] = 1;
|
||||
out_3527273669924628031[15] = 0;
|
||||
out_3527273669924628031[16] = 0;
|
||||
out_3527273669924628031[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_6520183176391962458) {
|
||||
out_6520183176391962458[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_6422194848459331995) {
|
||||
out_6422194848459331995[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_107475304562174621) {
|
||||
out_107475304562174621[0] = 0;
|
||||
out_107475304562174621[1] = 0;
|
||||
out_107475304562174621[2] = 0;
|
||||
out_107475304562174621[3] = 0;
|
||||
out_107475304562174621[4] = 1;
|
||||
out_107475304562174621[5] = 0;
|
||||
out_107475304562174621[6] = 0;
|
||||
out_107475304562174621[7] = 0;
|
||||
out_107475304562174621[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_612295407063519909) {
|
||||
out_612295407063519909[0] = 0;
|
||||
out_612295407063519909[1] = 0;
|
||||
out_612295407063519909[2] = 0;
|
||||
out_612295407063519909[3] = 0;
|
||||
out_612295407063519909[4] = 1;
|
||||
out_612295407063519909[5] = 0;
|
||||
out_612295407063519909[6] = 0;
|
||||
out_612295407063519909[7] = 0;
|
||||
out_612295407063519909[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_1518509908731116813) {
|
||||
out_1518509908731116813[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_4132346778849017835) {
|
||||
out_4132346778849017835[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_6367311581943479472) {
|
||||
out_6367311581943479472[0] = 0;
|
||||
out_6367311581943479472[1] = 0;
|
||||
out_6367311581943479472[2] = 0;
|
||||
out_6367311581943479472[3] = 0;
|
||||
out_6367311581943479472[4] = 0;
|
||||
out_6367311581943479472[5] = 0;
|
||||
out_6367311581943479472[6] = 0;
|
||||
out_6367311581943479472[7] = 1;
|
||||
out_6367311581943479472[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_6872131684444824760) {
|
||||
out_6872131684444824760[0] = 0;
|
||||
out_6872131684444824760[1] = 0;
|
||||
out_6872131684444824760[2] = 0;
|
||||
out_6872131684444824760[3] = 0;
|
||||
out_6872131684444824760[4] = 0;
|
||||
out_6872131684444824760[5] = 0;
|
||||
out_6872131684444824760[6] = 0;
|
||||
out_6872131684444824760[7] = 1;
|
||||
out_6872131684444824760[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_757773589685609210) {
|
||||
out_757773589685609210[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_6472417914086037009) {
|
||||
out_6472417914086037009[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_9118476168712095259) {
|
||||
out_9118476168712095259[0] = 0;
|
||||
out_9118476168712095259[1] = 0;
|
||||
out_9118476168712095259[2] = 0;
|
||||
out_9118476168712095259[3] = 1;
|
||||
out_9118476168712095259[4] = 0;
|
||||
out_9118476168712095259[5] = 0;
|
||||
out_9118476168712095259[6] = 0;
|
||||
out_9118476168712095259[7] = 0;
|
||||
out_9118476168712095259[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_1611298664120423308) {
|
||||
out_1611298664120423308[0] = 0;
|
||||
out_1611298664120423308[1] = 0;
|
||||
out_1611298664120423308[2] = 0;
|
||||
out_1611298664120423308[3] = 1;
|
||||
out_1611298664120423308[4] = 0;
|
||||
out_1611298664120423308[5] = 0;
|
||||
out_1611298664120423308[6] = 0;
|
||||
out_1611298664120423308[7] = 0;
|
||||
out_1611298664120423308[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_482579527401103321) {
|
||||
out_482579527401103321[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_8112642683195012955) {
|
||||
out_8112642683195012955[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_6643273248882639262) {
|
||||
out_6643273248882639262[0] = 0;
|
||||
out_6643273248882639262[1] = 1;
|
||||
out_6643273248882639262[2] = 0;
|
||||
out_6643273248882639262[3] = 0;
|
||||
out_6643273248882639262[4] = 0;
|
||||
out_6643273248882639262[5] = 0;
|
||||
out_6643273248882639262[6] = 0;
|
||||
out_6643273248882639262[7] = 0;
|
||||
out_6643273248882639262[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_102064062749127725) {
|
||||
out_102064062749127725[0] = 0;
|
||||
out_102064062749127725[1] = 1;
|
||||
out_102064062749127725[2] = 0;
|
||||
out_102064062749127725[3] = 0;
|
||||
out_102064062749127725[4] = 0;
|
||||
out_102064062749127725[5] = 0;
|
||||
out_102064062749127725[6] = 0;
|
||||
out_102064062749127725[7] = 0;
|
||||
out_102064062749127725[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_1701283442587271645) {
|
||||
out_1701283442587271645[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_738491572168184502) {
|
||||
out_738491572168184502[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_4679642977317313011) {
|
||||
out_4679642977317313011[0] = 1;
|
||||
out_4679642977317313011[1] = 0;
|
||||
out_4679642977317313011[2] = 0;
|
||||
out_4679642977317313011[3] = 0;
|
||||
out_4679642977317313011[4] = 0;
|
||||
out_4679642977317313011[5] = 0;
|
||||
out_4679642977317313011[6] = 0;
|
||||
out_4679642977317313011[7] = 0;
|
||||
out_4679642977317313011[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_5184463079818658299) {
|
||||
out_5184463079818658299[0] = 1;
|
||||
out_5184463079818658299[1] = 0;
|
||||
out_5184463079818658299[2] = 0;
|
||||
out_5184463079818658299[3] = 0;
|
||||
out_5184463079818658299[4] = 0;
|
||||
out_5184463079818658299[5] = 0;
|
||||
out_5184463079818658299[6] = 0;
|
||||
out_5184463079818658299[7] = 0;
|
||||
out_5184463079818658299[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_4998539002616109077) {
|
||||
out_4998539002616109077[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_8241579872775542300) {
|
||||
out_8241579872775542300[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_6993519684176830948) {
|
||||
out_6993519684176830948[0] = 0;
|
||||
out_6993519684176830948[1] = 0;
|
||||
out_6993519684176830948[2] = 0;
|
||||
out_6993519684176830948[3] = 0;
|
||||
out_6993519684176830948[4] = 0;
|
||||
out_6993519684176830948[5] = 0;
|
||||
out_6993519684176830948[6] = 0;
|
||||
out_6993519684176830948[7] = 0;
|
||||
out_6993519684176830948[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_7498339786678176236) {
|
||||
out_7498339786678176236[0] = 0;
|
||||
out_7498339786678176236[1] = 0;
|
||||
out_7498339786678176236[2] = 0;
|
||||
out_7498339786678176236[3] = 0;
|
||||
out_7498339786678176236[4] = 0;
|
||||
out_7498339786678176236[5] = 0;
|
||||
out_7498339786678176236[6] = 0;
|
||||
out_7498339786678176236[7] = 0;
|
||||
out_7498339786678176236[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_7226686656405340864) {
|
||||
err_fun(nom_x, delta_x, out_7226686656405340864);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_7418791751555442547) {
|
||||
err_fun(nom_x, delta_x, out_7418791751555442547);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7867434532787478357) {
|
||||
inv_err_fun(nom_x, true_x, out_7867434532787478357);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4675497126535007434) {
|
||||
inv_err_fun(nom_x, true_x, out_4675497126535007434);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_3288405267109084725) {
|
||||
H_mod_fun(state, out_3288405267109084725);
|
||||
void car_H_mod_fun(double *state, double *out_4383613899688875359) {
|
||||
H_mod_fun(state, out_4383613899688875359);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_8974995752329095985) {
|
||||
f_fun(state, dt, out_8974995752329095985);
|
||||
void car_f_fun(double *state, double dt, double *out_2672769777354396612) {
|
||||
f_fun(state, dt, out_2672769777354396612);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_2278957145959582876) {
|
||||
F_fun(state, dt, out_2278957145959582876);
|
||||
void car_F_fun(double *state, double dt, double *out_8981334264948615307) {
|
||||
F_fun(state, dt, out_8981334264948615307);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_6677369844743091603) {
|
||||
h_25(state, unused, out_6677369844743091603);
|
||||
void car_h_25(double *state, double *unused, double *out_2323891550179960131) {
|
||||
h_25(state, unused, out_2323891550179960131);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_2625808263069423248) {
|
||||
H_25(state, unused, out_2625808263069423248);
|
||||
void car_H_25(double *state, double *unused, double *out_3130628365570768536) {
|
||||
H_25(state, unused, out_3130628365570768536);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_4367008819681309639) {
|
||||
h_24(state, unused, out_4367008819681309639);
|
||||
void car_h_24(double *state, double *unused, double *out_3959618100859615665) {
|
||||
h_24(state, unused, out_3959618100859615665);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_4798457862074922814) {
|
||||
H_24(state, unused, out_4798457862074922814);
|
||||
void car_H_24(double *state, double *unused, double *out_3527273669924628031) {
|
||||
H_24(state, unused, out_3527273669924628031);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_6520183176391962458) {
|
||||
h_30(state, unused, out_6520183176391962458);
|
||||
void car_h_30(double *state, double *unused, double *out_6422194848459331995) {
|
||||
h_30(state, unused, out_6422194848459331995);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_107475304562174621) {
|
||||
H_30(state, unused, out_107475304562174621);
|
||||
void car_H_30(double *state, double *unused, double *out_612295407063519909) {
|
||||
H_30(state, unused, out_612295407063519909);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_1518509908731116813) {
|
||||
h_26(state, unused, out_1518509908731116813);
|
||||
void car_h_26(double *state, double *unused, double *out_4132346778849017835) {
|
||||
h_26(state, unused, out_4132346778849017835);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_6367311581943479472) {
|
||||
H_26(state, unused, out_6367311581943479472);
|
||||
void car_H_26(double *state, double *unused, double *out_6872131684444824760) {
|
||||
H_26(state, unused, out_6872131684444824760);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_757773589685609210) {
|
||||
h_27(state, unused, out_757773589685609210);
|
||||
void car_h_27(double *state, double *unused, double *out_6472417914086037009) {
|
||||
h_27(state, unused, out_6472417914086037009);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_9118476168712095259) {
|
||||
H_27(state, unused, out_9118476168712095259);
|
||||
void car_H_27(double *state, double *unused, double *out_1611298664120423308) {
|
||||
H_27(state, unused, out_1611298664120423308);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_482579527401103321) {
|
||||
h_29(state, unused, out_482579527401103321);
|
||||
void car_h_29(double *state, double *unused, double *out_8112642683195012955) {
|
||||
h_29(state, unused, out_8112642683195012955);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_6643273248882639262) {
|
||||
H_29(state, unused, out_6643273248882639262);
|
||||
void car_H_29(double *state, double *unused, double *out_102064062749127725) {
|
||||
H_29(state, unused, out_102064062749127725);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_1701283442587271645) {
|
||||
h_28(state, unused, out_1701283442587271645);
|
||||
void car_h_28(double *state, double *unused, double *out_738491572168184502) {
|
||||
h_28(state, unused, out_738491572168184502);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_4679642977317313011) {
|
||||
H_28(state, unused, out_4679642977317313011);
|
||||
void car_H_28(double *state, double *unused, double *out_5184463079818658299) {
|
||||
H_28(state, unused, out_5184463079818658299);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_4998539002616109077) {
|
||||
h_31(state, unused, out_4998539002616109077);
|
||||
void car_h_31(double *state, double *unused, double *out_8241579872775542300) {
|
||||
h_31(state, unused, out_8241579872775542300);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_6993519684176830948) {
|
||||
H_31(state, unused, out_6993519684176830948);
|
||||
void car_H_31(double *state, double *unused, double *out_7498339786678176236) {
|
||||
H_31(state, unused, out_7498339786678176236);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
Reference in New Issue
Block a user