mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-03-01 16:43:52 +08:00
dragonpilot 2023-04-13T02:41:11 for EON/C2
version: dragonpilot v0.9.2 beta for EON/C2 date: 2023-04-13T02:41:11 dp-dev(priv2) master commit: 4526e2dbd703c5d3e958825e64d5912ae452ab30
This commit is contained in:
Dragonpilot Team
@@ -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_5634330020296848070) {
|
||||
out_5634330020296848070[0] = delta_x[0] + nom_x[0];
|
||||
out_5634330020296848070[1] = delta_x[1] + nom_x[1];
|
||||
out_5634330020296848070[2] = delta_x[2] + nom_x[2];
|
||||
out_5634330020296848070[3] = delta_x[3] + nom_x[3];
|
||||
out_5634330020296848070[4] = delta_x[4] + nom_x[4];
|
||||
out_5634330020296848070[5] = delta_x[5] + nom_x[5];
|
||||
out_5634330020296848070[6] = delta_x[6] + nom_x[6];
|
||||
out_5634330020296848070[7] = delta_x[7] + nom_x[7];
|
||||
out_5634330020296848070[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_9008082079438066370) {
|
||||
out_9008082079438066370[0] = delta_x[0] + nom_x[0];
|
||||
out_9008082079438066370[1] = delta_x[1] + nom_x[1];
|
||||
out_9008082079438066370[2] = delta_x[2] + nom_x[2];
|
||||
out_9008082079438066370[3] = delta_x[3] + nom_x[3];
|
||||
out_9008082079438066370[4] = delta_x[4] + nom_x[4];
|
||||
out_9008082079438066370[5] = delta_x[5] + nom_x[5];
|
||||
out_9008082079438066370[6] = delta_x[6] + nom_x[6];
|
||||
out_9008082079438066370[7] = delta_x[7] + nom_x[7];
|
||||
out_9008082079438066370[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_1061559027503315281) {
|
||||
out_1061559027503315281[0] = -nom_x[0] + true_x[0];
|
||||
out_1061559027503315281[1] = -nom_x[1] + true_x[1];
|
||||
out_1061559027503315281[2] = -nom_x[2] + true_x[2];
|
||||
out_1061559027503315281[3] = -nom_x[3] + true_x[3];
|
||||
out_1061559027503315281[4] = -nom_x[4] + true_x[4];
|
||||
out_1061559027503315281[5] = -nom_x[5] + true_x[5];
|
||||
out_1061559027503315281[6] = -nom_x[6] + true_x[6];
|
||||
out_1061559027503315281[7] = -nom_x[7] + true_x[7];
|
||||
out_1061559027503315281[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_7705390806256602072) {
|
||||
out_7705390806256602072[0] = -nom_x[0] + true_x[0];
|
||||
out_7705390806256602072[1] = -nom_x[1] + true_x[1];
|
||||
out_7705390806256602072[2] = -nom_x[2] + true_x[2];
|
||||
out_7705390806256602072[3] = -nom_x[3] + true_x[3];
|
||||
out_7705390806256602072[4] = -nom_x[4] + true_x[4];
|
||||
out_7705390806256602072[5] = -nom_x[5] + true_x[5];
|
||||
out_7705390806256602072[6] = -nom_x[6] + true_x[6];
|
||||
out_7705390806256602072[7] = -nom_x[7] + true_x[7];
|
||||
out_7705390806256602072[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_1567569310651922730) {
|
||||
out_1567569310651922730[0] = 1.0;
|
||||
out_1567569310651922730[1] = 0;
|
||||
out_1567569310651922730[2] = 0;
|
||||
out_1567569310651922730[3] = 0;
|
||||
out_1567569310651922730[4] = 0;
|
||||
out_1567569310651922730[5] = 0;
|
||||
out_1567569310651922730[6] = 0;
|
||||
out_1567569310651922730[7] = 0;
|
||||
out_1567569310651922730[8] = 0;
|
||||
out_1567569310651922730[9] = 0;
|
||||
out_1567569310651922730[10] = 1.0;
|
||||
out_1567569310651922730[11] = 0;
|
||||
out_1567569310651922730[12] = 0;
|
||||
out_1567569310651922730[13] = 0;
|
||||
out_1567569310651922730[14] = 0;
|
||||
out_1567569310651922730[15] = 0;
|
||||
out_1567569310651922730[16] = 0;
|
||||
out_1567569310651922730[17] = 0;
|
||||
out_1567569310651922730[18] = 0;
|
||||
out_1567569310651922730[19] = 0;
|
||||
out_1567569310651922730[20] = 1.0;
|
||||
out_1567569310651922730[21] = 0;
|
||||
out_1567569310651922730[22] = 0;
|
||||
out_1567569310651922730[23] = 0;
|
||||
out_1567569310651922730[24] = 0;
|
||||
out_1567569310651922730[25] = 0;
|
||||
out_1567569310651922730[26] = 0;
|
||||
out_1567569310651922730[27] = 0;
|
||||
out_1567569310651922730[28] = 0;
|
||||
out_1567569310651922730[29] = 0;
|
||||
out_1567569310651922730[30] = 1.0;
|
||||
out_1567569310651922730[31] = 0;
|
||||
out_1567569310651922730[32] = 0;
|
||||
out_1567569310651922730[33] = 0;
|
||||
out_1567569310651922730[34] = 0;
|
||||
out_1567569310651922730[35] = 0;
|
||||
out_1567569310651922730[36] = 0;
|
||||
out_1567569310651922730[37] = 0;
|
||||
out_1567569310651922730[38] = 0;
|
||||
out_1567569310651922730[39] = 0;
|
||||
out_1567569310651922730[40] = 1.0;
|
||||
out_1567569310651922730[41] = 0;
|
||||
out_1567569310651922730[42] = 0;
|
||||
out_1567569310651922730[43] = 0;
|
||||
out_1567569310651922730[44] = 0;
|
||||
out_1567569310651922730[45] = 0;
|
||||
out_1567569310651922730[46] = 0;
|
||||
out_1567569310651922730[47] = 0;
|
||||
out_1567569310651922730[48] = 0;
|
||||
out_1567569310651922730[49] = 0;
|
||||
out_1567569310651922730[50] = 1.0;
|
||||
out_1567569310651922730[51] = 0;
|
||||
out_1567569310651922730[52] = 0;
|
||||
out_1567569310651922730[53] = 0;
|
||||
out_1567569310651922730[54] = 0;
|
||||
out_1567569310651922730[55] = 0;
|
||||
out_1567569310651922730[56] = 0;
|
||||
out_1567569310651922730[57] = 0;
|
||||
out_1567569310651922730[58] = 0;
|
||||
out_1567569310651922730[59] = 0;
|
||||
out_1567569310651922730[60] = 1.0;
|
||||
out_1567569310651922730[61] = 0;
|
||||
out_1567569310651922730[62] = 0;
|
||||
out_1567569310651922730[63] = 0;
|
||||
out_1567569310651922730[64] = 0;
|
||||
out_1567569310651922730[65] = 0;
|
||||
out_1567569310651922730[66] = 0;
|
||||
out_1567569310651922730[67] = 0;
|
||||
out_1567569310651922730[68] = 0;
|
||||
out_1567569310651922730[69] = 0;
|
||||
out_1567569310651922730[70] = 1.0;
|
||||
out_1567569310651922730[71] = 0;
|
||||
out_1567569310651922730[72] = 0;
|
||||
out_1567569310651922730[73] = 0;
|
||||
out_1567569310651922730[74] = 0;
|
||||
out_1567569310651922730[75] = 0;
|
||||
out_1567569310651922730[76] = 0;
|
||||
out_1567569310651922730[77] = 0;
|
||||
out_1567569310651922730[78] = 0;
|
||||
out_1567569310651922730[79] = 0;
|
||||
out_1567569310651922730[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_4044238657628669408) {
|
||||
out_4044238657628669408[0] = 1.0;
|
||||
out_4044238657628669408[1] = 0;
|
||||
out_4044238657628669408[2] = 0;
|
||||
out_4044238657628669408[3] = 0;
|
||||
out_4044238657628669408[4] = 0;
|
||||
out_4044238657628669408[5] = 0;
|
||||
out_4044238657628669408[6] = 0;
|
||||
out_4044238657628669408[7] = 0;
|
||||
out_4044238657628669408[8] = 0;
|
||||
out_4044238657628669408[9] = 0;
|
||||
out_4044238657628669408[10] = 1.0;
|
||||
out_4044238657628669408[11] = 0;
|
||||
out_4044238657628669408[12] = 0;
|
||||
out_4044238657628669408[13] = 0;
|
||||
out_4044238657628669408[14] = 0;
|
||||
out_4044238657628669408[15] = 0;
|
||||
out_4044238657628669408[16] = 0;
|
||||
out_4044238657628669408[17] = 0;
|
||||
out_4044238657628669408[18] = 0;
|
||||
out_4044238657628669408[19] = 0;
|
||||
out_4044238657628669408[20] = 1.0;
|
||||
out_4044238657628669408[21] = 0;
|
||||
out_4044238657628669408[22] = 0;
|
||||
out_4044238657628669408[23] = 0;
|
||||
out_4044238657628669408[24] = 0;
|
||||
out_4044238657628669408[25] = 0;
|
||||
out_4044238657628669408[26] = 0;
|
||||
out_4044238657628669408[27] = 0;
|
||||
out_4044238657628669408[28] = 0;
|
||||
out_4044238657628669408[29] = 0;
|
||||
out_4044238657628669408[30] = 1.0;
|
||||
out_4044238657628669408[31] = 0;
|
||||
out_4044238657628669408[32] = 0;
|
||||
out_4044238657628669408[33] = 0;
|
||||
out_4044238657628669408[34] = 0;
|
||||
out_4044238657628669408[35] = 0;
|
||||
out_4044238657628669408[36] = 0;
|
||||
out_4044238657628669408[37] = 0;
|
||||
out_4044238657628669408[38] = 0;
|
||||
out_4044238657628669408[39] = 0;
|
||||
out_4044238657628669408[40] = 1.0;
|
||||
out_4044238657628669408[41] = 0;
|
||||
out_4044238657628669408[42] = 0;
|
||||
out_4044238657628669408[43] = 0;
|
||||
out_4044238657628669408[44] = 0;
|
||||
out_4044238657628669408[45] = 0;
|
||||
out_4044238657628669408[46] = 0;
|
||||
out_4044238657628669408[47] = 0;
|
||||
out_4044238657628669408[48] = 0;
|
||||
out_4044238657628669408[49] = 0;
|
||||
out_4044238657628669408[50] = 1.0;
|
||||
out_4044238657628669408[51] = 0;
|
||||
out_4044238657628669408[52] = 0;
|
||||
out_4044238657628669408[53] = 0;
|
||||
out_4044238657628669408[54] = 0;
|
||||
out_4044238657628669408[55] = 0;
|
||||
out_4044238657628669408[56] = 0;
|
||||
out_4044238657628669408[57] = 0;
|
||||
out_4044238657628669408[58] = 0;
|
||||
out_4044238657628669408[59] = 0;
|
||||
out_4044238657628669408[60] = 1.0;
|
||||
out_4044238657628669408[61] = 0;
|
||||
out_4044238657628669408[62] = 0;
|
||||
out_4044238657628669408[63] = 0;
|
||||
out_4044238657628669408[64] = 0;
|
||||
out_4044238657628669408[65] = 0;
|
||||
out_4044238657628669408[66] = 0;
|
||||
out_4044238657628669408[67] = 0;
|
||||
out_4044238657628669408[68] = 0;
|
||||
out_4044238657628669408[69] = 0;
|
||||
out_4044238657628669408[70] = 1.0;
|
||||
out_4044238657628669408[71] = 0;
|
||||
out_4044238657628669408[72] = 0;
|
||||
out_4044238657628669408[73] = 0;
|
||||
out_4044238657628669408[74] = 0;
|
||||
out_4044238657628669408[75] = 0;
|
||||
out_4044238657628669408[76] = 0;
|
||||
out_4044238657628669408[77] = 0;
|
||||
out_4044238657628669408[78] = 0;
|
||||
out_4044238657628669408[79] = 0;
|
||||
out_4044238657628669408[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_1208381054458531187) {
|
||||
out_1208381054458531187[0] = state[0];
|
||||
out_1208381054458531187[1] = state[1];
|
||||
out_1208381054458531187[2] = state[2];
|
||||
out_1208381054458531187[3] = state[3];
|
||||
out_1208381054458531187[4] = state[4];
|
||||
out_1208381054458531187[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_1208381054458531187[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_1208381054458531187[7] = state[7];
|
||||
out_1208381054458531187[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_6587028471138044914) {
|
||||
out_6587028471138044914[0] = state[0];
|
||||
out_6587028471138044914[1] = state[1];
|
||||
out_6587028471138044914[2] = state[2];
|
||||
out_6587028471138044914[3] = state[3];
|
||||
out_6587028471138044914[4] = state[4];
|
||||
out_6587028471138044914[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_6587028471138044914[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_6587028471138044914[7] = state[7];
|
||||
out_6587028471138044914[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_8824004521572161416) {
|
||||
out_8824004521572161416[0] = 1;
|
||||
out_8824004521572161416[1] = 0;
|
||||
out_8824004521572161416[2] = 0;
|
||||
out_8824004521572161416[3] = 0;
|
||||
out_8824004521572161416[4] = 0;
|
||||
out_8824004521572161416[5] = 0;
|
||||
out_8824004521572161416[6] = 0;
|
||||
out_8824004521572161416[7] = 0;
|
||||
out_8824004521572161416[8] = 0;
|
||||
out_8824004521572161416[9] = 0;
|
||||
out_8824004521572161416[10] = 1;
|
||||
out_8824004521572161416[11] = 0;
|
||||
out_8824004521572161416[12] = 0;
|
||||
out_8824004521572161416[13] = 0;
|
||||
out_8824004521572161416[14] = 0;
|
||||
out_8824004521572161416[15] = 0;
|
||||
out_8824004521572161416[16] = 0;
|
||||
out_8824004521572161416[17] = 0;
|
||||
out_8824004521572161416[18] = 0;
|
||||
out_8824004521572161416[19] = 0;
|
||||
out_8824004521572161416[20] = 1;
|
||||
out_8824004521572161416[21] = 0;
|
||||
out_8824004521572161416[22] = 0;
|
||||
out_8824004521572161416[23] = 0;
|
||||
out_8824004521572161416[24] = 0;
|
||||
out_8824004521572161416[25] = 0;
|
||||
out_8824004521572161416[26] = 0;
|
||||
out_8824004521572161416[27] = 0;
|
||||
out_8824004521572161416[28] = 0;
|
||||
out_8824004521572161416[29] = 0;
|
||||
out_8824004521572161416[30] = 1;
|
||||
out_8824004521572161416[31] = 0;
|
||||
out_8824004521572161416[32] = 0;
|
||||
out_8824004521572161416[33] = 0;
|
||||
out_8824004521572161416[34] = 0;
|
||||
out_8824004521572161416[35] = 0;
|
||||
out_8824004521572161416[36] = 0;
|
||||
out_8824004521572161416[37] = 0;
|
||||
out_8824004521572161416[38] = 0;
|
||||
out_8824004521572161416[39] = 0;
|
||||
out_8824004521572161416[40] = 1;
|
||||
out_8824004521572161416[41] = 0;
|
||||
out_8824004521572161416[42] = 0;
|
||||
out_8824004521572161416[43] = 0;
|
||||
out_8824004521572161416[44] = 0;
|
||||
out_8824004521572161416[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_8824004521572161416[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_8824004521572161416[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_8824004521572161416[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_8824004521572161416[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_8824004521572161416[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_8824004521572161416[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_8824004521572161416[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_8824004521572161416[53] = -9.8000000000000007*dt;
|
||||
out_8824004521572161416[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_8824004521572161416[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_8824004521572161416[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_8824004521572161416[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_8824004521572161416[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_8824004521572161416[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_8824004521572161416[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_8824004521572161416[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_8824004521572161416[62] = 0;
|
||||
out_8824004521572161416[63] = 0;
|
||||
out_8824004521572161416[64] = 0;
|
||||
out_8824004521572161416[65] = 0;
|
||||
out_8824004521572161416[66] = 0;
|
||||
out_8824004521572161416[67] = 0;
|
||||
out_8824004521572161416[68] = 0;
|
||||
out_8824004521572161416[69] = 0;
|
||||
out_8824004521572161416[70] = 1;
|
||||
out_8824004521572161416[71] = 0;
|
||||
out_8824004521572161416[72] = 0;
|
||||
out_8824004521572161416[73] = 0;
|
||||
out_8824004521572161416[74] = 0;
|
||||
out_8824004521572161416[75] = 0;
|
||||
out_8824004521572161416[76] = 0;
|
||||
out_8824004521572161416[77] = 0;
|
||||
out_8824004521572161416[78] = 0;
|
||||
out_8824004521572161416[79] = 0;
|
||||
out_8824004521572161416[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_7531965082275181187) {
|
||||
out_7531965082275181187[0] = 1;
|
||||
out_7531965082275181187[1] = 0;
|
||||
out_7531965082275181187[2] = 0;
|
||||
out_7531965082275181187[3] = 0;
|
||||
out_7531965082275181187[4] = 0;
|
||||
out_7531965082275181187[5] = 0;
|
||||
out_7531965082275181187[6] = 0;
|
||||
out_7531965082275181187[7] = 0;
|
||||
out_7531965082275181187[8] = 0;
|
||||
out_7531965082275181187[9] = 0;
|
||||
out_7531965082275181187[10] = 1;
|
||||
out_7531965082275181187[11] = 0;
|
||||
out_7531965082275181187[12] = 0;
|
||||
out_7531965082275181187[13] = 0;
|
||||
out_7531965082275181187[14] = 0;
|
||||
out_7531965082275181187[15] = 0;
|
||||
out_7531965082275181187[16] = 0;
|
||||
out_7531965082275181187[17] = 0;
|
||||
out_7531965082275181187[18] = 0;
|
||||
out_7531965082275181187[19] = 0;
|
||||
out_7531965082275181187[20] = 1;
|
||||
out_7531965082275181187[21] = 0;
|
||||
out_7531965082275181187[22] = 0;
|
||||
out_7531965082275181187[23] = 0;
|
||||
out_7531965082275181187[24] = 0;
|
||||
out_7531965082275181187[25] = 0;
|
||||
out_7531965082275181187[26] = 0;
|
||||
out_7531965082275181187[27] = 0;
|
||||
out_7531965082275181187[28] = 0;
|
||||
out_7531965082275181187[29] = 0;
|
||||
out_7531965082275181187[30] = 1;
|
||||
out_7531965082275181187[31] = 0;
|
||||
out_7531965082275181187[32] = 0;
|
||||
out_7531965082275181187[33] = 0;
|
||||
out_7531965082275181187[34] = 0;
|
||||
out_7531965082275181187[35] = 0;
|
||||
out_7531965082275181187[36] = 0;
|
||||
out_7531965082275181187[37] = 0;
|
||||
out_7531965082275181187[38] = 0;
|
||||
out_7531965082275181187[39] = 0;
|
||||
out_7531965082275181187[40] = 1;
|
||||
out_7531965082275181187[41] = 0;
|
||||
out_7531965082275181187[42] = 0;
|
||||
out_7531965082275181187[43] = 0;
|
||||
out_7531965082275181187[44] = 0;
|
||||
out_7531965082275181187[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_7531965082275181187[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_7531965082275181187[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7531965082275181187[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7531965082275181187[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_7531965082275181187[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_7531965082275181187[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_7531965082275181187[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7531965082275181187[53] = -9.8000000000000007*dt;
|
||||
out_7531965082275181187[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_7531965082275181187[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_7531965082275181187[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7531965082275181187[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7531965082275181187[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_7531965082275181187[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_7531965082275181187[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_7531965082275181187[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7531965082275181187[62] = 0;
|
||||
out_7531965082275181187[63] = 0;
|
||||
out_7531965082275181187[64] = 0;
|
||||
out_7531965082275181187[65] = 0;
|
||||
out_7531965082275181187[66] = 0;
|
||||
out_7531965082275181187[67] = 0;
|
||||
out_7531965082275181187[68] = 0;
|
||||
out_7531965082275181187[69] = 0;
|
||||
out_7531965082275181187[70] = 1;
|
||||
out_7531965082275181187[71] = 0;
|
||||
out_7531965082275181187[72] = 0;
|
||||
out_7531965082275181187[73] = 0;
|
||||
out_7531965082275181187[74] = 0;
|
||||
out_7531965082275181187[75] = 0;
|
||||
out_7531965082275181187[76] = 0;
|
||||
out_7531965082275181187[77] = 0;
|
||||
out_7531965082275181187[78] = 0;
|
||||
out_7531965082275181187[79] = 0;
|
||||
out_7531965082275181187[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_1789446864250922111) {
|
||||
out_1789446864250922111[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_7956124124599872411) {
|
||||
out_7956124124599872411[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_9129886545195087354) {
|
||||
out_9129886545195087354[0] = 0;
|
||||
out_9129886545195087354[1] = 0;
|
||||
out_9129886545195087354[2] = 0;
|
||||
out_9129886545195087354[3] = 0;
|
||||
out_9129886545195087354[4] = 0;
|
||||
out_9129886545195087354[5] = 0;
|
||||
out_9129886545195087354[6] = 1;
|
||||
out_9129886545195087354[7] = 0;
|
||||
out_9129886545195087354[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_1152149855673758191) {
|
||||
out_1152149855673758191[0] = 0;
|
||||
out_1152149855673758191[1] = 0;
|
||||
out_1152149855673758191[2] = 0;
|
||||
out_1152149855673758191[3] = 0;
|
||||
out_1152149855673758191[4] = 0;
|
||||
out_1152149855673758191[5] = 0;
|
||||
out_1152149855673758191[6] = 1;
|
||||
out_1152149855673758191[7] = 0;
|
||||
out_1152149855673758191[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_3299160385053087312) {
|
||||
out_3299160385053087312[0] = state[4];
|
||||
out_3299160385053087312[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_8831563141601233713) {
|
||||
out_8831563141601233713[0] = state[4];
|
||||
out_8831563141601233713[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_7144207929508964696) {
|
||||
out_7144207929508964696[0] = 0;
|
||||
out_7144207929508964696[1] = 0;
|
||||
out_7144207929508964696[2] = 0;
|
||||
out_7144207929508964696[3] = 0;
|
||||
out_7144207929508964696[4] = 1;
|
||||
out_7144207929508964696[5] = 0;
|
||||
out_7144207929508964696[6] = 0;
|
||||
out_7144207929508964696[7] = 0;
|
||||
out_7144207929508964696[8] = 0;
|
||||
out_7144207929508964696[9] = 0;
|
||||
out_7144207929508964696[10] = 0;
|
||||
out_7144207929508964696[11] = 0;
|
||||
out_7144207929508964696[12] = 0;
|
||||
out_7144207929508964696[13] = 0;
|
||||
out_7144207929508964696[14] = 1;
|
||||
out_7144207929508964696[15] = 0;
|
||||
out_7144207929508964696[16] = 0;
|
||||
out_7144207929508964696[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_1020499743331741375) {
|
||||
out_1020499743331741375[0] = 0;
|
||||
out_1020499743331741375[1] = 0;
|
||||
out_1020499743331741375[2] = 0;
|
||||
out_1020499743331741375[3] = 0;
|
||||
out_1020499743331741375[4] = 1;
|
||||
out_1020499743331741375[5] = 0;
|
||||
out_1020499743331741375[6] = 0;
|
||||
out_1020499743331741375[7] = 0;
|
||||
out_1020499743331741375[8] = 0;
|
||||
out_1020499743331741375[9] = 0;
|
||||
out_1020499743331741375[10] = 0;
|
||||
out_1020499743331741375[11] = 0;
|
||||
out_1020499743331741375[12] = 0;
|
||||
out_1020499743331741375[13] = 0;
|
||||
out_1020499743331741375[14] = 1;
|
||||
out_1020499743331741375[15] = 0;
|
||||
out_1020499743331741375[16] = 0;
|
||||
out_1020499743331741375[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_7348270420432929071) {
|
||||
out_7348270420432929071[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_6628936472681177553) {
|
||||
out_6628936472681177553[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_6611553586687838727) {
|
||||
out_6611553586687838727[0] = 0;
|
||||
out_6611553586687838727[1] = 0;
|
||||
out_6611553586687838727[2] = 0;
|
||||
out_6611553586687838727[3] = 0;
|
||||
out_6611553586687838727[4] = 1;
|
||||
out_6611553586687838727[5] = 0;
|
||||
out_6611553586687838727[6] = 0;
|
||||
out_6611553586687838727[7] = 0;
|
||||
out_6611553586687838727[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_3375546474453850007) {
|
||||
out_3375546474453850007[0] = 0;
|
||||
out_3375546474453850007[1] = 0;
|
||||
out_3375546474453850007[2] = 0;
|
||||
out_3375546474453850007[3] = 0;
|
||||
out_3375546474453850007[4] = 1;
|
||||
out_3375546474453850007[5] = 0;
|
||||
out_3375546474453850007[6] = 0;
|
||||
out_3375546474453850007[7] = 0;
|
||||
out_3375546474453850007[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_6096800385615776900) {
|
||||
out_6096800385615776900[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_3816384369411324170) {
|
||||
out_3816384369411324170[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_5575354209640408038) {
|
||||
out_5575354209640408038[0] = 0;
|
||||
out_5575354209640408038[1] = 0;
|
||||
out_5575354209640408038[2] = 0;
|
||||
out_5575354209640408038[3] = 0;
|
||||
out_5575354209640408038[4] = 0;
|
||||
out_5575354209640408038[5] = 0;
|
||||
out_5575354209640408038[6] = 0;
|
||||
out_5575354209640408038[7] = 1;
|
||||
out_5575354209640408038[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_2589353463200298033) {
|
||||
out_2589353463200298033[0] = 0;
|
||||
out_2589353463200298033[1] = 0;
|
||||
out_2589353463200298033[2] = 0;
|
||||
out_2589353463200298033[3] = 0;
|
||||
out_2589353463200298033[4] = 0;
|
||||
out_2589353463200298033[5] = 0;
|
||||
out_2589353463200298033[6] = 0;
|
||||
out_2589353463200298033[7] = 1;
|
||||
out_2589353463200298033[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_1247339187193410574) {
|
||||
out_1247339187193410574[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_7012614271698209976) {
|
||||
out_7012614271698209976[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_2614397886586431153) {
|
||||
out_2614397886586431153[0] = 0;
|
||||
out_2614397886586431153[1] = 0;
|
||||
out_2614397886586431153[2] = 0;
|
||||
out_2614397886586431153[3] = 1;
|
||||
out_2614397886586431153[4] = 0;
|
||||
out_2614397886586431153[5] = 0;
|
||||
out_2614397886586431153[6] = 0;
|
||||
out_2614397886586431153[7] = 0;
|
||||
out_2614397886586431153[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_5550309786254274918) {
|
||||
out_5550309786254274918[0] = 0;
|
||||
out_5550309786254274918[1] = 0;
|
||||
out_5550309786254274918[2] = 0;
|
||||
out_5550309786254274918[3] = 1;
|
||||
out_5550309786254274918[4] = 0;
|
||||
out_5550309786254274918[5] = 0;
|
||||
out_5550309786254274918[6] = 0;
|
||||
out_5550309786254274918[7] = 0;
|
||||
out_5550309786254274918[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_1705540549351972236) {
|
||||
out_1705540549351972236[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_1627263205020331908) {
|
||||
out_1627263205020331908[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_5299392542701248248) {
|
||||
out_5299392542701248248[0] = 0;
|
||||
out_5299392542701248248[1] = 1;
|
||||
out_5299392542701248248[2] = 0;
|
||||
out_5299392542701248248[3] = 0;
|
||||
out_5299392542701248248[4] = 0;
|
||||
out_5299392542701248248[5] = 0;
|
||||
out_5299392542701248248[6] = 0;
|
||||
out_5299392542701248248[7] = 0;
|
||||
out_5299392542701248248[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_2865315130139457823) {
|
||||
out_2865315130139457823[0] = 0;
|
||||
out_2865315130139457823[1] = 1;
|
||||
out_2865315130139457823[2] = 0;
|
||||
out_2865315130139457823[3] = 0;
|
||||
out_2865315130139457823[4] = 0;
|
||||
out_2865315130139457823[5] = 0;
|
||||
out_2865315130139457823[6] = 0;
|
||||
out_2865315130139457823[7] = 0;
|
||||
out_2865315130139457823[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_4170451758575142464) {
|
||||
out_4170451758575142464[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_7994698365033508088) {
|
||||
out_7994698365033508088[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_7263022814266574499) {
|
||||
out_7263022814266574499[0] = 1;
|
||||
out_7263022814266574499[1] = 0;
|
||||
out_7263022814266574499[2] = 0;
|
||||
out_7263022814266574499[3] = 0;
|
||||
out_7263022814266574499[4] = 0;
|
||||
out_7263022814266574499[5] = 0;
|
||||
out_7263022814266574499[6] = 0;
|
||||
out_7263022814266574499[7] = 0;
|
||||
out_7263022814266574499[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_901684858574131572) {
|
||||
out_901684858574131572[0] = 1;
|
||||
out_901684858574131572[1] = 0;
|
||||
out_901684858574131572[2] = 0;
|
||||
out_901684858574131572[3] = 0;
|
||||
out_901684858574131572[4] = 0;
|
||||
out_901684858574131572[5] = 0;
|
||||
out_901684858574131572[6] = 0;
|
||||
out_901684858574131572[7] = 0;
|
||||
out_901684858574131572[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_1514252801966416222) {
|
||||
out_1514252801966416222[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_988808959918839917) {
|
||||
out_988808959918839917[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_4949146107407056562) {
|
||||
out_4949146107407056562[0] = 0;
|
||||
out_4949146107407056562[1] = 0;
|
||||
out_4949146107407056562[2] = 0;
|
||||
out_4949146107407056562[3] = 0;
|
||||
out_4949146107407056562[4] = 0;
|
||||
out_4949146107407056562[5] = 0;
|
||||
out_4949146107407056562[6] = 0;
|
||||
out_4949146107407056562[7] = 0;
|
||||
out_4949146107407056562[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_3215561565433649509) {
|
||||
out_3215561565433649509[0] = 0;
|
||||
out_3215561565433649509[1] = 0;
|
||||
out_3215561565433649509[2] = 0;
|
||||
out_3215561565433649509[3] = 0;
|
||||
out_3215561565433649509[4] = 0;
|
||||
out_3215561565433649509[5] = 0;
|
||||
out_3215561565433649509[6] = 0;
|
||||
out_3215561565433649509[7] = 0;
|
||||
out_3215561565433649509[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_5634330020296848070) {
|
||||
err_fun(nom_x, delta_x, out_5634330020296848070);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_9008082079438066370) {
|
||||
err_fun(nom_x, delta_x, out_9008082079438066370);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_1061559027503315281) {
|
||||
inv_err_fun(nom_x, true_x, out_1061559027503315281);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7705390806256602072) {
|
||||
inv_err_fun(nom_x, true_x, out_7705390806256602072);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_1567569310651922730) {
|
||||
H_mod_fun(state, out_1567569310651922730);
|
||||
void car_H_mod_fun(double *state, double *out_4044238657628669408) {
|
||||
H_mod_fun(state, out_4044238657628669408);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_1208381054458531187) {
|
||||
f_fun(state, dt, out_1208381054458531187);
|
||||
void car_f_fun(double *state, double dt, double *out_6587028471138044914) {
|
||||
f_fun(state, dt, out_6587028471138044914);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_8824004521572161416) {
|
||||
F_fun(state, dt, out_8824004521572161416);
|
||||
void car_F_fun(double *state, double dt, double *out_7531965082275181187) {
|
||||
F_fun(state, dt, out_7531965082275181187);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_1789446864250922111) {
|
||||
h_25(state, unused, out_1789446864250922111);
|
||||
void car_h_25(double *state, double *unused, double *out_7956124124599872411) {
|
||||
h_25(state, unused, out_7956124124599872411);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_9129886545195087354) {
|
||||
H_25(state, unused, out_9129886545195087354);
|
||||
void car_H_25(double *state, double *unused, double *out_1152149855673758191) {
|
||||
H_25(state, unused, out_1152149855673758191);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_3299160385053087312) {
|
||||
h_24(state, unused, out_3299160385053087312);
|
||||
void car_h_24(double *state, double *unused, double *out_8831563141601233713) {
|
||||
h_24(state, unused, out_8831563141601233713);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_7144207929508964696) {
|
||||
H_24(state, unused, out_7144207929508964696);
|
||||
void car_H_24(double *state, double *unused, double *out_1020499743331741375) {
|
||||
H_24(state, unused, out_1020499743331741375);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_7348270420432929071) {
|
||||
h_30(state, unused, out_7348270420432929071);
|
||||
void car_h_30(double *state, double *unused, double *out_6628936472681177553) {
|
||||
h_30(state, unused, out_6628936472681177553);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_6611553586687838727) {
|
||||
H_30(state, unused, out_6611553586687838727);
|
||||
void car_H_30(double *state, double *unused, double *out_3375546474453850007) {
|
||||
H_30(state, unused, out_3375546474453850007);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_6096800385615776900) {
|
||||
h_26(state, unused, out_6096800385615776900);
|
||||
void car_h_26(double *state, double *unused, double *out_3816384369411324170) {
|
||||
h_26(state, unused, out_3816384369411324170);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_5575354209640408038) {
|
||||
H_26(state, unused, out_5575354209640408038);
|
||||
void car_H_26(double *state, double *unused, double *out_2589353463200298033) {
|
||||
H_26(state, unused, out_2589353463200298033);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_1247339187193410574) {
|
||||
h_27(state, unused, out_1247339187193410574);
|
||||
void car_h_27(double *state, double *unused, double *out_7012614271698209976) {
|
||||
h_27(state, unused, out_7012614271698209976);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_2614397886586431153) {
|
||||
H_27(state, unused, out_2614397886586431153);
|
||||
void car_H_27(double *state, double *unused, double *out_5550309786254274918) {
|
||||
H_27(state, unused, out_5550309786254274918);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_1705540549351972236) {
|
||||
h_29(state, unused, out_1705540549351972236);
|
||||
void car_h_29(double *state, double *unused, double *out_1627263205020331908) {
|
||||
h_29(state, unused, out_1627263205020331908);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_5299392542701248248) {
|
||||
H_29(state, unused, out_5299392542701248248);
|
||||
void car_H_29(double *state, double *unused, double *out_2865315130139457823) {
|
||||
H_29(state, unused, out_2865315130139457823);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_4170451758575142464) {
|
||||
h_28(state, unused, out_4170451758575142464);
|
||||
void car_h_28(double *state, double *unused, double *out_7994698365033508088) {
|
||||
h_28(state, unused, out_7994698365033508088);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_7263022814266574499) {
|
||||
H_28(state, unused, out_7263022814266574499);
|
||||
void car_H_28(double *state, double *unused, double *out_901684858574131572) {
|
||||
H_28(state, unused, out_901684858574131572);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_1514252801966416222) {
|
||||
h_31(state, unused, out_1514252801966416222);
|
||||
void car_h_31(double *state, double *unused, double *out_988808959918839917) {
|
||||
h_31(state, unused, out_988808959918839917);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_4949146107407056562) {
|
||||
H_31(state, unused, out_4949146107407056562);
|
||||
void car_H_31(double *state, double *unused, double *out_3215561565433649509) {
|
||||
H_31(state, unused, out_3215561565433649509);
|
||||
}
|
||||
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