Merge pull request #133 from Stanford-NavLab/derek/precise-interpolate

Derek/precise interpolate

Author: kanhereashwin

data/unit_test/clk/grg21553_short.clk data/unit_test/clk/grg21553.clkdata/unit_test/clk/grg21553_short.clk renamed to data/unit_test/clk/grg21553.clk

@@ -93,80 +93,71 @@ def __init__(self, input_paths):
         self["sv_id"] = sv_id
         self["b_sv_m"] = b_sv_m
 
-    def extract_clk(self, gnss_sv_id, sidx, ipos = 10, \
-                         method='CubicSpline', verbose = False):
-        """Computing interpolated function over clk data for any GNSS
+    def interpolate_clk(self, navdata, window=6, verbose=False):
+        """Interpolate clock data from clk file, adding inplace to given
+        NavData instance.
 
         Parameters
         ----------
-        gnss_sv_id : string
-            Constellations and SV id of satellite for which position is
-            to be determined.
-            See standard nomenclature reference for more details.
-        sidx : int
-            Nearest index within clk time series around which interpolated
-            function needs to be centered.
-        ipos : int
-            No. of data points from clk data on either side of sidx
-            that will be used for computing interpolated function.
-        method : string
-            Type of interpolation method used for clk data (the default is
-            CubicSpline, which depicts third-order polynomial).
+        navdata : gnss_lib_py.parsers.navdata.NavData
+            Instance of the NavData class that must include rows for
+            ``gps_millis`` and ``gnss_sv_id``
+        window : int
+            Number of points to use in interpolation window.
         verbose : bool
-            If true, prints extra debugging statements.
+            Flag (True/False) for whether to print intermediate steps useful
+            for debugging/reviewing (the default is False)
 
