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)