Paramsd tuning

This commit is contained in:
Willem Melching 2020-03-24 09:39:57 -07:00
parent 8ca12dac27
commit bb1557f8de
2 changed files with 14 additions and 14 deletions

View file

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

View file

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