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

Correct reference frame name in car_kf

Browse source
This commit is contained in:
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 = 22
PSEUDORANGE_RATE = 23 PSEUDORANGE_RATE = 23
CAL_DEVICE_FRAME_XY_SPEED = 24 # (x, y) [m/s] ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
CAL_DEVICE_FRAME_YAW_RATE = 25 # [rad/s] ROAD_FRAME_YAW_RATE = 25 # [rad/s]
STEER_ANGLE = 26 # [rad] STEER_ANGLE = 26 # [rad]
ANGLE_OFFSET_FAST = 27 # [rad] ANGLE_OFFSET_FAST = 27 # [rad]
STIFFNESS = 28 # [-] STIFFNESS = 28 # [-]
@ -87,8 +87,8 @@ class ObservationKind():
'GLONASS pseudorange', 'GLONASS pseudorange',
'GLONASS pseudorange rate', 'GLONASS pseudorange rate',
'Calibrated Device Frame x,y speed', 'Road Frame x,y speed',
'Calibrated Device Frame yaw rate', 'Road Frame yaw rate',
'Steer Angle', 'Steer Angle',
'Fast Angle Offset', 'Fast Angle Offset',
'Stiffness', 'Stiffness',

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

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

4
selfdrive/locationd/paramsd.py
View file

@ -32,8 +32,8 @@ class ParamsLearner:
self.update_active() self.update_active()
if self.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.ROAD_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_XY_SPEED, [[v_device[0], -v_device[1]]])
# Clamp values # Clamp values
x = self.kf.x x = self.kf.x