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

# 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()