Update state_estimation.py

Author: Jugthegreat

GEMstack/onboard/perception/state_estimation.py

@@ -62,6 +62,11 @@ def update(self) -> VehicleState:
 
         #filtering speed
         raw.v = self.gnss_speed
+
+        # Assume no backward slide, use gear to decide velocity sign 
+        if raw.gear == -1:
+            raw.v *= -1
+
         #filt_vel     = self.speed_filter(raw.v)
         #raw.v = filt_vel
         return raw
@@ -90,4 +95,4 @@ def healthy(self):
         return self.vehicle_state is not None
 
     def update(self) -> VehicleState:
-        return self.vehicle_state
+        return self.vehicle_state