diff --git a/README.md b/README.md index f530df73f..19f0a653b 100644 --- a/README.md +++ b/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. diff --git a/docs/api-overview.md b/docs/api-overview.md index 010d13948..9d61c48d4 100644 --- a/docs/api-overview.md +++ b/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 diff --git a/docs/conventions.md b/docs/conventions.md index 608eb92f4..77ad712f9 100644 --- a/docs/conventions.md +++ b/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. diff --git a/examples/README.md b/examples/README.md index 36343f466..fbb80bc0e 100644 --- a/examples/README.md +++ b/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 diff --git a/examples/basic/scgp_measurement_reliability.py b/examples/basic/scgp_measurement_reliability.py new file mode 100644 index 000000000..51b10e1b0 --- /dev/null +++ b/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()) diff --git a/src/pyrecest/filters/gprhm_tracker.py b/src/pyrecest/filters/gprhm_tracker.py index 760c30172..66506b3a4 100644 --- a/src/pyrecest/filters/gprhm_tracker.py +++ b/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, ) diff --git a/tests/filters/test_scgp_tracker.py b/tests/filters/test_scgp_tracker.py index 8da79e1e8..170dd44b1 100644 --- a/tests/filters/test_scgp_tracker.py +++ b/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()