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"
        )
Esempio n. 2
0
# (undesirable) practice and requires that a :class:`~.Prediction` and :class:`~.Detection` are
# associated explicitly. This is done by way of a :class:`~.Hypothesis`, the most simple of which
# is a :class:`~.SingleHypothesis` which associates a single predicted state with a single
# detection. There is much more detail on how the :class:`~.Hypothesis` class is used in later
# tutorials.
from stonesoup.types.hypothesis import SingleHypothesis

# %%
# With this, we'll now loop through our measurements, predicting and updating at each timestep.
# Uncontroversially, a Predictor has :meth:`predict` function and an Updater an :meth:`update` to
# do this. Storing the information is facilitated by the top-level :class:`~.Track` class which
# holds a sequence of states.
from stonesoup.types.track import Track
track = Track()
for measurement in measurements:
    prediction = predictor.predict(prior, timestamp=measurement.timestamp)
    hypothesis = SingleHypothesis(
        prediction, measurement)  # Group a prediction and measurement
    post = updater.update(hypothesis)
    track.append(post)
    prior = track[-1]

# %%
# Plot the resulting track, including uncertainty ellipses
ax.plot([state.state_vector[0] for state in track],
        [state.state_vector[2] for state in track],
        marker=".")

from matplotlib.patches import Ellipse
for state in track:
    w, v = np.linalg.eig(measurement_model.matrix() @ state.covar
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_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])
# 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(
        transition_model_ais.covar(time_interval=predict_over_interval))
    transition_matrixes_ais.append(
        transition_model_ais.matrix(time_interval=predict_over_interval))
    # update
    post = updater_radar.update(hypothesis)
    tracks_radar.append(post)
Esempio n. 6
0
    def _smooth_traj(self, traj, process_noise_std=0.5, measurement_noise_std=1):
        # Get detector
        detector = self._get_detector(traj)

        # Models
        if not isinstance(process_noise_std, (list, tuple, np.ndarray)):
            process_noise_std = [process_noise_std, process_noise_std]
        if not isinstance(measurement_noise_std, (list, tuple, np.ndarray)):
            measurement_noise_std = [measurement_noise_std, measurement_noise_std]
        transition_model = CombinedLinearGaussianTransitionModel(
            [
                ConstantVelocity(process_noise_std[0] ** 2),
                ConstantVelocity(process_noise_std[1] ** 2),
            ]
        )
        measurement_model = LinearGaussian(
            ndim_state=4,
            mapping=[0, 2],
            noise_covar=np.diag(
                [measurement_noise_std[0] ** 2, measurement_noise_std[1] ** 2]
            ),
        )
        # Predictor and updater
        predictor = KalmanPredictor(transition_model)
        updater = KalmanUpdater(measurement_model)
        # Initiator
        state_vector = StateVector([0.0, 0.0, 0.0, 0.0])
        covar = CovarianceMatrix(np.diag([0.0, 0.0, 0.0, 0.0]))
        prior_state = GaussianStatePrediction(state_vector, covar)
        initiator = SimpleMeasurementInitiator(prior_state, measurement_model)
        # Filtering
        track = None
        for i, (timestamp, detections) in enumerate(detector):
            if i == 0:
                tracks = initiator.initiate(detections, timestamp)
                track = tracks.pop()
            else:
                detection = detections.pop()
                prediction = predictor.predict(track.state, timestamp=timestamp)
                hypothesis = SingleHypothesis(prediction, detection)
                posterior = updater.update(hypothesis)
                track.append(posterior)
        # Smoothing
        smoother = KalmanSmoother(transition_model)
        smooth_track = smoother.smooth(track)

        # Create new trajectory
        if traj.is_latlon:
            df = traj.df.to_crs("EPSG:3395")
            df.geometry = [
                Point(state.state_vector[0], state.state_vector[2])
                for state in smooth_track
            ]
            df.to_crs(traj.crs, inplace=True)
        else:
            df = traj.df.copy()
            df.geometry = [
                Point(state.state_vector[0], state.state_vector[2])
                for state in smooth_track
            ]
        new_traj = Trajectory(df, traj.id)
        return new_traj
class 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
Esempio n. 8
0
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
Esempio n. 9
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:
    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)
        track2.append(post)
Esempio n. 10
0
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
Esempio n. 11
0
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")