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
# 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)
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")
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")
def test_sqrt_kalman():
measurement_model = LinearGaussian(ndim_state=2,
mapping=[0],
noise_covar=np.array([[0.04]]))
prediction = GaussianStatePrediction(
np.array([[-6.45], [0.7]]),
np.array([[4.1123, 0.0013], [0.0013, 0.0365]]))
sqrt_prediction = SqrtGaussianState(prediction.state_vector,
np.linalg.cholesky(prediction.covar))
measurement = Detection(np.array([[-6.23]]))
# Calculate evaluation variables
eval_measurement_prediction = GaussianMeasurementPrediction(
measurement_model.matrix() @ prediction.mean,
measurement_model.matrix() @ prediction.covar
@ measurement_model.matrix().T + measurement_model.covar(),
cross_covar=prediction.covar @ measurement_model.matrix().T)
kalman_gain = eval_measurement_prediction.cross_covar @ np.linalg.inv(
eval_measurement_prediction.covar)
eval_posterior = GaussianState(
prediction.mean + kalman_gain
@ (measurement.state_vector - eval_measurement_prediction.mean),
prediction.covar -
kalman_gain @ eval_measurement_prediction.covar @ kalman_gain.T)
# Test Square root form returns the same as standard form
updater = KalmanUpdater(measurement_model=measurement_model)
sqrt_updater = SqrtKalmanUpdater(measurement_model=measurement_model,
qr_method=False)
qr_updater = SqrtKalmanUpdater(measurement_model=measurement_model,
qr_method=True)
posterior = updater.update(
SingleHypothesis(prediction=prediction, measurement=measurement))
posterior_s = sqrt_updater.update(
SingleHypothesis(prediction=sqrt_prediction, measurement=measurement))
posterior_q = qr_updater.update(
SingleHypothesis(prediction=sqrt_prediction, measurement=measurement))
assert np.allclose(posterior_s.mean, eval_posterior.mean, 0, atol=1.e-14)
assert np.allclose(posterior_q.mean, eval_posterior.mean, 0, atol=1.e-14)
assert np.allclose(posterior.covar, eval_posterior.covar, 0, atol=1.e-14)
assert np.allclose(eval_posterior.covar,
posterior_s.sqrt_covar @ posterior_s.sqrt_covar.T,
0,
atol=1.e-14)
assert np.allclose(posterior.covar,
posterior_s.sqrt_covar @ posterior_s.sqrt_covar.T,
0,
atol=1.e-14)
assert np.allclose(posterior.covar,
posterior_q.sqrt_covar @ posterior_q.sqrt_covar.T,
0,
atol=1.e-14)
# I'm not sure this is going to be true in all cases. Keep in order to find edge cases
assert np.allclose(posterior_s.covar, posterior_q.covar, 0, atol=1.e-14)
# Next create a prediction with a covariance that will cause problems
prediction = GaussianStatePrediction(
np.array([[-6.45], [0.7]]), np.array([[1e24, 1e-24], [1e-24, 1e24]]))
sqrt_prediction = SqrtGaussianState(prediction.state_vector,
np.linalg.cholesky(prediction.covar))
posterior = updater.update(
SingleHypothesis(prediction=prediction, measurement=measurement))
posterior_s = sqrt_updater.update(
SingleHypothesis(prediction=sqrt_prediction, measurement=measurement))
posterior_q = qr_updater.update(
SingleHypothesis(prediction=sqrt_prediction, measurement=measurement))
# The new posterior will be
eval_posterior = GaussianState(
prediction.mean + kalman_gain
@ (measurement.state_vector - eval_measurement_prediction.mean),
np.array([[0.04, 0], [
0, 1e24
]])) # Accessed by looking through the Decimal() quantities...
# It's actually [0.039999999999 1e-48], [1e-24 1e24 + 1e-48]] ish
# Test that the square root form succeeds where the standard form fails
assert not np.allclose(posterior.covar, eval_posterior.covar, rtol=5.e-3)
assert np.allclose(posterior_s.sqrt_covar @ posterior_s.sqrt_covar.T,
eval_posterior.covar,
rtol=5.e-3)
assert np.allclose(posterior_q.sqrt_covar @ posterior_s.sqrt_covar.T,
eval_posterior.covar,
rtol=5.e-3)
def 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])
# 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])))
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(
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)
# 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:
# %%
# 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
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 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]
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)
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)
# %%
#
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
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,
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