Replace bias_state with a bias_track in BiasFeeder and BiasUpdaters

Author: csherman-dstl

docs/examples/sensorfusion/senor_bias.py

@@ -159,16 +159,15 @@
 # measurements.
 #
 # These are  all added to a MultiDataFeeder to combine them into single detection feed.
-import copy
 from stonesoup.predictor.kalman import KalmanPredictor
 from stonesoup.feeder.bias import TranslationBiasFeeder
 from stonesoup.feeder.multi import MultiDataFeeder
 
 bias_state = GaussianState([[0.], [0.]], np.diag([5**2, 5**2]), start_time)
-bias_track = Track([copy.copy(bias_state)])
+bias_track = Track([bias_state])
 
 bias_predictor = KalmanPredictor(CombinedLinearGaussianTransitionModel([RandomWalk(1e-1)]*2))
-bias_feeder = TranslationBiasFeeder(measurements[0], bias_state)
+bias_feeder = TranslationBiasFeeder(measurements[0], bias_track)
 
 # %%
 # These are  all added to a MultiDataFeeder to combine them into single detection feed.
@@ -201,7 +200,7 @@
 from stonesoup.models.measurement.bias import TranslationBiasModelWrapper
 
 bias_updater = GaussianBiasUpdater(
-    bias_state, bias_predictor, TranslationBiasModelWrapper, updater)
+    bias_track, bias_predictor, TranslationBiasModelWrapper, updater)
 bias_hypothesiser = DistanceHypothesiser(
     predictor, bias_updater, measure=Mahalanobis(), missed_distance=5)
 bias_data_associator = GNNWith2DAssignment(bias_hypothesiser)
@@ -228,11 +227,10 @@
             track.append(hyp.prediction)
 
         # Adjust measurement models by removing relative bias for plotting later
-        rel_bias_vector = bias_track.state_vector - bias_state.state_vector
+        rel_bias_vector = bias_track[-2].state_vector - bias_track[-1].state_vector
         for model in {d.measurement_model for d in detections}:
             model.translation_offset -= rel_bias_vector
             model.applied_bias += rel_bias_vector  # No longer used, but for completeness
-        bias_track.append(copy.copy(bias_state))
     else:
         # Standard track update if no bias applied i.e. unbiased sensors
         hypotheses = data_associator.associate(tracks, detections, time)

stonesoup/feeder/bias.py

@@ -5,14 +5,15 @@
 from ..base import Property
 from ..buffered_generator import BufferedGenerator
 from ..types.state import GaussianState
+from ..types.track import Track
 
 
 class _BiasFeeder(DetectionFeeder):
-    bias_state: GaussianState = Property(doc="Bias State")
+    bias_track: Track[GaussianState] = Property(doc="Track of bias states")
 
     @property
     def bias(self):
-        return self.bias_state.mean
+        return self.bias_track.state.mean
 
     @abstractmethod
     @BufferedGenerator.generator_method
@@ -25,13 +26,13 @@ class TimeBiasFeeder(_BiasFeeder):
 
     Apply bias to detection timestamp and overall timestamp yielded.
     """
-    bias_state: GaussianState = Property(
-        doc="Bias state with state vector shape (1, 1) in units of seconds")
+    bias_track: Track[GaussianState] = Property(
+        doc="Track of bias states  with state vector shape (1, 1) in units of seconds")
 
     @BufferedGenerator.generator_method
     def data_gen(self):
         for time, detections in self.reader:
-            bias = self.bias_state.state_vector[0, 0]
+            bias = self.bias_track.state_vector[0, 0]
             bias_delta = datetime.timedelta(seconds=float(bias))
             time -= bias_delta
             models = set()
@@ -48,13 +49,13 @@ class OrientationBiasFeeder(_BiasFeeder):
 
     Apply bias to detection measurement model rotation offset
     """
-    bias_state: GaussianState = Property(
-        doc="Bias state with state vector shape (3, 1) is expected")
+    bias_track: Track[GaussianState] = Property(
+        doc="Track of bias states  with state vector shape (3, 1) is expected")
 
     @BufferedGenerator.generator_method
     def data_gen(self):
         for time, detections in self.reader:
-            bias = self.bias_state.state_vector.copy()
+            bias = self.bias_track.state_vector
             models = set()
             for detection in detections:
                 models.add(detection.measurement_model)