-        Returns
-        -------
-        func_satbias : np.ndarray
-            Instance with 1-D array of scipy.interpolate.interpolate.interp1d
-            that is loaded with .clk data
         """
+        # add satellite indexes if not present already.
+        sv_idx_keys = ['b_sv_m', 'b_dot_sv_mps']
 
-        clkdata = self.where("gnss_sv_id",gnss_sv_id)
+        for sv_idx_key in sv_idx_keys:
+            if sv_idx_key not in navdata.rows:
+                navdata[sv_idx_key] = np.nan
 
-        if method=='CubicSpline':
-            low_i = (sidx - ipos) if (sidx - ipos) >= 0 else 0
-            high_i = (sidx + ipos) if (sidx + ipos) <= len(clkdata["gps_millis"]) else -1
+        available_svs_from_clk = np.unique(self["gnss_sv_id"])
 
+        for gnss_sv_id in np.unique(navdata["gnss_sv_id"]):
             if verbose:
-                print('Nearest clk: ', sidx, clkdata["gps_millis"][sidx],
-                                             clkdata["b_sv_m"][sidx])
-
-            func_satbias = interpolate.CubicSpline(clkdata["gps_millis"][low_i:high_i], \
-                                                   clkdata["b_sv_m"][low_i:high_i])
-
-        return func_satbias
-
-
-    def clk_snapshot(self, func_satbias, cxtime, hstep = 5e-1,
-                     method='CubicSpline'):
-        """Compute satellite clock bias and drift from clk interpolated function
-
-        Parameters
-        ----------
-        func_satbias : scipy.interpolate._cubic.CubicSpline
-            Instance with interpolated function for satellite bias from .clk data
-        cxtime : float
-            Time at which satellite clock bias and drift is to be computed
-        hstep : float
-            Step size in milliseconds used to computing clock drift using
-            central differencing (the default is 5e-1)
-        method : string
-            Type of interpolation method used for sp3 data (the default is
-            CubicSpline, which depicts third-order polynomial)
-
-        Returns
-        -------
-        satbias_clk : float
-            Computed satellite clock bias (in seconds)
-        satdrift_clk : float
-            Computed satellite clock drift (in seconds/seconds)
-        """
-
-        if method=='CubicSpline':
-            sat_t = func_satbias([cxtime-0.5*hstep, cxtime, cxtime+0.5*hstep])
-
-        satbias_clk = sat_t[1]
-        satdrift_clk = (sat_t[2]-sat_t[0]) / hstep
-
-        return satbias_clk, (satdrift_clk * 1e3)
+                print("interpolating clk for ",gnss_sv_id)
+            navdata_id = navdata.where("gnss_sv_id",gnss_sv_id)
+            navdata_id_gps_millis = np.atleast_1d(navdata_id["gps_millis"])
+
+            if gnss_sv_id in available_svs_from_clk:
+                clk_id = self.where("gnss_sv_id",gnss_sv_id)
+                x_data = clk_id["gps_millis"]
+                y_data = clk_id["b_sv_m"]
+
+                if np.min(navdata_id_gps_millis) < x_data[0] \
+                    or np.max(navdata_id_gps_millis) > x_data[-1]:
+                    raise RuntimeError("clk data does not include all "\
+                                     + "times in measurement file.")
+
+                b_sv_m = np.zeros(len(navdata_id))
+                b_dot_sv_mps = np.zeros(len(navdata_id))
+
+                # iterate through needed polynomials so don't repeat fit
+                insert_indexes = np.searchsorted(x_data,navdata_id_gps_millis)
+                for insert_index in np.unique(insert_indexes):
+                    max_index = min(len(clk_id)-1,insert_index+int(window/2))
+                    min_index = max(0,insert_index-int(window/2))
+                    x_window = x_data[min_index:max_index]
+                    y_window = y_data[min_index:max_index]
+
+                    insert_index_idxs = np.argwhere(insert_indexes==insert_index)
+                    x_interpret = navdata_id_gps_millis[insert_index_idxs]
+
+                    poly = np.polynomial.polynomial.Polynomial.fit(x_window, y_window, deg=3)
+                    b_sv = poly(x_interpret)
+
+                    polyder = poly.deriv()
+                    b_dot_sv = polyder(x_interpret)
+
+                    b_sv_m[insert_index_idxs] = b_sv
+                    # multiply by 1000 since currently meters/milliseconds
+                    b_dot_sv_mps[insert_index_idxs] = b_dot_sv*1000
+
+                row_idx = navdata.argwhere("gnss_sv_id",gnss_sv_id)
+                navdata["b_sv_m",row_idx] = b_sv_m
+                navdata["b_dot_sv_mps",row_idx] = b_dot_sv_mps

data/unit_test/sp3/grg21553_short.sp3 data/unit_test/sp3/grg21553.sp3data/unit_test/sp3/grg21553_short.sp3 renamed to data/unit_test/sp3/grg21553.sp3

@@ -47,7 +47,6 @@ def __init__(self, input_paths):
             input_paths = [input_paths]
 
         gps_millis = []
-        unix_millis = []
         gnss_sv_ids = []
         gnss_id = []
         sv_id = []
@@ -104,94 +103,78 @@ def __init__(self, input_paths):
         self["y_sv_m"] = y_sv_m
         self["z_sv_m"] = z_sv_m
 
-    def extract_sp3(self, gnss_sv_id, sidx, ipos = 10, \
-                         method = 'CubicSpline', verbose = False):
-        """Computing interpolated function over sp3 data for any GNSS
+    def interpolate_sp3(self, navdata, window=6, verbose=False):
+        """Interpolate ECEF position data from sp3 file, inplace to
+        given navdata instance.
 
         Parameters
         ----------
-        gnss_sv_id : string
-            PRN of satellite for which position should be determined.
-        sidx : int
-            Nearest index within sp3 time series around which interpolated
-            function needs to be centered.
-        ipos : int
-            No. of data points from sp3 data on either side of sidx
-            that will be used for computing interpolated function.
-        method : string
-            Type of interpolation method used for sp3 data (the default is
-            CubicSpline, which depicts third-order polynomial).
+        navdata : gnss_lib_py.parsers.navdata.NavData
+            Instance of the NavData class that must include rows for
+            ``gps_millis`` and ``gnss_sv_id``.
+        window : int
+            Number of points to use in interpolation window.
         verbose : bool
-            If true, prints extra debugging statements.
+            Flag (True/False) for whether to print intermediate steps useful
+            for debugging/reviewing (the default is False)
 
-        Returns
-        -------
-        func_satpos : np.ndarray
-            Instance with 3-D array of scipy.interpolate.interpolate.interp1d
-            that is loaded with .sp3 data
         """
+        # add satellite indexes if not present already.
+        sv_idx_keys = ['x_sv_m', 'y_sv_m', 'z_sv_m', \
+                       'vx_sv_mps','vy_sv_mps','vz_sv_mps']
 
-        sp3data = self.where("gnss_sv_id",gnss_sv_id)
+        for sv_idx_key in sv_idx_keys:
+            if sv_idx_key not in navdata.rows:
+                navdata[sv_idx_key] = np.nan
 
-        func_satpos = np.empty((3,), dtype=object)
-        func_satpos[:] = np.nan
-
-        if method=='CubicSpline':
-            low_i = (sidx - ipos) if (sidx - ipos) >= 0 else 0
-            high_i = (sidx + ipos) if (sidx + ipos) <= len(sp3data["gps_millis"]) else -1
+        available_svs_from_sp3 = np.unique(self["gnss_sv_id"])
 
