def __init__(self,
start_time,
prior: GaussianState,
sigma_process_radar=0.01,
sigma_process_ais=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1):
"""
:param measurements_radar:
:param measurements_ais:
:param start_time:
:param prior:
:param sigma_process_radar:
:param sigma_process_ais:
:param sigma_meas_radar:
:param sigma_meas_ais:
"""
self.start_time = start_time
# transition models (process models)
self.transition_model_radar = CombinedLinearGaussianTransitionModel([
ConstantVelocity(sigma_process_radar),
ConstantVelocity(sigma_process_radar)
])
self.transition_model_ais = CombinedLinearGaussianTransitionModel([
ConstantVelocity(sigma_process_ais),
ConstantVelocity(sigma_process_ais)
])
# Specify measurement model for radar
self.measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([
[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]
]))
# Specify measurement model for AIS
self.measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# specify predictors
self.predictor_radar = KalmanPredictor(self.transition_model_radar)
self.predictor_ais = KalmanPredictor(self.transition_model_ais)
# specify updaters
self.updater_radar = KalmanUpdater(self.measurement_model_radar)
self.updater_ais = KalmanUpdater(self.measurement_model_ais)
# create prior, both trackers use the same starting point
self.prior_radar = prior
self.prior_ais = prior
def __init__(self,
measurements_radar,
measurements_ais,
start_time,
prior: GaussianState,
sigma_process=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1):
"""
:param measurements_radar:
:param measurements_ais:
:param start_time:
:param prior:
:param sigma_process:
:param sigma_meas_radar:
:param sigma_meas_ais:
"""
# measurements and start time
self.measurements_radar = measurements_radar
self.measurements_ais = measurements_ais
self.start_time = start_time
# transition model
self.transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(sigma_process),
ConstantVelocity(sigma_process)])
# same measurement models as used when generating the measurements
# Specify measurement model for radar
self.measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([
[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]
]))
# Specify measurement model for AIS
self.measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# specify predictor
self.predictor = KalmanPredictor(self.transition_model)
# specify updaters
self.updater_radar = KalmanUpdater(self.measurement_model_radar)
self.updater_ais = KalmanUpdater(self.measurement_model_ais)
# create prior todo move later and probably rename
self.prior = prior
birth_rate=0.2,
death_probability=0.05,
seed=3
)
detection_sim = SimpleDetectionSimulator(
groundtruth=groundtruth_sim,
measurement_model=measurement_model,
meas_range=np.array([[-1, 1], [-1, 1]]) * 5000, # Area to generate clutter
detection_probability=0.9,
clutter_rate=1,
seed=4
)
# Filter
predictor = KalmanPredictor(transition_model)
updater = KalmanUpdater(measurement_model)
# Data Associator
hypothesiser = DistanceHypothesiser(predictor, updater, Mahalanobis(), missed_distance=3)
data_associator = GNNWith2DAssignment(hypothesiser)
# Initiator & Deleter
deleter = CovarianceBasedDeleter(covar_trace_thresh=1E3)
initiator = MultiMeasurementInitiator(
GaussianState(np.array([[0], [0], [0], [0]]), np.diag([0, 100, 0, 1000])),
measurement_model=measurement_model,
deleter=deleter,
data_associator=data_associator,
updater=updater,
min_points=3,
)
#
# :math:`\mathbf{z}_k - H_{k}\mathbf{x}_{k|k-1}` is known as the *innovation* and :math:`S_k` the
# *innovation covariance*; :math:`K_k` is the *Kalman gain*.
# %%
# Constructing a predictor and updater in Stone Soup is simple. In a nice division of
# responsibility, a :class:`~.Predictor` takes a :class:`~.TransitionModel` as input and
# an :class:`~.Updater` takes a :class:`~.MeasurementModel` as input. Note that for now we're using
# the same models used to generate the ground truth and the simulated measurements. This won't
# usually be possible and it's an interesting exercise to explore what happens when these
# parameters are mismatched.
from stonesoup.predictor.kalman import KalmanPredictor
predictor = KalmanPredictor(transition_model)
from stonesoup.updater.kalman import KalmanUpdater
updater = KalmanUpdater(measurement_model)
# %%
# Run the Kalman filter
# ^^^^^^^^^^^^^^^^^^^^^
# Now we have the components, we can execute the Kalman filter estimator on the simulated data.
#
# In order to start, we'll need to create the first prior estimate. We're going to use the
# :class:`~.GaussianState` we mentioned earlier. As the name suggests, this parameterises the state
# as :math:`\mathcal{N}(\mathbf{x}_0, P_0)`. By happy chance the initial values are chosen to match
# the truth quite well. You might want to manipulate these to see what happens.
from stonesoup.types.state import GaussianState
prior = GaussianState([[0], [1], [0], [1]],
np.diag([1.5, 0.5, 1.5, 0.5]),
timestamp=start_time)
class kalman_filter_ais_as_measurement:
"""
todo
"""
def __init__(self,
measurements_radar,
measurements_ais,
start_time,
prior: GaussianState,
sigma_process=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1):
"""
:param measurements_radar:
:param measurements_ais:
:param start_time:
:param prior:
:param sigma_process:
:param sigma_meas_radar:
:param sigma_meas_ais:
"""
# measurements and start time
self.measurements_radar = measurements_radar
self.measurements_ais = measurements_ais
self.start_time = start_time
# transition model
self.transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(sigma_process),
ConstantVelocity(sigma_process)])
# same measurement models as used when generating the measurements
# Specify measurement model for radar
self.measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([
[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]
]))
# Specify measurement model for AIS
self.measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# specify predictor
self.predictor = KalmanPredictor(self.transition_model)
# specify updaters
self.updater_radar = KalmanUpdater(self.measurement_model_radar)
self.updater_ais = KalmanUpdater(self.measurement_model_ais)
# create prior todo move later and probably rename
self.prior = prior
def track(self):
self.tracks_fused = Track()
self.tracks_radar = Track()
for measurement_idx in range(0, len(self.measurements_radar)):
# radar measurement every timestep, AIS measurement every second
# first predict, then update with radar measurement. Then every second iteration, perform an extra update step
# using the AIS measurement
measurement_radar = self.measurements_radar[measurement_idx]
prediction = self.predictor.predict(
self.prior, timestamp=measurement_radar.timestamp)
hypothesis = SingleHypothesis(prediction, measurement_radar)
post = self.updater_radar.update(hypothesis)
# save radar track
self.tracks_radar.append(post)
if measurement_idx % 2:
measurement_ais = self.measurements_ais[measurement_idx // 2]
hypothesis = SingleHypothesis(post, measurement_ais)
post = self.updater_ais.update(hypothesis)
# save fused track
self.tracks_fused.append(post)
self.prior = self.tracks_fused[-1]
return self.tracks_fused, self.tracks_radar
def plot(self, ground_truth):
"""
:return:
"""
# PLOT
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlabel("$x$")
ax.set_ylabel("$y$")
ax.axis('equal')
ax.plot([state.state_vector[0] for state in ground_truth],
[state.state_vector[2] for state in ground_truth],
linestyle="--",
label='Ground truth')
ax.scatter(
[state.state_vector[0] for state in self.measurements_radar],
[state.state_vector[1] for state in self.measurements_radar],
color='b',
label='Measurements Radar')
ax.scatter([state.state_vector[0] for state in self.measurements_ais],
[state.state_vector[1] for state in self.measurements_ais],
color='r',
label='Measurements AIS')
# add ellipses to the posteriors
for state in self.tracks_fused:
w, v = np.linalg.eig(
self.measurement_model_radar.matrix() @ state.covar
@ self.measurement_model_radar.matrix().T)
max_ind = np.argmax(w)
min_ind = np.argmin(w)
orient = np.arctan2(v[1, max_ind], v[0, max_ind])
ellipse = Ellipse(xy=(state.state_vector[0],
state.state_vector[2]),
width=2 * np.sqrt(w[max_ind]),
height=2 * np.sqrt(w[min_ind]),
angle=np.rad2deg(orient),
alpha=0.2,
color='r')
ax.add_artist(ellipse)
for state in self.tracks_radar:
w, v = np.linalg.eig(
self.measurement_model_radar.matrix() @ state.covar
@ self.measurement_model_radar.matrix().T)
max_ind = np.argmax(w)
min_ind = np.argmin(w)
orient = np.arctan2(v[1, max_ind], v[0, max_ind])
ellipse = Ellipse(xy=(state.state_vector[0],
state.state_vector[2]),
width=2 * np.sqrt(w[max_ind]),
height=2 * np.sqrt(w[min_ind]),
angle=np.rad2deg(orient),
alpha=0.2,
color='b')
ax.add_artist(ellipse)
# add ellipses to add legend todo do this less ugly
ellipse = Ellipse(xy=(0, 0),
width=0,
height=0,
color='r',
alpha=0.2,
label='Posterior Fused')
ax.add_patch(ellipse)
ellipse = Ellipse(xy=(0, 0),
width=0,
height=0,
color='b',
alpha=0.2,
label='Posterior Radar')
ax.add_patch(ellipse)
# todo move or remove
ax.legend()
ax.set_title(
"Kalman filter tracking and fusion when AIS is viewed as a measurement"
)
fig.show()
fig.savefig(
"../results/scenario1/KF_tracking_and_fusion_viewing_ais_as_measurement.svg"
)
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])
class kalman_filter_dependent_fusion:
"""
todo
"""
def __init__(self,
measurements_radar,
measurements_ais,
start_time,
prior: GaussianState,
sigma_process_radar=0.01,
sigma_process_ais=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1):
"""
:param measurements_radar:
:param measurements_ais:
:param start_time:
:param prior:
:param sigma_process_radar:
:param sigma_process_ais:
:param sigma_meas_radar:
:param sigma_meas_ais:
"""
self.start_time = start_time
self.measurements_radar = measurements_radar
self.measurements_ais = measurements_ais
# same transition models (radar uses same as original)
self.transition_model_radar = CombinedLinearGaussianTransitionModel([
ConstantVelocity(sigma_process_radar),
ConstantVelocity(sigma_process_radar)
])
self.transition_model_ais = CombinedLinearGaussianTransitionModel([
ConstantVelocity(sigma_process_ais),
ConstantVelocity(sigma_process_ais)
])
# same measurement models as used when generating the measurements
# Specify measurement model for radar
self.measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([
[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]
]))
# Specify measurement model for AIS
self.measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# specify predictors
self.predictor_radar = KalmanPredictor(self.transition_model_radar)
self.predictor_ais = KalmanPredictor(self.transition_model_ais)
# specify updaters
self.updater_radar = KalmanUpdater(self.measurement_model_radar)
self.updater_ais = KalmanUpdater(self.measurement_model_ais)
# create prior, both trackers use the same starting point
self.prior_radar = prior
self.prior_ais = prior
def track(self):
"""
todo
:return:
"""
# create list for storing kalman gains
kf_gains_radar = []
kf_gains_ais = []
# create list for storing transition_noise_covar
transition_covars_radar = []
transition_covars_ais = []
# create list for storing tranisition matrixes
transition_matrixes_radar = []
transition_matrixes_ais = []
# create list for storing tracks
tracks_radar = Track()
tracks_ais = Track()
# track
for measurement in self.measurements_radar:
prediction = self.predictor_radar.predict(
self.prior_radar, timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
# calculate the kalman gain
hypothesis.measurement_prediction = self.updater_radar.predict_measurement(
hypothesis.prediction,
measurement_model=self.measurement_model_radar)
post_cov, kalman_gain = self.updater_radar._posterior_covariance(
hypothesis)
kf_gains_radar.append(kalman_gain)
# get the transition model covar NOTE; same for AIS and radar. Name change not a bug
predict_over_interval = measurement.timestamp - self.prior_radar.timestamp
transition_covars_radar.append(
self.transition_model_radar.covar(
time_interval=predict_over_interval))
transition_matrixes_radar.append(
self.transition_model_radar.matrix(
time_interval=predict_over_interval))
# update
post = self.updater_radar.update(hypothesis)
tracks_radar.append(post)
self.prior_radar = post
for measurement in self.measurements_ais:
prediction = self.predictor_ais.predict(
self.prior_ais, timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
# calculate the kalman gain
hypothesis.measurement_prediction = self.updater_ais.predict_measurement(
hypothesis.prediction,
measurement_model=self.measurement_model_ais)
post_cov, kalman_gain = self.updater_ais._posterior_covariance(
hypothesis)
kf_gains_ais.append(kalman_gain)
# get the transition model covar
predict_over_interval = measurement.timestamp - self.prior_ais.timestamp
transition_covars_ais.append(
self.transition_model_ais.covar(
time_interval=predict_over_interval))
transition_matrixes_ais.append(
self.transition_model_ais.matrix(
time_interval=predict_over_interval))
# update
post = self.updater_ais.update(hypothesis)
tracks_ais.append(post)
self.prior_ais = post
# FOR NOW: run track_to_track_association here, todo change pipeline flow
# FOR NOW: run the association only when both have a new posterior (so each time the AIS has a posterior)
# todo handle fusion when one track predicts and the other updates. (or both predicts) (Can't be done with the theory
# described in the article)
cross_cov_ij = [np.zeros([4, 4])]
cross_cov_ji = [np.zeros([4, 4])]
# TODO change flow to assume that the indexes decide whether its from the same iterations
# use indexes to loop through tracks, kf_gains etc
tracks_fused = []
# tracks_fused.append(tracks_radar[0])
for i in range(1, len(tracks_radar)):
# we assume that the indexes correlates with the timestamps. I.e. that the lists are 'synchronized'
# check to make sure
if tracks_ais[i].timestamp == tracks_radar[i].timestamp:
# calculate the cross-covariance estimation error
cross_cov_ij.append(
calc_cross_cov_estimate_error(
self.measurement_model_radar.matrix(),
self.measurement_model_ais.matrix(), kf_gains_radar[i],
kf_gains_ais[i], transition_matrixes_radar[i],
transition_covars_ais[i], cross_cov_ij[i - 1]))
cross_cov_ji.append(
calc_cross_cov_estimate_error(
self.measurement_model_ais.matrix(),
self.measurement_model_radar.matrix(), kf_gains_ais[i],
kf_gains_radar[i], transition_matrixes_ais[i],
transition_covars_radar[i], cross_cov_ji[i - 1]))
# test for track association
# same_target = track_to_track_association.test_association_dependent_tracks(tracks_radar[i],
# tracks_ais[i],
# cross_cov_ij[i],
# cross_cov_ji[i], 0.01)
same_target = True # ignore test for track association for now
if same_target:
fused_posterior, fused_covar = track_to_track_fusion.fuse_dependent_tracks(
tracks_radar[i], tracks_ais[i], cross_cov_ij[i],
cross_cov_ji[i])
estimate = GaussianState(fused_posterior,
fused_covar,
timestamp=tracks_ais[i].timestamp)
tracks_fused.append(estimate)
return tracks_fused, tracks_ais, tracks_radar
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)
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
noise_covar=np.array([
[1, 0], # covariance matrix for Gaussian PDF
[0, 1]
]))
# Specify measurement model for AIS
measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array([[1, 0], [0, 1]]))
# specify predictors
predictor_radar = KalmanPredictor(transition_model_radar)
predictor_ais = KalmanPredictor(transition_model_ais)
# specify updaters
updater_radar = KalmanUpdater(measurement_model_radar)
updater_ais = KalmanUpdater(measurement_model_ais)
# create prior, both trackers use the same starting point
prior_radar = GaussianState([0, 1, 0, 1],
np.diag([1.5, 0.5, 1.5, 0.5]),
timestamp=start_time)
prior_ais = GaussianState([0, 1, 0, 1],
np.diag([1.5, 0.5, 1.5, 0.5]),
timestamp=start_time)
# create list for storing kalman gains
kf_gains_radar = []
kf_gains_ais = []
# create list for storing transition_noise_covar
class kalman_filter_independent_fusion:
"""
todo
"""
def __init__(self,
start_time,
prior: GaussianState,
sigma_process_radar=0.01,
sigma_process_ais=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1):
"""
:param start_time:
:param prior:
:param sigma_process_radar:
:param sigma_process_ais:
:param sigma_meas_radar:
:param sigma_meas_ais:
"""
self.start_time = start_time
# transition models (process models)
self.transition_model_radar = CombinedLinearGaussianTransitionModel([
ConstantVelocity(sigma_process_radar),
ConstantVelocity(sigma_process_radar)
])
self.transition_model_ais = CombinedLinearGaussianTransitionModel([
ConstantVelocity(sigma_process_ais),
ConstantVelocity(sigma_process_ais)
])
# Specify measurement model for radar
self.measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([
[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]
]))
# Specify measurement model for AIS
self.measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# specify predictors
self.predictor_radar = KalmanPredictor(self.transition_model_radar)
self.predictor_ais = KalmanPredictor(self.transition_model_ais)
# specify updaters
self.updater_radar = KalmanUpdater(self.measurement_model_radar)
self.updater_ais = KalmanUpdater(self.measurement_model_ais)
# create prior, both trackers use the same starting point
self.prior_radar = prior
self.prior_ais = prior
def track(self,
start_time,
measurements_radar,
measurements_ais,
fusion_rate=1):
"""
returns fused tracks.
"""
time = start_time
tracks_radar = Track()
tracks_ais = Track()
tracks_fused = []
measurements_radar = measurements_radar.copy()
measurements_ais = measurements_ais.copy()
# loop until there are no more measurements
while measurements_radar or measurements_ais:
# get all new measurements
new_measurements_radar = \
[measurement for measurement in measurements_radar if measurement.timestamp <= time]
new_measurements_ais = \
[measurement for measurement in measurements_ais if measurement.timestamp <= time]
# remove the new measurements from the measurements lists
for new_meas in new_measurements_ais:
measurements_ais.remove(new_meas)
for new_meas in new_measurements_radar:
measurements_radar.remove(new_meas)
# for each new_meas, perform a prediction and an update
for measurement in new_measurements_ais:
prediction = self.predictor_ais.predict(
self.prior_ais, timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
post = self.updater_ais.update(hypothesis)
tracks_ais.append(post)
self.prior_ais = tracks_ais[-1]
for measurement in new_measurements_radar:
prediction = self.predictor_radar.predict(
self.prior_radar, timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
post = self.updater_radar.update(hypothesis)
tracks_radar.append(post)
self.prior_radar = tracks_radar[-1]
# perform a prediction up until this time (the newest measurement might not be at this exact time)
# note that this "prediction" might be the updated posterior, if the newest measurement was at this time
prediction_radar = self.predictor_radar.predict(self.prior_radar,
timestamp=time)
prediction_ais = self.predictor_ais.predict(self.prior_ais,
timestamp=time)
# fuse these predictions.
tracks_fused.append(
self._fuse_track(prediction_radar, prediction_ais))
time += timedelta(seconds=fusion_rate)
return tracks_fused, tracks_radar, tracks_ais
def _fuse_track(self, track_radar, track_ais):
"""
fuses the two tracks. Assumes that the tracks have the same timestamp
"""
# todo check the track-to-track association
fused_posterior, fused_covar = track_to_track_fusion.fuse_independent_tracks(
track_radar, track_ais)
estimate = GaussianState(fused_posterior,
fused_covar,
timestamp=track_radar.timestamp)
return estimate
def _fuse_tracks(self, tracks_radar, tracks_ais, fusion_rate=1):
tracks_fused = []
for track_radar in tracks_radar:
# find a track in tracks_radar with the same timestamp
estimate = track_radar
for track_ais in tracks_ais:
if track_ais.timestamp == track_radar.timestamp:
# same_target = track_to_track_association.test_association_independent_tracks(track_radar, track_ais,
# 0.01)
same_target = True # ignore association for now
if same_target:
fused_posterior, fused_covar = track_to_track_fusion.fuse_independent_tracks(
track_radar, track_ais)
estimate = GaussianState(
fused_posterior,
fused_covar,
timestamp=track_radar.timestamp)
break
tracks_fused.append(estimate)
return tracks_fused
class kalman_filter_independent_fusion:
"""
todo
"""
def __init__(self,
measurements_radar,
measurements_ais,
start_time,
prior: GaussianState,
sigma_process_radar=0.01,
sigma_process_ais=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1):
# same transition models (radar uses same as original)
self.transition_model_radar = CombinedLinearGaussianTransitionModel([
ConstantVelocity(sigma_process_radar),
ConstantVelocity(sigma_process_radar)
])
self.transition_model_ais = CombinedLinearGaussianTransitionModel([
ConstantVelocity(sigma_process_ais),
ConstantVelocity(sigma_process_ais)
])
self.start_time = start_time
self.measurements_radar = measurements_radar
self.measurements_ais = measurements_ais
# Specify measurement model for radar
self.measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([
[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]
]))
# Specify measurement model for AIS
self.measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# specify predictors
self.predictor_radar = KalmanPredictor(self.transition_model_radar)
self.predictor_ais = KalmanPredictor(self.transition_model_ais)
# specify updaters
self.updater_radar = KalmanUpdater(self.measurement_model_radar)
self.updater_ais = KalmanUpdater(self.measurement_model_ais)
# create prior, both trackers use the same starting point
self.prior_radar = prior
self.prior_ais = prior
def track(self):
self.tracks_radar = Track()
for measurement in self.measurements_radar:
prediction = self.predictor_radar.predict(
self.prior_radar, timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
post = self.updater_radar.update(hypothesis)
self.tracks_radar.append(post)
self.prior_radar = self.tracks_radar[-1]
self.tracks_ais = Track()
for measurement in self.measurements_ais:
prediction = self.predictor_radar.predict(
self.prior_ais, timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
post = self.updater_ais.update(hypothesis)
self.tracks_ais.append(post)
self.prior_ais = self.tracks_ais[-1]
self.tracks_fused = self._fuse_tracks(self.tracks_radar,
self.tracks_ais)
return self.tracks_fused, self.tracks_radar, self.tracks_ais
def _fuse_tracks(self, tracks_radar, tracks_ais):
tracks_fused = []
for track_radar in tracks_radar:
# find a track in tracks_radar with the same timestamp
estimate = track_radar
for track_ais in tracks_ais:
if track_ais.timestamp == track_radar.timestamp:
# same_target = track_to_track_association.test_association_independent_tracks(track_radar, track_ais,
# 0.01)
same_target = True # ignore association for now
if same_target:
fused_posterior, fused_covar = track_to_track_fusion.fuse_independent_tracks(
track_radar, track_ais)
estimate = GaussianState(
fused_posterior,
fused_covar,
timestamp=track_radar.timestamp)
break
tracks_fused.append(estimate)
return tracks_fused
class KalmanFilterDependentFusionAsyncSensors:
"""
todo
"""
def __init__(self, start_time, prior: GaussianState,
sigma_process_radar=0.01, sigma_process_ais=0.01, sigma_meas_radar=3, sigma_meas_ais=1):
"""
:param start_time:
:param prior:
:param sigma_process_radar:
:param sigma_process_ais:
:param sigma_meas_radar:
:param sigma_meas_ais:
"""
self.start_time = start_time
# same transition models (radar uses same as original)
self.transition_model_radar = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(sigma_process_radar), ConstantVelocity(sigma_process_radar)])
self.transition_model_ais = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(sigma_process_ais), ConstantVelocity(sigma_process_ais)])
# same measurement models as used when generating the measurements
# Specify measurement model for radar
self.measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]])
)
# Specify measurement model for AIS
self.measurement_model_ais = LinearGaussian(
ndim_state=4,
mapping=(0, 2),
noise_covar=np.array([[sigma_meas_ais, 0],
[0, sigma_meas_ais]])
)
# specify predictors
self.predictor_radar = KalmanPredictor(self.transition_model_radar)
self.predictor_ais = KalmanPredictor(self.transition_model_ais)
# specify updaters
self.updater_radar = KalmanUpdater(self.measurement_model_radar)
self.updater_ais = KalmanUpdater(self.measurement_model_ais)
# create prior, both trackers use the same starting point
self.prior_radar = prior
self.prior_ais = prior
self.cross_cov_list = []
def track(self, measurements_radar, measurements_ais):
"""
todo
:return:
"""
# create list for storing kalman gains
kf_gains_radar = []
kf_gains_ais = []
# create list for storing transition_noise_covar
transition_covars_radar = []
transition_covars_ais = []
# create list for storing transition matrices
transition_matrices_radar = []
transition_matrices_ais = []
# create list for storing tracks
tracks_radar = Track()
tracks_ais = Track()
# track
for measurement in measurements_radar:
prediction = self.predictor_radar.predict(self.prior_radar, timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
# calculate the kalman gain
hypothesis.measurement_prediction = self.updater_radar.predict_measurement(hypothesis.prediction,
measurement_model=self.measurement_model_radar)
post_cov, kalman_gain = self.updater_radar._posterior_covariance(hypothesis)
kf_gains_radar.append(kalman_gain)
# get the transition model covar
predict_over_interval = measurement.timestamp - self.prior_radar.timestamp
transition_covars_ais.append(self.transition_model_ais.covar(time_interval=predict_over_interval))
transition_matrices_ais.append(self.transition_model_ais.matrix(time_interval=predict_over_interval))
# update
post = self.updater_radar.update(hypothesis)
tracks_radar.append(post)
self.prior_radar = post
for measurement in measurements_ais:
prediction = self.predictor_radar.predict(self.prior_ais, timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
# calculate the kalman gain
hypothesis.measurement_prediction = self.updater_ais.predict_measurement(hypothesis.prediction,
measurement_model=self.measurement_model_ais)
post_cov, kalman_gain = self.updater_ais._posterior_covariance(hypothesis)
kf_gains_ais.append(kalman_gain)
# get the transition model covar
predict_over_interval = measurement.timestamp - self.prior_ais.timestamp
transition_covars_radar.append(self.transition_model_radar.covar(time_interval=predict_over_interval))
transition_matrices_radar.append(self.transition_model_radar.matrix(time_interval=predict_over_interval))
# update
post = self.updater_ais.update(hypothesis)
tracks_ais.append(post)
self.prior_ais = post
# FOR NOW: run track_to_track_association here, todo change pipeline flow
# FOR NOW: run the association only when both have a new posterior (so each time the AIS has a posterior)
# todo handle fusion when one track predicts and the other updates. (or both predicts) (Can't be done with the
# theory described in the article)
cross_cov_ij = [np.zeros([4, 4])]
cross_cov_ji = [np.zeros([4, 4])]
# TODO change flow to assume that the indexes decide whether its from the same iterations
# use indexes to loop through tracks, kf_gains etc
tracks_fused = [tracks_radar[0]]
for i in range(1, len(tracks_radar)):
# we assume that the indexes correlates with the timestamps. I.e. that the lists are 'synchronized'
# check to make sure
if tracks_ais[i].timestamp == tracks_radar[i].timestamp:
# calculate the cross-covariance estimation error
cross_cov_ji.append(calc_cross_cov_estimate_error(
self.measurement_model_ais.matrix(), self.measurement_model_radar.matrix(), kf_gains_ais[i],
kf_gains_radar[i],
transition_matrices_ais[i], transition_covars_ais[i], cross_cov_ji[i - 1]
))
cross_cov_ij.append(calc_cross_cov_estimate_error(
self.measurement_model_radar.matrix(), self.measurement_model_ais.matrix(), kf_gains_radar[i],
kf_gains_ais[i],
transition_matrices_radar[i], transition_covars_ais[i], cross_cov_ij[i - 1]
))
self.cross_cov_list.append(cross_cov_ij)
# test for track association
# same_target = track_to_track_association.test_association_dependent_tracks(tracks_radar[i],
# tracks_ais[i],
# cross_cov_ij[i],
# cross_cov_ji[i], 0.01)
same_target = True # ignore test for track association for now
if same_target:
fused_posterior, fused_covar = track_to_track_fusion.fuse_dependent_tracks(tracks_radar[i],
tracks_ais[i],
cross_cov_ij[i],
cross_cov_ji[i])
estimate = GaussianState(fused_posterior, fused_covar, timestamp=tracks_ais[i].timestamp)
tracks_fused.append(estimate)
return tracks_fused, tracks_ais, tracks_radar
def track_async(self, start_time, measurements_radar, measurements_ais, fusion_rate=1):
"""
Assumptions:
1) assumes that there are a maximum of one new measurement per sensor per fusion_rate.
2) assumes that the measurements arrives exactly at the timestep that the fusion is performed.
3) assumes kf gain of size (4,2)
"""
# create list for storing tracks
tracks_radar = Track()
tracks_ais = Track()
tracks_fused = []
time = start_time
cross_cov_ij = np.zeros([4, 4])
cross_cov_ji = np.zeros([4, 4])
measurements_radar = measurements_radar.copy()
measurements_ais = measurements_ais.copy()
# loop until there are no more measurements
while measurements_radar or measurements_ais:
# get all new measurements
new_measurements_radar = \
[measurement for measurement in measurements_radar if measurement.timestamp <= time]
new_measurements_ais = \
[measurement for measurement in measurements_ais if measurement.timestamp <= time]
# remove the new measurements from the measurements lists
for new_meas in new_measurements_ais:
measurements_ais.remove(new_meas)
for new_meas in new_measurements_radar:
measurements_radar.remove(new_meas)
# check whether there are more than one measurement per sensor
if len(new_measurements_ais) > 1 or len(new_measurements_radar) > 1:
# raise exception
raise Exception("More than one measurement per sensor per fusion rate")
# for each sensor, perform a prediction
prediction_radar = self.predictor_radar.predict(self.prior_radar, timestamp=time)
prediction_ais = self.predictor_ais.predict(self.prior_ais, timestamp=time)
# if a new AIS measurement
if new_measurements_ais:
measurement = new_measurements_ais[0]
# calc updated estimate
hypothesis = SingleHypothesis(prediction_ais, measurement)
# calc kalman gain
# calculate the kalman gain
hypothesis.measurement_prediction = self.updater_ais.predict_measurement(hypothesis.prediction,
measurement_model=self.measurement_model_ais)
post_cov, kf_gain_ais = self.updater_ais._posterior_covariance(hypothesis)
# get the transition model covar
predict_over_interval = measurement.timestamp - self.prior_ais.timestamp
# calc transition matrix
transition_covar_ais = self.transition_model_ais.covar(time_interval=predict_over_interval)
transition_matrix_ais = self.transition_model_ais.matrix(time_interval=predict_over_interval)
# calc posterior
post = self.updater_ais.update(hypothesis)
# append posterior and update prior_ais
tracks_ais.append(post)
self.prior_ais = post
else:
# calc transition matrix and set kalman gain to 0
# get the transition model covar
predict_over_interval = time - self.prior_ais.timestamp
# calc transition matrix
transition_covar_ais = self.transition_model_ais.covar(time_interval=predict_over_interval)
transition_matrix_ais = self.transition_model_ais.matrix(time_interval=predict_over_interval)
# set kalman gain to 0
kf_gain_ais = Matrix([[0, 0], [0, 0], [0, 0], [0, 0]])
# append prediction and update prior_ais
tracks_ais.append(prediction_ais)
self.prior_ais = prediction_ais
# if a new radar measurement
if new_measurements_radar:
measurement = new_measurements_radar[0]
# calc updated estimate
hypothesis = SingleHypothesis(prediction_radar, measurement)
# calc kalman gain
# calculate the kalman gain
hypothesis.measurement_prediction = self.updater_radar.predict_measurement(hypothesis.prediction,
measurement_model=self.measurement_model_radar)
post_cov, kf_gain_radar = self.updater_radar._posterior_covariance(hypothesis)
# get the transition model covar
predict_over_interval = measurement.timestamp - self.prior_radar.timestamp
# calc transition matrix
transition_covar_radar = self.transition_model_radar.covar(time_interval=predict_over_interval)
transition_matrix_radar = self.transition_model_radar.matrix(time_interval=predict_over_interval)
# calc posterior
post = self.updater_radar.update(hypothesis)
# append posterior and update prior_radar
self.prior_radar = post
else:
# calc transition matrix and set kalman gain to 0
# get the transition model covar
predict_over_interval = time - self.prior_radar.timestamp
# calc transition matrix
transition_covar_radar = self.transition_model_radar.covar(time_interval=predict_over_interval)
transition_matrix_radar = self.transition_model_radar.matrix(time_interval=predict_over_interval)
# set kalman gain to 0
kf_gain_radar = Matrix([[0, 0], [0, 0], [0, 0], [0, 0]])
# append prediction and update prior_radar
self.prior_radar = prediction_radar
# calculate the cross-covariance
cross_cov_ij = calc_cross_cov_estimate_error(
self.measurement_model_radar.matrix(), self.measurement_model_ais.matrix(), kf_gain_radar,
kf_gain_ais, transition_matrix_radar, transition_covar_radar, cross_cov_ij
)
cross_cov_ji = calc_cross_cov_estimate_error(
self.measurement_model_ais.matrix(), self.measurement_model_radar.matrix(), kf_gain_ais,
kf_gain_radar, transition_matrix_ais, transition_covar_ais, cross_cov_ji
)
same_target = True # ignore test for track association for now
if same_target:
fused_posterior, fused_covar = track_to_track_fusion.fuse_dependent_tracks(self.prior_radar,
self.prior_ais,
cross_cov_ij,
cross_cov_ji)
estimate = GaussianState(fused_posterior, fused_covar, timestamp=time)
tracks_fused.append(estimate)
# try T2TFwoMpF
# also have to update the cross-covariance
cross_cov_ij = calc_partial_feedback_cross_cov(self.prior_radar, self.prior_ais, cross_cov_ij,
cross_cov_ji)
cross_cov_ji = cross_cov_ij.copy().T # right??
# TEMPORARY: try to let prior radar become the fused result, i.e. partial feedback
self.prior_radar = estimate
# append to radar tracks
tracks_radar.append(estimate)
self.cross_cov_list.append(cross_cov_ij)
time += timedelta(seconds=fusion_rate)
return tracks_fused, tracks_radar, tracks_ais
def test_information(UpdaterClass, measurement_model, prediction, measurement):
"""Tests the information form of the Kalman filter update step."""
# This is how the Kalman filter does it
kupdater = KalmanUpdater(measurement_model)
kposterior = kupdater.update(SingleHypothesis(prediction, measurement))
# Create the information state representation
prediction_precision = np.linalg.inv(prediction.covar)
info_prediction_mean = prediction_precision @ prediction.state_vector
info_prediction = InformationStatePrediction(info_prediction_mean,
prediction_precision)
# Initialise a information form of the Kalman updater
updater = UpdaterClass(measurement_model=measurement_model)
# Perform and assert state update (without measurement prediction)
posterior = updater.update(
SingleHypothesis(prediction=info_prediction, measurement=measurement))
# Check to see if the information matrix is positive definite (i.e. are all the eigenvalues
# positive?)
assert (np.all(np.linalg.eigvals(posterior.precision) >= 0))
# Does the measurement prediction work?
assert (np.allclose(
kupdater.predict_measurement(prediction).state_vector,
updater.predict_measurement(info_prediction).state_vector,
0,
atol=1.e-14))
# Do the
assert (np.allclose(
kposterior.state_vector,
np.linalg.inv(posterior.precision) @ posterior.state_vector,
0,
atol=1.e-14))
assert (np.allclose(kposterior.covar,
np.linalg.inv(posterior.precision),
0,
atol=1.e-14))
assert (np.array_equal(posterior.hypothesis.prediction, info_prediction))
assert (np.array_equal(posterior.hypothesis.measurement, measurement))
assert (posterior.timestamp == prediction.timestamp)
# test that we can get to the inverse matrix
class LinearGaussianwithInverse(LinearGaussian):
def inverse_covar(self, **kwargs):
return np.linalg.inv(self.covar(**kwargs))
meas_model_winv = LinearGaussianwithInverse(ndim_state=2,
mapping=[0],
noise_covar=np.array([[0.04]]))
updater_winv = UpdaterClass(meas_model_winv)
# Test this still works
post_from_inv = updater_winv.update(
SingleHypothesis(prediction=info_prediction, measurement=measurement))
# and check
assert (np.allclose(posterior.state_vector,
post_from_inv.state_vector,
0,
atol=1.e-14))
# Can one force symmetric covariance?
updater.force_symmetric_covariance = True
posterior = updater.update(
SingleHypothesis(prediction=info_prediction, measurement=measurement))
assert (np.allclose(posterior.precision - posterior.precision.T,
np.zeros(np.shape(posterior.precision)),
0,
atol=1.e-14))
class KFFusionUnsyncSensors:
"""
todo
"""
def __init__(self, start_time, prior: GaussianState, sigma_process=0.01,
sigma_meas_radar=3, sigma_meas_ais=1):
"""
:param start_time:
:param prior:
:param sigma_process:
:param sigma_meas_radar:
:param sigma_meas_ais:
"""
# start time
self.start_time = start_time
# transition model
self.transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity(sigma_process),
ConstantVelocity(sigma_process)])
# same measurement models as used when generating the measurements
# Specify measurement model for radar
self.measurement_model_radar = LinearGaussian(
ndim_state=4, # number of state dimensions
mapping=(0, 2), # mapping measurement vector index to state index
noise_covar=np.array([[sigma_meas_radar, 0], # covariance matrix for Gaussian PDF
[0, sigma_meas_radar]])
)
# Specify measurement model for AIS
self.measurement_model_ais = LinearGaussian(
ndim_state=4,
mapping=(0, 2),
noise_covar=np.array([[sigma_meas_ais, 0],
[0, sigma_meas_ais]])
)
# specify predictor
self.predictor = KalmanPredictor(self.transition_model)
# specify updaters
self.updater_radar = KalmanUpdater(self.measurement_model_radar)
self.updater_ais = KalmanUpdater(self.measurement_model_ais)
# create prior todo move later and probably rename
self.prior = prior
def track(self, measurements_radar, measurements_ais, estimation_rate=1):
"""
Uses the Kalman Filter to fuse the measurements received. Produces a new estimate at each estimation_rate.
A prediction is performed when no new measurements are received when a new estimate is calculated.
Note: when estimation_rate is lower than either of the measurements rates, it might not use all measurements
when updating.
:param measurements_radar:
:param measurements_ais:
:param estimation_rate: How often a new estimate should be calculated.
"""
time = self.start_time
tracks_fused = Track()
tracks_radar = Track()
# copy measurements
measurements_radar = measurements_radar.copy()
measurements_ais = measurements_ais.copy()
# loop until there are no more measurements
while measurements_ais or measurements_radar:
# get all new measurements
new_measurements_radar = \
[measurement for measurement in measurements_radar if measurement.timestamp <= time]
new_measurements_ais = \
[measurement for measurement in measurements_ais if measurement.timestamp <= time]
# remove the new measurements from the measurements lists
for new_meas in new_measurements_ais:
measurements_ais.remove(new_meas)
for new_meas in new_measurements_radar:
measurements_radar.remove(new_meas)
# sort the new measurements
new_measurements_radar.sort(key=lambda meas: meas.timestamp, reverse=True)
new_measurements_ais.sort(key=lambda meas: meas.timestamp, reverse=True)
while new_measurements_radar or new_measurements_ais:
if new_measurements_radar and \
(not new_measurements_ais or
new_measurements_radar[0].timestamp <= new_measurements_ais[0].timestamp):
# predict and update with radar measurement
new_measurement = new_measurements_radar[0]
prediction = self.predictor.predict(self.prior, timestamp=new_measurement.timestamp)
hypothesis = SingleHypothesis(prediction, new_measurement)
post = self.updater_radar.update(hypothesis)
tracks_radar.append(post)
# remove measurement
new_measurements_radar.remove(new_measurement)
else:
# predict and update with radar measurement
new_measurement = new_measurements_ais[0]
prediction = self.predictor.predict(self.prior, timestamp=new_measurement.timestamp)
hypothesis = SingleHypothesis(prediction, new_measurement)
post = self.updater_ais.update(hypothesis)
# remove measurement
new_measurements_ais.remove(new_measurement)
# add to fused list
self.prior = post
# perform a prediction up until this time (the newest measurement might not be at this exact time)
# note that this "prediction" might be the updated posterior, if the newest measurement was at this time
prediction = self.predictor.predict(self.prior, timestamp=time)
tracks_fused.append(GaussianState(prediction.mean, prediction.covar, prediction.timestamp))
# increment time
time += timedelta(seconds=estimation_rate)
return tracks_fused, tracks_radar
def plot(self, ground_truth, measurements_radar, measurements_ais, tracks_fused, tracks_radar):
"""
Is this being used?
:return:
"""
# PLOT
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlabel("$x$")
ax.set_ylabel("$y$")
ax.axis('equal')
ax.plot([state.state_vector[0] for state in ground_truth],
[state.state_vector[2] for state in ground_truth],
linestyle="--",
label='Ground truth')
ax.scatter([state.state_vector[0] for state in measurements_radar],
[state.state_vector[1] for state in measurements_radar],
color='b',
label='Measurements Radar')
ax.scatter([state.state_vector[0] for state in measurements_ais],
[state.state_vector[1] for state in measurements_ais],
color='r',
label='Measurements AIS')
# add ellipses to the posteriors
for state in tracks_fused:
w, v = np.linalg.eig(
self.measurement_model_radar.matrix() @ state.covar @ self.measurement_model_radar.matrix().T)
max_ind = np.argmax(w)
min_ind = np.argmin(w)
orient = np.arctan2(v[1, max_ind], v[0, max_ind])
ellipse = Ellipse(xy=(state.state_vector[0], state.state_vector[2]),
width=2 * np.sqrt(w[max_ind]), height=2 * np.sqrt(w[min_ind]),
angle=np.rad2deg(orient),
alpha=0.2,
color='r')
ax.add_artist(ellipse)
for state in tracks_radar:
w, v = np.linalg.eig(
self.measurement_model_radar.matrix() @ state.covar @ self.measurement_model_radar.matrix().T)
max_ind = np.argmax(w)
min_ind = np.argmin(w)
orient = np.arctan2(v[1, max_ind], v[0, max_ind])
ellipse = Ellipse(xy=(state.state_vector[0], state.state_vector[2]),
width=2 * np.sqrt(w[max_ind]), height=2 * np.sqrt(w[min_ind]),
angle=np.rad2deg(orient),
alpha=0.2,
color='b')
ax.add_artist(ellipse)
# add ellipses to add legend todo do this less ugly
ellipse = Ellipse(xy=(0, 0),
width=0,
height=0,
color='r',
alpha=0.2,
label='Posterior Fused')
ax.add_patch(ellipse)
ellipse = Ellipse(xy=(0, 0),
width=0,
height=0,
color='b',
alpha=0.2,
label='Posterior Radar')
ax.add_patch(ellipse)
# todo move or remove
ax.legend()
ax.set_title("Kalman filter tracking and fusion when AIS is viewed as a measurement")
fig.show()
fig.savefig("../results/scenario1/KF_tracking_and_fusion_viewing_ais_as_measurement.svg")
