Error when adding range rate to measurements

[lgolstongov]

I was trying to adapt the UAV tutorial to use meas_covar with ndim_meas=4 instead of 3, but am encountering issues making it work. The primary change made (besides re-organizing the code) was adding a row to meas_covar and then switching to CartesianToElevationBearingRangeRate and RadarElevationBearingRangeRate accordingly. Any ideas on what is going on here?

This is the error traceback being generated:

IndexError                                Traceback (most recent call last)
File c:\StoneSoup\UAV_tutorial.py:190
    186     return 0
    189 if __name__ == '__main__':
--> 190     main()

File c:\StoneSoup\UAV_tutorial.py:152
    149 true_X = []
    150 true_Y = []
--> 152 for time, tracks in tracker:
    153     for ground_truth in ground_truth_reader.groundtruth_paths:
    154         true_X.append(ground_truth.state_vector[x_index])

File c:\Users\User\AppData\Local\miniforge3\envs\ci\Lib\site-packages\stonesoup\tracker\simple.py:56, in SingleTargetTracker.__next__(self)
     55 def __next__(self) -> tuple[datetime.datetime, set[Track]]:
---> 56     time, detections = next(self.detector_iter)
     57     if self._track is not None:
     58         associations = self.data_associator.associate(
     59             self.tracks, detections, time)

File c:\Users\User\AppData\Local\miniforge3\envs\ci\Lib\site-packages\stonesoup\buffered_generator.py:44, in BufferedGenerator.__iter__(self)
     42 for _, method in inspect.getmembers(self, predicate=inspect.ismethod):
     43     if getattr(method, 'is_generator', False):
---> 44         for data in method():
     45             self.current = data
     46             yield self.current

File c:\Users\User\AppData\Local\miniforge3\envs\ci\Lib\site-packages\stonesoup\simulator\platform.py:36, in PlatformDetectionSimulator.detections_gen(self)
     34 for sensor in platform.sensors:
     35     truths_to_be_measured = truths.union(self.platforms) - {platform}
---> 36     detections = sensor.measure(truths_to_be_measured)
     37     yield time, detections

File c:\Users\User\AppData\Local\miniforge3\envs\ci\Lib\site-packages\stonesoup\sensor\sensor.py:92, in SimpleSensor.measure(self, ground_truths, noise, **kwargs)
     86 def measure(self, ground_truths: set[GroundTruthState], noise: Union[np.ndarray, bool] = True,
     87             **kwargs) -> set[TrueDetection]:
     89     measurement_model = self.measurement_model
     91     detectable_ground_truths = [truth for truth in ground_truths
---> 92                                 if self.is_detectable(truth, measurement_model)]
     94     if noise is True:
     95         if len(detectable_ground_truths) > 1:

File c:\Users\User\AppData\Local\miniforge3\envs\ci\Lib\site-packages\stonesoup\sensor\radar\radar.py:435, in RadarElevationBearingRangeRate.is_detectable(self, state, measurement_model)
    433 if measurement_model is None:
    434     measurement_model = self.measurement_model
--> 435 measurement_vector = measurement_model.function(state, noise=False)
    436 true_range = measurement_vector[2, 0]  # Elevation(0), Bearing(1), Range(2), Range-Rate(3)
    437 return true_range <= self.max_range

File c:\Users\User\AppData\Local\miniforge3\envs\ci\Lib\site-packages\stonesoup\models\measurement\nonlinear.py:154, in _AngleNonLinearGaussianMeasurement.function(self, state, noise, **kwargs)
    153 def function(self, state, noise=False, **kwargs) -> Union[StateVector, StateVectors]:
--> 154     return self._typed_vector() + self._function(state, noise, **kwargs)

File c:\Users\User\AppData\Local\miniforge3\envs\ci\Lib\site-packages\stonesoup\models\measurement\nonlinear.py:850, in CartesianToElevationBearingRangeRate._function(self, state, noise, **kwargs)
    847 rho, phi, theta = cart2sphere(xyz_rot[0, :], xyz_rot[1, :], xyz_rot[2, :])
    849 # Determine the net velocity component in the engagement
--> 850 xyz_vel = state.state_vector[self.velocity_mapping, :] - self.velocity
    852 # Use polar to calculate range rate
    853 rr = np.einsum('ij,ij->j', xyz_pos, xyz_vel) / np.linalg.norm(xyz_pos, axis=0)