+        for gnss_sv_id in np.unique(navdata["gnss_sv_id"]):
             if verbose:
-                print('Nearest sp3: ', sidx, sp3data["gps_millis"][sidx], \
-                                       sp3data["x_sv_m"][sidx],
-                                       sp3data["y_sv_m"][sidx],
-                                       sp3data["z_sv_m"][sidx])
-
-            func_satpos[0] = interpolate.CubicSpline(sp3data["gps_millis"][low_i:high_i], \
-                                                     sp3data["x_sv_m"][low_i:high_i])
-            func_satpos[1] = interpolate.CubicSpline(sp3data["gps_millis"][low_i:high_i], \
-                                                     sp3data["y_sv_m"][low_i:high_i])
-            func_satpos[2] = interpolate.CubicSpline(sp3data["gps_millis"][low_i:high_i], \
-                                                     sp3data["z_sv_m"][low_i:high_i])
-
-        return func_satpos
-
-
-    def sp3_snapshot(self, func_satpos, cxtime, hstep = 5e-1,
-                     method='CubicSpline'):
-        """Compute satellite 3D position and velocity.
-
-        Computes from the sp3 interpolated function.
-
-        Parameters
-        ----------
-        func_satpos : np.ndarray
-            Instance with 3-D array of scipy.interpolate.interpolate.interp1d
-            that is loaded with .sp3 data.
-        cxtime : float
-            Time at which the satellite 3-D position and velocity needs to be
-            computed, given 3-D array of interpolated functions.
-        hstep : float
-            Step size in milliseconds used to computing 3-D velocity of any
-            given satellite using central differencing the default is 5e-1).
-        method : string
-            Type of interpolation method used for sp3 data (the default is
-            CubicSpline, which depicts third-order polynomial).
-
-        Returns
-        -------
-        satpos_sp3 : 3-D array
-            Computed satellite position in ECEF frame (Earth's rotation not included)
-        satvel_sp3 : 3-D array
-            Computed satellite velocity in ECEF frame (Earth's rotation not included)
-        """
-        if method=='CubicSpline':
-            sat_x = func_satpos[0]([cxtime-0.5*hstep, cxtime, cxtime+0.5*hstep])
-            sat_y = func_satpos[1]([cxtime-0.5*hstep, cxtime, cxtime+0.5*hstep])
-            sat_z = func_satpos[2]([cxtime-0.5*hstep, cxtime, cxtime+0.5*hstep])
-
-        satpos_sp3 = np.array([sat_x[1], sat_y[1], sat_z[1]])
-        satvel_sp3 = np.array([ (sat_x[2]-sat_x[0]) / hstep, \
-                                (sat_y[2]-sat_y[0]) / hstep, \
-                                (sat_z[2]-sat_z[0]) / hstep ])
-
-        return satpos_sp3, (satvel_sp3 * 1e3)
+                print("interpolating sp3 for ",gnss_sv_id)
+            navdata_id = navdata.where("gnss_sv_id",gnss_sv_id)
+            navdata_id_gps_millis = np.atleast_1d(navdata_id["gps_millis"])
+
+            if gnss_sv_id in available_svs_from_sp3:
+                sp3_id = self.where("gnss_sv_id",gnss_sv_id)
+                x_data = sp3_id["gps_millis"]
+                y_data = sp3_id[["x_sv_m","y_sv_m","z_sv_m"]]
+
+                if np.min(navdata_id_gps_millis) < x_data[0] \
+                    or np.max(navdata_id_gps_millis) > x_data[-1]:
+                    raise RuntimeError("sp3 data does not include "\
+                                     + "appropriate times in measurement"\
+                                     + " file for SV ",gnss_sv_id)
+
+                xyz_sv_m = np.zeros((6,len(navdata_id)))
+
+                # iterate through needed polynomials so don't repeat fit
+                insert_indexes = np.searchsorted(x_data,navdata_id_gps_millis)
+                for insert_index in np.unique(insert_indexes):
+                    max_index = min(len(sp3_id)-1,insert_index+int(window/2))
+                    min_index = max(0,insert_index-int(window/2))
+                    x_window = x_data[min_index:max_index]
+                    y_window = y_data[:,min_index:max_index]
+
+                    insert_index_idxs = np.argwhere(insert_indexes==insert_index)
+                    x_interpret = navdata_id_gps_millis[insert_index_idxs]
+
+                    # iterate through x,y, and z
+                    for xyz_index in range(3):
+                        poly = np.polynomial.polynomial.Polynomial.fit(x_window, y_window[xyz_index,:], deg=3)
+                        xyz_sv = poly(x_interpret)
+
+                        polyder = poly.deriv()
+                        vxyz_sv = polyder(x_interpret)
+
+                        xyz_sv_m[xyz_index,insert_index_idxs] = xyz_sv
+                        # multiply by 1000 since currently meters/milliseconds
+                        xyz_sv_m[xyz_index+3,insert_index_idxs] = vxyz_sv*1000
+
+                row_idx = navdata.argwhere("gnss_sv_id",gnss_sv_id)
+                navdata["x_sv_m",row_idx] = xyz_sv_m[0,:]
+                navdata["y_sv_m",row_idx] = xyz_sv_m[1,:]
+                navdata["z_sv_m",row_idx] = xyz_sv_m[2,:]
+                navdata["vx_sv_mps",row_idx] = xyz_sv_m[3,:]
+                navdata["vy_sv_mps",row_idx] = xyz_sv_m[4,:]
+                navdata["vz_sv_mps",row_idx] = xyz_sv_m[5,:]

