# %% # 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: measurement = measurement_model.function(state, noise=True) measurements.append(Detection(measurement, timestamp=state.timestamp)) # Plot the result ax.scatter([state.state_vector[0] for state in measurements], [state.state_vector[1] for state in measurements], color='b')
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 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" )
# %% # 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: measurement = measurement_model.function(state, noise=True) measurements.append( Detection(measurement, timestamp=state.timestamp, measurement_model=measurement_model)) # %%
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)
cross_cov_ji = [] cross_cov_ji.append(np.zeros([4, 4])) cross_cov_ij.append(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_ji.append( calc_cross_cov_estimate_error(measurement_model_ais.matrix(), measurement_model_radar.matrix(), kf_gains_ais[i], kf_gains_radar[i], transition_matrixes_ais[i], transition_covars_ais[i], cross_cov_ji[i - 1])) cross_cov_ij.append( calc_cross_cov_estimate_error(measurement_model_radar.matrix(), 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])) # TODO check whether these are the same, just transposed, aka equal (cov matrice is symmetric) # test for track association
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")