Skip to content

Commit 15e3161

Browse files
betaBisonbetaBison
committed
vectorized precise interpolation
1 parent 330199d commit 15e3161

7 files changed

Lines changed: 243 additions & 588 deletions

File tree

‎gnss_lib_py/parsers/clk.py‎

Lines changed: 48 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -93,80 +93,68 @@ def __init__(self, input_paths):
9393
self["sv_id"] = sv_id
9494
self["b_sv_m"] = b_sv_m
9595

96-
def extract_clk(self, gnss_sv_id, sidx, ipos = 10, \
97-
method='CubicSpline', verbose = False):
98-
"""Computing interpolated function over clk data for any GNSS
96+
def interpolate_clk(self, navdata, window=6, verbose=False):
97+
"""Interpolate clock data from clk file.
9998
10099
Parameters
101100
----------
102-
gnss_sv_id : string
103-
Constellations and SV id of satellite for which position is
104-
to be determined.
105-
See standard nomenclature reference for more details.
106-
sidx : int
107-
Nearest index within clk time series around which interpolated
108-
function needs to be centered.
109-
ipos : int
110-
No. of data points from clk data on either side of sidx
111-
that will be used for computing interpolated function.
112-
method : string
113-
Type of interpolation method used for clk data (the default is
114-
CubicSpline, which depicts third-order polynomial).
101+
navdata : gnss_lib_py.parsers.navdata.NavData
102+
Instance of the NavData class that must include rows for
103+
``gps_millis`` and ``gnss_sv_id``
104+
window : int
105+
Number of points to use in interpolation window.
115106
verbose : bool
116-
If true, prints extra debugging statements.
107+
Flag (True/False) for whether to print intermediate steps useful
108+
for debugging/reviewing (the default is False)
117109
118-
Returns
119-
-------
120-
func_satbias : np.ndarray
121-
Instance with 1-D array of scipy.interpolate.interpolate.interp1d
122-
that is loaded with .clk data
123110
"""
111+
# add satellite indexes if not present already.
112+
sv_idx_keys = ['b_sv_m', 'b_dot_sv_mps']
124113

125-
clkdata = self.where("gnss_sv_id",gnss_sv_id)
114+
for sv_idx_key in sv_idx_keys:
115+
if sv_idx_key not in navdata.rows:
116+
navdata[sv_idx_key] = np.nan
126117

127-
if method=='CubicSpline':
128-
low_i = (sidx - ipos) if (sidx - ipos) >= 0 else 0
129-
high_i = (sidx + ipos) if (sidx + ipos) <= len(clkdata["gps_millis"]) else -1
118+
available_svs_from_clk = np.unique(self["gnss_sv_id"])
130119

131-
if verbose:
132-
print('Nearest clk: ', sidx, clkdata["gps_millis"][sidx],
133-
clkdata["b_sv_m"][sidx])
120+
for gnss_sv_id in np.unique(navdata["gnss_sv_id"]):
121+
navdata_id = navdata.where("gnss_sv_id",gnss_sv_id)
122+
navdata_id_gps_millis = np.atleast_1d(navdata_id["gps_millis"])
134123

135-
func_satbias = interpolate.CubicSpline(clkdata["gps_millis"][low_i:high_i], \
136-
clkdata["b_sv_m"][low_i:high_i])
124+
if gnss_sv_id in available_svs_from_clk:
125+
clk_id = self.where("gnss_sv_id",gnss_sv_id)
126+
x_data = clk_id["gps_millis"]
127+
y_data = clk_id["b_sv_m"]
137128

138-
return func_satbias
129+
if np.min(navdata_id_gps_millis) < x_data[0] \
130+
or np.max(navdata_id_gps_millis) > x_data[-1]:
131+
raise RuntimeError("clk data does not include all "\
132+
+ "times in measurement file.")
139133

134+
b_sv_m = np.zeros(len(navdata_id))
135+
b_dot_sv_mps = np.zeros(len(navdata_id))
140136

141-
def clk_snapshot(self, func_satbias, cxtime, hstep = 5e-1,
142-
method='CubicSpline'):
143-
"""Compute satellite clock bias and drift from clk interpolated function
137+
# iterate through needed polynoials so don't repeat fit
138+
insert_indexes = np.searchsorted(x_data,navdata_id_gps_millis)
139+
for insert_index in np.unique(insert_indexes):
140+
max_index = min(len(clk_id)-1,insert_index+int(window/2))
141+
min_index = max(0,insert_index-int(window/2))
142+
x_window = x_data[min_index:max_index]
143+
y_window = y_data[min_index:max_index]
144144