gnss_lib_py/parsers/clk.py

@@ -104,6 +104,9 @@ def load_ephemeris(file_type, gps_millis,
 
     """
 
+    if isinstance(file_paths, (str, os.PathLike)):
+        file_paths = [file_paths]
+
     existing_paths, needed_files = _verify_ephemeris(file_type,
                                                      gps_millis,
                                                      constellations,
@@ -477,6 +480,9 @@ def _valid_ephemeris_in_paths(date, possible_types, file_paths=None):
             for path in file_paths:
                 if os.path.split(path)[1][10:] == os.path.split(recommended_file[1])[1][10:-3]:
                     return True, path
+            for path in file_paths:
+                if os.path.split(path)[1][3:] == str(gps_week).zfill(4) + str((timetuple.tm_wday+1)%7) + ".sp3":
+                    return True, path
 
         # sp3 from last two weeks
         elif possible_type == "sp3_rapid_GFZ":
@@ -498,6 +504,9 @@ def _valid_ephemeris_in_paths(date, possible_types, file_paths=None):
             for path in file_paths:
                 if os.path.split(path)[1][10:] == os.path.split(recommended_file[1])[1][10:-3]:
                     return True, path
+            for path in file_paths:
+                if os.path.split(path)[1][3:] == str(gps_week).zfill(4) + str((timetuple.tm_wday+1)%7) + ".sp3":
+                    return True, path
 
         # sp3 if longer than two weeks ago
         elif possible_type == "sp3_final_CODE":
@@ -519,6 +528,9 @@ def _valid_ephemeris_in_paths(date, possible_types, file_paths=None):
             for path in file_paths:
                 if os.path.split(path)[1][10:] == os.path.split(recommended_file[1])[1][10:-3]:
                     return True, path
+            for path in file_paths:
+                if os.path.split(path)[1][3:] == str(gps_week).zfill(4) + str((timetuple.tm_wday+1)%7) + ".sp3":
+                    return True, path
 
         # clk from last three days
         elif possible_type == "clk_rapid_CODE":
@@ -540,6 +552,9 @@ def _valid_ephemeris_in_paths(date, possible_types, file_paths=None):
             for path in file_paths:
                 if os.path.split(path)[1][10:] == os.path.split(recommended_file[1])[1][10:-3]:
                     return True, path
+            for path in file_paths:
+                if os.path.split(path)[1][3:] == str(gps_week).zfill(4) + str((timetuple.tm_wday+1)%7) + ".clk":
+                    return True, path
 
         # clk from last two weeks
         elif possible_type == "clk_rapid_GFZ":
@@ -561,6 +576,9 @@ def _valid_ephemeris_in_paths(date, possible_types, file_paths=None):
             for path in file_paths:
                 if os.path.split(path)[1][10:] == os.path.split(recommended_file[1])[1][10:-3]:
                     return True, path
+            for path in file_paths:
+                if os.path.split(path)[1][3:] == str(gps_week).zfill(4) + str((timetuple.tm_wday+1)%7) + ".clk":
+                    return True, path
 
         # clk if longer than two weeks ago
         elif possible_type == "clk_final_CODE":
@@ -582,6 +600,9 @@ def _valid_ephemeris_in_paths(date, possible_types, file_paths=None):
             for path in file_paths:
                 if os.path.split(path)[1][10:] == os.path.split(recommended_file[1])[1][10:-3]:
                     return True, path
+            for path in file_paths:
+                if os.path.split(path)[1][3:] == str(gps_week).zfill(4) + str((timetuple.tm_wday+1)%7) + ".clk":
+                    return True, path
 
         else:
             raise RuntimeError(possible_type,"invalid possible_type "\