Changed default use_tx_time to False

kanhereashwin · kanhereashwin · commit dee7a343cc83 · 2023-07-13T11:29:11.000-07:00
diff --git a/gnss_lib_py/algorithms/gnss_filters.py b/gnss_lib_py/algorithms/gnss_filters.py
@@ -59,7 +59,6 @@ def solve_gnss_ekf(measurements, init_dict = None,
                                                                "z_rx*_m",
                                                                "b_rx*_m"],
                                                                max_allow=1)
-
             not_nan_idxs = measurements.argwhere(pos_est_rows['x_rx*_m'],
                                                  np.nan, 'neq')
             state_0 = np.zeros((7,1))
@@ -83,10 +82,11 @@ def solve_gnss_ekf(measurements, init_dict = None,
                 state_0[1,0] = measurements[pos_est_rows['y_rx*_m'], not_nan_idxs[0]]
                 state_0[2,0] = measurements[pos_est_rows['z_rx*_m'], not_nan_idxs[0]]
                 pos_0 = NavData()
-                pos_0[pos_est_rows['x_rx*_m']] = state_0[0,0]
-                pos_0[pos_est_rows['y_rx*_m']] = state_0[1,0]
-                pos_0[pos_est_rows['z_rx*_m']] = state_0[2,0]
-                measurement_subset = measurements.copy(col=not_nan_idxs[0])
+                pos_0['gps_millis'] = measurements['gps_millis', not_nan_idxs[0]]
+                pos_0['x_rx_m'] = state_0[0,0]
+                pos_0['y_rx_m'] = state_0[1,0]
+                pos_0['z_rx_m'] = state_0[2,0]
+                measurement_subset = measurements.copy(cols=not_nan_idxs[0])
                 pos_0 = solve_wls(measurement_subset,
                                     receiver_state=pos_0,
                                     only_bias=True)
@@ -111,7 +111,6 @@ def solve_gnss_ekf(measurements, init_dict = None,
                 if pos_0 is not None:
                     state_0[:3,0] = pos_0[["x_rx_wls_m","y_rx_wls_m","z_rx_wls_m"]]
                     state_0[6,0] = pos_0[["b_rx_wls_m"]]
-
         init_dict["state_0"] = state_0
 
     if "sigma_0" not in init_dict:
@@ -124,6 +123,9 @@ def solve_gnss_ekf(measurements, init_dict = None,
     if "R" not in init_dict:
         raise RuntimeError("Measurement noise must be specified in init_dict")
 
+    if "use_tx_time" not in init_dict:
+        init_dict["use_tx_time"] = False
+
     # initialize parameter dictionary
     if params_dict is None:
         params_dict = {}
@@ -210,6 +212,7 @@ def __init__(self, init_dict, params_dict):
         self.delta_t = params_dict.get('dt',1.0)
         self.motion_type = params_dict.get('motion_type','stationary')
         self.measure_type = params_dict.get('measure_type','pseudorange')
+        self.use_tx_time = init_dict.get('use_tx_time', False)
 
     def dyn_model(self, u, predict_dict=None):
         """Nonlinear dynamics
@@ -266,7 +269,13 @@ def measure_model(self, update_dict):
         ----------
         update_dict : dict
             Update dictionary containing satellite positions with key
-            ``pos_sv_m``.
+            ``pos_sv_m`` and optionally ``tx_time``. ``tx_time`` specifies
+            if the filter should use the SV positions at time of
+            transmission (if True). If False, the the time it takes for
+            the signal to propagate from the satellite to the receiver
+            is accounted for and the SV positions are propagated forward
+            to the ECEF coordinate frame at the receiver time. By default,
+            ``tx_time`` is True.
 
         Returns
         -------
@@ -282,17 +291,18 @@ def measure_model(self, update_dict):
         """
         if self.measure_type=='pseudorange':
             pos_sv_m = update_dict['pos_sv_m']
-            rx_pos_m = np.array([[self.state[0]], [self.state[1]], [self.state[2]]])
-            num_svs = np.shape(pos_sv_m)[1]
-            _, true_range = _find_delxyz_range(pos_sv_m.T, rx_pos_m, num_svs)
-            tx_time = (true_range - self.state[6])/consts.C
-            dtheta = consts.OMEGA_E_DOT*tx_time
-            # The following two lines are expanded position updates for a
-            # rotation by dtheta radians about the z-axis, which updates
-            # the positions along the x and y axes but the position along
-            # the z-axis is unchanged.
-            pos_sv_m[0, :] = np.cos(dtheta)*pos_sv_m[0,:] + np.sin(dtheta)*pos_sv_m[1,:]
-            pos_sv_m[1, :] = -np.sin(dtheta)*pos_sv_m[0,:] + np.cos(dtheta)*pos_sv_m[1,:]
+            if not self.use_tx_time:
+                rx_pos_m = np.array([[self.state[0]], [self.state[1]], [self.state[2]]])
+                num_svs = np.shape(pos_sv_m)[1]
+                _, true_range = _find_delxyz_range(pos_sv_m.T, rx_pos_m, num_svs)
+                tx_time = (true_range - self.state[6])/consts.C
+                dtheta = consts.OMEGA_E_DOT*tx_time
+                # The following two lines are expanded position updates for a
+                # rotation by dtheta radians about the z-axis, which updates
+                # the positions along the x and y axes but the position along
+                # the z-axis is unchanged.
+                pos_sv_m[0, :] = np.cos(dtheta)*pos_sv_m[0,:] + np.sin(dtheta)*pos_sv_m[1,:]
+                pos_sv_m[1, :] = -np.sin(dtheta)*pos_sv_m[0,:] + np.cos(dtheta)*pos_sv_m[1,:]
             pseudo = np.sqrt((self.state[0] - pos_sv_m[0, :])**2
                            + (self.state[1] - pos_sv_m[1, :])**2
                            + (self.state[2] - pos_sv_m[2, :])**2) \
diff --git a/gnss_lib_py/algorithms/snapshot.py b/gnss_lib_py/algorithms/snapshot.py
@@ -19,7 +19,7 @@
 
 def solve_wls(measurements, weight_type = None, only_bias = False,
               receiver_state=None, tol = 1e-7, max_count = 20,
-              delta_t_decimals=-2):
+              use_tx_time=False, delta_t_decimals=-2):
     """Runs weighted least squares across each timestep.
 
     Runs weighted least squares across each timestep and adds a new
@@ -51,6 +51,11 @@ def solve_wls(measurements, weight_type = None, only_bias = False,
     max_count : int
         Number of maximum iterations before process is aborted and
         solution returned.
+    use_tx_time : bool
+        Flag that specifies whether the SV positions at transmit time
+        should be used as is or if they should be transformed to the
+        corresponding ECEF coordinates when the signal is received. If
+        True, the SV positions will not be modified.
     delta_t_decimals : int
             Decimal places after which times are considered equal.
 
@@ -97,6 +102,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")
@@ -113,7 +119,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, use_tx_time)
             states.append([timestamp] + np.squeeze(position).tolist())
         except RuntimeError as error:
             if str(error) not in runtime_error_idxs:
@@ -150,7 +156,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, use_tx_time=False):
     """Weighted least squares solver for GNSS measurements.
 
     The option for only_bias allows the user to only calculate the clock
@@ -181,6 +187,14 @@ 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.
+    use_tx_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 ``use_tx_time=False`` to account for the Earth's
+        rotation. For simulations, ``use_tx_time=True`` can be used to
+        simplify the simulation and estimation. By default,
+        ``use_tx_time=True``.
 
     Returns
     -------
@@ -264,13 +278,15 @@ def wls(rx_est_m, pos_sv_m, corr_pr_m, weights = None,
         else:
             rx_est_m += pos_x_delta
 
-
-        # Update the satellite positions based on the time taken for
-        # the signal to reach the Earth and the satellite clock bias.
-        tx_time = (corr_pr_m.reshape(-1) - rx_est_m[3,0])/consts.C
-        dtheta = consts.OMEGA_E_DOT*tx_time
-        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]
+        if not use_tx_time:
+            # Update the satellite positions based on the time taken for
+            # the signal to reach the Earth and the satellite clock bias.
+            tx_time = (corr_pr_m.reshape(-1) - rx_est_m[3,0])/consts.C
+            dtheta = consts.OMEGA_E_DOT*tx_time
+            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
 
diff --git a/tests/algorithms/test_gnss_filters.py b/tests/algorithms/test_gnss_filters.py
@@ -12,7 +12,7 @@
 from numpy.random import default_rng
 
 from gnss_lib_py.parsers.navdata import NavData
-from gnss_lib_py.parsers.android import AndroidDerived2021
+from gnss_lib_py.parsers.android import AndroidDerived2021, AndroidDerived2022
 from gnss_lib_py.algorithms.gnss_filters import GNSSEKF, solve_gnss_ekf
 
 @pytest.fixture(name='init_dict')
@@ -35,7 +35,8 @@ def gnss_init_params():
                 'state_0': state_0,
                 'sigma_0': 5*np.eye(state_dim),
                 'Q': Q,
-                'R': R}
+                'R': R,
+                'use_tx_time': True}
     return init_dict
 
 
