pygnss 1.0.0__cp312-cp312-manylinux2010_x86_64.manylinux_2_12_x86_64.manylinux_2_28_x86_64.whl

pygnss/__init__.py

@@ -0,0 +1 @@
+__version__ = "1.0.0"

pygnss/_c_ext/src/hatanaka.c

@@ -0,0 +1,95 @@
+#include <Python.h>
+#include <datetime.h>
+
+#include "hatanaka/include/crx2rnx.h"
+
+
+static char* get_crx_line(void* _args, size_t n_max, char* dst) {
+
+    FILE* input_fh = (FILE*)_args;
+    return fgets(dst, n_max, input_fh);
+
+}
+
+static bool is_eof(void* _args) {
+
+    FILE* input_fh = (FILE*)_args;
+    return (fgetc(input_fh) == EOF);
+
+}
+
+static int on_measurement(const struct gnss_meas* gnss_meas, void* _args) {
+
+    static const int N_FIELDS = 5;  // Number of fields for struct gnss_meas
+
+    int ret = -1;
+    PyObject* list = (PyObject*)_args;
+
+    if (gnss_meas == NULL) {
+        goto exit;
+    }
+
+    PyDateTime_IMPORT;
+
+    // Create Python lists for each inner list
+    PyObject* row = PyList_New(N_FIELDS);
+
+    double timestamp = (double)gnss_meas->gps_time.tv_sec + (double)gnss_meas->gps_time.tv_nsec / 1e9;
+    PyObject* time_tuple = Py_BuildValue("(d)", timestamp);
+    PyObject* date_time = PyDateTime_FromTimestamp(time_tuple);
+
+    PyList_SetItem(row, 0, date_time);
+    PyList_SetItem(row, 1, PyUnicode_FromStringAndSize(gnss_meas->satid, 3));
+    PyList_SetItem(row, 2, PyUnicode_FromStringAndSize(gnss_meas->rinex3_code, 3));
+    PyList_SetItem(row, 3, PyFloat_FromDouble(gnss_meas->value));
+    PyList_SetItem(row, 4, PyLong_FromUnsignedLong(gnss_meas->lli));
+
+    // Add inner lists to the outer list
+    PyList_Append(list, row);
+    Py_DECREF(row);  // Decrement the reference count of 'row'
+
+    ret = 0;
+exit:
+    return ret;
+}
+
+PyObject *_read_crx(PyObject* self, PyObject* args, PyObject* kwargs) {
+
+    char *filename = NULL;
+    struct crx2rnx* crx2rnx = NULL;
+    int ret = -1;
+    PyObject* list = PyList_New(0);
+
+    struct crx2rnx_callbacks callbacks = {
+        .on_measurement = on_measurement,
+        .on_measurement_args = list
+    };
+
+    // Parse the filename argument
+    if (!PyArg_ParseTuple(args, "s", &filename)) {
+        PyErr_SetString(PyExc_TypeError, "Expected a string filename");
+        goto end;
+    }
+
+    // Open the file
+    FILE* fp = fopen(filename, "r");
+    if (fp == NULL) {
+        PyErr_SetString(PyExc_IOError, "Could not open file");
+        goto end;
+    }
+
+    crx2rnx = crx2rnx__init(false, false, NULL, get_crx_line, (void*)fp, is_eof, (void*)fp, &callbacks);
+
+    ret = crx2rnx__run(crx2rnx);
+
+    if (ret < 0) {
+        PyErr_SetString(PyExc_IOError, "There was an issue processing the Hatanaka file");
+        PyList_SetSlice(list, 0, PY_SSIZE_T_MAX, NULL);  // clear the list
+    }
+
+    // Clean-up
+    fclose(fp);
+end:
+    return list;
+
+}

pygnss/_c_ext/src/helpers.c

@@ -0,0 +1,17 @@
+#include <stdlib.h>
+#include <Python.h>
+
+#include "../include/helpers.h"
+
+PyObject *convert_to_pylist(const double* array, size_t n) {
+
+    Py_ssize_t len = n;
+    PyObject* list = PyList_New(len);
+
+    for (Py_ssize_t i = 0; i < len; i++) {
+        PyObject* value = PyFloat_FromDouble(array[i]);
+        PyList_SetItem(list, i, value);
+    }
+
+    return list;
+}

pygnss/_c_ext/src/mtable_init.c

@@ -0,0 +1,44 @@
+#include <Python.h>
+
+#include "hatanaka.h"
+
+static PyMethodDef module_methods[] = {
+    { "_read_crx",         (PyCFunction)_read_crx, METH_VARARGS | METH_KEYWORDS,
+      "Read a Hatanaka (gzip uncompressed) file and generate a numpy array\n\n"
+      ":param filename: Name of the Hatanaka file to process\n"
+      ":return: Numpy array\n\n"},
+      {NULL, NULL, 0, NULL},  /* Sentinel */
+};
+
+/*----------------------------------------------------------------------------*/
+
+static struct PyModuleDef module = {
+    PyModuleDef_HEAD_INIT,
+    "_c_ext", /* name of the module*/
+    "C extension methods",
+    -1,  // size of per-interpreter state of the module,
+         // or -1 if the module keeps state in global variables.
+    module_methods
+};
+
+
+PyMODINIT_FUNC PyInit__c_ext(void) {
+
+    PyObject* m = NULL;
+
+//     // Classes
+//     if (PyType_Ready(HatanakaReaderType) < 0) {
+//         goto end;
+//     }
+
+    m = PyModule_Create(&module);
+    if (m == NULL) {
+        goto end;
+    }
+
+//     Py_INCREF(HatanakaReaderType);
+//     PyModule_AddObject(m, "HatanakaReader", (PyObject*)HatanakaReaderType);
+
+end:
+    return m;
+}

