mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-29 10:53:49 +08:00
build
This commit is contained in:
firestar5683
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
|
||||
* *
|
||||
* This file is part of 'ekf' *
|
||||
******************************************************************************/
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_5552049021186220575) {
|
||||
out_5552049021186220575[0] = delta_x[0] + nom_x[0];
|
||||
out_5552049021186220575[1] = delta_x[1] + nom_x[1];
|
||||
out_5552049021186220575[2] = delta_x[2] + nom_x[2];
|
||||
out_5552049021186220575[3] = delta_x[3] + nom_x[3];
|
||||
out_5552049021186220575[4] = delta_x[4] + nom_x[4];
|
||||
out_5552049021186220575[5] = delta_x[5] + nom_x[5];
|
||||
out_5552049021186220575[6] = delta_x[6] + nom_x[6];
|
||||
out_5552049021186220575[7] = delta_x[7] + nom_x[7];
|
||||
out_5552049021186220575[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_5579271098234299589) {
|
||||
out_5579271098234299589[0] = delta_x[0] + nom_x[0];
|
||||
out_5579271098234299589[1] = delta_x[1] + nom_x[1];
|
||||
out_5579271098234299589[2] = delta_x[2] + nom_x[2];
|
||||
out_5579271098234299589[3] = delta_x[3] + nom_x[3];
|
||||
out_5579271098234299589[4] = delta_x[4] + nom_x[4];
|
||||
out_5579271098234299589[5] = delta_x[5] + nom_x[5];
|
||||
out_5579271098234299589[6] = delta_x[6] + nom_x[6];
|
||||
out_5579271098234299589[7] = delta_x[7] + nom_x[7];
|
||||
out_5579271098234299589[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_590150806491182217) {
|
||||
out_590150806491182217[0] = -nom_x[0] + true_x[0];
|
||||
out_590150806491182217[1] = -nom_x[1] + true_x[1];
|
||||
out_590150806491182217[2] = -nom_x[2] + true_x[2];
|
||||
out_590150806491182217[3] = -nom_x[3] + true_x[3];
|
||||
out_590150806491182217[4] = -nom_x[4] + true_x[4];
|
||||
out_590150806491182217[5] = -nom_x[5] + true_x[5];
|
||||
out_590150806491182217[6] = -nom_x[6] + true_x[6];
|
||||
out_590150806491182217[7] = -nom_x[7] + true_x[7];
|
||||
out_590150806491182217[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8879118943542071452) {
|
||||
out_8879118943542071452[0] = -nom_x[0] + true_x[0];
|
||||
out_8879118943542071452[1] = -nom_x[1] + true_x[1];
|
||||
out_8879118943542071452[2] = -nom_x[2] + true_x[2];
|
||||
out_8879118943542071452[3] = -nom_x[3] + true_x[3];
|
||||
out_8879118943542071452[4] = -nom_x[4] + true_x[4];
|
||||
out_8879118943542071452[5] = -nom_x[5] + true_x[5];
|
||||
out_8879118943542071452[6] = -nom_x[6] + true_x[6];
|
||||
out_8879118943542071452[7] = -nom_x[7] + true_x[7];
|
||||
out_8879118943542071452[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_462982770310503507) {
|
||||
out_462982770310503507[0] = 1.0;
|
||||
out_462982770310503507[1] = 0.0;
|
||||
out_462982770310503507[2] = 0.0;
|
||||
out_462982770310503507[3] = 0.0;
|
||||
out_462982770310503507[4] = 0.0;
|
||||
out_462982770310503507[5] = 0.0;
|
||||
out_462982770310503507[6] = 0.0;
|
||||
out_462982770310503507[7] = 0.0;
|
||||
out_462982770310503507[8] = 0.0;
|
||||
out_462982770310503507[9] = 0.0;
|
||||
out_462982770310503507[10] = 1.0;
|
||||
out_462982770310503507[11] = 0.0;
|
||||
out_462982770310503507[12] = 0.0;
|
||||
out_462982770310503507[13] = 0.0;
|
||||
out_462982770310503507[14] = 0.0;
|
||||
out_462982770310503507[15] = 0.0;
|
||||
out_462982770310503507[16] = 0.0;
|
||||
out_462982770310503507[17] = 0.0;
|
||||
out_462982770310503507[18] = 0.0;
|
||||
out_462982770310503507[19] = 0.0;
|
||||
out_462982770310503507[20] = 1.0;
|
||||
out_462982770310503507[21] = 0.0;
|
||||
out_462982770310503507[22] = 0.0;
|
||||
out_462982770310503507[23] = 0.0;
|
||||
out_462982770310503507[24] = 0.0;
|
||||
out_462982770310503507[25] = 0.0;
|
||||
out_462982770310503507[26] = 0.0;
|
||||
out_462982770310503507[27] = 0.0;
|
||||
out_462982770310503507[28] = 0.0;
|
||||
out_462982770310503507[29] = 0.0;
|
||||
out_462982770310503507[30] = 1.0;
|
||||
out_462982770310503507[31] = 0.0;
|
||||
out_462982770310503507[32] = 0.0;
|
||||
out_462982770310503507[33] = 0.0;
|
||||
out_462982770310503507[34] = 0.0;
|
||||
out_462982770310503507[35] = 0.0;
|
||||
out_462982770310503507[36] = 0.0;
|
||||
out_462982770310503507[37] = 0.0;
|
||||
out_462982770310503507[38] = 0.0;
|
||||
out_462982770310503507[39] = 0.0;
|
||||
out_462982770310503507[40] = 1.0;
|
||||
out_462982770310503507[41] = 0.0;
|
||||
out_462982770310503507[42] = 0.0;
|
||||
out_462982770310503507[43] = 0.0;
|
||||
out_462982770310503507[44] = 0.0;
|
||||
out_462982770310503507[45] = 0.0;
|
||||
out_462982770310503507[46] = 0.0;
|
||||
out_462982770310503507[47] = 0.0;
|
||||
out_462982770310503507[48] = 0.0;
|
||||
out_462982770310503507[49] = 0.0;
|
||||
out_462982770310503507[50] = 1.0;
|
||||
out_462982770310503507[51] = 0.0;
|
||||
out_462982770310503507[52] = 0.0;
|
||||
out_462982770310503507[53] = 0.0;
|
||||
out_462982770310503507[54] = 0.0;
|
||||
out_462982770310503507[55] = 0.0;
|
||||
out_462982770310503507[56] = 0.0;
|
||||
out_462982770310503507[57] = 0.0;
|
||||
out_462982770310503507[58] = 0.0;
|
||||
out_462982770310503507[59] = 0.0;
|
||||
out_462982770310503507[60] = 1.0;
|
||||
out_462982770310503507[61] = 0.0;
|
||||
out_462982770310503507[62] = 0.0;
|
||||
out_462982770310503507[63] = 0.0;
|
||||
out_462982770310503507[64] = 0.0;
|
||||
out_462982770310503507[65] = 0.0;
|
||||
out_462982770310503507[66] = 0.0;
|
||||
out_462982770310503507[67] = 0.0;
|
||||
out_462982770310503507[68] = 0.0;
|
||||
out_462982770310503507[69] = 0.0;
|
||||
out_462982770310503507[70] = 1.0;
|
||||
out_462982770310503507[71] = 0.0;
|
||||
out_462982770310503507[72] = 0.0;
|
||||
out_462982770310503507[73] = 0.0;
|
||||
out_462982770310503507[74] = 0.0;
|
||||
out_462982770310503507[75] = 0.0;
|
||||
out_462982770310503507[76] = 0.0;
|
||||
out_462982770310503507[77] = 0.0;
|
||||
out_462982770310503507[78] = 0.0;
|
||||
out_462982770310503507[79] = 0.0;
|
||||
out_462982770310503507[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_4346759063564356553) {
|
||||
out_4346759063564356553[0] = 1.0;
|
||||
out_4346759063564356553[1] = 0.0;
|
||||
out_4346759063564356553[2] = 0.0;
|
||||
out_4346759063564356553[3] = 0.0;
|
||||
out_4346759063564356553[4] = 0.0;
|
||||
out_4346759063564356553[5] = 0.0;
|
||||
out_4346759063564356553[6] = 0.0;
|
||||
out_4346759063564356553[7] = 0.0;
|
||||
out_4346759063564356553[8] = 0.0;
|
||||
out_4346759063564356553[9] = 0.0;
|
||||
out_4346759063564356553[10] = 1.0;
|
||||
out_4346759063564356553[11] = 0.0;
|
||||
out_4346759063564356553[12] = 0.0;
|
||||
out_4346759063564356553[13] = 0.0;
|
||||
out_4346759063564356553[14] = 0.0;
|
||||
out_4346759063564356553[15] = 0.0;
|
||||
out_4346759063564356553[16] = 0.0;
|
||||
out_4346759063564356553[17] = 0.0;
|
||||
out_4346759063564356553[18] = 0.0;
|
||||
out_4346759063564356553[19] = 0.0;
|
||||
out_4346759063564356553[20] = 1.0;
|
||||
out_4346759063564356553[21] = 0.0;
|
||||
out_4346759063564356553[22] = 0.0;
|
||||
out_4346759063564356553[23] = 0.0;
|
||||
out_4346759063564356553[24] = 0.0;
|
||||
out_4346759063564356553[25] = 0.0;
|
||||
out_4346759063564356553[26] = 0.0;
|
||||
out_4346759063564356553[27] = 0.0;
|
||||
out_4346759063564356553[28] = 0.0;
|
||||
out_4346759063564356553[29] = 0.0;
|
||||
out_4346759063564356553[30] = 1.0;
|
||||
out_4346759063564356553[31] = 0.0;
|
||||
out_4346759063564356553[32] = 0.0;
|
||||
out_4346759063564356553[33] = 0.0;
|
||||
out_4346759063564356553[34] = 0.0;
|
||||
out_4346759063564356553[35] = 0.0;
|
||||
out_4346759063564356553[36] = 0.0;
|
||||
out_4346759063564356553[37] = 0.0;
|
||||
out_4346759063564356553[38] = 0.0;
|
||||
out_4346759063564356553[39] = 0.0;
|
||||
out_4346759063564356553[40] = 1.0;
|
||||
out_4346759063564356553[41] = 0.0;
|
||||
out_4346759063564356553[42] = 0.0;
|
||||
out_4346759063564356553[43] = 0.0;
|
||||
out_4346759063564356553[44] = 0.0;
|
||||
out_4346759063564356553[45] = 0.0;
|
||||
out_4346759063564356553[46] = 0.0;
|
||||
out_4346759063564356553[47] = 0.0;
|
||||
out_4346759063564356553[48] = 0.0;
|
||||
out_4346759063564356553[49] = 0.0;
|
||||
out_4346759063564356553[50] = 1.0;
|
||||
out_4346759063564356553[51] = 0.0;
|
||||
out_4346759063564356553[52] = 0.0;
|
||||
out_4346759063564356553[53] = 0.0;
|
||||
out_4346759063564356553[54] = 0.0;
|
||||
out_4346759063564356553[55] = 0.0;
|
||||
out_4346759063564356553[56] = 0.0;
|
||||
out_4346759063564356553[57] = 0.0;
|
||||
out_4346759063564356553[58] = 0.0;
|
||||
out_4346759063564356553[59] = 0.0;
|
||||
out_4346759063564356553[60] = 1.0;
|
||||
out_4346759063564356553[61] = 0.0;
|
||||
out_4346759063564356553[62] = 0.0;
|
||||
out_4346759063564356553[63] = 0.0;
|
||||
out_4346759063564356553[64] = 0.0;
|
||||
out_4346759063564356553[65] = 0.0;
|
||||
out_4346759063564356553[66] = 0.0;
|
||||
out_4346759063564356553[67] = 0.0;
|
||||
out_4346759063564356553[68] = 0.0;
|
||||
out_4346759063564356553[69] = 0.0;
|
||||
out_4346759063564356553[70] = 1.0;
|
||||
out_4346759063564356553[71] = 0.0;
|
||||
out_4346759063564356553[72] = 0.0;
|
||||
out_4346759063564356553[73] = 0.0;
|
||||
out_4346759063564356553[74] = 0.0;
|
||||
out_4346759063564356553[75] = 0.0;
|
||||
out_4346759063564356553[76] = 0.0;
|
||||
out_4346759063564356553[77] = 0.0;
|
||||
out_4346759063564356553[78] = 0.0;
|
||||
out_4346759063564356553[79] = 0.0;
|
||||
out_4346759063564356553[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_169833714219358489) {
|
||||
out_169833714219358489[0] = state[0];
|
||||
out_169833714219358489[1] = state[1];
|
||||
out_169833714219358489[2] = state[2];
|
||||
out_169833714219358489[3] = state[3];
|
||||
out_169833714219358489[4] = state[4];
|
||||
out_169833714219358489[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_169833714219358489[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_169833714219358489[7] = state[7];
|
||||
out_169833714219358489[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_8401415236153157083) {
|
||||
out_8401415236153157083[0] = state[0];
|
||||
out_8401415236153157083[1] = state[1];
|
||||
out_8401415236153157083[2] = state[2];
|
||||
out_8401415236153157083[3] = state[3];
|
||||
out_8401415236153157083[4] = state[4];
|
||||
out_8401415236153157083[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_8401415236153157083[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_8401415236153157083[7] = state[7];
|
||||
out_8401415236153157083[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_3869384383894482819) {
|
||||
out_3869384383894482819[0] = 1;
|
||||
out_3869384383894482819[1] = 0;
|
||||
out_3869384383894482819[2] = 0;
|
||||
out_3869384383894482819[3] = 0;
|
||||
out_3869384383894482819[4] = 0;
|
||||
out_3869384383894482819[5] = 0;
|
||||
out_3869384383894482819[6] = 0;
|
||||
out_3869384383894482819[7] = 0;
|
||||
out_3869384383894482819[8] = 0;
|
||||
out_3869384383894482819[9] = 0;
|
||||
out_3869384383894482819[10] = 1;
|
||||
out_3869384383894482819[11] = 0;
|
||||
out_3869384383894482819[12] = 0;
|
||||
out_3869384383894482819[13] = 0;
|
||||
out_3869384383894482819[14] = 0;
|
||||
out_3869384383894482819[15] = 0;
|
||||
out_3869384383894482819[16] = 0;
|
||||
out_3869384383894482819[17] = 0;
|
||||
out_3869384383894482819[18] = 0;
|
||||
out_3869384383894482819[19] = 0;
|
||||
out_3869384383894482819[20] = 1;
|
||||
out_3869384383894482819[21] = 0;
|
||||
out_3869384383894482819[22] = 0;
|
||||
out_3869384383894482819[23] = 0;
|
||||
out_3869384383894482819[24] = 0;
|
||||
out_3869384383894482819[25] = 0;
|
||||
out_3869384383894482819[26] = 0;
|
||||
out_3869384383894482819[27] = 0;
|
||||
out_3869384383894482819[28] = 0;
|
||||
out_3869384383894482819[29] = 0;
|
||||
out_3869384383894482819[30] = 1;
|
||||
out_3869384383894482819[31] = 0;
|
||||
out_3869384383894482819[32] = 0;
|
||||
out_3869384383894482819[33] = 0;
|
||||
out_3869384383894482819[34] = 0;
|
||||
out_3869384383894482819[35] = 0;
|
||||
out_3869384383894482819[36] = 0;
|
||||
out_3869384383894482819[37] = 0;
|
||||
out_3869384383894482819[38] = 0;
|
||||
out_3869384383894482819[39] = 0;
|
||||
out_3869384383894482819[40] = 1;
|
||||
out_3869384383894482819[41] = 0;
|
||||
out_3869384383894482819[42] = 0;
|
||||
out_3869384383894482819[43] = 0;
|
||||
out_3869384383894482819[44] = 0;
|
||||
out_3869384383894482819[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_3869384383894482819[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_3869384383894482819[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3869384383894482819[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3869384383894482819[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_3869384383894482819[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_3869384383894482819[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_3869384383894482819[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3869384383894482819[53] = -9.8100000000000005*dt;
|
||||
out_3869384383894482819[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_3869384383894482819[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_3869384383894482819[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3869384383894482819[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3869384383894482819[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_3869384383894482819[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_3869384383894482819[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_3869384383894482819[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3869384383894482819[62] = 0;
|
||||
out_3869384383894482819[63] = 0;
|
||||
out_3869384383894482819[64] = 0;
|
||||
out_3869384383894482819[65] = 0;
|
||||
out_3869384383894482819[66] = 0;
|
||||
out_3869384383894482819[67] = 0;
|
||||
out_3869384383894482819[68] = 0;
|
||||
out_3869384383894482819[69] = 0;
|
||||
out_3869384383894482819[70] = 1;
|
||||
out_3869384383894482819[71] = 0;
|
||||
out_3869384383894482819[72] = 0;
|
||||
out_3869384383894482819[73] = 0;
|
||||
out_3869384383894482819[74] = 0;
|
||||
out_3869384383894482819[75] = 0;
|
||||
out_3869384383894482819[76] = 0;
|
||||
out_3869384383894482819[77] = 0;
|
||||
out_3869384383894482819[78] = 0;
|
||||
out_3869384383894482819[79] = 0;
|
||||
out_3869384383894482819[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_1973239055632703499) {
|
||||
out_1973239055632703499[0] = 1;
|
||||
out_1973239055632703499[1] = 0;
|
||||
out_1973239055632703499[2] = 0;
|
||||
out_1973239055632703499[3] = 0;
|
||||
out_1973239055632703499[4] = 0;
|
||||
out_1973239055632703499[5] = 0;
|
||||
out_1973239055632703499[6] = 0;
|
||||
out_1973239055632703499[7] = 0;
|
||||
out_1973239055632703499[8] = 0;
|
||||
out_1973239055632703499[9] = 0;
|
||||
out_1973239055632703499[10] = 1;
|
||||
out_1973239055632703499[11] = 0;
|
||||
out_1973239055632703499[12] = 0;
|
||||
out_1973239055632703499[13] = 0;
|
||||
out_1973239055632703499[14] = 0;
|
||||
out_1973239055632703499[15] = 0;
|
||||
out_1973239055632703499[16] = 0;
|
||||
out_1973239055632703499[17] = 0;
|
||||
out_1973239055632703499[18] = 0;
|
||||
out_1973239055632703499[19] = 0;
|
||||
out_1973239055632703499[20] = 1;
|
||||
out_1973239055632703499[21] = 0;
|
||||
out_1973239055632703499[22] = 0;
|
||||
out_1973239055632703499[23] = 0;
|
||||
out_1973239055632703499[24] = 0;
|
||||
out_1973239055632703499[25] = 0;
|
||||
out_1973239055632703499[26] = 0;
|
||||
out_1973239055632703499[27] = 0;
|
||||
out_1973239055632703499[28] = 0;
|
||||
out_1973239055632703499[29] = 0;
|
||||
out_1973239055632703499[30] = 1;
|
||||
out_1973239055632703499[31] = 0;
|
||||
out_1973239055632703499[32] = 0;
|
||||
out_1973239055632703499[33] = 0;
|
||||
out_1973239055632703499[34] = 0;
|
||||
out_1973239055632703499[35] = 0;
|
||||
out_1973239055632703499[36] = 0;
|
||||
out_1973239055632703499[37] = 0;
|
||||
out_1973239055632703499[38] = 0;
|
||||
out_1973239055632703499[39] = 0;
|
||||
out_1973239055632703499[40] = 1;
|
||||
out_1973239055632703499[41] = 0;
|
||||
out_1973239055632703499[42] = 0;
|
||||
out_1973239055632703499[43] = 0;
|
||||
out_1973239055632703499[44] = 0;
|
||||
out_1973239055632703499[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_1973239055632703499[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_1973239055632703499[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1973239055632703499[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1973239055632703499[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_1973239055632703499[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_1973239055632703499[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_1973239055632703499[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1973239055632703499[53] = -9.8100000000000005*dt;
|
||||
out_1973239055632703499[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_1973239055632703499[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_1973239055632703499[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1973239055632703499[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1973239055632703499[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_1973239055632703499[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_1973239055632703499[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_1973239055632703499[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1973239055632703499[62] = 0;
|
||||
out_1973239055632703499[63] = 0;
|
||||
out_1973239055632703499[64] = 0;
|
||||
out_1973239055632703499[65] = 0;
|
||||
out_1973239055632703499[66] = 0;
|
||||
out_1973239055632703499[67] = 0;
|
||||
out_1973239055632703499[68] = 0;
|
||||
out_1973239055632703499[69] = 0;
|
||||
out_1973239055632703499[70] = 1;
|
||||
out_1973239055632703499[71] = 0;
|
||||
out_1973239055632703499[72] = 0;
|
||||
out_1973239055632703499[73] = 0;
|
||||
out_1973239055632703499[74] = 0;
|
||||
out_1973239055632703499[75] = 0;
|
||||
out_1973239055632703499[76] = 0;
|
||||
out_1973239055632703499[77] = 0;
|
||||
out_1973239055632703499[78] = 0;
|
||||
out_1973239055632703499[79] = 0;
|
||||
out_1973239055632703499[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_3087348870251025033) {
|
||||
out_3087348870251025033[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_7709444436246576106) {
|
||||
out_7709444436246576106[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_8790573077486810872) {
|
||||
out_8790573077486810872[0] = 0;
|
||||
out_8790573077486810872[1] = 0;
|
||||
out_8790573077486810872[2] = 0;
|
||||
out_8790573077486810872[3] = 0;
|
||||
out_8790573077486810872[4] = 0;
|
||||
out_8790573077486810872[5] = 0;
|
||||
out_8790573077486810872[6] = 1;
|
||||
out_8790573077486810872[7] = 0;
|
||||
out_8790573077486810872[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_4906796784232957826) {
|
||||
out_4906796784232957826[0] = 0;
|
||||
out_4906796784232957826[1] = 0;
|
||||
out_4906796784232957826[2] = 0;
|
||||
out_4906796784232957826[3] = 0;
|
||||
out_4906796784232957826[4] = 0;
|
||||
out_4906796784232957826[5] = 0;
|
||||
out_4906796784232957826[6] = 1;
|
||||
out_4906796784232957826[7] = 0;
|
||||
out_4906796784232957826[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_6915008056190712895) {
|
||||
out_6915008056190712895[0] = state[4];
|
||||
out_6915008056190712895[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_8161953024412082811) {
|
||||
out_8161953024412082811[0] = state[4];
|
||||
out_8161953024412082811[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_6617923478481311306) {
|
||||
out_6617923478481311306[0] = 0;
|
||||
out_6617923478481311306[1] = 0;
|
||||
out_6617923478481311306[2] = 0;
|
||||
out_6617923478481311306[3] = 0;
|
||||
out_6617923478481311306[4] = 1;
|
||||
out_6617923478481311306[5] = 0;
|
||||
out_6617923478481311306[6] = 0;
|
||||
out_6617923478481311306[7] = 0;
|
||||
out_6617923478481311306[8] = 0;
|
||||
out_6617923478481311306[9] = 0;
|
||||
out_6617923478481311306[10] = 0;
|
||||
out_6617923478481311306[11] = 0;
|
||||
out_6617923478481311306[12] = 0;
|
||||
out_6617923478481311306[13] = 0;
|
||||
out_6617923478481311306[14] = 1;
|
||||
out_6617923478481311306[15] = 0;
|
||||
out_6617923478481311306[16] = 0;
|
||||
out_6617923478481311306[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_6319825032271366663) {
|
||||
out_6319825032271366663[0] = 0;
|
||||
out_6319825032271366663[1] = 0;
|
||||
out_6319825032271366663[2] = 0;
|
||||
out_6319825032271366663[3] = 0;
|
||||
out_6319825032271366663[4] = 1;
|
||||
out_6319825032271366663[5] = 0;
|
||||
out_6319825032271366663[6] = 0;
|
||||
out_6319825032271366663[7] = 0;
|
||||
out_6319825032271366663[8] = 0;
|
||||
out_6319825032271366663[9] = 0;
|
||||
out_6319825032271366663[10] = 0;
|
||||
out_6319825032271366663[11] = 0;
|
||||
out_6319825032271366663[12] = 0;
|
||||
out_6319825032271366663[13] = 0;
|
||||
out_6319825032271366663[14] = 1;
|
||||
out_6319825032271366663[15] = 0;
|
||||
out_6319825032271366663[16] = 0;
|
||||
out_6319825032271366663[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_3500479056225270130) {
|
||||
out_3500479056225270130[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_8890419815179839294) {
|
||||
out_8890419815179839294[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_2739480654731123989) {
|
||||
out_2739480654731123989[0] = 0;
|
||||
out_2739480654731123989[1] = 0;
|
||||
out_2739480654731123989[2] = 0;
|
||||
out_2739480654731123989[3] = 0;
|
||||
out_2739480654731123989[4] = 1;
|
||||
out_2739480654731123989[5] = 0;
|
||||
out_2739480654731123989[6] = 0;
|
||||
out_2739480654731123989[7] = 0;
|
||||
out_2739480654731123989[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_7425129742740206453) {
|
||||
out_7425129742740206453[0] = 0;
|
||||
out_7425129742740206453[1] = 0;
|
||||
out_7425129742740206453[2] = 0;
|
||||
out_7425129742740206453[3] = 0;
|
||||
out_7425129742740206453[4] = 1;
|
||||
out_7425129742740206453[5] = 0;
|
||||
out_7425129742740206453[6] = 0;
|
||||
out_7425129742740206453[7] = 0;
|
||||
out_7425129742740206453[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_2897163171548792613) {
|
||||
out_2897163171548792613[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_8798474113368893512) {
|
||||
out_8798474113368893512[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_5049069758612754648) {
|
||||
out_5049069758612754648[0] = 0;
|
||||
out_5049069758612754648[1] = 0;
|
||||
out_5049069758612754648[2] = 0;
|
||||
out_5049069758612754648[3] = 0;
|
||||
out_5049069758612754648[4] = 0;
|
||||
out_5049069758612754648[5] = 0;
|
||||
out_5049069758612754648[6] = 0;
|
||||
out_5049069758612754648[7] = 1;
|
||||
out_5049069758612754648[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_1165293465358901602) {
|
||||
out_1165293465358901602[0] = 0;
|
||||
out_1165293465358901602[1] = 0;
|
||||
out_1165293465358901602[2] = 0;
|
||||
out_1165293465358901602[3] = 0;
|
||||
out_1165293465358901602[4] = 0;
|
||||
out_1165293465358901602[5] = 0;
|
||||
out_1165293465358901602[6] = 0;
|
||||
out_1165293465358901602[7] = 1;
|
||||
out_1165293465358901602[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_5691754191059722684) {
|
||||
out_5691754191059722684[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_4672658384802243421) {
|
||||
out_4672658384802243421[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_4914243966531548900) {
|
||||
out_4914243966531548900[0] = 0;
|
||||
out_4914243966531548900[1] = 0;
|
||||
out_4914243966531548900[2] = 0;
|
||||
out_4914243966531548900[3] = 1;
|
||||
out_4914243966531548900[4] = 0;
|
||||
out_4914243966531548900[5] = 0;
|
||||
out_4914243966531548900[6] = 0;
|
||||
out_4914243966531548900[7] = 0;
|
||||
out_4914243966531548900[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_8798020259785401946) {
|
||||
out_8798020259785401946[0] = 0;
|
||||
out_8798020259785401946[1] = 0;
|
||||
out_8798020259785401946[2] = 0;
|
||||
out_8798020259785401946[3] = 1;
|
||||
out_8798020259785401946[4] = 0;
|
||||
out_8798020259785401946[5] = 0;
|
||||
out_8798020259785401946[6] = 0;
|
||||
out_8798020259785401946[7] = 0;
|
||||
out_8798020259785401946[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_8502152323886115775) {
|
||||
out_8502152323886115775[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_4214457022643681759) {
|
||||
out_4214457022643681759[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_6627606693401099933) {
|
||||
out_6627606693401099933[0] = 0;
|
||||
out_6627606693401099933[1] = 1;
|
||||
out_6627606693401099933[2] = 0;
|
||||
out_6627606693401099933[3] = 0;
|
||||
out_6627606693401099933[4] = 0;
|
||||
out_6627606693401099933[5] = 0;
|
||||
out_6627606693401099933[6] = 0;
|
||||
out_6627606693401099933[7] = 0;
|
||||
out_6627606693401099933[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_7935361087054598637) {
|
||||
out_7935361087054598637[0] = 0;
|
||||
out_7935361087054598637[1] = 1;
|
||||
out_7935361087054598637[2] = 0;
|
||||
out_7935361087054598637[3] = 0;
|
||||
out_7935361087054598637[4] = 0;
|
||||
out_7935361087054598637[5] = 0;
|
||||
out_7935361087054598637[6] = 0;
|
||||
out_7935361087054598637[7] = 0;
|
||||
out_7935361087054598637[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_7413122646763798369) {
|
||||
out_7413122646763798369[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_8772412421246462550) {
|
||||
out_8772412421246462550[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_6736738363238921109) {
|
||||
out_6736738363238921109[0] = 1;
|
||||
out_6736738363238921109[1] = 0;
|
||||
out_6736738363238921109[2] = 0;
|
||||
out_6736738363238921109[3] = 0;
|
||||
out_6736738363238921109[4] = 0;
|
||||
out_6736738363238921109[5] = 0;
|
||||
out_6736738363238921109[6] = 0;
|
||||
out_6736738363238921109[7] = 0;
|
||||
out_6736738363238921109[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_2852962069985068063) {
|
||||
out_2852962069985068063[0] = 1;
|
||||
out_2852962069985068063[1] = 0;
|
||||
out_2852962069985068063[2] = 0;
|
||||
out_2852962069985068063[3] = 0;
|
||||
out_2852962069985068063[4] = 0;
|
||||
out_2852962069985068063[5] = 0;
|
||||
out_2852962069985068063[6] = 0;
|
||||
out_2852962069985068063[7] = 0;
|
||||
out_2852962069985068063[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_2930162201899895888) {
|
||||
out_2930162201899895888[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_7434250373962070217) {
|
||||
out_7434250373962070217[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_8821219039363771300) {
|
||||
out_8821219039363771300[0] = 0;
|
||||
out_8821219039363771300[1] = 0;
|
||||
out_8821219039363771300[2] = 0;
|
||||
out_8821219039363771300[3] = 0;
|
||||
out_8821219039363771300[4] = 0;
|
||||
out_8821219039363771300[5] = 0;
|
||||
out_8821219039363771300[6] = 0;
|
||||
out_8821219039363771300[7] = 0;
|
||||
out_8821219039363771300[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_539085363125550126) {
|
||||
out_539085363125550126[0] = 0;
|
||||
out_539085363125550126[1] = 0;
|
||||
out_539085363125550126[2] = 0;
|
||||
out_539085363125550126[3] = 0;
|
||||
out_539085363125550126[4] = 0;
|
||||
out_539085363125550126[5] = 0;
|
||||
out_539085363125550126[6] = 0;
|
||||
out_539085363125550126[7] = 0;
|
||||
out_539085363125550126[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_5552049021186220575) {
|
||||
err_fun(nom_x, delta_x, out_5552049021186220575);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_5579271098234299589) {
|
||||
err_fun(nom_x, delta_x, out_5579271098234299589);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_590150806491182217) {
|
||||
inv_err_fun(nom_x, true_x, out_590150806491182217);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8879118943542071452) {
|
||||
inv_err_fun(nom_x, true_x, out_8879118943542071452);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_462982770310503507) {
|
||||
H_mod_fun(state, out_462982770310503507);
|
||||
void car_H_mod_fun(double *state, double *out_4346759063564356553) {
|
||||
H_mod_fun(state, out_4346759063564356553);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_169833714219358489) {
|
||||
f_fun(state, dt, out_169833714219358489);
|
||||
void car_f_fun(double *state, double dt, double *out_8401415236153157083) {
|
||||
f_fun(state, dt, out_8401415236153157083);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_3869384383894482819) {
|
||||
F_fun(state, dt, out_3869384383894482819);
|
||||
void car_F_fun(double *state, double dt, double *out_1973239055632703499) {
|
||||
F_fun(state, dt, out_1973239055632703499);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_3087348870251025033) {
|
||||
h_25(state, unused, out_3087348870251025033);
|
||||
void car_h_25(double *state, double *unused, double *out_7709444436246576106) {
|
||||
h_25(state, unused, out_7709444436246576106);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_8790573077486810872) {
|
||||
H_25(state, unused, out_8790573077486810872);
|
||||
void car_H_25(double *state, double *unused, double *out_4906796784232957826) {
|
||||
H_25(state, unused, out_4906796784232957826);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_6915008056190712895) {
|
||||
h_24(state, unused, out_6915008056190712895);
|
||||
void car_h_24(double *state, double *unused, double *out_8161953024412082811) {
|
||||
h_24(state, unused, out_8161953024412082811);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_6617923478481311306) {
|
||||
H_24(state, unused, out_6617923478481311306);
|
||||
void car_H_24(double *state, double *unused, double *out_6319825032271366663) {
|
||||
H_24(state, unused, out_6319825032271366663);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_3500479056225270130) {
|
||||
h_30(state, unused, out_3500479056225270130);
|
||||
void car_h_30(double *state, double *unused, double *out_8890419815179839294) {
|
||||
h_30(state, unused, out_8890419815179839294);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_2739480654731123989) {
|
||||
H_30(state, unused, out_2739480654731123989);
|
||||
void car_H_30(double *state, double *unused, double *out_7425129742740206453) {
|
||||
H_30(state, unused, out_7425129742740206453);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_2897163171548792613) {
|
||||
h_26(state, unused, out_2897163171548792613);
|
||||
void car_h_26(double *state, double *unused, double *out_8798474113368893512) {
|
||||
h_26(state, unused, out_8798474113368893512);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_5049069758612754648) {
|
||||
H_26(state, unused, out_5049069758612754648);
|
||||
void car_H_26(double *state, double *unused, double *out_1165293465358901602) {
|
||||
H_26(state, unused, out_1165293465358901602);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_5691754191059722684) {
|
||||
h_27(state, unused, out_5691754191059722684);
|
||||
void car_h_27(double *state, double *unused, double *out_4672658384802243421) {
|
||||
h_27(state, unused, out_4672658384802243421);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_4914243966531548900) {
|
||||
H_27(state, unused, out_4914243966531548900);
|
||||
void car_H_27(double *state, double *unused, double *out_8798020259785401946) {
|
||||
H_27(state, unused, out_8798020259785401946);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_8502152323886115775) {
|
||||
h_29(state, unused, out_8502152323886115775);
|
||||
void car_h_29(double *state, double *unused, double *out_4214457022643681759) {
|
||||
h_29(state, unused, out_4214457022643681759);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_6627606693401099933) {
|
||||
H_29(state, unused, out_6627606693401099933);
|
||||
void car_H_29(double *state, double *unused, double *out_7935361087054598637) {
|
||||
H_29(state, unused, out_7935361087054598637);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_7413122646763798369) {
|
||||
h_28(state, unused, out_7413122646763798369);
|
||||
void car_h_28(double *state, double *unused, double *out_8772412421246462550) {
|
||||
h_28(state, unused, out_8772412421246462550);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_6736738363238921109) {
|
||||
H_28(state, unused, out_6736738363238921109);
|
||||
void car_H_28(double *state, double *unused, double *out_2852962069985068063) {
|
||||
H_28(state, unused, out_2852962069985068063);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_2930162201899895888) {
|
||||
h_31(state, unused, out_2930162201899895888);
|
||||
void car_h_31(double *state, double *unused, double *out_7434250373962070217) {
|
||||
h_31(state, unused, out_7434250373962070217);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_8821219039363771300) {
|
||||
H_31(state, unused, out_8821219039363771300);
|
||||
void car_H_31(double *state, double *unused, double *out_539085363125550126) {
|
||||
H_31(state, unused, out_539085363125550126);
|
||||
}
|
||||
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