Add sensor bias estimation components and multi-sensor fusion bias example #1217

sdhiscocks · 2025-09-11T16:26:45Z

This has components which can be used to estimate sensor bias in time, translation and/or orientation by using model wrappers, and feeders.

codecov · 2025-09-12T07:38:26Z

Codecov Report

❌ Patch coverage is 99.11504% with 2 lines in your changes missing coverage. Please review.
✅ Project coverage is 94.13%. Comparing base (c65bd69) to head (96d5420).
⚠️ Report is 245 commits behind head on main.

Files with missing lines	Patch %	Lines
stonesoup/updater/bias.py	97.22%	1 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #1217      +/-   ##
==========================================
+ Coverage   94.05%   94.13%   +0.07%     
==========================================
  Files         227      230       +3     
  Lines       15754    15979     +225     
  Branches     2175     2203      +28     
==========================================
+ Hits        14818    15042     +224     
  Misses        652      652              
- Partials      284      285       +1

Flag	Coverage Δ
integration	`66.70% <66.81%> (+0.14%)`	⬆️
unittests	`91.55% <98.67%> (+0.06%)`	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

This moves update logic from feeder to updater class which is more fitting to the wider design. This also now includes measurement prediction which factors in sensor bias, and such better handles data association, etc. This enables example now to include a prior bias making bias more visible and demonstrating this functionality.

csherman-dstl

All looks good just needs some docstrings

docs/examples/sensorfusion/senor_bias.py

stonesoup/updater/bias.py

csherman-dstl · 2025-10-31T14:08:37Z

stonesoup/feeder/bias.py

+class TimeBiasFeeder(_BiasFeeder):
+    """Time Bias Feeder
+
+    Apply bias to detection timestamp and overall timestamp yielded.


My understanding is that these feeders remove the bias from the detection, but the docstrings for all of them state that they add the bias

csherman-dstl · 2025-10-31T14:23:55Z

stonesoup/feeder/tests/test_bias.py

+            expected_rotated = build_rotation_matrix(-feeder.bias) \
+                @ (det.state_vector - det.measurement_model.translation_offset)


Suggested change

expected_rotated = build_rotation_matrix(-feeder.bias) \

@ (det.state_vector - det.measurement_model.translation_offset)

expected_rotated = build_rotation_matrix(-feeder.bias[:3]) \

@ (det.state_vector + feeder.bias[3:])

csherman-dstl · 2025-10-31T14:30:03Z

stonesoup/models/measurement/bias.py

+
+
+class BiasModelWrapper(MeasurementModel):
+    """Abstract wrapper that applies bias values to an existing MeasurementModel.


Similarly to comment on feeders, are these not taking in an already biased sensor and removing the bias?

csherman-dstl · 2025-10-31T14:57:21Z

stonesoup/updater/bias.py

+            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


This logic is presumably only required due to the fact that the bias_feeder and bias_updater both use the bias_state but don't get it from each other.
Does it make more sense for the bias_feeder to take the updater as a property instead of the state, or to use a bias_track (like in the example) to just add the updated bias to

hpritchett-dstl

Looks good to me. Minor changes in wording, especially for the updater, as it's a bit more involved than a typical updater.

hpritchett-dstl · 2025-11-04T11:17:21Z

stonesoup/updater/bias.py

+        # 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
+        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
+
+        # Create joint state
+        states = [hypothesis.prediction for hypothesis in hypotheses]
+        applied_bias = next(
+            (h.measurement.measurement_model.applied_bias
+             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))
+        combined_pred = GaussianState(
+            np.vstack([state.state_vector for state in states]).view(StateVector),
+            block_diag(*[state.covar for state in states]).view(CovarianceMatrix),
+            timestamp=pred_time,
+        )
+
+        # Create joint measurement
+        offset = 0
+        models = []
+        for prediction, measurement in (
+                (hypothesis.prediction, hypothesis.measurement) for hypothesis in hypotheses):
+            models.append(self.bias_model_wrapper(
+                ndim_state=combined_pred.state_vector.shape[0],
+                measurement_model=measurement.measurement_model,
+                state_mapping=[offset + n for n in range(prediction.ndim)],
+                bias_mapping=list(range(-ndim_bias, 0))
+            ))
+            offset += prediction.ndim
+        combined_meas = Detection(
+            np.vstack([hypothesis.measurement.state_vector for hypothesis in hypotheses]),
+            timestamp=pred_time,
+            measurement_model=CombinedReversibleGaussianMeasurementModel(models))
+
+        # 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
+        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:]
+