pygnss/cl.py

@@ -0,0 +1,148 @@
+"""
+Program to perform various columnar operations on inputs
+
+All indicators have this format
+
+ 'x0'
+
+where 'x' can be one of the following
+
+- 'c' - Select column
+- 'd' - diff column relative to the previous value
+- 'f' - diff column relative to the first value of the column
+- 'm' - Compute the minutes elapsed since the first value (divide column by 60,
+        as it assumes that the values are in seconds))
+- 'h' - Compute the hours elapsed since the first value (divide column by 3600,
+        as it assumes that the values are in seconds))
+
+and '0' is the column number (1 based)
+
+Examples:
+
+(a) Select columns with the indicated order (first output 5th column and then the
+    first column)
+        cat file.txt | cl c5 c1
+
+(b) Select 6th column and output 1st column relative to the first one
+        cat file.txt | cl c6 f1
+
+(c) Make a diff of the third column relative to the first value
+        cat file.txt | cl f3
+"""
+import argparse
+import sys
+
+
+class ColumnProcess:
+    """
+    Class that manages the processing of a set of fields based on some criteria
+    """
+
+    def __init__(self, colprocstr):
+        """
+        Class initialization. This method receives a string defining the type
+        of operation to be performed
+        """
+
+        if len(colprocstr) < 2:
+            raise ValueError(f"Do not know how to interpret [ {colprocstr} ], "
+                             "column selector should be of the form 'n0', with "
+                             "'n' being a character and '0' a column number")
+
+        self.process_type = colprocstr[0]
+
+        # Obtain the column number, taking into account that the indices must
+        # be translated from 1-based to 0-based
+        self.process_column = int(colprocstr[1:]) - 1
+
+        self.previous_value = None
+        self.first_value = None
+
+    def process(self, fields):
+        """
+        Process a set of fields. Raise an exception if
+        """
+
+        if self.process_column >= len(fields):
+            raise IndexError(f"Unable to fecth column [ {self.process_column + 1} ] (1-based) "
+                             f"in line with [ {len(fields)} ] fields. "
+                             f"Offending line [ {' '.join(fields)} ]\n")
+
+        column_value = fields[self.process_column]
+
+        if self.process_type == 'c':
+
+            return column_value
+
+        elif self.process_type == 'f' or self.process_type == 'm' or self.process_type == 'h':
+
+            incoming_value = float(column_value)
+
+            if self.first_value is None:
+                self.first_value = incoming_value
+
+            value = incoming_value - self.first_value
+
+            if self.process_type == 'm':
+                value = value / 60.0
+            elif self.process_type == 'h':
+                value = value / 3600.0
+
+            return str(value)
+
+        elif self.process_type == 'd':
+
+            incoming_value = float(column_value)
+
+            if self.previous_value is None:
+                self.previous_value = incoming_value
+
+            value = incoming_value - self.previous_value
+
+            # Update internal value only if the process method is the difference
+            # relative to the previous value
+            if self.process_type == 'd':
+                self.previous_value = incoming_value
+
+            return str(value)
+
+        else:
+            raise ValueError("Do not know what process type is '%c'" % self.process_type)
+
+    def __str__(self):
+
+        return self.__repr__()
+
+    def __repr__(self):
+
+        return "Process type [ %s ], process column [ %d ]\n" % (self.process_type, self.process_column)
+
+
+def entry_point():
+
+    # Process the options of the executable
+
+    parser = argparse.ArgumentParser(description=__doc__,
+                                     formatter_class=argparse.RawDescriptionHelpFormatter)
+
+    parser.add_argument('columns', metavar='<selector>', type=str, nargs='+',
+                        help="Set of column selectors and operators")
+
+    args = parser.parse_args()
+
+    # Make an array of objects that will take care of processing the fields
+    colprocs = [ColumnProcess(colproc) for colproc in args.columns]
+
+    for line in sys.stdin:
+
+        # If line is empty, print an empty line
+        if len(line.strip()) == 0:
+            sys.stdout.write("\n")
+            continue
+
+        fields = line.strip().split()
+
+        # Process each column
+        newfields = [cp.process(fields) for cp in colprocs]
+
+        sys.stdout.write(" ".join(newfields) + "\n")

pygnss/constants.py

@@ -0,0 +1,4 @@
+SPEED_OF_LIGHT = 299792458  # in vacuum [m/s]
+
+EARTH_GRAVITATION_PARAM_MU = 3.986005e14
+EARTH_ROTATION_RATE = 7.2921151466999996e-05

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,62 @@
+"""
+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 Tuple
+
+import numpy as np
+
+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
+        """

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
+
+
+class Ekf(object):
+    """
+    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,73 @@
+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)
+        (array([2.23606798, 8.06225775, 9.21954446]), None)
+
+        >>> model.to_observations(state_m, compute_jacobian=True)
+        (array([2.23606798, 8.06225775, 9.21954446]), 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 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