Move unit vector code to coordinates.

Author: danineamati

gnss_lib_py/utils/coordinates.py

@@ -570,3 +570,34 @@ def wrap_0_to_2pi(angles):
     angles = np.mod(angles, 2*np.pi)
 
     return angles
+
+
+def el_az_to_enu_unit_vector(el_deg, az_deg):
+    """
+    Convert elevation and azimuth to ENU unit vectors.
+    
+    Parameters
+    ----------
+    el_deg : np.ndarray
+        Elevation angle in degrees.
+        
+    az_deg : np.ndarray
+        Azimuth angle in degrees.
+        
+    Returns
+    -------
+    unit_dir_mat : np.ndarray
+        ENU unit vectors.
+    """
+    el_rad = np.deg2rad(el_deg)
+    az_rad = np.deg2rad(az_deg)
+    
+    unit_dir_mat = np.vstack(
+        (np.atleast_2d(np.cos(el_rad) * np.sin(az_rad)),
+         np.atleast_2d(np.cos(el_rad) * np.cos(az_rad)),
+         np.atleast_2d(np.sin(el_rad))
+         )).T
+    
+    return unit_dir_mat
+
+

gnss_lib_py/utils/dop.py

@@ -9,6 +9,7 @@
 
 from gnss_lib_py.navdata.navdata import NavData
 from gnss_lib_py.navdata.operations import loop_time
+from gnss_lib_py.utils.coordinates import el_az_to_enu_unit_vector
 
 
 def get_dop(navdata, **which_dop):
@@ -114,60 +115,70 @@ def get_dop(navdata, **which_dop):
     return dop_navdata
 
 
-def calculate_dop(derived):
-    """Calculate DOP from elevation and azimuth (ENU).
 
+def calculate_enu_dop_matrix(derived):
+    """
+    Calculate the DOP matrix from elevation and azimuth (ENU).
+    
     Parameters
     ----------
     derived : gnss_lib_py.navdata.navdata.NavData
         NavData instance containing received GNSS measurements for a
         particular time instance, contains elevation and azimuth angle
         information for an estimated location.
-
+        
     Returns
     -------
-    dop : Dict
-        Dilution of precision, with DOP type as the keys: "HDOP", "VDOP",
-        "TDOP", "PDOP", "GDOP".
+    dop_matrix : np.ndarray (4, 4)
+        DOP matrix.
     """
 
     # Use the elevation and azimuth angles to get the ENU and Time matrix
     # Each row is [d_e, d_n, d_u, 1] for each satellite.
     enut_matrix = _calculate_enut_matrix(derived)
     enut_gram_matrix = enut_matrix.T @ enut_matrix
-    
-    try:
-        dop_matrix = np.linalg.inv(enut_gram_matrix)    
-
-        # Calculate the DOP
-        dop = {}
-        dop["dop_matrix"] = dop_matrix
-        dop["GDOP"] = np.sqrt(np.trace(dop_matrix))
-        dop["HDOP"] = np.sqrt(dop_matrix[0, 0] + dop_matrix[1, 1])
-        dop["VDOP"] = np.sqrt(dop_matrix[2, 2])
-        dop["PDOP"] = np.sqrt(dop_matrix[0, 0] + \
-                              dop_matrix[1, 1] + \
-                              dop_matrix[2, 2])
-        dop["TDOP"] = np.sqrt(dop_matrix[3, 3])
 
+    # Calculate the DOP matrix
+    try:
+        dop_matrix = np.linalg.inv(enut_gram_matrix)
     except np.linalg.LinAlgError:
         # If the matrix is singular, return NaNs for the DOP
-        dop = {}
-        dop["dop_matrix"] = np.nan * np.ones((4, 4))
-        dop["GDOP"] = np.nan
-        dop["HDOP"] = np.nan
-        dop["VDOP"] = np.nan
-        dop["PDOP"] = np.nan
-        dop["TDOP"] = np.nan
+        dop_matrix = np.nan * np.ones((4, 4))
+    
+    return dop_matrix
+
+
+def parse_dop(dop_matrix):
+    """Calculate DOP types from the DOP matrix.
+
+    Parameters
+    ----------
+    dop_matrix : np.ndarray (4, 4)
+        DOP matrix in ENU coordinates.
+
+    Returns
+    -------
+    dop : Dict
+        Dilution of precision, with DOP type as the keys: "HDOP", "VDOP",
+        "TDOP", "PDOP", "GDOP".
+    """
+
+    dop = {}
+    dop["dop_matrix"] = dop_matrix
+    dop["GDOP"] = np.sqrt(np.trace(dop_matrix))
+    dop["HDOP"] = np.sqrt(dop_matrix[0, 0] + dop_matrix[1, 1])
+    dop["VDOP"] = np.sqrt(dop_matrix[2, 2])
+    dop["PDOP"] = np.sqrt(dop_matrix[0, 0] + \
+                            dop_matrix[1, 1] + \
+                            dop_matrix[2, 2])
+    dop["TDOP"] = np.sqrt(dop_matrix[3, 3])
 
     return dop
 
 
-def calculate_enu_unit_vectors(derived):
+def calculate_dop(derived):
     """
