# %%
# Create a Tracker
# ------------------------------------
# Now that we have setup our sensor platform, target and simulation we need to create a tracker. For this example we
# will use a Particle Filter as this enables us to handle the non-linear nature of the imaging sensor. In this example
# we will use an inflated constant noise model to account for target motion uncertainty.
#
# Note that we don't add a measurement model to the updater, this is because each sensor adds their measurement model to
# each detection they generate. The tracker handles this internally by checking for a measurement model with each
# detection it receives and applying only the relevant measurement model.
target_transition_model = CombinedLinearGaussianTransitionModel(
[ConstantVelocity(5), ConstantVelocity(5), ConstantVelocity(1)])
# First add a Particle Predictor
predictor = ParticlePredictor(target_transition_model)
# Now create a resampler and particle updater
resampler = SystematicResampler()
updater = ParticleUpdater(measurement_model=None,
resampler=resampler)
# Create a particle initiator
from stonesoup.initiator.simple import GaussianParticleInitiator, SinglePointInitiator
single_point_initiator = SinglePointInitiator(
GaussianState([[0], [-40], [2000], [0], [8000], [0]], np.diag([10000, 1000, 10000, 1000, 10000, 1000])),
None)
initiator = GaussianParticleInitiator(number_particles=500,
initiator=single_point_initiator)
# %% # 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. # To cope with sample sparsity we also include a resampler, in this instance # :class:`~.SystematicResampler`, which is passed to the updater. It should be noted that there are # many resampling schemes, and almost as many choices as to when to undertake resampling. The # systematic resampler is described in [#]_, and in what follows below resampling is undertaken # at each time-step. from stonesoup.predictor.particle import ParticlePredictor predictor = ParticlePredictor(transition_model) from stonesoup.resampler.particle import SystematicResampler resampler = SystematicResampler() from stonesoup.updater.particle import ParticleUpdater updater = ParticleUpdater(measurement_model, resampler) # %% # Initialise a prior # ^^^^^^^^^^^^^^^^^^ # To start we create a prior estimate. This is a set of :class:`~.Particle` and we sample from # Gaussian distribution (using the same parameters we had in the previous examples). from scipy.stats import multivariate_normal
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]
number_particles = 1000
samples = multivariate_normal.rvs(np.array([0, 1, 0, 1]),
np.diag([1.5, 0.5, 1.5, 0.5]),
size=number_particles)
# Note weights not used in particle flow, so value won't effect it.