Merge pull request #121 from Stanford-NavLab/ashwin/wls-fixes-2

Ashwin/wls fixes 2

Author: betaBison

gnss_lib_py/algorithms/snapshot.py

@@ -14,11 +14,12 @@
 import numpy as np
 
 from gnss_lib_py.parsers.navdata import NavData
+from gnss_lib_py.utils import constants as consts
 from gnss_lib_py.utils.coordinates import ecef_to_geodetic
 
 def solve_wls(measurements, weight_type = None, only_bias = False,
               receiver_state=None, tol = 1e-7, max_count = 20,
-              delta_t_decimals=-2):
+              sv_rx_time=False, delta_t_decimals=-2):
     """Runs weighted least squares across each timestep.
 
     Runs weighted least squares across each timestep and adds a new
@@ -29,13 +30,7 @@ def solve_wls(measurements, weight_type = None, only_bias = False,
     in rx_est_m will be updated if only_bias is set to True.
 
     If only_bias is set to True, then the receiver position must also
-    be passed in as the receiver_state
-
-    receiver_state : gnss_lib_py.parsers.navdata.NavData
-        Either estimated or ground truth receiver position in ECEF frame
-        in meters as an instance of the NavData class with the
-        following rows: ``x_rx*_m``, `y_rx*_m``, ``z_rx*_m``,
-        ``gps_millis``.
+    be passed in as the receiver_state.
 
     Parameters
     ----------
@@ -56,6 +51,14 @@ def solve_wls(measurements, weight_type = None, only_bias = False,
     max_count : int
         Number of maximum iterations before process is aborted and
         solution returned.
+    sv_rx_time : bool
+        Flag that specifies whether the input SV positions are in the ECEF
+        frame of reference corresponding to when the measurements were
+        received. If set to `True`, the satellite positions are used as
+        is. The default value is `False`, in which case the ECEF positions
+        are assumed to in the ECEF frame at the time of signal transmission
+        and are converted to the ECEF frame at the time of signal reception,
+        while solving the WLS problem.
     delta_t_decimals : int
             Decimal places after which times are considered equal.
 
@@ -102,6 +105,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")
@@ -118,7 +122,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, sv_rx_time=sv_rx_time)
             states.append([timestamp] + np.squeeze(position).tolist())
         except RuntimeError as error:
             if str(error) not in runtime_error_idxs:
@@ -155,7 +159,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, sv_rx_time=False):
     """Weighted least squares solver for GNSS measurements.
 
     The option for only_bias allows the user to only calculate the clock
