Revert GNSS EKF changes

kanhereashwin · kanhereashwin · commit 57654a76931d · 2023-07-17T12:58:26.000-07:00
diff --git a/gnss_lib_py/algorithms/gnss_filters.py b/gnss_lib_py/algorithms/gnss_filters.py
@@ -11,8 +11,6 @@
 
 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
 
@@ -51,80 +49,31 @@ def solve_gnss_ekf(measurements, init_dict = None,
         init_dict = {}
 
     if "state_0" not in init_dict:
-        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"]]
+        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"]]
+
         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:
-        raise RuntimeError("Process noise must be specified in init_dict")
+        process_noise = np.eye(init_dict["state_0"].size)
+        init_dict["Q"] = process_noise
 
     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
+        measurement_noise = np.eye(1) # gets overwritten
+        init_dict["R"] = measurement_noise
 
     # initialize parameter dictionary
     if params_dict is None:
@@ -212,7 +161,6 @@ 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
@@ -248,34 +196,11 @@ 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`` 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.
+            ``pos_sv_m``.
 
         Returns
         -------
@@ -291,18 +216,6 @@ 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) \
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, AndroidDerived2022
+from gnss_lib_py.parsers.android import AndroidDerived2021
 from gnss_lib_py.algorithms.gnss_filters import GNSSEKF, solve_gnss_ekf
 
 @pytest.fixture(name='init_dict')
@@ -35,8 +35,7 @@ def gnss_init_params():
                 'state_0': state_0,
                 'sigma_0': 5*np.eye(state_dim),
                 'Q': Q,
-                'R': R,
-                'use_tx_time': True}
+                'R': R}
     return init_dict
 
 
@@ -160,83 +159,17 @@ def fixture_load_derived(derived_path):
     derived = AndroidDerived2021(derived_path)
     return 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):
+def test_solve_gnss_ekf(derived):
     """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, noise_tx_init_dict)
+    state_estimate = solve_gnss_ekf(derived)
+
     # result should be a NavData Class instance
     assert isinstance(state_estimate,type(NavData()))
 
@@ -269,76 +202,23 @@ def test_solve_gnss_ekf(derived, noise_tx_init_dict):
         assert row_index in str(excinfo.value)
 
 
-
-def test_solve_gnss_ekf_fails(derived, noise_tx_init_dict):
+def test_solve_gnss_ekf_fails(derived):
     """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, noise_tx_init_dict)
+        solve_gnss_ekf(navdata)
 
     # 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/parsers/test_android.py b/tests/parsers/test_android.py
@@ -750,13 +750,7 @@ def test_solve_kaggle_dataset(root_path):
                    solve_gnss_ekf,
                   ]:
         for verbose in [True, False]:
-            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,
+            solution = android.solve_kaggle_dataset(folder_path, solver,
                                                     verbose)
 
             solution.in_rows(["tripId","UnixTimeMillis",
