Skip to content

Commit 4f367ae

Browse files
kanhereashwinkanhereashwin
committed
Initial delay and initialization changes
1 parent ef6473a commit 4f367ae

1 file changed

Lines changed: 92 additions & 15 deletions

File tree

‎gnss_lib_py/algorithms/gnss_filters.py‎

Lines changed: 92 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,8 @@
1111

1212
from gnss_lib_py.parsers.navdata import NavData
1313
from gnss_lib_py.algorithms.snapshot import solve_wls
14+
from gnss_lib_py.utils import constants as consts
15+
from gnss_lib_py.utils.sim_gnss import _find_delxyz_range
1416
from gnss_lib_py.utils.coordinates import ecef_to_geodetic
1517
from gnss_lib_py.utils.filters import BaseExtendedKalmanFilter
1618

@@ -49,17 +51,66 @@ def solve_gnss_ekf(measurements, init_dict = None,
4951
init_dict = {}
5052

5153
if "state_0" not in init_dict:
52-
pos_0 = None
53-
for _, _, measurement_subset in measurements.loop_time("gps_millis",
54-
delta_t_decimals=delta_t_decimals):
55-
pos_0 = solve_wls(measurement_subset)
56-
if pos_0 is not None:
57-
break
58-
59-
state_0 = np.zeros((7,1))
60-
if pos_0 is not None:
61-
state_0[:3,0] = pos_0[["x_rx_wls_m","y_rx_wls_m","z_rx_wls_m"]]
62-
state_0[6,0] = pos_0[["b_rx_wls_m"]]
54+
try:
55+
# if the given measurement frame has a state estimate, use
56+
# that, including the clock bias estimate
57+
pos_est_rows = measurements.find_wildcard_indexes(["x_rx*_m",
58+
"y_rx*_m",
59+
"z_rx*_m",
60+
"b_rx*_m"],
61+
max_allow=1)
62+
63+
not_nan_idxs = measurements.argwhere(pos_est_rows['x_rx*_m'],
64+
np.nan, 'neq')
65+
state_0 = np.zeros((7,1))
66+
state_0[0,0] = measurements[pos_est_rows['x_rx*_m'], not_nan_idxs[0]]
67+
state_0[1,0] = measurements[pos_est_rows['y_rx*_m'], not_nan_idxs[0]]
68+
state_0[2,0] = measurements[pos_est_rows['z_rx*_m'], not_nan_idxs[0]]
69+
state_0[6,0] = measurements[pos_est_rows['b_rx*_m'], not_nan_idxs[0]]
70+
except KeyError:
71+
try:
72+
# a key error happened and one of the rows from the last
73+
# try clause is not present. Try again without bias,
74+
# which often missing from datasets
75+
pos_est_rows = measurements.find_wildcard_indexes(["x_rx*_m",
76+
"y_rx*_m",
77+
"z_rx*_m"],
78+
max_allow=1)
79+
not_nan_idxs = measurements.argwhere(pos_est_rows['x_rx*_m'],
80+
np.nan, 'neq')
81+
state_0 = np.zeros((7,1))
82+
state_0[0,0] = measurements[pos_est_rows['x_rx*_m'], not_nan_idxs[0]]
83+
state_0[1,0] = measurements[pos_est_rows['y_rx*_m'], not_nan_idxs[0]]
84+
state_0[2,0] = measurements[pos_est_rows['z_rx*_m'], not_nan_idxs[0]]
85+
pos_0 = NavData()
86+
pos_0[pos_est_rows['x_rx*_m']] = state_0[0,0]
87+
pos_0[pos_est_rows['y_rx*_m']] = state_0[1,0]
88+
pos_0[pos_est_rows['z_rx*_m']] = state_0[2,0]
89+
measurement_subset = measurements.copy(col=not_nan_idxs[0])
90+
pos_0 = solve_wls(measurement_subset,
91+
receiver_state=pos_0,
92+
only_bias=True)
93+
# if len(pos_0.where('b_rx_wls_m', np.nan, 'eq'))==0:
94+
# break
95+
state_0[6,0] = pos_0['b_rx_wls_m']
96+
except KeyError:
97+
# position rows were not found again, use a WLS estimate
98+
pos_0 = None
99+
for _, _, measurement_subset in measurements.loop_time("gps_millis",
100+
delta_t_decimals=delta_t_decimals):
101+
pos_0 = solve_wls(measurement_subset)
102+
# Assume that if 'x_rx_wls_m' is np.nan, then a state estimate
103+
# has not been found and all x, y, and z are np.nan.
104+
# If the length of elements where 'x_rx_wls_m' is np.nan is
105+
# 0, then a solution has been found and can be used as an
106+
# initialization
107+
if len(pos_0.where('x_rx_wls_m', np.nan, 'eq'))==0:
108+
break
109+
110+
state_0 = np.zeros((7,1))
111+
if pos_0 is not None:
112+
state_0[:3,0] = pos_0[["x_rx_wls_m","y_rx_wls_m","z_rx_wls_m"]]
113+
state_0[6,0] = pos_0[["b_rx_wls_m"]]
63114

64115
init_dict["state_0"] = state_0
65116

@@ -68,12 +119,10 @@ def solve_gnss_ekf(measurements, init_dict = None,
68119
init_dict["sigma_0"] = sigma_0
69120

70121
if "Q" not in init_dict:
71-
process_noise = np.eye(init_dict["state_0"].size)
72-
init_dict["Q"] = process_noise
122+
raise RuntimeError("Process noise must be specified in init_dict")
73123

74124
if "R" not in init_dict:
75-
measurement_noise = np.eye(1) # gets overwritten
76-
init_dict["R"] = measurement_noise
125+
raise RuntimeError("Measurement noise must be specified in init_dict")
77126

78127
# initialize parameter dictionary
79128
if params_dict is None:
@@ -196,6 +245,23 @@ def measure_model(self, update_dict):
196245
of shape [3 x N] with rows of x_sv_m, y_sv_m, and z_sv_m in that
197246
order.
198247
248+
This measurment model uses the current state estimate to find
249+
the time taken for signals to propagate from the satellites to
250+
the receiver and updates the SV positions to reflect the changed
251+
ECEF reference frame.
252+
253+
Since the ECEF reference frame moves with the Earth, the frame
254+
of reference is different at different times.
255+
SV positions are calculated for the time at which the signal was
256+
transmitted but the receiver position is computed for the ECEF
257+
frame of reference when the signals are received. Consequently,
258+
the SV positions must be updated to account for the change in the
259+
ECEF frame between signal transmission and reception.
260+
However, given the EKF has an initial position guess around the
261+
true position (either through prior knowledge or a prior state
262+
estimation process such as WLS), we can simply correct the SV
263+
positions once and use them as such, without further modification.
264+
199265
Parameters
200266
----------
201267
update_dict : dict
@@ -216,6 +282,17 @@ def measure_model(self, update_dict):
216282
"""
217283
if self.measure_type=='pseudorange':
218284
pos_sv_m = update_dict['pos_sv_m']
285+
rx_pos_m = np.array([[self.state[0]], [self.state[1]], [self.state[2]]])
286+
num_svs = np.shape(pos_sv_m)[1]
287+
_, true_range = _find_delxyz_range(pos_sv_m.T, rx_pos_m, num_svs)
288+
tx_time = (true_range - self.state[6])/consts.C
289+
dtheta = consts.OMEGA_E_DOT*tx_time
290+
# The following two lines are expanded position updates for a
291+
# rotation by dtheta radians about the z-axis, which updates
292+
# the positions along the x and y axes but the position along
293+
# the z-axis is unchanged.
294+
pos_sv_m[0, :] = np.cos(dtheta)*pos_sv_m[0,:] + np.sin(dtheta)*pos_sv_m[1,:]
295+
pos_sv_m[1, :] = -np.sin(dtheta)*pos_sv_m[0,:] + np.cos(dtheta)*pos_sv_m[1,:]
219296
pseudo = np.sqrt((self.state[0] - pos_sv_m[0, :])**2
220297
+ (self.state[1] - pos_sv_m[1, :])**2
221298
+ (self.state[2] - pos_sv_m[2, :])**2) \

0 commit comments

Comments
 (0)