Feature/ab#2198 add speed and smooth location

README.md

@@ -42,4 +42,10 @@ The following Github Actions are available:
 
 ## Dependabot Version Update
 
-With [dependabot.yml](.github/dependabot.yml) a scheduled version update via Dependabot is configured. Dependabot creates a pull request if newer versions are available and the compilation is checked via PR build.
+With [dependabot.yml](.github/dependabot.yml) a scheduled version update via Dependabot is configured. Dependabot creates a pull request if newer versions are available and the compilation is checked via PR build.
+
+## Changelog
+### 2.0.0
+- Remove serial GPS reader
+- Update `visionapi` to 3.6.0 (extend `PositionMessage`)
+- Add GPS filtering (w/ speed/heading estimate), see `gps_filter` config section

positionsource/config.py

@@ -1,5 +1,4 @@
-from pathlib import Path
-from typing import Literal, Union, List
+from typing import Literal, List
 
 from pydantic import BaseModel, Field
 from pydantic_settings import BaseSettings, SettingsConfigDict
@@ -17,19 +16,28 @@ class GPSStaticConfig(BaseModel):
     type: Literal['static']
     lat: float = 0.0
     lon: float = 0.0
-
-class GPSSerialConfig(BaseModel):
-    type: Literal['serial']
-    serial_device: Path
+    read_interval_s: float = 0.5
 
 class GPSCommandConfig(BaseModel):
     type: Literal['command']
     command: Annotated[List[str], Field(min_length=1)]
+    read_timeout_s: float = 2
+
+class GPSFilterConfig(BaseModel):
+    enabled: Literal[True]
+    alpha: Annotated[float, Field(ge=0.0, le=1.0)] = 0.70
+    beta: Annotated[float, Field(ge=0.0, le=1.0)] = 0.04
+    spike_radius_m: Annotated[float, Field(ge=0.0)] = 80.0
+
+class GPSFilterConfigDisabled(BaseModel):
+    enabled: Literal[False] = False
 
 class SaePositionSourceConfig(BaseSettings):
     log_level: LogLevel = LogLevel.WARNING
     redis: RedisConfig = RedisConfig()
-    position_source: Annotated[Union[GPSStaticConfig, GPSSerialConfig, GPSCommandConfig], Field(discriminator='type')]
+    position_source: GPSStaticConfig | GPSCommandConfig = Field(discriminator='type')
+    gps_read_timeout_s: Annotated[float, Field(ge=0.0)] = 2.0
+    gps_filter: GPSFilterConfig | GPSFilterConfigDisabled = Field(discriminator='enabled', default=GPSFilterConfigDisabled())
     prometheus_port: Annotated[int, Field(ge=1024, le=65536)] = 8000
 
     model_config = SettingsConfigDict(env_nested_delimiter='__')

positionsource/gps/filter.py

