Reintroduced tx_time changes to the EKF code

Author: kanhereashwin

gnss_lib_py/algorithms/gnss_filters.py

@@ -11,6 +11,8 @@
 
 from gnss_lib_py.parsers.navdata import NavData
 from gnss_lib_py.algorithms.snapshot import solve_wls
+from gnss_lib_py.utils import constants as consts
+from gnss_lib_py.utils.sim_gnss import _find_delxyz_range
 from gnss_lib_py.utils.coordinates import ecef_to_geodetic
 from gnss_lib_py.utils.filters import BaseExtendedKalmanFilter
 
@@ -49,31 +51,80 @@ def solve_gnss_ekf(measurements, init_dict = None,
         init_dict = {}
 
     if "state_0" not in init_dict:
-        pos_0 = None
-        for _, _, measurement_subset in measurements.loop_time("gps_millis",
-                                        delta_t_decimals=delta_t_decimals):
-            pos_0 = solve_wls(measurement_subset)
-            if pos_0 is not None:
-                break
-
-        state_0 = np.zeros((7,1))
-        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"]]
-
+        try:
+            # if the given measurement frame has a state estimate, use
+            # that, including the clock bias estimate
+            pos_est_rows = measurements.find_wildcard_indexes(["x_rx*_m",
+                                                               "y_rx*_m",
+                                                               "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))
+            state_0[0,0] = measurements[pos_est_rows['x_rx*_m'], not_nan_idxs[0]]
+            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]]
+            state_0[6,0] = measurements[pos_est_rows['b_rx*_m'], not_nan_idxs[0]]
+        except KeyError:
+            try:
+                # a key error happened and one of the rows from the last
+                # try clause is not present. Try again without bias,
+                # which often missing from datasets
+                pos_est_rows = measurements.find_wildcard_indexes(["x_rx*_m",
+                                                                   "y_rx*_m",
+                                                                   "z_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))
+                state_0[0,0] = measurements[pos_est_rows['x_rx*_m'], not_nan_idxs[0]]
+                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['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)
+                    # if len(pos_0.where('b_rx_wls_m', np.nan, 'eq'))==0:
+                    #     break
+                state_0[6,0] = pos_0['b_rx_wls_m']
+            except KeyError:
+                # position rows were not found again, use a WLS estimate
+                pos_0 = None
+                for _, _, measurement_subset in measurements.loop_time("gps_millis",
+                                                delta_t_decimals=delta_t_decimals):
+                    pos_0 = solve_wls(measurement_subset)
+                    # Assume that if 'x_rx_wls_m' is np.nan, then a state estimate
+                    # has not been found and all x, y, and z are np.nan.
+                    # If the length of elements where 'x_rx_wls_m' is np.nan is
+                    # 0, then a solution has been found and can be used as an
+                    # initialization
+                    if len(pos_0.where('x_rx_wls_m', np.nan, 'eq'))==0:
+                        break
+
+                state_0 = np.zeros((7,1))
+                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:
         sigma_0 = np.eye(init_dict["state_0"].size)
         init_dict["sigma_0"] = sigma_0
 
     if "Q" not in init_dict:
-        process_noise = np.eye(init_dict["state_0"].size)
-        init_dict["Q"] = process_noise
+        raise RuntimeError("Process noise must be specified in init_dict")
 
     if "R" not in init_dict:
-        measurement_noise = np.eye(1) # gets overwritten
-        init_dict["R"] = measurement_noise
+        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:
@@ -161,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
@@ -196,11 +248,34 @@ def measure_model(self, update_dict):
         of shape [3 x N] with rows of x_sv_m, y_sv_m, and z_sv_m in that
         order.
 
+        This measurment model uses the current state estimate to find
+        the time taken for signals to propagate from the satellites to
+        the receiver and updates the SV positions to reflect the changed
+        ECEF reference frame.
+
+        Since the ECEF reference frame moves with the Earth, the frame
+        of reference is different at different times.
+        SV positions are calculated for the time at which the signal was
+        transmitted but the receiver position is computed for the ECEF
+        frame of reference when the signals are received. Consequently,
+        the SV positions must be updated to account for the change in the
+        ECEF frame between signal transmission and reception.
+        However, given the EKF has an initial position guess around the
+        true position (either through prior knowledge or a prior state
+        estimation process such as WLS), we can simply correct the SV
+        positions once and use them as such, without further modification.
+
         Parameters
         ----------
         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
         -------
@@ -216,6 +291,18 @@ def measure_model(self, update_dict):
         """
         if self.measure_type=='pseudorange':
             pos_sv_m = update_dict['pos_sv_m']
+            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) \

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)

tests/parsers/test_android.py

@@ -750,7 +750,13 @@ def test_solve_kaggle_dataset(root_path):
                    solve_gnss_ekf,
                   ]:
         for verbose in [True, False]:
-            solution = android.solve_kaggle_dataset(folder_path, solver,
+            if solver == solve_gnss_ekf:
+                init_dict = {'Q': np.eye(7), 'R': np.eye(1)}
+                solution = android.solve_kaggle_dataset(folder_path, solver,
+                                                    verbose, init_dict=init_dict)
+
+            else:
+                solution = android.solve_kaggle_dataset(folder_path, solver,
                                                     verbose)
 
             solution.in_rows(["tripId","UnixTimeMillis",