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 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, 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_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)
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")
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")
# of 1d models and combines them in a linear Gaussian model of arbitrary dimension, :math:`D`. # # .. 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)
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
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
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,
# :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`:
from stonesoup.sensor.radar import RadarRotatingBearingRange 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)
measurements = [] for state in truth: x, y = multivariate_normal.rvs(state.state_vector.ravel(), cov=np.diag([0.75, 0.75])) measurements.append( Detection(np.array([[x], [y]]), timestamp=state.timestamp)) # Plot the result ax.scatter([state.state_vector[0, 0] for state in measurements], [state.state_vector[1, 0] for state in measurements], color='b') fig # Create Models and Kalman Filter from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, ConstantVelocity transition_model = CombinedLinearGaussianTransitionModel( (ConstantVelocity(0.05), ConstantVelocity(0.05))) transition_model.matrix(time_interval=timedelta(seconds=1)) transition_model.covar(time_interval=timedelta(seconds=1)) 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.75, 0], # Covariance matrix for Gaussian PDF [0, 0.75] ]))
class KalmanFilterDependentFusionAsyncSensors: """ todo """ 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 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 # same transition models (radar uses same as original) 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)]) # 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 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 self.cross_cov_list = [] def track(self, measurements_radar, measurements_ais): """ todo :return: """ # create list for storing kalman gains kf_gains_radar = [] kf_gains_ais = [] # create list for storing transition_noise_covar transition_covars_radar = [] transition_covars_ais = [] # create list for storing transition matrices transition_matrices_radar = [] transition_matrices_ais = [] # create list for storing tracks tracks_radar = Track() tracks_ais = Track() # track for measurement in measurements_radar: prediction = self.predictor_radar.predict(self.prior_radar, timestamp=measurement.timestamp) hypothesis = SingleHypothesis(prediction, measurement) # calculate the kalman gain hypothesis.measurement_prediction = self.updater_radar.predict_measurement(hypothesis.prediction, measurement_model=self.measurement_model_radar) post_cov, kalman_gain = self.updater_radar._posterior_covariance(hypothesis) kf_gains_radar.append(kalman_gain) # get the transition model covar predict_over_interval = measurement.timestamp - self.prior_radar.timestamp transition_covars_ais.append(self.transition_model_ais.covar(time_interval=predict_over_interval)) transition_matrices_ais.append(self.transition_model_ais.matrix(time_interval=predict_over_interval)) # update post = self.updater_radar.update(hypothesis) tracks_radar.append(post) self.prior_radar = post for measurement in measurements_ais: prediction = self.predictor_radar.predict(self.prior_ais, timestamp=measurement.timestamp) hypothesis = SingleHypothesis(prediction, measurement) # calculate the kalman gain hypothesis.measurement_prediction = self.updater_ais.predict_measurement(hypothesis.prediction, measurement_model=self.measurement_model_ais) post_cov, kalman_gain = self.updater_ais._posterior_covariance(hypothesis) kf_gains_ais.append(kalman_gain) # get the transition model covar predict_over_interval = measurement.timestamp - self.prior_ais.timestamp transition_covars_radar.append(self.transition_model_radar.covar(time_interval=predict_over_interval)) transition_matrices_radar.append(self.transition_model_radar.matrix(time_interval=predict_over_interval)) # update post = self.updater_ais.update(hypothesis) tracks_ais.append(post) self.prior_ais = post # FOR NOW: run track_to_track_association here, todo change pipeline flow # FOR NOW: run the association only when both have a new posterior (so each time the AIS has a posterior) # todo handle fusion when one track predicts and the other updates. (or both predicts) (Can't be done with the # theory described in the article) cross_cov_ij = [np.zeros([4, 4])] cross_cov_ji = [np.zeros([4, 4])] # TODO change flow to assume that the indexes decide whether its from the same iterations # use indexes to loop through tracks, kf_gains etc tracks_fused = [tracks_radar[0]] for i in range(1, len(tracks_radar)): # we assume that the indexes correlates with the timestamps. I.e. that the lists are 'synchronized' # check to make sure if tracks_ais[i].timestamp == tracks_radar[i].timestamp: # calculate the cross-covariance estimation error cross_cov_ji.append(calc_cross_cov_estimate_error( self.measurement_model_ais.matrix(), self.measurement_model_radar.matrix(), kf_gains_ais[i], kf_gains_radar[i], transition_matrices_ais[i], transition_covars_ais[i], cross_cov_ji[i - 1] )) cross_cov_ij.append(calc_cross_cov_estimate_error( self.measurement_model_radar.matrix(), self.measurement_model_ais.matrix(), kf_gains_radar[i], kf_gains_ais[i], transition_matrices_radar[i], transition_covars_ais[i], cross_cov_ij[i - 1] )) self.cross_cov_list.append(cross_cov_ij) # test for track association # same_target = track_to_track_association.test_association_dependent_tracks(tracks_radar[i], # tracks_ais[i], # cross_cov_ij[i], # cross_cov_ji[i], 0.01) same_target = True # ignore test for track association for now if same_target: fused_posterior, fused_covar = track_to_track_fusion.fuse_dependent_tracks(tracks_radar[i], tracks_ais[i], cross_cov_ij[i], cross_cov_ji[i]) estimate = GaussianState(fused_posterior, fused_covar, timestamp=tracks_ais[i].timestamp) tracks_fused.append(estimate) return tracks_fused, tracks_ais, tracks_radar def track_async(self, start_time, measurements_radar, measurements_ais, fusion_rate=1): """ Assumptions: 1) assumes that there are a maximum of one new measurement per sensor per fusion_rate. 2) assumes that the measurements arrives exactly at the timestep that the fusion is performed. 3) assumes kf gain of size (4,2) """ # create list for storing tracks tracks_radar = Track() tracks_ais = Track() tracks_fused = [] time = start_time cross_cov_ij = np.zeros([4, 4]) cross_cov_ji = np.zeros([4, 4]) measurements_radar = measurements_radar.copy() measurements_ais = measurements_ais.copy() # loop until there are no more measurements while measurements_radar or measurements_ais: # get all new measurements new_measurements_radar = \ [measurement for measurement in measurements_radar if measurement.timestamp <= time] new_measurements_ais = \ [measurement for measurement in measurements_ais if measurement.timestamp <= time] # remove the new measurements from the measurements lists for new_meas in new_measurements_ais: measurements_ais.remove(new_meas) for new_meas in new_measurements_radar: measurements_radar.remove(new_meas) # check whether there are more than one measurement per sensor if len(new_measurements_ais) > 1 or len(new_measurements_radar) > 1: # raise exception raise Exception("More than one measurement per sensor per fusion rate") # for each sensor, perform a prediction prediction_radar = self.predictor_radar.predict(self.prior_radar, timestamp=time) prediction_ais = self.predictor_ais.predict(self.prior_ais, timestamp=time) # if a new AIS measurement if new_measurements_ais: measurement = new_measurements_ais[0] # calc updated estimate hypothesis = SingleHypothesis(prediction_ais, measurement) # calc kalman gain # calculate the kalman gain hypothesis.measurement_prediction = self.updater_ais.predict_measurement(hypothesis.prediction, measurement_model=self.measurement_model_ais) post_cov, kf_gain_ais = self.updater_ais._posterior_covariance(hypothesis) # get the transition model covar predict_over_interval = measurement.timestamp - self.prior_ais.timestamp # calc transition matrix transition_covar_ais = self.transition_model_ais.covar(time_interval=predict_over_interval) transition_matrix_ais = self.transition_model_ais.matrix(time_interval=predict_over_interval) # calc posterior post = self.updater_ais.update(hypothesis) # append posterior and update prior_ais tracks_ais.append(post) self.prior_ais = post else: # calc transition matrix and set kalman gain to 0 # get the transition model covar predict_over_interval = time - self.prior_ais.timestamp # calc transition matrix transition_covar_ais = self.transition_model_ais.covar(time_interval=predict_over_interval) transition_matrix_ais = self.transition_model_ais.matrix(time_interval=predict_over_interval) # set kalman gain to 0 kf_gain_ais = Matrix([[0, 0], [0, 0], [0, 0], [0, 0]]) # append prediction and update prior_ais tracks_ais.append(prediction_ais) self.prior_ais = prediction_ais # if a new radar measurement if new_measurements_radar: measurement = new_measurements_radar[0] # calc updated estimate hypothesis = SingleHypothesis(prediction_radar, measurement) # calc kalman gain # calculate the kalman gain hypothesis.measurement_prediction = self.updater_radar.predict_measurement(hypothesis.prediction, measurement_model=self.measurement_model_radar) post_cov, kf_gain_radar = self.updater_radar._posterior_covariance(hypothesis) # get the transition model covar predict_over_interval = measurement.timestamp - self.prior_radar.timestamp # calc transition matrix transition_covar_radar = self.transition_model_radar.covar(time_interval=predict_over_interval) transition_matrix_radar = self.transition_model_radar.matrix(time_interval=predict_over_interval) # calc posterior post = self.updater_radar.update(hypothesis) # append posterior and update prior_radar self.prior_radar = post else: # calc transition matrix and set kalman gain to 0 # get the transition model covar predict_over_interval = time - self.prior_radar.timestamp # calc transition matrix transition_covar_radar = self.transition_model_radar.covar(time_interval=predict_over_interval) transition_matrix_radar = self.transition_model_radar.matrix(time_interval=predict_over_interval) # set kalman gain to 0 kf_gain_radar = Matrix([[0, 0], [0, 0], [0, 0], [0, 0]]) # append prediction and update prior_radar self.prior_radar = prediction_radar # calculate the cross-covariance cross_cov_ij = calc_cross_cov_estimate_error( self.measurement_model_radar.matrix(), self.measurement_model_ais.matrix(), kf_gain_radar, kf_gain_ais, transition_matrix_radar, transition_covar_radar, cross_cov_ij ) cross_cov_ji = calc_cross_cov_estimate_error( self.measurement_model_ais.matrix(), self.measurement_model_radar.matrix(), kf_gain_ais, kf_gain_radar, transition_matrix_ais, transition_covar_ais, cross_cov_ji ) same_target = True # ignore test for track association for now if same_target: fused_posterior, fused_covar = track_to_track_fusion.fuse_dependent_tracks(self.prior_radar, self.prior_ais, cross_cov_ij, cross_cov_ji) estimate = GaussianState(fused_posterior, fused_covar, timestamp=time) tracks_fused.append(estimate) # try T2TFwoMpF # also have to update the cross-covariance cross_cov_ij = calc_partial_feedback_cross_cov(self.prior_radar, self.prior_ais, cross_cov_ij, cross_cov_ji) cross_cov_ji = cross_cov_ij.copy().T # right?? # TEMPORARY: try to let prior radar become the fused result, i.e. partial feedback self.prior_radar = estimate # append to radar tracks tracks_radar.append(estimate) self.cross_cov_list.append(cross_cov_ij) time += timedelta(seconds=fusion_rate) return tracks_fused, tracks_radar, tracks_ais
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:`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 )
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
# 0 & 1 & 0 & 0 & 0 & 0\\ # 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.
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]
import numpy as np 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
# %% # Creating the models and filters # ------------------------------- # Now we begin to build our tracker from Stone Soup components. The first component we will build # is a linear transition model. We create a two-dimensional transition model by combining two # individual one-dimensional `Ornstein Uhlenbeck`_ models. This is similar to # :class:`~.ConstantVelocity`, which has an additional :attr:`~.OrnsteinUhlenbeck.damping_coeff` # which models decaying velocity (we assume vessels will slow down to zero over time, rather than # continually speed up) # # .. _Ornstein Uhlenbeck: https://en.wikipedia.org/wiki/Ornstein%E2%80%93Uhlenbeck_process from stonesoup.models.transition.linear import ( CombinedLinearGaussianTransitionModel, OrnsteinUhlenbeck) transition_model = CombinedLinearGaussianTransitionModel( (OrnsteinUhlenbeck(0.5, 1e-4), OrnsteinUhlenbeck(0.5, 1e-4))) # %% # Next we build a measurement model to describe the uncertainty on our detections. In this case, we # are just using the measured position (`[0, 2]` dimensions of state space), assuming from ships # |GNSS|_ receiver system with covariance of :math:`\begin{bmatrix}15&0\\0&15\end{bmatrix}` # # .. |GNSS| replace:: :abbr:`GNSS (Global Navigation Satellite System)` # .. _GNSS: https://en.wikipedia.org/wiki/Satellite_navigation import numpy as np from stonesoup.models.measurement.linear import LinearGaussian measurement_model = LinearGaussian(ndim_state=4, mapping=[0, 2], noise_covar=np.diag([15, 15]))
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
from stonesoup.deleter.error import CovarianceBasedDeleter 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 )
class kalman_filter_dependent_fusion: """ todo """ def __init__(self, measurements_radar, measurements_ais, 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 self.measurements_radar = measurements_radar self.measurements_ais = measurements_ais # same transition models (radar uses same as original) 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) ]) # 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 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 track(self): """ todo :return: """ # create list for storing kalman gains kf_gains_radar = [] kf_gains_ais = [] # create list for storing transition_noise_covar transition_covars_radar = [] transition_covars_ais = [] # create list for storing tranisition matrixes transition_matrixes_radar = [] transition_matrixes_ais = [] # create list for storing tracks tracks_radar = Track() tracks_ais = Track() # track for measurement in self.measurements_radar: prediction = self.predictor_radar.predict( self.prior_radar, timestamp=measurement.timestamp) hypothesis = SingleHypothesis(prediction, measurement) # calculate the kalman gain hypothesis.measurement_prediction = self.updater_radar.predict_measurement( hypothesis.prediction, measurement_model=self.measurement_model_radar) post_cov, kalman_gain = self.updater_radar._posterior_covariance( hypothesis) kf_gains_radar.append(kalman_gain) # get the transition model covar NOTE; same for AIS and radar. Name change not a bug predict_over_interval = measurement.timestamp - self.prior_radar.timestamp transition_covars_radar.append( self.transition_model_radar.covar( time_interval=predict_over_interval)) transition_matrixes_radar.append( self.transition_model_radar.matrix( time_interval=predict_over_interval)) # update post = self.updater_radar.update(hypothesis) tracks_radar.append(post) self.prior_radar = post for measurement in self.measurements_ais: prediction = self.predictor_ais.predict( self.prior_ais, timestamp=measurement.timestamp) hypothesis = SingleHypothesis(prediction, measurement) # calculate the kalman gain hypothesis.measurement_prediction = self.updater_ais.predict_measurement( hypothesis.prediction, measurement_model=self.measurement_model_ais) post_cov, kalman_gain = self.updater_ais._posterior_covariance( hypothesis) kf_gains_ais.append(kalman_gain) # get the transition model covar predict_over_interval = measurement.timestamp - self.prior_ais.timestamp transition_covars_ais.append( self.transition_model_ais.covar( time_interval=predict_over_interval)) transition_matrixes_ais.append( self.transition_model_ais.matrix( time_interval=predict_over_interval)) # update post = self.updater_ais.update(hypothesis) tracks_ais.append(post) self.prior_ais = post # FOR NOW: run track_to_track_association here, todo change pipeline flow # FOR NOW: run the association only when both have a new posterior (so each time the AIS has a posterior) # todo handle fusion when one track predicts and the other updates. (or both predicts) (Can't be done with the theory # described in the article) cross_cov_ij = [np.zeros([4, 4])] cross_cov_ji = [np.zeros([4, 4])] # TODO change flow to assume that the indexes decide whether its from the same iterations # use indexes to loop through tracks, kf_gains etc tracks_fused = [] # tracks_fused.append(tracks_radar[0]) for i in range(1, len(tracks_radar)): # we assume that the indexes correlates with the timestamps. I.e. that the lists are 'synchronized' # check to make sure if tracks_ais[i].timestamp == tracks_radar[i].timestamp: # calculate the cross-covariance estimation error cross_cov_ij.append( calc_cross_cov_estimate_error( self.measurement_model_radar.matrix(), self.measurement_model_ais.matrix(), kf_gains_radar[i], kf_gains_ais[i], transition_matrixes_radar[i], transition_covars_ais[i], cross_cov_ij[i - 1])) cross_cov_ji.append( calc_cross_cov_estimate_error( self.measurement_model_ais.matrix(), self.measurement_model_radar.matrix(), kf_gains_ais[i], kf_gains_radar[i], transition_matrixes_ais[i], transition_covars_radar[i], cross_cov_ji[i - 1])) # test for track association # same_target = track_to_track_association.test_association_dependent_tracks(tracks_radar[i], # tracks_ais[i], # cross_cov_ij[i], # cross_cov_ji[i], 0.01) same_target = True # ignore test for track association for now if same_target: fused_posterior, fused_covar = track_to_track_fusion.fuse_dependent_tracks( tracks_radar[i], tracks_ais[i], cross_cov_ij[i], cross_cov_ji[i]) estimate = GaussianState(fused_posterior, fused_covar, timestamp=tracks_ais[i].timestamp) tracks_fused.append(estimate) return tracks_fused, tracks_ais, tracks_radar