def track(self, measurements_radar, measurements_ais, fusion_rate=1):
"""
returns fused tracks. Assumes that the rate of the radar and ais measurements are the same, and that they are
synchornized.
"""
tracks_radar = Track()
for measurement in 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]
tracks_ais = Track()
for measurement in measurements_ais:
prediction = self.predictor_radar.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]
tracks_fused = self._fuse_tracks(tracks_radar,
tracks_ais,
fusion_rate=fusion_rate)
return tracks_fused, tracks_ais, tracks_radar
updater = SDFUpdater(measurement_model)
"""Erstellen eines Anfangszustandes"""
prior = GaussianState([[0.0], [0.0], [0.0], [0.0]],
np.diag([0.0, 0.0, 0.0, 0.0]),
timestamp=0)
"""Erstellen einer Trajektorie, sodass das Filter arbeiten kann"""
track = Track()
for measurement in measurements:
prediction = predictor.predict(prior, timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
post = updater.update(hypothesis, measurement_model)
track.append(post)
prior = track[-1]
# Plot
ax.plot([state.state_vector[0] for state in track],
[state.state_vector[2] for state in track],
marker=".",
color="yellow")
"""Darstellen der Kovarianz durch Ellipsen"""
for state in track:
w, v = np.linalg.eig(measurement_model.matrix() @ state.covar
@ measurement_model.matrix().T)
max_ind = np.argmax(v[0, :])
orient = np.arctan2(v[max_ind, 1], v[max_ind, 0])
ellipse = Ellipse(xy=state.state_vector[(0, 2), 0],
from stonesoup.types.state import GaussianState
prior = GaussianState([[0], [1], [0], [1]], np.diag([0.25, 0.1, 0.25, 0.1]), timestamp=start_time)
from stonesoup.types.track import Track
track = Track([prior])
for n, (measurements, clutter_set) in enumerate(zip(measurementss, clutter), 1):
detections = clutter_set.copy()
detections.update(measurements) # Add measurements and clutter together
hypotheses = data_associator.associate({track}, detections, start_time+timedelta(seconds=n))
hypothesis = hypotheses[track]
if hypothesis.measurement:
post = updater.update(hypothesis)
track.append(post)
else: # When data associator says no detections are good enough, we'll keep the prediction
track.append(hypothesis.prediction)
# Plot the resulting track
ax.plot([state.state_vector[0, 0] for state in track[1:]], # Skip plotting the prior
[state.state_vector[2, 0] for state in track[1:]],
marker=".")
fig
from matplotlib.patches import Ellipse
for state in track[1:]: # Skip the prior
w, v = np.linalg.eig(measurement_model.matrix()@state.covar@measurement_model.matrix().T)
max_ind = np.argmax(v[0, :])
orient = np.arctan2(v[max_ind, 1], v[max_ind, 0])
ellipse = Ellipse(xy=state.state_vector[(0,2), 0],
prior = GaussianState([[0.0], [0.0], [0.0], [0.0]], np.diag([0.0, 0.0, 0.0, 0.0]), timestamp=0)
from stonesoup.types.hypothesis import SingleHypothesis
from stonesoup.types.track import Track
track = Track()
for measurement in measurements:
prediction = predictor.predict(prior, timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
post = updater.update(hypothesis, measurement_model)
track.append(post)
prior = track[-1]
# Plot the resulting track
ax.plot([state.state_vector[0] for state in track],
[state.state_vector[2] for state in track],
marker=".", color="yellow")
from matplotlib.patches import Ellipse
for state in track:
w, v = np.linalg.eig(measurement_model.matrix() @ state.covar @ measurement_model.matrix().T)
max_ind = np.argmax(v[0, :])
orient = np.arctan2(v[max_ind, 1], v[max_ind, 0])
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])
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
posterior_states.append(hypothesis.prediction)
else:
posterior_state = updater.update(hypothesis)
posterior_states.append(posterior_state)
posterior_state_weights.append(hypothesis.probability)
means = StateVectors([state.state_vector for state in posterior_states])
covars = np.stack([state.covar for state in posterior_states], axis=2)
weights = np.asarray(posterior_state_weights)
# Reduce mixture of states to one posterior estimate Gaussian.
post_mean, post_covar = gm_reduce_single(means, covars, weights)
# Add a Gaussian state approximation to the track.
track.append(
GaussianStateUpdate(post_mean, post_covar, hypotheses,
hypotheses[0].measurement.timestamp))
# %%
# Plot the resulting track
plotter.plot_tracks(track, [0, 2], uncertainty=True)
plotter.fig
# %%
# References
# ----------
# 1. Bar-Shalom Y, Daum F, Huang F 2009, The Probabilistic Data Association Filter, IEEE Control
# Systems Magazine
# sphinx_gallery_thumbnail_number = 2
timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
# calculate the kalman gain
hypothesis.measurement_prediction = updater_radar.predict_measurement(
hypothesis.prediction, measurement_model=measurement_model_radar)
post_cov, kalman_gain = updater_radar._posterior_covariance(hypothesis)
kf_gains_radar.append(kalman_gain)
# get the transition model covar
predict_over_interval = measurement.timestamp - prior_radar.timestamp
transition_covars_ais.append(
transition_model_ais.covar(time_interval=predict_over_interval))
transition_matrixes_ais.append(
transition_model_ais.matrix(time_interval=predict_over_interval))
# update
post = updater_radar.update(hypothesis)
tracks_radar.append(post)
prior_radar = tracks_radar[-1]
for measurement in measurements_ais:
prediction = predictor_radar.predict(prior_ais,
timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
# calculate the kalman gain
hypothesis.measurement_prediction = updater_ais.predict_measurement(
hypothesis.prediction, measurement_model=measurement_model_ais)
post_cov, kalman_gain = updater_ais._posterior_covariance(hypothesis)
kf_gains_ais.append(kalman_gain)
# get the transition model covar
predict_over_interval = measurement.timestamp - prior_radar.timestamp
transition_covars_radar.append(
transition_model_radar.covar(time_interval=predict_over_interval))
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
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
from stonesoup.types.hypothesis import SingleHypothesis
from stonesoup.types.track import Track
track1 = Track()
track2 = Track()
print("Capon detections:")
for timestep, detections in detector1:
for detection in detections:
print(detection)
prediction = predictor.predict(prior, timestamp=detection.timestamp)
hypothesis = SingleHypothesis(
prediction, detection) # Group a prediction and measurement
post = updater.update(hypothesis)
track1.append(post)
prior = track1[-1]
print("RJMCMC detections:")
for timestep, detections in detector2:
for detection in detections:
print(detection)
prediction = predictor.predict(prior, timestamp=detection.timestamp)
hypothesis = SingleHypothesis(
prediction, detection) # Group a prediction and measurement
post = updater.update(hypothesis)
track2.append(post)
prior = track2[-1]
plotter = Plotter()
plotter.plot_tracks(set([track1, track2]), [0, 2], uncertainty=True)
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 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
