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 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
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) pfk_updater = GromovFlowKalmanParticleUpdater( measurement_model) # By default, parallels EKF