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
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
# 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 measurements_radar:
prediction = predictor_radar.predict(prior_radar,
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,
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))