transition_matrixes_radar = [] transition_matrixes_ais = [] # create list for storing tracks tracks_radar = Track() tracks_ais = Track() # track for measurement in measurements_radar: prediction = predictor_radar.predict(prior_radar, timestamp=measurement.timestamp) hypothesis = SingleHypothesis(prediction, measurement) # calculate the kalman gain hypothesis.measurement_prediction = updater_radar.predict_measurement( hypothesis.prediction, measurement_model=measurement_model_radar) post_cov, kalman_gain = updater_radar._posterior_covariance(hypothesis) kf_gains_radar.append(kalman_gain) # get the transition model covar predict_over_interval = measurement.timestamp - prior_radar.timestamp transition_covars_ais.append( transition_model_ais.covar(time_interval=predict_over_interval)) transition_matrixes_ais.append( transition_model_ais.matrix(time_interval=predict_over_interval)) # update post = updater_radar.update(hypothesis) tracks_radar.append(post) prior_radar = tracks_radar[-1] for measurement in measurements_ais: prediction = predictor_radar.predict(prior_ais, timestamp=measurement.timestamp)
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
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