pygnss 2.1.2__cp314-cp314t-macosx_11_0_arm64.whl

pygnss/decorator.py

@@ -0,0 +1,47 @@
+import gzip
+from functools import wraps
+import subprocess
+import warnings
+
+
+def deprecated(alternative):
+    def decorator(func):
+        def new_func(*args, **kwargs):
+            # Raise a DeprecationWarning with the specified message.
+            message = f"Call to deprecated function {func.__name__}."
+            if alternative:
+                message += f" Use {alternative} instead."
+            warnings.warn(message, DeprecationWarning, stacklevel=2)
+            return func(*args, **kwargs)
+        return new_func
+    return decorator
+
+
+def read_contents(func):
+    """
+    Decorator to handle gzip compression based on filename and pass its contents
+    to the function
+    """
+
+    @wraps(func)
+    def wrapper(filename, *args, **kwargs):
+
+        doc = None
+
+        if filename.endswith('.gz'):
+            with gzip.open(filename, 'rt', encoding='utf-8') as fh:
+                doc = fh.read()
+        elif filename.endswith('.Z'):
+            result = subprocess.run(['uncompress', '-c', filename],
+                                    stdout=subprocess.PIPE,
+                                    stderr=subprocess.PIPE,
+                                    check=True,
+                                    text=True)
+            doc = result.stdout
+        else:
+            with open(filename, 'rt', encoding='utf-8') as fh:
+                doc = fh.read()
+
+        return func(doc, *args, **kwargs)
+
+    return wrapper

pygnss/file.py

@@ -0,0 +1,36 @@
+from functools import wraps
+from typing import IO
+
+
+def process_filename_or_file_handler(mode):
+    def decorator(func):
+        @wraps(func)
+        def wrapper(input, *args, **kwargs):
+            if isinstance(input, str):
+                with open(input, mode) as fh:
+                    return func(fh, *args, **kwargs)
+            else:
+                return func(input, *args, **kwargs)
+        return wrapper
+    return decorator
+
+
+def grep_lines(filename: str, pattern_string: str):
+    """
+    Generator function used to grep lines from a file. Can be used in methods
+    such as numpy.genfromtxt, ...
+
+    >>> generator = grep_lines(filename, "pattern")
+    >>> data = numpy.loadtxt(generator)
+    """
+
+    with open(filename, 'r') as fh:
+        for line in fh:
+            if pattern_string in line:
+                yield line
+
+
+def skip_lines(fh: IO, n_lines: int):
+
+    for _ in range(n_lines):
+        fh.readline()

pygnss/filter/__init__.py

@@ -0,0 +1,77 @@
+"""
+Module for the filter class
+
+Some notation conventions:
+
+- $x_m$ Predicted state from the previous k-1 state
+- $y_m$ indicates the observations resulted from the predicted
+state ($x_m$)
+- $H$ is the design (Jacobian) matrix, that translates from state to observation
+(i.e. $y = H \\cdot x$)
+- $\\Phi$ is the state transition matrix, that translates from the
+state k-1 to the predicted state ($x_m$)
+"""
+from abc import ABC, abstractmethod
+from collections import namedtuple
+from typing import List
+
+import numpy as np
+
+State = List[float]  # e.g. np.array
+
+ModelObs = namedtuple('ModelObs', ('y_m', 'H'))  # y_m must be an array of arrays (2D shaped)
+
+
+class Model(ABC):
+    """
+    Abstract class that declares the interface for entities that model
+    an entity to be used by an estimation filter
+    """
+
+    @abstractmethod
+    def propagate_state(self, state: np.array) -> np.array:
+        """
+        Propagate a state from time k-1 to k
+        """
+
+    @abstractmethod
+    def to_observations(self, state: np.array, compute_jacobian: bool = False, **kwargs) -> ModelObs:
+        """
+        Propagate a state to its corresponding modelled observations (i.e.
+        compute expected observations/measurements for the input state)
+
+        :return: a tuple where the first element are the observations and the second
+        is the Jacobian matrix (if compute_jacobian is True, otherwise the second
+        element will be None)
+        """
+
+    def Phi(self):
+        """
+        Provide with the state transition matrix (also noted F in certain
+        Kalman notation)
+        """
+
+
+class StateHandler(ABC):
+    """
+    Abstract class that handles the state generated by UKF
+    """
+
+    @abstractmethod
+    def process_state(self, state: np.array, covariance_matrix: np.array, **kwargs):
+        """
+        Process the state and associated covariance_matrix
+        """
+
+
+class FilterInterface(ABC):
+    """Interface for the Filter class"""
+
+    @abstractmethod
+    def process(self, y_k: np.array, R: np.array, **kwargs):
+        """
+        Process an observation batch
+
+        :param y_k: object that contains the observations
+        :param R: matrix with the covariance of the measurement (i.e. measurement noise)
+        """

pygnss/filter/ekf.py

