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

Correct reference frame name in car_kf

pull/1224/head
Willem Melching 2020-03-09 12:49:52 -07:00
parent 01ac9d4722
commit 01a14400cb
3 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -56,8 +56,8 @@ class ObservationKind():
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
CAL_DEVICE_FRAME_XY_SPEED = 24 # (x, y) [m/s]
CAL_DEVICE_FRAME_YAW_RATE = 25 # [rad/s]
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
STEER_ANGLE = 26 # [rad]
ANGLE_OFFSET_FAST = 27 # [rad]
STIFFNESS = 28 # [-]
@ -87,8 +87,8 @@ class ObservationKind():
'GLONASS pseudorange',
'GLONASS pseudorange rate',
'Calibrated Device Frame x,y speed',
'Calibrated Device Frame yaw rate',
'Road Frame x,y speed',
'Road Frame yaw rate',
'Steer Angle',
'Fast Angle Offset',
'Stiffness',

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

@ -69,15 +69,15 @@ class CarKalman():
])
obs_noise = {
ObservationKind.CAL_DEVICE_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]),
ObservationKind.CAL_DEVICE_FRAME_YAW_RATE: np.atleast_2d(math.radians(0.1)**2),
ObservationKind.ROAD_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]),
ObservationKind.ROAD_FRAME_YAW_RATE: np.atleast_2d(math.radians(0.1)**2),
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.1)**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),
}
maha_test_kinds = [] # [ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED]
maha_test_kinds = [] # [ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_XY_SPEED]
@staticmethod
def generate_code():
@ -141,8 +141,8 @@ class CarKalman():
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, None],
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_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],

4
selfdrive/locationd/paramsd.py
View File

@ -32,8 +32,8 @@ class ParamsLearner:
self.update_active()
if self.active:
self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [-yaw_rate])
self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, [[v_device[0], -v_device[1]]])
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_YAW_RATE, [-yaw_rate])
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_XY_SPEED, [[v_device[0], -v_device[1]]])
# Clamp values
x = self.kf.x