def covar(self, timedelta=5, **kwargs):
delta_t = timedelta
Sigma = 5.0
covar = np.array([[np.power(delta_t, 4) / 4,
np.power(delta_t, 3) / 2],
[np.power(delta_t, 3) / 2,
np.power(delta_t, 2)]]) * np.power(Sigma, 2)
covar = block_diag(covar, covar)
return CovarianceMatrix(covar)
from stonesoup.types.array import StateVector, CovarianceMatrix
from stonesoup.types.state import GaussianState
from stonesoup.updater.kalman import KalmanUpdater
# Models
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(1), ConstantVelocity(1)], seed=1)
measurement_model = LinearGaussian(4, [0, 2], np.diag([0.5, 0.5]), seed=2)
# Simulators
groundtruth_sim = MultiTargetGroundTruthSimulator(
transition_model=transition_model,
initial_state=GaussianState(
StateVector([[0], [0], [0], [0]]),
CovarianceMatrix(np.diag([1000, 10, 1000, 10]))),
timestep=datetime.timedelta(seconds=5),
number_steps=100,
birth_rate=0.2,
death_probability=0.05,
seed=3
)
detection_sim = SimpleDetectionSimulator(
groundtruth=groundtruth_sim,
measurement_model=measurement_model,
meas_range=np.array([[-1, 1], [-1, 1]]) * 5000, # Area to generate clutter
detection_probability=0.9,
clutter_rate=1,
seed=4
)
def covar(self):
sigma = 50
cov = CovarianceMatrix([[np.power(sigma, 2), 0],
[0, np.power(sigma, 2)]])
return cov
pruning=True,
merge_threshold=merge_threshold,
merging=True)
# %%
# Now we initialize the Gaussian mixture at time k=0. In this implementation, the GM-PHD
# tracker knows the start state of the first 3 tracks that were created. After that it
# must pick up on new tracks and discard old ones. It is not necessary to provide the
# tracker with these start states, you can simply define the `tracks` as an empty set.
#
# Feel free to change the `state_vector` from the actual truth state vector to something
# else. This would mimic if the tracker was unsure about where the objects were originating.
from stonesoup.types.state import TaggedWeightedGaussianState
from stonesoup.types.track import Track
from stonesoup.types.array import CovarianceMatrix
covar = CovarianceMatrix(np.diag([10, 5, 10, 5]))
tracks = set()
for truth in start_truths:
new_track = TaggedWeightedGaussianState(state_vector=truth.state_vector,
covar=covar**2,
weight=0.25,
tag='birth',
timestamp=start_time)
tracks.add(Track(new_track))
# %%
# The hypothesier takes the current Gaussian mixture as a parameter. Here we will
# initialize it to use later.
reduced_states = set([track[-1] for track in tracks])
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
# R = \begin{bmatrix}
# \sigma_{\theta}^2 & 0 & 0 \\
# 0 & \sigma_{\phi}^2 & 0 \\
# 0 & 0 & \sigma_{r}^2
# \end{bmatrix}
#
# We now create our radar.
# Import a radar sensor model
from stonesoup.sensor.radar.radar import RadarElevationBearingRange
# First we need to configure a radar
# Generate a radar sensor with a suitable measurement accuracy
noise_covar = CovarianceMatrix(
np.array(np.diag([np.deg2rad(3)**2,
np.deg2rad(0.15)**2, 25**2])))
# this radar measures range with an accuracy of +/- 25m, and elevation accuracy +/- 3
# degrees and bearing accuracy of +/- 0.15 degrees
# The radar needs to be informed of where x, y, and z are in the target state space
radar_mapping = (0, 2, 4)
# Instantiate the radar
radar = RadarElevationBearingRange(ndim_state=6,
position_mapping=radar_mapping,
noise_covar=noise_covar)
# %%
# Attach the sensor to the platform
# ---------------------------------
# Now that we have created our radar sensor we need to mount the sensor onto the platform we have previously created.
# %%
# 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.
# Import a range rate bearing elevation capable radar
from stonesoup.sensor.radar.radar import RadarElevationBearingRangeRate
# Create a radar sensor
radar_noise_covar = CovarianceMatrix(np.diag(
np.array([np.deg2rad(3), # Elevation
np.deg2rad(3), # Bearing
100., # Range
25.]))) # Range Rate
# radar mountings
radar_mounting_offsets = StateVector([10, 0, 0]) # e.g. nose cone
radar_rotation_offsets = StateVector([0, 0, 0])
# Mount the radar onto the platform
radar = RadarElevationBearingRangeRate(ndim_state=6,
position_mapping=(0, 2, 4),
velocity_mapping=(1, 3, 5),
noise_covar=radar_noise_covar,
mounting_offset=radar_mounting_offsets,
rotation_offset=radar_rotation_offsets,
# %%
# Separate out the imports
import numpy as np
import datetime
# %%
# Initialise ground truth
# ^^^^^^^^^^^^^^^^^^^^^^^
# Here are some configurable parameters associated with the ground truth, e.g. defining where
# tracks are born and at what rate, death probability. This follows similar logic to the code
# in previous tutorial section :ref:`auto_tutorials/09_Initiators_&_Deleters:Simulating Multiple
# Targets`.
from stonesoup.types.array import StateVector, CovarianceMatrix
from stonesoup.types.state import GaussianState
initial_state_mean = StateVector([[0], [0], [0], [0]])
initial_state_covariance = CovarianceMatrix(np.diag([4, 0.5, 4, 0.5]))
timestep_size = datetime.timedelta(seconds=5)
number_of_steps = 20
birth_rate = 0.3
death_probability = 0.05
initial_state = GaussianState(initial_state_mean, initial_state_covariance)
# %%
# Create the transition model - default set to 2d nearly-constant velocity with small (0.05)
# variance.
from stonesoup.models.transition.linear import (
CombinedLinearGaussianTransitionModel, ConstantVelocity)
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.05), ConstantVelocity(0.05)])
# %%
reducer = GaussianMixtureReducer(prune_threshold=prune_threshold,
pruning=True,
merge_threshold=merge_threshold,
merging=True)
# %%
# Now we initialize the Gaussian mixture at time k=0. In this implementation, the GM-PHD
# tracker knows the start state of the first 3 tracks that were created. After that it
# must pick up on new tracks and discard old ones.
#
# Feel free to change the `state_vector` from the actual truth state vector to something
# else. This would mimic if the tracker did not know where the objects were originating.
from stonesoup.types.state import TaggedWeightedGaussianState
from stonesoup.types.track import Track
from stonesoup.types.array import CovarianceMatrix
covar = CovarianceMatrix(np.diag([10, 5, 10, 5]))
tracks = set()
for truth in start_truths:
new_track = TaggedWeightedGaussianState(state_vector=truth.state_vector,
covar=covar**2,
weight=0.25,
tag='birth',
timestamp=start_time)
tracks.add(Track(new_track))
reduced_states = None
# %%
# Run the Tracker
# ^^^^^^^^^^^^^^^
# tutorials.
# %%
# Track Initiation
# ****************
# For initialising tracks we will use a :class:`~.MultiMeasurementInitiator`, which allows our
# tracker to tentatively initiate tracks from unassociated measurements, and hold them within the
# initiator until they have survived for at least 10 frames. We also define a
# :class:`~.UpdateTimeStepsDeleter` deleter to be used by the initiator to delete tentative tracks
# that have not been associated to a measurement in the last 3 frames.
from stonesoup.types.state import GaussianState
from stonesoup.types.array import CovarianceMatrix, StateVector
from stonesoup.initiator.simple import MultiMeasurementInitiator
from stonesoup.deleter.time import UpdateTimeStepsDeleter
prior_state = GaussianState(StateVector(np.zeros((6,1))),
CovarianceMatrix(np.diag([100**2, 30**2, 100**2, 30**2, 100**2, 100**2])))
deleter_init = UpdateTimeStepsDeleter(time_steps_since_update=3)
initiator = MultiMeasurementInitiator(prior_state, measurement_model, deleter_init,
data_associator, updater, min_points=10)
# %%
# Track Deletion
# **************
# For confirmed tracks we used again a :class:`~.UpdateTimeStepsDeleter`, but this time configured
# to delete tracks after they have not bee associated to a measurement in the last 15 frames.
deleter = UpdateTimeStepsDeleter(time_steps_since_update=15)
# %%
# .. note::
#
# For more information on the above classes and how they operate you can refer to the Stone
# `Initiators & Deleters <https://stonesoup.readthedocs.io/en/latest/auto_tutorials/09_Initiators_&_Deleters.html>`_
def detections_gen(self):
detections = set()
current_time = datetime.now()
num_samps = 1000000
d = 10
omega = 50
fs = 20000
l = 1 # expected number of targets
window = 20000
windowm1 = window - 1
y = np.loadtxt(self.csv_path, delimiter=',')
L = len(y)
N = 9 * window
max_targets = 5
nbins = 128
bin_steps = [(math.pi + 0.1) / (2 * nbins), 2 * math.pi / nbins]
scans = []
winstarts = np.linspace(0, L - window, num=int(L / window), dtype=int)
for win in winstarts:
# initialise histograms
param_hist = np.zeros([max_targets, nbins, nbins])
order_hist = np.zeros([max_targets])
# initialise params
p_params = np.empty([max_targets, 2])
noise = noise_proposal(0)
[params, K] = proposal([], 0, p_params)
# calculate sinTy and cosTy
sinTy = np.zeros([9])
cosTy = np.zeros([9])
alpha = np.zeros([9])
yTy = 0
for k in range(0, 9):
for t in range(0, window):
sinTy[k] = sinTy[k] + math.sin(
2 * math.pi * t * omega / fs) * y[t + win, k]
cosTy[k] = cosTy[k] + math.cos(
2 * math.pi * t * omega / fs) * y[t + win, k]
yTy = yTy + y[t + win, k] * y[t + win, k]
sumsinsq = 0
sumcossq = 0
sumsincos = 0
for t in range(0, window):
sumsinsq = sumsinsq + math.sin(
2 * math.pi * t * omega / fs) * math.sin(
2 * math.pi * t * omega / fs)
sumcossq = sumcossq + math.cos(
2 * math.pi * t * omega / fs) * math.cos(
2 * math.pi * t * omega / fs)
sumsincos = sumsincos + math.sin(
2 * math.pi * t * omega / fs) * math.cos(
2 * math.pi * t * omega / fs)
old_logp = calc_acceptance(noise, params, K, omega, 1, d, y,
window, sinTy, cosTy, yTy, alpha,
sumsinsq, sumcossq, sumsincos, N, l)
n = 0
while n < num_samps:
p_noise = noise_proposal(noise)
[p_params, p_K,
Qratio] = proposal_func(params, K, p_params, max_targets)
if p_K != 0:
new_logp = calc_acceptance(p_noise, p_params, p_K, omega,
1, d, y, window, sinTy, cosTy,
yTy, alpha, sumsinsq, sumcossq,
sumsincos, N, l)
logA = new_logp - old_logp + np.log(Qratio)
# do a Metropolis-Hastings step
if logA > np.log(random.uniform(0, 1)):
old_logp = new_logp
params = copy.deepcopy(p_params)
K = copy.deepcopy(p_K)
for k in range(0, K):
bin_ind = [0, 0]
for l in range(0, 2):
edge = bin_steps[l]
while edge < params[k, l]:
edge += bin_steps[l]
bin_ind[l] += 1
if bin_ind[l] == nbins - 1:
break
param_hist[K - 1, bin_ind[0], bin_ind[1]] += 1
order_hist[K - 1] += 1
n += 1
# look for peaks in histograms
max_peak = 0
max_ind = 0
for ind in range(0, max_targets):
if order_hist[ind] > max_peak:
max_peak = order_hist[ind]
max_ind = ind
# FOR TESTING PURPOSES ONLY - SET max_ind = 0
max_ind = 0
# look for largest N peaks, where N corresponds to peak in the order histogram
# use divide-and-conquer quadrant-based approach
if max_ind == 0:
[unique_peak_inds1, unique_peak_inds2
] = np.unravel_index(param_hist[0, :, :].argmax(),
param_hist[0, :, :].shape)
num_peaks = 1
else:
order_ind = max_ind - 1
quadrant_factor = 2
nstart = 0
mstart = 0
nend = quadrant_factor
mend = quadrant_factor
peak_inds1 = [None] * 16
peak_inds2 = [None] * 16
k = 0
while quadrant_factor < 32:
max_quadrant = 0
quadrant_size = nbins / quadrant_factor
for n in range(nstart, nend):
for m in range(mstart, mend):
[ind1, ind2] = np.unravel_index(
param_hist[order_ind,
int(n * quadrant_size):int(
(n + 1) * quadrant_size - 1),
int(m * quadrant_size):int(
(m + 1) * quadrant_size -
1)].argmax(),
param_hist[order_ind,
int(n * quadrant_size):int(
(n + 1) * quadrant_size - 1),
int(m * quadrant_size):int(
(m + 1) * quadrant_size -
1)].shape)
peak_inds1[k] = int(ind1 + n * quadrant_size)
peak_inds2[k] = int(ind2 + m * quadrant_size)
if param_hist[order_ind, peak_inds1[k],
peak_inds2[k]] > max_quadrant:
max_quadrant = param_hist[order_ind,
peak_inds1[k],
peak_inds2[k]]
max_ind1 = n
max_ind2 = m
k += 1
quadrant_factor = 2 * quadrant_factor
# on next loop look for other peaks in the quadrant containing the highest peak
nstart = 2 * max_ind1
mstart = 2 * max_ind2
nend = 2 * (max_ind1 + 1)
mend = 2 * (max_ind2 + 1)
# determine unique peaks
unique_peak_inds1 = [None] * 16
unique_peak_inds2 = [None] * 16
unique_peak_inds1[0] = peak_inds1[0]
unique_peak_inds2[0] = peak_inds2[0]
num_peaks = 1
for n in range(0, 16):
flag_unique = 1
for k in range(0, num_peaks):
# check if peak is close to any other known peaks
if (unique_peak_inds1[k] - peak_inds1[n]) < 2:
if (unique_peak_inds2[k] - peak_inds2[n]) < 2:
# part of same peak (check if bin is taller)
if param_hist[order_ind, peak_inds1[n],
peak_inds2[n]] > param_hist[
order_ind,
unique_peak_inds1[k],
unique_peak_inds2[k]]:
unique_peak_inds1 = peak_inds1[n]
unique_peak_inds2 = peak_inds2[n]
flag_unique = 0
break
if flag_unique == 1:
unique_peak_inds1[num_peaks] = peak_inds1[n]
unique_peak_inds2[num_peaks] = peak_inds2[n]
num_peaks += 1
# Defining a detection
state_vector = StateVector([
unique_peak_inds2 * bin_steps[1],
unique_peak_inds1 * bin_steps[0]
]) # [Azimuth, Elevation]
covar = CovarianceMatrix(np.array([[1, 0],
[0, 1]])) # [[AA, AE],[AE, EE]]
measurement_model = LinearGaussian(ndim_state=4,
mapping=[0, 2],
noise_covar=covar)
current_time = current_time + timedelta(milliseconds=window)
detection = Detection(state_vector,
timestamp=current_time,
measurement_model=measurement_model)
detections = set([detection])
scans.append((current_time, detections))
# For every timestep
for scan in scans:
yield scan[0], scan[1]
def detections_gen(self):
detections = set()
current_time = datetime.now()
y = np.loadtxt(self.csv_path, delimiter=',')
L = len(y)
# frequency of sinusoidal signal
omega = 50
window = 20000
windowm1 = window - 1
thetavals = np.linspace(0, 2 * math.pi, num=400)
phivals = np.linspace(0, math.pi / 2, num=100)
# spatial locations of hydrophones
z = np.matrix(
'0 0 0; 0 10 0; 0 20 0; 10 0 0; 10 10 0; 10 20 0; 20 0 0; 20 10 0; 20 20 0'
)
N = 9 # No. of hydrophones
# steering vector
v = np.zeros(N, dtype=np.complex)
# directional unit vector
a = np.zeros(3)
scans = []
winstarts = np.linspace(0, L - window, num=int(L / window), dtype=int)
c = 1481 / (2 * omega * math.pi)
for t in winstarts:
# calculate covariance estimate
R = np.matmul(np.transpose(y[t:t + windowm1]), y[t:t + windowm1])
R_inv = np.linalg.inv(R)
maxF = 0
maxtheta = 0
maxfreq = 0
for theta in thetavals:
for phi in phivals:
# convert from spherical polar coordinates to cartesian
a[0] = math.cos(theta) * math.sin(phi)
a[1] = math.sin(theta) * math.sin(phi)
a[2] = math.cos(phi)
a = a / math.sqrt(np.sum(a * a))
for n in range(0, N):
phase = np.sum(a * np.transpose(z[n, ])) / c
v[n] = math.cos(phase) - math.sin(phase) * 1j
F = 1 / (
(window - N) * np.transpose(np.conj(v)) @ R_inv @ v)
if F > maxF:
maxF = F
maxtheta = theta
maxphi = phi
# Defining a detection
state_vector = StateVector([maxtheta,
maxphi]) # [Azimuth, Elevation]
covar = CovarianceMatrix(np.array([[1, 0],
[0, 1]])) # [[AA, AE],[AE, EE]]
measurement_model = LinearGaussian(ndim_state=4,
mapping=[0, 2],
noise_covar=covar)
current_time = current_time + timedelta(milliseconds=window)
detection = Detection(state_vector,
timestamp=current_time,
measurement_model=measurement_model)
detections = set([detection])
scans.append((current_time, detections))
# For every timestep
for scan in scans:
yield scan[0], scan[1]
found = False
for arg in args:
if (arg.find(flag) >= 0):
found = True
return arg.split(flag)[1]
if (found == False):
raise Exception('Required argument {} was not found'.format(flag))
args = sys.argv
data_file = get_arg(args, '--datafile=')
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(0.01), ConstantVelocity(0.01)])
covar = CovarianceMatrix(np.array([[1, 0], [0, 1]])) # [[AA, AF],[AF, FF]]
measurement_model = LinearGaussian(ndim_state=4,
mapping=[0, 2],
noise_covar=covar)
from stonesoup.predictor.kalman import KalmanPredictor
predictor = KalmanPredictor(transition_model)
from stonesoup.updater.kalman import KalmanUpdater
updater = KalmanUpdater(measurement_model)
from stonesoup.types.state import GaussianState
prior = GaussianState([[0.5], [0], [0.5], [0]],
np.diag([1, 0, 1, 0]),
timestamp=datetime.now())
from stonesoup.updater.kalman import ExtendedKalmanUpdater
from stonesoup.models.measurement.nonlinear import (
CartesianToElevationBearingRange)
from stonesoup.types.array import CovarianceMatrix
transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(1.0),
ConstantVelocity(1.0),
ConstantVelocity(1.0)])
# Model coords = elev, bearing, range. Angles in radians
meas_covar = np.diag(
[np.radians(np.sqrt(10.0))**2,
np.radians(0.6)**2, 3.14**2])
meas_covar_trk = CovarianceMatrix(1.0 * meas_covar)
meas_model = CartesianToElevationBearingRange(ndim_state=6,
mapping=np.array([0, 2, 4]),
noise_covar=meas_covar_trk)
predictor = ExtendedKalmanPredictor(transition_model)
updater = ExtendedKalmanUpdater(measurement_model=meas_model)
# %%
# Setup CSV reader & feeder
# -------------------------
# Setup the reader and feeder to read the GPS data in
# :download:`CSV file <../../demos/UAV_Rot.csv>`.
# This part uses 2 Stone Soup detector type of classes:
#
# - :class:`~.CSVGroundTruthReader` - reads our CSV file which contains: timestamp,
# latitude, longitude, altitude and other miscellaneous data.
