mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-01 03:43:46 +08:00
Nav Long
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_284207352707246819) {
|
||||
out_284207352707246819[0] = delta_x[0] + nom_x[0];
|
||||
out_284207352707246819[1] = delta_x[1] + nom_x[1];
|
||||
out_284207352707246819[2] = delta_x[2] + nom_x[2];
|
||||
out_284207352707246819[3] = delta_x[3] + nom_x[3];
|
||||
out_284207352707246819[4] = delta_x[4] + nom_x[4];
|
||||
out_284207352707246819[5] = delta_x[5] + nom_x[5];
|
||||
out_284207352707246819[6] = delta_x[6] + nom_x[6];
|
||||
out_284207352707246819[7] = delta_x[7] + nom_x[7];
|
||||
out_284207352707246819[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_575866575379183764) {
|
||||
out_575866575379183764[0] = delta_x[0] + nom_x[0];
|
||||
out_575866575379183764[1] = delta_x[1] + nom_x[1];
|
||||
out_575866575379183764[2] = delta_x[2] + nom_x[2];
|
||||
out_575866575379183764[3] = delta_x[3] + nom_x[3];
|
||||
out_575866575379183764[4] = delta_x[4] + nom_x[4];
|
||||
out_575866575379183764[5] = delta_x[5] + nom_x[5];
|
||||
out_575866575379183764[6] = delta_x[6] + nom_x[6];
|
||||
out_575866575379183764[7] = delta_x[7] + nom_x[7];
|
||||
out_575866575379183764[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_494072413816411138) {
|
||||
out_494072413816411138[0] = -nom_x[0] + true_x[0];
|
||||
out_494072413816411138[1] = -nom_x[1] + true_x[1];
|
||||
out_494072413816411138[2] = -nom_x[2] + true_x[2];
|
||||
out_494072413816411138[3] = -nom_x[3] + true_x[3];
|
||||
out_494072413816411138[4] = -nom_x[4] + true_x[4];
|
||||
out_494072413816411138[5] = -nom_x[5] + true_x[5];
|
||||
out_494072413816411138[6] = -nom_x[6] + true_x[6];
|
||||
out_494072413816411138[7] = -nom_x[7] + true_x[7];
|
||||
out_494072413816411138[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_5616167858322380498) {
|
||||
out_5616167858322380498[0] = -nom_x[0] + true_x[0];
|
||||
out_5616167858322380498[1] = -nom_x[1] + true_x[1];
|
||||
out_5616167858322380498[2] = -nom_x[2] + true_x[2];
|
||||
out_5616167858322380498[3] = -nom_x[3] + true_x[3];
|
||||
out_5616167858322380498[4] = -nom_x[4] + true_x[4];
|
||||
out_5616167858322380498[5] = -nom_x[5] + true_x[5];
|
||||
out_5616167858322380498[6] = -nom_x[6] + true_x[6];
|
||||
out_5616167858322380498[7] = -nom_x[7] + true_x[7];
|
||||
out_5616167858322380498[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_4975994215663677500) {
|
||||
out_4975994215663677500[0] = 1.0;
|
||||
out_4975994215663677500[1] = 0.0;
|
||||
out_4975994215663677500[2] = 0.0;
|
||||
out_4975994215663677500[3] = 0.0;
|
||||
out_4975994215663677500[4] = 0.0;
|
||||
out_4975994215663677500[5] = 0.0;
|
||||
out_4975994215663677500[6] = 0.0;
|
||||
out_4975994215663677500[7] = 0.0;
|
||||
out_4975994215663677500[8] = 0.0;
|
||||
out_4975994215663677500[9] = 0.0;
|
||||
out_4975994215663677500[10] = 1.0;
|
||||
out_4975994215663677500[11] = 0.0;
|
||||
out_4975994215663677500[12] = 0.0;
|
||||
out_4975994215663677500[13] = 0.0;
|
||||
out_4975994215663677500[14] = 0.0;
|
||||
out_4975994215663677500[15] = 0.0;
|
||||
out_4975994215663677500[16] = 0.0;
|
||||
out_4975994215663677500[17] = 0.0;
|
||||
out_4975994215663677500[18] = 0.0;
|
||||
out_4975994215663677500[19] = 0.0;
|
||||
out_4975994215663677500[20] = 1.0;
|
||||
out_4975994215663677500[21] = 0.0;
|
||||
out_4975994215663677500[22] = 0.0;
|
||||
out_4975994215663677500[23] = 0.0;
|
||||
out_4975994215663677500[24] = 0.0;
|
||||
out_4975994215663677500[25] = 0.0;
|
||||
out_4975994215663677500[26] = 0.0;
|
||||
out_4975994215663677500[27] = 0.0;
|
||||
out_4975994215663677500[28] = 0.0;
|
||||
out_4975994215663677500[29] = 0.0;
|
||||
out_4975994215663677500[30] = 1.0;
|
||||
out_4975994215663677500[31] = 0.0;
|
||||
out_4975994215663677500[32] = 0.0;
|
||||
out_4975994215663677500[33] = 0.0;
|
||||
out_4975994215663677500[34] = 0.0;
|
||||
out_4975994215663677500[35] = 0.0;
|
||||
out_4975994215663677500[36] = 0.0;
|
||||
out_4975994215663677500[37] = 0.0;
|
||||
out_4975994215663677500[38] = 0.0;
|
||||
out_4975994215663677500[39] = 0.0;
|
||||
out_4975994215663677500[40] = 1.0;
|
||||
out_4975994215663677500[41] = 0.0;
|
||||
out_4975994215663677500[42] = 0.0;
|
||||
out_4975994215663677500[43] = 0.0;
|
||||
out_4975994215663677500[44] = 0.0;
|
||||
out_4975994215663677500[45] = 0.0;
|
||||
out_4975994215663677500[46] = 0.0;
|
||||
out_4975994215663677500[47] = 0.0;
|
||||
out_4975994215663677500[48] = 0.0;
|
||||
out_4975994215663677500[49] = 0.0;
|
||||
out_4975994215663677500[50] = 1.0;
|
||||
out_4975994215663677500[51] = 0.0;
|
||||
out_4975994215663677500[52] = 0.0;
|
||||
out_4975994215663677500[53] = 0.0;
|
||||
out_4975994215663677500[54] = 0.0;
|
||||
out_4975994215663677500[55] = 0.0;
|
||||
out_4975994215663677500[56] = 0.0;
|
||||
out_4975994215663677500[57] = 0.0;
|
||||
out_4975994215663677500[58] = 0.0;
|
||||
out_4975994215663677500[59] = 0.0;
|
||||
out_4975994215663677500[60] = 1.0;
|
||||
out_4975994215663677500[61] = 0.0;
|
||||
out_4975994215663677500[62] = 0.0;
|
||||
out_4975994215663677500[63] = 0.0;
|
||||
out_4975994215663677500[64] = 0.0;
|
||||
out_4975994215663677500[65] = 0.0;
|
||||
out_4975994215663677500[66] = 0.0;
|
||||
out_4975994215663677500[67] = 0.0;
|
||||
out_4975994215663677500[68] = 0.0;
|
||||
out_4975994215663677500[69] = 0.0;
|
||||
out_4975994215663677500[70] = 1.0;
|
||||
out_4975994215663677500[71] = 0.0;
|
||||
out_4975994215663677500[72] = 0.0;
|
||||
out_4975994215663677500[73] = 0.0;
|
||||
out_4975994215663677500[74] = 0.0;
|
||||
out_4975994215663677500[75] = 0.0;
|
||||
out_4975994215663677500[76] = 0.0;
|
||||
out_4975994215663677500[77] = 0.0;
|
||||
out_4975994215663677500[78] = 0.0;
|
||||
out_4975994215663677500[79] = 0.0;
|
||||
out_4975994215663677500[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_6639538403135995006) {
|
||||
out_6639538403135995006[0] = 1.0;
|
||||
out_6639538403135995006[1] = 0.0;
|
||||
out_6639538403135995006[2] = 0.0;
|
||||
out_6639538403135995006[3] = 0.0;
|
||||
out_6639538403135995006[4] = 0.0;
|
||||
out_6639538403135995006[5] = 0.0;
|
||||
out_6639538403135995006[6] = 0.0;
|
||||
out_6639538403135995006[7] = 0.0;
|
||||
out_6639538403135995006[8] = 0.0;
|
||||
out_6639538403135995006[9] = 0.0;
|
||||
out_6639538403135995006[10] = 1.0;
|
||||
out_6639538403135995006[11] = 0.0;
|
||||
out_6639538403135995006[12] = 0.0;
|
||||
out_6639538403135995006[13] = 0.0;
|
||||
out_6639538403135995006[14] = 0.0;
|
||||
out_6639538403135995006[15] = 0.0;
|
||||
out_6639538403135995006[16] = 0.0;
|
||||
out_6639538403135995006[17] = 0.0;
|
||||
out_6639538403135995006[18] = 0.0;
|
||||
out_6639538403135995006[19] = 0.0;
|
||||
out_6639538403135995006[20] = 1.0;
|
||||
out_6639538403135995006[21] = 0.0;
|
||||
out_6639538403135995006[22] = 0.0;
|
||||
out_6639538403135995006[23] = 0.0;
|
||||
out_6639538403135995006[24] = 0.0;
|
||||
out_6639538403135995006[25] = 0.0;
|
||||
out_6639538403135995006[26] = 0.0;
|
||||
out_6639538403135995006[27] = 0.0;
|
||||
out_6639538403135995006[28] = 0.0;
|
||||
out_6639538403135995006[29] = 0.0;
|
||||
out_6639538403135995006[30] = 1.0;
|
||||
out_6639538403135995006[31] = 0.0;
|
||||
out_6639538403135995006[32] = 0.0;
|
||||
out_6639538403135995006[33] = 0.0;
|
||||
out_6639538403135995006[34] = 0.0;
|
||||
out_6639538403135995006[35] = 0.0;
|
||||
out_6639538403135995006[36] = 0.0;
|
||||
out_6639538403135995006[37] = 0.0;
|
||||
out_6639538403135995006[38] = 0.0;
|
||||
out_6639538403135995006[39] = 0.0;
|
||||
out_6639538403135995006[40] = 1.0;
|
||||
out_6639538403135995006[41] = 0.0;
|
||||
out_6639538403135995006[42] = 0.0;
|
||||
out_6639538403135995006[43] = 0.0;
|
||||
out_6639538403135995006[44] = 0.0;
|
||||
out_6639538403135995006[45] = 0.0;
|
||||
out_6639538403135995006[46] = 0.0;
|
||||
out_6639538403135995006[47] = 0.0;
|
||||
out_6639538403135995006[48] = 0.0;
|
||||
out_6639538403135995006[49] = 0.0;
|
||||
out_6639538403135995006[50] = 1.0;
|
||||
out_6639538403135995006[51] = 0.0;
|
||||
out_6639538403135995006[52] = 0.0;
|
||||
out_6639538403135995006[53] = 0.0;
|
||||
out_6639538403135995006[54] = 0.0;
|
||||
out_6639538403135995006[55] = 0.0;
|
||||
out_6639538403135995006[56] = 0.0;
|
||||
out_6639538403135995006[57] = 0.0;
|
||||
out_6639538403135995006[58] = 0.0;
|
||||
out_6639538403135995006[59] = 0.0;
|
||||
out_6639538403135995006[60] = 1.0;
|
||||
out_6639538403135995006[61] = 0.0;
|
||||
out_6639538403135995006[62] = 0.0;
|
||||
out_6639538403135995006[63] = 0.0;
|
||||
out_6639538403135995006[64] = 0.0;
|
||||
out_6639538403135995006[65] = 0.0;
|
||||
out_6639538403135995006[66] = 0.0;
|
||||
out_6639538403135995006[67] = 0.0;
|
||||
out_6639538403135995006[68] = 0.0;
|
||||
out_6639538403135995006[69] = 0.0;
|
||||
out_6639538403135995006[70] = 1.0;
|
||||
out_6639538403135995006[71] = 0.0;
|
||||
out_6639538403135995006[72] = 0.0;
|
||||
out_6639538403135995006[73] = 0.0;
|
||||
out_6639538403135995006[74] = 0.0;
|
||||
out_6639538403135995006[75] = 0.0;
|
||||
out_6639538403135995006[76] = 0.0;
|
||||
out_6639538403135995006[77] = 0.0;
|
||||
out_6639538403135995006[78] = 0.0;
|
||||
out_6639538403135995006[79] = 0.0;
|
||||
out_6639538403135995006[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_1721040043494422786) {
|
||||
out_1721040043494422786[0] = state[0];
|
||||
out_1721040043494422786[1] = state[1];
|
||||
out_1721040043494422786[2] = state[2];
|
||||
out_1721040043494422786[3] = state[3];
|
||||
out_1721040043494422786[4] = state[4];
|
||||
out_1721040043494422786[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_1721040043494422786[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_1721040043494422786[7] = state[7];
|
||||
out_1721040043494422786[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_6721417124249708482) {
|
||||
out_6721417124249708482[0] = state[0];
|
||||
out_6721417124249708482[1] = state[1];
|
||||
out_6721417124249708482[2] = state[2];
|
||||
out_6721417124249708482[3] = state[3];
|
||||
out_6721417124249708482[4] = state[4];
|
||||
out_6721417124249708482[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_6721417124249708482[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_6721417124249708482[7] = state[7];
|
||||
out_6721417124249708482[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_6408657319260780093) {
|
||||
out_6408657319260780093[0] = 1;
|
||||
out_6408657319260780093[1] = 0;
|
||||
out_6408657319260780093[2] = 0;
|
||||
out_6408657319260780093[3] = 0;
|
||||
out_6408657319260780093[4] = 0;
|
||||
out_6408657319260780093[5] = 0;
|
||||
out_6408657319260780093[6] = 0;
|
||||
out_6408657319260780093[7] = 0;
|
||||
out_6408657319260780093[8] = 0;
|
||||
out_6408657319260780093[9] = 0;
|
||||
out_6408657319260780093[10] = 1;
|
||||
out_6408657319260780093[11] = 0;
|
||||
out_6408657319260780093[12] = 0;
|
||||
out_6408657319260780093[13] = 0;
|
||||
out_6408657319260780093[14] = 0;
|
||||
out_6408657319260780093[15] = 0;
|
||||
out_6408657319260780093[16] = 0;
|
||||
out_6408657319260780093[17] = 0;
|
||||
out_6408657319260780093[18] = 0;
|
||||
out_6408657319260780093[19] = 0;
|
||||
out_6408657319260780093[20] = 1;
|
||||
out_6408657319260780093[21] = 0;
|
||||
out_6408657319260780093[22] = 0;
|
||||
out_6408657319260780093[23] = 0;
|
||||
out_6408657319260780093[24] = 0;
|
||||
out_6408657319260780093[25] = 0;
|
||||
out_6408657319260780093[26] = 0;
|
||||
out_6408657319260780093[27] = 0;
|
||||
out_6408657319260780093[28] = 0;
|
||||
out_6408657319260780093[29] = 0;
|
||||
out_6408657319260780093[30] = 1;
|
||||
out_6408657319260780093[31] = 0;
|
||||
out_6408657319260780093[32] = 0;
|
||||
out_6408657319260780093[33] = 0;
|
||||
out_6408657319260780093[34] = 0;
|
||||
out_6408657319260780093[35] = 0;
|
||||
out_6408657319260780093[36] = 0;
|
||||
out_6408657319260780093[37] = 0;
|
||||
out_6408657319260780093[38] = 0;
|
||||
out_6408657319260780093[39] = 0;
|
||||
out_6408657319260780093[40] = 1;
|
||||
out_6408657319260780093[41] = 0;
|
||||
out_6408657319260780093[42] = 0;
|
||||
out_6408657319260780093[43] = 0;
|
||||
out_6408657319260780093[44] = 0;
|
||||
out_6408657319260780093[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_6408657319260780093[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_6408657319260780093[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6408657319260780093[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6408657319260780093[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_6408657319260780093[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_6408657319260780093[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_6408657319260780093[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6408657319260780093[53] = -9.8100000000000005*dt;
|
||||
out_6408657319260780093[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_6408657319260780093[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_6408657319260780093[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6408657319260780093[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6408657319260780093[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_6408657319260780093[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_6408657319260780093[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_6408657319260780093[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6408657319260780093[62] = 0;
|
||||
out_6408657319260780093[63] = 0;
|
||||
out_6408657319260780093[64] = 0;
|
||||
out_6408657319260780093[65] = 0;
|
||||
out_6408657319260780093[66] = 0;
|
||||
out_6408657319260780093[67] = 0;
|
||||
out_6408657319260780093[68] = 0;
|
||||
out_6408657319260780093[69] = 0;
|
||||
out_6408657319260780093[70] = 1;
|
||||
out_6408657319260780093[71] = 0;
|
||||
out_6408657319260780093[72] = 0;
|
||||
out_6408657319260780093[73] = 0;
|
||||
out_6408657319260780093[74] = 0;
|
||||
out_6408657319260780093[75] = 0;
|
||||
out_6408657319260780093[76] = 0;
|
||||
out_6408657319260780093[77] = 0;
|
||||
out_6408657319260780093[78] = 0;
|
||||
out_6408657319260780093[79] = 0;
|
||||
out_6408657319260780093[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_2916284287213904038) {
|
||||
out_2916284287213904038[0] = 1;
|
||||
out_2916284287213904038[1] = 0;
|
||||
out_2916284287213904038[2] = 0;
|
||||
out_2916284287213904038[3] = 0;
|
||||
out_2916284287213904038[4] = 0;
|
||||
out_2916284287213904038[5] = 0;
|
||||
out_2916284287213904038[6] = 0;
|
||||
out_2916284287213904038[7] = 0;
|
||||
out_2916284287213904038[8] = 0;
|
||||
out_2916284287213904038[9] = 0;
|
||||
out_2916284287213904038[10] = 1;
|
||||
out_2916284287213904038[11] = 0;
|
||||
out_2916284287213904038[12] = 0;
|
||||
out_2916284287213904038[13] = 0;
|
||||
out_2916284287213904038[14] = 0;
|
||||
out_2916284287213904038[15] = 0;
|
||||
out_2916284287213904038[16] = 0;
|
||||
out_2916284287213904038[17] = 0;
|
||||
out_2916284287213904038[18] = 0;
|
||||
out_2916284287213904038[19] = 0;
|
||||
out_2916284287213904038[20] = 1;
|
||||
out_2916284287213904038[21] = 0;
|
||||
out_2916284287213904038[22] = 0;
|
||||
out_2916284287213904038[23] = 0;
|
||||
out_2916284287213904038[24] = 0;
|
||||
out_2916284287213904038[25] = 0;
|
||||
out_2916284287213904038[26] = 0;
|
||||
out_2916284287213904038[27] = 0;
|
||||
out_2916284287213904038[28] = 0;
|
||||
out_2916284287213904038[29] = 0;
|
||||
out_2916284287213904038[30] = 1;
|
||||
out_2916284287213904038[31] = 0;
|
||||
out_2916284287213904038[32] = 0;
|
||||
out_2916284287213904038[33] = 0;
|
||||
out_2916284287213904038[34] = 0;
|
||||
out_2916284287213904038[35] = 0;
|
||||
out_2916284287213904038[36] = 0;
|
||||
out_2916284287213904038[37] = 0;
|
||||
out_2916284287213904038[38] = 0;
|
||||
out_2916284287213904038[39] = 0;
|
||||
out_2916284287213904038[40] = 1;
|
||||
out_2916284287213904038[41] = 0;
|
||||
out_2916284287213904038[42] = 0;
|
||||
out_2916284287213904038[43] = 0;
|
||||
out_2916284287213904038[44] = 0;
|
||||
out_2916284287213904038[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_2916284287213904038[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_2916284287213904038[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2916284287213904038[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2916284287213904038[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_2916284287213904038[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_2916284287213904038[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_2916284287213904038[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2916284287213904038[53] = -9.8100000000000005*dt;
|
||||
out_2916284287213904038[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_2916284287213904038[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_2916284287213904038[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2916284287213904038[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2916284287213904038[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_2916284287213904038[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_2916284287213904038[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_2916284287213904038[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2916284287213904038[62] = 0;
|
||||
out_2916284287213904038[63] = 0;
|
||||
out_2916284287213904038[64] = 0;
|
||||
out_2916284287213904038[65] = 0;
|
||||
out_2916284287213904038[66] = 0;
|
||||
out_2916284287213904038[67] = 0;
|
||||
out_2916284287213904038[68] = 0;
|
||||
out_2916284287213904038[69] = 0;
|
||||
out_2916284287213904038[70] = 1;
|
||||
out_2916284287213904038[71] = 0;
|
||||
out_2916284287213904038[72] = 0;
|
||||
out_2916284287213904038[73] = 0;
|
||||
out_2916284287213904038[74] = 0;
|
||||
out_2916284287213904038[75] = 0;
|
||||
out_2916284287213904038[76] = 0;
|
||||
out_2916284287213904038[77] = 0;
|
||||
out_2916284287213904038[78] = 0;
|
||||
out_2916284287213904038[79] = 0;
|
||||
out_2916284287213904038[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_8679682106878424978) {
|
||||
out_8679682106878424978[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_2044795451145406261) {
|
||||
out_2044795451145406261[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_4470659773463380788) {
|
||||
out_4470659773463380788[0] = 0;
|
||||
out_4470659773463380788[1] = 0;
|
||||
out_4470659773463380788[2] = 0;
|
||||
out_4470659773463380788[3] = 0;
|
||||
out_4470659773463380788[4] = 0;
|
||||
out_4470659773463380788[5] = 0;
|
||||
out_4470659773463380788[6] = 1;
|
||||
out_4470659773463380788[7] = 0;
|
||||
out_4470659773463380788[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_8284225020584064933) {
|
||||
out_8284225020584064933[0] = 0;
|
||||
out_8284225020584064933[1] = 0;
|
||||
out_8284225020584064933[2] = 0;
|
||||
out_8284225020584064933[3] = 0;
|
||||
out_8284225020584064933[4] = 0;
|
||||
out_8284225020584064933[5] = 0;
|
||||
out_8284225020584064933[6] = 1;
|
||||
out_8284225020584064933[7] = 0;
|
||||
out_8284225020584064933[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_5665719925403943373) {
|
||||
out_5665719925403943373[0] = state[4];
|
||||
out_5665719925403943373[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_1441131931248447039) {
|
||||
out_1441131931248447039[0] = state[4];
|
||||
out_1441131931248447039[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_7514976983467728980) {
|
||||
out_7514976983467728980[0] = 0;
|
||||
out_7514976983467728980[1] = 0;
|
||||
out_7514976983467728980[2] = 0;
|
||||
out_7514976983467728980[3] = 0;
|
||||
out_7514976983467728980[4] = 1;
|
||||
out_7514976983467728980[5] = 0;
|
||||
out_7514976983467728980[6] = 0;
|
||||
out_7514976983467728980[7] = 0;
|
||||
out_7514976983467728980[8] = 0;
|
||||
out_7514976983467728980[9] = 0;
|
||||
out_7514976983467728980[10] = 0;
|
||||
out_7514976983467728980[11] = 0;
|
||||
out_7514976983467728980[12] = 0;
|
||||
out_7514976983467728980[13] = 0;
|
||||
out_7514976983467728980[14] = 1;
|
||||
out_7514976983467728980[15] = 0;
|
||||
out_7514976983467728980[16] = 0;
|
||||
out_7514976983467728980[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_4726299421781741797) {
|
||||
out_4726299421781741797[0] = 0;
|
||||
out_4726299421781741797[1] = 0;
|
||||
out_4726299421781741797[2] = 0;
|
||||
out_4726299421781741797[3] = 0;
|
||||
out_4726299421781741797[4] = 1;
|
||||
out_4726299421781741797[5] = 0;
|
||||
out_4726299421781741797[6] = 0;
|
||||
out_4726299421781741797[7] = 0;
|
||||
out_4726299421781741797[8] = 0;
|
||||
out_4726299421781741797[9] = 0;
|
||||
out_4726299421781741797[10] = 0;
|
||||
out_4726299421781741797[11] = 0;
|
||||
out_4726299421781741797[12] = 0;
|
||||
out_4726299421781741797[13] = 0;
|
||||
out_4726299421781741797[14] = 1;
|
||||
out_4726299421781741797[15] = 0;
|
||||
out_4726299421781741797[16] = 0;
|
||||
out_4726299421781741797[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_6129612438577104288) {
|
||||
out_6129612438577104288[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_8638683365743472914) {
|
||||
out_8638683365743472914[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_4599998720606620858) {
|
||||
out_4599998720606620858[0] = 0;
|
||||
out_4599998720606620858[1] = 0;
|
||||
out_4599998720606620858[2] = 0;
|
||||
out_4599998720606620858[3] = 0;
|
||||
out_4599998720606620858[4] = 1;
|
||||
out_4599998720606620858[5] = 0;
|
||||
out_4599998720606620858[6] = 0;
|
||||
out_4599998720606620858[7] = 0;
|
||||
out_4599998720606620858[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_8413563967727305003) {
|
||||
out_8413563967727305003[0] = 0;
|
||||
out_8413563967727305003[1] = 0;
|
||||
out_8413563967727305003[2] = 0;
|
||||
out_8413563967727305003[3] = 0;
|
||||
out_8413563967727305003[4] = 1;
|
||||
out_8413563967727305003[5] = 0;
|
||||
out_8413563967727305003[6] = 0;
|
||||
out_8413563967727305003[7] = 0;
|
||||
out_8413563967727305003[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_6887462583737923098) {
|
||||
out_6887462583737923098[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_989338192432138572) {
|
||||
out_989338192432138572[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_3813805709353068884) {
|
||||
out_3813805709353068884[0] = 0;
|
||||
out_3813805709353068884[1] = 0;
|
||||
out_3813805709353068884[2] = 0;
|
||||
out_3813805709353068884[3] = 0;
|
||||
out_3813805709353068884[4] = 0;
|
||||
out_3813805709353068884[5] = 0;
|
||||
out_3813805709353068884[6] = 0;
|
||||
out_3813805709353068884[7] = 1;
|
||||
out_3813805709353068884[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_6421015734251430459) {
|
||||
out_6421015734251430459[0] = 0;
|
||||
out_6421015734251430459[1] = 0;
|
||||
out_6421015734251430459[2] = 0;
|
||||
out_6421015734251430459[3] = 0;
|
||||
out_6421015734251430459[4] = 0;
|
||||
out_6421015734251430459[5] = 0;
|
||||
out_6421015734251430459[6] = 0;
|
||||
out_6421015734251430459[7] = 1;
|
||||
out_6421015734251430459[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_3337814870992394415) {
|
||||
out_3337814870992394415[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_7375345222001897423) {
|
||||
out_7375345222001897423[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_6774762032407045769) {
|
||||
out_6774762032407045769[0] = 0;
|
||||
out_6774762032407045769[1] = 0;
|
||||
out_6774762032407045769[2] = 0;
|
||||
out_6774762032407045769[3] = 1;
|
||||
out_6774762032407045769[4] = 0;
|
||||
out_6774762032407045769[5] = 0;
|
||||
out_6774762032407045769[6] = 0;
|
||||
out_6774762032407045769[7] = 0;
|
||||
out_6774762032407045769[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_7858416794181821702) {
|
||||
out_7858416794181821702[0] = 0;
|
||||
out_7858416794181821702[1] = 0;
|
||||
out_7858416794181821702[2] = 0;
|
||||
out_7858416794181821702[3] = 1;
|
||||
out_7858416794181821702[4] = 0;
|
||||
out_7858416794181821702[5] = 0;
|
||||
out_7858416794181821702[6] = 0;
|
||||
out_7858416794181821702[7] = 0;
|
||||
out_7858416794181821702[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_5302062605726147469) {
|
||||
out_5302062605726147469[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_4806387440305233057) {
|
||||
out_4806387440305233057[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_4089767376292228674) {
|
||||
out_4089767376292228674[0] = 0;
|
||||
out_4089767376292228674[1] = 1;
|
||||
out_4089767376292228674[2] = 0;
|
||||
out_4089767376292228674[3] = 0;
|
||||
out_4089767376292228674[4] = 0;
|
||||
out_4089767376292228674[5] = 0;
|
||||
out_4089767376292228674[6] = 0;
|
||||
out_4089767376292228674[7] = 0;
|
||||
out_4089767376292228674[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_6145054067312270669) {
|
||||
out_6145054067312270669[0] = 0;
|
||||
out_6145054067312270669[1] = 1;
|
||||
out_6145054067312270669[2] = 0;
|
||||
out_6145054067312270669[3] = 0;
|
||||
out_6145054067312270669[4] = 0;
|
||||
out_6145054067312270669[5] = 0;
|
||||
out_6145054067312270669[6] = 0;
|
||||
out_6145054067312270669[7] = 0;
|
||||
out_6145054067312270669[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_301028819548061730) {
|
||||
out_301028819548061730[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_6025091355491401381) {
|
||||
out_6025091355491401381[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_2126137104726902423) {
|
||||
out_2126137104726902423[0] = 1;
|
||||
out_2126137104726902423[1] = 0;
|
||||
out_2126137104726902423[2] = 0;
|
||||
out_2126137104726902423[3] = 0;
|
||||
out_2126137104726902423[4] = 0;
|
||||
out_2126137104726902423[5] = 0;
|
||||
out_2126137104726902423[6] = 0;
|
||||
out_2126137104726902423[7] = 0;
|
||||
out_2126137104726902423[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_8108684338877596920) {
|
||||
out_8108684338877596920[0] = 1;
|
||||
out_8108684338877596920[1] = 0;
|
||||
out_8108684338877596920[2] = 0;
|
||||
out_8108684338877596920[3] = 0;
|
||||
out_8108684338877596920[4] = 0;
|
||||
out_8108684338877596920[5] = 0;
|
||||
out_8108684338877596920[6] = 0;
|
||||
out_8108684338877596920[7] = 0;
|
||||
out_8108684338877596920[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_213227004039568063) {
|
||||
out_213227004039568063[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_2208042107618306896) {
|
||||
out_2208042107618306896[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_4440013811586420360) {
|
||||
out_4440013811586420360[0] = 0;
|
||||
out_4440013811586420360[1] = 0;
|
||||
out_4440013811586420360[2] = 0;
|
||||
out_4440013811586420360[3] = 0;
|
||||
out_4440013811586420360[4] = 0;
|
||||
out_4440013811586420360[5] = 0;
|
||||
out_4440013811586420360[6] = 0;
|
||||
out_4440013811586420360[7] = 0;
|
||||
out_4440013811586420360[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_8253579058707104505) {
|
||||
out_8253579058707104505[0] = 0;
|
||||
out_8253579058707104505[1] = 0;
|
||||
out_8253579058707104505[2] = 0;
|
||||
out_8253579058707104505[3] = 0;
|
||||
out_8253579058707104505[4] = 0;
|
||||
out_8253579058707104505[5] = 0;
|
||||
out_8253579058707104505[6] = 0;
|
||||
out_8253579058707104505[7] = 0;
|
||||
out_8253579058707104505[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_284207352707246819) {
|
||||
err_fun(nom_x, delta_x, out_284207352707246819);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_575866575379183764) {
|
||||
err_fun(nom_x, delta_x, out_575866575379183764);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_494072413816411138) {
|
||||
inv_err_fun(nom_x, true_x, out_494072413816411138);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5616167858322380498) {
|
||||
inv_err_fun(nom_x, true_x, out_5616167858322380498);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_4975994215663677500) {
|
||||
H_mod_fun(state, out_4975994215663677500);
|
||||
void car_H_mod_fun(double *state, double *out_6639538403135995006) {
|
||||
H_mod_fun(state, out_6639538403135995006);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_1721040043494422786) {
|
||||
f_fun(state, dt, out_1721040043494422786);
|
||||
void car_f_fun(double *state, double dt, double *out_6721417124249708482) {
|
||||
f_fun(state, dt, out_6721417124249708482);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_6408657319260780093) {
|
||||
F_fun(state, dt, out_6408657319260780093);
|
||||
void car_F_fun(double *state, double dt, double *out_2916284287213904038) {
|
||||
F_fun(state, dt, out_2916284287213904038);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_8679682106878424978) {
|
||||
h_25(state, unused, out_8679682106878424978);
|
||||
void car_h_25(double *state, double *unused, double *out_2044795451145406261) {
|
||||
h_25(state, unused, out_2044795451145406261);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_4470659773463380788) {
|
||||
H_25(state, unused, out_4470659773463380788);
|
||||
void car_H_25(double *state, double *unused, double *out_8284225020584064933) {
|
||||
H_25(state, unused, out_8284225020584064933);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_5665719925403943373) {
|
||||
h_24(state, unused, out_5665719925403943373);
|
||||
void car_h_24(double *state, double *unused, double *out_1441131931248447039) {
|
||||
h_24(state, unused, out_1441131931248447039);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_7514976983467728980) {
|
||||
H_24(state, unused, out_7514976983467728980);
|
||||
void car_H_24(double *state, double *unused, double *out_4726299421781741797) {
|
||||
H_24(state, unused, out_4726299421781741797);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_6129612438577104288) {
|
||||
h_30(state, unused, out_6129612438577104288);
|
||||
void car_h_30(double *state, double *unused, double *out_8638683365743472914) {
|
||||
h_30(state, unused, out_8638683365743472914);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_4599998720606620858) {
|
||||
H_30(state, unused, out_4599998720606620858);
|
||||
void car_H_30(double *state, double *unused, double *out_8413563967727305003) {
|
||||
H_30(state, unused, out_8413563967727305003);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_6887462583737923098) {
|
||||
h_26(state, unused, out_6887462583737923098);
|
||||
void car_h_26(double *state, double *unused, double *out_989338192432138572) {
|
||||
h_26(state, unused, out_989338192432138572);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_3813805709353068884) {
|
||||
H_26(state, unused, out_3813805709353068884);
|
||||
void car_H_26(double *state, double *unused, double *out_6421015734251430459) {
|
||||
H_26(state, unused, out_6421015734251430459);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_3337814870992394415) {
|
||||
h_27(state, unused, out_3337814870992394415);
|
||||
void car_h_27(double *state, double *unused, double *out_7375345222001897423) {
|
||||
h_27(state, unused, out_7375345222001897423);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_6774762032407045769) {
|
||||
H_27(state, unused, out_6774762032407045769);
|
||||
void car_H_27(double *state, double *unused, double *out_7858416794181821702) {
|
||||
H_27(state, unused, out_7858416794181821702);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_5302062605726147469) {
|
||||
h_29(state, unused, out_5302062605726147469);
|
||||
void car_h_29(double *state, double *unused, double *out_4806387440305233057) {
|
||||
h_29(state, unused, out_4806387440305233057);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_4089767376292228674) {
|
||||
H_29(state, unused, out_4089767376292228674);
|
||||
void car_H_29(double *state, double *unused, double *out_6145054067312270669) {
|
||||
H_29(state, unused, out_6145054067312270669);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_301028819548061730) {
|
||||
h_28(state, unused, out_301028819548061730);
|
||||
void car_h_28(double *state, double *unused, double *out_6025091355491401381) {
|
||||
h_28(state, unused, out_6025091355491401381);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_2126137104726902423) {
|
||||
H_28(state, unused, out_2126137104726902423);
|
||||
void car_H_28(double *state, double *unused, double *out_8108684338877596920) {
|
||||
H_28(state, unused, out_8108684338877596920);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_213227004039568063) {
|
||||
h_31(state, unused, out_213227004039568063);
|
||||
void car_h_31(double *state, double *unused, double *out_2208042107618306896) {
|
||||
h_31(state, unused, out_2208042107618306896);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_4440013811586420360) {
|
||||
H_31(state, unused, out_4440013811586420360);
|
||||
void car_H_31(double *state, double *unused, double *out_8253579058707104505) {
|
||||
H_31(state, unused, out_8253579058707104505);
|
||||
}
|
||||
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