add heading, velocity, acceleration standards

Author: betaBison

docs/source/reference/reference.rst

@@ -175,6 +175,8 @@ State estimate naming conventions are as follows:
 * :code:`x_rx_m` : (float) receiver ECEF x position estimate in meters.
 * :code:`y_rx_m` : (float) receiver ECEF y position estimate in meters.
 * :code:`z_rx_m` : (float) receiver ECEF z position estimate in meters.
+* :code:`v_rx_mps` : (float) receiver total velocity estimate in
+  meters per second.
 * :code:`vx_rx_mps` : (float) receiver ECEF x velocity estimate in
   meters per second.
 * :code:`vy_rx_mps` : (float) receiver ECEF y velocity estimate in
@@ -183,6 +185,8 @@ State estimate naming conventions are as follows:
   meters per second.
 * :code:`ax_rx_mps2` : (float) receiver ECEF x acceleration estimate in
   meters per second squared.
+* :code:`a_rx_mps2` : (float) receiver total acceleration estimate in
+  meters per second squared.
 * :code:`ay_rx_mps2` : (float) receiver ECEF y acceleration estimate in
   meters per second squared.
 * :code:`az_rx_mps2` : (float) receiver ECEF z acceleration estimate in
@@ -196,6 +200,9 @@ State estimate naming conventions are as follows:
   degrees.
 * :code:`alt_rx_m` : (float) receiver altitude position estimate in
   meters. Referenced to the WGS-84 ellipsoid.
+* :code:`heading_rx_rad` : (float) receiver heading estimate in radians
+  where 0 radians is North and pi/2 radians is East, etc.
+  Assumed to be radians in the range between 0 and 2pi.
 
 Receiver ground truth naming conventions are as follows:
 
@@ -209,19 +216,31 @@ Receiver ground truth naming conventions are as follows:
   meters.
 * :code:`z_rx_gt_m` : (float) receiver ECEF z ground truth position in
   meters.
+* :code:`v_rx_gt_mps` : (float) receiver total velocity ground truth in
+  meters per second.
 * :code:`vx_rx_gt_mps` : (float) receiver ECEF x velocity ground truth
   in meters per second.
 * :code:`vy_rx_gt_mps` : (float) receiver ECEF y velocity ground truth
   in meters per second.
 * :code:`vz_rx_gt_mps` : (float) receiver ECEF z velocity ground truth
   in meters per second.
+* :code:`a_rx_gt_mps2` : (float) receiver total acceleration estimate in
+  meters per second squared.
+* :code:`ax_rx_gt_mps2` : (float) receiver ECEF x acceleration ground truth
+  in meters per second squared.
+* :code:`ay_rx_gt_mps2` : (float) receiver ECEF y acceleration ground truth
+  in meters per second squared.
+* :code:`az_rx_gt_mps2` : (float) receiver ECEF z acceleration ground truth
+  in meters per second squared.
 * :code:`lat_rx_gt_deg` : (float) receiver ground truth latitude in
   degrees.
 * :code:`lon_rx_gt_deg` : (float) receiver ground truth longitude in
   degrees.
 * :code:`alt_rx_gt_m` : (float) receiver ground truth altitude in meters.
   Referenced to the WGS-84 ellipsoid.
-
+* :code:`heading_rx_gt_rad` : (float) receiver heading estimate in
+  radians where 0 radians is North and pi/2 radians is East, etc.
+  Assumed to be radians in the range between 0 and 2pi.
 
 Module Level Function References
 --------------------------------

gnss_lib_py/parsers/android.py

@@ -14,6 +14,7 @@
 import pandas as pd
 
 from gnss_lib_py.parsers.navdata import NavData
+from gnss_lib_py.utils.coordinates import wrap_0_to_2pi
 from gnss_lib_py.utils.coordinates import geodetic_to_ecef
 from gnss_lib_py.utils.coordinates import ecef_to_geodetic
 from gnss_lib_py.utils.time_conversions import unix_to_gps_millis
