from stonesoup.plotter import Plotter
plotter = Plotter()
plotter.plot_ground_truths(truth, [0, 2])
# %%
# Initialise the bearing, range sensor using the appropriate measurement model.
from stonesoup.models.measurement.nonlinear import CartesianToBearingRange
from stonesoup.types.detection import Detection
sensor_x = 50
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
# \sqrt{(x-x_p)^2 + (y-y_p)^2}
# \end{bmatrix}
#
# where :math:`x_p,y_p` is the 2d Cartesian position of the sensor and :math:`x,y` that of the
# target. Note also that the arctan function has to resolve the quadrant ambiguity and so is
# implemented as the :class:`~.numpy.arctan2`:math:`(y,x)` function in Python.
from stonesoup.models.measurement.nonlinear import CartesianToBearingRange
sensor_x = 50 # Placing the sensor off-centre
sensor_y = 0
measurement_model = CartesianToBearingRange(
ndim_state=4,
mapping=(0, 2),
noise_covar=np.diag([np.radians(0.2),
1]), # Covariance matrix. 0.2 degree variance in
# bearing and 1 metre in range
translation_offset=np.array([[sensor_x], [sensor_y]
]) # Offset measurements to location of
# sensor in cartesian.
)
# %%
# We create a set of detections using this sensor model.
from stonesoup.types.detection import Detection
measurements = []
for state in truth:
measurement = measurement_model.function(state, noise=True)
measurements.append(Detection(measurement, timestamp=state.timestamp))
# %%
[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)))
from stonesoup.predictor.kalman import ExtendedKalmanPredictor
predictor = ExtendedKalmanPredictor(transition_model)
from stonesoup.models.measurement.nonlinear import CartesianToBearingRange
measurement_model = CartesianToBearingRange(
4, # Number of state dimensions (position and velocity in 2D)
(0, 2), # Mapping measurement vector index to state index
np.diag([np.radians(0.2), 1]), # Covariance matrix for Gaussian PDF
translation_offset=np.array([[sensor_x], [sensor_y]
]) # Location of sensor in cartesian.
)
from stonesoup.updater.kalman import ExtendedKalmanUpdater
updater = ExtendedKalmanUpdater(measurement_model)
# Running the Extended Kalman Filter
from stonesoup.types.state import GaussianState
prior = GaussianState([[0], [1], [0], [1]],
np.diag([1, 1, 1, 1]),
timestamp=start_time)
from stonesoup.types.hypothesis import SingleHypothesis
plotter = Plotter()
plotter.plot_ground_truths(truth, [0, 2])
# %%
# Simulate the measurement
# ^^^^^^^^^^^^^^^^^^^^^^^^
#
from stonesoup.models.measurement.nonlinear import CartesianToBearingRange
# Sensor position
sensor_x = 50
sensor_y = 0
# Make noisy measurement (with bearing variance = 0.2 degrees).
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]]))
# %%
from stonesoup.types.detection import Detection
# Make sensor that produces the noisy measurements.
measurements = []
for state in truth:
measurement = measurement_model.function(state, noise=True)
measurements.append(
Detection(measurement,
timestamp=state.timestamp,
measurement_model=measurement_model))
from stonesoup.updater.particle import ParticleUpdater, GromovFlowParticleUpdater, \
GromovFlowKalmanParticleUpdater
from stonesoup.types.particle import Particle
from stonesoup.types.numeric import Probability
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)