Some questions on basing tutorial 9 for real world implementation

[adielstatman]

Hi.
I am trying to implement a system that based on Tutorial 9 but has some more features, which are:
A. Besides sensors that supply measurements in x,y,z, there are sensors that supply relative measurements in polar coordinates (theta,phi and r), and I have the sensors' self-location in x,y,z.
B. the sensors supply also "Categorical" features of the detected objects, such as color. We can assume that each measurement is of [x,y,z,color] or [theta,phi,r,color], each with the sensor's self location.
C. Every measurement might be lack of some features. e.g, measurement of time 100 lacks y position, and measurement of time 110 lacks the color.

My questions are:

1. About C, for keeping a feature as 'null' I just should not feed there any value?
2. What is the metric result in a case that one has value and one is null? what about the case that the two of the arguments are 'null'? I would expect it will be attended is it is equal to the mean in numerical feature, and be a metric of 1 in categorical.
3. Should the general measurement format be [x,y,z,theta,phi,r,color], where always one of the triplets [x,y,z] or [there,phi,r] are 'null'?
   I guess this format should be constructed by stonesoup.models.measurement.nonlinear.CombinedReversibleGaussianMeasurementModel
   with the sequence
   -stonesoup.models.measurement.linear.LinearGaussian -for x,y,x
   -stonesoup.models.measurement.nonlinear.CartesianToElevationBearingRange -theta,phi and r
   -stonesoup.models.measurement.categorical.CategoricalMeasurementModel -for the color.
   Am I right?