@@ -0,0 +1,80 @@
+"""
+Module for the EKF
+"""
+import logging
+from typing import Tuple
+
+import numpy as np
+
+from . import StateHandler, Model, FilterInterface
+
+
+class Ekf(FilterInterface):
+    """
+    Extended Kalman Filter (EKF)
+    """
+
+    def __init__(self,
+                 x0: np.array,
+                 P0: np.array,
+                 Q: np.array,
+                 model: Model,
+                 state_handler: StateHandler,
+                 logger: logging.Logger = logging):
+        """
+        Initialize the EKF filter object
+        """
+
+        self.x = x0
+        self.P = P0
+        self.Q = Q
+
+        self.model = model
+        self.state_handler = state_handler
+
+        self.logger = logger
+
+        self.L = len(self.x)
+
+    def process(self, y_k: np.array, R: np.array, **kwargs):
+        """
+        Process an observation batch
+        """
+
+        # Time update ----------------------------------------------------------
+        x_m, P_m = self._time_update()
+
+        # Measurement update ---------------------------------------------------
+        y_m, H = self.model.to_observations(x_m, compute_jacobian=True, **kwargs)
+
+        P_yy = H @ P_m @ H.T + R
+        P_xy = P_m @ H.T
+
+        self.x = x_m
+        self.P = P_m
+
+        try:
+            K = P_xy @ np.linalg.inv(P_yy)  # Calculate Kalman gain
+
+            self.x = self.x + K @ (y_k - y_m)  # Update state estimate
+            self.P = self.P - K @ H @ P_m  # Update covariance estimate
+
+        except np.linalg.LinAlgError as e:
+            self.logger.warning(f'Unable to compute state, keeping previous one. Error: {e}')
+
+        # Compute postfit residuals
+        r = y_k - self.model.to_observations(self.x, **kwargs).y_m
+
+        self.state_handler.process_state(self.x, self.P, postfits=r, **kwargs)
+
+    def _time_update(self) -> Tuple[np.array, np.array]:
+        """
+        Perform a time update step
+        """
+
+        Phi = self.model.Phi
+
+        x_m = self.model.propagate_state(self.x)
+        P_m = Phi @ self.P @ Phi.T + self.Q
+
+        return x_m, P_m

pygnss/filter/models.py

@@ -0,0 +1,74 @@
+import numpy as np
+
+from . import Model, ModelObs
+
+
+class RangePositioning2D(Model):
+    """
+    Basic 2D range-based positioning model
+    """
+
+    def __init__(self, Phi: np.array, nodes: np.array):
+        """
+        Instantiate a RangePositioning2D
+
+        :param Phi: a 2 x 2 matrix that propagates the state from k-1 to k
+        :param nodes: list of nodes of the positioning system, from which the
+        range will be computed
+        """
+        self._Phi = Phi
+        self.nodes = nodes
+
+    def propagate_state(self, state: np.array):
+        """
+        Propagate the state from k-1 to k
+
+        >>> Phi = np.eye(2)
+        >>> nodes = np.array([[0, 0], [0, 10], [10, 0]])
+        >>> model = RangePositioning2D(Phi, nodes)
+        >>> state_m = np.array([1, 2])
+        >>> model.propagate_state(state_m)
+        array([1., 2.])
+        """
+
+        return np.dot(self._Phi, state)
+
+    def to_observations(self, state: np.array, compute_jacobian: bool = False) -> ModelObs:
+        """
+        Convert the state into observations using a range based 2D positioning model
+
+        >>> Phi = np.eye(2)
+        >>> nodes = np.array([[0, 0], [0, 10], [10, 0]])
+        >>> model = RangePositioning2D(Phi, nodes)
+        >>> state_m = np.array([1, 2])
+        >>> model.to_observations(state_m)
+        ModelObs(y_m=array([2.23606798, 8.06225775, 9.21954446]), H=None)
+
+        >>> model.to_observations(state_m, compute_jacobian=True)
+        ModelObs(y_m=array([2.23606798, 8.06225775, 9.21954446]), H=array([[ 0.4472136 ,  0.89442719],
+               [ 0.12403473, -0.99227788],
+               [-0.97618706,  0.21693046]]))
+        """
+        rho = state - self.nodes
+        ranges = np.sqrt(np.sum(np.power(rho, 2), axis=1))
+
+        H = None
+
+        if compute_jacobian is True:
+            H = rho / ranges[:, np.newaxis]
+        # Return a ModelObs namedtuple for compatibility with the filter
+        # API which expects an object with a ``y_m`` attribute.
+        return ModelObs(ranges, H)
+
+    def Phi(self):
+        """
+        Get the state transition matrix
+
+        >>> Phi = np.eye(2)
+        >>> nodes = np.array([[0, 0], [0, 10], [10, 0]])
+        >>> model = RangePositioning2D(Phi, nodes)
+        >>> model.Phi()
+        array([[1., 0.],
+               [0., 1.]])
+        """
+        return self._Phi