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

locationd: improve circular buffer handling with np.roll (#33470)

Browse Source 
improve circular buffer handling
This commit is contained in:
Dean Lee
2024-09-20 04:09:41 +08:00
committed by GitHub
parent 34f5ba46b3
commit 7a11d0180b
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.py
+3 -3
View File
@@ -49,7 +49,7 @@ class LocationEstimator:
self.debug = debug
self.posenet_stds = [POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2)
self.posenet_stds = np.array([POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2))
self.car_speed = 0.0
self.camodo_yawrate_distribution = np.array([0.0, 10.0]) # mean, std
self.device_from_calib = np.eye(3)
@@ -168,8 +168,8 @@ class LocationEstimator:
if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
self.posenet_stds.pop(0)
self.posenet_stds.append(trans_calib_std[0])
self.posenet_stds = np.roll(self.posenet_stds, -1)
self.posenet_stds[-1] = trans_calib_std[0]
# Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise
rot_calib_std *= 10