def test_multi_transition_movable_move(): input_state_vector = StateVector([0, 1, 2.2, 78.6]) pre_state = State(input_state_vector, timestamp=None) models = [CombinedLinearGaussianTransitionModel((ConstantVelocity(0), ConstantVelocity(0))), ConstantTurn([0, 0], turn_rate=np.pi / 2)] times = [timedelta(seconds=10), timedelta(seconds=10)] movable = MultiTransitionMovable(states=pre_state, position_mapping=[0, 2], velocity_mapping=[1, 3], transition_models=models, transition_times=times, ) assert movable.state.state_vector is input_state_vector assert movable.state.timestamp is None now = datetime.now() movable.move(now) assert movable.state.state_vector is input_state_vector assert movable.state.timestamp is now movable.move(None) assert movable.state.state_vector is input_state_vector assert movable.state.timestamp is now movable.move(now + timedelta(seconds=10)) assert movable.state.state_vector is not input_state_vector assert movable.state.timestamp is not now
def __init__(self, start_time, prior: GaussianState, sigma_process_radar=0.01, sigma_process_ais=0.01, sigma_meas_radar=3, sigma_meas_ais=1): """ :param measurements_radar: :param measurements_ais: :param start_time: :param prior: :param sigma_process_radar: :param sigma_process_ais: :param sigma_meas_radar: :param sigma_meas_ais: """ self.start_time = start_time # transition models (process models) self.transition_model_radar = CombinedLinearGaussianTransitionModel([ ConstantVelocity(sigma_process_radar), ConstantVelocity(sigma_process_radar) ]) self.transition_model_ais = CombinedLinearGaussianTransitionModel([ ConstantVelocity(sigma_process_ais), ConstantVelocity(sigma_process_ais) ]) # Specify measurement model for radar self.measurement_model_radar = LinearGaussian( ndim_state=4, # number of state dimensions mapping=(0, 2), # mapping measurement vector index to state index noise_covar=np.array([ [sigma_meas_radar, 0], # covariance matrix for Gaussian PDF [0, sigma_meas_radar] ])) # Specify measurement model for AIS self.measurement_model_ais = LinearGaussian(ndim_state=4, mapping=(0, 2), noise_covar=np.array( [[sigma_meas_ais, 0], [0, sigma_meas_ais]])) # specify predictors self.predictor_radar = KalmanPredictor(self.transition_model_radar) self.predictor_ais = KalmanPredictor(self.transition_model_ais) # specify updaters self.updater_radar = KalmanUpdater(self.measurement_model_radar) self.updater_ais = KalmanUpdater(self.measurement_model_ais) # create prior, both trackers use the same starting point self.prior_radar = prior self.prior_ais = prior
def __init__(self, measurements_radar, measurements_ais, start_time, prior: GaussianState, sigma_process=0.01, sigma_meas_radar=3, sigma_meas_ais=1): """ :param measurements_radar: :param measurements_ais: :param start_time: :param prior: :param sigma_process: :param sigma_meas_radar: :param sigma_meas_ais: """ # measurements and start time self.measurements_radar = measurements_radar self.measurements_ais = measurements_ais self.start_time = start_time # transition model self.transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(sigma_process), ConstantVelocity(sigma_process)]) # same measurement models as used when generating the measurements # Specify measurement model for radar self.measurement_model_radar = LinearGaussian( ndim_state=4, # number of state dimensions mapping=(0, 2), # mapping measurement vector index to state index noise_covar=np.array([ [sigma_meas_radar, 0], # covariance matrix for Gaussian PDF [0, sigma_meas_radar] ])) # Specify measurement model for AIS self.measurement_model_ais = LinearGaussian(ndim_state=4, mapping=(0, 2), noise_covar=np.array( [[sigma_meas_ais, 0], [0, sigma_meas_ais]])) # specify predictor self.predictor = KalmanPredictor(self.transition_model) # specify updaters self.updater_radar = KalmanUpdater(self.measurement_model_radar) self.updater_ais = KalmanUpdater(self.measurement_model_ais) # create prior todo move later and probably rename self.prior = prior
def test_platform_getitem(): timestamp = datetime.datetime.now() state_before = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp) cv_model = CombinedLinearGaussianTransitionModel( (ConstantVelocity(0), ConstantVelocity(0), ConstantVelocity(0))) platform = MovingPlatform(states=state_before, transition_model=cv_model, position_mapping=[0, 2, 4], velocity_mapping=[1, 3, 5]) platform.move(timestamp + datetime.timedelta(seconds=1)) state_after = platform.state assert platform[0] is state_before assert platform[1] is state_after
def test_multi_transition_movable_errors(): # First check no error models = [ConstantVelocity(0), ConstantTurn(0, np.pi/2)] now = datetime.now() times = [timedelta(seconds=10), timedelta(seconds=10)] _ = MultiTransitionMovable(states=State(StateVector([0, 0, 0, 0, 0, 0])), position_mapping=[0, 2, 4], velocity_mapping=[1, 2, 5], transition_models=models, transition_times=times, ) with pytest.raises(AttributeError, match='transition_models and transition_times must be same length'): _ = MultiTransitionMovable(states=State(StateVector([0, 0, 0, 0, 0, 0])), position_mapping=[0, 2, 4], velocity_mapping=[1, 2, 5], transition_models=[models[0]], transition_times=times, ) with pytest.raises(AttributeError, match='transition_models and transition_times must be same length'): _ = MultiTransitionMovable(states=State(StateVector([0, 0, 0, 0, 0, 0])), position_mapping=[0, 2, 4], velocity_mapping=[1, 2, 5], transition_models=models, transition_times=[now], )
def test_platform_orientation_2d(state, orientation): model_1d = ConstantVelocity(0.0) model_2d = CombinedLinearGaussianTransitionModel([model_1d, model_1d]) timestamp = datetime.datetime.now() timediff = 2 # 2sec new_timestamp = timestamp + datetime.timedelta(seconds=timediff) platform_state = State(state, timestamp) platform = MovingPlatform(states=platform_state, transition_model=model_2d, position_mapping=[0, 2]) assert np.allclose(platform.orientation, orientation) # moving with a constant velocity model should not change the orientation platform.move(new_timestamp) assert np.allclose(platform.orientation, orientation)
#from IPython import get_ipython #get_ipython().run_line_magic('matplotlib', 'inline') import matplotlib from matplotlib import pyplot as plt plt.rcParams['figure.figsize'] = (10, 6) plt.style.use('seaborn-colorblind') # 2. Generate Data #MODELS # Linear Gaussian transition model like in the 01-Kalman example from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity transition_model = CombinedLinearGaussianTransitionModel( (ConstantVelocity(1), ConstantVelocity(1))) # Use a Linear Gaussian measurement model like in 01-Kalman example from stonesoup.models.measurement.linear import LinearGaussian measurement_model = LinearGaussian( ndim_state=4, # Number of state dimensions (position and velocity in 2D) mapping=[0, 2], # Mapping measurement vector index to state index noise_covar=np.diag([10, 10]) # Covariance matrix for Gaussian PDF ) #SIMULATORS # GROUND TRUTH SIMULATOR # Before running the Detection Simulator, a Ground Truth reader/simulator must be used in order to generate # Ground Truth Data which is used as an input to the Detection Simulator.
def generate_scenario_1(seed=1996, permanent_save=True, sigma_process=0.01, sigma_meas_radar=3, sigma_meas_ais=1): """ Generates scenario 1. Todo define scenario 1 :param seed: :param permanent_save: :param sigma_process: :param sigma_meas_radar: :param sigma_meas_ais: :return: """ # specify seed to be able repeat example start_time = datetime.now() np.random.seed(seed) # combine two 1-D CV models to create a 2-D CV model transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(sigma_process), ConstantVelocity(sigma_process)]) # starting at 0,0 and moving NE truth = GroundTruthPath( [GroundTruthState([0, 1, 0, 1], timestamp=start_time)]) # generate truth using transition_model and noise for k in range(1, 21): truth.append( GroundTruthState(transition_model.function( truth[k - 1], noise=True, time_interval=timedelta(seconds=1)), timestamp=start_time + timedelta(seconds=k))) # Simulate measurements # Specify measurement model for radar measurement_model_radar = LinearGaussian( ndim_state=4, # number of state dimensions mapping=(0, 2), # mapping measurement vector index to state index noise_covar=np.array([ [sigma_meas_radar, 0], # covariance matrix for Gaussian PDF [0, sigma_meas_radar] ])) # Specify measurement model for AIS measurement_model_ais = LinearGaussian(ndim_state=4, mapping=(0, 2), noise_covar=np.array( [[sigma_meas_ais, 0], [0, sigma_meas_ais]])) # generate "radar" measurements measurements_radar = [] for state in truth: measurement = measurement_model_radar.function(state, noise=True) measurements_radar.append( Detection(measurement, timestamp=state.timestamp)) # generate "AIS" measurements measurements_ais = [] state_num = 0 for state in truth: state_num += 1 if not state_num % 2: # measurement every second time step measurement = measurement_model_ais.function(state, noise=True) measurements_ais.append( Detection(measurement, timestamp=state.timestamp)) if permanent_save: save_folder_name = seed.__str__() else: save_folder_name = "temp" save_folder = "../scenarios/scenario1/" + save_folder_name + "/" # save the ground truth and the measurements for the radar and the AIS store_object.store_object(truth, save_folder, "ground_truth.pk1") store_object.store_object(measurements_radar, save_folder, "measurements_radar.pk1") store_object.store_object(measurements_ais, save_folder, "measurements_ais.pk1") store_object.store_object(start_time, save_folder, "start_time.pk1") store_object.store_object(measurement_model_radar, save_folder, "measurement_model_radar.pk1") store_object.store_object(measurement_model_ais, save_folder, "measurement_model_ais.pk1") store_object.store_object(transition_model, save_folder, "transition_model.pk1")
def generate_scenario_3(seed=1996, permanent_save=True, radar_meas_rate=1, ais_meas_rate=5, sigma_process=0.01, sigma_meas_radar=3, sigma_meas_ais=1, timesteps=20): """ Generates scenario 3. Scenario 3 consists of radar and ais measurements with different sampling rate. The sampling rate is specified in the input params. A groundtruth is generated for each second. :param seed: :param permanent_save: :param radar_meas_rate: :param ais_meas_rate: :param sigma_process: :param sigma_meas_radar: :param sigma_meas_ais: :param timesteps: The amount of measurements from the slowest sensor :return: Nothing. Saves the scenario to a specified folder """ start_time = datetime.now() # specify seed to be able to repeat the example np.random.seed(seed) # combine two 1-D CV models to create a 2-D CV model transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(sigma_process), ConstantVelocity(sigma_process)]) # starting at 0,0 and moving NE truth = GroundTruthPath( [GroundTruthState([0, 1, 0, 1], timestamp=start_time)]) # generate truth using transition_model and noise end_time = start_time + timedelta(seconds=timesteps * max(radar_meas_rate, ais_meas_rate)) time = start_time + timedelta(seconds=1) while time < end_time: truth.append( GroundTruthState(transition_model.function( truth[-1], noise=True, time_interval=timedelta(seconds=1)), timestamp=time)) time += timedelta(seconds=1) # Simulate measurements # Specify measurement model for radar measurement_model_radar = LinearGaussian( ndim_state=4, # number of state dimensions mapping=(0, 2), # mapping measurement vector index to state index noise_covar=np.array([ [sigma_meas_radar, 0], # covariance matrix for Gaussian PDF [0, sigma_meas_radar] ])) # Specify measurement model for AIS (Same as for radar) measurement_model_ais = LinearGaussian(ndim_state=4, mapping=(0, 2), noise_covar=np.array( [[sigma_meas_ais, 0], [0, sigma_meas_ais]])) # generate "radar" measurements measurements_radar = [] measurements_ais = [] next_radar_meas_time = start_time next_ais_meas_time = start_time for state in truth: # check whether we want to generate a measurement from this gt if state.timestamp == next_radar_meas_time: measurement = measurement_model_radar.function(state, noise=True) measurements_radar.append( Detection(measurement, timestamp=state.timestamp)) next_radar_meas_time += timedelta(seconds=radar_meas_rate) if state.timestamp == next_ais_meas_time: measurement = measurement_model_ais.function(state, noise=True) measurements_ais.append( Detection(measurement, timestamp=state.timestamp)) next_ais_meas_time += timedelta(seconds=ais_meas_rate) if permanent_save: save_folder_name = seed.__str__() else: save_folder_name = "temp" save_folder = "../scenarios/scenario3/" + save_folder_name + "/" # save the ground truth and the measurements for the radar and the AIS store_object.store_object(truth, save_folder, "ground_truth.pk1") store_object.store_object(measurements_radar, save_folder, "measurements_radar.pk1") store_object.store_object(measurements_ais, save_folder, "measurements_ais.pk1") store_object.store_object(start_time, save_folder, "start_time.pk1") store_object.store_object(measurement_model_radar, save_folder, "measurement_model_radar.pk1") store_object.store_object(measurement_model_ais, save_folder, "measurement_model_ais.pk1") store_object.store_object(transition_model, save_folder, "transition_model.pk1")
[0., 0., 0., 0.])) # %% # Create target # ------------- # Then a target is created for the model to measure from stonesoup.models.transition.linear import ( CombinedLinearGaussianTransitionModel, ConstantVelocity) from stonesoup.platform.base import MovingPlatform from stonesoup.types.state import State time_step = datetime.timedelta(seconds=0.1) time_init = datetime.datetime.now() transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(1.), ConstantVelocity(1.), ConstantVelocity(1.)]) red = MovingPlatform(position_mapping=[0, 2, 4], velocity_mapping=[0, 2, 4], states=State([50., 0., -50., 10., 0., 0.], timestamp=time_init), transition_model=transition_model) # %% # Move target for s in range(1, 100): red.move(time_init + s * time_step) # %%
def test_kalman_smoother(SmootherClass): # First create a track from some detections and then smooth - check the output. # Setup list of Detections start = datetime.now() times = [start + timedelta(seconds=i) for i in range(0, 5)] measurements = [ np.array([[2.486559674128609]]), np.array([[2.424165626519697]]), np.array([[6.603176662762473]]), np.array([[9.329099124074590]]), np.array([[14.637975326666801]]), ] detections = [ Detection(m, timestamp=timest) for m, timest in zip(measurements, times) ] # Setup models. trans_model = ConstantVelocity(noise_diff_coeff=1) meas_model = LinearGaussian(ndim_state=2, mapping=[0], noise_covar=np.array([[0.4]])) # Tracking components predictor = KalmanPredictor(transition_model=trans_model) updater = KalmanUpdater(measurement_model=meas_model) # Prior cstate = GaussianState(np.ones([2, 1]), np.eye(2), timestamp=start) track = Track() for detection in detections: # Predict pred = predictor.predict(cstate, timestamp=detection.timestamp) # form hypothesis hypothesis = SingleHypothesis(pred, detection) # Update cstate = updater.update(hypothesis) # write to track track.append(cstate) smoother = SmootherClass(transition_model=trans_model) smoothed_track = smoother.smooth(track) smoothed_state_vectors = [state.state_vector for state in smoothed_track] # Verify Values target_smoothed_vectors = [ np.array([[1.688813974839928], [1.267196351952188]]), np.array([[3.307200214998506], [2.187167840595264]]), np.array([[6.130402001958210], [3.308896367021604]]), np.array([[9.821303658438408], [4.119557021638030]]), np.array([[14.257730973981149], [4.594862462495096]]) ] assert np.allclose(smoothed_state_vectors, target_smoothed_vectors) # Check that a prediction is smoothable and that no error chucked # Also remove the transition model and use the one provided by the smoother track[1] = GaussianStatePrediction(pred.state_vector, pred.covar, timestamp=pred.timestamp) smoothed_track2 = smoother.smooth(track) assert isinstance(smoothed_track2[1], GaussianStatePrediction) # Check appropriate error chucked if not GaussianStatePrediction/Update track[-1] = detections[-1] with pytest.raises(TypeError): smoother._prediction(track[-1])
def test_base(): # Define time related variables timestamp = datetime.datetime.now() timediff = 2 # 2sec new_timestamp = timestamp + datetime.timedelta(seconds=timediff) # Define a static 2d platform and check it does not move platform_state2d = State(np.array([[2], [2]]), timestamp) platform = FixedPlatform(states=platform_state2d, position_mapping=np.array([0, 1])) platform.move(new_timestamp) new_statevector = np.array([[2], [2]]) # Ensure 2d platform has not moved assert (np.array_equal(platform.state.state_vector, new_statevector)) # Test to ensure platform time has updated assert (platform.state.timestamp == new_timestamp) assert np.array_equal(platform.velocity, StateVector([0, 0])) assert platform.ndim == 2 assert not platform.is_moving # Define a static 3d platform and check it does not move platform_state3d = State(np.array([[2], [2], [2]]), timestamp) platform = FixedPlatform(states=platform_state3d, position_mapping=[0, 1, 2]) platform.move(new_timestamp) new_statevector = np.array([[2], [2], [2]]) # Ensure 2d platform has not moved assert np.array_equal(platform.state.state_vector, new_statevector) assert np.array_equal(platform.velocity, StateVector([0, 0, 0])) assert platform.ndim == 3 assert not platform.is_moving # Define zero noise 2d constant velocity transition model model_1d = ConstantVelocity(0.0) model_2d = CombinedLinearGaussianTransitionModel([model_1d, model_1d]) # Define a 2d platform with constant velocity motion and test motion platform_state2d = State(np.array([[2], [1], [2], [1]]), timestamp) platform = MovingPlatform(states=platform_state2d, transition_model=model_2d, position_mapping=[0, 2]) platform.move(new_timestamp) # Define expected platform location after movement new_statevector = np.array([[4], [1], [4], [1]]) assert (np.array_equal(platform.state.state_vector, new_statevector)) assert np.array_equal(platform.velocity, StateVector([1, 1])) assert platform.ndim == 2 assert platform.is_moving # Define zero noise 3d constant velocity transition model model_3d = CombinedLinearGaussianTransitionModel( [model_1d, model_1d, model_1d]) # Define a 3d platform with constant velocity motion and test motion platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=model_3d, position_mapping=[0, 2, 4]) platform.move(new_timestamp) # Define expected platform location in 3d after movement new_statevector = np.array([[4], [1], [4], [1], [2], [1]]) assert (np.array_equal(platform.state.state_vector, new_statevector)) assert np.array_equal(platform.velocity, StateVector([1, 1, 1])) assert platform.ndim == 3 assert platform.is_moving
# We begin our definition of the state-space models by defining the hidden state # :math:`\mathrm{x}_k`, i.e. the state that we wish to estimate: # # .. math:: # \mathrm{x}_k = [x_k, \dot{x}_k, y_k, \dot{y}_k, w_k, h_k] # # where :math:`x_k, y_k` denote the pixel coordinates of the top-left corner of the bounding box # containing an object, with :math:`\dot{x}_k, \dot{y}_k` denoting their respective rate of change, # while :math:`w_k` and :math:`h_k` denote the width and height of the box, respectively. # # We assume that :math:`x_k` and :math:`y_k` move with nearly :class:`~.ConstantVelocity`, while # :math:`w_k` and :math:`h_k` evolve according to a :class:`~.RandomWalk`.Using these assumptions, # we proceed to construct our Stone Soup :class:`~.TransitionModel` as follows: from stonesoup.models.transition.linear import (CombinedLinearGaussianTransitionModel, ConstantVelocity, RandomWalk) t_models = [ConstantVelocity(20**2), ConstantVelocity(20**2), RandomWalk(20**2), RandomWalk(20**2)] transition_model = CombinedLinearGaussianTransitionModel(t_models) # %% # Measurement Model # ***************** # Continuing, we define the measurement state :math:`\mathrm{y}_k`, which follows naturally from # the form of the detections generated by the :class:`~.TensorFlowBoxObjectDetector` we previously # discussed: # # .. math:: # \mathrm{y}_k = [x_k, y_k, w_k, h_k] # # We make use of a 4-dimensional :class:`~.LinearGaussian` model as our :class:`~.MeasurementModel`, # whereby we can see that the individual indices of :math:`\mathrm{y}_k` map to indices `[0,2,4,5]` # of the 6-dimensional state :math:`\mathrm{x}_k`:
def test_setting_position(): timestamp = datetime.datetime.now() platform_state = State(np.array([[2], [2], [0]]), timestamp) model_1d = ConstantVelocity(0.0) model_3d = CombinedLinearGaussianTransitionModel( [model_1d, model_1d, model_1d]) platform = MovingPlatform(states=platform_state, transition_model=model_3d, position_mapping=[0, 1, 2]) with pytest.raises(AttributeError): platform.position = [0, 0, 0] with pytest.raises(AttributeError): platform.velocity = [0, 0, 0] platform_state = State(np.array([[2], [2], [0]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=None, position_mapping=[0, 1, 2]) assert np.array_equal(platform.position, StateVector([2, 2, 0])) platform.position = StateVector([0, 0, 0]) assert np.array_equal(platform.position, StateVector([0, 0, 0])) with pytest.raises(AttributeError): platform.velocity = [0, 0, 0] platform_state = State(np.array([[2], [2], [0]]), timestamp) platform = FixedPlatform(states=platform_state, position_mapping=[0, 1, 2]) assert np.array_equal(platform.position, StateVector([2, 2, 0])) platform.position = StateVector([0, 0, 0]) assert np.array_equal(platform.position, StateVector([0, 0, 0])) assert np.array_equal(platform.state_vector, StateVector([0, 0, 0])) with pytest.raises(AttributeError): platform.velocity = [0, 0, 0] platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=model_3d, position_mapping=[0, 2, 4]) with pytest.raises(AttributeError): platform.position = [0, 0, 0] platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]), timestamp) platform = FixedPlatform(states=platform_state, position_mapping=[0, 2, 4]) assert np.array_equal(platform.position, StateVector([2, 2, 2])) platform.position = StateVector([0, 0, 1]) assert np.array_equal(platform.position, StateVector([0, 0, 1])) assert np.array_equal(platform.state_vector, StateVector([0, 1, 0, -1, 1, 0])) with pytest.raises(AttributeError): platform.velocity = [0, 0, 0] platform_state = State(np.array([[2], [1], [2], [-1], [2], [0]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=None, position_mapping=[0, 2, 4]) assert np.array_equal(platform.position, StateVector([2, 2, 2])) platform.position = StateVector([0, 0, 1]) assert np.array_equal(platform.position, StateVector([0, 0, 1])) assert np.array_equal(platform.state_vector, StateVector([0, 1, 0, -1, 1, 0])) with pytest.raises(AttributeError): platform.velocity = [0, 0, 0]
# # .. math:: # F_{k}^{D} &= \begin{bmatrix} # F_k^{1} & & \mathbf{0} \\ # & \ddots & \\ # \mathbf{0} & & F_k^d \\ # \end{bmatrix}\\ # Q_{k}^{D} &= \begin{bmatrix} # Q_k^{1} & & \mathbf{0} \\ # & \ddots & \\ # \mathbf{0} & & Q_k^d \\ # \end{bmatrix} # # We want a 2d simulation, so we'll do: transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(0.05), ConstantVelocity(0.05)]) # %% # A 'truth path' is created starting at 0,0 moving to the NE truth = GroundTruthPath([GroundTruthState([0, 1, 0, 1], timestamp=start_time)]) for k in range(1, 21): truth.append( GroundTruthState(transition_model.function( truth[k - 1], noise=True, time_interval=timedelta(seconds=1)), timestamp=start_time + timedelta(seconds=k))) # %% # Thus the ground truth is generated and we can plot the result fig = plt.figure(figsize=(10, 6)) ax = fig.add_subplot(1, 1, 1)
from trackers.calc_cross_cov_estimate_error import calc_cross_cov_estimate_error # load ground truth and the measurements ground_truth = open_object.open_object( "../scenarios/scenario2/ground_truth.pk1") measurements_radar = open_object.open_object( "../scenarios/scenario2/measurements_radar.pk1") measurements_ais = open_object.open_object( "../scenarios/scenario2/measurements_ais.pk1") # load start_time start_time = open_object.open_object("../scenarios/scenario2/start_time.pk1") # same transition models (radar uses same as original) transition_model_radar = CombinedLinearGaussianTransitionModel( [ConstantVelocity(0.01), ConstantVelocity(0.01)]) transition_model_ais = CombinedLinearGaussianTransitionModel( [ConstantVelocity(0.01), ConstantVelocity(0.01)]) # same measurement models as used when generating the measurements # Specify measurement model for radar measurement_model_radar = LinearGaussian( ndim_state=4, # number of state dimensions mapping=(0, 2), # mapping measurement vector index to state index noise_covar=np.array([ [1, 0], # covariance matrix for Gaussian PDF [0, 1] ])) # Specify measurement model for AIS measurement_model_ais = LinearGaussian(ndim_state=4,
from stonesoup.hypothesiser.distance import DistanceHypothesiser from stonesoup.initiator.simple import MultiMeasurementInitiator from stonesoup.measures import Mahalanobis from stonesoup.models.transition.linear import ( CombinedLinearGaussianTransitionModel, ConstantVelocity) from stonesoup.models.measurement.linear import LinearGaussian from stonesoup.predictor.kalman import KalmanPredictor from stonesoup.simulator.simple import MultiTargetGroundTruthSimulator, SimpleDetectionSimulator from stonesoup.tracker.simple import MultiTargetTracker 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)]) measurement_model = LinearGaussian(4, [0, 2], np.diag([0.5, 0.5])) # 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) detection_sim = SimpleDetectionSimulator( groundtruth=groundtruth_sim, measurement_model=measurement_model,
timestamp=state.timestamp)) # Plot the result (back in cart.) x, y = pol2cart(np.hstack(state.state_vector[1, 0] for state in measurements), np.hstack(state.state_vector[0, 0] for state in measurements)) ax.scatter(x + sensor_x, y + sensor_y, color='b') fig plt.polar([state.state_vector[0, 0] for state in measurements], [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)
from stonesoup.types.state import State from stonesoup.platform.base import FixedPlatform, MultiTransitionMovingPlatform from stonesoup.simulator.platform import PlatformDetectionSimulator from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel,\ ConstantVelocity, ConstantTurn # Create locations for reference later # Using Heathrow as origin *heathrow, utm_zone, _ = utm.from_latlon(51.47, -0.4543) # Use heathrow utm grid num as reference number for utm conversions later manchester = utm.from_latlon(53.35, -2.280, utm_zone) # Create transition models for moving platforms transition_modelStraight = CombinedLinearGaussianTransitionModel( (ConstantVelocity(0.01), ConstantVelocity(0.01), ConstantVelocity(0.01))) transition_modelLeft = CombinedLinearGaussianTransitionModel((ConstantTurn( (0.01, 0.01), np.radians(3)), ConstantVelocity(0.01))) transition_modelRight = CombinedLinearGaussianTransitionModel((ConstantTurn( (0.01, 0.01), np.radians(-3)), ConstantVelocity(0.01))) # Create specific transition model for example moving platform transition_models = [transition_modelStraight, transition_modelLeft] transition_times = [ datetime.timedelta(seconds=160), datetime.timedelta(seconds=20) ] # List sensors in stationary platforms (sensor orientations are overwritten)
from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, \ ConstantVelocity from stonesoup.types.groundtruth import GroundTruthPath, GroundTruthState from stonesoup.types.detection import Detection from stonesoup.models.measurement.linear import LinearGaussian from utils import store_object start_time = datetime.now() # specify seed to be able repeat example np.random.seed(1996) # combine two 1-D CV models to create a 2-D CV model transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity(0.01), ConstantVelocity(0.01)]) # starting at 0,0 and moving NE truth = GroundTruthPath([GroundTruthState([0, 1, 0, 1], timestamp=start_time)]) # generate truth using transition_model and noise for k in range(1, 21): truth.append(GroundTruthState( transition_model.function(truth[k - 1], noise=True, time_interval=timedelta(seconds=1)), timestamp=start_time + timedelta(seconds=k))) # Simulate measurements # Specify measurement model for radar measurement_model_radar = LinearGaussian( ndim_state=4, # number of state dimensions mapping=(0, 2), # mapping measurement vector index to state index
clutter = [] for n in range(1, 21): clutter.append(set()) for _ in range(np.random.randint(10)): x = uniform.rvs(0, 20) y = uniform.rvs(0, 20) clutter[-1].add(Clutter( np.array([[x], [y]]), timestamp=start_time+timedelta(seconds=n))) # Plot the result ax.scatter([state.state_vector[0, 0] for clutter_set in clutter for state in clutter_set], [state.state_vector[1, 0] for clutter_set in clutter for state in clutter_set], color='y', marker='2') fig from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity transition_model = CombinedLinearGaussianTransitionModel((ConstantVelocity(0.05), ConstantVelocity(0.05))) from stonesoup.predictor.kalman import KalmanPredictor predictor = KalmanPredictor(transition_model) from stonesoup.models.measurement.linear import LinearGaussian measurement_model = LinearGaussian( 4, # Number of state dimensions (position and velocity in 2D) (0,2), # Mapping measurement vector index to state index np.array([[0.25, 0], # Covariance matrix for Gaussian PDF [0, 0.25]]) ) from stonesoup.updater.kalman import KalmanUpdater updater = KalmanUpdater(measurement_model) from stonesoup.hypothesiser.distance import DistanceHypothesiser
# :math:`\mathrm{x}_k`, i.e. the state that we wish to estimate: # # .. math:: # \mathrm{x}_k = [x_k, \dot{x}_k, y_k, \dot{y}_k, w_k, h_k] # # where :math:`x_k, y_k` denote the pixel coordinates of the top-left corner of the bounding box # containing an object, with :math:`\dot{x}_k, \dot{y}_k` denoting their respective rate of change, # while :math:`w_k` and :math:`h_k` denote the width and height of the box, respectively. # # We assume that :math:`x_k` and :math:`y_k` move with nearly :class:`~.ConstantVelocity`, while # :math:`w_k` and :math:`h_k` evolve according to a :class:`~.RandomWalk`.Using these assumptions, # we proceed to construct our Stone Soup :class:`~.TransitionModel` as follows: from stonesoup.models.transition.linear import ( CombinedLinearGaussianTransitionModel, ConstantVelocity, RandomWalk) t_models = [ ConstantVelocity(20**2), ConstantVelocity(20**2), RandomWalk(20**2), RandomWalk(20**2) ] transition_model = CombinedLinearGaussianTransitionModel(t_models) # %% # Measurement Model # ***************** # Continuing, we define the measurement state :math:`\mathrm{y}_k`, which follows naturally from # the form of the detections generated by the :class:`~.TensorFlowBoxObjectDetector` we previously # discussed: # # .. math:: # \mathrm{y}_k = [x_k, y_k, w_k, h_k]
def test_velocity_properties(velocity_mapping): model_1d = ConstantVelocity(0.0) model_3d = CombinedLinearGaussianTransitionModel( [model_1d, model_1d, model_1d]) timestamp = datetime.datetime.now() timediff = 2 # 2sec new_timestamp = timestamp + datetime.timedelta(seconds=timediff) platform_state = State(np.array([[2], [0], [2], [0], [0], [0]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=model_3d, position_mapping=[0, 2, 4]) old_position = platform.position assert not platform.is_moving assert np.array_equal(platform.velocity, StateVector([0, 0, 0])) # check it doesn't move with timestamp = None platform.move(timestamp=None) assert np.array_equal(platform.position, old_position) # check it doesn't move (as it has zero velocity) platform.move(timestamp) assert np.array_equal(platform.position, old_position) platform.move(new_timestamp) assert np.array_equal(platform.position, old_position) platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=None, position_mapping=[0, 2, 4]) assert platform.is_moving assert np.array_equal(platform.velocity, StateVector([1, 1, 1])) old_position = platform.position # check it doesn't move with timestamp = None platform.move(None) assert np.array_equal(platform.position, old_position) with pytest.raises(AttributeError): platform.move(timestamp) # moving platform without velocity defined platform_state = State(np.array([[2], [2], [0]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=None, position_mapping=[0, 2, 4]) with pytest.raises(AttributeError): _ = platform.velocity # pass in a velocity mapping platform_state = State(np.array([[2], [1], [2], [1], [0], [1]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=None, position_mapping=[0, 2, 4], velocity_mapping=velocity_mapping) assert platform.is_moving assert np.array_equal(platform.velocity, StateVector([1, 1, 1])) old_position = platform.position # check it doesn't move with timestamp = None platform.move(None) assert np.array_equal(platform.position, old_position) with pytest.raises(AttributeError): platform.move(timestamp) # moving platform without velocity defined platform_state = State(np.array([[2], [2], [0]]), timestamp) platform = MovingPlatform(states=platform_state, transition_model=None, position_mapping=[0, 2, 4], velocity_mapping=velocity_mapping) with pytest.raises(AttributeError): _ = platform.velocity
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
# * :math:`z` is Gaussian distributed around an altitude of :math:`\mathrm{9}km` with variance of :math:`\mathrm{0.1}km` # * :math:`\dot{x}` is Gaussian distributed around :math:`\mathrm{100}ms^{-1}` with variance of :math:`\mathrm{50}ms^{-1}` # * :math:`\dot{y}` is Gaussian distributed around :math:`\mathrm{100}ms^{-1}` with variance of :math:`\mathrm{50}ms^{-1}` # * :math:`\dot{z}` is Gaussian distributed around :math:`\mathrm{0}ms^{-1}` with variance of :math:`\mathrm{1}ms^{-1}` # # We will also configure our simulator to randomly create and delete targets, based on a birth rate and death rate we # specify. In this example we set the birth rate to be 0.10, i.e. on any given time step there is a 10% chance of a new # target being initiated. We have set the death rate to 0.01, i.e. on any given time step there is a 1% chance that a # target will be removed from the simulation. # # The above setup will provide a case which loosely approximates an air surveillance radar at an airport. from stonesoup.simulator.simple import MultiTargetGroundTruthSimulator # Set a constant velocity transition model for the targets transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(0.5), ConstantVelocity(0.5), ConstantVelocity(0.1)]) # Define the Gaussian State from which new targets are sampled on initialisation initial_target_state = GaussianState( StateVector([[0], [0], [0], [0], [9000], [0]]), CovarianceMatrix(np.diag([2000, 50, 2000, 50, 100, 1]))) groundtruth_sim = MultiTargetGroundTruthSimulator( transition_model=transition_model, # target transition model initial_state=initial_target_state, # add our initial state for targets timestep=timedelta(seconds=1), # time between measurements number_steps=120, # 2 minute birth_rate=0.10, # 10% chance of a new target being born death_probability=0.01 # 1% chance of a target being killed
np.diag([0.1, np.radians(0.1)])) measurements.append( Detection(np.array([[Bearing(phi)], [rho]]), timestamp=state.timestamp)) # Plot the result (back in cart.) x, y = pol2cart(np.hstack(state.state_vector[1, 0] for state in measurements), np.hstack(state.state_vector[0, 0] for state in measurements)) fig ax.scatter(x + sensor_x, y + sensor_y, color='b') # Create Models and Particle Filter from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity transition_model = CombinedLinearGaussianTransitionModel( (ConstantVelocity(0.05), ConstantVelocity(0.05))) from stonesoup.predictor.particle import ParticlePredictor predictor = ParticlePredictor(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.1), 0.1]), # Covariance matrix for Gaussian PDF translation_offset=np.array([[sensor_x], [sensor_y] ]) # Location of sensor in cartesian. ) from stonesoup.resampler.particle import SystematicResampler resampler = SystematicResampler()
def test_multi_transition(): transition_model1 = CombinedLinearGaussianTransitionModel( (ConstantVelocity(0), ConstantVelocity(0))) transition_model2 = ConstantTurn((0, 0), np.radians(4.5)) transition_models = [transition_model1, transition_model2] transition_times = [ datetime.timedelta(seconds=10), datetime.timedelta(seconds=20) ] platform_state = State(state_vector=[[0], [1], [0], [0]], timestamp=datetime.datetime.now()) platform = MultiTransitionMovingPlatform( transition_models=transition_models, transition_times=transition_times, states=platform_state, position_mapping=[0, 2], sensors=None) assert len(platform.transition_models) == 2 assert len(platform.transition_times) == 2 # Check that platform states length increases as platform moves assert len(platform) == 1 time = datetime.datetime.now() time += datetime.timedelta(seconds=1) platform.move(timestamp=time) assert len(platform) == 2 time += datetime.timedelta(seconds=1) platform.move(timestamp=time) assert len(platform) == 3 time += datetime.timedelta(seconds=1) platform.move(timestamp=time) assert len(platform) == 4 px, py = platform.position[0], platform.position[1] # Starting transition model is index 0 assert platform.transition_index == 0 time += datetime.timedelta(seconds=7) platform.move(timestamp=time) x, y = platform.position[0], platform.position[1] # Platform initially moves horizontally assert x > px assert np.allclose(y, py, atol=1e-6) px, py = x, y # Transition model changes after corresponding interval is done/ Next transition is left-turn assert platform.transition_index == 1 time += datetime.timedelta(seconds=10) platform.move(timestamp=time) x, y = platform.position[0], platform.position[1] # Platform starts turning left to 45 degrees assert x > px assert y > py # Transition interval is not done. Next transition is left-turn assert platform.transition_index == 1 time += datetime.timedelta(seconds=10) platform.move(timestamp=time) x, y = platform.position[0], platform.position[1] px, py = x, y # Platform turned left to 90 degrees # Transition interval is done. Next transition is straight-on assert platform.transition_index == 0 time += datetime.timedelta(seconds=10) platform.move(timestamp=time) x, y = platform.position[0], platform.position[1] # Platform travelling vertically up assert np.allclose(x, px, atol=1e-6) assert y > py # Next transition is left-turn assert platform.transition_index == 1 # Add new transition model (right-turn) to list transition_model3 = ConstantTurn((0, 0), np.radians(-9)) platform.transition_models.append(transition_model3) platform.transition_times.append(datetime.timedelta(seconds=10)) # New model and transition interval are added to model list and to interval list assert len(platform.transition_models) == 3 assert len(platform.transition_times) == 3 time += datetime.timedelta(seconds=20) platform.move(timestamp=time) # Platform turned left by 90 degrees (now travelling in -x direction) px, py = platform.position[0], platform.position[1] # Next transition is right-turn assert platform.transition_index == 2 time += datetime.timedelta(seconds=10) platform.move(timestamp=time) x, y = platform.position[0], platform.position[1] px, py = x, y # Next transition straight-on, travelling vertically up again assert platform.transition_index == 0 time += datetime.timedelta(seconds=10) platform.move(timestamp=time) x, y = platform.position[0], platform.position[1] # Platform travelled vertically up assert np.allclose(x, px, atol=1e-6) assert y > py # Next transition is left-turn assert platform.transition_index == 1
# 0 & 0 & 1 & \triangle k & 0 & 0\\ # 0 & 0 & 0 & 1 & 0 & 0\\ # 0 & 0 & 0 & 0 & 1 & \triangle k \\ # 0 & 0 & 0 & 0 & 0 & 1\\ # \end{bmatrix} # First import the Moving platform from stonesoup.platform.base import MovingPlatform # Define the initial platform position, in this case the origin initial_loc = StateVector([[0], [0], [0], [50], [8000], [0]]) initial_state = State(initial_loc, start_time) # Define transition model and position for 3D platform transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(0.), ConstantVelocity(0.), ConstantVelocity(0.)]) # create our fixed platform sensor_platform = MovingPlatform(states=initial_state, position_mapping=(0, 2, 4), velocity_mapping=(1, 3, 5), transition_model=transition_model) # %% # 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.
from stonesoup.hypothesiser.distance import DistanceHypothesiser from stonesoup.initiator.simple import MultiMeasurementInitiator from stonesoup.measures import Mahalanobis from stonesoup.models.transition.linear import ( CombinedLinearGaussianTransitionModel, ConstantVelocity) from stonesoup.models.measurement.linear import LinearGaussian from stonesoup.predictor.kalman import KalmanPredictor from stonesoup.simulator.simple import MultiTargetGroundTruthSimulator, SimpleDetectionSimulator from stonesoup.tracker.simple import MultiTargetTracker 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 )
from datetime import datetime, timedelta start_time = datetime.now() # %% # Generate ground truth # ^^^^^^^^^^^^^^^^^^^^^ # # At the end of the tutorial we will plot the Gaussian mixtures. The ground truth Gaussian # mixtures are stored in this list where each index refers to an instance in time and holds # all ground truths at that time step. truths_by_time = [] # Create transition model from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity transition_model = CombinedLinearGaussianTransitionModel( (ConstantVelocity(0.3), ConstantVelocity(0.3))) from stonesoup.types.groundtruth import GroundTruthPath, GroundTruthState start_time = datetime.now() truths = set() # Truths across all time current_truths = set() # Truths alive at current time start_truths = set() number_steps = 20 death_probability = 0.005 birth_probability = 0.2 # Initialize 3 truths. This can be changed to any number of truths you wish. truths_by_time.append([]) for i in range(3): x, y = initial_position = np.random.uniform( -30, 30, 2) # Range [-30, 30] for x and y