-    Calculate the ENU unit vectors from elevation and azimuth. 
-    Each row is [d_e, d_n, d_u] for each satellite, where d is a unit 
-    line-of-sight vector in the ENU frame.
+    Calculate the DOP from elevation and azimuth (ENU).
 
     Parameters
     ----------
@@ -178,21 +189,18 @@ def calculate_enu_unit_vectors(derived):
 
     Returns
     -------
-    unit_dir_mat : np.ndarray (num_satellites, 3)
-        Matrix of ENU unit vectors.
+    dop : Dict
+        Dilution of precision, with DOP type as the keys: "HDOP", "VDOP",
+        "TDOP", "PDOP", "GDOP".
     """
 
-    # Get the elevation and azimuth angles
-    sv_el_az_rad = np.deg2rad(derived['el_sv_deg', 'az_sv_deg'])
+    # Calculate the DOP matrix
+    dop_matrix = calculate_enu_dop_matrix(derived)
 
-    # Important: This matrix is in ENU coordinates, not NED.
-    unit_dir_mat = np.vstack(
-        (np.atleast_2d(np.cos(sv_el_az_rad[0,:]) * np.sin(sv_el_az_rad[1,:])),
-         np.atleast_2d(np.cos(sv_el_az_rad[0,:]) * np.cos(sv_el_az_rad[1,:])),
-         np.atleast_2d(np.sin(sv_el_az_rad[0,:]))
-         )).T
+    # Parse the DOP matrix to get the DOP values
+    dop = parse_dop(dop_matrix)
 
-    return unit_dir_mat
+    return dop
 
 
 def _calculate_enut_matrix(derived):
@@ -213,8 +221,9 @@ def _calculate_enut_matrix(derived):
         Matrix of ENU and Time vectors.
 
     """
-    unit_dir_mat = calculate_enu_unit_vectors(derived)
-    enut_matrix = np.hstack((unit_dir_mat, 
-                             np.ones((unit_dir_mat.shape[0], 1))))
+    enu_unit_dir_mat = el_az_to_enu_unit_vector(derived['el_sv_deg'], 
+                                                derived['az_sv_deg'])
+    enut_matrix = np.hstack((enu_unit_dir_mat, 
+                             np.ones((enu_unit_dir_mat.shape[0], 1))))
 
     return enut_matrix

notebooks/tutorials/utils/dop.ipynb

