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
# from stonesoup.plotter import Plotter plotter = Plotter() plotter.plot_ground_truths(truths, [0, 2]) # %% # Generate detections with clutter # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # Next, generate detections with clutter just as in the previous tutorial. The clutter is # assumed to be uniformly distributed accross the entire field of view, here assumed to # be the space where :math:`x \in [-100, 100]` and :math:`y \in [-100, 100]`. # Make the measurement model from stonesoup.models.measurement.linear import LinearGaussian measurement_model = LinearGaussian(ndim_state=4, mapping=(0, 2), noise_covar=np.array([[0.75, 0], [0, 0.75]])) # Generate detections and clutter from scipy.stats import uniform from stonesoup.types.detection import TrueDetection from stonesoup.types.detection import Clutter all_measurements = [] # The probability detection and clutter rate play key roles in the posterior intensity. # They can be changed to see their effect. probability_detection = 0.9 clutter_rate = 3.0
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
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
class KFFusionUnsyncSensors: """ todo """ def __init__(self, start_time, prior: GaussianState, sigma_process=0.01, sigma_meas_radar=3, sigma_meas_ais=1): """ :param start_time: :param prior: :param sigma_process: :param sigma_meas_radar: :param sigma_meas_ais: """ # start time 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 track(self, measurements_radar, measurements_ais, estimation_rate=1): """ Uses the Kalman Filter to fuse the measurements received. Produces a new estimate at each estimation_rate. A prediction is performed when no new measurements are received when a new estimate is calculated. Note: when estimation_rate is lower than either of the measurements rates, it might not use all measurements when updating. :param measurements_radar: :param measurements_ais: :param estimation_rate: How often a new estimate should be calculated. """ time = self.start_time tracks_fused = Track() tracks_radar = Track() # copy measurements measurements_radar = measurements_radar.copy() measurements_ais = measurements_ais.copy() # loop until there are no more measurements while measurements_ais or measurements_radar: # 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) # sort the new measurements new_measurements_radar.sort(key=lambda meas: meas.timestamp, reverse=True) new_measurements_ais.sort(key=lambda meas: meas.timestamp, reverse=True) while new_measurements_radar or new_measurements_ais: if new_measurements_radar and \ (not new_measurements_ais or new_measurements_radar[0].timestamp <= new_measurements_ais[0].timestamp): # predict and update with radar measurement new_measurement = new_measurements_radar[0] prediction = self.predictor.predict(self.prior, timestamp=new_measurement.timestamp) hypothesis = SingleHypothesis(prediction, new_measurement) post = self.updater_radar.update(hypothesis) tracks_radar.append(post) # remove measurement new_measurements_radar.remove(new_measurement) else: # predict and update with radar measurement new_measurement = new_measurements_ais[0] prediction = self.predictor.predict(self.prior, timestamp=new_measurement.timestamp) hypothesis = SingleHypothesis(prediction, new_measurement) post = self.updater_ais.update(hypothesis) # remove measurement new_measurements_ais.remove(new_measurement) # add to fused list self.prior = post # perform a prediction up until this time (the newest measurement might not be at this exact time) # note that this "prediction" might be the updated posterior, if the newest measurement was at this time prediction = self.predictor.predict(self.prior, timestamp=time) tracks_fused.append(GaussianState(prediction.mean, prediction.covar, prediction.timestamp)) # increment time time += timedelta(seconds=estimation_rate) return tracks_fused, tracks_radar def plot(self, ground_truth, measurements_radar, measurements_ais, tracks_fused, tracks_radar): """ Is this being used? :return: """ # PLOT fig = plt.figure(figsize=(10, 6)) ax = fig.add_subplot(1, 1, 1) ax.set_xlabel("$x$") ax.set_ylabel("$y$") ax.axis('equal') ax.plot([state.state_vector[0] for state in ground_truth], [state.state_vector[2] for state in ground_truth], linestyle="--", label='Ground truth') ax.scatter([state.state_vector[0] for state in measurements_radar], [state.state_vector[1] for state in measurements_radar], color='b', label='Measurements Radar') ax.scatter([state.state_vector[0] for state in measurements_ais], [state.state_vector[1] for state in measurements_ais], color='r', label='Measurements AIS') # add ellipses to the posteriors for state in tracks_fused: w, v = np.linalg.eig( self.measurement_model_radar.matrix() @ state.covar @ self.measurement_model_radar.matrix().T) max_ind = np.argmax(w) min_ind = np.argmin(w) orient = np.arctan2(v[1, max_ind], v[0, max_ind]) ellipse = Ellipse(xy=(state.state_vector[0], state.state_vector[2]), width=2 * np.sqrt(w[max_ind]), height=2 * np.sqrt(w[min_ind]), angle=np.rad2deg(orient), alpha=0.2, color='r') ax.add_artist(ellipse) for state in tracks_radar: w, v = np.linalg.eig( self.measurement_model_radar.matrix() @ state.covar @ self.measurement_model_radar.matrix().T) max_ind = np.argmax(w) min_ind = np.argmin(w) orient = np.arctan2(v[1, max_ind], v[0, max_ind]) ellipse = Ellipse(xy=(state.state_vector[0], state.state_vector[2]), width=2 * np.sqrt(w[max_ind]), height=2 * np.sqrt(w[min_ind]), angle=np.rad2deg(orient), alpha=0.2, color='b') ax.add_artist(ellipse) # add ellipses to add legend todo do this less ugly ellipse = Ellipse(xy=(0, 0), width=0, height=0, color='r', alpha=0.2, label='Posterior Fused') ax.add_patch(ellipse) ellipse = Ellipse(xy=(0, 0), width=0, height=0, color='b', alpha=0.2, label='Posterior Radar') ax.add_patch(ellipse) # todo move or remove ax.legend() ax.set_title("Kalman filter tracking and fusion when AIS is viewed as a measurement") fig.show() fig.savefig("../results/scenario1/KF_tracking_and_fusion_viewing_ais_as_measurement.svg")
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")
class kalman_filter_ais_as_measurement: """ todo """ 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 track(self): self.tracks_fused = Track() self.tracks_radar = Track() for measurement_idx in range(0, len(self.measurements_radar)): # radar measurement every timestep, AIS measurement every second # first predict, then update with radar measurement. Then every second iteration, perform an extra update step # using the AIS measurement measurement_radar = self.measurements_radar[measurement_idx] prediction = self.predictor.predict( self.prior, timestamp=measurement_radar.timestamp) hypothesis = SingleHypothesis(prediction, measurement_radar) post = self.updater_radar.update(hypothesis) # save radar track self.tracks_radar.append(post) if measurement_idx % 2: measurement_ais = self.measurements_ais[measurement_idx // 2] hypothesis = SingleHypothesis(post, measurement_ais) post = self.updater_ais.update(hypothesis) # save fused track self.tracks_fused.append(post) self.prior = self.tracks_fused[-1] return self.tracks_fused, self.tracks_radar def plot(self, ground_truth): """ :return: """ # PLOT fig = plt.figure(figsize=(10, 6)) ax = fig.add_subplot(1, 1, 1) ax.set_xlabel("$x$") ax.set_ylabel("$y$") ax.axis('equal') ax.plot([state.state_vector[0] for state in ground_truth], [state.state_vector[2] for state in ground_truth], linestyle="--", label='Ground truth') ax.scatter( [state.state_vector[0] for state in self.measurements_radar], [state.state_vector[1] for state in self.measurements_radar], color='b', label='Measurements Radar') ax.scatter([state.state_vector[0] for state in self.measurements_ais], [state.state_vector[1] for state in self.measurements_ais], color='r', label='Measurements AIS') # add ellipses to the posteriors for state in self.tracks_fused: w, v = np.linalg.eig( self.measurement_model_radar.matrix() @ state.covar @ self.measurement_model_radar.matrix().T) max_ind = np.argmax(w) min_ind = np.argmin(w) orient = np.arctan2(v[1, max_ind], v[0, max_ind]) ellipse = Ellipse(xy=(state.state_vector[0], state.state_vector[2]), width=2 * np.sqrt(w[max_ind]), height=2 * np.sqrt(w[min_ind]), angle=np.rad2deg(orient), alpha=0.2, color='r') ax.add_artist(ellipse) for state in self.tracks_radar: w, v = np.linalg.eig( self.measurement_model_radar.matrix() @ state.covar @ self.measurement_model_radar.matrix().T) max_ind = np.argmax(w) min_ind = np.argmin(w) orient = np.arctan2(v[1, max_ind], v[0, max_ind]) ellipse = Ellipse(xy=(state.state_vector[0], state.state_vector[2]), width=2 * np.sqrt(w[max_ind]), height=2 * np.sqrt(w[min_ind]), angle=np.rad2deg(orient), alpha=0.2, color='b') ax.add_artist(ellipse) # add ellipses to add legend todo do this less ugly ellipse = Ellipse(xy=(0, 0), width=0, height=0, color='r', alpha=0.2, label='Posterior Fused') ax.add_patch(ellipse) ellipse = Ellipse(xy=(0, 0), width=0, height=0, color='b', alpha=0.2, label='Posterior Radar') ax.add_patch(ellipse) # todo move or remove ax.legend() ax.set_title( "Kalman filter tracking and fusion when AIS is viewed as a measurement" ) fig.show() fig.savefig( "../results/scenario1/KF_tracking_and_fusion_viewing_ais_as_measurement.svg" )
def detections_gen(self): detections = set() current_time = datetime.now() num_samps = 1000000 d = 10 omega = 50 fs = 20000 l = 1 # expected number of targets window = 20000 windowm1 = window - 1 y = np.loadtxt(self.csv_path, delimiter=',') L = len(y) N = 9 * window max_targets = 5 nbins = 128 bin_steps = [(math.pi + 0.1) / (2 * nbins), 2 * math.pi / nbins] scans = [] winstarts = np.linspace(0, L - window, num=int(L / window), dtype=int) for win in winstarts: # initialise histograms param_hist = np.zeros([max_targets, nbins, nbins]) order_hist = np.zeros([max_targets]) # initialise params p_params = np.empty([max_targets, 2]) noise = noise_proposal(0) [params, K] = proposal([], 0, p_params) # calculate sinTy and cosTy sinTy = np.zeros([9]) cosTy = np.zeros([9]) alpha = np.zeros([9]) yTy = 0 for k in range(0, 9): for t in range(0, window): sinTy[k] = sinTy[k] + math.sin( 2 * math.pi * t * omega / fs) * y[t + win, k] cosTy[k] = cosTy[k] + math.cos( 2 * math.pi * t * omega / fs) * y[t + win, k] yTy = yTy + y[t + win, k] * y[t + win, k] sumsinsq = 0 sumcossq = 0 sumsincos = 0 for t in range(0, window): sumsinsq = sumsinsq + math.sin( 2 * math.pi * t * omega / fs) * math.sin( 2 * math.pi * t * omega / fs) sumcossq = sumcossq + math.cos( 2 * math.pi * t * omega / fs) * math.cos( 2 * math.pi * t * omega / fs) sumsincos = sumsincos + math.sin( 2 * math.pi * t * omega / fs) * math.cos( 2 * math.pi * t * omega / fs) old_logp = calc_acceptance(noise, params, K, omega, 1, d, y, window, sinTy, cosTy, yTy, alpha, sumsinsq, sumcossq, sumsincos, N, l) n = 0 while n < num_samps: p_noise = noise_proposal(noise) [p_params, p_K, Qratio] = proposal_func(params, K, p_params, max_targets) if p_K != 0: new_logp = calc_acceptance(p_noise, p_params, p_K, omega, 1, d, y, window, sinTy, cosTy, yTy, alpha, sumsinsq, sumcossq, sumsincos, N, l) logA = new_logp - old_logp + np.log(Qratio) # do a Metropolis-Hastings step if logA > np.log(random.uniform(0, 1)): old_logp = new_logp params = copy.deepcopy(p_params) K = copy.deepcopy(p_K) for k in range(0, K): bin_ind = [0, 0] for l in range(0, 2): edge = bin_steps[l] while edge < params[k, l]: edge += bin_steps[l] bin_ind[l] += 1 if bin_ind[l] == nbins - 1: break param_hist[K - 1, bin_ind[0], bin_ind[1]] += 1 order_hist[K - 1] += 1 n += 1 # look for peaks in histograms max_peak = 0 max_ind = 0 for ind in range(0, max_targets): if order_hist[ind] > max_peak: max_peak = order_hist[ind] max_ind = ind # FOR TESTING PURPOSES ONLY - SET max_ind = 0 max_ind = 0 # look for largest N peaks, where N corresponds to peak in the order histogram # use divide-and-conquer quadrant-based approach if max_ind == 0: [unique_peak_inds1, unique_peak_inds2 ] = np.unravel_index(param_hist[0, :, :].argmax(), param_hist[0, :, :].shape) num_peaks = 1 else: order_ind = max_ind - 1 quadrant_factor = 2 nstart = 0 mstart = 0 nend = quadrant_factor mend = quadrant_factor peak_inds1 = [None] * 16 peak_inds2 = [None] * 16 k = 0 while quadrant_factor < 32: max_quadrant = 0 quadrant_size = nbins / quadrant_factor for n in range(nstart, nend): for m in range(mstart, mend): [ind1, ind2] = np.unravel_index( param_hist[order_ind, int(n * quadrant_size):int( (n + 1) * quadrant_size - 1), int(m * quadrant_size):int( (m + 1) * quadrant_size - 1)].argmax(), param_hist[order_ind, int(n * quadrant_size):int( (n + 1) * quadrant_size - 1), int(m * quadrant_size):int( (m + 1) * quadrant_size - 1)].shape) peak_inds1[k] = int(ind1 + n * quadrant_size) peak_inds2[k] = int(ind2 + m * quadrant_size) if param_hist[order_ind, peak_inds1[k], peak_inds2[k]] > max_quadrant: max_quadrant = param_hist[order_ind, peak_inds1[k], peak_inds2[k]] max_ind1 = n max_ind2 = m k += 1 quadrant_factor = 2 * quadrant_factor # on next loop look for other peaks in the quadrant containing the highest peak nstart = 2 * max_ind1 mstart = 2 * max_ind2 nend = 2 * (max_ind1 + 1) mend = 2 * (max_ind2 + 1) # determine unique peaks unique_peak_inds1 = [None] * 16 unique_peak_inds2 = [None] * 16 unique_peak_inds1[0] = peak_inds1[0] unique_peak_inds2[0] = peak_inds2[0] num_peaks = 1 for n in range(0, 16): flag_unique = 1 for k in range(0, num_peaks): # check if peak is close to any other known peaks if (unique_peak_inds1[k] - peak_inds1[n]) < 2: if (unique_peak_inds2[k] - peak_inds2[n]) < 2: # part of same peak (check if bin is taller) if param_hist[order_ind, peak_inds1[n], peak_inds2[n]] > param_hist[ order_ind, unique_peak_inds1[k], unique_peak_inds2[k]]: unique_peak_inds1 = peak_inds1[n] unique_peak_inds2 = peak_inds2[n] flag_unique = 0 break if flag_unique == 1: unique_peak_inds1[num_peaks] = peak_inds1[n] unique_peak_inds2[num_peaks] = peak_inds2[n] num_peaks += 1 # Defining a detection state_vector = StateVector([ unique_peak_inds2 * bin_steps[1], unique_peak_inds1 * bin_steps[0] ]) # [Azimuth, Elevation] covar = CovarianceMatrix(np.array([[1, 0], [0, 1]])) # [[AA, AE],[AE, EE]] measurement_model = LinearGaussian(ndim_state=4, mapping=[0, 2], noise_covar=covar) current_time = current_time + timedelta(milliseconds=window) detection = Detection(state_vector, timestamp=current_time, measurement_model=measurement_model) detections = set([detection]) scans.append((current_time, detections)) # For every timestep for scan in scans: yield scan[0], scan[1]
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
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 noise_covar=np.array([[1, 0], # covariance matrix for Gaussian PDF [0, 1]]) ) # Specify measurement model for AIS (Same as for radar) measurement_model_ais = LinearGaussian( ndim_state=4, mapping=(0, 2), noise_covar=np.array([[1, 0], [0, 1]]) ) # generate "radar" measurements measurements_radar = [] for state in truth: measurement = measurement_model_radar.function(state, noise=True)
def detections_gen(self): detections = set() current_time = datetime.now() y = np.loadtxt(self.csv_path, delimiter=',') L = len(y) # frequency of sinusoidal signal omega = 50 window = 20000 windowm1 = window - 1 thetavals = np.linspace(0, 2 * math.pi, num=400) phivals = np.linspace(0, math.pi / 2, num=100) # spatial locations of hydrophones z = np.matrix( '0 0 0; 0 10 0; 0 20 0; 10 0 0; 10 10 0; 10 20 0; 20 0 0; 20 10 0; 20 20 0' ) N = 9 # No. of hydrophones # steering vector v = np.zeros(N, dtype=np.complex) # directional unit vector a = np.zeros(3) scans = [] winstarts = np.linspace(0, L - window, num=int(L / window), dtype=int) c = 1481 / (2 * omega * math.pi) for t in winstarts: # calculate covariance estimate R = np.matmul(np.transpose(y[t:t + windowm1]), y[t:t + windowm1]) R_inv = np.linalg.inv(R) maxF = 0 maxtheta = 0 maxfreq = 0 for theta in thetavals: for phi in phivals: # convert from spherical polar coordinates to cartesian a[0] = math.cos(theta) * math.sin(phi) a[1] = math.sin(theta) * math.sin(phi) a[2] = math.cos(phi) a = a / math.sqrt(np.sum(a * a)) for n in range(0, N): phase = np.sum(a * np.transpose(z[n, ])) / c v[n] = math.cos(phase) - math.sin(phase) * 1j F = 1 / ( (window - N) * np.transpose(np.conj(v)) @ R_inv @ v) if F > maxF: maxF = F maxtheta = theta maxphi = phi # Defining a detection state_vector = StateVector([maxtheta, maxphi]) # [Azimuth, Elevation] covar = CovarianceMatrix(np.array([[1, 0], [0, 1]])) # [[AA, AE],[AE, EE]] measurement_model = LinearGaussian(ndim_state=4, mapping=[0, 2], noise_covar=covar) current_time = current_time + timedelta(milliseconds=window) detection = Detection(state_vector, timestamp=current_time, measurement_model=measurement_model) detections = set([detection]) scans.append((current_time, detections)) # For every timestep for scan in scans: yield scan[0], scan[1]
def measurement_model(): return LinearGaussian(ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]]))
from datetime import timedelta import numpy as np from stonesoup.types.array import StateVector from stonesoup.models.measurement.linear import LinearGaussian from stonesoup.types.detection import Detection from stonesoup.types.state import State from stonesoup.types.prediction import StatePrediction, StateMeasurementPrediction from stonesoup.updater.alphabeta import AlphaBetaUpdater from stonesoup.types.hypothesis import SingleHypothesis @pytest.mark.parametrize( "measurement_model, prediction, measurement, alpha, beta", [( # Standard Alpha-Beta LinearGaussian(ndim_state=4, mapping=[0, 2], noise_covar=0), StatePrediction(StateVector([-6.45, 0.7, -6.45, 0.7])), Detection(StateVector([-6.23, -6.23])), 0.9, 0.3)], ids=["standard"]) def test_alphabeta(measurement_model, prediction, measurement, alpha, beta): # Time delta timediff = timedelta(seconds=2) # Calculate evaluation variables - converts # to measurement from prediction space eval_measurement_prediction = StateMeasurementPrediction( measurement_model.matrix() @ prediction.state_vector) eval_posterior_position = prediction.state_vector[[0, 2]] + \ alpha * (measurement.state_vector - eval_measurement_prediction.state_vector)
if (arg.find(flag) >= 0): found = True return arg.split(flag)[1] if (found == False): raise Exception('Required argument {} was not found'.format(flag)) args = sys.argv data_file = get_arg(args, '--datafile=') transition_model = CombinedLinearGaussianTransitionModel( [ConstantVelocity(0.01), ConstantVelocity(0.01)]) covar = CovarianceMatrix(np.array([[1, 0], [0, 1]])) # [[AA, AF],[AF, FF]] measurement_model = LinearGaussian(ndim_state=4, mapping=[0, 2], noise_covar=covar) from stonesoup.predictor.kalman import KalmanPredictor predictor = KalmanPredictor(transition_model) from stonesoup.updater.kalman import KalmanUpdater updater = KalmanUpdater(measurement_model) from stonesoup.types.state import GaussianState prior = GaussianState([[0.5], [0], [0.5], [0]], np.diag([1, 0, 1, 0]), timestamp=datetime.now()) detector1 = beamformers_2d.capon(data_file) detector2 = beamformers_2d.rjmcmc(data_file)
from stonesoup.types.detection import Detection from stonesoup.types.hypothesis import SingleHypothesis from stonesoup.types.prediction import (GaussianStatePrediction, GaussianMeasurementPrediction) from stonesoup.types.state import GaussianState, SqrtGaussianState from stonesoup.updater.kalman import (KalmanUpdater, ExtendedKalmanUpdater, UnscentedKalmanUpdater, SqrtKalmanUpdater, IteratedKalmanUpdater) @pytest.mark.parametrize( "UpdaterClass, measurement_model, prediction, measurement", [ ( # Standard Kalman KalmanUpdater, LinearGaussian( ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])), GaussianStatePrediction( np.array([[-6.45], [0.7]]), np.array([[4.1123, 0.0013], [0.0013, 0.0365] ])), Detection(np.array([[-6.23]]))), ( # Extended Kalman ExtendedKalmanUpdater, LinearGaussian( ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])), GaussianStatePrediction( np.array([[-6.45], [0.7]]), np.array([[4.1123, 0.0013], [0.0013, 0.0365] ])), Detection(np.array([[-6.23]]))), ( # Unscented Kalman UnscentedKalmanUpdater, LinearGaussian(
# %% # 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.models.measurement.linear import LinearGaussian measurement_model = LinearGaussian(ndim_state=6, mapping=[0, 2, 4, 5], noise_covar=np.diag([1**2, 1**2, 3**2, 3**2])) # %% # Defining the tracker components # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # With the state-space models defined, we proceed to build our tracking components # # Filtering # ********* # Since we have assumed Linear-Gaussian models, we will be using a Kalman Filter to perform # filtering of the underlying single-target densities. This is done by making use of the # :class:`~.KalmanPredictor` and :class:`~.KalmanUpdater` classes, which we define below: from stonesoup.predictor.kalman import KalmanPredictor predictor = KalmanPredictor(transition_model) # %% from stonesoup.updater.kalman import KalmanUpdater
def test_sqrt_kalman(): measurement_model = LinearGaussian(ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])) prediction = GaussianStatePrediction( np.array([[-6.45], [0.7]]), np.array([[4.1123, 0.0013], [0.0013, 0.0365]])) sqrt_prediction = SqrtGaussianState(prediction.state_vector, np.linalg.cholesky(prediction.covar)) measurement = Detection(np.array([[-6.23]])) # Calculate evaluation variables eval_measurement_prediction = GaussianMeasurementPrediction( measurement_model.matrix() @ prediction.mean, measurement_model.matrix() @ prediction.covar @ measurement_model.matrix().T + measurement_model.covar(), cross_covar=prediction.covar @ measurement_model.matrix().T) kalman_gain = eval_measurement_prediction.cross_covar @ np.linalg.inv( eval_measurement_prediction.covar) eval_posterior = GaussianState( prediction.mean + kalman_gain @ (measurement.state_vector - eval_measurement_prediction.mean), prediction.covar - kalman_gain @ eval_measurement_prediction.covar @ kalman_gain.T) # Test Square root form returns the same as standard form updater = KalmanUpdater(measurement_model=measurement_model) sqrt_updater = SqrtKalmanUpdater(measurement_model=measurement_model, qr_method=False) qr_updater = SqrtKalmanUpdater(measurement_model=measurement_model, qr_method=True) posterior = updater.update( SingleHypothesis(prediction=prediction, measurement=measurement)) posterior_s = sqrt_updater.update( SingleHypothesis(prediction=sqrt_prediction, measurement=measurement)) posterior_q = qr_updater.update( SingleHypothesis(prediction=sqrt_prediction, measurement=measurement)) assert np.allclose(posterior_s.mean, eval_posterior.mean, 0, atol=1.e-14) assert np.allclose(posterior_q.mean, eval_posterior.mean, 0, atol=1.e-14) assert np.allclose(posterior.covar, eval_posterior.covar, 0, atol=1.e-14) assert np.allclose(eval_posterior.covar, posterior_s.sqrt_covar @ posterior_s.sqrt_covar.T, 0, atol=1.e-14) assert np.allclose(posterior.covar, posterior_s.sqrt_covar @ posterior_s.sqrt_covar.T, 0, atol=1.e-14) assert np.allclose(posterior.covar, posterior_q.sqrt_covar @ posterior_q.sqrt_covar.T, 0, atol=1.e-14) # I'm not sure this is going to be true in all cases. Keep in order to find edge cases assert np.allclose(posterior_s.covar, posterior_q.covar, 0, atol=1.e-14) # Next create a prediction with a covariance that will cause problems prediction = GaussianStatePrediction( np.array([[-6.45], [0.7]]), np.array([[1e24, 1e-24], [1e-24, 1e24]])) sqrt_prediction = SqrtGaussianState(prediction.state_vector, np.linalg.cholesky(prediction.covar)) posterior = updater.update( SingleHypothesis(prediction=prediction, measurement=measurement)) posterior_s = sqrt_updater.update( SingleHypothesis(prediction=sqrt_prediction, measurement=measurement)) posterior_q = qr_updater.update( SingleHypothesis(prediction=sqrt_prediction, measurement=measurement)) # The new posterior will be eval_posterior = GaussianState( prediction.mean + kalman_gain @ (measurement.state_vector - eval_measurement_prediction.mean), np.array([[0.04, 0], [ 0, 1e24 ]])) # Accessed by looking through the Decimal() quantities... # It's actually [0.039999999999 1e-48], [1e-24 1e24 + 1e-48]] ish # Test that the square root form succeeds where the standard form fails assert not np.allclose(posterior.covar, eval_posterior.covar, rtol=5.e-3) assert np.allclose(posterior_s.sqrt_covar @ posterior_s.sqrt_covar.T, eval_posterior.covar, rtol=5.e-3) assert np.allclose(posterior_q.sqrt_covar @ posterior_s.sqrt_covar.T, eval_posterior.covar, rtol=5.e-3)
birth_rate=birth_rate, death_probability=death_probability) # %% # Initialise the measurement models # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ # The simulated ground truth will then be passed to a simple detection simulator. This again has a # number of configurable parameters, e.g. where clutter is generated and at what rate, and # detection probability. This implements similar logic to the code in the previous tutorial section # :ref:`auto_tutorials/09_Initiators_&_Deleters:Generate Detections and Clutter`. from stonesoup.simulator.simple import SimpleDetectionSimulator from stonesoup.models.measurement.linear import LinearGaussian # initialise the measurement model measurement_model_covariance = np.diag([0.25, 0.25]) measurement_model = LinearGaussian(4, [0, 2], measurement_model_covariance) # probability of detection probability_detection = 0.9 # clutter will be generated uniformly in this are around the target clutter_area = np.array([[-1, 1], [-1, 1]]) * 30 clutter_rate = 1 # %% # The detection simulator detection_sim = SimpleDetectionSimulator( groundtruth=groundtruth_sim, measurement_model=measurement_model, detection_probability=probability_detection, meas_range=clutter_area,
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])
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 from stonesoup.measures import Mahalanobis hypothesiser = DistanceHypothesiser(predictor, updater, measure=Mahalanobis(), missed_distance=3) from stonesoup.dataassociator.neighbour import NearestNeighbour data_associator = NearestNeighbour(hypothesiser) from stonesoup.types.state import GaussianState prior = GaussianState([[0], [1], [0], [1]], np.diag([0.25, 0.1, 0.25, 0.1]), timestamp=start_time)
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")
# where :math:`\omega` is set to 5 initially (but again, feel free to play around). # %% # We're going to need a :class:`~.Detection` type to # store the detections, and a :class:`~.LinearGaussian` measurement model. from stonesoup.types.detection import Detection from stonesoup.models.measurement.linear import LinearGaussian # %% # The linear Gaussian measurement model is set up by indicating the number of dimensions in the # state vector and the dimensions that are measured (so specifying :math:`H_k`) and the noise # covariance matrix :math:`R`. 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.array([ [5, 0], # Covariance matrix for Gaussian PDF [0, 5] ])) # %% # Check the output is as we expect measurement_model.matrix() # %% measurement_model.covar() # %% # Generate the measurements measurements = [] for state in truth:
# 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, mapping=(0, 2), noise_covar=np.array([[1, 0], [0, 1]])) # specify predictors predictor_radar = KalmanPredictor(transition_model_radar) predictor_ais = KalmanPredictor(transition_model_ais) # specify updaters updater_radar = KalmanUpdater(measurement_model_radar)
# 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. from stonesoup.types.state import GaussianState from stonesoup.types.array import StateVector, CovarianceMatrix # Arbitrary initial state of target. initial_state = GaussianState( StateVector([[0], [0], [0], [0]]), CovarianceMatrix(np.diag([1000000, 10, 1000000, 10])))
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])) # %% # With these models we can now create the predictor and updater components of the tracker, passing # in the respective models. from stonesoup.predictor.kalman import KalmanPredictor predictor = KalmanPredictor(transition_model) # %% from stonesoup.updater.kalman import KalmanUpdater updater = KalmanUpdater(measurement_model) # %%
axm.set_ylim(0, 25) axm.set_xlim(0, 25) # Plot ground truth. for truth in truths: axm.plot( [state.state_vector[0] for state in truth], [state.state_vector[2] for state in truth], linestyle="--", ) # Generate measurements. all_measurements = [] measurement_model = LinearGaussian(ndim_state=4, mapping=(0, 2), noise_covar=np.array([[0.75, 0], [0, 0.75]])) prob_detect = 0.9 # 90% chance of detection. for k in range(20): measurement_set = set() for truth in truths: # Generate actual detection from the state with a 10% chance that no detection is received. if np.random.rand() <= prob_detect: measurement = measurement_model.function(truth[k], noise=True) measurement_set.add( TrueDetection(state_vector=measurement, groundtruth_path=truth, timestamp=truth[k].timestamp))
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 ) detection_sim = SimpleDetectionSimulator( groundtruth=groundtruth_sim,
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(0.05), ConstantVelocity(0.05))) # Use a Linear Gaussian measurement model like in 01-Kalman example 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([[10, 0], # Covariance matrix for Gaussian PDF [0, 10]]) ) #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. from stonesoup.types.state import GaussianState from stonesoup.types.array import StateVector, CovarianceMatrix # Arbitrary initial state of target. initial_state=GaussianState( StateVector([[0], [0], [0], [0]]), CovarianceMatrix(np.diag([1000000, 10, 1000000, 10]))),