@@ -271,14 +272,18 @@ def postprocess(self):
         https://www.kaggle.com/code/gymf123/tips-notes-from-the-competition-hosts
         """
         # Correcting reported altitude
-        self['alt_gt_m'] = self['alt_gt_m'] - 61.
-        gt_lla = np.transpose(np.vstack([self['lat_gt_deg'],
-                                         self['lon_gt_deg'],
-                                         self['alt_gt_m']]))
+        self['alt_rx_gt_m'] = self['alt_rx_gt_m'] - 61.
+        gt_lla = np.transpose(np.vstack([self['lat_rx_gt_deg'],
+                                         self['lon_rx_gt_deg'],
+                                         self['alt_rx_gt_m']]))
         gt_ecef = geodetic_to_ecef(gt_lla)
-        self["x_gt_m"] = gt_ecef[:,0]
-        self["y_gt_m"] = gt_ecef[:,1]
-        self["z_gt_m"] = gt_ecef[:,2]
+        self["x_rx_gt_m"] = gt_ecef[:,0]
+        self["y_rx_gt_m"] = gt_ecef[:,1]
+        self["z_rx_gt_m"] = gt_ecef[:,2]
+
+        # convert bearing degrees to heading in radians
+        self["heading_rx_gt_rad"] = np.deg2rad(self["heading_rx_gt_rad"])
+        self["heading_rx_gt_rad"] = wrap_0_to_2pi(self["heading_rx_gt_rad"])
 
     @staticmethod
     def _row_map():
@@ -289,10 +294,12 @@ def _row_map():
         row_map : Dict
             Dictionary of the form {old_name : new_name}
         """
