Merge branch 's2025_EstCal' of https://github.com/krishauser/GEMstack into s2025_EstCal

Author: michalj1

GEMstack/onboard/perception/gnss_kalman_filter.py

@@ -21,12 +21,15 @@ def __init__(self):
         self.Q = np.eye(12) * 0.1
         self.R = np.eye(6) * 0.1
 
-    def update(self, time: float, state: ObjectPose = None, th = 0) -> ObjectPose:
+        
+        self.speed = None #TODO
+
+    def update(self, time: float, state: ObjectPose = None, speed = None, th = 0) -> ObjectPose:
         if not self.is_initialized:
             if state is None:
                 return None
             else:
-                self._initialize(state, time)
+                self._initialize(state, speed, time)
                 return self._current_pose()
 
         dt = time - self.last_time
@@ -59,7 +62,7 @@ def update(self, time: float, state: ObjectPose = None, th = 0) -> ObjectPose:
         self.last_time = time
         return self._current_pose()
 
-    def _initialize(self, state: ObjectPose, time: float):
+    def _initialize(self, state: ObjectPose, speed: float, time: float):
         self.state = np.zeros(12)
         self.state[0] = state.x
         self.state[2] = state.y
@@ -68,6 +71,8 @@ def _initialize(self, state: ObjectPose, time: float):
         self.state[8] = state.roll
         self.state[10] = state.pitch
 
+        self.speed = speed #TODO
+
         self.covariance = np.eye(12) * 0.1
         for i in [1, 3, 5, 7, 9, 11]:
             self.covariance[i, i] = 10.0  # Higher uncertainty for velocities
@@ -92,7 +97,7 @@ def _current_pose(self) -> ObjectPose:
             yaw=self.state[6],
             roll=self.state[8],
             pitch=self.state[10]
-        )
+        ),self.speed
 
 def test0():
     th = 0.95  # 95% confidence threshold
@@ -109,7 +114,7 @@ def test0():
     print("-- Initialization --")
     print("Update 0:", kf.update(0, ObjectPose(ObjectFrameEnum.GLOBAL,
         0, -0.118092, 51.509865, 35.0, 0, 0, 0
-    ), th))
+    ), 0.00017,th=th))
 
     # Straight movement north at 60 km/h (~16.67 m/s)
     print("\n-- Straight movement --")
@@ -125,7 +130,7 @@ def test0():
         5.0,   # Slight right turn
         1.5,    # Small roll from turning
         0.5     # Small pitch from acceleration
-    ), th))
+    ), 0.00017,th=th))
 
     # Turning right sequence with realistic coordinate changes
     print("\n-- Right turn sequence --")
@@ -138,7 +143,7 @@ def test0():
             5.0 + 15*(t-3),  # Increasing yaw
             1.5 + 0.5*(t-3),  # Roll increasing
             0.5 - 0.2*(t-3)   # Pitch decreasing
-        ), th))
+        ), 0.00017,th=th))
 
     # Hill climb with pitch changes
     print("\n-- Hill climb --")
@@ -151,7 +156,7 @@ def test0():
         35.0,   # Completed turn
         2.0,    # Reduced roll
         4.0     # Significant pitch from hill
-    ), th))
+    ), 0.00017,th=th))
 
     # Test outlier rejection (sudden jump to China coordinates)
     print("\n-- Outlier test --")
@@ -164,7 +169,7 @@ def test0():
         35.0,        # Same yaw
         2.0,
         4.0
-    ), th))  # Should be rejected by threshold
+    ), 0.00017,th=th))
 
     # Continue normal operation
     print("\n-- Post-outlier recovery --")
@@ -177,7 +182,7 @@ def test0():
             35.0 + 2*(t-9),
             2.0 - 0.3*(t-9),
             4.0 - 0.5*(t-9)
-        ), th))
+        ), 0.00017,th=th))
 
 if __name__ == "__main__":
     import sys

GEMstack/onboard/perception/kalman_filter_estimation.py

@@ -0,0 +1,54 @@
+from dataclasses import replace
+import math
+from typing import List
+from ...utils import settings
+from ...mathutils import transforms
+from ...state.vehicle import VehicleState,VehicleGearEnum
+from ...state.physical_object import ObjectFrameEnum,ObjectPose,convert_xyhead
+from ...knowledge.vehicle.geometry import front2steer,steer2front
+from ...mathutils.signal import OnlineLowPassFilter
+from ..interface.gem import GEMInterface
+from ..component import Component
+from ..interface.gem import GNSSReading
+
+import numpy as np
+import open3d as o3d
+import copy
+import utm
+import time
+import argparse
+import os
+import glob
+from scipy.spatial.transform import Rotation as R
+from .gnss_kalman_filter import GNSSKalmanFilter
+import rospy
+
+class KFStateEstimator(Component):
+    def __init__(self,vehicle_interface : GEMInterface):
+        self.vehicle_interface = vehicle_interface
+        self.filter = GNSSKalmanFilter
+        vehicle_interface.subscribe_sensor('gnss',self.gnss_callback,GNSSReading)
+        #TODO create subscription on mapbased estimation with self.map_based_estimation_callback
+    # Get GNSS information
+    def gnss_callback(self, reading : GNSSReading):
+        self.filter.update(rospy.get_time(),reading.pose,reading.speed,0.98)
+
+    def map_based_estimation_callback(self, reading : GNSSReading):
+        self.filter.update(rospy.get_time(),reading.pose,reading.speed,0.93)
+
+    def rate(self):
+        return 1.0
+    
+    def state_outputs(self) -> List[str]:
+        return ['vehicle']
+
+    def healthy(self):
+        return self.filter.is_initialized
+
+    def update(self) -> VehicleState:
+
+        return GNSSReading(
+            *(self.filter.update(rospy.get_time())),
+            "ok",#or error but when
+        )
+            

GEMstack/onboard/perception/map_based_estimation.py

@@ -218,9 +218,7 @@ def multi_scale_icp(source, target, voxel_sizes=[2.0, 1.0, 0.5], max_iterations=
 
     return current_transform
 
-
-
-def transform_to_pose(transformation_matrix,origin):
+def transform_to_pose(transformation_matrix):
     """Convert transformation matrix to position and orientation (RPY)."""
     # Extract translation
     x, y, z = transformation_matrix[:3, 3]