4. Is 'translation offset' in, say, stonesoup.models.measurement.nonlinear.CartesianToElevationBearingRange should be the sensor's self location? and is 'rotation offset' is the calibration of the camera relative to the vector between the origin and the self location?
5. I'm afraid there is a typo here
   (https://stonesoup.readthedocs.io/en/v0.1b8/stonesoup.models.measurement.html#stonesoup.models.measurement.nonlinear.CartesianToBearingRange.mapping) property of the model is a 2 element vector, whose first (i.e. mapping[0]) and second (i.e. mapping[0]) elements..

Regards,
Adiel.

[sdhiscocks]

1. About C, for keeping a feature as 'null' I just should not feed there any value?
2. What is the metric result in a case that one has value and one is null? what about the case that the two of the arguments are 'null'? I would expect it will be attended is it is equal to the mean in numerical feature, and be a metric of 1 in categorical.
3. Should the general measurement format be [x,y,z,theta,phi,r,color], where always one of the triplets [x,y,z] or [there,phi,r] are 'null'?
   I guess this format should be constructed by stonesoup.models.measurement.nonlinear.CombinedReversibleGaussianMeasurementModel
   with the sequence
   -stonesoup.models.measurement.linear.LinearGaussian -for x,y,x
   -stonesoup.models.measurement.nonlinear.CartesianToElevationBearingRange -theta,phi and r
   -stonesoup.models.measurement.categorical.CategoricalMeasurementModel -for the color.
   Am I right?

So there is no need for a common measurement space, just need models that enable measurements to work in common state/track space.

For example, with sensor providing [x, y, z] measurements and sometimes [x, z] you could produce these with (assuming 3D CV state space):

xyz_model = LinearGaussian(ndim_state=6, mapping=[0, 2, 4], noise_covar=np.diag([2, 1, 3]))
detection = Detection([x, y, z], timestamp=time, measurement_model=xyz_model)

xz_model = LinearGaussian(ndim_state=6, mapping=[0, 6], noise_covar=np.diag([2, 3]))
detection = Detection([x, z], timestamp=time, measurement_model=xz_model)

or for [theta,phi,r] and [theta,phi] for example:

ebr_model = CartesianToElevationBearingRange(
    ndim_state=ndim_state,
    mapping=position_mapping,
    noise_covar=noise_covar,
    translation_offset=position,
    rotation_offset=orientation)
detection = Detection([theta, phi, r], timestamp=time, measurement_model=ebr_model)

eb_model = CartesianToElevationBearing(
    ndim_state=ndim_state,
    mapping=position_mapping,
    noise_covar=noise_covar,
    translation_offset=position,
    rotation_offset=orientation)
detection = Detection([theta, phi], timestamp=time, measurement_model=eb_model)

The joint tracking and classification is slightly more complex at the moment, as work in progress in #576 to combine that (but we welcome testers 😉 )

In interim, you could add colour for example as "metadata", so for example:

detection = Detection([theta, phi, r], timestamp=time, metadata={'color': color}, measurement_model=ebr_model)

and exploit that in data association:

from stonesoup.gater.filtered import FilteredDetectionsGater
hypothesiser = FilteredDetectionsGater(
    hypothesiser_being_wrapped,
    metadata_filter="color"
)

This would avoid associating detections that are red to tracks that are labelled blue for example.

4. Is 'translation offset' in, say, stonesoup.models.measurement.nonlinear.CartesianToElevationBearingRange should be the sensor's self location? and is 'rotation offset' is the calibration of the camera relative to the vector between the origin and the self location?

Yes, the translation offset should be the sensor's location, and rotation offset the sensors orientation. As a sensor is attached to a platform, these may be dynamic, and offset themselves from the platform.

You'll see in this example that this sensor uses the senor's position and orientation (self.position and self.orientation) as the offsets for the model;

Stone-Soup/stonesoup/sensor/passive.py

Lines 39 to 44 in 071abc8

	measurement_model = CartesianToElevationBearing(
	ndim_state=self.ndim_state,
	mapping=self.mapping,
	noise_covar=self.noise_covar,
	translation_offset=self.position,
	rotation_offset=self.orientation)

5. I'm afraid there is a typo here
   (https://stonesoup.readthedocs.io/en/v0.1b8/stonesoup.models.measurement.html#stonesoup.models.measurement.nonlinear.CartesianToBearingRange.mapping) property of the model is a 2 element vector, whose first (i.e. mapping[0]) and second (i.e. mapping[0]) elements..

Thanks

---

You may also want to look at the Multi-Sensor Moving Platform Simulation Example, as this is fusing two different types of sensor.

[adielstatman]

Thank you very much for your elaborated answer!
Should be any difference between defining all of the subsets of [x,y,z] as measurement formats,
and defining only the [x,y,z] and to miss some fields sometimes when streaming?
Adiel.

[sdhiscocks]

In terms of tracking performance? This would effect how the state gets updated, but certainly better to include all the measurements you have, as should improve estimates.

[adielstatman]

OK, thanks.

1. Regarding the 'color', I want it to be a discretion in the association, not a veto.
   Why shouldn't I use
   stonesoup.models.measurement.categorical.CategoricalMeasurementModel,
   as an element in
   nonlinear.CombinedReversibleGaussianMeasurementModel?
   Is it since the categorical is not a 'nonlinear model'?

2. I'm afraid line 269 in Plotter.py
   tracks_kwargs['color'] = plt.getp(line[0], 'color')
   is redundant. My traclings had been at the same color, and when removing it I get a veriaty of colors.

[sdhiscocks]

OK, thanks.

1. Regarding the 'color', I want it to be a discretion in the association, not a veto.
   Why shouldn't I use
   stonesoup.models.measurement.categorical.CategoricalMeasurementModel,
   as an element in
   nonlinear.CombinedReversibleGaussianMeasurementModel?
   Is it since the categorical is not a 'nonlinear model'?

The main issue is the fact that it isn't Gaussian, and such can't readily combine the models.
You could create a custom detection gater that filters by some rule, or in combination with PDA, apply some association probability.

Or, depending on the use case / sensor, you could possibly add HSV (handling Hue as a Angle) to state space and measurement space. This will allow association modelled on your uncertainty of targets colour.

2. I'm afraid line 269 in Plotter.py
   tracks_kwargs['color'] = plt.getp(line[0], 'color')
   is redundant. My traclings had been at the same color, and when removing it I get a veriaty of colors.

I think this is right. This is used for the legends so it matches the colour of the tracks (when getting variety of colours it'll use colour of the last one).

[adielstatman]

OK, thanks.
Sorry if bothering, but I think I can now sharpen my interest, and want to emphasize it in order to be sure whether it is implemented in Stonesoup:
I do not want to integrate the categorical variable in the estimation itself, e.g. to depend it on the location etc.,
but my interest is, given a detection with some continuous variables and categorical variables, how can each categorical variable (with a confidence) of this detection, be integrated in

1. The calculation of the probability to associate the detection to a track, in in stonesoup.types.hypothesis.SingleProbabilityHypothesis?
2. The calculation of the distance between a detection to a track, in stonesoup.types.hypothesis.SingleDistanceHypothesis?

In other words, how does one use/build a detection state that is integrated with continuous and categorical variables, not in the context of estimation, but in the context of the association?
N.B. The most relevant example I find for this is Xmap (Ferry, 2006) model.

[sdhiscocks]

OK, thanks. Sorry if bothering, but I think I can now sharpen my interest, and want to emphasize it in order to be sure whether it is implemented in Stonesoup: I do not want to integrate the categorical variable in the estimation itself, e.g. to depend it on the location etc., but my interest is, given a detection with some continuous variables and categorical variables, how can each categorical variable (with a confidence) of this detection, be integrated in

1. The calculation of the probability to associate the detection to a track, in in stonesoup.types.hypothesis.SingleProbabilityHypothesis?
2. The calculation of the distance between a detection to a track, in stonesoup.types.hypothesis.SingleDistanceHypothesis?

In other words, how does one use/build a detection state that is integrated with continuous and categorical variables, not in the context of estimation, but in the context of the association? N.B. The most relevant example I find for this is Xmap (Ferry, 2006) model.

So a "practical" approach not using the categorical/composite (#576), could be to use the PDAHypothesiser as a starting point, which will calculate association probability based on the measurement space. Then you could use metadata on the track and measurement to adjust the association probability per approach that makes sense for your use case/data.

Something like this may work to create custom component (NOTE: I've not tested this):

from stonesoup.hypothesiser.probability import PDAHypothesiser

class CategoryPDAHypothesiser(PDAHypothesiser):
    def hypothesise(self, track, detections, timestamp, **kwargs):
        hypotheses = super().hypothesise(track, detections, timestamp, **kwargs)
        for hypothesis in hypotheses:
            if not hypothesis:
                # skip "missed detection" hypothesis
                continue
            track_category = track.metadata.get('category')
            meas_category = hypothesis.measurement.metadata.get('category')
            meas_category_confidence = hypothesis.measurement.metadata.get('category_confidence')
            association_probability = calculate_association_probability(track_category, meas_category, meas_category_confidence)
            hypothesis.probability *= association_probability

        # Renormalise as probabilities have been modified
        hypotheses.normalise_probabilities()
        return hypotheses 

where you'll need to define calculate_association_probability function to return your association probability. The track will automatically adopt the metadata from the measurements when it's initiated/updated.

[adielstatman]

Wow! Thank you very much!

If it is OK,I have just one last thing, that can nicely summarize the issue.
For entering (say in Tutorial 9) instead of the x,y,z measurements, measurements in CartesianToElevationBearingRange model (and the self location in translation_offset in x,y,x), which other modifications should be made for an appropriate results?
When I'm trying to the same actual measurements, but in CartesianToElevationBearingRange format, I get the measurements plotting in radians, and the tracks themselves are not clustered but everything is very unstable.
If you can list the modifications should be done in Tutorial 9 to support polar coordinates- it will be great.

P.S. the examples of "Sensor Platform Simulatio"n and "Multi-Sensor Moving Platform Simulation" could help but since they are based on "simulators" it is hard for me to stream there real data, differently than in the tutorials in which I've found it easy, since the simulated GT are simple.

Thanks again,
Adiel.

[sdhiscocks]

Here is a diff against v0.1b8. This includes changing it to a 3D simulation, change from linear measurement model to CartesianToElevationBearingRange, as switching to EKF (as model now non-linear):

diff --git a/docs/tutorials/09_Initiators_&_Deleters.py b/docs/tutorials/09_Initiators_&_Deleters.py
index 604e692f..cc69266b 100644
--- a/docs/tutorials/09_Initiators_&_Deleters.py
+++ b/docs/tutorials/09_Initiators_&_Deleters.py
@@ -33,6 +33,7 @@ truths = set()  # Truths across all time
 current_truths = set()  # Truths alive at current time
 
 transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity(0.005),
+                                                          ConstantVelocity(0.005),
                                                           ConstantVelocity(0.005)])
 
 for k in range(20):
@@ -47,9 +48,9 @@ for k in range(20):
             timestamp=start_time + timedelta(seconds=k)))
     # Birth
     for _ in range(np.random.poisson(0.6)):  # Birth probability
-        x, y = initial_position = np.random.rand(2) * [20, 20]  # Range [0, 20] for x and y
-        x_vel, y_vel = (np.random.rand(2))*2 - 1  # Range [-1, 1] for x and y velocity
-        state = GroundTruthState([x, x_vel, y, y_vel], timestamp=start_time + timedelta(seconds=k))
+        x, y, z = initial_position = np.random.rand(3) * [20, 20, 20]  # Range [0, 20] for x, y and z
+        x_vel, y_vel, z_vel = (np.random.rand(3))*2 - 1  # Range [-1, 1] for x, y and z velocity
+        state = GroundTruthState([x, x_vel, y, y_vel, z, z_vel], timestamp=start_time + timedelta(seconds=k))
 
         # Add to truth set for current and for all timestamps
         truth = GroundTruthPath([state])
@@ -66,16 +67,15 @@ plotter.plot_ground_truths(truths, [0, 2])
 # -------------------------------
 # Next, generate detections with clutter just as in the previous tutorials, skipping over the truth
 # paths that weren't alive at the current time step.
-
+import copy
 from scipy.stats import uniform
 from stonesoup.types.detection import TrueDetection
 from stonesoup.types.detection import Clutter
-from stonesoup.models.measurement.linear import LinearGaussian
-measurement_model = LinearGaussian(
-    ndim_state=4,
-    mapping=(0, 2),
-    noise_covar=np.array([[0.25, 0],
-                          [0, 0.25]])
+from stonesoup.models.measurement.nonlinear import CartesianToElevationBearingRange
+measurement_model = CartesianToElevationBearingRange(
+    ndim_state=6,
+    mapping=(0, 2, 4),
+    noise_covar=np.diag([np.radians(1)**2, np.radians(1)**2, 1**2]),
     )
 all_measurements = []
 
@@ -99,12 +99,11 @@ for k in range(20):
                                               measurement_model=measurement_model))
 
             # Generate clutter at this time-step
