mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-14 05:42:13 +08:00
honda
This commit is contained in:
firestar5683
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-4254090c-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-25633be9-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-4254090c-DEBUG
|
||||
DEV-25633be9-DEBUG
|
||||
@@ -49,7 +49,7 @@ MIN_LATERAL_CONTROL_SPEED = 0.3
|
||||
CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30
|
||||
CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20
|
||||
CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.52
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.75
|
||||
CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED = 12.0
|
||||
CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE = 0.08
|
||||
CIVIC_BOSCH_MODIFIED_B_FF_ONSET = 0.18
|
||||
@@ -76,16 +76,16 @@ CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00
|
||||
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
|
||||
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.03
|
||||
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.07
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.34
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.62
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.02
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.50
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.82
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.00
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.00
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 3.20
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 4.40
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.02
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 4.40
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 6.20
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 2.00
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 3.00
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 2.80
|
||||
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 4.20
|
||||
|
||||
BOLT_2022_2023_CARS = (
|
||||
GM_CAR.CHEVROLET_BOLT_ACC_2022_2023,
|
||||
|
||||
@@ -489,7 +489,7 @@ class TestLatControl:
|
||||
monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_lateral_testing_ground_active", lambda: True)
|
||||
variant_controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL)
|
||||
|
||||
assert variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 1.52)
|
||||
assert variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 1.75)
|
||||
|
||||
def test_modified_civic_b_torque_ff_scale_curve(self):
|
||||
steady_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.0, 12.0)
|
||||
@@ -539,11 +539,11 @@ class TestLatControl:
|
||||
|
||||
assert variant_steady_right < base_steady_right
|
||||
assert variant_turn_in_right < base_turn_in_right
|
||||
assert variant_turn_in_right > variant_steady_right
|
||||
assert variant_turn_in_right >= variant_steady_right
|
||||
assert variant_turn_in_left > variant_steady_left
|
||||
assert variant_unwind_right < base_unwind_right
|
||||
assert variant_unwind_right_friction < base_unwind_right_friction
|
||||
assert variant_turn_in_right_friction > base_turn_in_right_friction
|
||||
assert variant_turn_in_right_friction >= base_turn_in_right_friction
|
||||
|
||||
def test_modified_civic_a_variant_extra_torque_shaping_curve(self, monkeypatch):
|
||||
base_steady_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.0, 12.0)
|
||||
|
||||
@@ -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_8796389704996072171) {
|
||||
out_8796389704996072171[0] = delta_x[0] + nom_x[0];
|
||||
out_8796389704996072171[1] = delta_x[1] + nom_x[1];
|
||||
out_8796389704996072171[2] = delta_x[2] + nom_x[2];
|
||||
out_8796389704996072171[3] = delta_x[3] + nom_x[3];
|
||||
out_8796389704996072171[4] = delta_x[4] + nom_x[4];
|
||||
out_8796389704996072171[5] = delta_x[5] + nom_x[5];
|
||||
out_8796389704996072171[6] = delta_x[6] + nom_x[6];
|
||||
out_8796389704996072171[7] = delta_x[7] + nom_x[7];
|
||||
out_8796389704996072171[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_5325374827696631328) {
|
||||
out_5325374827696631328[0] = delta_x[0] + nom_x[0];
|
||||
out_5325374827696631328[1] = delta_x[1] + nom_x[1];
|
||||
out_5325374827696631328[2] = delta_x[2] + nom_x[2];
|
||||
out_5325374827696631328[3] = delta_x[3] + nom_x[3];
|
||||
out_5325374827696631328[4] = delta_x[4] + nom_x[4];
|
||||
out_5325374827696631328[5] = delta_x[5] + nom_x[5];
|
||||
out_5325374827696631328[6] = delta_x[6] + nom_x[6];
|
||||
out_5325374827696631328[7] = delta_x[7] + nom_x[7];
|
||||
out_5325374827696631328[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8849207833158339340) {
|
||||
out_8849207833158339340[0] = -nom_x[0] + true_x[0];
|
||||
out_8849207833158339340[1] = -nom_x[1] + true_x[1];
|
||||
out_8849207833158339340[2] = -nom_x[2] + true_x[2];
|
||||
out_8849207833158339340[3] = -nom_x[3] + true_x[3];
|
||||
out_8849207833158339340[4] = -nom_x[4] + true_x[4];
|
||||
out_8849207833158339340[5] = -nom_x[5] + true_x[5];
|
||||
out_8849207833158339340[6] = -nom_x[6] + true_x[6];
|
||||
out_8849207833158339340[7] = -nom_x[7] + true_x[7];
|
||||
out_8849207833158339340[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_263695042767338590) {
|
||||
out_263695042767338590[0] = -nom_x[0] + true_x[0];
|
||||
out_263695042767338590[1] = -nom_x[1] + true_x[1];
|
||||
out_263695042767338590[2] = -nom_x[2] + true_x[2];
|
||||
out_263695042767338590[3] = -nom_x[3] + true_x[3];
|
||||
out_263695042767338590[4] = -nom_x[4] + true_x[4];
|
||||
out_263695042767338590[5] = -nom_x[5] + true_x[5];
|
||||
out_263695042767338590[6] = -nom_x[6] + true_x[6];
|
||||
out_263695042767338590[7] = -nom_x[7] + true_x[7];
|
||||
out_263695042767338590[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_1622786580378488408) {
|
||||
out_1622786580378488408[0] = 1.0;
|
||||
out_1622786580378488408[1] = 0.0;
|
||||
out_1622786580378488408[2] = 0.0;
|
||||
out_1622786580378488408[3] = 0.0;
|
||||
out_1622786580378488408[4] = 0.0;
|
||||
out_1622786580378488408[5] = 0.0;
|
||||
out_1622786580378488408[6] = 0.0;
|
||||
out_1622786580378488408[7] = 0.0;
|
||||
out_1622786580378488408[8] = 0.0;
|
||||
out_1622786580378488408[9] = 0.0;
|
||||
out_1622786580378488408[10] = 1.0;
|
||||
out_1622786580378488408[11] = 0.0;
|
||||
out_1622786580378488408[12] = 0.0;
|
||||
out_1622786580378488408[13] = 0.0;
|
||||
out_1622786580378488408[14] = 0.0;
|
||||
out_1622786580378488408[15] = 0.0;
|
||||
out_1622786580378488408[16] = 0.0;
|
||||
out_1622786580378488408[17] = 0.0;
|
||||
out_1622786580378488408[18] = 0.0;
|
||||
out_1622786580378488408[19] = 0.0;
|
||||
out_1622786580378488408[20] = 1.0;
|
||||
out_1622786580378488408[21] = 0.0;
|
||||
out_1622786580378488408[22] = 0.0;
|
||||
out_1622786580378488408[23] = 0.0;
|
||||
out_1622786580378488408[24] = 0.0;
|
||||
out_1622786580378488408[25] = 0.0;
|
||||
out_1622786580378488408[26] = 0.0;
|
||||
out_1622786580378488408[27] = 0.0;
|
||||
out_1622786580378488408[28] = 0.0;
|
||||
out_1622786580378488408[29] = 0.0;
|
||||
out_1622786580378488408[30] = 1.0;
|
||||
out_1622786580378488408[31] = 0.0;
|
||||
out_1622786580378488408[32] = 0.0;
|
||||
out_1622786580378488408[33] = 0.0;
|
||||
out_1622786580378488408[34] = 0.0;
|
||||
out_1622786580378488408[35] = 0.0;
|
||||
out_1622786580378488408[36] = 0.0;
|
||||
out_1622786580378488408[37] = 0.0;
|
||||
out_1622786580378488408[38] = 0.0;
|
||||
out_1622786580378488408[39] = 0.0;
|
||||
out_1622786580378488408[40] = 1.0;
|
||||
out_1622786580378488408[41] = 0.0;
|
||||
out_1622786580378488408[42] = 0.0;
|
||||
out_1622786580378488408[43] = 0.0;
|
||||
out_1622786580378488408[44] = 0.0;
|
||||
out_1622786580378488408[45] = 0.0;
|
||||
out_1622786580378488408[46] = 0.0;
|
||||
out_1622786580378488408[47] = 0.0;
|
||||
out_1622786580378488408[48] = 0.0;
|
||||
out_1622786580378488408[49] = 0.0;
|
||||
out_1622786580378488408[50] = 1.0;
|
||||
out_1622786580378488408[51] = 0.0;
|
||||
out_1622786580378488408[52] = 0.0;
|
||||
out_1622786580378488408[53] = 0.0;
|
||||
out_1622786580378488408[54] = 0.0;
|
||||
out_1622786580378488408[55] = 0.0;
|
||||
out_1622786580378488408[56] = 0.0;
|
||||
out_1622786580378488408[57] = 0.0;
|
||||
out_1622786580378488408[58] = 0.0;
|
||||
out_1622786580378488408[59] = 0.0;
|
||||
out_1622786580378488408[60] = 1.0;
|
||||
out_1622786580378488408[61] = 0.0;
|
||||
out_1622786580378488408[62] = 0.0;
|
||||
out_1622786580378488408[63] = 0.0;
|
||||
out_1622786580378488408[64] = 0.0;
|
||||
out_1622786580378488408[65] = 0.0;
|
||||
out_1622786580378488408[66] = 0.0;
|
||||
out_1622786580378488408[67] = 0.0;
|
||||
out_1622786580378488408[68] = 0.0;
|
||||
out_1622786580378488408[69] = 0.0;
|
||||
out_1622786580378488408[70] = 1.0;
|
||||
out_1622786580378488408[71] = 0.0;
|
||||
out_1622786580378488408[72] = 0.0;
|
||||
out_1622786580378488408[73] = 0.0;
|
||||
out_1622786580378488408[74] = 0.0;
|
||||
out_1622786580378488408[75] = 0.0;
|
||||
out_1622786580378488408[76] = 0.0;
|
||||
out_1622786580378488408[77] = 0.0;
|
||||
out_1622786580378488408[78] = 0.0;
|
||||
out_1622786580378488408[79] = 0.0;
|
||||
out_1622786580378488408[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_2663737869404174494) {
|
||||
out_2663737869404174494[0] = 1.0;
|
||||
out_2663737869404174494[1] = 0.0;
|
||||
out_2663737869404174494[2] = 0.0;
|
||||
out_2663737869404174494[3] = 0.0;
|
||||
out_2663737869404174494[4] = 0.0;
|
||||
out_2663737869404174494[5] = 0.0;
|
||||
out_2663737869404174494[6] = 0.0;
|
||||
out_2663737869404174494[7] = 0.0;
|
||||
out_2663737869404174494[8] = 0.0;
|
||||
out_2663737869404174494[9] = 0.0;
|
||||
out_2663737869404174494[10] = 1.0;
|
||||
out_2663737869404174494[11] = 0.0;
|
||||
out_2663737869404174494[12] = 0.0;
|
||||
out_2663737869404174494[13] = 0.0;
|
||||
out_2663737869404174494[14] = 0.0;
|
||||
out_2663737869404174494[15] = 0.0;
|
||||
out_2663737869404174494[16] = 0.0;
|
||||
out_2663737869404174494[17] = 0.0;
|
||||
out_2663737869404174494[18] = 0.0;
|
||||
out_2663737869404174494[19] = 0.0;
|
||||
out_2663737869404174494[20] = 1.0;
|
||||
out_2663737869404174494[21] = 0.0;
|
||||
out_2663737869404174494[22] = 0.0;
|
||||
out_2663737869404174494[23] = 0.0;
|
||||
out_2663737869404174494[24] = 0.0;
|
||||
out_2663737869404174494[25] = 0.0;
|
||||
out_2663737869404174494[26] = 0.0;
|
||||
out_2663737869404174494[27] = 0.0;
|
||||
out_2663737869404174494[28] = 0.0;
|
||||
out_2663737869404174494[29] = 0.0;
|
||||
out_2663737869404174494[30] = 1.0;
|
||||
out_2663737869404174494[31] = 0.0;
|
||||
out_2663737869404174494[32] = 0.0;
|
||||
out_2663737869404174494[33] = 0.0;
|
||||
out_2663737869404174494[34] = 0.0;
|
||||
out_2663737869404174494[35] = 0.0;
|
||||
out_2663737869404174494[36] = 0.0;
|
||||
out_2663737869404174494[37] = 0.0;
|
||||
out_2663737869404174494[38] = 0.0;
|
||||
out_2663737869404174494[39] = 0.0;
|
||||
out_2663737869404174494[40] = 1.0;
|
||||
out_2663737869404174494[41] = 0.0;
|
||||
out_2663737869404174494[42] = 0.0;
|
||||
out_2663737869404174494[43] = 0.0;
|
||||
out_2663737869404174494[44] = 0.0;
|
||||
out_2663737869404174494[45] = 0.0;
|
||||
out_2663737869404174494[46] = 0.0;
|
||||
out_2663737869404174494[47] = 0.0;
|
||||
out_2663737869404174494[48] = 0.0;
|
||||
out_2663737869404174494[49] = 0.0;
|
||||
out_2663737869404174494[50] = 1.0;
|
||||
out_2663737869404174494[51] = 0.0;
|
||||
out_2663737869404174494[52] = 0.0;
|
||||
out_2663737869404174494[53] = 0.0;
|
||||
out_2663737869404174494[54] = 0.0;
|
||||
out_2663737869404174494[55] = 0.0;
|
||||
out_2663737869404174494[56] = 0.0;
|
||||
out_2663737869404174494[57] = 0.0;
|
||||
out_2663737869404174494[58] = 0.0;
|
||||
out_2663737869404174494[59] = 0.0;
|
||||
out_2663737869404174494[60] = 1.0;
|
||||
out_2663737869404174494[61] = 0.0;
|
||||
out_2663737869404174494[62] = 0.0;
|
||||
out_2663737869404174494[63] = 0.0;
|
||||
out_2663737869404174494[64] = 0.0;
|
||||
out_2663737869404174494[65] = 0.0;
|
||||
out_2663737869404174494[66] = 0.0;
|
||||
out_2663737869404174494[67] = 0.0;
|
||||
out_2663737869404174494[68] = 0.0;
|
||||
out_2663737869404174494[69] = 0.0;
|
||||
out_2663737869404174494[70] = 1.0;
|
||||
out_2663737869404174494[71] = 0.0;
|
||||
out_2663737869404174494[72] = 0.0;
|
||||
out_2663737869404174494[73] = 0.0;
|
||||
out_2663737869404174494[74] = 0.0;
|
||||
out_2663737869404174494[75] = 0.0;
|
||||
out_2663737869404174494[76] = 0.0;
|
||||
out_2663737869404174494[77] = 0.0;
|
||||
out_2663737869404174494[78] = 0.0;
|
||||
out_2663737869404174494[79] = 0.0;
|
||||
out_2663737869404174494[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_1981095305174081329) {
|
||||
out_1981095305174081329[0] = state[0];
|
||||
out_1981095305174081329[1] = state[1];
|
||||
out_1981095305174081329[2] = state[2];
|
||||
out_1981095305174081329[3] = state[3];
|
||||
out_1981095305174081329[4] = state[4];
|
||||
out_1981095305174081329[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_1981095305174081329[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_1981095305174081329[7] = state[7];
|
||||
out_1981095305174081329[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_8230420991833863893) {
|
||||
out_8230420991833863893[0] = state[0];
|
||||
out_8230420991833863893[1] = state[1];
|
||||
out_8230420991833863893[2] = state[2];
|
||||
out_8230420991833863893[3] = state[3];
|
||||
out_8230420991833863893[4] = state[4];
|
||||
out_8230420991833863893[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_8230420991833863893[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_8230420991833863893[7] = state[7];
|
||||
out_8230420991833863893[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_3561550856242897713) {
|
||||
out_3561550856242897713[0] = 1;
|
||||
out_3561550856242897713[1] = 0;
|
||||
out_3561550856242897713[2] = 0;
|
||||
out_3561550856242897713[3] = 0;
|
||||
out_3561550856242897713[4] = 0;
|
||||
out_3561550856242897713[5] = 0;
|
||||
out_3561550856242897713[6] = 0;
|
||||
out_3561550856242897713[7] = 0;
|
||||
out_3561550856242897713[8] = 0;
|
||||
out_3561550856242897713[9] = 0;
|
||||
out_3561550856242897713[10] = 1;
|
||||
out_3561550856242897713[11] = 0;
|
||||
out_3561550856242897713[12] = 0;
|
||||
out_3561550856242897713[13] = 0;
|
||||
out_3561550856242897713[14] = 0;
|
||||
out_3561550856242897713[15] = 0;
|
||||
out_3561550856242897713[16] = 0;
|
||||
out_3561550856242897713[17] = 0;
|
||||
out_3561550856242897713[18] = 0;
|
||||
out_3561550856242897713[19] = 0;
|
||||
out_3561550856242897713[20] = 1;
|
||||
out_3561550856242897713[21] = 0;
|
||||
out_3561550856242897713[22] = 0;
|
||||
out_3561550856242897713[23] = 0;
|
||||
out_3561550856242897713[24] = 0;
|
||||
out_3561550856242897713[25] = 0;
|
||||
out_3561550856242897713[26] = 0;
|
||||
out_3561550856242897713[27] = 0;
|
||||
out_3561550856242897713[28] = 0;
|
||||
out_3561550856242897713[29] = 0;
|
||||
out_3561550856242897713[30] = 1;
|
||||
out_3561550856242897713[31] = 0;
|
||||
out_3561550856242897713[32] = 0;
|
||||
out_3561550856242897713[33] = 0;
|
||||
out_3561550856242897713[34] = 0;
|
||||
out_3561550856242897713[35] = 0;
|
||||
out_3561550856242897713[36] = 0;
|
||||
out_3561550856242897713[37] = 0;
|
||||
out_3561550856242897713[38] = 0;
|
||||
out_3561550856242897713[39] = 0;
|
||||
out_3561550856242897713[40] = 1;
|
||||
out_3561550856242897713[41] = 0;
|
||||
out_3561550856242897713[42] = 0;
|
||||
out_3561550856242897713[43] = 0;
|
||||
out_3561550856242897713[44] = 0;
|
||||
out_3561550856242897713[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_3561550856242897713[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_3561550856242897713[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3561550856242897713[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3561550856242897713[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_3561550856242897713[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_3561550856242897713[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_3561550856242897713[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3561550856242897713[53] = -9.8100000000000005*dt;
|
||||
out_3561550856242897713[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_3561550856242897713[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_3561550856242897713[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3561550856242897713[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3561550856242897713[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_3561550856242897713[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_3561550856242897713[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_3561550856242897713[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3561550856242897713[62] = 0;
|
||||
out_3561550856242897713[63] = 0;
|
||||
out_3561550856242897713[64] = 0;
|
||||
out_3561550856242897713[65] = 0;
|
||||
out_3561550856242897713[66] = 0;
|
||||
out_3561550856242897713[67] = 0;
|
||||
out_3561550856242897713[68] = 0;
|
||||
out_3561550856242897713[69] = 0;
|
||||
out_3561550856242897713[70] = 1;
|
||||
out_3561550856242897713[71] = 0;
|
||||
out_3561550856242897713[72] = 0;
|
||||
out_3561550856242897713[73] = 0;
|
||||
out_3561550856242897713[74] = 0;
|
||||
out_3561550856242897713[75] = 0;
|
||||
out_3561550856242897713[76] = 0;
|
||||
out_3561550856242897713[77] = 0;
|
||||
out_3561550856242897713[78] = 0;
|
||||
out_3561550856242897713[79] = 0;
|
||||
out_3561550856242897713[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_2756618938696411588) {
|
||||
out_2756618938696411588[0] = 1;
|
||||
out_2756618938696411588[1] = 0;
|
||||
out_2756618938696411588[2] = 0;
|
||||
out_2756618938696411588[3] = 0;
|
||||
out_2756618938696411588[4] = 0;
|
||||
out_2756618938696411588[5] = 0;
|
||||
out_2756618938696411588[6] = 0;
|
||||
out_2756618938696411588[7] = 0;
|
||||
out_2756618938696411588[8] = 0;
|
||||
out_2756618938696411588[9] = 0;
|
||||
out_2756618938696411588[10] = 1;
|
||||
out_2756618938696411588[11] = 0;
|
||||
out_2756618938696411588[12] = 0;
|
||||
out_2756618938696411588[13] = 0;
|
||||
out_2756618938696411588[14] = 0;
|
||||
out_2756618938696411588[15] = 0;
|
||||
out_2756618938696411588[16] = 0;
|
||||
out_2756618938696411588[17] = 0;
|
||||
out_2756618938696411588[18] = 0;
|
||||
out_2756618938696411588[19] = 0;
|
||||
out_2756618938696411588[20] = 1;
|
||||
out_2756618938696411588[21] = 0;
|
||||
out_2756618938696411588[22] = 0;
|
||||
out_2756618938696411588[23] = 0;
|
||||
out_2756618938696411588[24] = 0;
|
||||
out_2756618938696411588[25] = 0;
|
||||
out_2756618938696411588[26] = 0;
|
||||
out_2756618938696411588[27] = 0;
|
||||
out_2756618938696411588[28] = 0;
|
||||
out_2756618938696411588[29] = 0;
|
||||
out_2756618938696411588[30] = 1;
|
||||
out_2756618938696411588[31] = 0;
|
||||
out_2756618938696411588[32] = 0;
|
||||
out_2756618938696411588[33] = 0;
|
||||
out_2756618938696411588[34] = 0;
|
||||
out_2756618938696411588[35] = 0;
|
||||
out_2756618938696411588[36] = 0;
|
||||
out_2756618938696411588[37] = 0;
|
||||
out_2756618938696411588[38] = 0;
|
||||
out_2756618938696411588[39] = 0;
|
||||
out_2756618938696411588[40] = 1;
|
||||
out_2756618938696411588[41] = 0;
|
||||
out_2756618938696411588[42] = 0;
|
||||
out_2756618938696411588[43] = 0;
|
||||
out_2756618938696411588[44] = 0;
|
||||
out_2756618938696411588[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_2756618938696411588[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_2756618938696411588[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2756618938696411588[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2756618938696411588[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_2756618938696411588[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_2756618938696411588[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_2756618938696411588[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2756618938696411588[53] = -9.8100000000000005*dt;
|
||||
out_2756618938696411588[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_2756618938696411588[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_2756618938696411588[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2756618938696411588[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2756618938696411588[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_2756618938696411588[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_2756618938696411588[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_2756618938696411588[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2756618938696411588[62] = 0;
|
||||
out_2756618938696411588[63] = 0;
|
||||
out_2756618938696411588[64] = 0;
|
||||
out_2756618938696411588[65] = 0;
|
||||
out_2756618938696411588[66] = 0;
|
||||
out_2756618938696411588[67] = 0;
|
||||
out_2756618938696411588[68] = 0;
|
||||
out_2756618938696411588[69] = 0;
|
||||
out_2756618938696411588[70] = 1;
|
||||
out_2756618938696411588[71] = 0;
|
||||
out_2756618938696411588[72] = 0;
|
||||
out_2756618938696411588[73] = 0;
|
||||
out_2756618938696411588[74] = 0;
|
||||
out_2756618938696411588[75] = 0;
|
||||
out_2756618938696411588[76] = 0;
|
||||
out_2756618938696411588[77] = 0;
|
||||
out_2756618938696411588[78] = 0;
|
||||
out_2756618938696411588[79] = 0;
|
||||
out_2756618938696411588[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_3280789364307995853) {
|
||||
out_3280789364307995853[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_2585399580963523364) {
|
||||
out_2585399580963523364[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_445738705382224348) {
|
||||
out_445738705382224348[0] = 0;
|
||||
out_445738705382224348[1] = 0;
|
||||
out_445738705382224348[2] = 0;
|
||||
out_445738705382224348[3] = 0;
|
||||
out_445738705382224348[4] = 0;
|
||||
out_445738705382224348[5] = 0;
|
||||
out_445738705382224348[6] = 1;
|
||||
out_445738705382224348[7] = 0;
|
||||
out_445738705382224348[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_4993569966627829866) {
|
||||
out_4993569966627829866[0] = 0;
|
||||
out_4993569966627829866[1] = 0;
|
||||
out_4993569966627829866[2] = 0;
|
||||
out_4993569966627829866[3] = 0;
|
||||
out_4993569966627829866[4] = 0;
|
||||
out_4993569966627829866[5] = 0;
|
||||
out_4993569966627829866[6] = 1;
|
||||
out_4993569966627829866[7] = 0;
|
||||
out_4993569966627829866[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_5100775928185398157) {
|
||||
out_5100775928185398157[0] = state[4];
|
||||
out_5100775928185398157[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_7866369606921911129) {
|
||||
out_7866369606921911129[0] = state[4];
|
||||
out_7866369606921911129[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_399257903401471051) {
|
||||
out_399257903401471051[0] = 0;
|
||||
out_399257903401471051[1] = 0;
|
||||
out_399257903401471051[2] = 0;
|
||||
out_399257903401471051[3] = 0;
|
||||
out_399257903401471051[4] = 1;
|
||||
out_399257903401471051[5] = 0;
|
||||
out_399257903401471051[6] = 0;
|
||||
out_399257903401471051[7] = 0;
|
||||
out_399257903401471051[8] = 0;
|
||||
out_399257903401471051[9] = 0;
|
||||
out_399257903401471051[10] = 0;
|
||||
out_399257903401471051[11] = 0;
|
||||
out_399257903401471051[12] = 0;
|
||||
out_399257903401471051[13] = 0;
|
||||
out_399257903401471051[14] = 1;
|
||||
out_399257903401471051[15] = 0;
|
||||
out_399257903401471051[16] = 0;
|
||||
out_399257903401471051[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_124755101600123014) {
|
||||
out_124755101600123014[0] = 0;
|
||||
out_124755101600123014[1] = 0;
|
||||
out_124755101600123014[2] = 0;
|
||||
out_124755101600123014[3] = 0;
|
||||
out_124755101600123014[4] = 1;
|
||||
out_124755101600123014[5] = 0;
|
||||
out_124755101600123014[6] = 0;
|
||||
out_124755101600123014[7] = 0;
|
||||
out_124755101600123014[8] = 0;
|
||||
out_124755101600123014[9] = 0;
|
||||
out_124755101600123014[10] = 0;
|
||||
out_124755101600123014[11] = 0;
|
||||
out_124755101600123014[12] = 0;
|
||||
out_124755101600123014[13] = 0;
|
||||
out_124755101600123014[14] = 1;
|
||||
out_124755101600123014[15] = 0;
|
||||
out_124755101600123014[16] = 0;
|
||||
out_124755101600123014[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_3331749478418022440) {
|
||||
out_3331749478418022440[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_1052049947290498986) {
|
||||
out_1052049947290498986[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_2072594253125024279) {
|
||||
out_2072594253125024279[0] = 0;
|
||||
out_2072594253125024279[1] = 0;
|
||||
out_2072594253125024279[2] = 0;
|
||||
out_2072594253125024279[3] = 0;
|
||||
out_2072594253125024279[4] = 1;
|
||||
out_2072594253125024279[5] = 0;
|
||||
out_2072594253125024279[6] = 0;
|
||||
out_2072594253125024279[7] = 0;
|
||||
out_2072594253125024279[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_465873636500221668) {
|
||||
out_465873636500221668[0] = 0;
|
||||
out_465873636500221668[1] = 0;
|
||||
out_465873636500221668[2] = 0;
|
||||
out_465873636500221668[3] = 0;
|
||||
out_465873636500221668[4] = 1;
|
||||
out_465873636500221668[5] = 0;
|
||||
out_465873636500221668[6] = 0;
|
||||
out_465873636500221668[7] = 0;
|
||||
out_465873636500221668[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_1066202602863606572) {
|
||||
out_1066202602863606572[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_3137755351529974294) {
|
||||
out_3137755351529974294[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_4187242024256280572) {
|
||||
out_4187242024256280572[0] = 0;
|
||||
out_4187242024256280572[1] = 0;
|
||||
out_4187242024256280572[2] = 0;
|
||||
out_4187242024256280572[3] = 0;
|
||||
out_4187242024256280572[4] = 0;
|
||||
out_4187242024256280572[5] = 0;
|
||||
out_4187242024256280572[6] = 0;
|
||||
out_4187242024256280572[7] = 1;
|
||||
out_4187242024256280572[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_1252066647753773642) {
|
||||
out_1252066647753773642[0] = 0;
|
||||
out_1252066647753773642[1] = 0;
|
||||
out_1252066647753773642[2] = 0;
|
||||
out_1252066647753773642[3] = 0;
|
||||
out_1252066647753773642[4] = 0;
|
||||
out_1252066647753773642[5] = 0;
|
||||
out_1252066647753773642[6] = 0;
|
||||
out_1252066647753773642[7] = 1;
|
||||
out_1252066647753773642[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_3118252153105853293) {
|
||||
out_3118252153105853293[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_3292885906595191724) {
|
||||
out_3292885906595191724[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_4296188324308967496) {
|
||||
out_4296188324308967496[0] = 0;
|
||||
out_4296188324308967496[1] = 0;
|
||||
out_4296188324308967496[2] = 0;
|
||||
out_4296188324308967496[3] = 1;
|
||||
out_4296188324308967496[4] = 0;
|
||||
out_4296188324308967496[5] = 0;
|
||||
out_4296188324308967496[6] = 0;
|
||||
out_4296188324308967496[7] = 0;
|
||||
out_4296188324308967496[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_2689467707684164885) {
|
||||
out_2689467707684164885[0] = 0;
|
||||
out_2689467707684164885[1] = 0;
|
||||
out_2689467707684164885[2] = 0;
|
||||
out_2689467707684164885[3] = 1;
|
||||
out_2689467707684164885[4] = 0;
|
||||
out_2689467707684164885[5] = 0;
|
||||
out_2689467707684164885[6] = 0;
|
||||
out_2689467707684164885[7] = 0;
|
||||
out_2689467707684164885[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_8557656694253347387) {
|
||||
out_8557656694253347387[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_5346991570123350160) {
|
||||
out_5346991570123350160[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_2582825597439416463) {
|
||||
out_2582825597439416463[0] = 0;
|
||||
out_2582825597439416463[1] = 1;
|
||||
out_2582825597439416463[2] = 0;
|
||||
out_2582825597439416463[3] = 0;
|
||||
out_2582825597439416463[4] = 0;
|
||||
out_2582825597439416463[5] = 0;
|
||||
out_2582825597439416463[6] = 0;
|
||||
out_2582825597439416463[7] = 0;
|
||||
out_2582825597439416463[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_976104980814613852) {
|
||||
out_976104980814613852[0] = 0;
|
||||
out_976104980814613852[1] = 1;
|
||||
out_976104980814613852[2] = 0;
|
||||
out_976104980814613852[3] = 0;
|
||||
out_976104980814613852[4] = 0;
|
||||
out_976104980814613852[5] = 0;
|
||||
out_976104980814613852[6] = 0;
|
||||
out_976104980814613852[7] = 0;
|
||||
out_976104980814613852[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_4285995954181995030) {
|
||||
out_4285995954181995030[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_6565695485309518484) {
|
||||
out_6565695485309518484[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_2499573419630114111) {
|
||||
out_2499573419630114111[0] = 1;
|
||||
out_2499573419630114111[1] = 0;
|
||||
out_2499573419630114111[2] = 0;
|
||||
out_2499573419630114111[3] = 0;
|
||||
out_2499573419630114111[4] = 0;
|
||||
out_2499573419630114111[5] = 0;
|
||||
out_2499573419630114111[6] = 0;
|
||||
out_2499573419630114111[7] = 0;
|
||||
out_2499573419630114111[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_2939735252379940103) {
|
||||
out_2939735252379940103[0] = 1;
|
||||
out_2939735252379940103[1] = 0;
|
||||
out_2939735252379940103[2] = 0;
|
||||
out_2939735252379940103[3] = 0;
|
||||
out_2939735252379940103[4] = 0;
|
||||
out_2939735252379940103[5] = 0;
|
||||
out_2939735252379940103[6] = 0;
|
||||
out_2939735252379940103[7] = 0;
|
||||
out_2939735252379940103[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_6669294028137815771) {
|
||||
out_6669294028137815771[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_7496068987004855670) {
|
||||
out_7496068987004855670[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_415092743505263920) {
|
||||
out_415092743505263920[0] = 0;
|
||||
out_415092743505263920[1] = 0;
|
||||
out_415092743505263920[2] = 0;
|
||||
out_415092743505263920[3] = 0;
|
||||
out_415092743505263920[4] = 0;
|
||||
out_415092743505263920[5] = 0;
|
||||
out_415092743505263920[6] = 0;
|
||||
out_415092743505263920[7] = 0;
|
||||
out_415092743505263920[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_625858545520422166) {
|
||||
out_625858545520422166[0] = 0;
|
||||
out_625858545520422166[1] = 0;
|
||||
out_625858545520422166[2] = 0;
|
||||
out_625858545520422166[3] = 0;
|
||||
out_625858545520422166[4] = 0;
|
||||
out_625858545520422166[5] = 0;
|
||||
out_625858545520422166[6] = 0;
|
||||
out_625858545520422166[7] = 0;
|
||||
out_625858545520422166[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_8796389704996072171) {
|
||||
err_fun(nom_x, delta_x, out_8796389704996072171);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_5325374827696631328) {
|
||||
err_fun(nom_x, delta_x, out_5325374827696631328);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8849207833158339340) {
|
||||
inv_err_fun(nom_x, true_x, out_8849207833158339340);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_263695042767338590) {
|
||||
inv_err_fun(nom_x, true_x, out_263695042767338590);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_1622786580378488408) {
|
||||
H_mod_fun(state, out_1622786580378488408);
|
||||
void car_H_mod_fun(double *state, double *out_2663737869404174494) {
|
||||
H_mod_fun(state, out_2663737869404174494);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_1981095305174081329) {
|
||||
f_fun(state, dt, out_1981095305174081329);
|
||||
void car_f_fun(double *state, double dt, double *out_8230420991833863893) {
|
||||
f_fun(state, dt, out_8230420991833863893);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_3561550856242897713) {
|
||||
F_fun(state, dt, out_3561550856242897713);
|
||||
void car_F_fun(double *state, double dt, double *out_2756618938696411588) {
|
||||
F_fun(state, dt, out_2756618938696411588);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_3280789364307995853) {
|
||||
h_25(state, unused, out_3280789364307995853);
|
||||
void car_h_25(double *state, double *unused, double *out_2585399580963523364) {
|
||||
h_25(state, unused, out_2585399580963523364);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_445738705382224348) {
|
||||
H_25(state, unused, out_445738705382224348);
|
||||
void car_H_25(double *state, double *unused, double *out_4993569966627829866) {
|
||||
H_25(state, unused, out_4993569966627829866);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_5100775928185398157) {
|
||||
h_24(state, unused, out_5100775928185398157);
|
||||
void car_h_24(double *state, double *unused, double *out_7866369606921911129) {
|
||||
h_24(state, unused, out_7866369606921911129);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_399257903401471051) {
|
||||
H_24(state, unused, out_399257903401471051);
|
||||
void car_H_24(double *state, double *unused, double *out_124755101600123014) {
|
||||
H_24(state, unused, out_124755101600123014);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_3331749478418022440) {
|
||||
h_30(state, unused, out_3331749478418022440);
|
||||
void car_h_30(double *state, double *unused, double *out_1052049947290498986) {
|
||||
h_30(state, unused, out_1052049947290498986);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_2072594253125024279) {
|
||||
H_30(state, unused, out_2072594253125024279);
|
||||
void car_H_30(double *state, double *unused, double *out_465873636500221668) {
|
||||
H_30(state, unused, out_465873636500221668);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_1066202602863606572) {
|
||||
h_26(state, unused, out_1066202602863606572);
|
||||
void car_h_26(double *state, double *unused, double *out_3137755351529974294) {
|
||||
h_26(state, unused, out_3137755351529974294);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_4187242024256280572) {
|
||||
H_26(state, unused, out_4187242024256280572);
|
||||
void car_H_26(double *state, double *unused, double *out_1252066647753773642) {
|
||||
H_26(state, unused, out_1252066647753773642);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_3118252153105853293) {
|
||||
h_27(state, unused, out_3118252153105853293);
|
||||
void car_h_27(double *state, double *unused, double *out_3292885906595191724) {
|
||||
h_27(state, unused, out_3292885906595191724);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_4296188324308967496) {
|
||||
H_27(state, unused, out_4296188324308967496);
|
||||
void car_H_27(double *state, double *unused, double *out_2689467707684164885) {
|
||||
H_27(state, unused, out_2689467707684164885);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_8557656694253347387) {
|
||||
h_29(state, unused, out_8557656694253347387);
|
||||
void car_h_29(double *state, double *unused, double *out_5346991570123350160) {
|
||||
h_29(state, unused, out_5346991570123350160);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_2582825597439416463) {
|
||||
H_29(state, unused, out_2582825597439416463);
|
||||
void car_H_29(double *state, double *unused, double *out_976104980814613852) {
|
||||
H_29(state, unused, out_976104980814613852);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_4285995954181995030) {
|
||||
h_28(state, unused, out_4285995954181995030);
|
||||
void car_h_28(double *state, double *unused, double *out_6565695485309518484) {
|
||||
h_28(state, unused, out_6565695485309518484);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_2499573419630114111) {
|
||||
H_28(state, unused, out_2499573419630114111);
|
||||
void car_H_28(double *state, double *unused, double *out_2939735252379940103) {
|
||||
H_28(state, unused, out_2939735252379940103);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_6669294028137815771) {
|
||||
h_31(state, unused, out_6669294028137815771);
|
||||
void car_h_31(double *state, double *unused, double *out_7496068987004855670) {
|
||||
h_31(state, unused, out_7496068987004855670);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_415092743505263920) {
|
||||
H_31(state, unused, out_415092743505263920);
|
||||
void car_H_31(double *state, double *unused, double *out_625858545520422166) {
|
||||
H_31(state, unused, out_625858545520422166);
|
||||
}
|
||||
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_8796389704996072171);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8849207833158339340);
|
||||
void car_H_mod_fun(double *state, double *out_1622786580378488408);
|
||||
void car_f_fun(double *state, double dt, double *out_1981095305174081329);
|
||||
void car_F_fun(double *state, double dt, double *out_3561550856242897713);
|
||||
void car_h_25(double *state, double *unused, double *out_3280789364307995853);
|
||||
void car_H_25(double *state, double *unused, double *out_445738705382224348);
|
||||
void car_h_24(double *state, double *unused, double *out_5100775928185398157);
|
||||
void car_H_24(double *state, double *unused, double *out_399257903401471051);
|
||||
void car_h_30(double *state, double *unused, double *out_3331749478418022440);
|
||||
void car_H_30(double *state, double *unused, double *out_2072594253125024279);
|
||||
void car_h_26(double *state, double *unused, double *out_1066202602863606572);
|
||||
void car_H_26(double *state, double *unused, double *out_4187242024256280572);
|
||||
void car_h_27(double *state, double *unused, double *out_3118252153105853293);
|
||||
void car_H_27(double *state, double *unused, double *out_4296188324308967496);
|
||||
void car_h_29(double *state, double *unused, double *out_8557656694253347387);
|
||||
void car_H_29(double *state, double *unused, double *out_2582825597439416463);
|
||||
void car_h_28(double *state, double *unused, double *out_4285995954181995030);
|
||||
void car_H_28(double *state, double *unused, double *out_2499573419630114111);
|
||||
void car_h_31(double *state, double *unused, double *out_6669294028137815771);
|
||||
void car_H_31(double *state, double *unused, double *out_415092743505263920);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_5325374827696631328);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_263695042767338590);
|
||||
void car_H_mod_fun(double *state, double *out_2663737869404174494);
|
||||
void car_f_fun(double *state, double dt, double *out_8230420991833863893);
|
||||
void car_F_fun(double *state, double dt, double *out_2756618938696411588);
|
||||
void car_h_25(double *state, double *unused, double *out_2585399580963523364);
|
||||
void car_H_25(double *state, double *unused, double *out_4993569966627829866);
|
||||
void car_h_24(double *state, double *unused, double *out_7866369606921911129);
|
||||
void car_H_24(double *state, double *unused, double *out_124755101600123014);
|
||||
void car_h_30(double *state, double *unused, double *out_1052049947290498986);
|
||||
void car_H_30(double *state, double *unused, double *out_465873636500221668);
|
||||
void car_h_26(double *state, double *unused, double *out_3137755351529974294);
|
||||
void car_H_26(double *state, double *unused, double *out_1252066647753773642);
|
||||
void car_h_27(double *state, double *unused, double *out_3292885906595191724);
|
||||
void car_H_27(double *state, double *unused, double *out_2689467707684164885);
|
||||
void car_h_29(double *state, double *unused, double *out_5346991570123350160);
|
||||
void car_H_29(double *state, double *unused, double *out_976104980814613852);
|
||||
void car_h_28(double *state, double *unused, double *out_6565695485309518484);
|
||||
void car_H_28(double *state, double *unused, double *out_2939735252379940103);
|
||||
void car_h_31(double *state, double *unused, double *out_7496068987004855670);
|
||||
void car_H_31(double *state, double *unused, double *out_625858545520422166);
|
||||
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_2383575066684203465);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2531792213831641268);
|
||||
void pose_H_mod_fun(double *state, double *out_5270168453751144292);
|
||||
void pose_f_fun(double *state, double dt, double *out_5870118476100851027);
|
||||
void pose_F_fun(double *state, double dt, double *out_6643036112755764466);
|
||||
void pose_h_4(double *state, double *unused, double *out_3825151169355500496);
|
||||
void pose_H_4(double *state, double *unused, double *out_4438379387875418186);
|
||||
void pose_h_10(double *state, double *unused, double *out_6180343716098328505);
|
||||
void pose_H_10(double *state, double *unused, double *out_6436902649839043686);
|
||||
void pose_h_13(double *state, double *unused, double *out_5615804281251360017);
|
||||
void pose_H_13(double *state, double *unused, double *out_8272134851177942210);
|
||||
void pose_h_14(double *state, double *unused, double *out_1282457731118130678);
|
||||
void pose_H_14(double *state, double *unused, double *out_7521167820170790482);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_4154280524256563890);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5487211563177375728);
|
||||
void pose_H_mod_fun(double *state, double *out_6762039440372011872);
|
||||
void pose_f_fun(double *state, double dt, double *out_2487690271541209027);
|
||||
void pose_F_fun(double *state, double dt, double *out_950182517979020623);
|
||||
void pose_h_4(double *state, double *unused, double *out_1064982969821053038);
|
||||
void pose_H_4(double *state, double *unused, double *out_1976156791710977266);
|
||||
void pose_h_10(double *state, double *unused, double *out_9039008373436645978);
|
||||
void pose_H_10(double *state, double *unused, double *out_6832717040485250243);
|
||||
void pose_h_13(double *state, double *unused, double *out_6939466162668529956);
|
||||
void pose_H_13(double *state, double *unused, double *out_2540758711392821370);
|
||||
void pose_h_14(double *state, double *unused, double *out_542665533068743617);
|
||||
void pose_H_14(double *state, double *unused, double *out_1106631640584395030);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
@@ -14,6 +14,8 @@
|
||||
#include "starpilot/ui/qt/offroad/wheel_settings.h"
|
||||
#include "system/hardware/hw.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
bool nnffLogFileExists(const QString &carFingerprint) {
|
||||
static QStringList models;
|
||||
static QMap<QString, QString> substitutes;
|
||||
@@ -409,82 +411,35 @@ void StarPilotSettingsWindow::updateVariables() {
|
||||
float currentVEgoStartingStock = params.getFloat("VEgoStartingStock");
|
||||
float currentVEgoStoppingStock = params.getFloat("VEgoStoppingStock");
|
||||
|
||||
if (currentDelayStock != steerActuatorDelay && steerActuatorDelay != 0) {
|
||||
if (params.getFloat("SteerDelay") == currentDelayStock || currentDelayStock == 0) {
|
||||
params.putFloatNonBlocking("SteerDelay", steerActuatorDelay);
|
||||
}
|
||||
params.putFloatNonBlocking("SteerDelayStock", steerActuatorDelay);
|
||||
}
|
||||
auto nearlyEqual = [](float a, float b) {
|
||||
return std::fabs(a - b) <= 1e-6f;
|
||||
};
|
||||
|
||||
if (currentFrictionStock != friction && friction != 0) {
|
||||
if (params.getFloat("SteerFriction") == currentFrictionStock || currentFrictionStock == 0) {
|
||||
params.putFloatNonBlocking("SteerFriction", friction);
|
||||
auto syncStockParam = [&](const char *key, const char *stockKey, float currentStock, float liveValue) {
|
||||
if (nearlyEqual(currentStock, liveValue) || nearlyEqual(liveValue, 0.0f)) {
|
||||
return;
|
||||
}
|
||||
params.putFloatNonBlocking("SteerFrictionStock", friction);
|
||||
}
|
||||
|
||||
if (currentKPStock != steerKp && steerKp != 0) {
|
||||
if (params.getFloat("SteerKP") == currentKPStock || currentKPStock == 0) {
|
||||
params.putFloatNonBlocking("SteerKP", steerKp);
|
||||
float currentValue = params.getFloat(key);
|
||||
bool shouldUpdateLiveValue = nearlyEqual(currentValue, currentStock) || nearlyEqual(currentValue, 0.0f);
|
||||
if (shouldUpdateLiveValue) {
|
||||
params.putFloatNonBlocking(key, liveValue);
|
||||
}
|
||||
params.putFloatNonBlocking("SteerKPStock", steerKp);
|
||||
}
|
||||
|
||||
if (currentLatAccelStock != latAccelFactor && latAccelFactor != 0) {
|
||||
if (params.getFloat("SteerLatAccel") == currentLatAccelStock || currentLatAccelStock == 0) {
|
||||
params.putFloatNonBlocking("SteerLatAccel", latAccelFactor);
|
||||
}
|
||||
params.putFloatNonBlocking("SteerLatAccelStock", latAccelFactor);
|
||||
}
|
||||
params.putFloatNonBlocking(stockKey, liveValue);
|
||||
};
|
||||
|
||||
if (currentLongDelayStock != longitudinalActuatorDelay && longitudinalActuatorDelay != 0) {
|
||||
if (params.getFloat("LongitudinalActuatorDelay") == currentLongDelayStock || currentLongDelayStock == 0) {
|
||||
params.putFloatNonBlocking("LongitudinalActuatorDelay", longitudinalActuatorDelay);
|
||||
}
|
||||
params.putFloatNonBlocking("LongitudinalActuatorDelayStock", longitudinalActuatorDelay);
|
||||
}
|
||||
|
||||
if (currentStartAccelStock != startAccel && startAccel != 0) {
|
||||
if (params.getFloat("StartAccel") == currentStartAccelStock || currentStartAccelStock == 0) {
|
||||
params.putFloatNonBlocking("StartAccel", startAccel);
|
||||
}
|
||||
params.putFloatNonBlocking("StartAccelStock", startAccel);
|
||||
}
|
||||
|
||||
if (currentSteerRatioStock != steerRatio && steerRatio != 0) {
|
||||
if (params.getFloat("SteerRatio") == currentSteerRatioStock || currentSteerRatioStock == 0) {
|
||||
params.putFloatNonBlocking("SteerRatio", steerRatio);
|
||||
}
|
||||
params.putFloatNonBlocking("SteerRatioStock", steerRatio);
|
||||
}
|
||||
|
||||
if (currentStopAccelStock != stopAccel && stopAccel != 0) {
|
||||
if (params.getFloat("StopAccel") == currentStopAccelStock || currentStopAccelStock == 0) {
|
||||
params.putFloatNonBlocking("StopAccel", stopAccel);
|
||||
}
|
||||
params.putFloatNonBlocking("StopAccelStock", stopAccel);
|
||||
}
|
||||
|
||||
if (currentStoppingDecelRateStock != stoppingDecelRate && stoppingDecelRate != 0) {
|
||||
if (params.getFloat("StoppingDecelRate") == currentStoppingDecelRateStock || currentStoppingDecelRateStock == 0) {
|
||||
params.putFloatNonBlocking("StoppingDecelRate", stoppingDecelRate);
|
||||
}
|
||||
params.putFloatNonBlocking("StoppingDecelRateStock", stoppingDecelRate);
|
||||
}
|
||||
|
||||
if (currentVEgoStartingStock != vEgoStarting && vEgoStarting != 0) {
|
||||
if (params.getFloat("VEgoStarting") == currentVEgoStartingStock || currentVEgoStartingStock == 0) {
|
||||
params.putFloatNonBlocking("VEgoStarting", vEgoStarting);
|
||||
}
|
||||
params.putFloatNonBlocking("VEgoStartingStock", vEgoStarting);
|
||||
}
|
||||
|
||||
if (currentVEgoStoppingStock != vEgoStopping && vEgoStopping != 0) {
|
||||
if (params.getFloat("VEgoStopping") == currentVEgoStoppingStock || currentVEgoStoppingStock == 0) {
|
||||
params.putFloatNonBlocking("VEgoStopping", vEgoStopping);
|
||||
}
|
||||
params.putFloatNonBlocking("VEgoStoppingStock", vEgoStopping);
|
||||
}
|
||||
syncStockParam("SteerDelay", "SteerDelayStock", currentDelayStock, steerActuatorDelay);
|
||||
syncStockParam("SteerFriction", "SteerFrictionStock", currentFrictionStock, friction);
|
||||
syncStockParam("SteerKP", "SteerKPStock", currentKPStock, steerKp);
|
||||
syncStockParam("SteerLatAccel", "SteerLatAccelStock", currentLatAccelStock, latAccelFactor);
|
||||
syncStockParam("LongitudinalActuatorDelay", "LongitudinalActuatorDelayStock", currentLongDelayStock, longitudinalActuatorDelay);
|
||||
syncStockParam("StartAccel", "StartAccelStock", currentStartAccelStock, startAccel);
|
||||
syncStockParam("SteerRatio", "SteerRatioStock", currentSteerRatioStock, steerRatio);
|
||||
syncStockParam("StopAccel", "StopAccelStock", currentStopAccelStock, stopAccel);
|
||||
syncStockParam("StoppingDecelRate", "StoppingDecelRateStock", currentStoppingDecelRateStock, stoppingDecelRate);
|
||||
syncStockParam("VEgoStarting", "VEgoStartingStock", currentVEgoStartingStock, vEgoStarting);
|
||||
syncStockParam("VEgoStopping", "VEgoStoppingStock", currentVEgoStoppingStock, vEgoStopping);
|
||||
|
||||
if (Hardware::PC() && (carMake == "mock" || carFingerprint == "MOCK")) {
|
||||
applyDesktopVehicleFallback();
|
||||
|
||||
Reference in New Issue
Block a user