@@ -19,7 +19,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -38,7 +38,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -60,9 +60,23 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 3,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "     gps_millis      HDOP      VDOP\n",
+      "0  1.303771e+12  0.558578  0.830517\n",
+      "1  1.303771e+12  0.549160  0.829362\n",
+      "2  1.303771e+12  0.558593  0.830452\n",
+      "3  1.303771e+12  0.549176  0.829296\n",
+      "4  1.303771e+12  0.549185  0.829263\n",
+      "5  1.303771e+12  0.549193  0.829230\n"
+     ]
+    }
+   ],
    "source": [
     "dop_navdata = glp.get_dop(navdata)\n",
     "print(dop_navdata)"

tests/utils/test_coordinates.py

@@ -12,11 +12,13 @@
 from pytest_lazyfixture import lazy_fixture
 
 import gnss_lib_py.utils.constants as consts
+from gnss_lib_py.navdata.navdata import NavData
 from gnss_lib_py.parsers.google_decimeter import AndroidDerived2022
 from gnss_lib_py.utils.coordinates import ecef_to_el_az, add_el_az
 from gnss_lib_py.utils.coordinates import geodetic_to_ecef
 from gnss_lib_py.utils.coordinates import ecef_to_geodetic, LocalCoord
 from gnss_lib_py.utils.coordinates import wrap_0_to_2pi
+from gnss_lib_py.utils.coordinates import el_az_to_enu_unit_vector
 from gnss_lib_py.navdata.operations import loop_time
 
 @pytest.fixture(name="local_ecef")
@@ -457,3 +459,75 @@ def test_wrap_0_to_2pi():
     angles_out = np.concatenate((np.linspace(np.pi,7*np.pi/4.,4),
                                  np.linspace(0,3*np.pi/4.,4)))
     np.testing.assert_array_almost_equal(wrap_0_to_2pi(angles_in),angles_out)
+
+
+#######################################
+# Unit Vector Tests
+
+@pytest.fixture(name="simple_sat_scenario")
+def fixture_simple_sat_scenario():
+    """
+    A simple set of satellites for DOP calculation.
+    
+    """
+    # Create a simple NavData instance
+    navdata = NavData()
+    
+    # Add a few satellites
+    navdata['gps_millis'] = np.array([0, 0, 0, 0, 0], dtype=int)
+    navdata['el_sv_deg'] = np.array([0, 0, 45, 45, 90], dtype=float)
+    navdata['az_sv_deg'] = np.array([0, 90, 180, 270, 360], dtype=float)
+
+    return navdata
+
+
+@pytest.fixture(name="simple_sat_expected_enu_unit_vectors")
+def fixture_simple_sat_expected_enu_unit_vectors():
+    """
+    The expected ENU unit vectors for the simple satellite scenario.
+    
+    """
+    # Expected ENU unit vectors
+    divsqrt2 = 1 / np.sqrt(2)
+    expected_enu_unit_vectors = np.array([[0, 1, 0],
+                                          [1, 0, 0],
+                                          [0, -divsqrt2, divsqrt2],
+                                          [-divsqrt2, 0, divsqrt2],
+                                          [0, 0, 1]])
+    
+    return expected_enu_unit_vectors
+
+@pytest.mark.parametrize('navdata, expected_los_vectors',
+                    [
+                        (lazy_fixture('simple_sat_scenario'), 
+                         lazy_fixture('simple_sat_expected_enu_unit_vectors'))
+                    ])
+def test_el_az_to_enu_unit_vector(navdata, expected_los_vectors):
+    """
+    Test the conversion from elevation and azimuth to ENU unit vectors.
+
+    Parameters
+    ----------
+    navdata : NavData
+        The input NavData instance.
+    expected_los_vectors : np.ndarray
+        The expected ENU unit vectors.
+
+    """
+
+    # Construct the ENU unit vectors
+    enu_unit_vectors = el_az_to_enu_unit_vector(navdata['el_sv_deg'],
+                                                navdata['az_sv_deg'])
+
+    # First check the shape
+    assert enu_unit_vectors.shape == expected_los_vectors.shape
+    assert enu_unit_vectors.shape[0] > enu_unit_vectors.shape[1]
+
+    # Check the ENU unit vectors are as expected
+    np.testing.assert_array_almost_equal(enu_unit_vectors, expected_los_vectors)
+
+    # Check the ENU unit vectors are normalized
+    np.testing.assert_array_almost_equal(
+        np.linalg.norm(enu_unit_vectors, axis=1), 
+            np.ones(expected_los_vectors.shape[0]))
+

tests/utils/test_dop.py

@@ -16,8 +16,7 @@
 
 from gnss_lib_py.navdata.navdata import NavData
 from gnss_lib_py.utils.dop import \
-    get_dop, calculate_dop, calculate_enu_unit_vectors, _calculate_enut_matrix
-from gnss_lib_py.parsers.google_decimeter import AndroidDerived2022
+    get_dop, calculate_dop, _calculate_enut_matrix
 
 
 #####################################################################
@@ -94,21 +93,6 @@ def test_simple_enu_unit_vectors(navdata, expected_los_vectors):
     A simple set of satellites for ENU unit vector calculation.
     
     """
-    # Construct the ENU unit vectors
-    enu_unit_vectors = calculate_enu_unit_vectors(navdata)
-
-    # First check the shape
-    assert enu_unit_vectors.shape == expected_los_vectors.shape
-    assert enu_unit_vectors.shape[0] > enu_unit_vectors.shape[1]
-
-    # Check the ENU unit vectors are as expected
-    np.testing.assert_array_almost_equal(enu_unit_vectors, expected_los_vectors)
-
-    # Check the ENU unit vectors are normalized
-    np.testing.assert_array_almost_equal(
-        np.linalg.norm(enu_unit_vectors, axis=1), 
-            np.ones(expected_los_vectors.shape[0]))
-    
     # Check the we get the expected ENUT matrix
     enut_matrix = _calculate_enut_matrix(navdata)
 
@@ -376,6 +360,11 @@ def test_dop_across_time_with_selection(navdata, which_dop):
         if which_dop[base_row]:
             assert base_row in dop_navdata.rows
 
+            # Check no nans
+            assert all(np.isfinite(dop_navdata[base_row]))
+            # Check no negative values
+            assert all(dop_navdata[base_row] >= 0)
+
     # Check that the DOP NavData is the expected length
     unique_gps_millis = np.unique(navdata['gps_millis'])
     assert len(dop_navdata['gps_millis']) == len(unique_gps_millis)