145-
Parameters
146-
----------
147-
func_satbias : scipy.interpolate._cubic.CubicSpline
148-
Instance with interpolated function for satellite bias from .clk data
149-
cxtime : float
150-
Time at which satellite clock bias and drift is to be computed
151-
hstep : float
152-
Step size in milliseconds used to computing clock drift using
153-
central differencing (the default is 5e-1)
154-
method : string
155-
Type of interpolation method used for sp3 data (the default is
156-
CubicSpline, which depicts third-order polynomial)
157-
158-
Returns
159-
-------
160-
satbias_clk : float
161-
Computed satellite clock bias (in seconds)
162-
satdrift_clk : float
163-
Computed satellite clock drift (in seconds/seconds)
164-
"""
145+
insert_index_idxs = np.argwhere(insert_indexes==insert_index)
146+
x_interpret = navdata_id_gps_millis[insert_index_idxs]
147+
148+
poly = np.polynomial.polynomial.Polynomial.fit(x_window, y_window, deg=3)
149+
b_sv = poly(x_interpret)
165150

166-
if method=='CubicSpline':
167-
sat_t = func_satbias([cxtime-0.5*hstep, cxtime, cxtime+0.5*hstep])
151+
polyder = poly.deriv()
152+
b_dot_sv = polyder(x_interpret)
168153

169-
satbias_clk = sat_t[1]
170-
satdrift_clk = (sat_t[2]-sat_t[0]) / hstep
154+
b_sv_m[insert_index_idxs] = b_sv
155+
# multiply by 1000 since currently meters/milliseconds
156+
b_dot_sv_mps[insert_index_idxs] = b_dot_sv*1000
171157

172-
return satbias_clk, (satdrift_clk * 1e3)
158+
row_idx = navdata.argwhere("gnss_sv_id",gnss_sv_id)
159+
navdata["b_sv_m",row_idx] = b_sv_m
160+
navdata["b_dot_sv_mps",row_idx] = b_dot_sv_mps

‎gnss_lib_py/parsers/sp3.py‎

Lines changed: 66 additions & 85 deletions
Original file line numberDiff line numberDiff line change
@@ -104,94 +104,75 @@ def __init__(self, input_paths):
104104
self["y_sv_m"] = y_sv_m
105105
self["z_sv_m"] = z_sv_m
106106

107-
def extract_sp3(self, gnss_sv_id, sidx, ipos = 10, \
108-
method = 'CubicSpline', verbose = False):
109-
"""Computing interpolated function over sp3 data for any GNSS
107+
def interpolate_sp3(self, navdata, window=6, verbose=False):
108+
"""Interpolate ECEF position data from sp3 file.
110109
111110
Parameters
112111
----------
113-
gnss_sv_id : string
114-
PRN of satellite for which position should be determined.
115-
sidx : int
116-
Nearest index within sp3 time series around which interpolated
117-
function needs to be centered.
118-
ipos : int
119-
No. of data points from sp3 data on either side of sidx
120-
that will be used for computing interpolated function.
121-
method : string
122-
Type of interpolation method used for sp3 data (the default is
123-
CubicSpline, which depicts third-order polynomial).
112+
navdata : gnss_lib_py.parsers.navdata.NavData
113+
Instance of the NavData class that must include rows for
114+
``gps_millis`` and ``gnss_sv_id``.
115+
window : int
116+
Number of points to use in interpolation window.
124117
verbose : bool
125-
If true, prints extra debugging statements.
118+
Flag (True/False) for whether to print intermediate steps useful
119+
for debugging/reviewing (the default is False)
126120
127-
Returns
128-
-------
129-
func_satpos : np.ndarray
130-
Instance with 3-D array of scipy.interpolate.interpolate.interp1d
131-
that is loaded with .sp3 data
132121
"""
133-
134-
sp3data = self.where("gnss_sv_id",gnss_sv_id)
135-
136-
func_satpos = np.empty((3,), dtype=object)
137-
func_satpos[:] = np.nan
138-
139-
if method=='CubicSpline':
140-
low_i = (sidx - ipos) if (sidx - ipos) >= 0 else 0
141-
high_i = (sidx + ipos) if (sidx + ipos) <= len(sp3data["gps_millis"]) else -1
142-
143-
if verbose:
144-
print('Nearest sp3: ', sidx, sp3data["gps_millis"][sidx], \
145-
sp3data["x_sv_m"][sidx],
146-
sp3data["y_sv_m"][sidx],
147-
sp3data["z_sv_m"][sidx])
148-
149-
func_satpos[0] = interpolate.CubicSpline(sp3data["gps_millis"][low_i:high_i], \
150-
sp3data["x_sv_m"][low_i:high_i])
151-
func_satpos[1] = interpolate.CubicSpline(sp3data["gps_millis"][low_i:high_i], \
152-
sp3data["y_sv_m"][low_i:high_i])
153-
func_satpos[2] = interpolate.CubicSpline(sp3data["gps_millis"][low_i:high_i], \
154-
sp3data["z_sv_m"][low_i:high_i])
155-
156-
return func_satpos
157-
158-
159-
def sp3_snapshot(self, func_satpos, cxtime, hstep = 5e-1,
160-
method='CubicSpline'):
161-
"""Compute satellite 3D position and velocity.
162-
163-
Computes from the sp3 interpolated function.
164-
165-
Parameters
166-
----------
167-
func_satpos : np.ndarray
168-
Instance with 3-D array of scipy.interpolate.interpolate.interp1d
169-
that is loaded with .sp3 data.
170-
cxtime : float
171-
Time at which the satellite 3-D position and velocity needs to be
172-
computed, given 3-D array of interpolated functions.
173-
hstep : float
174-
Step size in milliseconds used to computing 3-D velocity of any
175-
given satellite using central differencing the default is 5e-1).
176-
method : string
177-
Type of interpolation method used for sp3 data (the default is
178-
CubicSpline, which depicts third-order polynomial).
179-
180-
Returns
181-
-------
182-
satpos_sp3 : 3-D array
183-
Computed satellite position in ECEF frame (Earth's rotation not included)
184-
satvel_sp3 : 3-D array
185-
Computed satellite velocity in ECEF frame (Earth's rotation not included)
186-
"""
187-
if method=='CubicSpline':
188-
sat_x = func_satpos[0]([cxtime-0.5*hstep, cxtime, cxtime+0.5*hstep])
189-
sat_y = func_satpos[1]([cxtime-0.5*hstep, cxtime, cxtime+0.5*hstep])
190-
sat_z = func_satpos[2]([cxtime-0.5*hstep, cxtime, cxtime+0.5*hstep])
191-
192-
satpos_sp3 = np.array([sat_x[1], sat_y[1], sat_z[1]])
193-
satvel_sp3 = np.array([ (sat_x[2]-sat_x[0]) / hstep, \
194-
(sat_y[2]-sat_y[0]) / hstep, \
195-
(sat_z[2]-sat_z[0]) / hstep ])
196-
197-
return satpos_sp3, (satvel_sp3 * 1e3)
122+
# add satellite indexes if not present already.
123+
sv_idx_keys = ['x_sv_m', 'y_sv_m', 'z_sv_m', \
124+
'vx_sv_mps','vy_sv_mps','vz_sv_mps']
125+
126+
for sv_idx_key in sv_idx_keys:
127+
if sv_idx_key not in navdata.rows:
128+
navdata[sv_idx_key] = np.nan
129+
130+
available_svs_from_sp3 = np.unique(self["gnss_sv_id"])
131+
132+
for gnss_sv_id in np.unique(navdata["gnss_sv_id"]):
133+
navdata_id = navdata.where("gnss_sv_id",gnss_sv_id)
134+
navdata_id_gps_millis = np.atleast_1d(navdata_id["gps_millis"])
135+
136+
if gnss_sv_id in available_svs_from_sp3:
137+
sp3_id = self.where("gnss_sv_id",gnss_sv_id)
138+
x_data = sp3_id["gps_millis"]
139+
y_data = sp3_id[["x_sv_m","y_sv_m","z_sv_m"]]
140+
141+
if np.min(navdata_id_gps_millis) < x_data[0] \
142+
or np.max(navdata_id_gps_millis) > x_data[-1]:
143+
raise RuntimeError("sp3 data does not include "\
144+
+ "appropriate times in measurement"\
145+
+ " file for SV ",gnss_sv_id)
146+
147+
xyz_sv_m = np.zeros((6,len(navdata_id)))
148+
149+
# iterate through needed polynoials so don't repeat fit
150+
insert_indexes = np.searchsorted(x_data,navdata_id_gps_millis)
151+
for insert_index in np.unique(insert_indexes):
152+
max_index = min(len(sp3_id)-1,insert_index+int(window/2))
153+
min_index = max(0,insert_index-int(window/2))
154+
x_window = x_data[min_index:max_index]
155+
y_window = y_data[:,min_index:max_index]
156+
157+
insert_index_idxs = np.argwhere(insert_indexes==insert_index)
158+
x_interpret = navdata_id_gps_millis[insert_index_idxs]
159+
160+
# iterate through x,y, and z
161+
for xyz_index in range(3):
162+
poly = np.polynomial.polynomial.Polynomial.fit(x_window, y_window[xyz_index,:], deg=3)
163+
xyz_sv = poly(x_interpret)
164+
165+
polyder = poly.deriv()
166+
vxyz_sv = polyder(x_interpret)
167+
168+
xyz_sv_m[xyz_index,insert_index_idxs] = xyz_sv
169+
# multiply by 1000 since currently meters/milliseconds
170+
xyz_sv_m[xyz_index+3,insert_index_idxs] = vxyz_sv*1000
171+
172+
row_idx = navdata.argwhere("gnss_sv_id",gnss_sv_id)
173+
navdata["x_sv_m",row_idx] = xyz_sv_m[0,:]
174+
navdata["y_sv_m",row_idx] = xyz_sv_m[1,:]
175+
navdata["z_sv_m",row_idx] = xyz_sv_m[2,:]
176+
navdata["vx_sv_mps",row_idx] = xyz_sv_m[3,:]
177+
navdata["vy_sv_mps",row_idx] = xyz_sv_m[4,:]
178+
navdata["vz_sv_mps",row_idx] = xyz_sv_m[5,:]

0 commit comments

Comments
 (0)