Merge pull request #122 from Stanford-NavLab/v0.1.12

V0.1.12

docs/source/conf.py

@@ -12,6 +12,11 @@
 #
 import os
 import sys
+import inspect
+import subprocess
+from os.path import relpath, dirname
+
+
 sys.path.insert(0, os.path.abspath('.'))
 sys.path.insert(0, os.path.abspath('../'))
 sys.path.insert(0, os.path.abspath('../../'))
@@ -40,6 +45,7 @@
 extensions = [
                 'sphinx.ext.autodoc',
                 'sphinx.ext.napoleon',
+                'sphinx.ext.linkcode',
                 'nbsphinx',
                 'nbsphinx_link',
                 'IPython.sphinxext.ipython_console_highlighting',
@@ -104,3 +110,124 @@
 
 # document __init__ methods
 autoclass_content = 'both'
+
+# Function to find URLs for the source code on GitHub for built docs
+
+# The original code to find the head tag was taken from:
+# https://gist.github.com/nlgranger/55ff2e7ff10c280731348a16d569cb73
+# This code was modified to use the current commit when the code differs from
+# main or a tag
+
+#Default to the main branch
+linkcode_revision = "main"
+
+
+#Default to the main branch, default to main and tags not existing
+linkcode_revision = "main"
+in_main = False
+tagged = False
+
+
+# lock to commit number
+cmd = "git log -n1 --pretty=%H"
+head = subprocess.check_output(cmd.split()).strip().decode('utf-8')
+# if we are on main's HEAD, use main as reference irrespective of
+# what branch you are on
+cmd = "git log --first-parent main -n1 --pretty=%H"
+main = subprocess.check_output(cmd.split()).strip().decode('utf-8')
+if head == main:
+    in_main = True
+
+# if we have a tag, use tag as reference, irrespective of what branch
+# you are actually on
+try:
+    cmd = "git describe --exact-match --tags " + head
+    tag = subprocess.check_output(cmd.split(" ")).strip().decode('utf-8')
+    linkcode_revision = tag
+    tagged = True
+except subprocess.CalledProcessError:
+    pass
+
+# If the current branch is main, or a tag exists, use the branch name.
+# If not, use the commit number
+if not tagged and not in_main:
+    linkcode_revision = head
+
+linkcode_url = "https://github.com/Stanford-NavLab/gnss_lib_py/blob/" \
+               + linkcode_revision + "/{filepath}#L{linestart}-L{linestop}"
+
+
+
+def linkcode_resolve(domain, info):
+    """Return GitHub link to Python file for docs.
+
+    This function does not return a link for non-Python objects.
+    For Python objects, `domain == 'py'`, `info['module']` contains the
+    name of the module containing the method being documented, and
+    `info['fullname']` contains the name of the method.
+
+    Notes
+    -----
+    Based off the numpy implementation of linkcode_resolve:
+    https://github.com/numpy/numpy/blob/2f375c0f9f19085684c9712d602d22a2b4cb4c8e/doc/source/conf.py#L443
+    Retrieved on 1 Jul, 2023.
+    """
+    if domain != 'py':
+        return None
+
+    modname = info['module']
+    fullname = info['fullname']
+    submod = sys.modules.get(modname)
+    if submod is None:
+        return None
+
+    obj = submod
+    for part in fullname.split('.'):
+        try:
+            obj = getattr(obj, part)
+        except Exception:
+            return None
+
+    # strip decorators, which would resolve to the source of the decorator
+    # possibly an upstream bug in getsourcefile, bpo-1764286
+    try:
+        unwrap = inspect.unwrap
+    except AttributeError:
+        pass
+    else:
+        obj = unwrap(obj)
+    filepath = None
+    lineno = None
+
+    if filepath is None:
+        try:
+            filepath = inspect.getsourcefile(obj)
+        except Exception:
+            filepath = None
+        if not filepath:
+            return None
+        #NOTE: Re-export filtering turned off because
+        # # Ignore re-exports as their source files are not within the gnss_lib_py repo
+        # module = inspect.getmodule(obj)
+        # if module is not None and not module.__name__.startswith("gnss_lib_py"):
+        #     return "no_module_not_gnss_lib_py"
+
+        try:
+            source, lineno = inspect.getsourcelines(obj)
+        except Exception:
+            lineno = ""
+        # The following line of code first finds the relative path from
+        # the location of conf.py and then goes up to the root directory
+
+        root_glp_path = os.path.join(dirname(os.path.abspath(__file__)), '../..')
+        filepath = relpath(filepath, root_glp_path)
+
+    if lineno:
+        linestart = lineno
+        linestop = lineno + len(source) - 1
+    else:
+        linestart = ""
+        linestop = ""
+    codelink = linkcode_url.format(
+            filepath=filepath, linestart=linestart, linestop=linestop)
+    return codelink

docs/source/reference/reference.rst

@@ -26,6 +26,29 @@ in learning more about GNSS:
    a visually-appealing and interactive blog post about some of the
    basic principles of GNSS positioning.
 
+Reference Documents for GNSS Standards
+--------------------------------------
+
+GNSS constellations and receivers use standardized file formats to transfer
+information such as estimated receiver coordinates, broadcast ephemeris
+parameters, and precise ephimerides.
+The parsers in ``gnss_lib_py`` are based on standard documentation for
+the GNSS constellations and file types, which are listed below along with
+their use in ``gnss_lib_py``.
+
+  * *Rinex v2.11* (`version format document <https://geodesy.noaa.gov/corsdata/RINEX211.txt>`__
+    retrieved on 2nd July, 2023): for parsing broadcast navigation ephimerides.
+  * *Rinex v3.05* (`version format document <https://files.igs.org/pub/data/format/rinex305.pdf>`__
+    retrieved on 2nd July, 2023): for parsing broadcast navigation ephimerides.
+  * *Rinex v4.00* (`version format document <https://files.igs.org/pub/data/format/rinex_4.00.pdf>`__
+    retrieved on 2nd July, 2023): currently not supported by ``gnss_lib_py``.
+  * *NMEA* (`reference manual <https://www.sparkfun.com/datasheets/GPS/NMEA%20Reference%20Manual-Rev2.1-Dec07.pdf>`__
+    retrieved on 23rd June, 2023): for parsing NMEA files with GGA and RMC messages.
+  * *SP3*: used to determine SV positions for precise
+  * *GLONASS ICD* (retrieved from this `link <https://www.unavco.org/help/glossary/docs/ICD_GLONASS_4.0_(1998)_en.pdf>`__
+    retrieved on 27th June, 2023): for determining GLOASS SV states from
+    broadcast satellite positions, velocities, and accelerations.
+
 Package Architecture
 --------------------
 

gnss_lib_py/algorithms/snapshot.py

@@ -14,11 +14,12 @@
 import numpy as np
 
 from gnss_lib_py.parsers.navdata import NavData
+from gnss_lib_py.utils import constants as consts
 from gnss_lib_py.utils.coordinates import ecef_to_geodetic
 
 def solve_wls(measurements, weight_type = None, only_bias = False,
               receiver_state=None, tol = 1e-7, max_count = 20,
-              delta_t_decimals=-2):
+              sv_rx_time=False, delta_t_decimals=-2):
     """Runs weighted least squares across each timestep.
 
     Runs weighted least squares across each timestep and adds a new
@@ -29,13 +30,7 @@ def solve_wls(measurements, weight_type = None, only_bias = False,
     in rx_est_m will be updated if only_bias is set to True.
 
     If only_bias is set to True, then the receiver position must also
-    be passed in as the receiver_state
-
-    receiver_state : gnss_lib_py.parsers.navdata.NavData
-        Either estimated or ground truth receiver position in ECEF frame
-        in meters as an instance of the NavData class with the
-        following rows: ``x_rx*_m``, `y_rx*_m``, ``z_rx*_m``,
-        ``gps_millis``.
+    be passed in as the receiver_state.
 
     Parameters
     ----------
@@ -56,6 +51,14 @@ def solve_wls(measurements, weight_type = None, only_bias = False,
     max_count : int
         Number of maximum iterations before process is aborted and
         solution returned.
+    sv_rx_time : bool
+        Flag that specifies whether the input SV positions are in the ECEF
+        frame of reference corresponding to when the measurements were
+        received. If set to `True`, the satellite positions are used as
+        is. The default value is `False`, in which case the ECEF positions
+        are assumed to in the ECEF frame at the time of signal transmission
+        and are converted to the ECEF frame at the time of signal reception,
+        while solving the WLS problem.
     delta_t_decimals : int
             Decimal places after which times are considered equal.
 
@@ -102,6 +105,7 @@ def solve_wls(measurements, weight_type = None, only_bias = False,
         if weight_type is not None:
             if isinstance(weight_type,str) and weight_type in measurements.rows:
                 weights = measurement_subset[weight_type].reshape(-1,1)
+                weights = weights[not_nan_indexes]
             else:
                 raise TypeError("WLS weights must be None or row"\
                                 +" in NavData")
@@ -118,7 +122,7 @@ def solve_wls(measurements, weight_type = None, only_bias = False,
                                               ,0].reshape(-1,1),
                                              position[3])) # clock bias
             position = wls(position, pos_sv_m, corr_pr_m, weights,
-                           only_bias, tol, max_count)
+                           only_bias, tol, max_count, sv_rx_time=sv_rx_time)
             states.append([timestamp] + np.squeeze(position).tolist())
         except RuntimeError as error:
             if str(error) not in runtime_error_idxs:
@@ -155,7 +159,7 @@ def solve_wls(measurements, weight_type = None, only_bias = False,
     return state_estimate
 
 def wls(rx_est_m, pos_sv_m, corr_pr_m, weights = None,
-        only_bias = False, tol = 1e-7, max_count = 20):
+        only_bias = False, tol = 1e-7, max_count = 20, sv_rx_time=False):
     """Weighted least squares solver for GNSS measurements.
 
     The option for only_bias allows the user to only calculate the clock
