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
#
from stonesoup.plotter import Plotter
plotter = Plotter()
plotter.plot_ground_truths(truths, [0, 2])
# %%
# Generate detections with clutter
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# Next, generate detections with clutter just as in the previous tutorial. The clutter is
# assumed to be uniformly distributed accross the entire field of view, here assumed to
# be the space where :math:`x \in [-100, 100]` and :math:`y \in [-100, 100]`.
# Make the measurement model
from stonesoup.models.measurement.linear import LinearGaussian
measurement_model = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array([[0.75, 0], [0,
0.75]]))
# Generate detections and clutter
from scipy.stats import uniform
from stonesoup.types.detection import TrueDetection
from stonesoup.types.detection import Clutter
all_measurements = []
# The probability detection and clutter rate play key roles in the posterior intensity.
# They can be changed to see their effect.
probability_detection = 0.9
clutter_rate = 3.0
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
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
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")
def generate_scenario_1(seed=1996,
permanent_save=True,
sigma_process=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1):
"""
Generates scenario 1. Todo define scenario 1
:param seed:
:param permanent_save:
:param sigma_process:
:param sigma_meas_radar:
:param sigma_meas_ais:
:return:
"""
# specify seed to be able repeat example
start_time = datetime.now()
np.random.seed(seed)
# combine two 1-D CV models to create a 2-D CV model
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(sigma_process),
ConstantVelocity(sigma_process)])
# starting at 0,0 and moving NE
truth = GroundTruthPath(
[GroundTruthState([0, 1, 0, 1], timestamp=start_time)])
# generate truth using transition_model and noise
for k in range(1, 21):
truth.append(
GroundTruthState(transition_model.function(
truth[k - 1], noise=True, time_interval=timedelta(seconds=1)),
timestamp=start_time + timedelta(seconds=k)))
# Simulate measurements
# Specify measurement model for radar
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
measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# generate "radar" measurements
measurements_radar = []
for state in truth:
measurement = measurement_model_radar.function(state, noise=True)
measurements_radar.append(
Detection(measurement, timestamp=state.timestamp))
# generate "AIS" measurements
measurements_ais = []
state_num = 0
for state in truth:
state_num += 1
if not state_num % 2: # measurement every second time step
measurement = measurement_model_ais.function(state, noise=True)
measurements_ais.append(
Detection(measurement, timestamp=state.timestamp))
if permanent_save:
save_folder_name = seed.__str__()
else:
save_folder_name = "temp"
save_folder = "../scenarios/scenario1/" + save_folder_name + "/"
# save the ground truth and the measurements for the radar and the AIS
store_object.store_object(truth, save_folder, "ground_truth.pk1")
store_object.store_object(measurements_radar, save_folder,
"measurements_radar.pk1")
store_object.store_object(measurements_ais, save_folder,
"measurements_ais.pk1")
store_object.store_object(start_time, save_folder, "start_time.pk1")
store_object.store_object(measurement_model_radar, save_folder,
"measurement_model_radar.pk1")
store_object.store_object(measurement_model_ais, save_folder,
"measurement_model_ais.pk1")
store_object.store_object(transition_model, save_folder,
"transition_model.pk1")
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 detections_gen(self):
detections = set()
current_time = datetime.now()
num_samps = 1000000
d = 10
omega = 50
fs = 20000
l = 1 # expected number of targets
window = 20000
windowm1 = window - 1
y = np.loadtxt(self.csv_path, delimiter=',')
L = len(y)
N = 9 * window
max_targets = 5
nbins = 128
bin_steps = [(math.pi + 0.1) / (2 * nbins), 2 * math.pi / nbins]
scans = []
winstarts = np.linspace(0, L - window, num=int(L / window), dtype=int)
for win in winstarts:
# initialise histograms
param_hist = np.zeros([max_targets, nbins, nbins])
order_hist = np.zeros([max_targets])
# initialise params
p_params = np.empty([max_targets, 2])
noise = noise_proposal(0)
[params, K] = proposal([], 0, p_params)
# calculate sinTy and cosTy
sinTy = np.zeros([9])
cosTy = np.zeros([9])
alpha = np.zeros([9])
yTy = 0
for k in range(0, 9):
for t in range(0, window):
sinTy[k] = sinTy[k] + math.sin(
2 * math.pi * t * omega / fs) * y[t + win, k]
cosTy[k] = cosTy[k] + math.cos(
2 * math.pi * t * omega / fs) * y[t + win, k]
yTy = yTy + y[t + win, k] * y[t + win, k]
sumsinsq = 0
sumcossq = 0
sumsincos = 0
for t in range(0, window):
sumsinsq = sumsinsq + math.sin(
2 * math.pi * t * omega / fs) * math.sin(
2 * math.pi * t * omega / fs)
sumcossq = sumcossq + math.cos(
2 * math.pi * t * omega / fs) * math.cos(
2 * math.pi * t * omega / fs)
sumsincos = sumsincos + math.sin(
2 * math.pi * t * omega / fs) * math.cos(
2 * math.pi * t * omega / fs)
old_logp = calc_acceptance(noise, params, K, omega, 1, d, y,
window, sinTy, cosTy, yTy, alpha,
sumsinsq, sumcossq, sumsincos, N, l)
n = 0
while n < num_samps:
p_noise = noise_proposal(noise)
[p_params, p_K,
Qratio] = proposal_func(params, K, p_params, max_targets)
if p_K != 0:
new_logp = calc_acceptance(p_noise, p_params, p_K, omega,
1, d, y, window, sinTy, cosTy,
yTy, alpha, sumsinsq, sumcossq,
sumsincos, N, l)
logA = new_logp - old_logp + np.log(Qratio)
# do a Metropolis-Hastings step
if logA > np.log(random.uniform(0, 1)):
old_logp = new_logp
params = copy.deepcopy(p_params)
K = copy.deepcopy(p_K)
for k in range(0, K):
bin_ind = [0, 0]
for l in range(0, 2):
edge = bin_steps[l]
while edge < params[k, l]:
edge += bin_steps[l]
bin_ind[l] += 1
if bin_ind[l] == nbins - 1:
break
param_hist[K - 1, bin_ind[0], bin_ind[1]] += 1
order_hist[K - 1] += 1
n += 1
# look for peaks in histograms
max_peak = 0
max_ind = 0
for ind in range(0, max_targets):
if order_hist[ind] > max_peak:
max_peak = order_hist[ind]
max_ind = ind
# FOR TESTING PURPOSES ONLY - SET max_ind = 0
max_ind = 0
# look for largest N peaks, where N corresponds to peak in the order histogram
# use divide-and-conquer quadrant-based approach
if max_ind == 0:
[unique_peak_inds1, unique_peak_inds2
] = np.unravel_index(param_hist[0, :, :].argmax(),
param_hist[0, :, :].shape)
num_peaks = 1
else:
order_ind = max_ind - 1
quadrant_factor = 2
nstart = 0
mstart = 0
nend = quadrant_factor
mend = quadrant_factor
peak_inds1 = [None] * 16
peak_inds2 = [None] * 16
k = 0
while quadrant_factor < 32:
max_quadrant = 0
quadrant_size = nbins / quadrant_factor
for n in range(nstart, nend):
for m in range(mstart, mend):
[ind1, ind2] = np.unravel_index(
param_hist[order_ind,
int(n * quadrant_size):int(
(n + 1) * quadrant_size - 1),
int(m * quadrant_size):int(
(m + 1) * quadrant_size -
1)].argmax(),
param_hist[order_ind,
int(n * quadrant_size):int(
(n + 1) * quadrant_size - 1),
int(m * quadrant_size):int(
(m + 1) * quadrant_size -
1)].shape)
peak_inds1[k] = int(ind1 + n * quadrant_size)
peak_inds2[k] = int(ind2 + m * quadrant_size)
if param_hist[order_ind, peak_inds1[k],
peak_inds2[k]] > max_quadrant:
max_quadrant = param_hist[order_ind,
peak_inds1[k],
peak_inds2[k]]
max_ind1 = n
max_ind2 = m
k += 1
quadrant_factor = 2 * quadrant_factor
# on next loop look for other peaks in the quadrant containing the highest peak
nstart = 2 * max_ind1
mstart = 2 * max_ind2
nend = 2 * (max_ind1 + 1)
mend = 2 * (max_ind2 + 1)
# determine unique peaks
unique_peak_inds1 = [None] * 16
unique_peak_inds2 = [None] * 16
unique_peak_inds1[0] = peak_inds1[0]
unique_peak_inds2[0] = peak_inds2[0]
num_peaks = 1
for n in range(0, 16):
flag_unique = 1
for k in range(0, num_peaks):
# check if peak is close to any other known peaks
if (unique_peak_inds1[k] - peak_inds1[n]) < 2:
if (unique_peak_inds2[k] - peak_inds2[n]) < 2:
# part of same peak (check if bin is taller)
if param_hist[order_ind, peak_inds1[n],
peak_inds2[n]] > param_hist[
order_ind,
unique_peak_inds1[k],
unique_peak_inds2[k]]:
unique_peak_inds1 = peak_inds1[n]
unique_peak_inds2 = peak_inds2[n]
flag_unique = 0
break
if flag_unique == 1:
unique_peak_inds1[num_peaks] = peak_inds1[n]
unique_peak_inds2[num_peaks] = peak_inds2[n]
num_peaks += 1
# Defining a detection
state_vector = StateVector([
unique_peak_inds2 * bin_steps[1],
unique_peak_inds1 * bin_steps[0]
]) # [Azimuth, Elevation]
covar = CovarianceMatrix(np.array([[1, 0],
[0, 1]])) # [[AA, AE],[AE, EE]]
measurement_model = LinearGaussian(ndim_state=4,
mapping=[0, 2],
noise_covar=covar)
current_time = current_time + timedelta(milliseconds=window)
detection = Detection(state_vector,
timestamp=current_time,
measurement_model=measurement_model)
detections = set([detection])
scans.append((current_time, detections))
# For every timestep
for scan in scans:
yield scan[0], scan[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
transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity(0.01), ConstantVelocity(0.01)])
# starting at 0,0 and moving NE
truth = GroundTruthPath([GroundTruthState([0, 1, 0, 1], timestamp=start_time)])
# generate truth using transition_model and noise
for k in range(1, 21):
truth.append(GroundTruthState(
transition_model.function(truth[k - 1], noise=True, time_interval=timedelta(seconds=1)),
timestamp=start_time + timedelta(seconds=k)))
# Simulate measurements
# Specify measurement model for radar
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([[1, 0], # covariance matrix for Gaussian PDF
[0, 1]])
)
# Specify measurement model for AIS (Same as for radar)
measurement_model_ais = LinearGaussian(
ndim_state=4,
mapping=(0, 2),
noise_covar=np.array([[1, 0],
[0, 1]])
)
# generate "radar" measurements
measurements_radar = []
for state in truth:
measurement = measurement_model_radar.function(state, noise=True)
def detections_gen(self):
detections = set()
current_time = datetime.now()
y = np.loadtxt(self.csv_path, delimiter=',')
L = len(y)
# frequency of sinusoidal signal
omega = 50
window = 20000
windowm1 = window - 1
thetavals = np.linspace(0, 2 * math.pi, num=400)
phivals = np.linspace(0, math.pi / 2, num=100)
# spatial locations of hydrophones
z = np.matrix(
'0 0 0; 0 10 0; 0 20 0; 10 0 0; 10 10 0; 10 20 0; 20 0 0; 20 10 0; 20 20 0'
)
N = 9 # No. of hydrophones
# steering vector
v = np.zeros(N, dtype=np.complex)
# directional unit vector
a = np.zeros(3)
scans = []
winstarts = np.linspace(0, L - window, num=int(L / window), dtype=int)
c = 1481 / (2 * omega * math.pi)
for t in winstarts:
# calculate covariance estimate
R = np.matmul(np.transpose(y[t:t + windowm1]), y[t:t + windowm1])
R_inv = np.linalg.inv(R)
maxF = 0
maxtheta = 0
maxfreq = 0
for theta in thetavals:
for phi in phivals:
# convert from spherical polar coordinates to cartesian
a[0] = math.cos(theta) * math.sin(phi)
a[1] = math.sin(theta) * math.sin(phi)
a[2] = math.cos(phi)
a = a / math.sqrt(np.sum(a * a))
for n in range(0, N):
phase = np.sum(a * np.transpose(z[n, ])) / c
v[n] = math.cos(phase) - math.sin(phase) * 1j
F = 1 / (
(window - N) * np.transpose(np.conj(v)) @ R_inv @ v)
if F > maxF:
maxF = F
maxtheta = theta
maxphi = phi
# Defining a detection
state_vector = StateVector([maxtheta,
maxphi]) # [Azimuth, Elevation]
covar = CovarianceMatrix(np.array([[1, 0],
[0, 1]])) # [[AA, AE],[AE, EE]]
measurement_model = LinearGaussian(ndim_state=4,
mapping=[0, 2],
noise_covar=covar)
current_time = current_time + timedelta(milliseconds=window)
detection = Detection(state_vector,
timestamp=current_time,
measurement_model=measurement_model)
detections = set([detection])
scans.append((current_time, detections))
# For every timestep
for scan in scans:
yield scan[0], scan[1]
def measurement_model():
return LinearGaussian(ndim_state=2,
mapping=[0],
noise_covar=np.array([[0.04]]))
from datetime import timedelta
import numpy as np
from stonesoup.types.array import StateVector
from stonesoup.models.measurement.linear import LinearGaussian
from stonesoup.types.detection import Detection
from stonesoup.types.state import State
from stonesoup.types.prediction import StatePrediction, StateMeasurementPrediction
from stonesoup.updater.alphabeta import AlphaBetaUpdater
from stonesoup.types.hypothesis import SingleHypothesis
@pytest.mark.parametrize(
"measurement_model, prediction, measurement, alpha, beta",
[( # Standard Alpha-Beta
LinearGaussian(ndim_state=4, mapping=[0, 2], noise_covar=0),
StatePrediction(StateVector([-6.45, 0.7, -6.45, 0.7])),
Detection(StateVector([-6.23, -6.23])), 0.9, 0.3)],
ids=["standard"])
def test_alphabeta(measurement_model, prediction, measurement, alpha, beta):
# Time delta
timediff = timedelta(seconds=2)
# Calculate evaluation variables - converts
# to measurement from prediction space
eval_measurement_prediction = StateMeasurementPrediction(
measurement_model.matrix() @ prediction.state_vector)
eval_posterior_position = prediction.state_vector[[0, 2]] + \
alpha * (measurement.state_vector - eval_measurement_prediction.state_vector)
if (arg.find(flag) >= 0):
found = True
return arg.split(flag)[1]
if (found == False):
raise Exception('Required argument {} was not found'.format(flag))
args = sys.argv
data_file = get_arg(args, '--datafile=')
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.01), ConstantVelocity(0.01)])
covar = CovarianceMatrix(np.array([[1, 0], [0, 1]])) # [[AA, AF],[AF, FF]]
measurement_model = LinearGaussian(ndim_state=4,
mapping=[0, 2],
noise_covar=covar)
from stonesoup.predictor.kalman import KalmanPredictor
predictor = KalmanPredictor(transition_model)
from stonesoup.updater.kalman import KalmanUpdater
updater = KalmanUpdater(measurement_model)
from stonesoup.types.state import GaussianState
prior = GaussianState([[0.5], [0], [0.5], [0]],
np.diag([1, 0, 1, 0]),
timestamp=datetime.now())
detector1 = beamformers_2d.capon(data_file)
detector2 = beamformers_2d.rjmcmc(data_file)
from stonesoup.types.detection import Detection
from stonesoup.types.hypothesis import SingleHypothesis
from stonesoup.types.prediction import (GaussianStatePrediction,
GaussianMeasurementPrediction)
from stonesoup.types.state import GaussianState, SqrtGaussianState
from stonesoup.updater.kalman import (KalmanUpdater, ExtendedKalmanUpdater,
UnscentedKalmanUpdater,
SqrtKalmanUpdater, IteratedKalmanUpdater)
@pytest.mark.parametrize(
"UpdaterClass, measurement_model, prediction, measurement",
[
( # Standard Kalman
KalmanUpdater,
LinearGaussian(
ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])),
GaussianStatePrediction(
np.array([[-6.45], [0.7]]),
np.array([[4.1123, 0.0013], [0.0013, 0.0365]
])), Detection(np.array([[-6.23]]))),
( # Extended Kalman
ExtendedKalmanUpdater,
LinearGaussian(
ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])),
GaussianStatePrediction(
np.array([[-6.45], [0.7]]),
np.array([[4.1123, 0.0013], [0.0013, 0.0365]
])), Detection(np.array([[-6.23]]))),
( # Unscented Kalman
UnscentedKalmanUpdater,
LinearGaussian(
# %%
# Measurement Model
# *****************
# Continuing, we define the measurement state :math:`\mathrm{y}_k`, which follows naturally from
# the form of the detections generated by the :class:`~.TensorFlowBoxObjectDetector` we previously
# discussed:
#
# .. math::
# \mathrm{y}_k = [x_k, y_k, w_k, h_k]
#
# We make use of a 4-dimensional :class:`~.LinearGaussian` model as our :class:`~.MeasurementModel`,
# whereby we can see that the individual indices of :math:`\mathrm{y}_k` map to indices `[0,2,4,5]`
# of the 6-dimensional state :math:`\mathrm{x}_k`:
from stonesoup.models.measurement.linear import LinearGaussian
measurement_model = LinearGaussian(ndim_state=6, mapping=[0, 2, 4, 5],
noise_covar=np.diag([1**2, 1**2, 3**2, 3**2]))
# %%
# Defining the tracker components
# ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# With the state-space models defined, we proceed to build our tracking components
#
# Filtering
# *********
# Since we have assumed Linear-Gaussian models, we will be using a Kalman Filter to perform
# filtering of the underlying single-target densities. This is done by making use of the
# :class:`~.KalmanPredictor` and :class:`~.KalmanUpdater` classes, which we define below:
from stonesoup.predictor.kalman import KalmanPredictor
predictor = KalmanPredictor(transition_model)
# %%
from stonesoup.updater.kalman import KalmanUpdater
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)
birth_rate=birth_rate,
death_probability=death_probability)
# %%
# Initialise the measurement models
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# The simulated ground truth will then be passed to a simple detection simulator. This again has a
# number of configurable parameters, e.g. where clutter is generated and at what rate, and
# detection probability. This implements similar logic to the code in the previous tutorial section
# :ref:`auto_tutorials/09_Initiators_&_Deleters:Generate Detections and Clutter`.
from stonesoup.simulator.simple import SimpleDetectionSimulator
from stonesoup.models.measurement.linear import LinearGaussian
# initialise the measurement model
measurement_model_covariance = np.diag([0.25, 0.25])
measurement_model = LinearGaussian(4, [0, 2], measurement_model_covariance)
# probability of detection
probability_detection = 0.9
# clutter will be generated uniformly in this are around the target
clutter_area = np.array([[-1, 1], [-1, 1]]) * 30
clutter_rate = 1
# %%
# The detection simulator
detection_sim = SimpleDetectionSimulator(
groundtruth=groundtruth_sim,
measurement_model=measurement_model,
detection_probability=probability_detection,
meas_range=clutter_area,
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])
np.array([[x], [y]]), timestamp=start_time+timedelta(seconds=n)))
# Plot the result
ax.scatter([state.state_vector[0, 0] for clutter_set in clutter for state in clutter_set],
[state.state_vector[1, 0] for clutter_set in clutter for state in clutter_set],
color='y', marker='2')
fig
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel((ConstantVelocity(0.05), ConstantVelocity(0.05)))
from stonesoup.predictor.kalman import KalmanPredictor
predictor = KalmanPredictor(transition_model)
from stonesoup.models.measurement.linear import LinearGaussian
measurement_model = LinearGaussian(
4, # Number of state dimensions (position and velocity in 2D)
(0,2), # Mapping measurement vector index to state index
np.array([[0.25, 0], # Covariance matrix for Gaussian PDF
[0, 0.25]])
)
from stonesoup.updater.kalman import KalmanUpdater
updater = KalmanUpdater(measurement_model)
from stonesoup.hypothesiser.distance import DistanceHypothesiser
from stonesoup.measures import Mahalanobis
hypothesiser = DistanceHypothesiser(predictor, updater, measure=Mahalanobis(), missed_distance=3)
from stonesoup.dataassociator.neighbour import NearestNeighbour
data_associator = NearestNeighbour(hypothesiser)
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)
def generate_scenario_3(seed=1996,
permanent_save=True,
radar_meas_rate=1,
ais_meas_rate=5,
sigma_process=0.01,
sigma_meas_radar=3,
sigma_meas_ais=1,
timesteps=20):
"""
Generates scenario 3. Scenario 3 consists of radar and ais measurements with different sampling rate. The sampling
rate is specified in the input params. A groundtruth is generated for each second.
:param seed:
:param permanent_save:
:param radar_meas_rate:
:param ais_meas_rate:
:param sigma_process:
:param sigma_meas_radar:
:param sigma_meas_ais:
:param timesteps: The amount of measurements from the slowest sensor
:return: Nothing. Saves the scenario to a specified folder
"""
start_time = datetime.now()
# specify seed to be able to repeat the example
np.random.seed(seed)
# combine two 1-D CV models to create a 2-D CV model
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(sigma_process),
ConstantVelocity(sigma_process)])
# starting at 0,0 and moving NE
truth = GroundTruthPath(
[GroundTruthState([0, 1, 0, 1], timestamp=start_time)])
# generate truth using transition_model and noise
end_time = start_time + timedelta(seconds=timesteps *
max(radar_meas_rate, ais_meas_rate))
time = start_time + timedelta(seconds=1)
while time < end_time:
truth.append(
GroundTruthState(transition_model.function(
truth[-1], noise=True, time_interval=timedelta(seconds=1)),
timestamp=time))
time += timedelta(seconds=1)
# Simulate measurements
# Specify measurement model for radar
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 (Same as for radar)
measurement_model_ais = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array(
[[sigma_meas_ais, 0],
[0, sigma_meas_ais]]))
# generate "radar" measurements
measurements_radar = []
measurements_ais = []
next_radar_meas_time = start_time
next_ais_meas_time = start_time
for state in truth:
# check whether we want to generate a measurement from this gt
if state.timestamp == next_radar_meas_time:
measurement = measurement_model_radar.function(state, noise=True)
measurements_radar.append(
Detection(measurement, timestamp=state.timestamp))
next_radar_meas_time += timedelta(seconds=radar_meas_rate)
if state.timestamp == next_ais_meas_time:
measurement = measurement_model_ais.function(state, noise=True)
measurements_ais.append(
Detection(measurement, timestamp=state.timestamp))
next_ais_meas_time += timedelta(seconds=ais_meas_rate)
if permanent_save:
save_folder_name = seed.__str__()
else:
save_folder_name = "temp"
save_folder = "../scenarios/scenario3/" + save_folder_name + "/"
# save the ground truth and the measurements for the radar and the AIS
store_object.store_object(truth, save_folder, "ground_truth.pk1")
store_object.store_object(measurements_radar, save_folder,
"measurements_radar.pk1")
store_object.store_object(measurements_ais, save_folder,
"measurements_ais.pk1")
store_object.store_object(start_time, save_folder, "start_time.pk1")
store_object.store_object(measurement_model_radar, save_folder,
"measurement_model_radar.pk1")
store_object.store_object(measurement_model_ais, save_folder,
"measurement_model_ais.pk1")
store_object.store_object(transition_model, save_folder,
"transition_model.pk1")
# where :math:`\omega` is set to 5 initially (but again, feel free to play around).
# %%
# We're going to need a :class:`~.Detection` type to
# store the detections, and a :class:`~.LinearGaussian` measurement model.
from stonesoup.types.detection import Detection
from stonesoup.models.measurement.linear import LinearGaussian
# %%
# The linear Gaussian measurement model is set up by indicating the number of dimensions in the
# state vector and the dimensions that are measured (so specifying :math:`H_k`) and the noise
# covariance matrix :math:`R`.
measurement_model = LinearGaussian(
ndim_state=4, # Number of state dimensions (position and velocity in 2D)
mapping=(0, 2), # Mapping measurement vector index to state index
noise_covar=np.array([
[5, 0], # Covariance matrix for Gaussian PDF
[0, 5]
]))
# %%
# Check the output is as we expect
measurement_model.matrix()
# %%
measurement_model.covar()
# %%
# Generate the measurements
measurements = []
for state in truth:
# load start_time
start_time = open_object.open_object("../scenarios/scenario2/start_time.pk1")
# same transition models (radar uses same as original)
transition_model_radar = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.01), ConstantVelocity(0.01)])
transition_model_ais = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.01), ConstantVelocity(0.01)])
# same measurement models as used when generating the measurements
# Specify measurement model for radar
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([
[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)
# 2. Generate Data
#MODELS
# Linear Gaussian transition model like in the 01-Kalman example
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(1), ConstantVelocity(1)))
# Use a Linear Gaussian measurement model like in 01-Kalman example
from stonesoup.models.measurement.linear import LinearGaussian
measurement_model = LinearGaussian(
ndim_state=4, # Number of state dimensions (position and velocity in 2D)
mapping=[0, 2], # Mapping measurement vector index to state index
noise_covar=np.diag([10, 10]) # Covariance matrix for Gaussian PDF
)
#SIMULATORS
# GROUND TRUTH SIMULATOR
# Before running the Detection Simulator, a Ground Truth reader/simulator must be used in order to generate
# Ground Truth Data which is used as an input to the Detection Simulator.
from stonesoup.types.state import GaussianState
from stonesoup.types.array import StateVector, CovarianceMatrix
# Arbitrary initial state of target.
initial_state = GaussianState(
StateVector([[0], [0], [0], [0]]),
CovarianceMatrix(np.diag([1000000, 10, 1000000, 10])))
transition_model = CombinedLinearGaussianTransitionModel(
(OrnsteinUhlenbeck(0.5, 1e-4), OrnsteinUhlenbeck(0.5, 1e-4)))
# %%
# Next we build a measurement model to describe the uncertainty on our detections. In this case, we
# are just using the measured position (`[0, 2]` dimensions of state space), assuming from ships
# |GNSS|_ receiver system with covariance of :math:`\begin{bmatrix}15&0\\0&15\end{bmatrix}`
#
# .. |GNSS| replace:: :abbr:`GNSS (Global Navigation Satellite System)`
# .. _GNSS: https://en.wikipedia.org/wiki/Satellite_navigation
import numpy as np
from stonesoup.models.measurement.linear import LinearGaussian
measurement_model = LinearGaussian(ndim_state=4,
mapping=[0, 2],
noise_covar=np.diag([15, 15]))
# %%
# With these models we can now create the predictor and updater components of the tracker, passing
# in the respective models.
from stonesoup.predictor.kalman import KalmanPredictor
predictor = KalmanPredictor(transition_model)
# %%
from stonesoup.updater.kalman import KalmanUpdater
updater = KalmanUpdater(measurement_model)
# %%
axm.set_ylim(0, 25)
axm.set_xlim(0, 25)
# Plot ground truth.
for truth in truths:
axm.plot(
[state.state_vector[0] for state in truth],
[state.state_vector[2] for state in truth],
linestyle="--",
)
# Generate measurements.
all_measurements = []
measurement_model = LinearGaussian(ndim_state=4,
mapping=(0, 2),
noise_covar=np.array([[0.75, 0], [0,
0.75]]))
prob_detect = 0.9 # 90% chance of detection.
for k in range(20):
measurement_set = set()
for truth in truths:
# Generate actual detection from the state with a 10% chance that no detection is received.
if np.random.rand() <= prob_detect:
measurement = measurement_model.function(truth[k], noise=True)
measurement_set.add(
TrueDetection(state_vector=measurement,
groundtruth_path=truth,
timestamp=truth[k].timestamp))
from stonesoup.measures import Mahalanobis
from stonesoup.models.transition.linear import (
CombinedLinearGaussianTransitionModel, ConstantVelocity)
from stonesoup.models.measurement.linear import LinearGaussian
from stonesoup.predictor.kalman import KalmanPredictor
from stonesoup.simulator.simple import MultiTargetGroundTruthSimulator, SimpleDetectionSimulator
from stonesoup.tracker.simple import MultiTargetTracker
from stonesoup.types.array import StateVector, CovarianceMatrix
from stonesoup.types.state import GaussianState
from stonesoup.updater.kalman import KalmanUpdater
# Models
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(1), ConstantVelocity(1)], seed=1)
measurement_model = LinearGaussian(4, [0, 2], np.diag([0.5, 0.5]), seed=2)
# Simulators
groundtruth_sim = MultiTargetGroundTruthSimulator(
transition_model=transition_model,
initial_state=GaussianState(
StateVector([[0], [0], [0], [0]]),
CovarianceMatrix(np.diag([1000, 10, 1000, 10]))),
timestep=datetime.timedelta(seconds=5),
number_steps=100,
birth_rate=0.2,
death_probability=0.05,
seed=3
)
detection_sim = SimpleDetectionSimulator(
groundtruth=groundtruth_sim,
from matplotlib import pyplot as plt
plt.rcParams['figure.figsize'] = (10, 6)
plt.style.use('seaborn-colorblind')
# 2. Generate Data
#MODELS
# Linear Gaussian transition model like in the 01-Kalman example
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel((ConstantVelocity(0.05), ConstantVelocity(0.05)))
# Use a Linear Gaussian measurement model like in 01-Kalman example
from stonesoup.models.measurement.linear import LinearGaussian
measurement_model = LinearGaussian(
4, # Number of state dimensions (position and velocity in 2D)
(0,2), # Mapping measurement vector index to state index
np.array([[10, 0], # Covariance matrix for Gaussian PDF
[0, 10]])
)
#SIMULATORS
# GROUND TRUTH SIMULATOR
# Before running the Detection Simulator, a Ground Truth reader/simulator must be used in order to generate
# Ground Truth Data which is used as an input to the Detection Simulator.
from stonesoup.types.state import GaussianState
from stonesoup.types.array import StateVector, CovarianceMatrix
# Arbitrary initial state of target.
initial_state=GaussianState( StateVector([[0], [0], [0], [0]]),
CovarianceMatrix(np.diag([1000000, 10, 1000000, 10]))),