@@ -170,7 +174,7 @@ def wls(rx_est_m, pos_sv_m, corr_pr_m, weights = None,
         array with shape (4 x 1) and the following order:
         x_rx_m, y_rx_m, z_rx_m, b_rx_m.
     pos_sv_m : np.ndarray
-        Satellite positions as an array of shape [# svs x 3] where
+        Satellite ECEF positions as an array of shape [# svs x 3] where
         the columns contain in order x_sv_m, y_sv_m, and z_sv_m.
     corr_pr_m : np.ndarray
         Corrected pseudoranges for all satellites with shape of
@@ -186,6 +190,17 @@ 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.
+    sv_rx_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 ``sv_rx_time=False`` to account for the Earth's
+        rotation and convert SV positions from the ECEF frame at the time
+        of signal transmission to the ECEF frame at the time of signal
+        reception. If the SV positions should be used as is, set
+        ``sv_rx_time=True`` to indicate that the given positions are in
+        the ECEF frame of reference for when the signals are received.
+        By default, ``sv_rx_time=False``.
 
     Returns
     -------
@@ -195,11 +210,35 @@ def wls(rx_est_m, pos_sv_m, corr_pr_m, weights = None,
         array with shape (4 x 1) and the following order:
         x_rx_m, y_rx_m, z_rx_m, b_rx_m.
 
+    Notes
+    -----
+    This function internally updates the used SV position to account for
+    the time taken for the signal to travel to the Earth from the GNSS
+    satellites.
+    Since the SV and receiver positions are calculated in an ECEF frame
+    of reference, which is moving with the Earth's rotation, the reference
+    frame is slightly (about 30 m along longitude) different when the
+    signals are received than when the signals were transmitted. Given
+    the receiver's position is estimated when the signal is received,
+    the SV positions need to be updated to reflect the change in the
+    frame of reference in which their position is calculated.
+
+    This update happens after every Gauss-Newton update step and is
+    adapted from [1]_.
+
+    References
+    ----------
+    .. [1] https://github.com/google/gps-measurement-tools/blob/master/opensource/FlightTimeCorrection.m
+
     """
 
     rx_est_m = rx_est_m.copy() # don't change referenced value
 
     count = 0
+    # Store the SV position at the original receiver time.
+    # This position will be modified by the time taken by the signal to
+    # travel to the receiver.
+    rx_time_pos_sv_m = pos_sv_m.copy()
     num_svs = pos_sv_m.shape[0]
     if num_svs < 4 and not only_bias:
         raise RuntimeError("Need at least four satellites for WLS.")
@@ -245,6 +284,16 @@ def wls(rx_est_m, pos_sv_m, corr_pr_m, weights = None,
         else:
             rx_est_m += pos_x_delta
 
+        if not sv_rx_time:
+            # Update the satellite positions based on the time taken for
+            # the signal to reach the Earth and the satellite clock bias.
+            delta_t = (corr_pr_m.reshape(-1) - rx_est_m[3,0])/consts.C
+            dtheta = consts.OMEGA_E_DOT*delta_t
+            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
 
         if count >= max_count:

gnss_lib_py/utils/sim_gnss.py

@@ -316,7 +316,10 @@ def _find_delxyz_range(sv_posvel, pos, satellites):
     pos = np.reshape(pos, [1, 3])
     if np.size(pos)!=3:
         raise ValueError('Position is not in XYZ')
-    _, sv_pos, _ = _extract_pos_vel_arr(sv_posvel)
+    if isinstance(sv_posvel, np.ndarray):
+        sv_pos = sv_posvel[:, :3]
+    else:
+        _, sv_pos, _ = _extract_pos_vel_arr(sv_posvel)
     del_pos = sv_pos - np.tile(np.reshape(pos, [-1, 3]), (satellites, 1))
     true_range = np.linalg.norm(del_pos, axis=1)
     return del_pos, true_range

notebooks/tutorials/algorithms.ipynb

@@ -35,7 +35,12 @@
    "id": "3bc6b5dd",
    "metadata": {},
    "source": [
-    "Solve for the Weighted Least Squares position estimate simply by passing the measurement data."
+    "Solve for the Weighted Least Squares position estimate simply by passing the measurement data.\n",
+    "\n",
+    "When obtaining WLS estimates for real measurements, the rotation of the Earth between the signal transmission and reception has to be accounted for.\n",
+    "`solve_wls` accounts for this by default and rotates the given SV positions into the ECEF frame of reference when the signals were received rather using the ECEF frame of reference of when the signals were transmitted.\n",
+    "\n",
+    "If you assume that the satellite positions are given in the ECEF frame of reference when the signals were received (and not transmitted), set the parameter `sv_rx_time = True` in the function call."
    ]
   },
   {
@@ -45,7 +50,9 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "state_wls = glp.solve_wls(derived_data)"
+    "state_wls = glp.solve_wls(derived_data)\n",
+    "# When assuming that SV positions are given in the ECEF frame when signals are received use\n",
+    "# state_wls = glp.solve_wls(derived_data, sv_rx_time=True)"
    ]
   },
   {
@@ -101,7 +108,7 @@
    "id": "0387e03e",
    "metadata": {},
    "source": [
-    "Solve for the Weighted Least Squares position estimate simply by passing the measurement data."
+    "Solve for the extended Kalman filter position estimate simply by passing the measurement data."
    ]
   },
   {
@@ -209,11 +216,19 @@
    "source": [
     "figs = glp.plot_metric_by_constellation(galileo_data, \"gps_millis\", \"residuals_m\")"
    ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "fdc6d9cb",
+   "metadata": {},
+   "outputs": [],
+   "source": []
   }
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Python 3",
+   "display_name": "Python 3 (ipykernel)",
    "language": "python",
    "name": "python3"
   },

tests/algorithms/test_snapshot.py

@@ -101,7 +101,7 @@ def test_wls(set_user_states, set_sv_states, tolerance):
                              keepdims = True) + rx_truth_m[3,0]
 
     rx_est_m = np.zeros((4,1))
-    user_fix = wls(rx_est_m, pos_sv_m, gt_pr_m)
+    user_fix = wls(rx_est_m, pos_sv_m, gt_pr_m, sv_rx_time=True)
     truth_fix = rx_truth_m
 
     np.testing.assert_array_almost_equal(user_fix, truth_fix,
@@ -229,15 +229,15 @@ def test_wls_only_bias(set_user_states, set_sv_states, tolerance):
 
     # check that position doesn't change but clock bias does change
     rx_est_m = np.zeros((4,1))
-    user_fix = wls(rx_est_m, pos_sv_m, gt_pr_m, None, True)
+    user_fix = wls(rx_est_m, pos_sv_m, gt_pr_m, None, True, sv_rx_time=True)
     np.testing.assert_array_almost_equal(user_fix[:3], np.zeros((3,1)),
                                          decimal=tolerance)
     assert abs(user_fix[3]) >= 1E5
 
     # check that correct clock bias is calculated
     rx_est_m = np.zeros((4,1))
     rx_est_m[:3] = rx_truth_m[:3]
-    user_fix = wls(rx_est_m, pos_sv_m, gt_pr_m, None, True)
+    user_fix = wls(rx_est_m, pos_sv_m, gt_pr_m, None, True, sv_rx_time=True)
     truth_fix = rx_truth_m
 
     np.testing.assert_array_almost_equal(user_fix, truth_fix,
@@ -362,7 +362,7 @@ def test_solve_wls(derived):
         Instance of AndroidDerived2021 for testing.
 
     """
-    state_estimate = solve_wls(derived)
+    state_estimate = solve_wls(derived, sv_rx_time=False)
 
     # result should be a NavData Class instance
     assert isinstance(state_estimate,type(NavData()))
@@ -403,12 +403,12 @@ def test_solve_wls_weights(derived, tolerance):
         Error threshold for test pass/fail
     """
 
-    state_estimate_1 = solve_wls(derived)
-    state_estimate_2 = solve_wls(derived, None)
+    state_estimate_1 = solve_wls(derived, sv_rx_time=False)
+    state_estimate_2 = solve_wls(derived, None, sv_rx_time=False)
 
     # create new column of unity weights
     derived["unity_weights"] = 1
-    state_estimate_3 = solve_wls(derived, "unity_weights")
+    state_estimate_3 = solve_wls(derived, "unity_weights",  sv_rx_time=False)
 
     # all of the above should be the same
     np.testing.assert_array_almost_equal(state_estimate_1.array,
@@ -418,7 +418,7 @@ def test_solve_wls_weights(derived, tolerance):
                                          state_estimate_3.array,
                                          decimal=tolerance)
 
-    state_estimate_4 = solve_wls(derived, "raw_pr_sigma_m")
+    state_estimate_4 = solve_wls(derived, "raw_pr_sigma_m",  sv_rx_time=False)
     with np.testing.assert_raises(AssertionError):
         np.testing.assert_array_almost_equal(state_estimate_1.array,
                                              state_estimate_4.array,
@@ -463,17 +463,20 @@ def test_wls_weights(set_user_states, set_sv_states, tolerance,
     rx_est_m = np.zeros((4,1))
 
     # should work if None is passed
-    user_fix_1 = wls(rx_est_m, pos_sv_m, noisy_pr_m, weights=None)
+    user_fix_1 = wls(rx_est_m, pos_sv_m, noisy_pr_m, weights=None,
+                     sv_rx_time=True)
     user_fix_2 = wls(rx_est_m, pos_sv_m, noisy_pr_m,
-                     weights=np.ones((pos_sv_m.shape[0],1)))
+                     weights=np.ones((pos_sv_m.shape[0],1)),
+                     sv_rx_time=True)
     # both should be unity weights and return the same result
     np.testing.assert_array_almost_equal(user_fix_1,
                                          user_fix_2,
                                          decimal=tolerance)
 
     # result should be different if different weights are used
     user_fix_3 = wls(rx_est_m, pos_sv_m, noisy_pr_m,
-                     weights=np.arange(pos_sv_m.shape[0]).reshape(-1,1))
+                     weights=np.arange(pos_sv_m.shape[0]).reshape(-1,1),
+                     sv_rx_time=True)
     with np.testing.assert_raises(AssertionError):
         np.testing.assert_array_almost_equal(user_fix_1,
                                              user_fix_3,
@@ -528,7 +531,7 @@ def test_solve_wls_bias_only(derived_2022):
             input_position[row, col] = measure_frame[row, 0]
         col += 1
     state_estimate = solve_wls(derived_2022, only_bias=True,
-                               receiver_state=derived_2022)
+                               receiver_state=derived_2022, sv_rx_time=False)
     # Verify that both structures have the same length
     assert len(input_position) == len(state_estimate)
     # Verify that solved positions are the same as input positions
@@ -557,11 +560,11 @@ def test_solve_wls_bias_only(derived_2022):
     derived_2022.remove(ecef_rows, inplace=True)
     with pytest.raises(KeyError):
         solve_wls(derived_2022, only_bias=True,
-                receiver_state=derived_2022)
+                receiver_state=derived_2022, sv_rx_time=False)
 
     # check error raised when receiver_state is not present in only_bias
     with pytest.raises(RuntimeError):
-        solve_wls(derived_2022, only_bias=True)
+        solve_wls(derived_2022, only_bias=True, sv_rx_time=False)
 
 
 def test_wls_fails(capsys):
@@ -613,3 +616,37 @@ def test_solve_wls_fails(derived):
 
     assert caught_four_sats
     assert caught_empty_state
+
+
+def test_rotation_of_earth_fix(derived_2022):
+    """Tests that accounting for Earth's rotation reduces WLS errors.
+
+    Parameters
+    ----------
+    derived_2022 : AndroidDerived2022
+        Instance of AndroidDerived2022 for testing
+    """
+    google_wls = derived_2022[['x_rx_m', 'y_rx_m', 'z_rx_m']]
+    google_wls = np.empty([3, len(np.unique(np.round(derived_2022['gps_millis'], decimals=-2)))])
+    for idx, (_, _, frame) in enumerate(derived_2022.loop_time('gps_millis')):
+        google_wls[:, idx] = frame[['x_rx_m', 'y_rx_m', 'z_rx_m'], 0]
+    derived_2022['wls_weights'] = 1/derived_2022['raw_pr_sigma_m']
+    state_with_rotn = solve_wls(derived_2022, weight_type='wls_weights',
+                               sv_rx_time=False)
+    glp_wls = state_with_rotn[['x_rx_wls_m', 'y_rx_wls_m', 'z_rx_wls_m']]
+    # Verify that the mean error between both estimates is less than 10m
+    for idx in range(3):
+        assert np.mean(np.abs(google_wls[idx, :] - glp_wls[idx, :])) < 30
+    state_without_rotn = solve_wls(derived_2022, weight_type='wls_weights',
+                                   sv_rx_time=True)
+    glp_wls_no_rotn = state_without_rotn[['x_rx_wls_m',
+                                          'y_rx_wls_m',
+                                          'z_rx_wls_m']]
+    for idx in range(3):
+        error_rotn = np.mean(np.abs(google_wls[idx, :] - glp_wls[idx, :]))
+        error_no_rotn = np.mean(np.abs(google_wls[idx, :] - glp_wls_no_rotn[idx, :]))
+        assert error_rotn < error_no_rotn
+
+
+
+