def detections_gen(self):
detections_df = traj.df.loc[:, "geometry"].to_frame()
if traj.is_latlon:
detections_df.to_crs("EPSG:3395", inplace=True)
detections_df["x"] = [
row.geometry.coords[0][0] for _, row in detections_df.iterrows()
]
detections_df["y"] = [
row.geometry.coords[0][1] for _, row in detections_df.iterrows()
]
detections_df.drop(columns="geometry", inplace=True)
for time, row in detections_df.iterrows():
detection = Detection([row.x, row.y], timestamp=time)
yield time, {detection}
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 test_kalman_smoother(SmootherClass):
# First create a track from some detections and then smooth - check the output.
# Setup list of Detections
start = datetime.now()
times = [start + timedelta(seconds=i) for i in range(0, 5)]
measurements = [
np.array([[2.486559674128609]]),
np.array([[2.424165626519697]]),
np.array([[6.603176662762473]]),
np.array([[9.329099124074590]]),
np.array([[14.637975326666801]]),
]
detections = [
Detection(m, timestamp=timest)
for m, timest in zip(measurements, times)
]
# Setup models.
trans_model = ConstantVelocity(noise_diff_coeff=1)
meas_model = LinearGaussian(ndim_state=2,
mapping=[0],
noise_covar=np.array([[0.4]]))
# Tracking components
predictor = KalmanPredictor(transition_model=trans_model)
updater = KalmanUpdater(measurement_model=meas_model)
# Prior
cstate = GaussianState(np.ones([2, 1]), np.eye(2), timestamp=start)
track = Track()
for detection in detections:
# Predict
pred = predictor.predict(cstate, timestamp=detection.timestamp)
# form hypothesis
hypothesis = SingleHypothesis(pred, detection)
# Update
cstate = updater.update(hypothesis)
# write to track
track.append(cstate)
smoother = SmootherClass(transition_model=trans_model)
smoothed_track = smoother.smooth(track)
smoothed_state_vectors = [state.state_vector for state in smoothed_track]
# Verify Values
target_smoothed_vectors = [
np.array([[1.688813974839928], [1.267196351952188]]),
np.array([[3.307200214998506], [2.187167840595264]]),
np.array([[6.130402001958210], [3.308896367021604]]),
np.array([[9.821303658438408], [4.119557021638030]]),
np.array([[14.257730973981149], [4.594862462495096]])
]
assert np.allclose(smoothed_state_vectors, target_smoothed_vectors)
# Check that a prediction is smoothable and that no error chucked
# Also remove the transition model and use the one provided by the smoother
track[1] = GaussianStatePrediction(pred.state_vector,
pred.covar,
timestamp=pred.timestamp)
smoothed_track2 = smoother.smooth(track)
assert isinstance(smoothed_track2[1], GaussianStatePrediction)
# Check appropriate error chucked if not GaussianStatePrediction/Update
track[-1] = detections[-1]
with pytest.raises(TypeError):
smoother._prediction(track[-1])
def 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)
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(
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]
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)
eval_posterior_velocity = prediction.state_vector[[1, 3]] + \
beta/timediff.total_seconds() * (measurement.state_vector -
[0, 5]
]))
# %%
# Check the output is as we expect
measurement_model.matrix()
# %%
measurement_model.covar()
# %%
# Generate the measurements
measurements = []
for state in truth:
measurement = measurement_model.function(state, noise=True)
measurements.append(Detection(measurement, timestamp=state.timestamp))
# Plot the result
ax.scatter([state.state_vector[0] for state in measurements],
[state.state_vector[1] for state in measurements],
color='b')
fig
# %%
# At this stage you should have a moderately linear ground truth path (dotted line) with a series
# of simulated measurements overplotted (blue circles). Take a moment to fiddle with the numbers in
# :math:`Q` and :math:`R` to see what it does to the path and measurements.
# %%
# Construct a Kalman filter
# ^^^^^^^^^^^^^^^^^^^^^^^^^
#Plot the result
ax.plot([state.state_vector[0, 0] for state in truth],
[state.state_vector[1, 0] for state in truth],
color='g', linestyle="--")
from scipy.stats import multivariate_normal
from stonesoup.types.detection import Detection
measurementss = [set() for _ in range(20)]
for n, state in enumerate(truth):
if np.random.rand() <= 0.85: # Probability of detection
x, y = multivariate_normal.rvs(
state.state_vector.ravel(), cov=np.diag([0.25, 0.25]))
measurementss[n].add(Detection(
np.array([[x], [y]]), timestamp=state.timestamp))
# Plot the result
ax.scatter([state.state_vector[0, 0] for measurements in measurementss for state in measurements],
[state.state_vector[1, 0] for measurements in measurementss for state in measurements],
color='b')
fig
from scipy.stats import uniform
from stonesoup.types.detection import Clutter
clutter = []
for n in range(1, 21):
clutter.append(set())
for _ in range(np.random.randint(10)):
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]
from stonesoup.types.state import ParticleState
np.random.seed(2020)
start_time = datetime.datetime(2020, 1, 1)
truth = GroundTruthPath([
GroundTruthState([4, 4, 4, 4],
timestamp=start_time + datetime.timedelta(seconds=1))
])
measurement_model = CartesianToBearingRange(ndim_state=4,
mapping=(0, 2),
noise_covar=np.diag(
[np.radians(0.5), 1]))
measurement = Detection(measurement_model.function(truth.state, noise=True),
timestamp=truth.state.timestamp,
measurement_model=measurement_model)
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.05), ConstantVelocity(0.05)])
p_predictor = ParticlePredictor(transition_model)
pfk_predictor = ParticleFlowKalmanPredictor(
transition_model) # By default, parallels EKF
predictors = [p_predictor, p_predictor, pfk_predictor]
p_updater = ParticleUpdater(measurement_model)
f_updater = GromovFlowParticleUpdater(measurement_model)
pfk_updater = GromovFlowKalmanParticleUpdater(
measurement_model) # By default, parallels EKF
updaters = [p_updater, f_updater, pfk_updater]
[0, 1]])
)
# Specify measurement model for AIS (Same as for radar)
measurement_model_ais = LinearGaussian(
ndim_state=4,
mapping=(0, 2),
noise_covar=np.array([[1, 0],
[0, 1]])
)
# generate "radar" measurements
measurements_radar = []
for state in truth:
measurement = measurement_model_radar.function(state, noise=True)
measurements_radar.append(Detection(measurement, timestamp=state.timestamp))
# generate "AIS" measurements
measurements_AIS = []
for state in truth:
measurement = measurement_model_ais.function(state, noise=True)
measurements_AIS.append(Detection(measurement, timestamp=state.timestamp))
# save the ground truth and the measurements for the radar and the AIS
store_object.store_object(truth, "../scenarios/scenario2/ground_truth.pk1")
store_object.store_object(measurements_radar, "../scenarios/scenario2/measurements_radar.pk1")
store_object.store_object(measurements_AIS, "../scenarios/scenario2/measurements_ais.pk1")
store_object.store_object(start_time, "../scenarios/scenario2/start_time.pk1")
store_object.store_object(measurement_model_radar, "../scenarios/scenario2/measurement_model_radar.pk1")
store_object.store_object(measurement_model_ais, "../scenarios/scenario2/measurement_model_ais.pk1")
store_object.store_object(transition_model, "../scenarios/scenario2/transition_model.pk1")
#Plot the result
ax.plot([state.state_vector[0, 0] for state in truth],
[state.state_vector[1, 0] for state in truth],
linestyle="--")
from scipy.stats import multivariate_normal
from stonesoup.types.detection import Detection
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
def measurement():
return Detection(np.array([[-6.23]]),
timestamp=datetime.datetime.now() +
datetime.timedelta(seconds=1))
def test_track_metadata():
track = Track()
assert track.metadata == {}
assert not track.metadatas
track = Track(init_metadata={'colour': 'blue'})
assert track.metadata == {'colour': 'blue'}
assert not track.metadatas
state = Update(
hypothesis=SingleHypothesis(None, Detection(np.array([[0]]), metadata={'side': 'ally'}))
)
track.append(state)
assert track.metadata == {'colour': 'blue', 'side': 'ally'}
assert len(track.metadatas) == 1
assert track.metadata == track.metadatas[-1]
state = Update(
hypothesis=SingleHypothesis(None, Detection(np.array([[0]]), metadata={'side': 'enemy'}))
)
track.append(state)
assert track.metadata == {'colour': 'blue', 'side': 'enemy'}
assert len(track.metadatas) == 2
state = Update(
hypothesis=SingleHypothesis(None, Detection(np.array([[0]]), metadata={'colour': 'red'}))
)
track[0] = state
assert track.metadata == track.metadatas[-1] == {'colour': 'red', 'side': 'enemy'}
assert len(track.metadatas) == 2
assert track.metadatas[0] == {'colour': 'red'}
state = Update(
hypothesis=SingleHypothesis(None, Detection(np.array([[0]]), metadata={'speed': 'fast'}))
)
track.insert(1, state)
assert track.metadata == {'colour': 'red', 'side': 'enemy', 'speed': 'fast'}
assert len(track.metadatas) == 3
assert track.metadatas[0] == {'colour': 'red'}
assert track.metadatas[1] == {'colour': 'red', 'speed': 'fast'}
assert track.metadatas[2] == {'colour': 'red', 'side': 'enemy', 'speed': 'fast'}
state = Update(
hypothesis=SingleHypothesis(None, Detection(np.array([[0]]), metadata={'size': 'small'}))
)
track.insert(-1, state)
assert track.metadata == {'colour': 'red', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
assert len(track.metadatas) == 4
assert track.metadatas[0] == {'colour': 'red'}
assert track.metadatas[1] == {'colour': 'red', 'speed': 'fast'}
assert track.metadatas[2] == {'colour': 'red', 'speed': 'fast', 'size': 'small'}
assert track.metadatas[3] == \
{'colour': 'red', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
state = Update(
hypothesis=SingleHypothesis(None, Detection(np.array([[0]]), metadata={'colour': 'black'}))
)
track.insert(-100, state)
assert track.metadata == {'colour': 'red', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
assert len(track.metadatas) == 5
assert track.metadatas[0] == {'colour': 'black'}
assert track.metadatas[1] == {'colour': 'red'}
assert track.metadatas[2] == {'colour': 'red', 'speed': 'fast'}
assert track.metadatas[3] == {'colour': 'red', 'size': 'small', 'speed': 'fast'}
assert track.metadatas[4] == \
{'colour': 'red', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
state = Update(
hypothesis=SingleHypothesis(None, Detection(np.array([[0]]), metadata={'colour': 'black'}))
)
track.insert(100, state)
assert track.metadata == {'colour': 'black', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
assert len(track.metadatas) == 6
assert track.metadatas[0] == {'colour': 'black'}
assert track.metadatas[1] == {'colour': 'red'}
assert track.metadatas[2] == {'colour': 'red', 'speed': 'fast'}
assert track.metadatas[3] == {'colour': 'red', 'size': 'small', 'speed': 'fast'}
assert track.metadatas[4] == \
{'colour': 'red', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
assert track.metadatas[5] == \
{'colour': 'black', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
state = Update(
hypothesis=SingleHypothesis(None, Detection(np.array([[0]]), metadata={'colour': 'green'}))
)
track.append(state)
assert track.metadata == {'colour': 'green', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
assert len(track.metadatas) == 7
assert track.metadatas[0] == {'colour': 'black'}
assert track.metadatas[1] == {'colour': 'red'}
assert track.metadatas[2] == {'colour': 'red', 'speed': 'fast'}
assert track.metadatas[3] == {'colour': 'red', 'size': 'small', 'speed': 'fast'}
assert track.metadatas[4] == \
{'colour': 'red', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
assert track.metadatas[5] == \
{'colour': 'black', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
assert track.metadatas[6] == \
{'colour': 'green', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
state = Update(
hypothesis=SingleHypothesis(None, Detection(np.array([[0]]), metadata={'colour': 'white'}))
)
track[-2] = state
assert track.metadata == {'colour': 'green', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
assert len(track.metadatas) == 7
assert track.metadatas[0] == {'colour': 'black'}
assert track.metadatas[1] == {'colour': 'red'}
assert track.metadatas[2] == {'colour': 'red', 'speed': 'fast'}
assert track.metadatas[3] == {'colour': 'red', 'size': 'small', 'speed': 'fast'}
assert track.metadatas[4] == \
{'colour': 'red', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
assert track.metadatas[5] == \
{'colour': 'white', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
assert track.metadatas[6] == \
{'colour': 'green', 'side': 'enemy', 'speed': 'fast', 'size': 'small'}
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")
temp = sensormessungen[i]
cov = np.linalg.inv(measurement_model.covar())
mean += cov @ state.state_vector
'''
# xOffset = 50 * np.random.normal(-1, 1, 1)
# yOffset = 50 * np.random.normal(-1, 1, 1)
x = state.state_vector[0, 0]
y = state.state_vector[1, 0]
mean = [x, y]
cov = [[2500, 0], [0, 2500]]
xDet, yDet = np.random.multivariate_normal(mean, cov)
x_Offsets.append(xDet - x)
measurements.append(Detection(np.array([[xDet], [yDet]]), timestamp=state.timestamp))
# measurements.append(Detection(
# np.array([[x] + xOffset, [y] + yOffset]), 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='black', s=10)
from tutorienklassen import PCWAModel
transition_model = PCWAModel()
transition_model.matrix()
transition_model.covar()
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]
sensor_y = 0
measurement_model = CartesianToBearingRange(
ndim_state=4,
mapping=(0, 2),
noise_covar=np.diag([np.radians(0.2), 1]),
translation_offset=np.array([[sensor_x], [sensor_y]]))
# %%
# Populate the measurement array
measurements = []
for state in truth:
measurement = measurement_model.function(state, noise=True)
measurements.append(
Detection(measurement,
timestamp=state.timestamp,
measurement_model=measurement_model))
# %%
# Plot those measurements
plotter.plot_measurements(measurements, [0, 2])
plotter.fig
# %%
# Set up the particle filter
# ^^^^^^^^^^^^^^^^^^^^^^^^^^
# Analogously to the Kalman family, we create a :class:`~.ParticlePredictor` and a
# :class:`~.ParticleUpdater` which take responsibility for the predict and update steps
# respectively. These require a :class:`~.TransitionModel` and :class:`~.MeasurementModel` as
# before.
from stonesoup.functions import cart2pol, pol2cart
measurements = []
sensor_x = 10
sensor_y = 0
for state in truth:
x = state.state_vector[0, 0]
y = state.state_vector[1, 0]
delta_x = (x - sensor_x)
delta_y = (y - sensor_y)
#rho, phi = multivariate_normal.rvs(
rho, phi = np.random.multivariate_normal(cart2pol(delta_x, delta_y),
np.diag([1, np.radians(0.2)]))
# Special Bearing type used to allow difference in angle calculations
measurements.append(
Detection(np.array([[Bearing(phi)], [rho]]),
timestamp=state.timestamp))
# Plot the result (back in cart.)
x, y = pol2cart(np.hstack(state.state_vector[1, 0] for state in measurements),
np.hstack(state.state_vector[0, 0] for state in measurements))
ax.scatter(x + sensor_x, y + sensor_y, color='b')
fig
plt.polar([state.state_vector[0, 0] for state in measurements],
[state.state_vector[1, 0] for state in measurements])
# Create Models and Extended Kalman Filter
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel(
(ConstantVelocity(0.1), ConstantVelocity(0.1)))
[state.state_vector[1, 0] for state in truth],
linestyle="--",
color="grey")
"""Erstellen der Messungen"""
measurements = []
for state in truth:
if state.timestamp % 5 == 0:
x = state.state_vector[0, 0]
y = state.state_vector[1, 0]
mean = [x, y]
cov = [[2500, 0], [0, 2500]]
xDet, yDet = np.random.multivariate_normal(mean, cov)
measurements.append(
Detection(np.array([[xDet], [yDet]]), timestamp=state.timestamp))
# Plot
ax.scatter([state.state_vector[0, 0] for state in measurements],
[state.state_vector[1, 0] for state in measurements],
color='black',
s=10)
"""Komponenten initiieren"""
transition_model = PCWAModel()
predictor = SdfKalmanPredictor(transition_model)
measurement_model = SDFMessmodell(
4, # Dimensionen (Position and Geschwindigkeit in 2D)
(0, 2), # Mapping
)
updater = SDFUpdater(measurement_model)
from stonesoup.functions import cart2pol, pol2cart
measurements = []
sensor_x = 10
sensor_y = 0
for state in truth:
x = state.state_vector[0, 0]
y = state.state_vector[1, 0]
delta_x = (x - sensor_x)
delta_y = (y - sensor_y)
#rho, phi = multivariate_normal.rvs(
rho, phi = np.random.multivariate_normal(
cart2pol(delta_x, delta_y),
np.diag([1, np.radians(0.2)]))
# Special Bearing type used to allow difference in angle calculations
measurements.append(Detection(
np.array([[Bearing(phi)], [rho]]), timestamp=state.timestamp))
# Plot the result (back in cart.)
x, y = pol2cart(
np.hstack(state.state_vector[1, 0] for state in measurements),
np.hstack(state.state_vector[0, 0] for state in measurements))
ax.scatter(x + sensor_x,
y + sensor_y,
color='b')
#%%
# Create Models and Extended Kalman Filter
#%%
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity
transition_model = CombinedLinearGaussianTransitionModel((ConstantVelocity(0.1), ConstantVelocity(0.1)))
#%%