File c:\Users\User\AppData\Local\miniforge3\envs\ci\Lib\site-packages\stonesoup\types\array.py:107, in StateVector.__getitem__(self, item)
    105     item = (item, 0)
    106 # Cast here, so StateVector isn't returned with invalid shape (e.g. (n, ))
--> 107 return self._cast(super().__getitem__(item))

IndexError: index 5 is out of bounds for axis 0 with size 5

And this is the code being run:

# Adapted from https://stonesoup.readthedocs.io/en/latest/auto_demos/UAV_tutorial.html

import sys

from matplotlib import pyplot as plt
import numpy as np
from stonesoup.dataassociator.neighbour import NearestNeighbour
from stonesoup.feeder.geo import LLAtoENUConverter
from stonesoup.initiator.simple import SimpleMeasurementInitiator
from stonesoup.hypothesiser.distance import DistanceHypothesiser
from stonesoup.measures import Euclidean
from stonesoup.models.measurement.nonlinear import (
    CartesianToElevationBearingRange,
    CartesianToElevationBearingRangeRate)
from stonesoup.models.transition.linear import (
    ConstantVelocity,
    CombinedLinearGaussianTransitionModel,
    OrnsteinUhlenbeck,
    Singer)
from stonesoup.platform.base import FixedPlatform
from stonesoup.predictor.kalman import ExtendedKalmanPredictor
from stonesoup.reader.generic import CSVGroundTruthReader
from stonesoup.sensor.radar.radar import (
    RadarElevationBearingRange,
    RadarElevationBearingRangeRate)
from stonesoup.simulator.platform import PlatformDetectionSimulator
from stonesoup.tracker.simple import SingleTargetTracker
from stonesoup.types.array import CovarianceMatrix
from stonesoup.types.hypothesis import SingleHypothesis
from stonesoup.types.state import GaussianState, State
from stonesoup.types.track import Track
from stonesoup.types.update import GaussianStateUpdate
from stonesoup.updater.kalman import ExtendedKalmanUpdater