Suggest using an intermediate predicted_bias_state variable to help with clarity when reading the code. something like:

Suggested change

# 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

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

# Create joint state

states = [hypothesis.prediction for hypothesis in hypotheses]

applied_bias = next(

(h.measurement.measurement_model.applied_bias

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))

combined_pred = GaussianState(

np.vstack([state.state_vector for state in states]).view(StateVector),

block_diag(*[state.covar for state in states]).view(CovarianceMatrix),

timestamp=pred_time,

)

# Create joint measurement

offset = 0

models = []

for prediction, measurement in (

(hypothesis.prediction, hypothesis.measurement) for hypothesis in hypotheses):

models.append(self.bias_model_wrapper(

ndim_state=combined_pred.state_vector.shape[0],

measurement_model=measurement.measurement_model,

state_mapping=[offset + n for n in range(prediction.ndim)],

bias_mapping=list(range(-ndim_bias, 0))

))

offset += prediction.ndim

combined_meas = Detection(

np.vstack([hypothesis.measurement.state_vector for hypothesis in hypotheses]),

timestamp=pred_time,

measurement_model=CombinedReversibleGaussianMeasurementModel(models))

# 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

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:]

# Predict bias

prediction_time = max(hypothesis.prediction.timestamp for hypothesis in hypotheses)

if self.bias_state.timestamp is not None:

predicted_bias_state = self.bias_predictor.predict(self.bias_state, timestamp=prediction_time)

else:

predicted_bias_state = self.bias_state

predicted_bias_state.timestamp = prediction_time

# Create joint state

states = [hypothesis.prediction for hypothesis in hypotheses]

applied_bias = next(

(h.measurement.measurement_model.applied_bias

for h in hypotheses

if hasattr(h.measurement.measurement_model, 'applied_bias')),

np.zeros((ndim_bias, 1)))

delta_bias = predicted_bias_state.state_vector - applied_bias

states.append(GaussianState(delta_bias, predicted_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),

timestamp=prediction_time,

)

# Create joint measurement

offset = 0

models = []

for prediction, measurement in (

(hypothesis.prediction, hypothesis.measurement) for hypothesis in hypotheses):

models.append(self.bias_model_wrapper(

ndim_state=combined_pred.state_vector.shape[0],

measurement_model=measurement.measurement_model,

state_mapping=[offset + n for n in range(prediction.ndim)],

bias_mapping=list(range(-ndim_bias, 0))

))

offset += prediction.ndim

combined_meas = Detection(

np.vstack([hypothesis.measurement.state_vector for hypothesis in hypotheses]),

timestamp=prediction_time,

measurement_model=CombinedReversibleGaussianMeasurementModel(models))

# 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 = predicted_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:]

self.bias_state.timestamp = predicted_bias_state.timestamp

can you confirm if this is accounted for in c09ac5a ?

mostly, i think it's still worth labelling self.bias_state as "prior_bias_state" on line 79, which then gets changed to predicted_bias_state after the prediction step on line 87, then we created an updated state which is appended to the track.

hpritchett-dstl · 2025-11-04T11:18:07Z

stonesoup/updater/bias.py

+
+        # Update bias
+        update = self.updater.update(SingleHypothesis(combined_pred, combined_meas), **kwargs)
+        rel_delta_bias = update.state_vector[-ndim_bias:, :] - delta_bias


What does rel stand for?

hpritchett-dstl · 2025-11-04T11:26:54Z

stonesoup/updater/bias.py

