add angle wrapping

Author: betaBison

gnss_lib_py/utils/coordinates.py

@@ -547,3 +547,26 @@ def add_el_az(navdata, receiver_state, inplace=False):
     data_el_az["az_sv_deg"] = sv_el_az[1,:]
 
     return data_el_az
+
+def wrap_0_to_2pi(angles):
+    """Wraps an arbitrary radian between [0, 2pi).
+
+    Angles must be in radians.
+
+    Parameters
+    ----------
+    angles : np.ndarray
+        Array of angles in radians to wrap between 0 and 2pi.
+
+    Returns
+    -------
+    angles : np.ndarray
+        Angles wrapped between 0 and 2pi in radians.
+
+    """
+    while np.any(angles < 0.):
+        angles = np.where(angles<0. , 2.*np.pi+angles, angles)
+    while np.any(angles >= 2*np.pi):
+        angles = np.where(angles>= 2*np.pi , angles-2.*np.pi, angles)
+
+    return angles

tests/utils/test_coordinates.py

@@ -16,6 +16,7 @@
 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
 
 
 @pytest.fixture(name="local_ecef")
@@ -424,3 +425,35 @@ def test_add_el_az(navdata):
                                          navdata["el_sv_deg"])
     np.testing.assert_array_almost_equal(data_el_az["az_sv_deg"],
                                          navdata["az_sv_deg"])
+
+
+def test_wrap_0_to_2pi():
+    """Test wrapping.
+
+    """
+    # nothing should change
+    angles_in = np.linspace(0,7*np.pi/4.,8)
+    angles_out = np.linspace(0,7*np.pi/4.,8)
+    np.testing.assert_array_equal(wrap_0_to_2pi(angles_in),angles_out)
+
+    # test greater than loop
+    angles_in = np.linspace(0,7*np.pi/4.,8) + 16*np.pi
+    angles_out = np.linspace(0,7*np.pi/4.,8)
+    np.testing.assert_array_almost_equal(wrap_0_to_2pi(angles_in),angles_out)
+
+    # test greater than loop
+    angles_in = np.linspace(0,7*np.pi/4.,8) - 12*np.pi
+    angles_out = np.linspace(0,7*np.pi/4.,8)
+    np.testing.assert_array_almost_equal(wrap_0_to_2pi(angles_in),angles_out)
+
+    # test positive offset loop
+    angles_in = np.linspace(0,7*np.pi/4.,8) + 7*np.pi
+    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)
+
+    # test positive offset loop
+    angles_in = np.linspace(0,7*np.pi/4.,8) - 11*np.pi
+    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)