Fixed reference YPR interpolation and plot hover text timestamps.

python/fusion_engine_client/analysis/analyzer.py

@@ -463,21 +463,25 @@ def _calc_stats(input):
                                           mode='markers', marker={'color': 'red'}),
                              1, 1)
             if dp1_stats is not None:
-                figure.add_trace(go.Scattergl(x=time, y=dp1_stats['max'], name='P1 Time Interval (Max)',
+                figure.add_trace(go.Scattergl(x=time, y=dp1_stats['max'], customdata=customdata,
+                                              name='P1 Time Interval (Max)',
                                               mode='markers', marker={'symbol': 'triangle-up-open'}),
                                  1, 1)
-                figure.add_trace(go.Scattergl(x=time, y=dp1_stats['min'], name='P1 Time Interval (Min)',
+                figure.add_trace(go.Scattergl(x=time, y=dp1_stats['min'], customdata=customdata,
+                                              name='P1 Time Interval (Min)',
                                               mode='markers', marker={'symbol': 'triangle-down-open'}),
                                  1, 1)
 
             figure.add_trace(go.Scattergl(x=time, y=dgps_time, name='GPS Time Interval', customdata=customdata,
                                           mode='markers', marker={'color': 'green'}),
                              1, 1)
             if dgps_stats is not None:
-                figure.add_trace(go.Scattergl(x=time, y=dgps_stats['max'], name='GPS Time Interval (Max)',
+                figure.add_trace(go.Scattergl(x=time, y=dgps_stats['max'], customdata=customdata,
+                                              name='GPS Time Interval (Max)',
                                               mode='markers', marker={'symbol': 'triangle-up-open'}),
                                  1, 1)
-                figure.add_trace(go.Scattergl(x=time, y=dgps_stats['min'], name='GPS Time Interval (Min)',
+                figure.add_trace(go.Scattergl(x=time, y=dgps_stats['min'], customdata=customdata,
+                                              name='GPS Time Interval (Min)',
                                               mode='markers', marker={'symbol': 'triangle-down-open'}),
                                  1, 1)
 
@@ -1353,16 +1357,16 @@ def plot_map(self, mapbox_token, reference: Optional[ReferenceData] = None):
         # `%{x}` tied to a real date-typed axis) -- but a customdata entry with no format spec at all is substituted
         # verbatim, so we precompute the UTC string in Python (cheap, vectorized) and reference it that way.
         _POSITION_HOVERTEMPLATE = (
-            "LLA: %{lat:.8f}, %{lon:.8f}, %{customdata[5]:.2f}<br>"
+            "LLA: %{lat:.8f}, %{lon:.8f}, %{customdata[6]:.2f}<br>"
             "Rel: %{customdata[1]:.3f} sec (P1: %{customdata[2]:.3f} sec)<br>"
             "UTC: %{customdata[0]}<br>"
-            "GPS: %{customdata[3]:.0f}:%{customdata[4]:.3f}<br>"
-            "Std Dev: %{customdata[6]:.2f} m (2D), %{customdata[7]:.2f} m (3D)"
+            "GPS: %{customdata[3]:.0f}:%{customdata[4]:.3f} (%{customdata[5]:.3f} sec)<br>"
+            "Std Dev: %{customdata[7]:.2f} m (2D), %{customdata[8]:.2f} m (3D)"
         )
         # Used instead of _POSITION_HOVERTEMPLATE when reference data is avaiable to compute position error.
         _POSITION_HOVERTEMPLATE_WITH_ERROR = (
             _POSITION_HOVERTEMPLATE +
-            "<br>Error: %{customdata[8]:.2f} m (2D), %{customdata[9]:.2f} m (3D)"
+            "<br>Error: %{customdata[9]:.2f} m (2D), %{customdata[10]:.2f} m (3D)"
         )
 
         def _build_position_customdata(p1_time: np.ndarray, gps_time: np.ndarray, lla_deg: np.ndarray,
@@ -1384,7 +1388,7 @@ def _build_position_customdata(p1_time: np.ndarray, gps_time: np.ndarray, lla_de
             # array (that would coerce every column to strings, breaking the numeric %{customdata[N]:.3f}-style
             # formatting for the rest); build it as a plain list of per-point rows instead. error_enu_m, when
             # present, is appended after the UTC string so its indices stay fixed regardless of whether it's used.
-            numeric = np.column_stack((rel_time, p1_time, gps_week, gps_tow_sec, lla_deg[2],
+            numeric = np.column_stack((rel_time, p1_time, gps_week, gps_tow_sec, gps_time, lla_deg[2],
                                        np.linalg.norm(std_enu_m[0:2, :], axis=0),
                                        np.linalg.norm(std_enu_m, axis=0)))
             if error_enu_m is None:
@@ -1767,8 +1771,8 @@ def _plot_gnss_skyplot_for_source(self, source_id: int, decimate=True):
             color_by_sv.append(color_by_prn[sv_id.get_prn()])
             color_by_cn0.append(max_cn0_dbhz)
 
-            text = ['P1: %.1f sec<br>(Az, El): (%.2f, %.2f) deg<br>C/N0: %.1f dB-Hz' %
-                    (t, a, e, c) for t, a, e, c in zip(p1_time, az_deg, el_deg, max_cn0_dbhz)]
+            text = ['(Az, El): (%.2f, %.2f) deg<br>C/N0: %.1f dB-Hz<br>%s' % (a, e, c, time_text)
+                    for a, e, c, time_text in zip(az_deg, el_deg, max_cn0_dbhz, self._time_hover_text(p1_time))]
 
             # The per-point P1 time is what the time slider below the plot filters on (see _time_slider_js()). It
             # is not referenced by the hover text, which uses `text` above.
@@ -4402,6 +4406,39 @@ def _time_hover_customdata(self, p1_time: np.ndarray, gps_time: Optional[np.ndar
         else:
             return np.vstack((p1_time,))
 
+    def _time_hover_text(self, p1_time: np.ndarray, gps_time: Optional[np.ndarray] = None) -> List[str]:
+        """!
+        @brief Build the same Rel/P1/UTC/GPS hover text as `BuildTimeHoverTextFromTimes()` in Python.
+
+        For plots whose hover text is generated ahead of time instead of by the injected hover JS (see
+        `plotly_data_support.js`).
+
+        @param p1_time The P1 time for each point.
+        @param gps_time The GPS time for each point, or `None` to compute it from `p1_time`.
+
+        @return A list of hover text strings, one per point, with lines separated by `<br>`.
+        """
+        # Look up the GPS and UTC times for each point.
+        if gps_time is None:
+            gps_time = self.time_provider.p1_to_gps(p1_time)
+        utc_time = self.time_provider.gps_sec_to_datetime64_array(gps_time)
+        p1_t0_sec = float(self.t0) if self.t0 is not None else np.nan
+
+        # Format the lines for each point, skipping any time that is not available.
+        result = []
+        for p1, gps, utc in zip(p1_time, gps_time, utc_time):
+            lines = []
+            if not np.isnan(p1):
+                lines.append(f'Rel: {p1 - p1_t0_sec:.3f} sec (P1: {p1:.3f} sec)')
+            if not np.isnan(gps):
+                if not np.isnat(utc):
+                    lines.append(f"UTC: {np.datetime_as_string(utc, unit='ms').replace('T', ' ')}")
+                week = int(gps // SECONDS_PER_WEEK)
+                tow_sec = gps - week * SECONDS_PER_WEEK
+                lines.append(f'GPS: {week}:{tow_sec:.3f} ({gps:.3f} sec)')
+            result.append('<br>'.join(lines))
+        return result
+
     def _custom_tooltip_js(self, time_source: str = 'p1', precision: Optional[int] = 3,
                            value_label: Optional[str] = None, show_name: bool = True, show_value: bool = True) -> str:
         """!

python/fusion_engine_client/analysis/reference.py

@@ -16,24 +16,44 @@
 _OWN_LOG_STATISTICS = ('first', 'first_fixed', 'median', 'median_fixed')
 
 
-def _interp_columns(x: np.ndarray, y: Optional[np.ndarray], x_new: np.ndarray) -> Optional[np.ndarray]:
+def _interp_columns(x: np.ndarray, y: Optional[np.ndarray], x_new: np.ndarray,
+                    angle_rows: tuple = ()) -> Optional[np.ndarray]:
     """!
-    @brief Linearly interpolate each row of `y` (sampled at `x`) onto `x_new`, returning NaN for any query point
-           outside the range of `x` or where too few valid (non-NaN) samples exist to interpolate.
+    @brief Linearly interpolate each row of `y` (sampled at `x`) onto `x_new`.
+
+    NaN samples in `y` are skipped, so a query point between two valid samples is interpolated across any NaN gap
+    between them. The result is NaN for any query point outside the range of `x`, and for every query point in a row
+    with fewer than 2 valid samples.
+
+    @param x The sample times, sorted in ascending order.
+    @param y An MxN array of values sampled at `x`, or `None`.
+    @param x_new The times to interpolate onto.
+    @param angle_rows Indices of rows in `y` containing angles (in degrees) that wrap at +/-180 degrees. These rows are
+           unwrapped before interpolating, so the result follows the shorter arc between samples, and the result is
+           wrapped back to [-180, 180).
+
+    @return An Mx`len(x_new)` array of interpolated values, or `None` if `y` is `None`.
     """
     if y is None:
         return None
 
+    # Values outside the range of the sample times are left as NaN.
     out = np.full((y.shape[0], len(x_new)), np.nan)
     in_range = np.logical_and(x_new >= x[0], x_new <= x[-1])
     if not np.any(in_range):
         return out
 
+    # Interpolate each row using only its valid samples.
     for row in range(y.shape[0]):
         valid = ~np.isnan(y[row, :])
         if np.count_nonzero(valid) < 2:
             continue
-        out[row, in_range] = np.interp(x_new[in_range], x[valid], y[row, valid])
+
+        if row in angle_rows:
+            y_valid = np.degrees(np.unwrap(np.radians(y[row, valid])))
+            out[row, in_range] = (np.interp(x_new[in_range], x[valid], y_valid) + 180.0) % 360.0 - 180.0
+        else:
+            out[row, in_range] = np.interp(x_new[in_range], x[valid], y[row, valid])
 
     return out
 
@@ -134,7 +154,8 @@ def get_coverage_mask(self, gps_time_sec: np.ndarray, warn: bool = True) -> np.n
                 "range will be omitted." % (self.description, coverage_percent))
         return in_range
 
-    def _interpolate_cached(self, key: str, y: np.ndarray, gps_time_sec: np.ndarray) -> np.ndarray:
+    def _interpolate_cached(self, key: str, y: np.ndarray, gps_time_sec: np.ndarray,
+                            angle_rows: tuple = ()) -> np.ndarray:
         """!
         @brief Interpolate `y` onto `gps_time_sec`, caching the result to avoid recomputing for repeated queries.
 
@@ -145,6 +166,7 @@ def _interpolate_cached(self, key: str, y: np.ndarray, gps_time_sec: np.ndarray)
         @param key A name identifying which field is being interpolated (e.g., 'position').
         @param y The data to be interpolated, sampled at `self.gps_time_sec`.
         @param gps_time_sec The GPS timestamps (sec) to interpolate onto.
+        @param angle_rows Indices of rows in `y` containing angles (in degrees) that wrap at +/-180 degrees.
 
         @return The interpolated data.
         """
@@ -156,7 +178,7 @@ def _interpolate_cached(self, key: str, y: np.ndarray, gps_time_sec: np.ndarray)
         else:
             signature = None
 
-        result = _interp_columns(self.gps_time_sec, y, gps_time_sec)
+        result = _interp_columns(self.gps_time_sec, y, gps_time_sec, angle_rows=angle_rows)
         if signature is not None:
             self._interp_cache[key] = (signature, result)
         return result
@@ -190,13 +212,16 @@ def interpolate_ypr_deg(self, gps_time_sec: np.ndarray) -> Optional[np.ndarray]:
         @brief Interpolate this reference's YPR orientation (deg) onto the specified GPS timestamps (sec).
 
         Returns `None` if orientation is not available. Entries outside the covered time range are set to NaN.
+
+        Yaw and roll are interpolated along the shorter arc between samples, so they wrap correctly at +/-180 degrees,
+        and are returned in the range [-180, 180). Pitch is bounded to +/-90 degrees and is interpolated directly.
         """
         if self.ypr_deg is None:
             return None
         elif self.is_stationary:
             return np.tile(self.ypr_deg.reshape(3, 1), (1, len(gps_time_sec)))
         else:
-            return self._interpolate_cached('ypr', self.ypr_deg, gps_time_sec)
+            return self._interpolate_cached('ypr', self.ypr_deg, gps_time_sec, angle_rows=(0, 2))
 
     # -------------------------------------------------------------------------------------------------------------
     # Constructors

python/tests/test_reference.py

@@ -337,6 +337,44 @@ def test_interpolate_velocity_with_all_nan_row(self, moving_ref):
     def test_interpolate_ypr_none_when_unavailable(self, moving_ref):
         assert moving_ref.interpolate_ypr_deg(np.array([5.0])) is None
 
+    @staticmethod
+    def _ypr_ref(ypr_deg):
+        return ReferenceData(description='test', is_truth=True,
+                             position_ecef_m=np.zeros((3, ypr_deg.shape[1])),
+                             gps_time_sec=np.arange(ypr_deg.shape[1], dtype=float) * 10.0,
+                             ypr_deg=ypr_deg)
+
+    def test_interpolate_ypr_wraps_yaw_and_roll(self):
+        # Yaw and roll midway between 179 and -179 degrees are 180, not 0.
+        ref = self._ypr_ref(np.array([[179.0, -179.0], [10.0, 20.0], [-179.0, 179.0]]))
+        ypr = ref.interpolate_ypr_deg(np.array([2.5, 5.0, 7.5]))
+        assert ypr[0, :] == pytest.approx([179.5, -180.0, -179.5])
+        assert ypr[2, :] == pytest.approx([-179.5, -180.0, 179.5])
+        assert ypr[1, :] == pytest.approx([12.5, 15.0, 17.5])
+
+    def test_interpolate_ypr_without_wrap_is_linear(self):
+        ref = self._ypr_ref(np.array([[10.0, 50.0], [-80.0, 80.0], [-30.0, 30.0]]))
+        ypr = ref.interpolate_ypr_deg(np.array([0.0, 5.0, 10.0]))
+        assert ypr[0, :] == pytest.approx([10.0, 30.0, 50.0])
+        assert ypr[1, :] == pytest.approx([-80.0, 0.0, 80.0])
+        assert ypr[2, :] == pytest.approx([-30.0, 0.0, 30.0])
+
+    def test_interpolate_ypr_does_not_unwrap_pitch(self):
+        # Pitch values far apart are interpolated directly, not along the shorter arc through +/-180.
+        ref = self._ypr_ref(np.array([[0.0, 0.0], [89.0, -89.0], [0.0, 0.0]]))
+        ypr = ref.interpolate_ypr_deg(np.array([5.0]))
+        assert ypr[1, 0] == pytest.approx(0.0)
+
+    def test_interpolate_ypr_nan_handling(self):
+        # A NaN sample is skipped (the wrap is still followed across the gap), an all-NaN row stays NaN, and query
+        # times outside the reference are NaN.
+        ref = self._ypr_ref(np.array([[170.0, np.nan, -170.0], [np.nan, np.nan, np.nan], [0.0, 1.0, 2.0]]))
+        ypr = ref.interpolate_ypr_deg(np.array([-1.0, 10.0, 21.0]))
+        assert ypr[0, 1] == pytest.approx(-180.0)
+        assert np.all(np.isnan(ypr[1, :]))
+        assert ypr[2, 1] == pytest.approx(1.0)
+        assert np.all(np.isnan(ypr[:, [0, 2]]))
+
     def test_stationary_interpolation_tiles(self):
         ref = ReferenceData.from_stationary_ecef(np.array([1.0, 2.0, 3.0]))
         pos = ref.interpolate_position_ecef_m(np.array([0.0, 1.0, 2.0]))