mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-19 00:43:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

paramsd: fix VehicleModelInvalid errors (#23726)

Browse Source 
* bound steerratio, stiffness std

* remove start steer_ratio limits after looking at data

* reduce sf obs noise

* update refs

* update refs

* add comment explaining change
This commit is contained in:
Vivek Aithal
2022-02-09 17:37:28 -08:00
committed by GitHub
parent d0060a9d9b
commit 8a2cbfe00b
3 changed files with 15 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
selfdrive/locationd/models/car_kf.py
View File

@@ -71,11 +71,11 @@ class CarKalman(KalmanFilter):
P_initial = Q.copy()
obs_noise: Dict[int, Any] = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2),
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
@@ -106,9 +106,9 @@ class CarKalman(KalmanFilter):
state = sp.Matrix(state_sym)
# Vehicle model constants
x = state[States.STIFFNESS, :][0, 0]
sf = state[States.STIFFNESS, :][0, 0]
cF, cR = x * cF_orig, x * cR_orig
cF, cR = sf * cF_orig, sf * cR_orig
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
theta = state[States.ROAD_ROLL, :][0, 0]
@@ -154,7 +154,7 @@ class CarKalman(KalmanFilter):
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([x]), ObservationKind.STIFFNESS, None],
[sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
[sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
]

10
selfdrive/locationd/paramsd.py
View File

@@ -45,7 +45,7 @@ class ParamsLearner:
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
localizer_roll = msg.orientationNED.value[0]
localizer_roll_std = msg.orientationNED.std[0]
localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0]
roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX
if roll_valid:
roll = localizer_roll
@@ -76,6 +76,14 @@ class ParamsLearner:
np.array([np.atleast_2d(roll_std**2)]))
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
# We observe the current stiffness and steer ratio (with a high observation noise) to bound
# the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the
# states in longer routes (especially straight stretches).
stiffness = float(self.kf.x[States.STIFFNESS])
steer_ratio = float(self.kf.x[States.STEER_RATIO])
self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]]))
self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
elif which == 'carState':
self.steering_angle = msg.steeringAngleDeg
self.steering_pressed = msg.steeringPressed

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
5cec6f1575235206c4e0341d49de53be8d4e3429
3bc128c5b47e036021ccfaab9a9924d61eeb59e2