Fix Build

This commit is contained in:
firestarsdog
2026-10-16 20:15:13 -04:00
parent 192729eb8d
commit e4e30e14fc
44 changed files with 1348 additions and 1348 deletions
@@ -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_7378461606150437313) {
out_7378461606150437313[0] = delta_x[0] + nom_x[0];
out_7378461606150437313[1] = delta_x[1] + nom_x[1];
out_7378461606150437313[2] = delta_x[2] + nom_x[2];
out_7378461606150437313[3] = delta_x[3] + nom_x[3];
out_7378461606150437313[4] = delta_x[4] + nom_x[4];
out_7378461606150437313[5] = delta_x[5] + nom_x[5];
out_7378461606150437313[6] = delta_x[6] + nom_x[6];
out_7378461606150437313[7] = delta_x[7] + nom_x[7];
out_7378461606150437313[8] = delta_x[8] + nom_x[8];
void err_fun(double *nom_x, double *delta_x, double *out_3470217287287697986) {
out_3470217287287697986[0] = delta_x[0] + nom_x[0];
out_3470217287287697986[1] = delta_x[1] + nom_x[1];
out_3470217287287697986[2] = delta_x[2] + nom_x[2];
out_3470217287287697986[3] = delta_x[3] + nom_x[3];
out_3470217287287697986[4] = delta_x[4] + nom_x[4];
out_3470217287287697986[5] = delta_x[5] + nom_x[5];
out_3470217287287697986[6] = delta_x[6] + nom_x[6];
out_3470217287287697986[7] = delta_x[7] + nom_x[7];
out_3470217287287697986[8] = delta_x[8] + nom_x[8];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_294189106821879139) {
out_294189106821879139[0] = -nom_x[0] + true_x[0];
out_294189106821879139[1] = -nom_x[1] + true_x[1];
out_294189106821879139[2] = -nom_x[2] + true_x[2];
out_294189106821879139[3] = -nom_x[3] + true_x[3];
out_294189106821879139[4] = -nom_x[4] + true_x[4];
out_294189106821879139[5] = -nom_x[5] + true_x[5];
out_294189106821879139[6] = -nom_x[6] + true_x[6];
out_294189106821879139[7] = -nom_x[7] + true_x[7];
out_294189106821879139[8] = -nom_x[8] + true_x[8];
void inv_err_fun(double *nom_x, double *true_x, double *out_5423086811818805380) {
out_5423086811818805380[0] = -nom_x[0] + true_x[0];
out_5423086811818805380[1] = -nom_x[1] + true_x[1];
out_5423086811818805380[2] = -nom_x[2] + true_x[2];
out_5423086811818805380[3] = -nom_x[3] + true_x[3];
out_5423086811818805380[4] = -nom_x[4] + true_x[4];
out_5423086811818805380[5] = -nom_x[5] + true_x[5];
out_5423086811818805380[6] = -nom_x[6] + true_x[6];
out_5423086811818805380[7] = -nom_x[7] + true_x[7];
out_5423086811818805380[8] = -nom_x[8] + true_x[8];
}
void H_mod_fun(double *state, double *out_4621094181659745510) {
out_4621094181659745510[0] = 1.0;
out_4621094181659745510[1] = 0.0;
out_4621094181659745510[2] = 0.0;
out_4621094181659745510[3] = 0.0;
out_4621094181659745510[4] = 0.0;
out_4621094181659745510[5] = 0.0;
out_4621094181659745510[6] = 0.0;
out_4621094181659745510[7] = 0.0;
out_4621094181659745510[8] = 0.0;
out_4621094181659745510[9] = 0.0;
out_4621094181659745510[10] = 1.0;
out_4621094181659745510[11] = 0.0;
out_4621094181659745510[12] = 0.0;
out_4621094181659745510[13] = 0.0;
out_4621094181659745510[14] = 0.0;
out_4621094181659745510[15] = 0.0;
out_4621094181659745510[16] = 0.0;
out_4621094181659745510[17] = 0.0;
out_4621094181659745510[18] = 0.0;
out_4621094181659745510[19] = 0.0;
out_4621094181659745510[20] = 1.0;
out_4621094181659745510[21] = 0.0;
out_4621094181659745510[22] = 0.0;
out_4621094181659745510[23] = 0.0;
out_4621094181659745510[24] = 0.0;
out_4621094181659745510[25] = 0.0;
out_4621094181659745510[26] = 0.0;
out_4621094181659745510[27] = 0.0;
out_4621094181659745510[28] = 0.0;
out_4621094181659745510[29] = 0.0;
out_4621094181659745510[30] = 1.0;
out_4621094181659745510[31] = 0.0;
out_4621094181659745510[32] = 0.0;
out_4621094181659745510[33] = 0.0;
out_4621094181659745510[34] = 0.0;
out_4621094181659745510[35] = 0.0;
out_4621094181659745510[36] = 0.0;
out_4621094181659745510[37] = 0.0;
out_4621094181659745510[38] = 0.0;
out_4621094181659745510[39] = 0.0;
out_4621094181659745510[40] = 1.0;
out_4621094181659745510[41] = 0.0;
out_4621094181659745510[42] = 0.0;
out_4621094181659745510[43] = 0.0;
out_4621094181659745510[44] = 0.0;
out_4621094181659745510[45] = 0.0;
out_4621094181659745510[46] = 0.0;
out_4621094181659745510[47] = 0.0;
out_4621094181659745510[48] = 0.0;
out_4621094181659745510[49] = 0.0;
out_4621094181659745510[50] = 1.0;
out_4621094181659745510[51] = 0.0;
out_4621094181659745510[52] = 0.0;
out_4621094181659745510[53] = 0.0;
out_4621094181659745510[54] = 0.0;
out_4621094181659745510[55] = 0.0;
out_4621094181659745510[56] = 0.0;
out_4621094181659745510[57] = 0.0;
out_4621094181659745510[58] = 0.0;
out_4621094181659745510[59] = 0.0;
out_4621094181659745510[60] = 1.0;
out_4621094181659745510[61] = 0.0;
out_4621094181659745510[62] = 0.0;
out_4621094181659745510[63] = 0.0;
out_4621094181659745510[64] = 0.0;
out_4621094181659745510[65] = 0.0;
out_4621094181659745510[66] = 0.0;
out_4621094181659745510[67] = 0.0;
out_4621094181659745510[68] = 0.0;
out_4621094181659745510[69] = 0.0;
out_4621094181659745510[70] = 1.0;
out_4621094181659745510[71] = 0.0;
out_4621094181659745510[72] = 0.0;
out_4621094181659745510[73] = 0.0;
out_4621094181659745510[74] = 0.0;
out_4621094181659745510[75] = 0.0;
out_4621094181659745510[76] = 0.0;
out_4621094181659745510[77] = 0.0;
out_4621094181659745510[78] = 0.0;
out_4621094181659745510[79] = 0.0;
out_4621094181659745510[80] = 1.0;
void H_mod_fun(double *state, double *out_8859145476210338021) {
out_8859145476210338021[0] = 1.0;
out_8859145476210338021[1] = 0.0;
out_8859145476210338021[2] = 0.0;
out_8859145476210338021[3] = 0.0;
out_8859145476210338021[4] = 0.0;
out_8859145476210338021[5] = 0.0;
out_8859145476210338021[6] = 0.0;
out_8859145476210338021[7] = 0.0;
out_8859145476210338021[8] = 0.0;
out_8859145476210338021[9] = 0.0;
out_8859145476210338021[10] = 1.0;
out_8859145476210338021[11] = 0.0;
out_8859145476210338021[12] = 0.0;
out_8859145476210338021[13] = 0.0;
out_8859145476210338021[14] = 0.0;
out_8859145476210338021[15] = 0.0;
out_8859145476210338021[16] = 0.0;
out_8859145476210338021[17] = 0.0;
out_8859145476210338021[18] = 0.0;
out_8859145476210338021[19] = 0.0;
out_8859145476210338021[20] = 1.0;
out_8859145476210338021[21] = 0.0;
out_8859145476210338021[22] = 0.0;
out_8859145476210338021[23] = 0.0;
out_8859145476210338021[24] = 0.0;
out_8859145476210338021[25] = 0.0;
out_8859145476210338021[26] = 0.0;
out_8859145476210338021[27] = 0.0;
out_8859145476210338021[28] = 0.0;
out_8859145476210338021[29] = 0.0;
out_8859145476210338021[30] = 1.0;
out_8859145476210338021[31] = 0.0;
out_8859145476210338021[32] = 0.0;
out_8859145476210338021[33] = 0.0;
out_8859145476210338021[34] = 0.0;
out_8859145476210338021[35] = 0.0;
out_8859145476210338021[36] = 0.0;
out_8859145476210338021[37] = 0.0;
out_8859145476210338021[38] = 0.0;
out_8859145476210338021[39] = 0.0;
out_8859145476210338021[40] = 1.0;
out_8859145476210338021[41] = 0.0;
out_8859145476210338021[42] = 0.0;
out_8859145476210338021[43] = 0.0;
out_8859145476210338021[44] = 0.0;
out_8859145476210338021[45] = 0.0;
out_8859145476210338021[46] = 0.0;
out_8859145476210338021[47] = 0.0;
out_8859145476210338021[48] = 0.0;
out_8859145476210338021[49] = 0.0;
out_8859145476210338021[50] = 1.0;
out_8859145476210338021[51] = 0.0;
out_8859145476210338021[52] = 0.0;
out_8859145476210338021[53] = 0.0;
out_8859145476210338021[54] = 0.0;
out_8859145476210338021[55] = 0.0;
out_8859145476210338021[56] = 0.0;
out_8859145476210338021[57] = 0.0;
out_8859145476210338021[58] = 0.0;
out_8859145476210338021[59] = 0.0;
out_8859145476210338021[60] = 1.0;
out_8859145476210338021[61] = 0.0;
out_8859145476210338021[62] = 0.0;
out_8859145476210338021[63] = 0.0;
out_8859145476210338021[64] = 0.0;
out_8859145476210338021[65] = 0.0;
out_8859145476210338021[66] = 0.0;
out_8859145476210338021[67] = 0.0;
out_8859145476210338021[68] = 0.0;
out_8859145476210338021[69] = 0.0;
out_8859145476210338021[70] = 1.0;
out_8859145476210338021[71] = 0.0;
out_8859145476210338021[72] = 0.0;
out_8859145476210338021[73] = 0.0;
out_8859145476210338021[74] = 0.0;
out_8859145476210338021[75] = 0.0;
out_8859145476210338021[76] = 0.0;
out_8859145476210338021[77] = 0.0;
out_8859145476210338021[78] = 0.0;
out_8859145476210338021[79] = 0.0;
out_8859145476210338021[80] = 1.0;
}
void f_fun(double *state, double dt, double *out_2614999472635489174) {
out_2614999472635489174[0] = state[0];
out_2614999472635489174[1] = state[1];
out_2614999472635489174[2] = state[2];
out_2614999472635489174[3] = state[3];
out_2614999472635489174[4] = state[4];
out_2614999472635489174[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_2614999472635489174[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_2614999472635489174[7] = state[7];
out_2614999472635489174[8] = state[8];
void f_fun(double *state, double dt, double *out_4364048461385736387) {
out_4364048461385736387[0] = state[0];
out_4364048461385736387[1] = state[1];
out_4364048461385736387[2] = state[2];
out_4364048461385736387[3] = state[3];
out_4364048461385736387[4] = state[4];
out_4364048461385736387[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_4364048461385736387[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_4364048461385736387[7] = state[7];
out_4364048461385736387[8] = state[8];
}
void F_fun(double *state, double dt, double *out_7956648669823198257) {
out_7956648669823198257[0] = 1;
out_7956648669823198257[1] = 0;
out_7956648669823198257[2] = 0;
out_7956648669823198257[3] = 0;
out_7956648669823198257[4] = 0;
out_7956648669823198257[5] = 0;
out_7956648669823198257[6] = 0;
out_7956648669823198257[7] = 0;
out_7956648669823198257[8] = 0;
out_7956648669823198257[9] = 0;
out_7956648669823198257[10] = 1;
out_7956648669823198257[11] = 0;
out_7956648669823198257[12] = 0;
out_7956648669823198257[13] = 0;
out_7956648669823198257[14] = 0;
out_7956648669823198257[15] = 0;
out_7956648669823198257[16] = 0;
out_7956648669823198257[17] = 0;
out_7956648669823198257[18] = 0;
out_7956648669823198257[19] = 0;
out_7956648669823198257[20] = 1;
out_7956648669823198257[21] = 0;
out_7956648669823198257[22] = 0;
out_7956648669823198257[23] = 0;
out_7956648669823198257[24] = 0;
out_7956648669823198257[25] = 0;
out_7956648669823198257[26] = 0;
out_7956648669823198257[27] = 0;
out_7956648669823198257[28] = 0;
out_7956648669823198257[29] = 0;
out_7956648669823198257[30] = 1;
out_7956648669823198257[31] = 0;
out_7956648669823198257[32] = 0;
out_7956648669823198257[33] = 0;
out_7956648669823198257[34] = 0;
out_7956648669823198257[35] = 0;
out_7956648669823198257[36] = 0;
out_7956648669823198257[37] = 0;
out_7956648669823198257[38] = 0;
out_7956648669823198257[39] = 0;
out_7956648669823198257[40] = 1;
out_7956648669823198257[41] = 0;
out_7956648669823198257[42] = 0;
out_7956648669823198257[43] = 0;
out_7956648669823198257[44] = 0;
out_7956648669823198257[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_7956648669823198257[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_7956648669823198257[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_7956648669823198257[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_7956648669823198257[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_7956648669823198257[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_7956648669823198257[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_7956648669823198257[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_7956648669823198257[53] = -9.8100000000000005*dt;
out_7956648669823198257[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_7956648669823198257[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_7956648669823198257[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7956648669823198257[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7956648669823198257[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_7956648669823198257[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_7956648669823198257[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_7956648669823198257[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7956648669823198257[62] = 0;
out_7956648669823198257[63] = 0;
out_7956648669823198257[64] = 0;
out_7956648669823198257[65] = 0;
out_7956648669823198257[66] = 0;
out_7956648669823198257[67] = 0;
out_7956648669823198257[68] = 0;
out_7956648669823198257[69] = 0;
out_7956648669823198257[70] = 1;
out_7956648669823198257[71] = 0;
out_7956648669823198257[72] = 0;
out_7956648669823198257[73] = 0;
out_7956648669823198257[74] = 0;
out_7956648669823198257[75] = 0;
out_7956648669823198257[76] = 0;
out_7956648669823198257[77] = 0;
out_7956648669823198257[78] = 0;
out_7956648669823198257[79] = 0;
out_7956648669823198257[80] = 1;
void F_fun(double *state, double dt, double *out_5736261136038400283) {
out_5736261136038400283[0] = 1;
out_5736261136038400283[1] = 0;
out_5736261136038400283[2] = 0;
out_5736261136038400283[3] = 0;
out_5736261136038400283[4] = 0;
out_5736261136038400283[5] = 0;
out_5736261136038400283[6] = 0;
out_5736261136038400283[7] = 0;
out_5736261136038400283[8] = 0;
out_5736261136038400283[9] = 0;
out_5736261136038400283[10] = 1;
out_5736261136038400283[11] = 0;
out_5736261136038400283[12] = 0;
out_5736261136038400283[13] = 0;
out_5736261136038400283[14] = 0;
out_5736261136038400283[15] = 0;
out_5736261136038400283[16] = 0;
out_5736261136038400283[17] = 0;
out_5736261136038400283[18] = 0;
out_5736261136038400283[19] = 0;
out_5736261136038400283[20] = 1;
out_5736261136038400283[21] = 0;
out_5736261136038400283[22] = 0;
out_5736261136038400283[23] = 0;
out_5736261136038400283[24] = 0;
out_5736261136038400283[25] = 0;
out_5736261136038400283[26] = 0;
out_5736261136038400283[27] = 0;
out_5736261136038400283[28] = 0;
out_5736261136038400283[29] = 0;
out_5736261136038400283[30] = 1;
out_5736261136038400283[31] = 0;
out_5736261136038400283[32] = 0;
out_5736261136038400283[33] = 0;
out_5736261136038400283[34] = 0;
out_5736261136038400283[35] = 0;
out_5736261136038400283[36] = 0;
out_5736261136038400283[37] = 0;
out_5736261136038400283[38] = 0;
out_5736261136038400283[39] = 0;
out_5736261136038400283[40] = 1;
out_5736261136038400283[41] = 0;
out_5736261136038400283[42] = 0;
out_5736261136038400283[43] = 0;
out_5736261136038400283[44] = 0;
out_5736261136038400283[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_5736261136038400283[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_5736261136038400283[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_5736261136038400283[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_5736261136038400283[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_5736261136038400283[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_5736261136038400283[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_5736261136038400283[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_5736261136038400283[53] = -9.8100000000000005*dt;
out_5736261136038400283[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_5736261136038400283[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_5736261136038400283[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_5736261136038400283[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_5736261136038400283[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_5736261136038400283[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_5736261136038400283[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_5736261136038400283[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_5736261136038400283[62] = 0;
out_5736261136038400283[63] = 0;
out_5736261136038400283[64] = 0;
out_5736261136038400283[65] = 0;
out_5736261136038400283[66] = 0;
out_5736261136038400283[67] = 0;
out_5736261136038400283[68] = 0;
out_5736261136038400283[69] = 0;
out_5736261136038400283[70] = 1;
out_5736261136038400283[71] = 0;
out_5736261136038400283[72] = 0;
out_5736261136038400283[73] = 0;
out_5736261136038400283[74] = 0;
out_5736261136038400283[75] = 0;
out_5736261136038400283[76] = 0;
out_5736261136038400283[77] = 0;
out_5736261136038400283[78] = 0;
out_5736261136038400283[79] = 0;
out_5736261136038400283[80] = 1;
}
void h_25(double *state, double *unused, double *out_5695726182074513639) {
out_5695726182074513639[0] = state[6];
void h_25(double *state, double *unused, double *out_2839818080272470940) {
out_2839818080272470940[0] = state[6];
}
void H_25(double *state, double *unused, double *out_5061492031990537509) {
out_5061492031990537509[0] = 0;
out_5061492031990537509[1] = 0;
out_5061492031990537509[2] = 0;
out_5061492031990537509[3] = 0;
out_5061492031990537509[4] = 0;
out_5061492031990537509[5] = 0;
out_5061492031990537509[6] = 1;
out_5061492031990537509[7] = 0;
out_5061492031990537509[8] = 0;
void H_25(double *state, double *unused, double *out_6064617947509721918) {
out_6064617947509721918[0] = 0;
out_6064617947509721918[1] = 0;
out_6064617947509721918[2] = 0;
out_6064617947509721918[3] = 0;
out_6064617947509721918[4] = 0;
out_6064617947509721918[5] = 0;
out_6064617947509721918[6] = 1;
out_6064617947509721918[7] = 0;
out_6064617947509721918[8] = 0;
}
void h_24(double *state, double *unused, double *out_7087152536231624463) {
out_7087152536231624463[0] = state[4];
out_7087152536231624463[1] = state[5];
void h_24(double *state, double *unused, double *out_6639924414496629328) {
out_6639924414496629328[0] = state[4];
out_6639924414496629328[1] = state[5];
}
void H_24(double *state, double *unused, double *out_7234141630996037075) {
out_7234141630996037075[0] = 0;
out_7234141630996037075[1] = 0;
out_7234141630996037075[2] = 0;
out_7234141630996037075[3] = 0;
out_7234141630996037075[4] = 1;
out_7234141630996037075[5] = 0;
out_7234141630996037075[6] = 0;
out_7234141630996037075[7] = 0;
out_7234141630996037075[8] = 0;
out_7234141630996037075[9] = 0;
out_7234141630996037075[10] = 0;
out_7234141630996037075[11] = 0;
out_7234141630996037075[12] = 0;
out_7234141630996037075[13] = 0;
out_7234141630996037075[14] = 1;
out_7234141630996037075[15] = 0;
out_7234141630996037075[16] = 0;
out_7234141630996037075[17] = 0;
void H_24(double *state, double *unused, double *out_2506692348707398782) {
out_2506692348707398782[0] = 0;
out_2506692348707398782[1] = 0;
out_2506692348707398782[2] = 0;
out_2506692348707398782[3] = 0;
out_2506692348707398782[4] = 1;
out_2506692348707398782[5] = 0;
out_2506692348707398782[6] = 0;
out_2506692348707398782[7] = 0;
out_2506692348707398782[8] = 0;
out_2506692348707398782[9] = 0;
out_2506692348707398782[10] = 0;
out_2506692348707398782[11] = 0;
out_2506692348707398782[12] = 0;
out_2506692348707398782[13] = 0;
out_2506692348707398782[14] = 1;
out_2506692348707398782[15] = 0;
out_2506692348707398782[16] = 0;
out_2506692348707398782[17] = 0;
}
void h_30(double *state, double *unused, double *out_2067539074746158802) {
out_2067539074746158802[0] = state[4];
void h_30(double *state, double *unused, double *out_1319846228188637228) {
out_1319846228188637228[0] = state[4];
}
void H_30(double *state, double *unused, double *out_1855198309501079246) {
out_1855198309501079246[0] = 0;
out_1855198309501079246[1] = 0;
out_1855198309501079246[2] = 0;
out_1855198309501079246[3] = 0;
out_1855198309501079246[4] = 1;
out_1855198309501079246[5] = 0;
out_1855198309501079246[6] = 0;
out_1855198309501079246[7] = 0;
out_1855198309501079246[8] = 0;
void H_30(double *state, double *unused, double *out_7854429796072221500) {
out_7854429796072221500[0] = 0;
out_7854429796072221500[1] = 0;
out_7854429796072221500[2] = 0;
out_7854429796072221500[3] = 0;
out_7854429796072221500[4] = 1;
out_7854429796072221500[5] = 0;
out_7854429796072221500[6] = 0;
out_7854429796072221500[7] = 0;
out_7854429796072221500[8] = 0;
}
void h_26(double *state, double *unused, double *out_7069212342407004447) {
out_7069212342407004447[0] = state[7];
void h_26(double *state, double *unused, double *out_7198131715747352316) {
out_7198131715747352316[0] = state[7];
}
void H_26(double *state, double *unused, double *out_1756966062229736908) {
out_1756966062229736908[0] = 0;
out_1756966062229736908[1] = 0;
out_1756966062229736908[2] = 0;
out_1756966062229736908[3] = 0;
out_1756966062229736908[4] = 0;
out_1756966062229736908[5] = 0;
out_1756966062229736908[6] = 0;
out_1756966062229736908[7] = 1;
out_1756966062229736908[8] = 0;
void H_26(double *state, double *unused, double *out_8640622807325773474) {
out_8640622807325773474[0] = 0;
out_8640622807325773474[1] = 0;
out_8640622807325773474[2] = 0;
out_8640622807325773474[3] = 0;
out_8640622807325773474[4] = 0;
out_8640622807325773474[5] = 0;
out_8640622807325773474[6] = 0;
out_8640622807325773474[7] = 1;
out_8640622807325773474[8] = 0;
}
void h_27(double *state, double *unused, double *out_4033180971766345651) {
out_4033180971766345651[0] = state[3];
void h_27(double *state, double *unused, double *out_3285488125208824077) {
out_3285488125208824077[0] = state[3];
}
void H_27(double *state, double *unused, double *out_319565002299345665) {
out_319565002299345665[0] = 0;
out_319565002299345665[1] = 0;
out_319565002299345665[2] = 0;
out_319565002299345665[3] = 1;
out_319565002299345665[4] = 0;
out_319565002299345665[5] = 0;
out_319565002299345665[6] = 0;
out_319565002299345665[7] = 0;
out_319565002299345665[8] = 0;
void H_27(double *state, double *unused, double *out_8368720206453386899) {
out_8368720206453386899[0] = 0;
out_8368720206453386899[1] = 0;
out_8368720206453386899[2] = 0;
out_8368720206453386899[3] = 1;
out_8368720206453386899[4] = 0;
out_8368720206453386899[5] = 0;
out_8368720206453386899[6] = 0;
out_8368720206453386899[7] = 0;
out_8368720206453386899[8] = 0;
}
void h_29(double *state, double *unused, double *out_2554646954709949512) {
out_2554646954709949512[0] = state[1];
void h_29(double *state, double *unused, double *out_716530343512159711) {
out_716530343512159711[0] = state[1];
}
void H_29(double *state, double *unused, double *out_2032927729168896698) {
out_2032927729168896698[0] = 0;
out_2032927729168896698[1] = 1;
out_2032927729168896698[2] = 0;
out_2032927729168896698[3] = 0;
out_2032927729168896698[4] = 0;
out_2032927729168896698[5] = 0;
out_2032927729168896698[6] = 0;
out_2032927729168896698[7] = 0;
out_2032927729168896698[8] = 0;
void H_29(double *state, double *unused, double *out_8364661140386613684) {
out_8364661140386613684[0] = 0;
out_8364661140386613684[1] = 1;
out_8364661140386613684[2] = 0;
out_8364661140386613684[3] = 0;
out_8364661140386613684[4] = 0;
out_8364661140386613684[5] = 0;
out_8364661140386613684[6] = 0;
out_8364661140386613684[7] = 0;
out_8364661140386613684[8] = 0;
}
void h_28(double *state, double *unused, double *out_8157738379984158711) {
out_8157738379984158711[0] = state[0];
void h_28(double *state, double *unused, double *out_1716185142452021568) {
out_1716185142452021568[0] = state[0];
}
void H_28(double *state, double *unused, double *out_7115326746238427272) {
out_7115326746238427272[0] = 1;
out_7115326746238427272[1] = 0;
out_7115326746238427272[2] = 0;
out_7115326746238427272[3] = 0;
out_7115326746238427272[4] = 0;
out_7115326746238427272[5] = 0;
out_7115326746238427272[6] = 0;
out_7115326746238427272[7] = 0;
out_7115326746238427272[8] = 0;
void H_28(double *state, double *unused, double *out_8118452661757611681) {
out_8118452661757611681[0] = 1;
out_8118452661757611681[1] = 0;
out_8118452661757611681[2] = 0;
out_8118452661757611681[3] = 0;
out_8118452661757611681[4] = 0;
out_8118452661757611681[5] = 0;
out_8118452661757611681[6] = 0;
out_8118452661757611681[7] = 0;
out_8118452661757611681[8] = 0;
}
void h_31(double *state, double *unused, double *out_5550206357853858668) {
out_5550206357853858668[0] = state[8];
void h_31(double *state, double *unused, double *out_4542766280828628078) {
out_4542766280828628078[0] = state[8];
}
void H_31(double *state, double *unused, double *out_2015183218521279744) {
out_2015183218521279744[0] = 0;
out_2015183218521279744[1] = 0;
out_2015183218521279744[2] = 0;
out_2015183218521279744[3] = 0;
out_2015183218521279744[4] = 0;
out_2015183218521279744[5] = 0;
out_2015183218521279744[6] = 0;
out_2015183218521279744[7] = 0;
out_2015183218521279744[8] = 1;
void H_31(double *state, double *unused, double *out_8014414705092421998) {
out_8014414705092421998[0] = 0;
out_8014414705092421998[1] = 0;
out_8014414705092421998[2] = 0;
out_8014414705092421998[3] = 0;
out_8014414705092421998[4] = 0;
out_8014414705092421998[5] = 0;
out_8014414705092421998[6] = 0;
out_8014414705092421998[7] = 0;
out_8014414705092421998[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_7378461606150437313) {
err_fun(nom_x, delta_x, out_7378461606150437313);
void car_err_fun(double *nom_x, double *delta_x, double *out_3470217287287697986) {
err_fun(nom_x, delta_x, out_3470217287287697986);
}
void car_inv_err_fun(double *nom_x, double *true_x, double *out_294189106821879139) {
inv_err_fun(nom_x, true_x, out_294189106821879139);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5423086811818805380) {
inv_err_fun(nom_x, true_x, out_5423086811818805380);
}
void car_H_mod_fun(double *state, double *out_4621094181659745510) {
H_mod_fun(state, out_4621094181659745510);
void car_H_mod_fun(double *state, double *out_8859145476210338021) {
H_mod_fun(state, out_8859145476210338021);
}
void car_f_fun(double *state, double dt, double *out_2614999472635489174) {
f_fun(state, dt, out_2614999472635489174);
void car_f_fun(double *state, double dt, double *out_4364048461385736387) {
f_fun(state, dt, out_4364048461385736387);
}
void car_F_fun(double *state, double dt, double *out_7956648669823198257) {
F_fun(state, dt, out_7956648669823198257);
void car_F_fun(double *state, double dt, double *out_5736261136038400283) {
F_fun(state, dt, out_5736261136038400283);
}
void car_h_25(double *state, double *unused, double *out_5695726182074513639) {
h_25(state, unused, out_5695726182074513639);
void car_h_25(double *state, double *unused, double *out_2839818080272470940) {
h_25(state, unused, out_2839818080272470940);
}
void car_H_25(double *state, double *unused, double *out_5061492031990537509) {
H_25(state, unused, out_5061492031990537509);
void car_H_25(double *state, double *unused, double *out_6064617947509721918) {
H_25(state, unused, out_6064617947509721918);
}
void car_h_24(double *state, double *unused, double *out_7087152536231624463) {
h_24(state, unused, out_7087152536231624463);
void car_h_24(double *state, double *unused, double *out_6639924414496629328) {
h_24(state, unused, out_6639924414496629328);
}
void car_H_24(double *state, double *unused, double *out_7234141630996037075) {
H_24(state, unused, out_7234141630996037075);
void car_H_24(double *state, double *unused, double *out_2506692348707398782) {
H_24(state, unused, out_2506692348707398782);
}
void car_h_30(double *state, double *unused, double *out_2067539074746158802) {
h_30(state, unused, out_2067539074746158802);
void car_h_30(double *state, double *unused, double *out_1319846228188637228) {
h_30(state, unused, out_1319846228188637228);
}
void car_H_30(double *state, double *unused, double *out_1855198309501079246) {
H_30(state, unused, out_1855198309501079246);
void car_H_30(double *state, double *unused, double *out_7854429796072221500) {
H_30(state, unused, out_7854429796072221500);
}
void car_h_26(double *state, double *unused, double *out_7069212342407004447) {
h_26(state, unused, out_7069212342407004447);
void car_h_26(double *state, double *unused, double *out_7198131715747352316) {
h_26(state, unused, out_7198131715747352316);
}
void car_H_26(double *state, double *unused, double *out_1756966062229736908) {
H_26(state, unused, out_1756966062229736908);
void car_H_26(double *state, double *unused, double *out_8640622807325773474) {
H_26(state, unused, out_8640622807325773474);
}
void car_h_27(double *state, double *unused, double *out_4033180971766345651) {
h_27(state, unused, out_4033180971766345651);
void car_h_27(double *state, double *unused, double *out_3285488125208824077) {
h_27(state, unused, out_3285488125208824077);
}
void car_H_27(double *state, double *unused, double *out_319565002299345665) {
H_27(state, unused, out_319565002299345665);
void car_H_27(double *state, double *unused, double *out_8368720206453386899) {
H_27(state, unused, out_8368720206453386899);
}
void car_h_29(double *state, double *unused, double *out_2554646954709949512) {
h_29(state, unused, out_2554646954709949512);
void car_h_29(double *state, double *unused, double *out_716530343512159711) {
h_29(state, unused, out_716530343512159711);
}
void car_H_29(double *state, double *unused, double *out_2032927729168896698) {
H_29(state, unused, out_2032927729168896698);
void car_H_29(double *state, double *unused, double *out_8364661140386613684) {
H_29(state, unused, out_8364661140386613684);
}
void car_h_28(double *state, double *unused, double *out_8157738379984158711) {
h_28(state, unused, out_8157738379984158711);
void car_h_28(double *state, double *unused, double *out_1716185142452021568) {
h_28(state, unused, out_1716185142452021568);
}
void car_H_28(double *state, double *unused, double *out_7115326746238427272) {
H_28(state, unused, out_7115326746238427272);
void car_H_28(double *state, double *unused, double *out_8118452661757611681) {
H_28(state, unused, out_8118452661757611681);
}
void car_h_31(double *state, double *unused, double *out_5550206357853858668) {
h_31(state, unused, out_5550206357853858668);
void car_h_31(double *state, double *unused, double *out_4542766280828628078) {
h_31(state, unused, out_4542766280828628078);
}
void car_H_31(double *state, double *unused, double *out_2015183218521279744) {
H_31(state, unused, out_2015183218521279744);
void car_H_31(double *state, double *unused, double *out_8014414705092421998) {
H_31(state, unused, out_8014414705092421998);
}
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_7378461606150437313);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_294189106821879139);
void car_H_mod_fun(double *state, double *out_4621094181659745510);
void car_f_fun(double *state, double dt, double *out_2614999472635489174);
void car_F_fun(double *state, double dt, double *out_7956648669823198257);
void car_h_25(double *state, double *unused, double *out_5695726182074513639);
void car_H_25(double *state, double *unused, double *out_5061492031990537509);
void car_h_24(double *state, double *unused, double *out_7087152536231624463);
void car_H_24(double *state, double *unused, double *out_7234141630996037075);
void car_h_30(double *state, double *unused, double *out_2067539074746158802);
void car_H_30(double *state, double *unused, double *out_1855198309501079246);
void car_h_26(double *state, double *unused, double *out_7069212342407004447);
void car_H_26(double *state, double *unused, double *out_1756966062229736908);
void car_h_27(double *state, double *unused, double *out_4033180971766345651);
void car_H_27(double *state, double *unused, double *out_319565002299345665);
void car_h_29(double *state, double *unused, double *out_2554646954709949512);
void car_H_29(double *state, double *unused, double *out_2032927729168896698);
void car_h_28(double *state, double *unused, double *out_8157738379984158711);
void car_H_28(double *state, double *unused, double *out_7115326746238427272);
void car_h_31(double *state, double *unused, double *out_5550206357853858668);
void car_H_31(double *state, double *unused, double *out_2015183218521279744);
void car_err_fun(double *nom_x, double *delta_x, double *out_3470217287287697986);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5423086811818805380);
void car_H_mod_fun(double *state, double *out_8859145476210338021);
void car_f_fun(double *state, double dt, double *out_4364048461385736387);
void car_F_fun(double *state, double dt, double *out_5736261136038400283);
void car_h_25(double *state, double *unused, double *out_2839818080272470940);
void car_H_25(double *state, double *unused, double *out_6064617947509721918);
void car_h_24(double *state, double *unused, double *out_6639924414496629328);
void car_H_24(double *state, double *unused, double *out_2506692348707398782);
void car_h_30(double *state, double *unused, double *out_1319846228188637228);
void car_H_30(double *state, double *unused, double *out_7854429796072221500);
void car_h_26(double *state, double *unused, double *out_7198131715747352316);
void car_H_26(double *state, double *unused, double *out_8640622807325773474);
void car_h_27(double *state, double *unused, double *out_3285488125208824077);
void car_H_27(double *state, double *unused, double *out_8368720206453386899);
void car_h_29(double *state, double *unused, double *out_716530343512159711);
void car_H_29(double *state, double *unused, double *out_8364661140386613684);
void car_h_28(double *state, double *unused, double *out_1716185142452021568);
void car_H_28(double *state, double *unused, double *out_8118452661757611681);
void car_h_31(double *state, double *unused, double *out_4542766280828628078);
void car_H_31(double *state, double *unused, double *out_8014414705092421998);
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_5566842885647767369);
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7605915063363770360);
void pose_H_mod_fun(double *state, double *out_9174940369185036217);
void pose_f_fun(double *state, double dt, double *out_4673717053718014348);
void pose_F_fun(double *state, double dt, double *out_7626949901910213403);
void pose_h_4(double *state, double *unused, double *out_2637904353676577);
void pose_H_4(double *state, double *unused, double *out_3423721897772556678);
void pose_h_10(double *state, double *unused, double *out_2208932603856339349);
void pose_H_10(double *state, double *unused, double *out_1086050679309335415);
void pose_h_13(double *state, double *unused, double *out_8307148822937221148);
void pose_H_13(double *state, double *unused, double *out_6635995723104889479);
void pose_h_14(double *state, double *unused, double *out_81297754970292080);
void pose_H_14(double *state, double *unused, double *out_7386962754112041207);
void pose_err_fun(double *nom_x, double *delta_x, double *out_6609500045983475884);
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2251582086712269854);
void pose_H_mod_fun(double *state, double *out_8200145802550385466);
void pose_f_fun(double *state, double dt, double *out_3154493756264028272);
void pose_F_fun(double *state, double dt, double *out_3844222318897373612);
void pose_h_4(double *state, double *unused, double *out_7417392044831538702);
void pose_H_4(double *state, double *unused, double *out_744215863507435876);
void pose_h_10(double *state, double *unused, double *out_5837768205443090035);
void pose_H_10(double *state, double *unused, double *out_6045792490383943628);
void pose_h_13(double *state, double *unused, double *out_886522868392701902);
void pose_H_13(double *state, double *unused, double *out_2468057961824896925);
void pose_h_14(double *state, double *unused, double *out_8921497272601995159);
void pose_H_14(double *state, double *unused, double *out_8225361678787176300);
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
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
View File
@@ -1,4 +1,4 @@
#pragma once
extern const uint8_t gitversion[29];
const uint8_t gitversion[29] = "DEV-53309097-P92eb5651-DEBUG";
const uint8_t gitversion[29] = "DEV-192729eb-P92eb5651-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.
+1 -1
View File
@@ -1 +1 @@
DEV-53309097-P92eb5651-DEBUG
DEV-192729eb-P92eb5651-DEBUG
View File
+1 -1
View File
@@ -1 +1 @@
{"protected_inventory_sha256": "20607e60ee00780c30313c0eb508f7a189a6efdd052932e1be8e285b6d3ba398", "schema_version": 1}
{"protected_inventory_sha256": "fceebd0a104d5d72e754217a553cf43fb51c6cf3b309148387f897441b94d41f", "schema_version": 1}