Conversation
There was a problem hiding this comment.
Pull request overview
Refactors locationd smoothing utilities by centralizing the masked symmetric moving-average helper in helpers.py and introducing a reusable PoseSmoother for symmetric Gaussian smoothing of Pose measurements.
Changes:
- Moved
masked_symmetric_moving_averagefromlagd.pyintoselfdrive/locationd/helpers.pyand updatedlagd.pyto import it. - Added
PoseSmoothertohelpers.pyfor windowed symmetric smoothing overPosebuffers.
Reviewed changes
Copilot reviewed 2 out of 2 changed files in this pull request and generated 2 comments.
| File | Description |
|---|---|
selfdrive/locationd/lagd.py |
Imports masked_symmetric_moving_average from helpers.py after removing the local definition. |
selfdrive/locationd/helpers.py |
Adds masked_symmetric_moving_average utility and introduces the new PoseSmoother class. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| def __init__(self, k: int, sigma: float): | ||
| self.k, self.sigma = k, sigma | ||
| self.t_buf = deque(maxlen=k) | ||
| self.pose_buf = deque(maxlen=k) | ||
| self.w = self._symmetric_average_weights(k, sigma) | ||
|
|
There was a problem hiding this comment.
PoseSmoother computes symmetric Gaussian weights and then chooses the center timestamp at k//2, but there’s no validation that k is positive/odd (and sigma > 0). With an even k the “center” is ambiguous and the smoothing becomes time-biased; with sigma<=0 the weights become invalid. Consider asserting k >= 1 and k % 2 == 1 (and sigma > 0) in __init__ to match the assumptions in the implementation (and the existing masked_symmetric_moving_average).
selfdrive/locationd/helpers.py
Outdated
| def feed_pose(self, t: int, pose: Pose): | ||
| self.t_buf.append(t) | ||
| self.pose_buf.append(pose) | ||
|
|
||
| def build_smoothed_pose(self) -> tuple[int, Pose] | None: |
There was a problem hiding this comment.
The time parameter for PoseSmoother.feed_pose/build_smoothed_pose is annotated as int, but locationd/lagd consistently use t: float seconds for log time. Using int here is misleading for callers and type-checkers; consider changing these annotations (and the return type) to float to match the rest of the locationd pipeline.
| def feed_pose(self, t: int, pose: Pose): | |
| self.t_buf.append(t) | |
| self.pose_buf.append(pose) | |
| def build_smoothed_pose(self) -> tuple[int, Pose] | None: | |
| def feed_pose(self, t: float, pose: Pose): | |
| self.t_buf.append(t) | |
| self.pose_buf.append(pose) | |
| def build_smoothed_pose(self) -> tuple[float, Pose] | None: |
No description provided.