+    bias i.e. all measurements from the same sensor.
+    """
+    measurement_model = None
+    bias_state: GaussianState = Property(doc="Prior bias")


I feel like there should be a bit more explicit documentation somewhere (possibly not in this exact line) expanding on that this object is manipulated within this class, and is not returned, and so you must point to the same object utilized in your feeder

hpritchett-dstl · 2025-11-04T11:30:32Z

stonesoup/models/measurement/bias.py

+            delta_orient = state_vector[self.bias_mapping[:3], :]
+            delta_trans = state_vector[self.bias_mapping[3:], :]
+            bias_model = copy.copy(self.measurement_model)
+            bias_model.rotation_offset = bias_model.rotation_offset - delta_orient
+            bias_model.translation_offset = bias_model.translation_offset - delta_trans
+            state_vectors.append(bias_model.function(
+                State(state_vector[self.state_mapping, :]), noise=noise, **kwargs))


Suggested change

delta_orient = state_vector[self.bias_mapping[:3], :]

delta_trans = state_vector[self.bias_mapping[3:], :]

bias_model = copy.copy(self.measurement_model)

bias_model.rotation_offset = bias_model.rotation_offset - delta_orient

bias_model.translation_offset = bias_model.translation_offset - delta_trans

state_vectors.append(bias_model.function(

State(state_vector[self.state_mapping, :]), noise=noise, **kwargs))

delta_orientation = state_vector[self.bias_mapping[:3], :]

delta_translation = state_vector[self.bias_mapping[3:], :]

bias_model = copy.copy(self.measurement_model)

bias_model.rotation_offset = bias_model.rotation_offset - delta_orientation

bias_model.translation_offset = bias_model.translation_offset - delta_translation

state_vectors.append(bias_model.function(

State(state_vector[self.state_mapping, :]), noise=noise, **kwargs))

hpritchett-dstl · 2025-11-04T11:31:08Z

stonesoup/models/measurement/bias.py

+            delta_trans = state_vector[self.bias_mapping, :]
+            bias_model = copy.copy(self.measurement_model)
+            bias_model.translation_offset = bias_model.translation_offset - delta_trans
+            state_vectors.append(bias_model.function(
+                State(state_vector[self.state_mapping, :]), noise=noise, **kwargs))
+        if len(state_vectors) == 1:
+            return state_vectors[0]
+        else:
+            return StateVectors(state_vectors)


Suggested change

delta_trans = state_vector[self.bias_mapping, :]

bias_model = copy.copy(self.measurement_model)

bias_model.translation_offset = bias_model.translation_offset - delta_trans

state_vectors.append(bias_model.function(

State(state_vector[self.state_mapping, :]), noise=noise, **kwargs))

if len(state_vectors) == 1:

return state_vectors[0]

else:

return StateVectors(state_vectors)

delta_translation = state_vector[self.bias_mapping, :]

bias_model = copy.copy(self.measurement_model)

bias_model.translation_offset = bias_model.translation_offset - delta_translation

state_vectors.append(bias_model.function(

State(state_vector[self.state_mapping, :]), noise=noise, **kwargs))

if len(state_vectors) == 1:

return state_vectors[0]

else:

return StateVectors(state_vectors)

hpritchett-dstl · 2025-11-04T11:31:33Z

stonesoup/models/measurement/bias.py

+            delta_orient = state_vector[self.bias_mapping, :]
+            bias_model = copy.copy(self.measurement_model)
+            bias_model.rotation_offset = bias_model.rotation_offset - delta_orient


Suggested change

delta_orient = state_vector[self.bias_mapping, :]

bias_model = copy.copy(self.measurement_model)

bias_model.rotation_offset = bias_model.rotation_offset - delta_orient

delta_orientation = state_vector[self.bias_mapping, :]

bias_model = copy.copy(self.measurement_model)

bias_model.rotation_offset = bias_model.rotation_offset - delta_orientation

hpritchett-dstl · 2025-11-04T11:32:08Z

stonesoup/models/measurement/bias.py

+            delta_t = state_vector[self.bias_mapping[0], 0]
+            predicted_state_vectors.append(self.transition_model.function(
+                State(state_vector[self.state_mapping, :]),
+                time_interval=datetime.timedelta(seconds=-delta_t),


Suggested change

delta_t = state_vector[self.bias_mapping[0], 0]

predicted_state_vectors.append(self.transition_model.function(

State(state_vector[self.state_mapping, :]),

time_interval=datetime.timedelta(seconds=-delta_t),

delta_time = state_vector[self.bias_mapping[0], 0]

predicted_state_vectors.append(self.transition_model.function(

State(state_vector[self.state_mapping, :]),

time_interval=datetime.timedelta(seconds=-delta_time),

hpritchett-dstl · 2025-11-04T11:33:47Z

docs/examples/sensorfusion/senor_bias.py

+# %%
+# This example demonstrates how to simulate and estimate a drifting bias in the position of a sensor platform.
+# Specifically, the platform at index 0 (and its sensor) will have a time-varying bias applied to its position.
+# We use Stone-Soup's bias wrappers and feeders to estimate this changing bias from sensor measurements.


Suggested change

# We use Stone-Soup's bias wrappers and feeders to estimate this changing bias from sensor measurements.

# We use Stone-Soup's bias wrappers, feeders, and updater to estimate this changing bias from sensor measurements.

hpritchett-dstl · 2025-11-04T13:36:43Z

docs/examples/sensorfusion/senor_bias.py

+
+# sphinx_gallery_thumbnail_number = 3
+
+plotter = Plotterly(dimension=1, axis_labels=['Bias', 'Time'])


Suggested change

plotter = Plotterly(dimension=1, axis_labels=['Bias', 'Time'])

plotter = Plotterly(dimension=1, axis_labels=['Time', 'Bias'])

gawebb-dstl · 2025-11-10T15:55:56Z

stonesoup/plotter.py

                    text=[self._format_state_text(state) for state in track],
                    **scatter_kwargs)

+                if uncertainty:


Nice work on implementing the uncertainty in 1D, it's definitely a needed addition and looks good.

I just wanted to share an alternative approach I’ve used locally: plotting error bars. They give a slightly different visual cue for uncertainty.

Here’s a snippet in case you want to explore it:

if particle: raise NotImplementedError # Create copy of scatter kwargs track_scatter_kwargs = {**scatter_kwargs} if uncertainty: mapping_0 = mapping[0] err_y = [np.sqrt(state.covar[mapping_0, mapping_0]) for state in track.states] track_scatter_kwargs['error_y'] = dict( type='data', # value of error bar given in data coordinates array=err_y, visible=True ) self.fig.add_scatter( x=[state.timestamp for state in track], y=[float(getattr(state, 'mean', state.state_vector)[mapping[0]]) for state in track], text=[self._format_state_text(state) for state in track], **track_scatter_kwargs)

Your current implementation works well, but thought I’d offer it as another option as I'd already written the code but hadn't got around to a PR. Let me know what you think.

sdhiscocks added 6 commits September 3, 2025 14:07

Work in progress sensor bias model/feeder

424ab54

Correct rotation offset in OrientationTranslationGaussianBiasFeeder

e7bfd4e

Fix max bias in bias feeders

6368993

Add tests for bias estimation components

bfc0487

Add uncertainty to Plotterly 1D plotter

0042d6c

Add sensor fusion based bias estimation example

44c84d2

sdhiscocks requested a review from a team as a code owner September 11, 2025 16:26

sdhiscocks requested review from jswright-dstl and removed request for a team September 11, 2025 16:26

sdhiscocks added the enhancement label Sep 11, 2025

sdhiscocks requested a review from spike-dstl September 11, 2025 16:26

sdhiscocks added 2 commits September 11, 2025 17:34

Fix plotterly 1d test

6b880b0

Minor doc fixes for sensor bias

ab28c81

sdhiscocks added 2 commits October 22, 2025 13:48

Bias feeder handle case of no bias already applied

2e81e3c

sdhiscocks requested review from csherman-dstl and removed request for spike-dstl October 24, 2025 13:32

Add test for GaussianBiasUpdater predict_measurement

015b3f0

csherman-dstl reviewed Oct 27, 2025

View reviewed changes

docs/examples/sensorfusion/senor_bias.py Outdated Show resolved Hide resolved

stonesoup/updater/bias.py Show resolved Hide resolved

Update sensor bias documentation

923e1be

csherman-dstl reviewed Oct 31, 2025

View reviewed changes

hpritchett-dstl reviewed Nov 4, 2025

View reviewed changes

Replace bias_state with a bias_track in BiasFeeder and BiasUpdaters

96d5420

csherman-dstl force-pushed the sensor_bias branch from c09ac5a to 96d5420 Compare November 4, 2025 14:43

sdhiscocks added 2 commits November 6, 2025 11:18

Fix docs on bias feeder and models; and example

9629d2d

Fixes for bias tests and variable names

271f03d

gawebb-dstl reviewed Nov 10, 2025

View reviewed changes

		expected_rotated = build_rotation_matrix(-feeder.bias) \
		@ (det.state_vector - det.measurement_model.translation_offset)



		class BiasModelWrapper(MeasurementModel):
		"""Abstract wrapper that applies bias values to an existing MeasurementModel.

	# We use Stone-Soup's bias wrappers and feeders to estimate this changing bias from sensor measurements.
	# We use Stone-Soup's bias wrappers, feeders, and updater to estimate this changing bias from sensor measurements.


		# sphinx_gallery_thumbnail_number = 3

		plotter = Plotterly(dimension=1, axis_labels=['Bias', 'Time'])

Add sensor bias estimation components and multi-sensor fusion bias example #1217

Are you sure you want to change the base?

Add sensor bias estimation components and multi-sensor fusion bias example #1217

Uh oh!

Conversation

sdhiscocks commented Sep 11, 2025

Uh oh!

codecov bot commented Sep 12, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

csherman-dstl left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

hpritchett-dstl left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

gawebb-dstl Nov 10, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

codecov bot commented Sep 12, 2025 •

edited

Loading

gawebb-dstl Nov 10, 2025 •

edited

Loading