spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Paramsd tuning

pull/1274/head
Willem Melching 2020-03-24 09:39:57 -07:00
parent 8ca12dac27
commit bb1557f8de
2 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
selfdrive/locationd/kalman/models/car_kf.py
View File

@ -46,22 +46,22 @@ class CarKalman():
# state covariance
P_initial = np.diag([
.1**2,
.1**2,
0.1**2,
0.1**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
10**2, 10**2,
1**2,
1**2,
1.0**2,
1.0**2,
])
# process noise
Q = np.diag([
(.05/10)**2,
(.05/100)**2,
.0001**2,
math.radians(0.01)**2,
math.radians(0.2)**2,
math.radians(0.001)**2,
math.radians(0.05)**2,
.1**2, .1**2,
math.radians(0.1)**2,

14
selfdrive/locationd/paramsd.py
View File

@ -52,17 +52,17 @@ class ParamsLearner:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_XY_SPEED,
np.array([[[v_calibrated[0], -v_calibrated[1]]]]),
np.array([np.diag([v_calibrated_std[0]**2, v_calibrated_std[1]**2])]))
np.array([np.diag([v_calibrated_std[0]**2, 1e6*v_calibrated_std[1]**2])]))
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
# Clamp values
x = self.kf.x
if not (10 < x[States.STEER_RATIO] < 25):
self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[[15.0]]]))
# x = self.kf.x
# if not (10 < x[States.STEER_RATIO] < 25):
# self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[[15.0]]]))
if not (0.5 < x[States.STIFFNESS] < 3.0):
self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[[1.0]]]))
# if not (0.5 < x[States.STIFFNESS] < 3.0):
# self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[[1.0]]]))
elif which == 'carState':
self.carstate_counter += 1
@ -136,7 +136,7 @@ def main(sm=None, pm=None):
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET])
msg.liveParameters.angleOffset = math.degrees(x[States.ANGLE_OFFSET_FAST])
msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(x[States.ANGLE_OFFSET_FAST])
i += 1
if i % 6000 == 0: # once a minute