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
birth_rate=0.2, death_probability=0.05, seed=3 ) detection_sim = SimpleDetectionSimulator( groundtruth=groundtruth_sim, measurement_model=measurement_model, meas_range=np.array([[-1, 1], [-1, 1]]) * 5000, # Area to generate clutter detection_probability=0.9, clutter_rate=1, seed=4 ) # Filter predictor = KalmanPredictor(transition_model) updater = KalmanUpdater(measurement_model) # Data Associator hypothesiser = DistanceHypothesiser(predictor, updater, Mahalanobis(), missed_distance=3) data_associator = GNNWith2DAssignment(hypothesiser) # Initiator & Deleter deleter = CovarianceBasedDeleter(covar_trace_thresh=1E3) initiator = MultiMeasurementInitiator( GaussianState(np.array([[0], [0], [0], [0]]), np.diag([0, 100, 0, 1000])), measurement_model=measurement_model, deleter=deleter, data_associator=data_associator, updater=updater, min_points=3, )
# # :math:`\mathbf{z}_k - H_{k}\mathbf{x}_{k|k-1}` is known as the *innovation* and :math:`S_k` the # *innovation covariance*; :math:`K_k` is the *Kalman gain*. # %% # Constructing a predictor and updater in Stone Soup is simple. In a nice division of # responsibility, a :class:`~.Predictor` takes a :class:`~.TransitionModel` as input and # an :class:`~.Updater` takes a :class:`~.MeasurementModel` as input. Note that for now we're using # the same models used to generate the ground truth and the simulated measurements. This won't # usually be possible and it's an interesting exercise to explore what happens when these # parameters are mismatched. from stonesoup.predictor.kalman import KalmanPredictor predictor = KalmanPredictor(transition_model) from stonesoup.updater.kalman import KalmanUpdater updater = KalmanUpdater(measurement_model) # %% # Run the Kalman filter # ^^^^^^^^^^^^^^^^^^^^^ # Now we have the components, we can execute the Kalman filter estimator on the simulated data. # # In order to start, we'll need to create the first prior estimate. We're going to use the # :class:`~.GaussianState` we mentioned earlier. As the name suggests, this parameterises the state # as :math:`\mathcal{N}(\mathbf{x}_0, P_0)`. By happy chance the initial values are chosen to match # the truth quite well. You might want to manipulate these to see what happens. from stonesoup.types.state import GaussianState prior = GaussianState([[0], [1], [0], [1]], np.diag([1.5, 0.5, 1.5, 0.5]), timestamp=start_time)
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 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])
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
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)
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
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) updater_ais = KalmanUpdater(measurement_model_ais) # create prior, both trackers use the same starting point prior_radar = GaussianState([0, 1, 0, 1], np.diag([1.5, 0.5, 1.5, 0.5]), timestamp=start_time) prior_ais = GaussianState([0, 1, 0, 1], np.diag([1.5, 0.5, 1.5, 0.5]), timestamp=start_time) # create list for storing kalman gains kf_gains_radar = [] kf_gains_ais = [] # create list for storing transition_noise_covar
class kalman_filter_independent_fusion: """ 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 # 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 track(self, start_time, measurements_radar, measurements_ais, fusion_rate=1): """ returns fused tracks. """ time = start_time tracks_radar = Track() tracks_ais = Track() tracks_fused = [] 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) # for each new_meas, perform a prediction and an update for measurement in new_measurements_ais: prediction = self.predictor_ais.predict( self.prior_ais, timestamp=measurement.timestamp) hypothesis = SingleHypothesis(prediction, measurement) post = self.updater_ais.update(hypothesis) tracks_ais.append(post) self.prior_ais = tracks_ais[-1] for measurement in new_measurements_radar: prediction = self.predictor_radar.predict( self.prior_radar, timestamp=measurement.timestamp) hypothesis = SingleHypothesis(prediction, measurement) post = self.updater_radar.update(hypothesis) tracks_radar.append(post) self.prior_radar = tracks_radar[-1] # 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_radar = self.predictor_radar.predict(self.prior_radar, timestamp=time) prediction_ais = self.predictor_ais.predict(self.prior_ais, timestamp=time) # fuse these predictions. tracks_fused.append( self._fuse_track(prediction_radar, prediction_ais)) time += timedelta(seconds=fusion_rate) return tracks_fused, tracks_radar, tracks_ais def _fuse_track(self, track_radar, track_ais): """ fuses the two tracks. Assumes that the tracks have the same timestamp """ # todo check the track-to-track association fused_posterior, fused_covar = track_to_track_fusion.fuse_independent_tracks( track_radar, track_ais) estimate = GaussianState(fused_posterior, fused_covar, timestamp=track_radar.timestamp) return estimate def _fuse_tracks(self, tracks_radar, tracks_ais, fusion_rate=1): tracks_fused = [] for track_radar in tracks_radar: # find a track in tracks_radar with the same timestamp estimate = track_radar for track_ais in tracks_ais: if track_ais.timestamp == track_radar.timestamp: # same_target = track_to_track_association.test_association_independent_tracks(track_radar, track_ais, # 0.01) same_target = True # ignore association for now if same_target: fused_posterior, fused_covar = track_to_track_fusion.fuse_independent_tracks( track_radar, track_ais) estimate = GaussianState( fused_posterior, fused_covar, timestamp=track_radar.timestamp) break tracks_fused.append(estimate) return tracks_fused
class kalman_filter_independent_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): # 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) ]) self.start_time = start_time self.measurements_radar = measurements_radar self.measurements_ais = measurements_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 track(self): self.tracks_radar = Track() for measurement in self.measurements_radar: prediction = self.predictor_radar.predict( self.prior_radar, timestamp=measurement.timestamp) hypothesis = SingleHypothesis(prediction, measurement) post = self.updater_radar.update(hypothesis) self.tracks_radar.append(post) self.prior_radar = self.tracks_radar[-1] self.tracks_ais = Track() for measurement in self.measurements_ais: prediction = self.predictor_radar.predict( self.prior_ais, timestamp=measurement.timestamp) hypothesis = SingleHypothesis(prediction, measurement) post = self.updater_ais.update(hypothesis) self.tracks_ais.append(post) self.prior_ais = self.tracks_ais[-1] self.tracks_fused = self._fuse_tracks(self.tracks_radar, self.tracks_ais) return self.tracks_fused, self.tracks_radar, self.tracks_ais def _fuse_tracks(self, tracks_radar, tracks_ais): tracks_fused = [] for track_radar in tracks_radar: # find a track in tracks_radar with the same timestamp estimate = track_radar for track_ais in tracks_ais: if track_ais.timestamp == track_radar.timestamp: # same_target = track_to_track_association.test_association_independent_tracks(track_radar, track_ais, # 0.01) same_target = True # ignore association for now if same_target: fused_posterior, fused_covar = track_to_track_fusion.fuse_independent_tracks( track_radar, track_ais) estimate = GaussianState( fused_posterior, fused_covar, timestamp=track_radar.timestamp) break tracks_fused.append(estimate) return tracks_fused
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
def test_information(UpdaterClass, measurement_model, prediction, measurement): """Tests the information form of the Kalman filter update step.""" # This is how the Kalman filter does it kupdater = KalmanUpdater(measurement_model) kposterior = kupdater.update(SingleHypothesis(prediction, measurement)) # Create the information state representation prediction_precision = np.linalg.inv(prediction.covar) info_prediction_mean = prediction_precision @ prediction.state_vector info_prediction = InformationStatePrediction(info_prediction_mean, prediction_precision) # Initialise a information form of the Kalman updater updater = UpdaterClass(measurement_model=measurement_model) # Perform and assert state update (without measurement prediction) posterior = updater.update( SingleHypothesis(prediction=info_prediction, measurement=measurement)) # Check to see if the information matrix is positive definite (i.e. are all the eigenvalues # positive?) assert (np.all(np.linalg.eigvals(posterior.precision) >= 0)) # Does the measurement prediction work? assert (np.allclose( kupdater.predict_measurement(prediction).state_vector, updater.predict_measurement(info_prediction).state_vector, 0, atol=1.e-14)) # Do the assert (np.allclose( kposterior.state_vector, np.linalg.inv(posterior.precision) @ posterior.state_vector, 0, atol=1.e-14)) assert (np.allclose(kposterior.covar, np.linalg.inv(posterior.precision), 0, atol=1.e-14)) assert (np.array_equal(posterior.hypothesis.prediction, info_prediction)) assert (np.array_equal(posterior.hypothesis.measurement, measurement)) assert (posterior.timestamp == prediction.timestamp) # test that we can get to the inverse matrix class LinearGaussianwithInverse(LinearGaussian): def inverse_covar(self, **kwargs): return np.linalg.inv(self.covar(**kwargs)) meas_model_winv = LinearGaussianwithInverse(ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])) updater_winv = UpdaterClass(meas_model_winv) # Test this still works post_from_inv = updater_winv.update( SingleHypothesis(prediction=info_prediction, measurement=measurement)) # and check assert (np.allclose(posterior.state_vector, post_from_inv.state_vector, 0, atol=1.e-14)) # Can one force symmetric covariance? updater.force_symmetric_covariance = True posterior = updater.update( SingleHypothesis(prediction=info_prediction, measurement=measurement)) assert (np.allclose(posterior.precision - posterior.precision.T, np.zeros(np.shape(posterior.precision)), 0, atol=1.e-14))
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")