retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Car_kf tuning

albatross
Willem Melching 2020-04-23 14:38:25 -07:00
parent 922055f464
commit f15d55c2b1
3 changed files with 16 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
selfdrive/locationd/kalman/helpers/__init__.py
View File

@ -62,6 +62,7 @@ class ObservationKind():
ANGLE_OFFSET_FAST = 27 # [rad]
STIFFNESS = 28 # [-]
STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
names = [
'Unknown',

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

@ -44,35 +44,25 @@ class CarKalman():
0.0,
])
# state covariance
P_initial = np.diag([
0.1**2,
0.1**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
10**2, 10**2,
1.0**2,
1.0**2,
])
# process noise
Q = np.diag([
(.05/100)**2,
.0001**2,
math.radians(0.001)**2,
math.radians(0.05)**2,
math.radians(0.0001)**2,
math.radians(0.1)**2,
.1**2, .1**2,
.1**2, .01**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
])
P_initial = Q.copy()
obs_noise = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.1)**2),
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(50.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(50.0**2),
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(5.0**2),
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
maha_test_kinds = [] # [ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_XY_SPEED]
@ -139,6 +129,7 @@ class CarKalman():
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],

17
selfdrive/locationd/paramsd.py
View File

@ -35,24 +35,14 @@ class ParamsLearner:
def handle_log(self, t, which, msg):
if which == 'liveLocationKalman':
v_calibrated = msg.velocityCalibrated.value
v_calibrated_std = msg.velocityCalibrated.std
yaw_rate = msg.angularVelocityCalibrated.value[2]
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
self.active = v_calibrated[0] > 5
in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
if self.active and in_linear_region:
if self.active:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[[-yaw_rate]]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
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, 1e6*v_calibrated_std[1]**2])]))
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
@ -69,9 +59,14 @@ class ParamsLearner:
if self.carstate_counter % CARSTATE_DECIMATION == 0:
self.steering_angle = msg.steeringAngle
self.steering_pressed = msg.steeringPressed
self.speed = msg.vEgo
in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
self.active = self.speed > 5 and in_linear_region
if self.active:
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
if not self.active:
# Reset time when stopped so uncertainty doesn't grow