Support SCGP measurement reliability weights

README.md

@@ -168,6 +168,8 @@ Install the matching optional extra before using a non-default backend.
 
 - `examples/basic/kalman_filter.py` contains a small executable Kalman filter
   example.
+- `examples/basic/scgp_measurement_reliability.py` demonstrates
+  reliability-weighted measurements for the full SCGP extended-object tracker.
 - `tests/` contains additional usage examples for distributions, filters,
   smoothers, evaluation, sampling, metrics, and tracking utilities.
 

docs/api-overview.md

@@ -107,6 +107,9 @@ Common starting points include:
   directional estimation;
 - `MultiBernoulliTracker`, `GlobalNearestNeighbor`, `JPDAF`, and
   `TrackManager` for tracking workflows.
+- `FullSCGPTracker` for star-convex Gaussian-process extended-object tracking,
+  including optional per-measurement covariance, reliability weights, and
+  active-measurement masks.
 
 The IDKF node stores one additive information-vector contribution per origin
 node and reconstructs the fused Gaussian from the shared information matrix and

docs/conventions.md

@@ -105,6 +105,30 @@ labels, estimates = tracker.get_labeled_point_estimate(number_of_targets=2)
 Here `estimates` has shape `(state_dim, num_targets)`, and the column order
 matches the order of `labels`.
 
+## Reliability-Weighted SCGP Measurements
+
+`FullSCGPTracker.update(...)` accepts one two-dimensional contour measurement
+or a measurement set with one two-dimensional measurement per row. A
+column-oriented `(2, num_measurements)` array is also accepted and transposed
+internally.
+
+For extended-object updates where only some measurements are reliable, pass
+`measurement_weights` and/or `active_measurement_mask`:
+
+```python
+tracker.update(
+    measurements,
+    R=measurement_noise,
+    measurement_weights=array([1.0, 0.25, 0.0]),
+    active_measurement_mask=array([True, True, False]),
+)
+```
+
+Each active measurement covariance block is scaled as `R_i / weight_i`.
+Zero-weight measurements and masked measurements are skipped. `R` may be a
+shared `(2, 2)` covariance matrix or a per-measurement array with shape
+`(num_measurements, 2, 2)`.
+
 ## Distribution Inputs
 
 Many distribution `pdf` methods accept either one point or a batch of points.

examples/README.md

@@ -87,6 +87,20 @@ python examples/basic/multi_target_tracking.py
 
 This example currently requires the NumPy backend.
 
+### SCGP measurement reliability
+
+`basic/scgp_measurement_reliability.py` runs one full star-convex
+Gaussian-process tracker update with per-measurement reliability weights and an
+active-measurement mask. It demonstrates how to down-weight partially reliable
+extended-object measurements and skip unsupported measurements without changing
+the measurement array.
+
+Run it with:
+
+```bash
+python examples/basic/scgp_measurement_reliability.py
+```
+
 ### von Mises-Fisher multiplication
 
 `basic/von_mises_fisher_multiplication.py` multiplies two von Mises-Fisher

examples/basic/scgp_measurement_reliability.py

@@ -0,0 +1,39 @@
+"""Use reliability-weighted measurements with the full SCGP tracker."""
+
+from pyrecest.backend import array, eye
+from pyrecest.filters import FullSCGPTracker
+
+
+def make_tracker():
+    n_base_points = 8
+    return FullSCGPTracker(
+        n_base_points,
+        kinematic_state=array([0.0, 0.0, 0.0, 1.0, 0.0]),
+        kinematic_covariance=1e-4 * eye(5),
+        shape_state=array([1.0] * n_base_points),
+        shape_covariance=0.05 * eye(n_base_points),
+        measurement_noise=0.02 * eye(2),
+        radial_noise_variance=0.01,
+        extent_forgetting_rate=0.2,
+        reference_extent=array([1.0] * n_base_points),
+    )
+
+
+tracker = make_tracker()
+measurements = array(
+    [
+        [1.4, 0.1],
+        [0.2, 1.2],
+        [3.0, 3.0],
+    ]
+)
+
+tracker.update(
+    measurements,
+    measurement_weights=array([1.0, 0.25, 0.0]),
+    active_measurement_mask=array([True, True, False]),
+)
+
+print("active measurement indices:", tracker.last_active_measurement_indices)
+print("measurement weights:", tracker.last_measurement_weights)
+print("kinematic estimate:", tracker.get_point_estimate_kinematics())

src/pyrecest/filters/gprhm_tracker.py

@@ -644,10 +644,48 @@ def _normalize_active_measurement_mask(
             )
         return [bool(mask[index]) for index in range(n_measurements)]
 
+    def _normalize_measurement_noise_covariances(
+        self,
+        measurement_noise,
+        n_measurements,
+    ):
+        noise = array(measurement_noise)
+        if noise.ndim == 3:
+            expected_shape = (
+                n_measurements,
+                self.measurement_dim,
+                self.measurement_dim,
+            )
+            if noise.shape != expected_shape:
+                raise ValueError(
+                    f"R must have shape ({self.measurement_dim}, {self.measurement_dim}) "
+                    f"or ({n_measurements}, {self.measurement_dim}, {self.measurement_dim}) "
+                    "for per-measurement covariances"
+                )
+            return stack(
+                [
+                    self._as_covariance_matrix(
+                        noise[index],
+                        self.measurement_dim,
+                        f"R[{index}]",
+                        require_positive_semidefinite=False,
+                    )
+                    for index in range(n_measurements)
+                ]
+            )
+
+        shared_noise = self._as_covariance_matrix(
+            noise,
+            self.measurement_dim,
+            "R",
+            require_positive_semidefinite=False,
+        )
+        return stack([shared_noise for _ in range(n_measurements)])
+
     def _stack_measurement_terms(
         self,
         measurements,
-        measurement_noise,
+        measurement_noises,
         measurement_weights=None,
         active_measurement_mask=None,
     ):
