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 test_multi_transition_movable_move():
input_state_vector = StateVector([0, 1, 2.2, 78.6])
pre_state = State(input_state_vector, timestamp=None)
models = [CombinedLinearGaussianTransitionModel((ConstantVelocity(0), ConstantVelocity(0))),
ConstantTurn([0, 0], turn_rate=np.pi / 2)]
times = [timedelta(seconds=10), timedelta(seconds=10)]
movable = MultiTransitionMovable(states=pre_state,
position_mapping=[0, 2],
velocity_mapping=[1, 3],
transition_models=models,
transition_times=times,
)
assert movable.state.state_vector is input_state_vector
assert movable.state.timestamp is None
now = datetime.now()
movable.move(now)
assert movable.state.state_vector is input_state_vector
assert movable.state.timestamp is now
movable.move(None)
assert movable.state.state_vector is input_state_vector
assert movable.state.timestamp is now
movable.move(now + timedelta(seconds=10))
assert movable.state.state_vector is not input_state_vector
assert movable.state.timestamp is not now
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 test_platform_getitem():
timestamp = datetime.datetime.now()
state_before = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp)
cv_model = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0), ConstantVelocity(0), ConstantVelocity(0)))
platform = MovingPlatform(states=state_before,
transition_model=cv_model,
position_mapping=[0, 2, 4],
velocity_mapping=[1, 3, 5])
platform.move(timestamp + datetime.timedelta(seconds=1))
state_after = platform.state
assert platform[0] is state_before
assert platform[1] is state_after
def test_platform_orientation_2d(state, orientation):
model_1d = ConstantVelocity(0.0)
model_2d = CombinedLinearGaussianTransitionModel([model_1d, model_1d])
timestamp = datetime.datetime.now()
timediff = 2 # 2sec
new_timestamp = timestamp + datetime.timedelta(seconds=timediff)
platform_state = State(state, timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=model_2d,
position_mapping=[0, 2])
assert np.allclose(platform.orientation, orientation)
# moving with a constant velocity model should not change the orientation
platform.move(new_timestamp)
assert np.allclose(platform.orientation, orientation)
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 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")
# of 1d models and combines them in a linear Gaussian model of arbitrary dimension, :math:`D`.
#
# .. math::
# F_{k}^{D} &= \begin{bmatrix}
# F_k^{1} & & \mathbf{0} \\
# & \ddots & \\
# \mathbf{0} & & F_k^d \\
# \end{bmatrix}\\
# Q_{k}^{D} &= \begin{bmatrix}
# Q_k^{1} & & \mathbf{0} \\
# & \ddots & \\
# \mathbf{0} & & Q_k^d \\
# \end{bmatrix}
#
# We want a 2d simulation, so we'll do:
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.05), ConstantVelocity(0.05)])
# %%
# A 'truth path' is created starting at 0,0 moving to the NE
truth = GroundTruthPath([GroundTruthState([0, 1, 0, 1], timestamp=start_time)])
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)))
# %%
# Thus the ground truth is generated and we can plot the result
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(1, 1, 1)
def test_multi_transition():
transition_model1 = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0), ConstantVelocity(0)))
transition_model2 = ConstantTurn((0, 0), np.radians(4.5))
transition_models = [transition_model1, transition_model2]
transition_times = [
datetime.timedelta(seconds=10),
datetime.timedelta(seconds=20)
]
platform_state = State(state_vector=[[0], [1], [0], [0]],
timestamp=datetime.datetime.now())
platform = MultiTransitionMovingPlatform(
transition_models=transition_models,
transition_times=transition_times,
states=platform_state,
position_mapping=[0, 2],
sensors=None)
assert len(platform.transition_models) == 2
assert len(platform.transition_times) == 2
# Check that platform states length increases as platform moves
assert len(platform) == 1
time = datetime.datetime.now()
time += datetime.timedelta(seconds=1)
platform.move(timestamp=time)
assert len(platform) == 2
time += datetime.timedelta(seconds=1)
platform.move(timestamp=time)
assert len(platform) == 3
time += datetime.timedelta(seconds=1)
platform.move(timestamp=time)
assert len(platform) == 4
px, py = platform.position[0], platform.position[1]
# Starting transition model is index 0
assert platform.transition_index == 0
time += datetime.timedelta(seconds=7)
platform.move(timestamp=time)
x, y = platform.position[0], platform.position[1]
# Platform initially moves horizontally
assert x > px
assert np.allclose(y, py, atol=1e-6)
px, py = x, y
# Transition model changes after corresponding interval is done/ Next transition is left-turn
assert platform.transition_index == 1
time += datetime.timedelta(seconds=10)
platform.move(timestamp=time)
x, y = platform.position[0], platform.position[1]
# Platform starts turning left to 45 degrees
assert x > px
assert y > py
# Transition interval is not done. Next transition is left-turn
assert platform.transition_index == 1
time += datetime.timedelta(seconds=10)
platform.move(timestamp=time)
x, y = platform.position[0], platform.position[1]
px, py = x, y
# Platform turned left to 90 degrees
# Transition interval is done. Next transition is straight-on
assert platform.transition_index == 0
time += datetime.timedelta(seconds=10)
platform.move(timestamp=time)
x, y = platform.position[0], platform.position[1]
# Platform travelling vertically up
assert np.allclose(x, px, atol=1e-6)
assert y > py
# Next transition is left-turn
assert platform.transition_index == 1
# Add new transition model (right-turn) to list
transition_model3 = ConstantTurn((0, 0), np.radians(-9))
platform.transition_models.append(transition_model3)
platform.transition_times.append(datetime.timedelta(seconds=10))
# New model and transition interval are added to model list and to interval list
assert len(platform.transition_models) == 3
assert len(platform.transition_times) == 3
time += datetime.timedelta(seconds=20)
platform.move(timestamp=time)
# Platform turned left by 90 degrees (now travelling in -x direction)
px, py = platform.position[0], platform.position[1]
# Next transition is right-turn
assert platform.transition_index == 2
time += datetime.timedelta(seconds=10)
platform.move(timestamp=time)
x, y = platform.position[0], platform.position[1]
px, py = x, y
# Next transition straight-on, travelling vertically up again
assert platform.transition_index == 0
time += datetime.timedelta(seconds=10)
platform.move(timestamp=time)
x, y = platform.position[0], platform.position[1]
# Platform travelled vertically up
assert np.allclose(x, px, atol=1e-6)
assert y > py
# Next transition is left-turn
assert platform.transition_index == 1
def test_velocity_properties(velocity_mapping):
model_1d = ConstantVelocity(0.0)
model_3d = CombinedLinearGaussianTransitionModel(
[model_1d, model_1d, model_1d])
timestamp = datetime.datetime.now()
timediff = 2 # 2sec
new_timestamp = timestamp + datetime.timedelta(seconds=timediff)
platform_state = State(np.array([[2], [0], [2], [0], [0], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=model_3d,
position_mapping=[0, 2, 4])
old_position = platform.position
assert not platform.is_moving
assert np.array_equal(platform.velocity, StateVector([0, 0, 0]))
# check it doesn't move with timestamp = None
platform.move(timestamp=None)
assert np.array_equal(platform.position, old_position)
# check it doesn't move (as it has zero velocity)
platform.move(timestamp)
assert np.array_equal(platform.position, old_position)
platform.move(new_timestamp)
assert np.array_equal(platform.position, old_position)
platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4])
assert platform.is_moving
assert np.array_equal(platform.velocity, StateVector([1, 1, 1]))
old_position = platform.position
# check it doesn't move with timestamp = None
platform.move(None)
assert np.array_equal(platform.position, old_position)
with pytest.raises(AttributeError):
platform.move(timestamp)
# moving platform without velocity defined
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4])
with pytest.raises(AttributeError):
_ = platform.velocity
# pass in a velocity mapping
platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4],
velocity_mapping=velocity_mapping)
assert platform.is_moving
assert np.array_equal(platform.velocity, StateVector([1, 1, 1]))
old_position = platform.position
# check it doesn't move with timestamp = None
platform.move(None)
assert np.array_equal(platform.position, old_position)
with pytest.raises(AttributeError):
platform.move(timestamp)
# moving platform without velocity defined
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4],
velocity_mapping=velocity_mapping)
with pytest.raises(AttributeError):
_ = platform.velocity
from trackers.calc_cross_cov_estimate_error import calc_cross_cov_estimate_error
# load ground truth and the measurements
ground_truth = open_object.open_object(
"../scenarios/scenario2/ground_truth.pk1")
measurements_radar = open_object.open_object(
"../scenarios/scenario2/measurements_radar.pk1")
measurements_ais = open_object.open_object(
"../scenarios/scenario2/measurements_ais.pk1")
# 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,
# :math:`\mathrm{x}_k`, i.e. the state that we wish to estimate:
#
# .. math::
# \mathrm{x}_k = [x_k, \dot{x}_k, y_k, \dot{y}_k, w_k, h_k]
#
# where :math:`x_k, y_k` denote the pixel coordinates of the top-left corner of the bounding box
# containing an object, with :math:`\dot{x}_k, \dot{y}_k` denoting their respective rate of change,
# while :math:`w_k` and :math:`h_k` denote the width and height of the box, respectively.
#
# We assume that :math:`x_k` and :math:`y_k` move with nearly :class:`~.ConstantVelocity`, while
# :math:`w_k` and :math:`h_k` evolve according to a :class:`~.RandomWalk`.Using these assumptions,
# we proceed to construct our Stone Soup :class:`~.TransitionModel` as follows:
from stonesoup.models.transition.linear import (CombinedLinearGaussianTransitionModel,
ConstantVelocity, RandomWalk)
t_models = [ConstantVelocity(20**2), ConstantVelocity(20**2), RandomWalk(20**2), RandomWalk(20**2)]
transition_model = CombinedLinearGaussianTransitionModel(t_models)
# %%
# 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.sensor.radar import RadarRotatingBearingRange
from stonesoup.types.state import State
from stonesoup.platform.base import FixedPlatform, MultiTransitionMovingPlatform
from stonesoup.simulator.platform import PlatformDetectionSimulator
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel,\
ConstantVelocity, ConstantTurn
# Create locations for reference later
# Using Heathrow as origin
*heathrow, utm_zone, _ = utm.from_latlon(51.47, -0.4543)
# Use heathrow utm grid num as reference number for utm conversions later
manchester = utm.from_latlon(53.35, -2.280, utm_zone)
# Create transition models for moving platforms
transition_modelStraight = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0.01), ConstantVelocity(0.01), ConstantVelocity(0.01)))
transition_modelLeft = CombinedLinearGaussianTransitionModel((ConstantTurn(
(0.01, 0.01), np.radians(3)), ConstantVelocity(0.01)))
transition_modelRight = CombinedLinearGaussianTransitionModel((ConstantTurn(
(0.01, 0.01), np.radians(-3)), ConstantVelocity(0.01)))
# Create specific transition model for example moving platform
transition_models = [transition_modelStraight, transition_modelLeft]
transition_times = [
datetime.timedelta(seconds=160),
datetime.timedelta(seconds=20)
]
# List sensors in stationary platforms (sensor orientations are overwritten)
measurements = []
for state in truth:
x, y = multivariate_normal.rvs(state.state_vector.ravel(),
cov=np.diag([0.75, 0.75]))
measurements.append(
Detection(np.array([[x], [y]]), timestamp=state.timestamp))
# Plot the result
ax.scatter([state.state_vector[0, 0] for state in measurements],
[state.state_vector[1, 0] for state in measurements],
color='b')
fig
# Create Models and Kalman Filter
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0.05), ConstantVelocity(0.05)))
transition_model.matrix(time_interval=timedelta(seconds=1))
transition_model.covar(time_interval=timedelta(seconds=1))
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.75, 0], # Covariance matrix for Gaussian PDF
[0, 0.75]
]))
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
clutter = []
for n in range(1, 21):
clutter.append(set())
for _ in range(np.random.randint(10)):
x = uniform.rvs(0, 20)
y = uniform.rvs(0, 20)
clutter[-1].add(Clutter(
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
# * :math:`z` is Gaussian distributed around an altitude of :math:`\mathrm{9}km` with variance of :math:`\mathrm{0.1}km`
# * :math:`\dot{x}` is Gaussian distributed around :math:`\mathrm{100}ms^{-1}` with variance of :math:`\mathrm{50}ms^{-1}`
# * :math:`\dot{y}` is Gaussian distributed around :math:`\mathrm{100}ms^{-1}` with variance of :math:`\mathrm{50}ms^{-1}`
# * :math:`\dot{z}` is Gaussian distributed around :math:`\mathrm{0}ms^{-1}` with variance of :math:`\mathrm{1}ms^{-1}`
#
# We will also configure our simulator to randomly create and delete targets, based on a birth rate and death rate we
# specify. In this example we set the birth rate to be 0.10, i.e. on any given time step there is a 10% chance of a new
# target being initiated. We have set the death rate to 0.01, i.e. on any given time step there is a 1% chance that a
# target will be removed from the simulation.
#
# The above setup will provide a case which loosely approximates an air surveillance radar at an airport.
from stonesoup.simulator.simple import MultiTargetGroundTruthSimulator
# Set a constant velocity transition model for the targets
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.5),
ConstantVelocity(0.5),
ConstantVelocity(0.1)])
# Define the Gaussian State from which new targets are sampled on initialisation
initial_target_state = GaussianState(
StateVector([[0], [0], [0], [0], [9000], [0]]),
CovarianceMatrix(np.diag([2000, 50, 2000, 50, 100, 1])))
groundtruth_sim = MultiTargetGroundTruthSimulator(
transition_model=transition_model, # target transition model
initial_state=initial_target_state, # add our initial state for targets
timestep=timedelta(seconds=1), # time between measurements
number_steps=120, # 2 minute
birth_rate=0.10, # 10% chance of a new target being born
death_probability=0.01 # 1% chance of a target being killed
)
def test_base():
# Define time related variables
timestamp = datetime.datetime.now()
timediff = 2 # 2sec
new_timestamp = timestamp + datetime.timedelta(seconds=timediff)
# Define a static 2d platform and check it does not move
platform_state2d = State(np.array([[2], [2]]), timestamp)
platform = FixedPlatform(states=platform_state2d,
position_mapping=np.array([0, 1]))
platform.move(new_timestamp)
new_statevector = np.array([[2], [2]])
# Ensure 2d platform has not moved
assert (np.array_equal(platform.state.state_vector, new_statevector))
# Test to ensure platform time has updated
assert (platform.state.timestamp == new_timestamp)
assert np.array_equal(platform.velocity, StateVector([0, 0]))
assert platform.ndim == 2
assert not platform.is_moving
# Define a static 3d platform and check it does not move
platform_state3d = State(np.array([[2], [2], [2]]), timestamp)
platform = FixedPlatform(states=platform_state3d,
position_mapping=[0, 1, 2])
platform.move(new_timestamp)
new_statevector = np.array([[2], [2], [2]])
# Ensure 2d platform has not moved
assert np.array_equal(platform.state.state_vector, new_statevector)
assert np.array_equal(platform.velocity, StateVector([0, 0, 0]))
assert platform.ndim == 3
assert not platform.is_moving
# Define zero noise 2d constant velocity transition model
model_1d = ConstantVelocity(0.0)
model_2d = CombinedLinearGaussianTransitionModel([model_1d, model_1d])
# Define a 2d platform with constant velocity motion and test motion
platform_state2d = State(np.array([[2], [1], [2], [1]]), timestamp)
platform = MovingPlatform(states=platform_state2d,
transition_model=model_2d,
position_mapping=[0, 2])
platform.move(new_timestamp)
# Define expected platform location after movement
new_statevector = np.array([[4], [1], [4], [1]])
assert (np.array_equal(platform.state.state_vector, new_statevector))
assert np.array_equal(platform.velocity, StateVector([1, 1]))
assert platform.ndim == 2
assert platform.is_moving
# Define zero noise 3d constant velocity transition model
model_3d = CombinedLinearGaussianTransitionModel(
[model_1d, model_1d, model_1d])
# Define a 3d platform with constant velocity motion and test motion
platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=model_3d,
position_mapping=[0, 2, 4])
platform.move(new_timestamp)
# Define expected platform location in 3d after movement
new_statevector = np.array([[4], [1], [4], [1], [2], [1]])
assert (np.array_equal(platform.state.state_vector, new_statevector))
assert np.array_equal(platform.velocity, StateVector([1, 1, 1]))
assert platform.ndim == 3
assert platform.is_moving
# 0 & 1 & 0 & 0 & 0 & 0\\
# 0 & 0 & 1 & \triangle k & 0 & 0\\
# 0 & 0 & 0 & 1 & 0 & 0\\
# 0 & 0 & 0 & 0 & 1 & \triangle k \\
# 0 & 0 & 0 & 0 & 0 & 1\\
# \end{bmatrix}
# First import the Moving platform
from stonesoup.platform.base import MovingPlatform
# Define the initial platform position, in this case the origin
initial_loc = StateVector([[0], [0], [0], [50], [8000], [0]])
initial_state = State(initial_loc, start_time)
# Define transition model and position for 3D platform
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.), ConstantVelocity(0.), ConstantVelocity(0.)])
# create our fixed platform
sensor_platform = MovingPlatform(states=initial_state,
position_mapping=(0, 2, 4),
velocity_mapping=(1, 3, 5),
transition_model=transition_model)
# %%
# With our platform generated we now need to build a set of sensors which will be mounted onto the platform. In this
# case we will exploit a :class:`~.RadarElevationBearingRangeRate` and a :class:`~.PassiveElevationBearing` sensor
# (e.g. an optical sensor, which has no capability to directly measure range).
#
# First we will create a radar which is capable of measuring bearing (:math:`\phi`), elevation (:math:`\theta`), range
# (:math:`r`) and range-rate (:math:`\dot{r}`) of the target platform.
def test_setting_position():
timestamp = datetime.datetime.now()
platform_state = State(np.array([[2], [2], [0]]), timestamp)
model_1d = ConstantVelocity(0.0)
model_3d = CombinedLinearGaussianTransitionModel(
[model_1d, model_1d, model_1d])
platform = MovingPlatform(states=platform_state,
transition_model=model_3d,
position_mapping=[0, 1, 2])
with pytest.raises(AttributeError):
platform.position = [0, 0, 0]
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 1, 2])
assert np.array_equal(platform.position, StateVector([2, 2, 0]))
platform.position = StateVector([0, 0, 0])
assert np.array_equal(platform.position, StateVector([0, 0, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [2], [0]]), timestamp)
platform = FixedPlatform(states=platform_state, position_mapping=[0, 1, 2])
assert np.array_equal(platform.position, StateVector([2, 2, 0]))
platform.position = StateVector([0, 0, 0])
assert np.array_equal(platform.position, StateVector([0, 0, 0]))
assert np.array_equal(platform.state_vector, StateVector([0, 0, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=model_3d,
position_mapping=[0, 2, 4])
with pytest.raises(AttributeError):
platform.position = [0, 0, 0]
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform = FixedPlatform(states=platform_state, position_mapping=[0, 2, 4])
assert np.array_equal(platform.position, StateVector([2, 2, 2]))
platform.position = StateVector([0, 0, 1])
assert np.array_equal(platform.position, StateVector([0, 0, 1]))
assert np.array_equal(platform.state_vector,
StateVector([0, 1, 0, -1, 1, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]),
timestamp)
platform = MovingPlatform(states=platform_state,
transition_model=None,
position_mapping=[0, 2, 4])
assert np.array_equal(platform.position, StateVector([2, 2, 2]))
platform.position = StateVector([0, 0, 1])
assert np.array_equal(platform.position, StateVector([0, 0, 1]))
assert np.array_equal(platform.state_vector,
StateVector([0, 1, 0, -1, 1, 0]))
with pytest.raises(AttributeError):
platform.velocity = [0, 0, 0]
import numpy as np
from datetime import datetime, timedelta
start_time = datetime.now()
# %%
# Generate ground truth
# ^^^^^^^^^^^^^^^^^^^^^
#
# At the end of the tutorial we will plot the Gaussian mixtures. The ground truth Gaussian
# mixtures are stored in this list where each index refers to an instance in time and holds
# all ground truths at that time step.
truths_by_time = []
# Create transition model
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0.3), ConstantVelocity(0.3)))
from stonesoup.types.groundtruth import GroundTruthPath, GroundTruthState
start_time = datetime.now()
truths = set() # Truths across all time
current_truths = set() # Truths alive at current time
start_truths = set()
number_steps = 20
death_probability = 0.005
birth_probability = 0.2
# Initialize 3 truths. This can be changed to any number of truths you wish.
truths_by_time.append([])
for i in range(3):
x, y = initial_position = np.random.uniform(
-30, 30, 2) # Range [-30, 30] for x and y
# %%
# Creating the models and filters
# -------------------------------
# Now we begin to build our tracker from Stone Soup components. The first component we will build
# is a linear transition model. We create a two-dimensional transition model by combining two
# individual one-dimensional `Ornstein Uhlenbeck`_ models. This is similar to
# :class:`~.ConstantVelocity`, which has an additional :attr:`~.OrnsteinUhlenbeck.damping_coeff`
# which models decaying velocity (we assume vessels will slow down to zero over time, rather than
# continually speed up)
#
# .. _Ornstein Uhlenbeck: https://en.wikipedia.org/wiki/Ornstein%E2%80%93Uhlenbeck_process
from stonesoup.models.transition.linear import (
CombinedLinearGaussianTransitionModel, OrnsteinUhlenbeck)
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]))
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
from stonesoup.deleter.error import CovarianceBasedDeleter
from stonesoup.hypothesiser.distance import DistanceHypothesiser
from stonesoup.initiator.simple import MultiMeasurementInitiator
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
)
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