@@ -170,7 +174,7 @@ def wls(rx_est_m, pos_sv_m, corr_pr_m, weights = None,
         array with shape (4 x 1) and the following order:
         x_rx_m, y_rx_m, z_rx_m, b_rx_m.
     pos_sv_m : np.ndarray
-        Satellite positions as an array of shape [# svs x 3] where
+        Satellite ECEF positions as an array of shape [# svs x 3] where
         the columns contain in order x_sv_m, y_sv_m, and z_sv_m.
     corr_pr_m : np.ndarray
         Corrected pseudoranges for all satellites with shape of
@@ -186,6 +190,17 @@ def wls(rx_est_m, pos_sv_m, corr_pr_m, weights = None,
     max_count : int
         Number of maximum iterations before process is aborted and
         solution returned.
+    sv_rx_time : bool
+        Flag to indicate if the satellite positions at the time of
+        transmission should be used as is or if they should be transformed
+        to the ECEF frame of reference at the time of reception. For real
+        measurements, use ``sv_rx_time=False`` to account for the Earth's
+        rotation and convert SV positions from the ECEF frame at the time
+        of signal transmission to the ECEF frame at the time of signal
+        reception. If the SV positions should be used as is, set
+        ``sv_rx_time=True`` to indicate that the given positions are in
+        the ECEF frame of reference for when the signals are received.
+        By default, ``sv_rx_time=False``.
 
     Returns
     -------
@@ -195,11 +210,35 @@ def wls(rx_est_m, pos_sv_m, corr_pr_m, weights = None,
         array with shape (4 x 1) and the following order:
         x_rx_m, y_rx_m, z_rx_m, b_rx_m.
 
+    Notes
+    -----
+    This function internally updates the used SV position to account for
+    the time taken for the signal to travel to the Earth from the GNSS
+    satellites.
+    Since the SV and receiver positions are calculated in an ECEF frame
+    of reference, which is moving with the Earth's rotation, the reference
+    frame is slightly (about 30 m along longitude) different when the
+    signals are received than when the signals were transmitted. Given
+    the receiver's position is estimated when the signal is received,
+    the SV positions need to be updated to reflect the change in the
+    frame of reference in which their position is calculated.
+
+    This update happens after every Gauss-Newton update step and is
+    adapted from [1]_.
+
+    References
+    ----------
+    .. [1] https://github.com/google/gps-measurement-tools/blob/master/opensource/FlightTimeCorrection.m
+
     """
 
     rx_est_m = rx_est_m.copy() # don't change referenced value
 
     count = 0
+    # Store the SV position at the original receiver time.
+    # This position will be modified by the time taken by the signal to
+    # travel to the receiver.
+    rx_time_pos_sv_m = pos_sv_m.copy()
     num_svs = pos_sv_m.shape[0]
     if num_svs < 4 and not only_bias:
         raise RuntimeError("Need at least four satellites for WLS.")
@@ -245,6 +284,16 @@ def wls(rx_est_m, pos_sv_m, corr_pr_m, weights = None,
         else:
             rx_est_m += pos_x_delta
 
+        if not sv_rx_time:
+            # Update the satellite positions based on the time taken for
+            # the signal to reach the Earth and the satellite clock bias.
+            delta_t = (corr_pr_m.reshape(-1) - rx_est_m[3,0])/consts.C
+            dtheta = consts.OMEGA_E_DOT*delta_t
+            pos_sv_m[:, 0] = np.cos(dtheta)*rx_time_pos_sv_m[:,0] + \
+                             np.sin(dtheta)*rx_time_pos_sv_m[:,1]
+            pos_sv_m[:, 1] = -np.sin(dtheta)*rx_time_pos_sv_m[:,0] + \
+                              np.cos(dtheta)*rx_time_pos_sv_m[:,1]
+
         count += 1
 
         if count >= max_count:

gnss_lib_py/utils/sim_gnss.py

@@ -316,7 +316,10 @@ def _find_delxyz_range(sv_posvel, pos, satellites):
     pos = np.reshape(pos, [1, 3])
     if np.size(pos)!=3:
         raise ValueError('Position is not in XYZ')
-    _, sv_pos, _ = _extract_pos_vel_arr(sv_posvel)
+    if isinstance(sv_posvel, np.ndarray):
+        sv_pos = sv_posvel[:, :3]
+    else:
+        _, sv_pos, _ = _extract_pos_vel_arr(sv_posvel)
     del_pos = sv_pos - np.tile(np.reshape(pos, [-1, 3]), (satellites, 1))
     true_range = np.linalg.norm(del_pos, axis=1)
     return del_pos, true_range

notebooks/tutorials/algorithms.ipynb

@@ -35,7 +35,12 @@
    "id": "3bc6b5dd",
    "metadata": {},
    "source": [
-    "Solve for the Weighted Least Squares position estimate simply by passing the measurement data."
+    "Solve for the Weighted Least Squares position estimate simply by passing the measurement data.\n",
+    "\n",
+    "When obtaining WLS estimates for real measurements, the rotation of the Earth between the signal transmission and reception has to be accounted for.\n",
+    "`solve_wls` accounts for this by default and rotates the given SV positions into the ECEF frame of reference when the signals were received rather using the ECEF frame of reference of when the signals were transmitted.\n",
+    "\n",
+    "If you assume that the satellite positions are given in the ECEF frame of reference when the signals were received (and not transmitted), set the parameter `sv_rx_time = True` in the function call."
    ]
   },
   {
@@ -45,7 +50,9 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "state_wls = glp.solve_wls(derived_data)"
+    "state_wls = glp.solve_wls(derived_data)\n",
+    "# When assuming that SV positions are given in the ECEF frame when signals are received use\n",
+    "# state_wls = glp.solve_wls(derived_data, sv_rx_time=True)"
    ]
   },
   {
@@ -101,7 +108,7 @@
    "id": "0387e03e",
    "metadata": {},
    "source": [
-    "Solve for the Weighted Least Squares position estimate simply by passing the measurement data."
+    "Solve for the extended Kalman filter position estimate simply by passing the measurement data."
    ]
   },
   {
@@ -209,11 +216,19 @@
    "source": [
     "figs = glp.plot_metric_by_constellation(galileo_data, \"gps_millis\", \"residuals_m\")"
    ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "fdc6d9cb",
+   "metadata": {},
+   "outputs": [],
+   "source": []
   }
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Python 3",
+   "display_name": "Python 3 (ipykernel)",
    "language": "python",
    "name": "python3"
   },

pyproject.toml

@@ -1,6 +1,6 @@
 [tool.poetry]
 name = "gnss-lib-py"
-version = "0.1.11"
+version = "0.1.12"
 description = "Modular Python tool for parsing, analyzing, and visualizing Global Navigation Satellite Systems (GNSS) data and state estimates"
 authors = ["Derek Knowles <[email protected]>",
            "Ashwin Kanhere <[email protected]>",