def predict(self, prior, timestamp=0, **kwargs):
delta_t = timestamp - prior.timestamp
# Transition model parameters
transition_matrix = self.transition_model.matrix(timedelta=delta_t,
**kwargs)
transition_noise_covar = self.transition_model.covar(timedelta=delta_t,
**kwargs)
# Perform prediction
prediction_mean = transition_matrix @ prior.mean
prediction_covar = transition_matrix @ prior.covar @ transition_matrix + transition_noise_covar
return GaussianStatePrediction(prediction_mean, prediction_covar)
def test_kalman_smoother(SmootherClass):
# First create a track from some detections and then smooth - check the output.
# Setup list of Detections
start = datetime.now()
times = [start + timedelta(seconds=i) for i in range(0, 5)]
measurements = [
np.array([[2.486559674128609]]),
np.array([[2.424165626519697]]),
np.array([[6.603176662762473]]),
np.array([[9.329099124074590]]),
np.array([[14.637975326666801]]),
]
detections = [
Detection(m, timestamp=timest)
for m, timest in zip(measurements, times)
]
# Setup models.
trans_model = ConstantVelocity(noise_diff_coeff=1)
meas_model = LinearGaussian(ndim_state=2,
mapping=[0],
noise_covar=np.array([[0.4]]))
# Tracking components
predictor = KalmanPredictor(transition_model=trans_model)
updater = KalmanUpdater(measurement_model=meas_model)
# Prior
cstate = GaussianState(np.ones([2, 1]), np.eye(2), timestamp=start)
track = Track()
for detection in detections:
# Predict
pred = predictor.predict(cstate, timestamp=detection.timestamp)
# form hypothesis
hypothesis = SingleHypothesis(pred, detection)
# Update
cstate = updater.update(hypothesis)
# write to track
track.append(cstate)
smoother = SmootherClass(transition_model=trans_model)
smoothed_track = smoother.smooth(track)
smoothed_state_vectors = [state.state_vector for state in smoothed_track]
# Verify Values
target_smoothed_vectors = [
np.array([[1.688813974839928], [1.267196351952188]]),
np.array([[3.307200214998506], [2.187167840595264]]),
np.array([[6.130402001958210], [3.308896367021604]]),
np.array([[9.821303658438408], [4.119557021638030]]),
np.array([[14.257730973981149], [4.594862462495096]])
]
assert np.allclose(smoothed_state_vectors, target_smoothed_vectors)
# Check that a prediction is smoothable and that no error chucked
# Also remove the transition model and use the one provided by the smoother
track[1] = GaussianStatePrediction(pred.state_vector,
pred.covar,
timestamp=pred.timestamp)
smoothed_track2 = smoother.smooth(track)
assert isinstance(smoothed_track2[1], GaussianStatePrediction)
# Check appropriate error chucked if not GaussianStatePrediction/Update
track[-1] = detections[-1]
with pytest.raises(TypeError):
smoother._prediction(track[-1])
def test_sqrt_kalman():
measurement_model = LinearGaussian(ndim_state=2,
mapping=[0],
noise_covar=np.array([[0.04]]))
prediction = GaussianStatePrediction(
np.array([[-6.45], [0.7]]),
np.array([[4.1123, 0.0013], [0.0013, 0.0365]]))
sqrt_prediction = SqrtGaussianState(prediction.state_vector,
np.linalg.cholesky(prediction.covar))
measurement = Detection(np.array([[-6.23]]))
# Calculate evaluation variables
eval_measurement_prediction = GaussianMeasurementPrediction(
measurement_model.matrix() @ prediction.mean,
measurement_model.matrix() @ prediction.covar
@ measurement_model.matrix().T + measurement_model.covar(),
cross_covar=prediction.covar @ measurement_model.matrix().T)
kalman_gain = eval_measurement_prediction.cross_covar @ np.linalg.inv(
eval_measurement_prediction.covar)
eval_posterior = GaussianState(
prediction.mean + kalman_gain
@ (measurement.state_vector - eval_measurement_prediction.mean),
prediction.covar -
kalman_gain @ eval_measurement_prediction.covar @ kalman_gain.T)
# Test Square root form returns the same as standard form
updater = KalmanUpdater(measurement_model=measurement_model)
sqrt_updater = SqrtKalmanUpdater(measurement_model=measurement_model,
qr_method=False)
qr_updater = SqrtKalmanUpdater(measurement_model=measurement_model,
qr_method=True)
posterior = updater.update(
SingleHypothesis(prediction=prediction, measurement=measurement))
posterior_s = sqrt_updater.update(
SingleHypothesis(prediction=sqrt_prediction, measurement=measurement))
posterior_q = qr_updater.update(
SingleHypothesis(prediction=sqrt_prediction, measurement=measurement))
assert np.allclose(posterior_s.mean, eval_posterior.mean, 0, atol=1.e-14)
assert np.allclose(posterior_q.mean, eval_posterior.mean, 0, atol=1.e-14)
assert np.allclose(posterior.covar, eval_posterior.covar, 0, atol=1.e-14)
assert np.allclose(eval_posterior.covar,
posterior_s.sqrt_covar @ posterior_s.sqrt_covar.T,
0,
atol=1.e-14)
assert np.allclose(posterior.covar,
posterior_s.sqrt_covar @ posterior_s.sqrt_covar.T,
0,
atol=1.e-14)
assert np.allclose(posterior.covar,
posterior_q.sqrt_covar @ posterior_q.sqrt_covar.T,
0,
atol=1.e-14)
# I'm not sure this is going to be true in all cases. Keep in order to find edge cases
assert np.allclose(posterior_s.covar, posterior_q.covar, 0, atol=1.e-14)
# Next create a prediction with a covariance that will cause problems
prediction = GaussianStatePrediction(
np.array([[-6.45], [0.7]]), np.array([[1e24, 1e-24], [1e-24, 1e24]]))
sqrt_prediction = SqrtGaussianState(prediction.state_vector,
np.linalg.cholesky(prediction.covar))
posterior = updater.update(
SingleHypothesis(prediction=prediction, measurement=measurement))
posterior_s = sqrt_updater.update(
SingleHypothesis(prediction=sqrt_prediction, measurement=measurement))
posterior_q = qr_updater.update(
SingleHypothesis(prediction=sqrt_prediction, measurement=measurement))
# The new posterior will be
eval_posterior = GaussianState(
prediction.mean + kalman_gain
@ (measurement.state_vector - eval_measurement_prediction.mean),
np.array([[0.04, 0], [
0, 1e24
]])) # Accessed by looking through the Decimal() quantities...
# It's actually [0.039999999999 1e-48], [1e-24 1e24 + 1e-48]] ish
# Test that the square root form succeeds where the standard form fails
assert not np.allclose(posterior.covar, eval_posterior.covar, rtol=5.e-3)
assert np.allclose(posterior_s.sqrt_covar @ posterior_s.sqrt_covar.T,
eval_posterior.covar,
rtol=5.e-3)
assert np.allclose(posterior_q.sqrt_covar @ posterior_s.sqrt_covar.T,
eval_posterior.covar,
rtol=5.e-3)
GaussianMeasurementPrediction)
from stonesoup.types.state import GaussianState, SqrtGaussianState
from stonesoup.updater.kalman import (KalmanUpdater, ExtendedKalmanUpdater,
UnscentedKalmanUpdater,
SqrtKalmanUpdater, IteratedKalmanUpdater)
@pytest.mark.parametrize(
"UpdaterClass, measurement_model, prediction, measurement",
[
( # Standard Kalman
KalmanUpdater,
LinearGaussian(
ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])),
GaussianStatePrediction(
np.array([[-6.45], [0.7]]),
np.array([[4.1123, 0.0013], [0.0013, 0.0365]
])), Detection(np.array([[-6.23]]))),
( # Extended Kalman
ExtendedKalmanUpdater,
LinearGaussian(
ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])),
GaussianStatePrediction(
np.array([[-6.45], [0.7]]),
np.array([[4.1123, 0.0013], [0.0013, 0.0365]
])), Detection(np.array([[-6.23]]))),
( # Unscented Kalman
UnscentedKalmanUpdater,
LinearGaussian(
ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])),
GaussianStatePrediction(
np.array([[-6.45], [0.7]]),
plotter.plot_tracks(track, [0, 2], uncertainty=True, color='r')
plotter.fig
# %%
# The UT in slightly more depth
# -----------------------------
# Now try and get a sense of what actually happens to the uncertainty when a non-linear combination
# of functions happens. Instead of deriving this analytically (and potentially getting bogged-down
# in the maths), let's just use a sampling method.
# We can start with a prediction, which is Gauss-distributed in state space, that we will use to
# make our measurement predictions from.
from stonesoup.types.prediction import GaussianStatePrediction
prediction = GaussianStatePrediction(state_vector=[[0], [0], [20], [0]],
covar=np.diag([1.5, 0.5, 1.5, 0.5]),
timestamp=datetime.now())
# %%
# We'll recapitulate the fact that the sensor position is where it previously was. But this time
# we'll make the measurement much noisier.
sensor_x = 0
sensor_y = 0
measurement_model = CartesianToBearingRange(
ndim_state=4,
mapping=(0, 2),
noise_covar=np.diag([np.radians(5), 0.1
]), # bearing variance = 5 degrees (accurate range)
translation_offset=np.array([[sensor_x], [sensor_y]]))
def _smooth_traj(self, traj, process_noise_std=0.5, measurement_noise_std=1):
# Get detector
detector = self._get_detector(traj)
# Models
if not isinstance(process_noise_std, (list, tuple, np.ndarray)):
process_noise_std = [process_noise_std, process_noise_std]
if not isinstance(measurement_noise_std, (list, tuple, np.ndarray)):
measurement_noise_std = [measurement_noise_std, measurement_noise_std]
transition_model = CombinedLinearGaussianTransitionModel(
[
ConstantVelocity(process_noise_std[0] ** 2),
ConstantVelocity(process_noise_std[1] ** 2),
]
)
measurement_model = LinearGaussian(
ndim_state=4,
mapping=[0, 2],
noise_covar=np.diag(
[measurement_noise_std[0] ** 2, measurement_noise_std[1] ** 2]
),
)
# Predictor and updater
predictor = KalmanPredictor(transition_model)
updater = KalmanUpdater(measurement_model)
# Initiator
state_vector = StateVector([0.0, 0.0, 0.0, 0.0])
covar = CovarianceMatrix(np.diag([0.0, 0.0, 0.0, 0.0]))
prior_state = GaussianStatePrediction(state_vector, covar)
initiator = SimpleMeasurementInitiator(prior_state, measurement_model)
# Filtering
track = None
for i, (timestamp, detections) in enumerate(detector):
if i == 0:
tracks = initiator.initiate(detections, timestamp)
track = tracks.pop()
else:
detection = detections.pop()
prediction = predictor.predict(track.state, timestamp=timestamp)
hypothesis = SingleHypothesis(prediction, detection)
posterior = updater.update(hypothesis)
track.append(posterior)
# Smoothing
smoother = KalmanSmoother(transition_model)
smooth_track = smoother.smooth(track)
# Create new trajectory
if traj.is_latlon:
df = traj.df.to_crs("EPSG:3395")
df.geometry = [
Point(state.state_vector[0], state.state_vector[2])
for state in smooth_track
]
df.to_crs(traj.crs, inplace=True)
else:
df = traj.df.copy()
df.geometry = [
Point(state.state_vector[0], state.state_vector[2])
for state in smooth_track
]
new_traj = Trajectory(df, traj.id)
return new_traj