Open mattbrown11 opened 1 year ago
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
I guess my confusion is in how I should interpret the mapping
attribute.
Here, for CartesianToBearingRangeRate
:
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:
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")
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.
I'm not sure I follow the intent of the existing implementation of
SimpleMeasurementInitiator.initiate
. It establishesprior_state_vector
from the prior state passed during init andstate_vector = measurement_model.inverse_function(detection)
as the state vector implied by the measurement, which makes sense. But, then the it zeros out themapped_dimensions
component fromprior_state_vector
andprior_covar
https://github.com/dstl/Stone-Soup/blob/68f5dd964fa0b5ff6a9af93c5e64f8243af7ad05/stonesoup/initiator/simple.py#L140
and then simply adds
prior_state_vector
andstate_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 fromstate_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.