From 44357e33c2e906c6d9a242a14a58e8f7b7a2cfb4 Mon Sep 17 00:00:00 2001 From: Pascal Schade Date: Tue, 17 Dec 2024 16:47:27 +0100 Subject: [PATCH] fix heading --- field_friend/robot_locator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/field_friend/robot_locator.py b/field_friend/robot_locator.py index c27917f3..a2edf891 100644 --- a/field_friend/robot_locator.py +++ b/field_friend/robot_locator.py @@ -158,6 +158,7 @@ def handle_gnss_measurement(self, gnss_measurement: GnssMeasurement) -> None: return self.predict(gnss_measurement.time) pose = gnss_measurement.pose.to_local() + pose.yaw = self.x[2, 0] + rosys.helpers.angle(self.x[2, 0], pose.yaw) z = [[pose.x], [pose.y], [pose.yaw]] h = [[self.x[0, 0]], [self.x[1, 0]], [self.x[2, 0]]] H = [ @@ -195,7 +196,6 @@ def predict(self, time: float | None = None) -> None: self.x[0] += v * np.cos(theta) * dt self.x[1] += v * np.sin(theta) * dt self.x[2] += omega * dt - self.x[2] = rosys.helpers.eliminate_2pi(self.x[2]) self.x[3] += a * dt self.Sxx = F @ self.Sxx @ F.T + R self.update_frame()