SimpleMeasurementInitiator new state definition

[mattbrown11]

I'm not sure I follow the intent of the existing implementation of SimpleMeasurementInitiator.initiate. It establishes prior_state_vector from the prior state passed during init and state_vector = measurement_model.inverse_function(detection) as the state vector implied by the measurement, which makes sense. But, then the it zeros out the mapped_dimensions component from prior_state_vector and prior_covar

https://github.com/dstl/Stone-Soup/blob/68f5dd964fa0b5ff6a9af93c5e64f8243af7ad05/stonesoup/initiator/simple.py#L140

and then simply adds prior_state_vector and state_vector to form the new state:

https://github.com/dstl/Stone-Soup/blob/68f5dd964fa0b5ff6a9af93c5e64f8243af7ad05/stonesoup/initiator/simple.py#L145

For the mapped_dimensions, this takes the values entirely from state_vector, but for the other dimensions, they are summed.

Currently, you can happen to achieve somewhat reasonable behavior if you just pass in all zeros for the prior state vector. But otherwise, you can get strange results.

[sdhiscocks]

So the result should be that for the mapped elements from the measurement, the values from the measurement are used. And the elements that are not mapped, they are taken from the prior.

import datetime

import numpy as np

from stonesoup.initiator.simple import SimpleMeasurementInitiator
from stonesoup.models.measurement.linear import LinearGaussian
from stonesoup.types.detection import Detection
from stonesoup.types.state import GaussianState

timestamp = datetime.datetime.now()
detection = Detection([1, 2], timestamp)
prior = GaussianState([10, 20, 30, 40], np.diag([10, 20, 30, 40]))
model = LinearGaussian(4, [0, 2], np.diag([100, 200]))

tracks = SimpleMeasurementInitiator(prior, model).initiate({detection}, timestamp)

track = tracks.pop()
print(track.state_vector)
print(track.covar)

Results in:

[[ 1.]   # From detection
 [20.]   # From prior
 [ 2.]   # From detection
 [40.]]  # From prior

[[100.   0.   0.   0.]   # From detection
 [  0.  25.   0.   0.]   # From prior
 [  0.   0. 200.   0.]   # From detection
 [  0.   0.   0.  45.]]  # From prior

[mattbrown11]

I guess my confusion is in how I should interpret the mapping attribute.

Here, for CartesianToBearingRangeRate:

https://github.com/dstl/Stone-Soup/blob/68f5dd964fa0b5ff6a9af93c5e64f8243af7ad05/stonesoup/models/measurement/nonlinear.py#L695

it indicates that the mapped dimensions are the 'x', 'y', 'z' components of the state vector (i.e., [0, 2, 4]). But, I get the impression from elsewhere that it should be the components of the state vector that the measurement model contributes an estimate for. This latter interpretation explains the code block here:

https://github.com/dstl/Stone-Soup/blob/68f5dd964fa0b5ff6a9af93c5e64f8243af7ad05/stonesoup/initiator/simple.py#L140

in that the measurement-estimated state_vector should necessarily be zero everywhere except the mapped_dimensions.

But currently, this is not the case for CartesianToBearingRangeRate where its mapping=[0, 2, 4] even though this model does provide a direct estimate for the state velocity components. It defines a seperate:

velocity_mapping: Tuple[int, int, int] = Property( default=(1, 3, 5), doc="Mapping to the targets velocity within its state space")

[sdhiscocks]

So the idea behind the mapping was identify the relevant components between the state space and the measurement space, primarily, and initially, for translating the state space to measurement space for the predicted measurement.

I think it's probably a assumption by the measurement initiator that the mapping is present and correct for this; and we probably need to double check the models.

So there is an issue with the CartesianToBearingRangeRate, this was debated a fair amount in #285 about the model only being partially reversible (and turns out in fact having velocity_mapping ends up useful in initiator case as only reversible elements (position) are used) and didn't really react a conclusion on best way forward.