-        row_map = {'latDeg' : 'lat_gt_deg',
-                   'lngDeg' : 'lon_gt_deg',
-                   'heightAboveWgs84EllipsoidM' : 'alt_gt_m',
-                   'millisSinceGpsEpoch' : 'gps_millis'
+        row_map = {'latDeg' : 'lat_rx_gt_deg',
+                   'lngDeg' : 'lon_rx_gt_deg',
+                   'heightAboveWgs84EllipsoidM' : 'alt_rx_gt_m',
+                   'millisSinceGpsEpoch' : 'gps_millis',
+                   'speedMps' : 'v_rx_gt_mps',
+                   'courseDegree' : 'heading_rx_gt_rad',
                 }
         return row_map
 
@@ -309,20 +316,24 @@ def postprocess(self):
         Notes
         -----
         """
-        if np.any(np.isnan(self['alt_gt_m'])):
+        if np.any(np.isnan(self['alt_rx_gt_m'])):
             warnings.warn("Some altitude values were missing, using 0m ", RuntimeWarning)
-            self['alt_gt_m'] = np.nan_to_num(self['alt_gt_m'])
-        gt_lla = np.transpose(np.vstack([self['lat_gt_deg'],
-                                         self['lon_gt_deg'],
-                                         self['alt_gt_m']]))
+            self['alt_rx_gt_m'] = np.nan_to_num(self['alt_rx_gt_m'])
+        gt_lla = np.transpose(np.vstack([self['lat_rx_gt_deg'],
+                                         self['lon_rx_gt_deg'],
+                                         self['alt_rx_gt_m']]))
         gt_ecef = geodetic_to_ecef(gt_lla)
-        self["x_gt_m"] = gt_ecef[:,0]
-        self["y_gt_m"] = gt_ecef[:,1]
-        self["z_gt_m"] = gt_ecef[:,2]
+        self["x_rx_gt_m"] = gt_ecef[:,0]
+        self["y_rx_gt_m"] = gt_ecef[:,1]
+        self["z_rx_gt_m"] = gt_ecef[:,2]
 
         # add gps milliseconds
         self["gps_millis"] = unix_to_gps_millis(self['unix_millis'])
 
+        # convert bearing degrees to heading in radians
+        self["heading_rx_gt_rad"] = np.deg2rad(self["heading_rx_gt_rad"])
+        self["heading_rx_gt_rad"] = wrap_0_to_2pi(self["heading_rx_gt_rad"])
+
     @staticmethod
     def _row_map():
         """Map row names from loaded data to gnss_lib_py standard
@@ -332,10 +343,13 @@ def _row_map():
         row_map : Dict
             Dictionary of the form {old_name : new_name}
         """
-        row_map = {'LatitudeDegrees' : 'lat_gt_deg',
-                   'LongitudeDegrees' : 'lon_gt_deg',
-                   'AltitudeMeters' : 'alt_gt_m',
-                   'UnixTimeMillis' : 'unix_millis'
+        row_map = {'LatitudeDegrees' : 'lat_rx_gt_deg',
+                   'LongitudeDegrees' : 'lon_rx_gt_deg',
+                   'AltitudeMeters' : 'alt_rx_gt_m',
+                   'SpeedMps' : 'v_rx_gt_mps',
+                   'BearingDegrees' : 'heading_rx_gt_rad',
+                   'UnixTimeMillis' : 'unix_millis',
+
                 }
         return row_map
 

gnss_lib_py/parsers/smart_loc.py

@@ -8,7 +8,7 @@
 import numpy as np
 
 from gnss_lib_py.parsers.navdata import NavData
-from gnss_lib_py.utils.coordinates import LocalCoord, geodetic_to_ecef
+from gnss_lib_py.utils.coordinates import LocalCoord, geodetic_to_ecef, wrap_0_to_2pi
 from gnss_lib_py.utils.time_conversions import tow_to_gps_millis
 
 
@@ -56,6 +56,16 @@ def postprocess(self):
         self["gps_millis"] = [tow_to_gps_millis(*x) for x in
                               zip(self["gps_week"],self["gps_tow"])]
 
+        # convert SmartLoc East counterclockwise heading into
+        # North clockwise heading standard
+        self["heading_rx_gt_rad"] = np.pi/2. - self["heading_rx_gt_rad"]
+        self["heading_rx_gt_rad"] = wrap_0_to_2pi(self["heading_rx_gt_rad"])
+
+        # remove duplicate rows
+        self.remove(rows=["GPSWeek [weeks]",
+                          "GPSSecondsOfWeek [s]"
+                          ],inplace=True)
+
 
     @staticmethod
     def _row_map():
@@ -78,12 +88,14 @@ def _row_map():
                    'Estimated Doppler measurement standard deviation (doStdev) [Hz]' \
                    : 'doppler_sigma_hz',
                    'Longitude (GT Lon) [deg]' : 'lon_rx_gt_deg',
-                   'Longitude Cov (GT Lon) [deg]' : 'lon_rx_gt_sigma_deg',
+                   'Longitude Cov (GT Lon) [deg]' : 'lon_sigma_rx_gt_deg',
                    'Latitude (GT Lat) [deg]' : 'lat_rx_gt_deg',
-                   'Latitude Cov (GT Lat) [deg]' : 'lat_rx_gt_sigma_deg',
+                   'Latitude Cov (GT Lat) [deg]' : 'lat_sigma_rx_gt_deg',
                    'Height above ellipsoid (GT Height) [m]' : 'alt_rx_gt_m',
-                   'Height above ellipsoid Cov (GT Height) [m]' : 'alt_rx_gt_sigma_m'
-
+                   'Height above ellipsoid Cov (GT Height) [m]' : 'alt_sigma_rx_gt_m',
+                   'Heading (0° = East, counterclockwise) - (GT Heading) [rad]' : 'heading_rx_gt_rad',
+                   'Velocity (GT Velocity) [m/s]' : 'v_rx_gt_mps',
+                   'Acceleration (GT Acceleration) [ms^2]' : 'a_rx_gt_mps2',
                    }
         return row_map
 
@@ -151,11 +163,11 @@ def calculate_gt_vel(smartloc_raw):
                 'az_rx_gt_mps2' : []}
     for _, _, measure_frame in smartloc_raw.loop_time('gps_millis', \
                                                         delta_t_decimals = -2):
-        yaw = measure_frame['Heading (0° = East, counterclockwise) - (GT Heading) [rad]', 0]
-        vel_gt = measure_frame['Velocity (GT Velocity) [m/s]', 0]
-        acc_gt = measure_frame['Acceleration (GT Acceleration) [ms^2]', 0]
-        vel_ned = np.array([[np.sin(yaw)*vel_gt], [np.cos(yaw)*vel_gt], [0.]])
-        acc_ned = np.array([[np.sin(yaw)*acc_gt], [np.cos(yaw)*acc_gt], [0.]])
+        yaw = measure_frame['heading_rx_gt_rad', 0]
+        vel_gt = measure_frame['v_rx_gt_mps', 0]
+        acc_gt = measure_frame['a_rx_gt_mps2', 0]
+        vel_ned = np.array([[np.cos(yaw)*vel_gt], [np.sin(yaw)*vel_gt], [0.]])
+        acc_ned = np.array([[np.cos(yaw)*acc_gt], [np.sin(yaw)*acc_gt], [0.]])
         vel_ecef = ned_frame.ned_to_ecefv(vel_ned)
         acc_ecef = ned_frame.ned_to_ecefv(acc_ned)
         vel_acc['vx_rx_gt_mps'].extend(np.repeat(vel_ecef[0, 0], len(measure_frame)))
@@ -167,4 +179,3 @@ def calculate_gt_vel(smartloc_raw):
     for row, values in vel_acc.items():
         smartloc_raw[row] = values
     return smartloc_raw
-

tests/parsers/test_android.py

@@ -634,7 +634,7 @@ def test_gt_alt_nan(root_path_2022):
     gt_2022_nan = os.path.join(root_path_2022, 'alt_nan_ground_truth.csv')
     with pytest.warns(RuntimeWarning):
         gt_2022 = android.AndroidGroundTruth2022(gt_2022_nan)
-        np.testing.assert_almost_equal(gt_2022['alt_gt_m'],
+        np.testing.assert_almost_equal(gt_2022['alt_rx_gt_m'],
                                        np.zeros(len(gt_2022)))
 
 def test_remove_all_data(derived_path_xl):

tests/parsers/test_smart_loc.py

@@ -12,9 +12,11 @@
 import pandas as pd
 
 from gnss_lib_py.parsers.navdata import NavData
+from gnss_lib_py.utils.coordinates import wrap_0_to_2pi
 from gnss_lib_py.parsers.smart_loc import SmartLocRaw, remove_nlos, \
                                         calculate_gt_ecef, calculate_gt_vel
 
+
 @pytest.fixture(name="root_path")
 def fixture_root_path():
     """Location of measurements for unit test
@@ -109,10 +111,14 @@ def test_smartloc_raw_df_equivalence(smartloc_raw, pd_df):
     """
     # Also tests if strings are being converted back correctly
     measure_df = smartloc_raw.pandas_df()
+    measure_df["heading_rx_gt_rad"] = -measure_df["heading_rx_gt_rad"]  + np.pi/2.
+    measure_df["heading_rx_gt_rad"] = wrap_0_to_2pi(measure_df["heading_rx_gt_rad"])
     inverse_row_map = {v : k for k,v in smartloc_raw._row_map().items()}
     measure_df.rename(columns=inverse_row_map, inplace=True)
     measure_df.drop(columns='gps_millis',inplace=True)
     pd_df['GNSS identifier (gnssId) []'] = pd_df['GNSS identifier (gnssId) []'].str.lower()
+    pd_df.drop(columns="GPSWeek [weeks]",inplace=True)
+    pd_df.drop(columns="GPSSecondsOfWeek [s]",inplace=True)
     pd.testing.assert_frame_equal(pd_df.sort_index(axis=1),
                                   measure_df.sort_index(axis=1),
                                   check_dtype=False, check_names=True)
@@ -266,5 +272,3 @@ def test_calculate_gt_vel_ecef(smartloc_raw):
                           'az_rx_gt_mps2',])
     old_shape[0] = old_shape[0] + 6
     np.testing.assert_equal(old_shape, new_shape)
-
-

tests/utils/test_gnss_models.py

@@ -58,21 +58,20 @@ def calculate_state(android_gt, idx):
     state : gnss_lib_py.parsers.navdata.NavData
         NavData containing state information for one time instance.
     """
-    bearing_rad = np.deg2rad(android_gt['BearingDegrees', idx])
 
-    v_gt_n = android_gt['SpeedMps', idx]*np.cos(bearing_rad)
-    v_gt_e = android_gt['SpeedMps', idx]*np.sin(bearing_rad)
+    v_gt_n = android_gt['v_rx_gt_mps', idx]*np.cos(android_gt['heading_rx_gt_rad', idx])
+    v_gt_e = android_gt['v_rx_gt_mps', idx]*np.sin(android_gt['heading_rx_gt_rad', idx])
     v_ned = np.array([[v_gt_n],[v_gt_e],[0]])
-    llh = np.array([[android_gt['lat_gt_deg', idx]],
-                    [android_gt['lon_gt_deg', idx]],
-                    [android_gt['alt_gt_m', idx]]])
+    llh = np.array([[android_gt['lat_rx_gt_deg', idx]],
+                    [android_gt['lon_rx_gt_deg', idx]],
+                    [android_gt['alt_rx_gt_m', idx]]])
     local_frame = LocalCoord.from_geodetic(llh)
     vx_ecef = local_frame.ned_to_ecefv(v_ned)
 
     state = NavData()
-    state['x_rx_m'] = android_gt['x_gt_m', idx]
-    state['y_rx_m'] = android_gt['y_gt_m', idx]
-    state['z_rx_m'] = android_gt['z_gt_m', idx]
+    state['x_rx_m'] = android_gt['x_rx_gt_m', idx]
+    state['y_rx_m'] = android_gt['y_rx_gt_m', idx]
+    state['z_rx_m'] = android_gt['z_rx_gt_m', idx]
     state['vx_rx_mps'] = vx_ecef[0,0]
     state['vy_rx_mps'] = vx_ecef[1,0]
     state['vz_rx_mps'] = vx_ecef[2,0]
@@ -128,7 +127,6 @@ def test_pseudorange_corrections(gps_measurement_frames, android_gt, iono_params
         curr_millis = frame['gps_millis', 0]
         gt_slice_idx = android_gt.argwhere('gps_millis', curr_millis)
         state = calculate_state(android_gt, gt_slice_idx)
-        # x_ecef = android_gt[['x_gt_m', 'y_gt_m', 'z_gt_m'], gt_slice_idx]
 
         # Test corrections with ephemeris parameters
         est_clk, est_trp, est_iono = gnss_models.calculate_pseudorange_corr(

tests/utils/test_sv_models.py

@@ -144,7 +144,7 @@ def test_sv_state_model(gps_measurement_frames, android_gt):
         # Get Android Derived states, sorted by SVs
         curr_millis = android_frames[idx]['gps_millis', 0]
         gt_slice_idx = android_gt.argwhere('gps_millis', curr_millis)
-        x_ecef = android_gt[['x_gt_m', 'y_gt_m', 'z_gt_m'], gt_slice_idx]
+        x_ecef = android_gt[['x_rx_gt_m', 'y_rx_gt_m', 'z_rx_gt_m'], gt_slice_idx]
 
 
         est_sv_posvel, _, _ = sv_models._find_sv_location(curr_millis, x_ecef, ephem=vis_ephems[idx])
@@ -181,7 +181,7 @@ def test_visible_ephem(all_gps_ephem, gps_measurement_frames, android_gt):
     for idx, frame in enumerate(android_frames):
         curr_millis = frame['gps_millis', 0]
         gt_slice_idx = android_gt.argwhere('gps_millis', curr_millis)
-        x_ecef = android_gt[['x_gt_m', 'y_gt_m', 'z_gt_m'], gt_slice_idx]
+        x_ecef = android_gt[['x_rx_gt_m', 'y_rx_gt_m', 'z_rx_gt_m'], gt_slice_idx]
         # Test visible satellite computation with ephemeris
         eph = sv_models._find_visible_ephem(curr_millis, x_ecef, ephem=vis_ephems[idx], el_mask=0.)
         vis_svs = set(eph['sv_id'])
@@ -210,7 +210,7 @@ def test_visible_sv_posvel(gps_measurement_frames, android_gt):
     for idx, sv_posvel in enumerate(sv_states):
         curr_millis = android_frames[idx]['gps_millis', 0]
         gt_slice_idx = android_gt.argwhere('gps_millis', curr_millis)
-        x_ecef = android_gt[['x_gt_m', 'y_gt_m', 'z_gt_m'], gt_slice_idx]
+        x_ecef = android_gt[['x_rx_gt_m', 'y_rx_gt_m', 'z_rx_gt_m'], gt_slice_idx]
 
         # Test that actually visible satellites are subset of expected satellites
         vis_posvel = sv_models._find_visible_sv_posvel(curr_millis, x_ecef, sv_posvel, el_mask=0.)