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 track(self, measurements_radar, measurements_ais, fusion_rate=1): """ returns fused tracks. Assumes that the rate of the radar and ais measurements are the same, and that they are synchornized. """ tracks_radar = Track() for measurement in 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] tracks_ais = Track() for measurement in measurements_ais: prediction = self.predictor_radar.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] tracks_fused = self._fuse_tracks(tracks_radar, tracks_ais, fusion_rate=fusion_rate) return tracks_fused, tracks_ais, tracks_radar
def initiate(self, detections, timestamp, **kwargs): MAX_DEV = 500. tracks = set() measurement_model = self.measurement_model for detection in detections: state_vector = measurement_model.inverse_function(detection) model_covar = measurement_model.covar() el_az_range = np.sqrt(np.diag(model_covar)) # elev, az, range std_pos = detection.state_vector[2, 0] * el_az_range[1] stdx = np.abs(std_pos * np.sin(el_az_range[1])) stdy = np.abs(std_pos * np.cos(el_az_range[1])) stdz = np.abs(detection.state_vector[2, 0] * el_az_range[0]) if stdx > MAX_DEV: print('Warning - X Deviation exceeds limit!!') if stdy > MAX_DEV: print('Warning - Y Deviation exceeds limit!!') if stdz > MAX_DEV: print('Warning - Z Deviation exceeds limit!!') C0 = np.diag(np.array([stdx, 50.0, stdy, 50.0, stdz, 10.0])**2) tracks.add( Track([ GaussianStateUpdate(state_vector, C0, SingleHypothesis(None, detection), timestamp=detection.timestamp) ])) return tracks
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
from tutorienklassen import SDFUpdater updater = SDFUpdater(measurement_model) from stonesoup.types.state import GaussianState prior = GaussianState([[0.0], [0.0], [0.0], [0.0]], np.diag([0.0, 0.0, 0.0, 0.0]), timestamp=0) from stonesoup.types.hypothesis import SingleHypothesis from stonesoup.types.track import Track track = Track() for measurement in measurements: prediction = predictor.predict(prior, timestamp=measurement.timestamp) hypothesis = SingleHypothesis(prediction, measurement) post = updater.update(hypothesis, measurement_model) track.append(post) prior = track[-1] # Plot the resulting track ax.plot([state.state_vector[0] for state in track], [state.state_vector[2] for state in track],
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 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])
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
# # Feel free to change the `state_vector` from the actual truth state vector to something # else. This would mimic if the tracker was unsure about where the objects were originating. from stonesoup.types.state import TaggedWeightedGaussianState from stonesoup.types.track import Track from stonesoup.types.array import CovarianceMatrix covar = CovarianceMatrix(np.diag([10, 5, 10, 5])) tracks = set() for truth in start_truths: new_track = TaggedWeightedGaussianState(state_vector=truth.state_vector, covar=covar**2, weight=0.25, tag='birth', timestamp=start_time) tracks.add(Track(new_track)) # %% # The hypothesier takes the current Gaussian mixture as a parameter. Here we will # initialize it to use later. reduced_states = set([track[-1] for track in tracks]) # %% # To ensure that new targets get represented in the filter, we must add a birth # component to the Gaussian mixture at every time step. The birth component's mean and # covariance must create a distribution that covers the entire state space, and its weight # must be equal to the expected number of births per timestep. For more information about # the birth component, see the algorithm provided in [#]_. If the state space is very # large, it becomes inefficient to hold a component that covers it. Alternative # implementations (as well as more dicussion about the birth component) are discussed in # [#]_.
data_associator = JPDA(hypothesiser=hypothesiser) # %% # Running the JPDA filter # ----------------------- from stonesoup.types.state import GaussianState from stonesoup.types.track import Track from stonesoup.types.array import StateVectors from stonesoup.functions import gm_reduce_single from stonesoup.types.update import GaussianStateUpdate prior1 = GaussianState([[0], [1], [0], [1]], np.diag([1.5, 0.5, 1.5, 0.5]), timestamp=start_time) prior2 = GaussianState([[0], [1], [20], [-1]], np.diag([1.5, 0.5, 1.5, 0.5]), timestamp=start_time) tracks = {Track([prior1]), Track([prior2])} for n, measurements in enumerate(all_measurements): hypotheses = data_associator.associate(tracks, measurements, start_time + timedelta(seconds=n)) # Loop through each track, performing the association step with weights adjusted according to # JPDA. for track in tracks: track_hypotheses = hypotheses[track] posterior_states = [] posterior_state_weights = [] for hypothesis in track_hypotheses: if not hypothesis:
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
from stonesoup.updater.kalman import KalmanUpdater updater = KalmanUpdater(measurement_model) from stonesoup.hypothesiser.distance import DistanceHypothesiser from stonesoup.measures import Mahalanobis hypothesiser = DistanceHypothesiser(predictor, updater, measure=Mahalanobis(), missed_distance=3) from stonesoup.dataassociator.neighbour import NearestNeighbour data_associator = NearestNeighbour(hypothesiser) from stonesoup.types.state import GaussianState prior = GaussianState([[0], [1], [0], [1]], np.diag([0.25, 0.1, 0.25, 0.1]), timestamp=start_time) from stonesoup.types.track import Track track = Track([prior]) for n, (measurements, clutter_set) in enumerate(zip(measurementss, clutter), 1): detections = clutter_set.copy() detections.update(measurements) # Add measurements and clutter together hypotheses = data_associator.associate({track}, detections, start_time+timedelta(seconds=n)) hypothesis = hypotheses[track] if hypothesis.measurement: post = updater.update(hypothesis) track.append(post) else: # When data associator says no detections are good enough, we'll keep the prediction track.append(hypothesis.prediction) # Plot the resulting track ax.plot([state.state_vector[0, 0] for state in track[1:]], # Skip plotting the prior
timestamp=start_time) # 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 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(
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
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
from stonesoup.hypothesiser.distance import DistanceHypothesiser from stonesoup.measures import Mahalanobis hypothesiser = DistanceHypothesiser(predictor, updater, measure=Mahalanobis(), missed_distance=3) from stonesoup.dataassociator.neighbour import GlobalNearestNeighbour data_associator = GlobalNearestNeighbour(hypothesiser) # Running the Kalman Filter from stonesoup.types.state import GaussianState prior_one = GaussianState([[0], [1], [0], [1]], np.diag([0.25, 0.1, 0.25, 0.1]), timestamp=start_time) prior_two = GaussianState([[0], [1], [21], [-1]], np.diag([0.25, 0.1, 0.25, 0.1]), timestamp=start_time) from stonesoup.types.track import Track tracks = {Track([prior_one]), Track([prior_two])} for n, (measurements, clutter_set) in enumerate(zip(measurementss, clutter), 1): detections = clutter_set.copy() detections.update(measurements) # Add measurements and clutter together hypotheses = data_associator.associate(tracks, detections, start_time+timedelta(seconds=n)) for track in tracks: hypothesis = hypotheses[track] if hypothesis.measurement: post = updater.update(hypothesis) track.append(post) else: # When data associator says no detections are good enough, we'll keep the prediction track.append(hypothesis.prediction) tracks_list = list(tracks) for track, color in zip(tracks_list, cycle(colors)):
from stonesoup.updater.kalman import KalmanUpdater updater = KalmanUpdater(measurement_model) from stonesoup.types.state import GaussianState prior = GaussianState([[0.5], [0], [0.5], [0]], np.diag([1, 0, 1, 0]), timestamp=datetime.now()) detector1 = beamformers_2d.capon(data_file) detector2 = beamformers_2d.rjmcmc(data_file) from stonesoup.types.hypothesis import SingleHypothesis from stonesoup.types.track import Track track1 = Track() track2 = Track() print("Capon detections:") for timestep, detections in detector1: for detection in detections: print(detection) prediction = predictor.predict(prior, timestamp=detection.timestamp) hypothesis = SingleHypothesis( prediction, detection) # Group a prediction and measurement post = updater.update(hypothesis) track1.append(post) prior = track1[-1] print("RJMCMC detections:") for timestep, detections in detector2:
s=10) """Komponenten initiieren""" transition_model = PCWAModel() predictor = SdfKalmanPredictor(transition_model) measurement_model = SDFMessmodell( 4, # Dimensionen (Position and Geschwindigkeit in 2D) (0, 2), # Mapping ) updater = SDFUpdater(measurement_model) """Erstellen eines Anfangszustandes""" prior = GaussianState([[0.0], [0.0], [0.0], [0.0]], np.diag([0.0, 0.0, 0.0, 0.0]), timestamp=0) """Erstellen einer Trajektorie, sodass das Filter arbeiten kann""" track = Track() for measurement in measurements: prediction = predictor.predict(prior, timestamp=measurement.timestamp) hypothesis = SingleHypothesis(prediction, measurement) post = updater.update(hypothesis, measurement_model) track.append(post) prior = track[-1] # Plot ax.plot([state.state_vector[0] for state in track], [state.state_vector[2] for state in track],
# # With these components, we can run the simulated data and clutter through the Kalman filter. # Create prior 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) # Loop through the predict, hypothesise, associate and update steps. from stonesoup.types.track import Track from stonesoup.types.array import StateVectors # For storing state vectors during association from stonesoup.functions import gm_reduce_single # For merging states to get posterior estimate from stonesoup.types.update import GaussianStateUpdate # To store posterior estimate track = Track([prior]) for n, measurements in enumerate(all_measurements): hypotheses = data_associator.associate({track}, measurements, start_time + timedelta(seconds=n)) hypotheses = hypotheses[track] # Loop through each hypothesis, creating posterior states for each, and merge to calculate # approximation to actual posterior state mean and covariance. posterior_states = [] posterior_state_weights = [] for hypothesis in hypotheses: if not hypothesis: posterior_states.append(hypothesis.prediction) else: posterior_state = updater.update(hypothesis)