@@ -0,0 +1,121 @@
+import math
+from typing import NamedTuple
+from datetime import datetime
+from .util import ll_to_xy, xy_to_ll
+
+
+class GPSFilterState(NamedTuple):
+    timestamp: datetime
+    lat: float
+    lon: float
+    speed_kmh: float
+    heading_deg: float
+
+
+class GPSFilter:
+    '''
+    An implementation of an alpha-beta filter for GPS position smoothing with additional spike rejection.
+    See: https://en.wikipedia.org/wiki/Alpha_beta_filter
+    '''
+    def __init__(
+        self,
+        alpha=0.75,
+        beta=0.05,
+        spike_radius_m=80.0,
+    ):
+        self.alpha = alpha
+        self.beta = beta
+
+        self.spike_radius_m = spike_radius_m
+
+        self._init_params()
+
+    def _init_params(self):
+        self.origin_lat = None
+        self.origin_lon = None
+
+        self.x = None
+        self.y = None
+        self.vx = 0.0
+        self.vy = 0.0
+
+        self.last_t = None
+        self.stop_timer = 0.0
+
+    def reset(self):
+        self._init_params()
+
+    def update(self, lat: float, lon: float, timestamp_ms: float) -> GPSFilterState:
+        """
+        lat/lon in decimal degrees
+        timestamp_ms = timestamp in milliseconds
+        """
+
+        t = timestamp_ms / 1000.0
+
+        # first point initializes system
+        if self.origin_lat is None:
+            self.origin_lat = lat
+            self.origin_lon = lon
+
+            self.x = 0.0
+            self.y = 0.0
+            self.last_t = t
+
+            return GPSFilterState(timestamp=datetime.fromtimestamp(t), lat=lat, lon=lon, speed_kmh=0.0, heading_deg=0.0)
+
+        # convert measurement to local coordinates
+        x_meas, y_meas = ll_to_xy(lat, lon, self.origin_lat, self.origin_lon)
+
+        dt = t - self.last_t
+        if dt <= 0:
+            dt = 1e-3
+
+        # ----------------------------
+        # Predict
+        # ----------------------------
+        x_pred = self.x + self.vx * dt
+        y_pred = self.y + self.vy * dt
+
+        # residual
+        rx = x_meas - x_pred
+        ry = y_meas - y_pred
+
+        residual_dist = math.hypot(rx, ry)
+
+        # ----------------------------
+        # Spike rejection
+        # ----------------------------
+        if residual_dist > self.spike_radius_m:
+            # ignore impossible jump
+            rx = 0.0
+            ry = 0.0
+
+        # ----------------------------
+        # Correct location and velocity
+        # ----------------------------
+        self.x = x_pred + self.alpha * rx
+        self.y = y_pred + self.alpha * ry
+
+        self.vx = self.vx + self.beta * rx / dt
+        self.vy = self.vy + self.beta * ry / dt
+
+        self.last_t = t
+
+        # ----------------------------
+        # Derived values
+        # ----------------------------
+        speed = math.hypot(self.vx, self.vy)
+
+        heading = math.degrees(math.atan2(self.vx, self.vy))
+        heading = (heading + 360) % 360
+
+        out_lat, out_lon = xy_to_ll(self.x, self.y, self.origin_lat, self.origin_lon)
+
+        return GPSFilterState(
+            timestamp=datetime.fromtimestamp(self.last_t),
+            lat=out_lat,
+            lon=out_lon,
+            speed_kmh=speed * 3.6,
+            heading_deg=heading,
+        )

positionsource/commandreader.py → positionsource/gps/reader/commandreader.py

@@ -7,7 +7,7 @@
 
 from pynmeagps import NMEAMessage, NMEAReader
 
-from .datatypes import GpsPosition
+from ...datatypes import GpsPosition
 
 logger = logging.getLogger(__name__)
 
@@ -17,8 +17,10 @@ class CommandGpsError(Exception):
 class CommandGpsReader:
     '''This class reads GPS data from a serial device and provides the latest position. It uses a separate thread to read data continuously.
         It needs to be closed properly to release the serial port.'''
-    def __init__(self, command: List[str]):
+    def __init__(self, command: List[str], read_timeout_s: float):
+        self._read_timeout_s = read_timeout_s
         self._stop_event = Event()
+        self._previous_position: GpsPosition = None
         self._current_position: GpsPosition = None
         self._read_thread: Thread = Thread(target=self._gps_read_loop, kwargs={'command': command, 'stop_event': self._stop_event}, daemon=True)
         self._last_position_lock = Lock()
@@ -28,6 +30,7 @@ def _gps_read_loop(self, command: List[str], stop_event: Event):
         '''This method runs in a separate thread. Do not call it directly.'''
         try:
             proc = None