@@ -668,7 +706,10 @@ def _stack_measurement_terms(
             if not active_mask[measurement_index] or weight <= 0.0:
                 continue
             measurement_jacobian, predicted_measurement, noise_covariance = (
-                self._measurement_model_terms(measurement, measurement_noise)
+                self._measurement_model_terms(
+                    measurement,
+                    measurement_noises[measurement_index],
+                )
             )
             active_indices.append(measurement_index)
             measurement_jacobians.append(measurement_jacobian)
@@ -696,8 +737,10 @@ def update(
     ):
         """Update the tracker with optional per-measurement reliabilities.
 
-        ``measurement_weights`` scales each measurement covariance block as
-        ``R_i / weight_i``. Zero-weight or masked measurements are skipped.
+        ``R`` may be a shared measurement covariance or an array of shape
+        ``(n_measurements, 2, 2)`` with one covariance per measurement.
+        ``measurement_weights`` scales each active measurement covariance block
+        as ``R_i / weight_i``. Zero-weight or masked measurements are skipped.
         ``active_measurement_mask`` can be used to explicitly disable cluttered,
         occluded, or otherwise unsupported measurements.
         """
@@ -706,14 +749,10 @@ def update(
         if sigma_squared_s is not None:
             self.scale_variance = float(sigma_squared_s)
         measurements = self._normalize_measurements(measurements)
-        measurement_noise = self.measurement_noise
-        if R is not None:
-            measurement_noise = self._as_covariance_matrix(
-                R,
-                self.measurement_dim,
-                "R",
-                require_positive_semidefinite=False,
-            )
+        measurement_noises = self._normalize_measurement_noise_covariances(
+            self.measurement_noise if R is None else R,
+            measurements.shape[0],
+        )
 
         (
             measurement_jacobian,
@@ -722,7 +761,7 @@ def update(
             active_indices,
         ) = self._stack_measurement_terms(
             measurements,
-            measurement_noise,
+            measurement_noises,
             measurement_weights=measurement_weights,
             active_measurement_mask=active_measurement_mask,
         )

tests/filters/test_scgp_tracker.py

@@ -28,7 +28,12 @@ def setUp(self):
         self.kinematic_covariance = 1e-4 * eye(5)
         self.measurement_noise = 0.02 * eye(2)
 
-    def _make_tracker(self, tracker_cls=FullSCGPTracker, shape_state=None):
+    def _make_tracker(
+        self,
+        tracker_cls=FullSCGPTracker,
+        shape_state=None,
+        radial_noise_variance=0.01,
+    ):
         if shape_state is None:
             shape_state = self.shape_state
         return tracker_cls(
@@ -38,7 +43,7 @@ def _make_tracker(self, tracker_cls=FullSCGPTracker, shape_state=None):
             shape_state=shape_state,
             shape_covariance=self.shape_covariance,
             measurement_noise=self.measurement_noise,
-            radial_noise_variance=0.01,
+            radial_noise_variance=radial_noise_variance,
             extent_forgetting_rate=0.2,
             reference_extent=self.shape_state,
         )
@@ -134,6 +139,50 @@ def test_measurement_weight_changes_update_strength(self):
         self.assertGreater(float(high_weight_delta), float(low_weight_delta))
         npt.assert_allclose(low_weight_tracker.last_measurement_weights, array([0.05]))
 
+    def test_measurement_weight_matches_scaled_measurement_noise(self):
+        weighted_tracker = self._make_tracker(radial_noise_variance=0.0)
+        scaled_noise_tracker = self._make_tracker(radial_noise_variance=0.0)
+        measurement = array([1.4, 0.0])
+        measurement_noise = 0.02 * eye(2)
+        measurement_weight = 0.25
+
+        weighted_tracker.update(
+            measurement,
+            R=measurement_noise,
+            measurement_weights=measurement_weight,
+        )
+        scaled_noise_tracker.update(
+            measurement,
+            R=measurement_noise / measurement_weight,
+        )
+
+        npt.assert_allclose(weighted_tracker.state, scaled_noise_tracker.state)
+        npt.assert_allclose(
+            weighted_tracker.covariance,
+            scaled_noise_tracker.covariance,
+        )
+
+    def test_per_measurement_noise_matches_single_active_measurement_update(self):
+        masked_tracker = self._make_tracker()
+        single_tracker = self._make_tracker()
+        measurements = array([[1.4, 0.2], [0.1, 1.3]])
+        measurement_noises = array([0.02 * eye(2), 0.2 * eye(2)])
+
+        masked_tracker.update(
+            measurements,
+            R=measurement_noises,
+            active_measurement_mask=array([False, True]),
+        )
+        single_tracker.update(measurements[1], R=measurement_noises[1])
+
+        npt.assert_allclose(masked_tracker.state, single_tracker.state, atol=1e-12)
+        npt.assert_allclose(
+            masked_tracker.covariance,
+            single_tracker.covariance,
+            atol=1e-12,
+        )
+        self.assertEqual(masked_tracker.last_active_measurement_indices, [1])
+
     def test_measurement_weights_validate_shape_and_values(self):
         tracker = self._make_tracker()
 
@@ -144,6 +193,11 @@ def test_measurement_weights_validate_shape_and_values(self):
             )
         with self.assertRaises(ValueError):
             tracker.update(array([1.4, 0.2]), measurement_weights=-1.0)
+        with self.assertRaises(ValueError):
+            tracker.update(
+                array([[1.4, 0.2], [0.1, 1.3]]),
+                R=array([eye(2)]),
+            )
 
     def test_full_tracker_contour_and_bounding_box(self):
         tracker = self._make_tracker()