mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-23 17:23:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

way cleaner

Browse Source 
old-commit-hash: a4ffd8c226ab1f286334c1cee9a90a03cbaec91a
This commit is contained in:
Harald Schafer
2020-05-15 14:24:04 -07:00
parent 8f4c431efc
commit 696fd01f9d
3 changed files with 26 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/models/constants.py
+2
View File
@@ -27,6 +27,7 @@ class ObservationKind:
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
ECEF_VEL = 31
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
@@ -36,6 +37,7 @@ class ObservationKind:
STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
names = [
'Unknown',
'No observation',
selfdrive/locationd/models/live_kf.py
+4 -1
View File
@@ -172,6 +172,7 @@ class LiveKalman():
h_speed_sym = sp.Matrix([speed * odo_scale])
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_imu_frame_sym = sp.Matrix(imu_angles)
h_relative_motion = sp.Matrix(quat_rot.T * v)
@@ -181,6 +182,7 @@ class LiveKalman():
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None],
[h_vel_sym, ObservationKind.ECEF_VEL, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None]]
@@ -197,7 +199,8 @@ class LiveKalman():
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]),
ObservationKind.ECEF_VEL: np.diag([1**2, 1**2, 1**2])}
# init filter
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)