@@ -159,17 +160,83 @@ def fixture_load_derived(derived_path):
     derived = AndroidDerived2021(derived_path)
     return derived
 
-def test_solve_gnss_ekf(derived):
+
+@pytest.fixture(name="derived_2022_path")
+def fixture_derived_2022_path(root_path):
+    """Filepath of Android Derived 2022 measurements
+
+    Returns
+    -------
+    derived_2022_path : string
+        Location for the unit_test Android 2022 derived measurements
+
+    Notes
+    -----
+    Test data is a subset of the Android Raw Measurement Dataset [3]_,
+    from the 2022 Decimeter Challenge. Particularly, the
+    train/2021-04-29-MTV-2/SamsungGalaxyS20Ultra trace. The dataset
+    was retrieved from
+    https://www.kaggle.com/competitions/smartphone-decimeter-2022/data
+
+    References
+    ----------
+    .. [3] Fu, Guoyu Michael, Mohammed Khider, and Frank van Diggelen.
+        "Android Raw GNSS Measurement Datasets for Precise Positioning."
+        Proceedings of the 33rd International Technical Meeting of the
+        Satellite Division of The Institute of Navigation (ION GNSS+
+        2020). 2020.
+    """
+    derived_2022_path = os.path.join(root_path, '../android_2022/device_gnss.csv')
+    return derived_2022_path
+
+
+@pytest.fixture(name="derived_2022")
+def fixture_load_derived_2022(derived_2022_path):
+    """Load instance of AndroidDerived2021
+
+    Parameters
+    ----------
+    derived_2022_path : pytest.fixture
+        String with location of Android derived 2022 measurement file
+
+    Returns
+    -------
+    derived_2022 : AndroidDerived2021
+        Instance of AndroidDerived2022 for testing
+    """
+    derived_2022 = AndroidDerived2022(derived_2022_path)
+    return derived_2022
+
+
+@pytest.fixture(name="noise_tx_init_dict")
+def fixture_android_init_dict():
+    """Define dictionary containing identity process and measure noises.
+
+    Returns
+    -------
+    init_dict : dict
+        Dictionary of initialization parameters, in this case, containing
+        just the process and measurement noise covariance matrices.
+    """
+    init_dict = {}
+    init_dict['Q'] = np.eye(7)
+    init_dict['R'] = np.eye(1)
+    init_dict['use_tx_time'] = False
+    return init_dict
+
+def test_solve_gnss_ekf(derived, noise_tx_init_dict):
     """Test that solving for GNSS EKF doesn't fail
 
     Parameters
     ----------
     derived : AndroidDerived2021
         Instance of AndroidDerived2021 for testing.
+    init_dict : dict
+        Dictionary of initialization parameters, in this case, containing
+        just the process and measurement noise covariance matrices.
 
     """
