mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-07-26 12:22:04 +08:00
DM ruh roh
This commit is contained in:
firestar5683
Binary file not shown.
@@ -2179,6 +2179,7 @@ struct DriverStateV2 {
|
||||
rightBlinkProb @8 :Float32;
|
||||
sunglassesProb @9 :Float32;
|
||||
phoneProb @13 :Float32;
|
||||
sleepProb @14 :Float32;
|
||||
notReadyProbDEPRECATED @12 :List(Float32);
|
||||
occludedProbDEPRECATED @10 :Float32;
|
||||
readyProbDEPRECATED @11 :List(Float32);
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-01bd291b-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-8e8225b6-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
DEV-01bd291b-DEBUG
|
||||
DEV-8e8225b6-DEBUG
|
||||
@@ -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_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 err_fun(double *nom_x, double *delta_x, double *out_2503888728252844227) {
|
||||
out_2503888728252844227[0] = delta_x[0] + nom_x[0];
|
||||
out_2503888728252844227[1] = delta_x[1] + nom_x[1];
|
||||
out_2503888728252844227[2] = delta_x[2] + nom_x[2];
|
||||
out_2503888728252844227[3] = delta_x[3] + nom_x[3];
|
||||
out_2503888728252844227[4] = delta_x[4] + nom_x[4];
|
||||
out_2503888728252844227[5] = delta_x[5] + nom_x[5];
|
||||
out_2503888728252844227[6] = delta_x[6] + nom_x[6];
|
||||
out_2503888728252844227[7] = delta_x[7] + nom_x[7];
|
||||
out_2503888728252844227[8] = delta_x[8] + nom_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 inv_err_fun(double *nom_x, double *true_x, double *out_3671647430606809601) {
|
||||
out_3671647430606809601[0] = -nom_x[0] + true_x[0];
|
||||
out_3671647430606809601[1] = -nom_x[1] + true_x[1];
|
||||
out_3671647430606809601[2] = -nom_x[2] + true_x[2];
|
||||
out_3671647430606809601[3] = -nom_x[3] + true_x[3];
|
||||
out_3671647430606809601[4] = -nom_x[4] + true_x[4];
|
||||
out_3671647430606809601[5] = -nom_x[5] + true_x[5];
|
||||
out_3671647430606809601[6] = -nom_x[6] + true_x[6];
|
||||
out_3671647430606809601[7] = -nom_x[7] + true_x[7];
|
||||
out_3671647430606809601[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
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 H_mod_fun(double *state, double *out_3172612914634759799) {
|
||||
out_3172612914634759799[0] = 1.0;
|
||||
out_3172612914634759799[1] = 0.0;
|
||||
out_3172612914634759799[2] = 0.0;
|
||||
out_3172612914634759799[3] = 0.0;
|
||||
out_3172612914634759799[4] = 0.0;
|
||||
out_3172612914634759799[5] = 0.0;
|
||||
out_3172612914634759799[6] = 0.0;
|
||||
out_3172612914634759799[7] = 0.0;
|
||||
out_3172612914634759799[8] = 0.0;
|
||||
out_3172612914634759799[9] = 0.0;
|
||||
out_3172612914634759799[10] = 1.0;
|
||||
out_3172612914634759799[11] = 0.0;
|
||||
out_3172612914634759799[12] = 0.0;
|
||||
out_3172612914634759799[13] = 0.0;
|
||||
out_3172612914634759799[14] = 0.0;
|
||||
out_3172612914634759799[15] = 0.0;
|
||||
out_3172612914634759799[16] = 0.0;
|
||||
out_3172612914634759799[17] = 0.0;
|
||||
out_3172612914634759799[18] = 0.0;
|
||||
out_3172612914634759799[19] = 0.0;
|
||||
out_3172612914634759799[20] = 1.0;
|
||||
out_3172612914634759799[21] = 0.0;
|
||||
out_3172612914634759799[22] = 0.0;
|
||||
out_3172612914634759799[23] = 0.0;
|
||||
out_3172612914634759799[24] = 0.0;
|
||||
out_3172612914634759799[25] = 0.0;
|
||||
out_3172612914634759799[26] = 0.0;
|
||||
out_3172612914634759799[27] = 0.0;
|
||||
out_3172612914634759799[28] = 0.0;
|
||||
out_3172612914634759799[29] = 0.0;
|
||||
out_3172612914634759799[30] = 1.0;
|
||||
out_3172612914634759799[31] = 0.0;
|
||||
out_3172612914634759799[32] = 0.0;
|
||||
out_3172612914634759799[33] = 0.0;
|
||||
out_3172612914634759799[34] = 0.0;
|
||||
out_3172612914634759799[35] = 0.0;
|
||||
out_3172612914634759799[36] = 0.0;
|
||||
out_3172612914634759799[37] = 0.0;
|
||||
out_3172612914634759799[38] = 0.0;
|
||||
out_3172612914634759799[39] = 0.0;
|
||||
out_3172612914634759799[40] = 1.0;
|
||||
out_3172612914634759799[41] = 0.0;
|
||||
out_3172612914634759799[42] = 0.0;
|
||||
out_3172612914634759799[43] = 0.0;
|
||||
out_3172612914634759799[44] = 0.0;
|
||||
out_3172612914634759799[45] = 0.0;
|
||||
out_3172612914634759799[46] = 0.0;
|
||||
out_3172612914634759799[47] = 0.0;
|
||||
out_3172612914634759799[48] = 0.0;
|
||||
out_3172612914634759799[49] = 0.0;
|
||||
out_3172612914634759799[50] = 1.0;
|
||||
out_3172612914634759799[51] = 0.0;
|
||||
out_3172612914634759799[52] = 0.0;
|
||||
out_3172612914634759799[53] = 0.0;
|
||||
out_3172612914634759799[54] = 0.0;
|
||||
out_3172612914634759799[55] = 0.0;
|
||||
out_3172612914634759799[56] = 0.0;
|
||||
out_3172612914634759799[57] = 0.0;
|
||||
out_3172612914634759799[58] = 0.0;
|
||||
out_3172612914634759799[59] = 0.0;
|
||||
out_3172612914634759799[60] = 1.0;
|
||||
out_3172612914634759799[61] = 0.0;
|
||||
out_3172612914634759799[62] = 0.0;
|
||||
out_3172612914634759799[63] = 0.0;
|
||||
out_3172612914634759799[64] = 0.0;
|
||||
out_3172612914634759799[65] = 0.0;
|
||||
out_3172612914634759799[66] = 0.0;
|
||||
out_3172612914634759799[67] = 0.0;
|
||||
out_3172612914634759799[68] = 0.0;
|
||||
out_3172612914634759799[69] = 0.0;
|
||||
out_3172612914634759799[70] = 1.0;
|
||||
out_3172612914634759799[71] = 0.0;
|
||||
out_3172612914634759799[72] = 0.0;
|
||||
out_3172612914634759799[73] = 0.0;
|
||||
out_3172612914634759799[74] = 0.0;
|
||||
out_3172612914634759799[75] = 0.0;
|
||||
out_3172612914634759799[76] = 0.0;
|
||||
out_3172612914634759799[77] = 0.0;
|
||||
out_3172612914634759799[78] = 0.0;
|
||||
out_3172612914634759799[79] = 0.0;
|
||||
out_3172612914634759799[80] = 1.0;
|
||||
}
|
||||
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_7316960543418142517) {
|
||||
out_7316960543418142517[0] = state[0];
|
||||
out_7316960543418142517[1] = state[1];
|
||||
out_7316960543418142517[2] = state[2];
|
||||
out_7316960543418142517[3] = state[3];
|
||||
out_7316960543418142517[4] = state[4];
|
||||
out_7316960543418142517[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_7316960543418142517[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_7316960543418142517[7] = state[7];
|
||||
out_7316960543418142517[8] = state[8];
|
||||
}
|
||||
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 F_fun(double *state, double dt, double *out_1492910262171570578) {
|
||||
out_1492910262171570578[0] = 1;
|
||||
out_1492910262171570578[1] = 0;
|
||||
out_1492910262171570578[2] = 0;
|
||||
out_1492910262171570578[3] = 0;
|
||||
out_1492910262171570578[4] = 0;
|
||||
out_1492910262171570578[5] = 0;
|
||||
out_1492910262171570578[6] = 0;
|
||||
out_1492910262171570578[7] = 0;
|
||||
out_1492910262171570578[8] = 0;
|
||||
out_1492910262171570578[9] = 0;
|
||||
out_1492910262171570578[10] = 1;
|
||||
out_1492910262171570578[11] = 0;
|
||||
out_1492910262171570578[12] = 0;
|
||||
out_1492910262171570578[13] = 0;
|
||||
out_1492910262171570578[14] = 0;
|
||||
out_1492910262171570578[15] = 0;
|
||||
out_1492910262171570578[16] = 0;
|
||||
out_1492910262171570578[17] = 0;
|
||||
out_1492910262171570578[18] = 0;
|
||||
out_1492910262171570578[19] = 0;
|
||||
out_1492910262171570578[20] = 1;
|
||||
out_1492910262171570578[21] = 0;
|
||||
out_1492910262171570578[22] = 0;
|
||||
out_1492910262171570578[23] = 0;
|
||||
out_1492910262171570578[24] = 0;
|
||||
out_1492910262171570578[25] = 0;
|
||||
out_1492910262171570578[26] = 0;
|
||||
out_1492910262171570578[27] = 0;
|
||||
out_1492910262171570578[28] = 0;
|
||||
out_1492910262171570578[29] = 0;
|
||||
out_1492910262171570578[30] = 1;
|
||||
out_1492910262171570578[31] = 0;
|
||||
out_1492910262171570578[32] = 0;
|
||||
out_1492910262171570578[33] = 0;
|
||||
out_1492910262171570578[34] = 0;
|
||||
out_1492910262171570578[35] = 0;
|
||||
out_1492910262171570578[36] = 0;
|
||||
out_1492910262171570578[37] = 0;
|
||||
out_1492910262171570578[38] = 0;
|
||||
out_1492910262171570578[39] = 0;
|
||||
out_1492910262171570578[40] = 1;
|
||||
out_1492910262171570578[41] = 0;
|
||||
out_1492910262171570578[42] = 0;
|
||||
out_1492910262171570578[43] = 0;
|
||||
out_1492910262171570578[44] = 0;
|
||||
out_1492910262171570578[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_1492910262171570578[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_1492910262171570578[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1492910262171570578[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1492910262171570578[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_1492910262171570578[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_1492910262171570578[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_1492910262171570578[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1492910262171570578[53] = -9.8100000000000005*dt;
|
||||
out_1492910262171570578[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_1492910262171570578[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_1492910262171570578[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1492910262171570578[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1492910262171570578[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_1492910262171570578[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_1492910262171570578[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_1492910262171570578[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1492910262171570578[62] = 0;
|
||||
out_1492910262171570578[63] = 0;
|
||||
out_1492910262171570578[64] = 0;
|
||||
out_1492910262171570578[65] = 0;
|
||||
out_1492910262171570578[66] = 0;
|
||||
out_1492910262171570578[67] = 0;
|
||||
out_1492910262171570578[68] = 0;
|
||||
out_1492910262171570578[69] = 0;
|
||||
out_1492910262171570578[70] = 1;
|
||||
out_1492910262171570578[71] = 0;
|
||||
out_1492910262171570578[72] = 0;
|
||||
out_1492910262171570578[73] = 0;
|
||||
out_1492910262171570578[74] = 0;
|
||||
out_1492910262171570578[75] = 0;
|
||||
out_1492910262171570578[76] = 0;
|
||||
out_1492910262171570578[77] = 0;
|
||||
out_1492910262171570578[78] = 0;
|
||||
out_1492910262171570578[79] = 0;
|
||||
out_1492910262171570578[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_2044795451145406261) {
|
||||
out_2044795451145406261[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_595544251427276973) {
|
||||
out_595544251427276973[0] = state[6];
|
||||
}
|
||||
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_25(double *state, double *unused, double *out_350435724010605349) {
|
||||
out_350435724010605349[0] = 0;
|
||||
out_350435724010605349[1] = 0;
|
||||
out_350435724010605349[2] = 0;
|
||||
out_350435724010605349[3] = 0;
|
||||
out_350435724010605349[4] = 0;
|
||||
out_350435724010605349[5] = 0;
|
||||
out_350435724010605349[6] = 1;
|
||||
out_350435724010605349[7] = 0;
|
||||
out_350435724010605349[8] = 0;
|
||||
}
|
||||
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_2954294579100300160) {
|
||||
out_2954294579100300160[0] = state[4];
|
||||
out_2954294579100300160[1] = state[5];
|
||||
}
|
||||
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_24(double *state, double *unused, double *out_3207489874791717787) {
|
||||
out_3207489874791717787[0] = 0;
|
||||
out_3207489874791717787[1] = 0;
|
||||
out_3207489874791717787[2] = 0;
|
||||
out_3207489874791717787[3] = 0;
|
||||
out_3207489874791717787[4] = 1;
|
||||
out_3207489874791717787[5] = 0;
|
||||
out_3207489874791717787[6] = 0;
|
||||
out_3207489874791717787[7] = 0;
|
||||
out_3207489874791717787[8] = 0;
|
||||
out_3207489874791717787[9] = 0;
|
||||
out_3207489874791717787[10] = 0;
|
||||
out_3207489874791717787[11] = 0;
|
||||
out_3207489874791717787[12] = 0;
|
||||
out_3207489874791717787[13] = 0;
|
||||
out_3207489874791717787[14] = 1;
|
||||
out_3207489874791717787[15] = 0;
|
||||
out_3207489874791717787[16] = 0;
|
||||
out_3207489874791717787[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_8638683365743472914) {
|
||||
out_8638683365743472914[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_4457227727855778625) {
|
||||
out_4457227727855778625[0] = state[4];
|
||||
}
|
||||
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_30(double *state, double *unused, double *out_2167897234496643278) {
|
||||
out_2167897234496643278[0] = 0;
|
||||
out_2167897234496643278[1] = 0;
|
||||
out_2167897234496643278[2] = 0;
|
||||
out_2167897234496643278[3] = 0;
|
||||
out_2167897234496643278[4] = 1;
|
||||
out_2167897234496643278[5] = 0;
|
||||
out_2167897234496643278[6] = 0;
|
||||
out_2167897234496643278[7] = 0;
|
||||
out_2167897234496643278[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_989338192432138572) {
|
||||
out_989338192432138572[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_6778606859599551480) {
|
||||
out_6778606859599551480[0] = state[7];
|
||||
}
|
||||
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_26(double *state, double *unused, double *out_4091939042884661573) {
|
||||
out_4091939042884661573[0] = 0;
|
||||
out_4091939042884661573[1] = 0;
|
||||
out_4091939042884661573[2] = 0;
|
||||
out_4091939042884661573[3] = 0;
|
||||
out_4091939042884661573[4] = 0;
|
||||
out_4091939042884661573[5] = 0;
|
||||
out_4091939042884661573[6] = 0;
|
||||
out_4091939042884661573[7] = 1;
|
||||
out_4091939042884661573[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_7375345222001897423) {
|
||||
out_7375345222001897423[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_2871073069040329107) {
|
||||
out_2871073069040329107[0] = state[3];
|
||||
}
|
||||
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_27(double *state, double *unused, double *out_4391491305680586495) {
|
||||
out_4391491305680586495[0] = 0;
|
||||
out_4391491305680586495[1] = 0;
|
||||
out_4391491305680586495[2] = 0;
|
||||
out_4391491305680586495[3] = 1;
|
||||
out_4391491305680586495[4] = 0;
|
||||
out_4391491305680586495[5] = 0;
|
||||
out_4391491305680586495[6] = 0;
|
||||
out_4391491305680586495[7] = 0;
|
||||
out_4391491305680586495[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_4806387440305233057) {
|
||||
out_4806387440305233057[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_8987843078192927346) {
|
||||
out_8987843078192927346[0] = state[1];
|
||||
}
|
||||
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_29(double *state, double *unused, double *out_2678128578811035462) {
|
||||
out_2678128578811035462[0] = 0;
|
||||
out_2678128578811035462[1] = 1;
|
||||
out_2678128578811035462[2] = 0;
|
||||
out_2678128578811035462[3] = 0;
|
||||
out_2678128578811035462[4] = 0;
|
||||
out_2678128578811035462[5] = 0;
|
||||
out_2678128578811035462[6] = 0;
|
||||
out_2678128578811035462[7] = 0;
|
||||
out_2678128578811035462[8] = 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_3384751652918718147) {
|
||||
out_3384751652918718147[0] = state[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_28(double *state, double *unused, double *out_2404270438258495112) {
|
||||
out_2404270438258495112[0] = 1;
|
||||
out_2404270438258495112[1] = 0;
|
||||
out_2404270438258495112[2] = 0;
|
||||
out_2404270438258495112[3] = 0;
|
||||
out_2404270438258495112[4] = 0;
|
||||
out_2404270438258495112[5] = 0;
|
||||
out_2404270438258495112[6] = 0;
|
||||
out_2404270438258495112[7] = 0;
|
||||
out_2404270438258495112[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_2208042107618306896) {
|
||||
out_2208042107618306896[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_1973413530269387393) {
|
||||
out_1973413530269387393[0] = state[8];
|
||||
}
|
||||
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;
|
||||
void H_31(double *state, double *unused, double *out_4718147145118013049) {
|
||||
out_4718147145118013049[0] = 0;
|
||||
out_4718147145118013049[1] = 0;
|
||||
out_4718147145118013049[2] = 0;
|
||||
out_4718147145118013049[3] = 0;
|
||||
out_4718147145118013049[4] = 0;
|
||||
out_4718147145118013049[5] = 0;
|
||||
out_4718147145118013049[6] = 0;
|
||||
out_4718147145118013049[7] = 0;
|
||||
out_4718147145118013049[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_575866575379183764) {
|
||||
err_fun(nom_x, delta_x, out_575866575379183764);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_2503888728252844227) {
|
||||
err_fun(nom_x, delta_x, out_2503888728252844227);
|
||||
}
|
||||
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_inv_err_fun(double *nom_x, double *true_x, double *out_3671647430606809601) {
|
||||
inv_err_fun(nom_x, true_x, out_3671647430606809601);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_6639538403135995006) {
|
||||
H_mod_fun(state, out_6639538403135995006);
|
||||
void car_H_mod_fun(double *state, double *out_3172612914634759799) {
|
||||
H_mod_fun(state, out_3172612914634759799);
|
||||
}
|
||||
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_7316960543418142517) {
|
||||
f_fun(state, dt, out_7316960543418142517);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_2916284287213904038) {
|
||||
F_fun(state, dt, out_2916284287213904038);
|
||||
void car_F_fun(double *state, double dt, double *out_1492910262171570578) {
|
||||
F_fun(state, dt, out_1492910262171570578);
|
||||
}
|
||||
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_595544251427276973) {
|
||||
h_25(state, unused, out_595544251427276973);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_8284225020584064933) {
|
||||
H_25(state, unused, out_8284225020584064933);
|
||||
void car_H_25(double *state, double *unused, double *out_350435724010605349) {
|
||||
H_25(state, unused, out_350435724010605349);
|
||||
}
|
||||
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_2954294579100300160) {
|
||||
h_24(state, unused, out_2954294579100300160);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_4726299421781741797) {
|
||||
H_24(state, unused, out_4726299421781741797);
|
||||
void car_H_24(double *state, double *unused, double *out_3207489874791717787) {
|
||||
H_24(state, unused, out_3207489874791717787);
|
||||
}
|
||||
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_4457227727855778625) {
|
||||
h_30(state, unused, out_4457227727855778625);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_8413563967727305003) {
|
||||
H_30(state, unused, out_8413563967727305003);
|
||||
void car_H_30(double *state, double *unused, double *out_2167897234496643278) {
|
||||
H_30(state, unused, out_2167897234496643278);
|
||||
}
|
||||
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_6778606859599551480) {
|
||||
h_26(state, unused, out_6778606859599551480);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_6421015734251430459) {
|
||||
H_26(state, unused, out_6421015734251430459);
|
||||
void car_H_26(double *state, double *unused, double *out_4091939042884661573) {
|
||||
H_26(state, unused, out_4091939042884661573);
|
||||
}
|
||||
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_2871073069040329107) {
|
||||
h_27(state, unused, out_2871073069040329107);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_7858416794181821702) {
|
||||
H_27(state, unused, out_7858416794181821702);
|
||||
void car_H_27(double *state, double *unused, double *out_4391491305680586495) {
|
||||
H_27(state, unused, out_4391491305680586495);
|
||||
}
|
||||
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_8987843078192927346) {
|
||||
h_29(state, unused, out_8987843078192927346);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_6145054067312270669) {
|
||||
H_29(state, unused, out_6145054067312270669);
|
||||
void car_H_29(double *state, double *unused, double *out_2678128578811035462) {
|
||||
H_29(state, unused, out_2678128578811035462);
|
||||
}
|
||||
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_3384751652918718147) {
|
||||
h_28(state, unused, out_3384751652918718147);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_8108684338877596920) {
|
||||
H_28(state, unused, out_8108684338877596920);
|
||||
void car_H_28(double *state, double *unused, double *out_2404270438258495112) {
|
||||
H_28(state, unused, out_2404270438258495112);
|
||||
}
|
||||
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_1973413530269387393) {
|
||||
h_31(state, unused, out_1973413530269387393);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_8253579058707104505) {
|
||||
H_31(state, unused, out_8253579058707104505);
|
||||
void car_H_31(double *state, double *unused, double *out_4718147145118013049) {
|
||||
H_31(state, unused, out_4718147145118013049);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_575866575379183764);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5616167858322380498);
|
||||
void car_H_mod_fun(double *state, double *out_6639538403135995006);
|
||||
void car_f_fun(double *state, double dt, double *out_6721417124249708482);
|
||||
void car_F_fun(double *state, double dt, double *out_2916284287213904038);
|
||||
void car_h_25(double *state, double *unused, double *out_2044795451145406261);
|
||||
void car_H_25(double *state, double *unused, double *out_8284225020584064933);
|
||||
void car_h_24(double *state, double *unused, double *out_1441131931248447039);
|
||||
void car_H_24(double *state, double *unused, double *out_4726299421781741797);
|
||||
void car_h_30(double *state, double *unused, double *out_8638683365743472914);
|
||||
void car_H_30(double *state, double *unused, double *out_8413563967727305003);
|
||||
void car_h_26(double *state, double *unused, double *out_989338192432138572);
|
||||
void car_H_26(double *state, double *unused, double *out_6421015734251430459);
|
||||
void car_h_27(double *state, double *unused, double *out_7375345222001897423);
|
||||
void car_H_27(double *state, double *unused, double *out_7858416794181821702);
|
||||
void car_h_29(double *state, double *unused, double *out_4806387440305233057);
|
||||
void car_H_29(double *state, double *unused, double *out_6145054067312270669);
|
||||
void car_h_28(double *state, double *unused, double *out_6025091355491401381);
|
||||
void car_H_28(double *state, double *unused, double *out_8108684338877596920);
|
||||
void car_h_31(double *state, double *unused, double *out_2208042107618306896);
|
||||
void car_H_31(double *state, double *unused, double *out_8253579058707104505);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_2503888728252844227);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3671647430606809601);
|
||||
void car_H_mod_fun(double *state, double *out_3172612914634759799);
|
||||
void car_f_fun(double *state, double dt, double *out_7316960543418142517);
|
||||
void car_F_fun(double *state, double dt, double *out_1492910262171570578);
|
||||
void car_h_25(double *state, double *unused, double *out_595544251427276973);
|
||||
void car_H_25(double *state, double *unused, double *out_350435724010605349);
|
||||
void car_h_24(double *state, double *unused, double *out_2954294579100300160);
|
||||
void car_H_24(double *state, double *unused, double *out_3207489874791717787);
|
||||
void car_h_30(double *state, double *unused, double *out_4457227727855778625);
|
||||
void car_H_30(double *state, double *unused, double *out_2167897234496643278);
|
||||
void car_h_26(double *state, double *unused, double *out_6778606859599551480);
|
||||
void car_H_26(double *state, double *unused, double *out_4091939042884661573);
|
||||
void car_h_27(double *state, double *unused, double *out_2871073069040329107);
|
||||
void car_H_27(double *state, double *unused, double *out_4391491305680586495);
|
||||
void car_h_29(double *state, double *unused, double *out_8987843078192927346);
|
||||
void car_H_29(double *state, double *unused, double *out_2678128578811035462);
|
||||
void car_h_28(double *state, double *unused, double *out_3384751652918718147);
|
||||
void car_H_28(double *state, double *unused, double *out_2404270438258495112);
|
||||
void car_h_31(double *state, double *unused, double *out_1973413530269387393);
|
||||
void car_H_31(double *state, double *unused, double *out_4718147145118013049);
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
void car_set_mass(double x);
|
||||
void car_set_rotational_inertia(double x);
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_8315489890310503187);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6008596051390826841);
|
||||
void pose_H_mod_fun(double *state, double *out_5634677350720166788);
|
||||
void pose_f_fun(double *state, double dt, double *out_5401222257802876036);
|
||||
void pose_F_fun(double *state, double dt, double *out_1313183186567530475);
|
||||
void pose_h_4(double *state, double *unused, double *out_717943751669182438);
|
||||
void pose_H_4(double *state, double *unused, double *out_7659548337950304093);
|
||||
void pose_h_10(double *state, double *unused, double *out_5351201218378086589);
|
||||
void pose_H_10(double *state, double *unused, double *out_5063285185285945841);
|
||||
void pose_h_13(double *state, double *unused, double *out_8511287818621351769);
|
||||
void pose_H_13(double *state, double *unused, double *out_4447274512617971292);
|
||||
void pose_h_14(double *state, double *unused, double *out_7796170631529448638);
|
||||
void pose_H_14(double *state, double *unused, double *out_3696307481610819564);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_6989025868442055132);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3172938755845140042);
|
||||
void pose_H_mod_fun(double *state, double *out_3225867917449183727);
|
||||
void pose_f_fun(double *state, double dt, double *out_2533972772359230172);
|
||||
void pose_F_fun(double *state, double dt, double *out_3663800095301942537);
|
||||
void pose_h_4(double *state, double *unused, double *out_223446469702120852);
|
||||
void pose_H_4(double *state, double *unused, double *out_4324406006264935836);
|
||||
void pose_h_10(double *state, double *unused, double *out_2753848967308463071);
|
||||
void pose_H_10(double *state, double *unused, double *out_1159288756680005657);
|
||||
void pose_h_13(double *state, double *unused, double *out_4379566868646893753);
|
||||
void pose_H_13(double *state, double *unused, double *out_8158161469567459860);
|
||||
void pose_h_14(double *state, double *unused, double *out_8438977474921151403);
|
||||
void pose_H_14(double *state, double *unused, double *out_7407194438560308132);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
@@ -66,7 +66,7 @@ def dm_warp_pkl_path(w, h):
|
||||
return MODELS_DIR / f'dm_warp_{w}x{h}_tinygrad.pkl'
|
||||
|
||||
|
||||
def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad):
|
||||
def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None):
|
||||
w_dst, h_dst = dst_shape
|
||||
h_src, w_src = src_shape
|
||||
|
||||
@@ -81,11 +81,19 @@ def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad)
|
||||
src_x = src_x / src_w
|
||||
src_y = src_y / src_w
|
||||
|
||||
x_nn_clipped = Tensor.round(src_x).clip(0, w_src - 1).cast('int')
|
||||
y_nn_clipped = Tensor.round(src_y).clip(0, h_src - 1).cast('int')
|
||||
x_round = Tensor.round(src_x)
|
||||
y_round = Tensor.round(src_y)
|
||||
x_nn_clipped = x_round.clip(0, w_src - 1).cast('int')
|
||||
y_nn_clipped = y_round.clip(0, h_src - 1).cast('int')
|
||||
idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped
|
||||
|
||||
return src_flat[idx]
|
||||
sampled = src_flat[idx]
|
||||
if border_fill_val is None:
|
||||
return sampled
|
||||
|
||||
in_bounds = ((x_round >= 0) & (x_round <= w_src - 1) &
|
||||
(y_round >= 0) & (y_round <= h_src - 1)).cast(sampled.dtype)
|
||||
return sampled * in_bounds + Tensor(border_fill_val, dtype=sampled.dtype) * (1 - in_bounds)
|
||||
|
||||
|
||||
def frames_to_tensor(frames, model_w, model_h):
|
||||
@@ -152,7 +160,9 @@ def make_warp_dm(cam_w, cam_h, dm_w, dm_h):
|
||||
|
||||
def warp_dm(input_frame, M_inv):
|
||||
M_inv = M_inv.to(Device.DEFAULT)
|
||||
result = warp_perspective_tinygrad(input_frame[:cam_h*stride], M_inv, (dm_w, dm_h), (cam_h, cam_w), stride_pad).reshape(-1, dm_h * dm_w)
|
||||
result = warp_perspective_tinygrad(
|
||||
input_frame[:cam_h*stride], M_inv, (dm_w, dm_h), (cam_h, cam_w), stride_pad, border_fill_val=16
|
||||
).reshape(-1, dm_h * dm_w)
|
||||
return result
|
||||
return warp_dm
|
||||
|
||||
|
||||
@@ -80,6 +80,11 @@ def parse_model_output(model_output):
|
||||
parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
|
||||
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob', 'left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
|
||||
parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
|
||||
sleep_key = f'sleep_prob_{ds_suffix}'
|
||||
if sleep_key in model_output:
|
||||
parsed[sleep_key] = sigmoid(model_output[sleep_key])
|
||||
else:
|
||||
parsed[sleep_key] = np.zeros((1, 1), dtype=np.float32)
|
||||
return parsed
|
||||
|
||||
|
||||
@@ -95,6 +100,7 @@ def fill_driver_data(msg, model_output, ds_suffix):
|
||||
msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.sleepProb = model_output[f'sleep_prob_{ds_suffix}'][0, 0].item()
|
||||
|
||||
|
||||
def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -57,7 +57,7 @@ class DRIVER_MONITOR_SETTINGS:
|
||||
self._YAW_MIN_OFFSET = -0.0246
|
||||
|
||||
self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1
|
||||
self._DCAM_UNCERTAIN_RESET_COUNT = int(20 / self._DT_DMON)
|
||||
self._DCAM_UNCERTAIN_RESET_COUNT = int(2 / self._DT_DMON)
|
||||
self._POSESTD_THRESHOLD = 0.3
|
||||
self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s
|
||||
self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
||||
|
||||
Binary file not shown.
@@ -1,4 +1,5 @@
|
||||
import os, sys, pickle, time, re
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
if "FLOAT16" not in os.environ: os.environ["FLOAT16"] = "1"
|
||||
if "IMAGE" not in os.environ: os.environ["IMAGE"] = "2"
|
||||
@@ -131,7 +132,8 @@ def bench(run, inputs):
|
||||
run(**inputs).numpy()
|
||||
|
||||
if __name__ == "__main__":
|
||||
onnx_file = fetch(OPENPILOT_MODEL)
|
||||
local_onnx = Path(OPENPILOT_MODEL).expanduser()
|
||||
onnx_file = str(local_onnx if local_onnx.exists() else fetch(OPENPILOT_MODEL))
|
||||
inputs, outputs = compile(onnx_file)
|
||||
|
||||
with open(OUTPUT, "rb") as f: pickle_loaded = pickle.load(f)
|
||||
|
||||
Reference in New Issue
Block a user