-            truth_x = truth_state.state_vector[0]
-            truth_y = truth_state.state_vector[2]
             for _ in range(np.random.randint(2)):
-                x = uniform.rvs(truth_x - 10, 20)
-                y = uniform.rvs(truth_y - 10, 20)
-                measurement_set.add(Clutter(np.array([[x], [y]]), timestamp=timestamp,
+                clutter_state = copy.deepcopy(truth_state)
+                clutter_state.state_vector[[0, 2, 4], 0] + uniform.rvs(-10, 20, 3)
+                clutter = measurement_model.function(clutter_state)
+                measurement_set.add(Clutter(clutter, timestamp=timestamp,
                                             measurement_model=measurement_model))
     all_measurements.append(measurement_set)
 
@@ -120,8 +119,8 @@ plotter.fig
 from stonesoup.predictor.kalman import KalmanPredictor
 predictor = KalmanPredictor(transition_model)
 
-from stonesoup.updater.kalman import KalmanUpdater
-updater = KalmanUpdater(measurement_model)
+from stonesoup.updater.kalman import ExtendedKalmanUpdater
+updater = ExtendedKalmanUpdater(measurement_model)
 
 from stonesoup.hypothesiser.distance import DistanceHypothesiser
 from stonesoup.measures import Mahalanobis
@@ -159,7 +158,7 @@ deleter = CovarianceBasedDeleter(covar_trace_thresh=4)
 from stonesoup.types.state import GaussianState
 from stonesoup.initiator.simple import MultiMeasurementInitiator
 initiator = MultiMeasurementInitiator(
-    prior_state=GaussianState([[0], [0], [0], [0]], np.diag([0, 1, 0, 1])),
+    prior_state=GaussianState([[0], [0], [0], [0], [0], [0]], np.diag([0, 1, 0, 1, 0, 1])),
     measurement_model=measurement_model,
     deleter=deleter,
     data_associator=data_associator,

Also, on latest branch, you can enable 3D plotting, with further changes:

diff --git a/docs/tutorials/09_Initiators_&_Deleters.py b/docs/tutorials/09_Initiators_&_Deleters.py
index 7f5047ab..bdd820dc 100644
--- a/docs/tutorials/09_Initiators_&_Deleters.py
+++ b/docs/tutorials/09_Initiators_&_Deleters.py
@@ -57,10 +57,10 @@ for k in range(20):
         current_truths.add(truth)
         truths.add(truth)
 
-from stonesoup.plotter import Plotter
-plotter = Plotter()
+from stonesoup.plotter import Plotter, Dimension
+plotter = Plotter(Dimension.THREE)
 plotter.ax.set_ylim(-5, 25)
-plotter.plot_ground_truths(truths, [0, 2])
+plotter.plot_ground_truths(truths, [0, 2, 4])
 
 # %%
 # Generate Detections and Clutter
@@ -108,7 +108,7 @@ for k in range(20):
     all_measurements.append(measurement_set)
 
 # Plot true detections and clutter.
-plotter.plot_measurements(all_measurements, [0, 2], color='g')
+plotter.plot_measurements(all_measurements, [0, 2, 4], color='g')
 plotter.fig
 
 # %%
@@ -204,5 +204,5 @@ for n, measurements in enumerate(all_measurements):
 
 # sphinx_gallery_thumbnail_number = 3
 
-plotter.plot_tracks(all_tracks, [0, 2], uncertainty=True)
+plotter.plot_tracks(all_tracks, [0, 2, 4], uncertainty=True)
 plotter.fig

[adielstatman]

Thank you very much!
Since the transition model you have suggested ,RandomWalk with a small parameter, was great and gives perfect results for Cartesian, it gives bad results for (same data in) polar. I get the same measurement positions so there are no convention errors. I get that it successes to establish tracks, but they are are deleted immediately. Interestingly, It is the same result for Cartesian with RandomWalk(>0.3).
Do you have an idea what the transition model should be, and how can I improve the tracking?
Thanks,
Adiel.

[sdhiscocks]

Are your detections in polar coordinates? Or trying to track with state/track space in polar coordinates?

Maybe try adjusting the deleter if your having issues with tracks being deleted too quickly.

[adielstatman]

Hi.
I have succeeded to estimate tracks from detections in polar (theta, phi and range) coordinates. Thank you!
In order to input this to CartesianToElevationBearingRange model I translated them to Cartesian with sphere2cart.

Now, say I have two sources which get only theta and phi of a detection. Since they are two, a triangulation may be operated in order to estimate the range.
However, technically, I can not translate theta and phi to [x,y,z] in order to input to CartesianToElevationBearingRange measurement model. How should I perform such estimation?
I suspect a similar problem might be raised where I have the azimuth and elevation relating to the detector, and having its "rotation offset".
Thus, my question is how should I feed the angles to the measurement, but show them on Cartesian x,y space?

Thanks.