+            prev_msg_time = None
             while not stop_event.is_set():
                 if proc is None:
                     proc = subprocess.Popen(args=command, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
@@ -38,7 +41,7 @@ def _gps_read_loop(self, command: List[str], stop_event: Event):
 
                 # Check if the started process is still alive
                 if (ret_code := proc.poll()) is not None:
-                    logger.warning(f'Subprocess has ended with code {ret_code}. Restarting...)')
+                    logger.warning(f'Subprocess has ended with code {ret_code}. Restarting...')
                     self._log_multiline_output(proc.stderr.readlines(), 'stderr', logging.WARNING)
                     proc = None
                     time.sleep(1)
@@ -57,6 +60,12 @@ def _gps_read_loop(self, command: List[str], stop_event: Event):
                 if nmea.msgID != 'GGA':
                     continue
 
+                # Ignore duplicate GGA messages (this has been observed in reality)
+                if prev_msg_time == nmea.time:
+                    continue
+
+                prev_msg_time = nmea.time
+
                 new_position = GpsPosition(
                     fix=nmea.quality == 1,
                     lat=nmea.lat if nmea.lat != '' else 0,
@@ -76,6 +85,24 @@ def _log_multiline_output(self, lines: List[bytes], prefix: str, log_level: int)
 
     @property
     def position(self) -> Optional[GpsPosition]:
+        '''Gets latest position reading. Blocks until position is updated. Returns None if read_timeout expires while waiting.'''
+        timeout_time = time.time() + self._read_timeout_s
+
+        position = self._get_position()
+
+        # Retry until either the position changes or the timeout expires
+        while position is None or position == self._previous_position:
+            if time.time() - timeout_time > 0:
+                position = None
+                break
+            time.sleep(0.01)
+            position = self._get_position()
+
+        self._previous_position = position
+
+        return position
+
+    def _get_position(self) -> Optional[GpsPosition]:
         with self._last_position_lock:
             return self._current_position
 

positionsource/staticreader.py → positionsource/gps/reader/staticreader.py

@@ -2,18 +2,19 @@
 import time
 from typing import Optional
 
-from .datatypes import GpsPosition
+from ...datatypes import GpsPosition
 
 
 class StaticReader:
     '''This class mimicks GPS reading by sending static configured position data.'''
-    def __init__(self, lat, lon):
+    def __init__(self, lat: float, lon: float, read_interval_s: float):
         self._lat = lat
         self._lon = lon
+        self._read_interval_s = read_interval_s
 
     @property
     def position(self) -> Optional[GpsPosition]:
-        time.sleep(0.5)
+        time.sleep(self._read_interval_s)
         return GpsPosition(fix=True, lat=self._lat, lon=self._lon, hdop=0, timestamp_utc_ms=int(time.time() * 1000))
 
     @property

positionsource/gps/util.py

@@ -0,0 +1,36 @@
+import math
+from typing import Tuple
+
+EARTH_RADIUS = 6378137.0  # meters
+
+def distance_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
+    # use ll_to_xy to convert to local meters and then compute distance from origin
+    dx, dy = ll_to_xy(lat1, lon1, lat2, lon2)
+    return math.hypot(dx, dy)
+
+def ll_to_xy(lat: float, lon: float, origin_lat: float, origin_lon: float) -> Tuple[float, float]:
+    """
+    Convert lat/lon to local meters (i.e. distance relative to origin)
+    """
+    dlat = math.radians(lat - origin_lat)
+    dlon = math.radians(lon - origin_lon)
+
+    mean_lat = math.radians((lat + origin_lat) / 2)
+
+    x = EARTH_RADIUS * dlon * math.cos(mean_lat)
+    y = EARTH_RADIUS * dlat
+    return x, y
+
+def xy_to_ll(x: float, y: float, origin_lat: float, origin_lon: float) -> Tuple[float, float]:
+    """
+    Convert local meters (i.e. distance relative to origin) to lat/lon
+    """
+    lat = origin_lat + math.degrees(y / EARTH_RADIUS)
+
+    mean_lat = math.radians((lat + origin_lat) / 2)
+
+    lon = origin_lon + math.degrees(
+        x / (EARTH_RADIUS * math.cos(mean_lat))
+    )
+
+    return lat, lon