def main() -> int:
    # state parameters
    ndim_state=6
    ndim_meas=4

    if ndim_state == 6:
        position_mapping=[0, 2, 4]
        velocity_mapping=[1, 3, 5]
        x_index = 0
        y_index = 2
        prior_covariance = [0, 30.0]*3
        csv_state_vector_fields=['longitude', 'Vx m/s', 'latitude', 'Vy m/s', 'altitude (m)']
        transition_model = CombinedLinearGaussianTransitionModel(
            [OrnsteinUhlenbeck(1,0.1), OrnsteinUhlenbeck(1,0.1), OrnsteinUhlenbeck(1,0.1)])
    else:
        position_mapping=[0, 3, 6]
        velocity_mapping=[1, 4, 7]
        x_index = 0
        y_index = 3
        prior_covariance = [0, 30.0, 1]*3
        csv_state_vector_fields=['longitude', 'Vx m/s', 'ax', 'latitude', 'Vy m/s', 'ay', 'altitude (m)', 'vz', 'az']
        transition_model = CombinedLinearGaussianTransitionModel(
            [Singer(1,1), Singer(1,1), Singer(1,1)])

    # 1. Setup: transition model, measurement model, updater and predictor
    # Model coords = elev, bearing, range, (optional: range rate). Angles in radians
    if ndim_meas == 3:
        meas_covar = CovarianceMatrix(np.diag(
            [0.01**2,
             0.01**2,
             10**2]))
        meas_model = CartesianToElevationBearingRange(
                ndim_state=ndim_state,
                mapping=np.array(position_mapping),
                noise_covar=meas_covar)
    else:
        meas_covar = CovarianceMatrix(np.diag(
            [np.radians(np.sqrt(10.0))**2,
             np.radians(0.6)**2,
             3.14**2,
             5**2]))
        meas_model = CartesianToElevationBearingRangeRate(
                ndim_state=ndim_state,
                mapping=np.array(position_mapping),
                velocity_mapping=np.array(velocity_mapping),
                noise_covar=meas_covar)

    predictor = ExtendedKalmanPredictor(transition_model)
    updater = ExtendedKalmanUpdater(measurement_model=meas_model)

    # 2. Set up CSV reader & feeder
    ground_truth_reader = CSVGroundTruthReader(
        path='UAV_Rot.csv',
        state_vector_fields=csv_state_vector_fields,
        time_field='time',
        path_id_field='groupNb',
    )

    sensor_location = [-30.948, 50.297311666, 0]  # Radar position [long, lat, alt]
    ground_truth_reader = LLAtoENUConverter(ground_truth_reader, sensor_location, position_mapping) # to Cartesian (East-North-Up)

    # 3. Define Sensor, Platform and Detector
    if ndim_meas == 3:
        sensor = RadarElevationBearingRange(
            position_mapping=position_mapping,
            noise_covar=meas_covar,
            ndim_state=ndim_state,
        )
    else:
        sensor = RadarElevationBearingRangeRate(
            position_mapping=position_mapping,
            velocity_mapping=velocity_mapping,
            noise_covar=meas_covar,
            ndim_state=ndim_state,
        )

    platform = FixedPlatform(
        State([0]*ndim_state),  # Sensor at reference point, zero velocity
        position_mapping=position_mapping,
        sensors=[sensor]
    )

    # Create the detector and initialize it.
    detector = PlatformDetectionSimulator(ground_truth_reader, [platform])

    prior_state = GaussianState(
            np.array([[0]]*ndim_state),
            np.diag(prior_covariance)**2)
    initiator = SimpleMeasurementInitiator(prior_state, meas_model)

    # 5. Setup Deleter for the Tracker
    class MyDeleter:
        def delete_tracks(self, tracks):
            return set()

    deleter = MyDeleter()

    # 6. Setup Hypothesiser and Associator
    meas = Euclidean()
    hypothesiser = DistanceHypothesiser(predictor, updater, meas)
    associator = NearestNeighbour(hypothesiser)

    tracker = SingleTargetTracker(initiator,
                                deleter,
                                detector,
                                associator,
                                updater)

    # 7. Run the Tracker
    est_X=[]
    est_Y=[]
    meas_X=[]
    meas_Y=[]
    true_X = []
    true_Y = []

    for time, tracks in tracker:
        for ground_truth in ground_truth_reader.groundtruth_paths:
            true_X.append(ground_truth.state_vector[x_index])
            true_Y.append(ground_truth.state_vector[y_index])

        # Because this is a single target tracker, I know there is only 1 track.
        for track in tracks:
            #Get the corresponding measurement
            detection = track.states[-1].hypothesis.measurement
            # Convert measurement into xy
            xyz = meas_model.inverse_function(detection)
            meas_X.append(xyz[x_index])
            meas_Y.append(xyz[y_index])

            vec = track.states[-1].state_vector
            est_X.append(vec[x_index])
            est_Y.append(vec[y_index])

    fig = plt.figure(figsize=(10, 6))
    ax1 = fig.add_subplot(1, 1, 1)
    plt.plot(meas_X, meas_Y, 'xb', label='Measurements')
    ax1.plot(true_X, true_Y, 'd-k', label='Truth', markerfacecolor='None')
    ax1.legend()
    ax1.set_xlabel('X (m)')
    ax1.set_ylabel('Y (m)')

    fig = plt.figure(figsize=(10, 6))
    ax2 = fig.add_subplot(1, 1, 1)
    ax2.plot(true_X, true_Y, 'd-k', label='Truth', markerfacecolor='None')
    ax2.plot(est_X, est_Y, 'r.', label='Estimates')
    ax2.set_xlabel('X (m)')
    ax2.set_ylabel('Y (m)')
    ax2.legend()

    return 0


if __name__ == '__main__':
    main()

[hpritchett-dstl]

Hi @lgolstongov,

From reading your code, it seems that the problem is that the groundtruth is 5 dimensional (x, x velocity, y, y velocity, altitude). Since you're simulating ElevationBearingRangeRate measurements, you will need the z velocity also in the groundtruth. The error message reaffirms this by trying to index element 5 (the altitude velocity).

Since you have the altitude in the groundtruth, i suppose a quick fix could be to calculate the velocity from the data and add it as a new column in the UAV_Rot.csv. You could then add this new column name to be the 6th dimension in your groundtruth reader.

Hope this helps!

[lgolstongov]

Thanks! I thought I already had added vz to csv_state_vector_fields as the sixth element, so it was hard to see that was missing here.

With that change, there is still an issue due to the inverse_function and jacobian methods for CartesianToElevationBearingRangeRate not supporting ndim_state=9, needed if using the Singer motion parameters. I will look at submitting a PR to address that.