@@ -69,13 +70,14 @@ class TranslationBiasFeeder(_BiasFeeder):
 
     Apply bias to detection measurement model translation offset
     """
-    bias_state: GaussianState = Property(
-        doc="Bias state with state vector shape (n, 1), where n is dimensions of the model")
+    bias_track: Track[GaussianState] = Property(
+        doc="Track of bias states with state vector shape (n, 1), "
+            "where n is dimensions of the model")
 
     @BufferedGenerator.generator_method
     def data_gen(self):
         for time, detections in self.reader:
-            bias = self.bias_state.state_vector.copy()
+            bias = self.bias_track.state_vector
             models = set()
             for detection in detections:
                 models.add(detection.measurement_model)
@@ -90,14 +92,14 @@ class OrientationTranslationBiasFeeder(_BiasFeeder):
 
     Apply bias to detection measurement model rotation and translation offset
     """
-    bias_state: GaussianState = Property(
-        doc="Bias state with state vector shape (3+n, 1), 3 for rotation and where n is "
+    bias_track: Track[GaussianState] = Property(
+        doc="Track of bias states with state vector shape (3+n, 1), 3 for rotation and where n is "
             "dimensions of the model")
 
     @BufferedGenerator.generator_method
     def data_gen(self):
         for time, detections in self.reader:
-            bias = self.bias_state.state_vector.copy()
+            bias = self.bias_track.state_vector
             models = set()
             for detection in detections:
                 models.add(detection.measurement_model)

stonesoup/feeder/tests/test_bias.py

@@ -9,6 +9,7 @@
 from stonesoup.functions import build_rotation_matrix
 from stonesoup.types.detection import Detection
 from stonesoup.types.state import GaussianState, StateVector
+from stonesoup.types.track import Track
 
 
 def make_dummy_detection(state_vector, measurement_model):
@@ -49,7 +50,7 @@ def test_translation_gaussian_bias_feeder_iter():
     detection = make_dummy_detection(StateVector([[10.], [20.], [30.]]), measurement_model)
     feeder = TranslationBiasFeeder(
         reader=[(datetime.datetime(2025, 9, 10), [detection])],
-        bias_state=bias_state,
+        bias_track=Track([bias_state]),
     )
     # Iterate over feeder
     for time, detections in feeder:
@@ -69,7 +70,7 @@ def test_orientation_gaussian_bias_feeder_iter():
     detection = make_dummy_detection(StateVector([[10.], [20.], [30.]]), measurement_model)
     feeder = OrientationBiasFeeder(
         reader=[(datetime.datetime(2025, 9, 10), [detection])],
-        bias_state=bias_state,
+        bias_track=Track([bias_state]),
     )
     for time, detections in feeder:
         for det in detections:
@@ -92,7 +93,7 @@ def test_orientation_translation_gaussian_bias_feeder_iter():
     detection = make_dummy_detection(StateVector([[10.], [20.], [30.]]), measurement_model)
     feeder = OrientationTranslationBiasFeeder(
         reader=[(datetime.datetime(2025, 9, 10), [detection])],
-        bias_state=bias_state,
+        bias_track=Track([bias_state]),
     )
     for time, detections in feeder:
         for det in detections:
@@ -117,7 +118,7 @@ def test_time_gaussian_bias_feeder_iter():
     orig_timestamp = detection.timestamp
     feeder = TimeBiasFeeder(
         reader=[(datetime.datetime(2025, 9, 10), [detection])],
-        bias_state=bias_state,
+        bias_track=Track([bias_state]),
     )
     for time, detections in feeder:
         for det in detections:

stonesoup/updater/bias.py

@@ -11,6 +11,7 @@
 from ..types.detection import Detection
 from ..types.hypothesis import SingleHypothesis
 from ..types.state import GaussianState
+from ..types.track import Track
 from ..types.update import Update
 from ..updater import Updater
 from ..updater.kalman import UnscentedKalmanUpdater
@@ -28,7 +29,7 @@ class GaussianBiasUpdater(Updater):
     bias i.e. all measurements from the same sensor.
     """
     measurement_model = None
-    bias_state: GaussianState = Property(doc="Prior bias")
+    bias_track: Track[GaussianState] = Property(doc="Prior bias")
     bias_predictor: KalmanPredictor = Property(doc="Predictor for bias")
     bias_model_wrapper: BiasModelWrapper = Property()
     updater: Updater = Property(
@@ -44,15 +45,15 @@ def __init__(self, *args, **kwargs):
 
     def predict_measurement(
             self, predicted_state, measurement_model=None, measurement_noise=True, **kwargs):
-        ndim_bias = self.bias_state.ndim
-
+        bias_state = self.bias_track.state
+        ndim_bias = bias_state.ndim
         # Predict bias
-        if self.bias_state.timestamp is None:
-            pred_bias_state = copy.copy(self.bias_state)
+        if bias_state.timestamp is None:
+            pred_bias_state = copy.copy(bias_state)
             pred_bias_state.timestamp = predicted_state.timestamp
         else:
             pred_bias_state = self.bias_predictor.predict(
-                self.bias_state, timestamp=predicted_state.timestamp)
+                bias_state, timestamp=predicted_state.timestamp)
 
         applied_bias = getattr(measurement_model, 'applied_bias', np.zeros((ndim_bias, 1)))
         delta_bias = pred_bias_state.state_vector - applied_bias
@@ -75,18 +76,15 @@ def predict_measurement(
     def update(self, hypotheses, **kwargs):
         if any(not hyp for hyp in hypotheses):
             raise ValueError("Must provide only non-missed detection hypotheses")
-
-        ndim_bias = self.bias_state.ndim
+        bias_state = self.bias_track.state
+        ndim_bias = bias_state.ndim
 
         # Predict bias
         pred_time = max(hypothesis.prediction.timestamp for hypothesis in hypotheses)
-        if self.bias_state.timestamp is None:
-            self.bias_state.timestamp = pred_time
+        if bias_state.timestamp is None:
+            bias_state.timestamp = pred_time
         else:
-            new_bias_state = self.bias_predictor.predict(self.bias_state, timestamp=pred_time)
-            self.bias_state.state_vector = new_bias_state.state_vector
-            self.bias_state.covar = new_bias_state.covar
-            self.bias_state.timestamp = new_bias_state.timestamp
+            bias_state = self.bias_predictor.predict(bias_state, timestamp=pred_time)
 
         # Create joint state
         states = [hypothesis.prediction for hypothesis in hypotheses]
@@ -95,8 +93,8 @@ def update(self, hypotheses, **kwargs):
              for h in hypotheses
              if hasattr(h.measurement.measurement_model, 'applied_bias')),
             np.zeros((ndim_bias, 1)))
-        delta_bias = self.bias_state.state_vector - applied_bias
-        states.append(GaussianState(delta_bias, self.bias_state.covar))
+        delta_bias = bias_state.state_vector - applied_bias
+        states.append(GaussianState(delta_bias, bias_state.covar))
         combined_pred = GaussianState(
             np.vstack([state.state_vector for state in states]).view(StateVector),
             block_diag(*[state.covar for state in states]).view(CovarianceMatrix),
@@ -123,12 +121,13 @@ def update(self, hypotheses, **kwargs):
         # Update bias
         update = self.updater.update(SingleHypothesis(combined_pred, combined_meas), **kwargs)
         rel_delta_bias = update.state_vector[-ndim_bias:, :] - delta_bias
-        self.bias_state.state_vector = self.bias_state.state_vector + rel_delta_bias
+        bias_state.state_vector = bias_state.state_vector + rel_delta_bias
         if self.max_bias is not None:
-            self.bias_state.state_vector = \
-                np.min([abs(self.bias_state.state_vector), self.max_bias], axis=0) \
-                * np.sign(self.bias_state.state_vector)
-        self.bias_state.covar = update.covar[-ndim_bias:, -ndim_bias:]
+            bias_state.state_vector = \
+                np.min([abs(bias_state.state_vector), self.max_bias], axis=0) \
+                * np.sign(bias_state.state_vector)
+        bias_state.covar = update.covar[-ndim_bias:, -ndim_bias:]
+        self.bias_track.append(bias_state)
 
         # Create update states
         offset = 0

stonesoup/updater/tests/test_bias.py

@@ -15,6 +15,7 @@
 from ...types.detection import Detection
 from ...types.hypothesis import SingleHypothesis
 from ...types.state import GaussianState, StateVector
+from ...types.track import Track
 
 
 def make_dummy_measurement_model():
@@ -58,7 +59,7 @@ def test_translation_gaussian_bias_feeder_update_bias(bias_timestamp):
     bias_predictor = KalmanPredictor(
         CombinedLinearGaussianTransitionModel([RandomWalk(1)] * 3))
     updater = GaussianBiasUpdater(
-        bias_state=copy.copy(bias_prior),
+        bias_track=Track([copy.copy(bias_prior)]),
         bias_predictor=bias_predictor,
         bias_model_wrapper=TranslationBiasModelWrapper,
     )
@@ -85,9 +86,9 @@ def test_translation_gaussian_bias_feeder_update_bias(bias_timestamp):
     # Call update_bias
     updates = updater.update([hyp])
     # The bias_state should be updated (should not be the same as initial)...
-    assert not np.allclose(updater.bias_state.state_vector, bias_prior.state_vector)
+    assert not np.allclose(updater.bias_track.state_vector, bias_prior.state_vector)
     # and should be around 0.8 ± 0.1
-    assert np.allclose(updater.bias_state.state_vector, [[0.8], [0.8], [0.8]], atol=0.1)
+    assert np.allclose(updater.bias_track.state_vector, [[0.8], [0.8], [0.8]], atol=0.1)
     # The returned updates should match the updated state shape
     assert updates[0].state_vector.shape == pred.state_vector.shape
 
@@ -98,7 +99,7 @@ def test_orientation_gaussian_bias_feeder_update_bias(bias_timestamp):
     bias_predictor = KalmanPredictor(
         CombinedLinearGaussianTransitionModel([RandomWalk(1e-6)] * 3))
     updater = GaussianBiasUpdater(
-        bias_state=copy.copy(bias_prior),
+        bias_track=Track([copy.copy(bias_prior)]),
         bias_predictor=bias_predictor,
         bias_model_wrapper=OrientationBiasModelWrapper,
         max_bias=StateVector([[0.], [0.], [np.pi]])
@@ -123,8 +124,8 @@ def test_orientation_gaussian_bias_feeder_update_bias(bias_timestamp):
 
     hyp = SingleHypothesis(pred, meas)
     updates = updater.update([hyp])
-    assert not np.allclose(updater.bias_state.state_vector, bias_prior.state_vector)
-    assert np.allclose(updater.bias_state.state_vector, [[0.], [0.], [np.pi/16]], atol=0.05)
+    assert not np.allclose(updater.bias_track.state_vector, bias_prior.state_vector)
+    assert np.allclose(updater.bias_track.state_vector, [[0.], [0.], [np.pi/16]], atol=0.05)
     assert updates[0].state_vector.shape == pred.state_vector.shape
 
 
@@ -136,7 +137,7 @@ def test_orientation_translation_gaussian_bias_feeder_update_bias(bias_timestamp
     bias_predictor = KalmanPredictor(
         CombinedLinearGaussianTransitionModel([RandomWalk(1e-6)]*3 + [RandomWalk(1)]*3))
     updater = GaussianBiasUpdater(
-        bias_state=copy.copy(bias_prior),
+        bias_track=Track([copy.copy(bias_prior)]),
         bias_predictor=bias_predictor,
         bias_model_wrapper=OrientationTranslationBiasModelWrapper
     )
@@ -158,9 +159,9 @@ def test_orientation_translation_gaussian_bias_feeder_update_bias(bias_timestamp
         hyp = SingleHypothesis(pred, meas)
         hyps.append(hyp)
     updates = updater.update(hyps)
-    assert not np.allclose(updater.bias_state.state_vector, bias_prior.state_vector)
+    assert not np.allclose(updater.bias_track.state_vector, bias_prior.state_vector)
     assert np.allclose(
-        updater.bias_state.state_vector,
+        updater.bias_track.state_vector,
         [[0.], [0.], [np.pi/16], [1.], [1.], [1.]],
         atol=0.1)
     assert updates[0].state_vector.shape == pred.state_vector.shape
@@ -171,7 +172,7 @@ def test_time_gaussian_bias_feeder_update_bias(bias_timestamp):
     bias_predictor = KalmanPredictor(
         CombinedLinearGaussianTransitionModel([RandomWalk(1e-3)]))
     updater = GaussianBiasUpdater(
-        bias_state=copy.copy(bias_prior),
+        bias_track=Track([copy.copy(bias_prior)]),
         bias_predictor=bias_predictor,
         bias_model_wrapper=partial(
             TimeBiasModelWrapper, transition_model=make_dummy_transition_model())
@@ -195,6 +196,6 @@ def test_time_gaussian_bias_feeder_update_bias(bias_timestamp):
 
     hyp = SingleHypothesis(pred, meas)
     updates = updater.update([hyp])
-    assert not np.allclose(updater.bias_state.state_vector, bias_prior.state_vector)
-    assert np.allclose(updater.bias_state.state_vector, [[1.0]], atol=0.1)
+    assert not np.allclose(updater.bias_track.state_vector, bias_prior.state_vector)
+    assert np.allclose(updater.bias_track.state_vector, [[1.0]], atol=0.1)
     assert updates[0].state_vector.shape == pred.state_vector.shape