-    state_estimate = solve_gnss_ekf(derived)
-
+    state_estimate = solve_gnss_ekf(derived, noise_tx_init_dict)
     # result should be a NavData Class instance
     assert isinstance(state_estimate,type(NavData()))
 
@@ -202,23 +269,76 @@ def test_solve_gnss_ekf(derived):
         assert row_index in str(excinfo.value)
 
 
-def test_solve_gnss_ekf_fails(derived):
+
+def test_solve_gnss_ekf_fails(derived, noise_tx_init_dict):
     """Test expected fails for the GNSS EKF.
 
     Parameters
     ----------
     derived : AndroidDerived2021
         Instance of AndroidDerived2021 for testing
+    init_dict : dict
+        Dictionary of initialization parameters, in this case, containing
+        just the process and measurement noise covariance matrices.
 
     """
 
     navdata = derived.remove(cols=list(range(len(derived))))
 
     with pytest.warns(RuntimeWarning) as warns:
-        solve_gnss_ekf(navdata)
+        solve_gnss_ekf(navdata, noise_tx_init_dict)
 
     # verify RuntimeWarning
     assert len(warns) == 1
     warn = warns[0]
     assert issubclass(warn.category, RuntimeWarning)
     assert "No valid state" in str(warn.message)
+
+
+    # Test that RuntimeError is raised if no measurment noise is provided
+    with pytest.raises(RuntimeError):
+        del(noise_tx_init_dict['R'])
+        solve_gnss_ekf(derived, noise_tx_init_dict)
+    # Test that RuntimeError is raised if no process noise is provided
+    with pytest.raises(RuntimeError):
+        del(noise_tx_init_dict['Q'])
+        solve_gnss_ekf(derived, noise_tx_init_dict)
+    # Test that RuntimeError is raised if no initial dictionary is provided
+    with pytest.raises(RuntimeError):
+        solve_gnss_ekf(derived)
+
+
+def test_solve_gnss_ekf_initializations(derived_2022):
+    """Tests that different initial state cases run without error.
+
+    Parameters
+    ----------
+    derived_2022 : AndroidDerived2022
+        Instance of AndroidDerived2022 for testing
+    init_dict : dict
+        Dictionary of initialization parameters, in this case, containing
+        just the process and measurement noise covariance matrices.
+    """
+    # GNSS EKF solution when initial states and biases are given
+    derived_2022['b_rx_m'] = 0
+    # Reinitializing the initial dictionary because other functions might
+    # have added to this.
+    reset_init_dict = {}
+    reset_init_dict['Q'] = np.eye(7)
+    reset_init_dict['R'] = np.eye(1)
+    reset_init_dict['use_tx_time'] = True
+    _ = solve_gnss_ekf(derived_2022, reset_init_dict)
+    # GNSS EKF solution when initial positions are given
+    derived_2022.remove(rows=['b_rx_m'], inplace=True)
+    reset_init_dict = {}
+    reset_init_dict['Q'] = np.eye(7)
+    reset_init_dict['R'] = np.eye(1)
+    reset_init_dict['use_tx_time'] = True
+    _ = solve_gnss_ekf(derived_2022, reset_init_dict)
+    # GNSS EKF solution when no initial states are given
+    derived_no_rx_rows = derived_2022.remove(rows=['x_rx_m', 'y_rx_m', 'z_rx_m'])
+    reset_init_dict = {}
+    reset_init_dict['Q'] = np.eye(7)
+    reset_init_dict['R'] = np.eye(1)
+    reset_init_dict['use_tx_time'] = True
+    _ = solve_gnss_ekf(derived_no_rx_rows, reset_init_dict)
diff --git a/tests/algorithms/test_snapshot.py b/tests/algorithms/test_snapshot.py
