mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-01 03:43:46 +08:00
build and kia forte
This commit is contained in:
firestar5683
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
|
||||
* *
|
||||
* This file is part of 'ekf' *
|
||||
******************************************************************************/
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_587611173581676283) {
|
||||
out_587611173581676283[0] = delta_x[0] + nom_x[0];
|
||||
out_587611173581676283[1] = delta_x[1] + nom_x[1];
|
||||
out_587611173581676283[2] = delta_x[2] + nom_x[2];
|
||||
out_587611173581676283[3] = delta_x[3] + nom_x[3];
|
||||
out_587611173581676283[4] = delta_x[4] + nom_x[4];
|
||||
out_587611173581676283[5] = delta_x[5] + nom_x[5];
|
||||
out_587611173581676283[6] = delta_x[6] + nom_x[6];
|
||||
out_587611173581676283[7] = delta_x[7] + nom_x[7];
|
||||
out_587611173581676283[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_652600290238585961) {
|
||||
out_652600290238585961[0] = delta_x[0] + nom_x[0];
|
||||
out_652600290238585961[1] = delta_x[1] + nom_x[1];
|
||||
out_652600290238585961[2] = delta_x[2] + nom_x[2];
|
||||
out_652600290238585961[3] = delta_x[3] + nom_x[3];
|
||||
out_652600290238585961[4] = delta_x[4] + nom_x[4];
|
||||
out_652600290238585961[5] = delta_x[5] + nom_x[5];
|
||||
out_652600290238585961[6] = delta_x[6] + nom_x[6];
|
||||
out_652600290238585961[7] = delta_x[7] + nom_x[7];
|
||||
out_652600290238585961[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8120728011522269919) {
|
||||
out_8120728011522269919[0] = -nom_x[0] + true_x[0];
|
||||
out_8120728011522269919[1] = -nom_x[1] + true_x[1];
|
||||
out_8120728011522269919[2] = -nom_x[2] + true_x[2];
|
||||
out_8120728011522269919[3] = -nom_x[3] + true_x[3];
|
||||
out_8120728011522269919[4] = -nom_x[4] + true_x[4];
|
||||
out_8120728011522269919[5] = -nom_x[5] + true_x[5];
|
||||
out_8120728011522269919[6] = -nom_x[6] + true_x[6];
|
||||
out_8120728011522269919[7] = -nom_x[7] + true_x[7];
|
||||
out_8120728011522269919[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_428056314371954903) {
|
||||
out_428056314371954903[0] = -nom_x[0] + true_x[0];
|
||||
out_428056314371954903[1] = -nom_x[1] + true_x[1];
|
||||
out_428056314371954903[2] = -nom_x[2] + true_x[2];
|
||||
out_428056314371954903[3] = -nom_x[3] + true_x[3];
|
||||
out_428056314371954903[4] = -nom_x[4] + true_x[4];
|
||||
out_428056314371954903[5] = -nom_x[5] + true_x[5];
|
||||
out_428056314371954903[6] = -nom_x[6] + true_x[6];
|
||||
out_428056314371954903[7] = -nom_x[7] + true_x[7];
|
||||
out_428056314371954903[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_4001675034436284807) {
|
||||
out_4001675034436284807[0] = 1.0;
|
||||
out_4001675034436284807[1] = 0.0;
|
||||
out_4001675034436284807[2] = 0.0;
|
||||
out_4001675034436284807[3] = 0.0;
|
||||
out_4001675034436284807[4] = 0.0;
|
||||
out_4001675034436284807[5] = 0.0;
|
||||
out_4001675034436284807[6] = 0.0;
|
||||
out_4001675034436284807[7] = 0.0;
|
||||
out_4001675034436284807[8] = 0.0;
|
||||
out_4001675034436284807[9] = 0.0;
|
||||
out_4001675034436284807[10] = 1.0;
|
||||
out_4001675034436284807[11] = 0.0;
|
||||
out_4001675034436284807[12] = 0.0;
|
||||
out_4001675034436284807[13] = 0.0;
|
||||
out_4001675034436284807[14] = 0.0;
|
||||
out_4001675034436284807[15] = 0.0;
|
||||
out_4001675034436284807[16] = 0.0;
|
||||
out_4001675034436284807[17] = 0.0;
|
||||
out_4001675034436284807[18] = 0.0;
|
||||
out_4001675034436284807[19] = 0.0;
|
||||
out_4001675034436284807[20] = 1.0;
|
||||
out_4001675034436284807[21] = 0.0;
|
||||
out_4001675034436284807[22] = 0.0;
|
||||
out_4001675034436284807[23] = 0.0;
|
||||
out_4001675034436284807[24] = 0.0;
|
||||
out_4001675034436284807[25] = 0.0;
|
||||
out_4001675034436284807[26] = 0.0;
|
||||
out_4001675034436284807[27] = 0.0;
|
||||
out_4001675034436284807[28] = 0.0;
|
||||
out_4001675034436284807[29] = 0.0;
|
||||
out_4001675034436284807[30] = 1.0;
|
||||
out_4001675034436284807[31] = 0.0;
|
||||
out_4001675034436284807[32] = 0.0;
|
||||
out_4001675034436284807[33] = 0.0;
|
||||
out_4001675034436284807[34] = 0.0;
|
||||
out_4001675034436284807[35] = 0.0;
|
||||
out_4001675034436284807[36] = 0.0;
|
||||
out_4001675034436284807[37] = 0.0;
|
||||
out_4001675034436284807[38] = 0.0;
|
||||
out_4001675034436284807[39] = 0.0;
|
||||
out_4001675034436284807[40] = 1.0;
|
||||
out_4001675034436284807[41] = 0.0;
|
||||
out_4001675034436284807[42] = 0.0;
|
||||
out_4001675034436284807[43] = 0.0;
|
||||
out_4001675034436284807[44] = 0.0;
|
||||
out_4001675034436284807[45] = 0.0;
|
||||
out_4001675034436284807[46] = 0.0;
|
||||
out_4001675034436284807[47] = 0.0;
|
||||
out_4001675034436284807[48] = 0.0;
|
||||
out_4001675034436284807[49] = 0.0;
|
||||
out_4001675034436284807[50] = 1.0;
|
||||
out_4001675034436284807[51] = 0.0;
|
||||
out_4001675034436284807[52] = 0.0;
|
||||
out_4001675034436284807[53] = 0.0;
|
||||
out_4001675034436284807[54] = 0.0;
|
||||
out_4001675034436284807[55] = 0.0;
|
||||
out_4001675034436284807[56] = 0.0;
|
||||
out_4001675034436284807[57] = 0.0;
|
||||
out_4001675034436284807[58] = 0.0;
|
||||
out_4001675034436284807[59] = 0.0;
|
||||
out_4001675034436284807[60] = 1.0;
|
||||
out_4001675034436284807[61] = 0.0;
|
||||
out_4001675034436284807[62] = 0.0;
|
||||
out_4001675034436284807[63] = 0.0;
|
||||
out_4001675034436284807[64] = 0.0;
|
||||
out_4001675034436284807[65] = 0.0;
|
||||
out_4001675034436284807[66] = 0.0;
|
||||
out_4001675034436284807[67] = 0.0;
|
||||
out_4001675034436284807[68] = 0.0;
|
||||
out_4001675034436284807[69] = 0.0;
|
||||
out_4001675034436284807[70] = 1.0;
|
||||
out_4001675034436284807[71] = 0.0;
|
||||
out_4001675034436284807[72] = 0.0;
|
||||
out_4001675034436284807[73] = 0.0;
|
||||
out_4001675034436284807[74] = 0.0;
|
||||
out_4001675034436284807[75] = 0.0;
|
||||
out_4001675034436284807[76] = 0.0;
|
||||
out_4001675034436284807[77] = 0.0;
|
||||
out_4001675034436284807[78] = 0.0;
|
||||
out_4001675034436284807[79] = 0.0;
|
||||
out_4001675034436284807[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_636098599351187827) {
|
||||
out_636098599351187827[0] = 1.0;
|
||||
out_636098599351187827[1] = 0.0;
|
||||
out_636098599351187827[2] = 0.0;
|
||||
out_636098599351187827[3] = 0.0;
|
||||
out_636098599351187827[4] = 0.0;
|
||||
out_636098599351187827[5] = 0.0;
|
||||
out_636098599351187827[6] = 0.0;
|
||||
out_636098599351187827[7] = 0.0;
|
||||
out_636098599351187827[8] = 0.0;
|
||||
out_636098599351187827[9] = 0.0;
|
||||
out_636098599351187827[10] = 1.0;
|
||||
out_636098599351187827[11] = 0.0;
|
||||
out_636098599351187827[12] = 0.0;
|
||||
out_636098599351187827[13] = 0.0;
|
||||
out_636098599351187827[14] = 0.0;
|
||||
out_636098599351187827[15] = 0.0;
|
||||
out_636098599351187827[16] = 0.0;
|
||||
out_636098599351187827[17] = 0.0;
|
||||
out_636098599351187827[18] = 0.0;
|
||||
out_636098599351187827[19] = 0.0;
|
||||
out_636098599351187827[20] = 1.0;
|
||||
out_636098599351187827[21] = 0.0;
|
||||
out_636098599351187827[22] = 0.0;
|
||||
out_636098599351187827[23] = 0.0;
|
||||
out_636098599351187827[24] = 0.0;
|
||||
out_636098599351187827[25] = 0.0;
|
||||
out_636098599351187827[26] = 0.0;
|
||||
out_636098599351187827[27] = 0.0;
|
||||
out_636098599351187827[28] = 0.0;
|
||||
out_636098599351187827[29] = 0.0;
|
||||
out_636098599351187827[30] = 1.0;
|
||||
out_636098599351187827[31] = 0.0;
|
||||
out_636098599351187827[32] = 0.0;
|
||||
out_636098599351187827[33] = 0.0;
|
||||
out_636098599351187827[34] = 0.0;
|
||||
out_636098599351187827[35] = 0.0;
|
||||
out_636098599351187827[36] = 0.0;
|
||||
out_636098599351187827[37] = 0.0;
|
||||
out_636098599351187827[38] = 0.0;
|
||||
out_636098599351187827[39] = 0.0;
|
||||
out_636098599351187827[40] = 1.0;
|
||||
out_636098599351187827[41] = 0.0;
|
||||
out_636098599351187827[42] = 0.0;
|
||||
out_636098599351187827[43] = 0.0;
|
||||
out_636098599351187827[44] = 0.0;
|
||||
out_636098599351187827[45] = 0.0;
|
||||
out_636098599351187827[46] = 0.0;
|
||||
out_636098599351187827[47] = 0.0;
|
||||
out_636098599351187827[48] = 0.0;
|
||||
out_636098599351187827[49] = 0.0;
|
||||
out_636098599351187827[50] = 1.0;
|
||||
out_636098599351187827[51] = 0.0;
|
||||
out_636098599351187827[52] = 0.0;
|
||||
out_636098599351187827[53] = 0.0;
|
||||
out_636098599351187827[54] = 0.0;
|
||||
out_636098599351187827[55] = 0.0;
|
||||
out_636098599351187827[56] = 0.0;
|
||||
out_636098599351187827[57] = 0.0;
|
||||
out_636098599351187827[58] = 0.0;
|
||||
out_636098599351187827[59] = 0.0;
|
||||
out_636098599351187827[60] = 1.0;
|
||||
out_636098599351187827[61] = 0.0;
|
||||
out_636098599351187827[62] = 0.0;
|
||||
out_636098599351187827[63] = 0.0;
|
||||
out_636098599351187827[64] = 0.0;
|
||||
out_636098599351187827[65] = 0.0;
|
||||
out_636098599351187827[66] = 0.0;
|
||||
out_636098599351187827[67] = 0.0;
|
||||
out_636098599351187827[68] = 0.0;
|
||||
out_636098599351187827[69] = 0.0;
|
||||
out_636098599351187827[70] = 1.0;
|
||||
out_636098599351187827[71] = 0.0;
|
||||
out_636098599351187827[72] = 0.0;
|
||||
out_636098599351187827[73] = 0.0;
|
||||
out_636098599351187827[74] = 0.0;
|
||||
out_636098599351187827[75] = 0.0;
|
||||
out_636098599351187827[76] = 0.0;
|
||||
out_636098599351187827[77] = 0.0;
|
||||
out_636098599351187827[78] = 0.0;
|
||||
out_636098599351187827[79] = 0.0;
|
||||
out_636098599351187827[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_6995360374056238489) {
|
||||
out_6995360374056238489[0] = state[0];
|
||||
out_6995360374056238489[1] = state[1];
|
||||
out_6995360374056238489[2] = state[2];
|
||||
out_6995360374056238489[3] = state[3];
|
||||
out_6995360374056238489[4] = state[4];
|
||||
out_6995360374056238489[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_6995360374056238489[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_6995360374056238489[7] = state[7];
|
||||
out_6995360374056238489[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_190909160143243867) {
|
||||
out_190909160143243867[0] = state[0];
|
||||
out_190909160143243867[1] = state[1];
|
||||
out_190909160143243867[2] = state[2];
|
||||
out_190909160143243867[3] = state[3];
|
||||
out_190909160143243867[4] = state[4];
|
||||
out_190909160143243867[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_190909160143243867[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_190909160143243867[7] = state[7];
|
||||
out_190909160143243867[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_5620828310005666299) {
|
||||
out_5620828310005666299[0] = 1;
|
||||
out_5620828310005666299[1] = 0;
|
||||
out_5620828310005666299[2] = 0;
|
||||
out_5620828310005666299[3] = 0;
|
||||
out_5620828310005666299[4] = 0;
|
||||
out_5620828310005666299[5] = 0;
|
||||
out_5620828310005666299[6] = 0;
|
||||
out_5620828310005666299[7] = 0;
|
||||
out_5620828310005666299[8] = 0;
|
||||
out_5620828310005666299[9] = 0;
|
||||
out_5620828310005666299[10] = 1;
|
||||
out_5620828310005666299[11] = 0;
|
||||
out_5620828310005666299[12] = 0;
|
||||
out_5620828310005666299[13] = 0;
|
||||
out_5620828310005666299[14] = 0;
|
||||
out_5620828310005666299[15] = 0;
|
||||
out_5620828310005666299[16] = 0;
|
||||
out_5620828310005666299[17] = 0;
|
||||
out_5620828310005666299[18] = 0;
|
||||
out_5620828310005666299[19] = 0;
|
||||
out_5620828310005666299[20] = 1;
|
||||
out_5620828310005666299[21] = 0;
|
||||
out_5620828310005666299[22] = 0;
|
||||
out_5620828310005666299[23] = 0;
|
||||
out_5620828310005666299[24] = 0;
|
||||
out_5620828310005666299[25] = 0;
|
||||
out_5620828310005666299[26] = 0;
|
||||
out_5620828310005666299[27] = 0;
|
||||
out_5620828310005666299[28] = 0;
|
||||
out_5620828310005666299[29] = 0;
|
||||
out_5620828310005666299[30] = 1;
|
||||
out_5620828310005666299[31] = 0;
|
||||
out_5620828310005666299[32] = 0;
|
||||
out_5620828310005666299[33] = 0;
|
||||
out_5620828310005666299[34] = 0;
|
||||
out_5620828310005666299[35] = 0;
|
||||
out_5620828310005666299[36] = 0;
|
||||
out_5620828310005666299[37] = 0;
|
||||
out_5620828310005666299[38] = 0;
|
||||
out_5620828310005666299[39] = 0;
|
||||
out_5620828310005666299[40] = 1;
|
||||
out_5620828310005666299[41] = 0;
|
||||
out_5620828310005666299[42] = 0;
|
||||
out_5620828310005666299[43] = 0;
|
||||
out_5620828310005666299[44] = 0;
|
||||
out_5620828310005666299[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_5620828310005666299[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_5620828310005666299[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5620828310005666299[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5620828310005666299[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_5620828310005666299[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_5620828310005666299[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_5620828310005666299[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5620828310005666299[53] = -9.8100000000000005*dt;
|
||||
out_5620828310005666299[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_5620828310005666299[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_5620828310005666299[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5620828310005666299[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5620828310005666299[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_5620828310005666299[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_5620828310005666299[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_5620828310005666299[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5620828310005666299[62] = 0;
|
||||
out_5620828310005666299[63] = 0;
|
||||
out_5620828310005666299[64] = 0;
|
||||
out_5620828310005666299[65] = 0;
|
||||
out_5620828310005666299[66] = 0;
|
||||
out_5620828310005666299[67] = 0;
|
||||
out_5620828310005666299[68] = 0;
|
||||
out_5620828310005666299[69] = 0;
|
||||
out_5620828310005666299[70] = 1;
|
||||
out_5620828310005666299[71] = 0;
|
||||
out_5620828310005666299[72] = 0;
|
||||
out_5620828310005666299[73] = 0;
|
||||
out_5620828310005666299[74] = 0;
|
||||
out_5620828310005666299[75] = 0;
|
||||
out_5620828310005666299[76] = 0;
|
||||
out_5620828310005666299[77] = 0;
|
||||
out_5620828310005666299[78] = 0;
|
||||
out_5620828310005666299[79] = 0;
|
||||
out_5620828310005666299[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_1195327144916471551) {
|
||||
out_1195327144916471551[0] = 1;
|
||||
out_1195327144916471551[1] = 0;
|
||||
out_1195327144916471551[2] = 0;
|
||||
out_1195327144916471551[3] = 0;
|
||||
out_1195327144916471551[4] = 0;
|
||||
out_1195327144916471551[5] = 0;
|
||||
out_1195327144916471551[6] = 0;
|
||||
out_1195327144916471551[7] = 0;
|
||||
out_1195327144916471551[8] = 0;
|
||||
out_1195327144916471551[9] = 0;
|
||||
out_1195327144916471551[10] = 1;
|
||||
out_1195327144916471551[11] = 0;
|
||||
out_1195327144916471551[12] = 0;
|
||||
out_1195327144916471551[13] = 0;
|
||||
out_1195327144916471551[14] = 0;
|
||||
out_1195327144916471551[15] = 0;
|
||||
out_1195327144916471551[16] = 0;
|
||||
out_1195327144916471551[17] = 0;
|
||||
out_1195327144916471551[18] = 0;
|
||||
out_1195327144916471551[19] = 0;
|
||||
out_1195327144916471551[20] = 1;
|
||||
out_1195327144916471551[21] = 0;
|
||||
out_1195327144916471551[22] = 0;
|
||||
out_1195327144916471551[23] = 0;
|
||||
out_1195327144916471551[24] = 0;
|
||||
out_1195327144916471551[25] = 0;
|
||||
out_1195327144916471551[26] = 0;
|
||||
out_1195327144916471551[27] = 0;
|
||||
out_1195327144916471551[28] = 0;
|
||||
out_1195327144916471551[29] = 0;
|
||||
out_1195327144916471551[30] = 1;
|
||||
out_1195327144916471551[31] = 0;
|
||||
out_1195327144916471551[32] = 0;
|
||||
out_1195327144916471551[33] = 0;
|
||||
out_1195327144916471551[34] = 0;
|
||||
out_1195327144916471551[35] = 0;
|
||||
out_1195327144916471551[36] = 0;
|
||||
out_1195327144916471551[37] = 0;
|
||||
out_1195327144916471551[38] = 0;
|
||||
out_1195327144916471551[39] = 0;
|
||||
out_1195327144916471551[40] = 1;
|
||||
out_1195327144916471551[41] = 0;
|
||||
out_1195327144916471551[42] = 0;
|
||||
out_1195327144916471551[43] = 0;
|
||||
out_1195327144916471551[44] = 0;
|
||||
out_1195327144916471551[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_1195327144916471551[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_1195327144916471551[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1195327144916471551[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1195327144916471551[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_1195327144916471551[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_1195327144916471551[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_1195327144916471551[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1195327144916471551[53] = -9.8100000000000005*dt;
|
||||
out_1195327144916471551[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_1195327144916471551[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_1195327144916471551[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1195327144916471551[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1195327144916471551[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_1195327144916471551[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_1195327144916471551[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_1195327144916471551[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1195327144916471551[62] = 0;
|
||||
out_1195327144916471551[63] = 0;
|
||||
out_1195327144916471551[64] = 0;
|
||||
out_1195327144916471551[65] = 0;
|
||||
out_1195327144916471551[66] = 0;
|
||||
out_1195327144916471551[67] = 0;
|
||||
out_1195327144916471551[68] = 0;
|
||||
out_1195327144916471551[69] = 0;
|
||||
out_1195327144916471551[70] = 1;
|
||||
out_1195327144916471551[71] = 0;
|
||||
out_1195327144916471551[72] = 0;
|
||||
out_1195327144916471551[73] = 0;
|
||||
out_1195327144916471551[74] = 0;
|
||||
out_1195327144916471551[75] = 0;
|
||||
out_1195327144916471551[76] = 0;
|
||||
out_1195327144916471551[77] = 0;
|
||||
out_1195327144916471551[78] = 0;
|
||||
out_1195327144916471551[79] = 0;
|
||||
out_1195327144916471551[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_5932710635121359621) {
|
||||
out_5932710635121359621[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_8972033615374819768) {
|
||||
out_8972033615374819768[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_4507009476636581519) {
|
||||
out_4507009476636581519[0] = 0;
|
||||
out_4507009476636581519[1] = 0;
|
||||
out_4507009476636581519[2] = 0;
|
||||
out_4507009476636581519[3] = 0;
|
||||
out_4507009476636581519[4] = 0;
|
||||
out_4507009476636581519[5] = 0;
|
||||
out_4507009476636581519[6] = 1;
|
||||
out_4507009476636581519[7] = 0;
|
||||
out_4507009476636581519[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_4267593225833477013) {
|
||||
out_4267593225833477013[0] = 0;
|
||||
out_4267593225833477013[1] = 0;
|
||||
out_4267593225833477013[2] = 0;
|
||||
out_4267593225833477013[3] = 0;
|
||||
out_4267593225833477013[4] = 0;
|
||||
out_4267593225833477013[5] = 0;
|
||||
out_4267593225833477013[6] = 1;
|
||||
out_4267593225833477013[7] = 0;
|
||||
out_4267593225833477013[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_8960081603047522700) {
|
||||
out_8960081603047522700[0] = state[4];
|
||||
out_8960081603047522700[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_1619093620334214225) {
|
||||
out_1619093620334214225[0] = state[4];
|
||||
out_1619093620334214225[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_7364162790481451996) {
|
||||
out_7364162790481451996[0] = 0;
|
||||
out_7364162790481451996[1] = 0;
|
||||
out_7364162790481451996[2] = 0;
|
||||
out_7364162790481451996[3] = 0;
|
||||
out_7364162790481451996[4] = 1;
|
||||
out_7364162790481451996[5] = 0;
|
||||
out_7364162790481451996[6] = 0;
|
||||
out_7364162790481451996[7] = 0;
|
||||
out_7364162790481451996[8] = 0;
|
||||
out_7364162790481451996[9] = 0;
|
||||
out_7364162790481451996[10] = 0;
|
||||
out_7364162790481451996[11] = 0;
|
||||
out_7364162790481451996[12] = 0;
|
||||
out_7364162790481451996[13] = 0;
|
||||
out_7364162790481451996[14] = 1;
|
||||
out_7364162790481451996[15] = 0;
|
||||
out_7364162790481451996[16] = 0;
|
||||
out_7364162790481451996[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_2538730106656162944) {
|
||||
out_2538730106656162944[0] = 0;
|
||||
out_2538730106656162944[1] = 0;
|
||||
out_2538730106656162944[2] = 0;
|
||||
out_2538730106656162944[3] = 0;
|
||||
out_2538730106656162944[4] = 1;
|
||||
out_2538730106656162944[5] = 0;
|
||||
out_2538730106656162944[6] = 0;
|
||||
out_2538730106656162944[7] = 0;
|
||||
out_2538730106656162944[8] = 0;
|
||||
out_2538730106656162944[9] = 0;
|
||||
out_2538730106656162944[10] = 0;
|
||||
out_2538730106656162944[11] = 0;
|
||||
out_2538730106656162944[12] = 0;
|
||||
out_2538730106656162944[13] = 0;
|
||||
out_2538730106656162944[14] = 1;
|
||||
out_2538730106656162944[15] = 0;
|
||||
out_2538730106656162944[16] = 0;
|
||||
out_2538730106656162944[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_6293656255741178026) {
|
||||
out_6293656255741178026[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_2004718450693787274) {
|
||||
out_2004718450693787274[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_7023044255581353342) {
|
||||
out_7023044255581353342[0] = 0;
|
||||
out_7023044255581353342[1] = 0;
|
||||
out_7023044255581353342[2] = 0;
|
||||
out_7023044255581353342[3] = 0;
|
||||
out_7023044255581353342[4] = 1;
|
||||
out_7023044255581353342[5] = 0;
|
||||
out_7023044255581353342[6] = 0;
|
||||
out_7023044255581353342[7] = 0;
|
||||
out_7023044255581353342[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_6785926184340725640) {
|
||||
out_6785926184340725640[0] = 0;
|
||||
out_6785926184340725640[1] = 0;
|
||||
out_6785926184340725640[2] = 0;
|
||||
out_6785926184340725640[3] = 0;
|
||||
out_6785926184340725640[4] = 1;
|
||||
out_6785926184340725640[5] = 0;
|
||||
out_6785926184340725640[6] = 0;
|
||||
out_6785926184340725640[7] = 0;
|
||||
out_6785926184340725640[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_7151414550307527945) {
|
||||
out_7151414550307527945[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_7006391718354632919) {
|
||||
out_7006391718354632919[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_765506157762525295) {
|
||||
out_765506157762525295[0] = 0;
|
||||
out_765506157762525295[1] = 0;
|
||||
out_765506157762525295[2] = 0;
|
||||
out_765506157762525295[3] = 0;
|
||||
out_765506157762525295[4] = 0;
|
||||
out_765506157762525295[5] = 0;
|
||||
out_765506157762525295[6] = 0;
|
||||
out_765506157762525295[7] = 1;
|
||||
out_765506157762525295[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_7572119195594277614) {
|
||||
out_7572119195594277614[0] = 0;
|
||||
out_7572119195594277614[1] = 0;
|
||||
out_7572119195594277614[2] = 0;
|
||||
out_7572119195594277614[3] = 0;
|
||||
out_7572119195594277614[4] = 0;
|
||||
out_7572119195594277614[5] = 0;
|
||||
out_7572119195594277614[6] = 0;
|
||||
out_7572119195594277614[7] = 1;
|
||||
out_7572119195594277614[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_7512360170927346350) {
|
||||
out_7512360170927346350[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_4512330460653438616) {
|
||||
out_4512330460653438616[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_9197807567381778253) {
|
||||
out_9197807567381778253[0] = 0;
|
||||
out_9197807567381778253[1] = 0;
|
||||
out_9197807567381778253[2] = 0;
|
||||
out_9197807567381778253[3] = 1;
|
||||
out_9197807567381778253[4] = 0;
|
||||
out_9197807567381778253[5] = 0;
|
||||
out_9197807567381778253[6] = 0;
|
||||
out_9197807567381778253[7] = 0;
|
||||
out_9197807567381778253[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_9009520255524668857) {
|
||||
out_9009520255524668857[0] = 0;
|
||||
out_9009520255524668857[1] = 0;
|
||||
out_9009520255524668857[2] = 0;
|
||||
out_9009520255524668857[3] = 1;
|
||||
out_9009520255524668857[4] = 0;
|
||||
out_9009520255524668857[5] = 0;
|
||||
out_9009520255524668857[6] = 0;
|
||||
out_9009520255524668857[7] = 0;
|
||||
out_9009520255524668857[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_3171118645961532825) {
|
||||
out_3171118645961532825[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_2948228303595449709) {
|
||||
out_2948228303595449709[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_6512812911266961158) {
|
||||
out_6512812911266961158[0] = 0;
|
||||
out_6512812911266961158[1] = 1;
|
||||
out_6512812911266961158[2] = 0;
|
||||
out_6512812911266961158[3] = 0;
|
||||
out_6512812911266961158[4] = 0;
|
||||
out_6512812911266961158[5] = 0;
|
||||
out_6512812911266961158[6] = 0;
|
||||
out_6512812911266961158[7] = 0;
|
||||
out_6512812911266961158[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_7296157528655117824) {
|
||||
out_7296157528655117824[0] = 0;
|
||||
out_7296157528655117824[1] = 1;
|
||||
out_7296157528655117824[2] = 0;
|
||||
out_7296157528655117824[3] = 0;
|
||||
out_7296157528655117824[4] = 0;
|
||||
out_7296157528655117824[5] = 0;
|
||||
out_7296157528655117824[6] = 0;
|
||||
out_7296157528655117824[7] = 0;
|
||||
out_7296157528655117824[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_8329978581973507615) {
|
||||
out_8329978581973507615[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_1907925139844740965) {
|
||||
out_1907925139844740965[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_2453174762388691756) {
|
||||
out_2453174762388691756[0] = 1;
|
||||
out_2453174762388691756[1] = 0;
|
||||
out_2453174762388691756[2] = 0;
|
||||
out_2453174762388691756[3] = 0;
|
||||
out_2453174762388691756[4] = 0;
|
||||
out_2453174762388691756[5] = 0;
|
||||
out_2453174762388691756[6] = 0;
|
||||
out_2453174762388691756[7] = 0;
|
||||
out_2453174762388691756[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_2213758511585587250) {
|
||||
out_2213758511585587250[0] = 1;
|
||||
out_2213758511585587250[1] = 0;
|
||||
out_2213758511585587250[2] = 0;
|
||||
out_2213758511585587250[3] = 0;
|
||||
out_2213758511585587250[4] = 0;
|
||||
out_2213758511585587250[5] = 0;
|
||||
out_2213758511585587250[6] = 0;
|
||||
out_2213758511585587250[7] = 0;
|
||||
out_2213758511585587250[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_222736560719300508) {
|
||||
out_222736560719300508[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_332156138656277884) {
|
||||
out_332156138656277884[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_4537655438513541947) {
|
||||
out_4537655438513541947[0] = 0;
|
||||
out_4537655438513541947[1] = 0;
|
||||
out_4537655438513541947[2] = 0;
|
||||
out_4537655438513541947[3] = 0;
|
||||
out_4537655438513541947[4] = 0;
|
||||
out_4537655438513541947[5] = 0;
|
||||
out_4537655438513541947[6] = 0;
|
||||
out_4537655438513541947[7] = 0;
|
||||
out_4537655438513541947[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_4298239187710437441) {
|
||||
out_4298239187710437441[0] = 0;
|
||||
out_4298239187710437441[1] = 0;
|
||||
out_4298239187710437441[2] = 0;
|
||||
out_4298239187710437441[3] = 0;
|
||||
out_4298239187710437441[4] = 0;
|
||||
out_4298239187710437441[5] = 0;
|
||||
out_4298239187710437441[6] = 0;
|
||||
out_4298239187710437441[7] = 0;
|
||||
out_4298239187710437441[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_587611173581676283) {
|
||||
err_fun(nom_x, delta_x, out_587611173581676283);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_652600290238585961) {
|
||||
err_fun(nom_x, delta_x, out_652600290238585961);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8120728011522269919) {
|
||||
inv_err_fun(nom_x, true_x, out_8120728011522269919);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_428056314371954903) {
|
||||
inv_err_fun(nom_x, true_x, out_428056314371954903);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_4001675034436284807) {
|
||||
H_mod_fun(state, out_4001675034436284807);
|
||||
void car_H_mod_fun(double *state, double *out_636098599351187827) {
|
||||
H_mod_fun(state, out_636098599351187827);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_6995360374056238489) {
|
||||
f_fun(state, dt, out_6995360374056238489);
|
||||
void car_f_fun(double *state, double dt, double *out_190909160143243867) {
|
||||
f_fun(state, dt, out_190909160143243867);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_5620828310005666299) {
|
||||
F_fun(state, dt, out_5620828310005666299);
|
||||
void car_F_fun(double *state, double dt, double *out_1195327144916471551) {
|
||||
F_fun(state, dt, out_1195327144916471551);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_5932710635121359621) {
|
||||
h_25(state, unused, out_5932710635121359621);
|
||||
void car_h_25(double *state, double *unused, double *out_8972033615374819768) {
|
||||
h_25(state, unused, out_8972033615374819768);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_4507009476636581519) {
|
||||
H_25(state, unused, out_4507009476636581519);
|
||||
void car_H_25(double *state, double *unused, double *out_4267593225833477013) {
|
||||
H_25(state, unused, out_4267593225833477013);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_8960081603047522700) {
|
||||
h_24(state, unused, out_8960081603047522700);
|
||||
void car_h_24(double *state, double *unused, double *out_1619093620334214225) {
|
||||
h_24(state, unused, out_1619093620334214225);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_7364162790481451996) {
|
||||
H_24(state, unused, out_7364162790481451996);
|
||||
void car_H_24(double *state, double *unused, double *out_2538730106656162944) {
|
||||
H_24(state, unused, out_2538730106656162944);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_6293656255741178026) {
|
||||
h_30(state, unused, out_6293656255741178026);
|
||||
void car_h_30(double *state, double *unused, double *out_2004718450693787274) {
|
||||
h_30(state, unused, out_2004718450693787274);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_7023044255581353342) {
|
||||
H_30(state, unused, out_7023044255581353342);
|
||||
void car_H_30(double *state, double *unused, double *out_6785926184340725640) {
|
||||
H_30(state, unused, out_6785926184340725640);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_7151414550307527945) {
|
||||
h_26(state, unused, out_7151414550307527945);
|
||||
void car_h_26(double *state, double *unused, double *out_7006391718354632919) {
|
||||
h_26(state, unused, out_7006391718354632919);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_765506157762525295) {
|
||||
H_26(state, unused, out_765506157762525295);
|
||||
void car_H_26(double *state, double *unused, double *out_7572119195594277614) {
|
||||
H_26(state, unused, out_7572119195594277614);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_7512360170927346350) {
|
||||
h_27(state, unused, out_7512360170927346350);
|
||||
void car_h_27(double *state, double *unused, double *out_4512330460653438616) {
|
||||
h_27(state, unused, out_4512330460653438616);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_9197807567381778253) {
|
||||
H_27(state, unused, out_9197807567381778253);
|
||||
void car_H_27(double *state, double *unused, double *out_9009520255524668857) {
|
||||
H_27(state, unused, out_9009520255524668857);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_3171118645961532825) {
|
||||
h_29(state, unused, out_3171118645961532825);
|
||||
void car_h_29(double *state, double *unused, double *out_2948228303595449709) {
|
||||
h_29(state, unused, out_2948228303595449709);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_6512812911266961158) {
|
||||
H_29(state, unused, out_6512812911266961158);
|
||||
void car_H_29(double *state, double *unused, double *out_7296157528655117824) {
|
||||
H_29(state, unused, out_7296157528655117824);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_8329978581973507615) {
|
||||
h_28(state, unused, out_8329978581973507615);
|
||||
void car_h_28(double *state, double *unused, double *out_1907925139844740965) {
|
||||
h_28(state, unused, out_1907925139844740965);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_2453174762388691756) {
|
||||
H_28(state, unused, out_2453174762388691756);
|
||||
void car_H_28(double *state, double *unused, double *out_2213758511585587250) {
|
||||
H_28(state, unused, out_2213758511585587250);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_222736560719300508) {
|
||||
h_31(state, unused, out_222736560719300508);
|
||||
void car_h_31(double *state, double *unused, double *out_332156138656277884) {
|
||||
h_31(state, unused, out_332156138656277884);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_4537655438513541947) {
|
||||
H_31(state, unused, out_4537655438513541947);
|
||||
void car_H_31(double *state, double *unused, double *out_4298239187710437441) {
|
||||
H_31(state, unused, out_4298239187710437441);
|
||||
}
|
||||
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