search

dstl / Stone-Soup

A software project to provide the target tracking community with a framework for the development and testing of tracking algorithms.
https://stonesoup.rtfd.io
MIT License
384 stars 126 forks source link

Problematic results with the inverse function in a measurement model #1007

Closed narykov closed 3 weeks ago

narykov commented 1 month ago

I am trying to initiate a track with SimpleMeasurementInitiator, which depends on measurement inversion, for a target observed with a moving radar described by the CartesianToElevationBearingRangeRate model. Naturally, it is difficult to determine target's velocity, as we only measure Doppler, but it seems that even this information is not projected into the target state correctly. I've put together a simple demonstration, which is discussed below.

Essentially, when the sensor is stationary, we can recover the state and generate a measurement vector with Doppler. However, it only roughly matches the originally collected Doppler (i.e., not as accurately as range or angles):

Measurement from the original state: [[Elevation(0.6154797086703873)] [Bearing(0.7853981633974483)] [17.320508075688775] [5.773502691896257]] Measurement from the recovered state: [[Elevation(0.6154797086703873)] [Bearing(0.7853981633974483)] [17.320508075688775] [5.642351136133454]]

However, if the sensor is in motion, the results diverge significantly; here are the results for the target and sensor moving with the same velocity:

Measurement from the original state: [[Elevation(0.6154797086703873)] [Bearing(0.7853981633974483)] [17.320508075688775] [0.0]]

Measurement from the recovered state: [[Elevation(0.6154797086703873)] [Bearing(0.7853981633974483)] [17.320508075688775] [-5.773502691896258]]

A possible reason lies in how the recovered state is obtained. Specifically, it appears that inverse_function does not make use of self.velocity in any way: https://github.com/dstl/Stone-Soup/blob/ab6af2b30c8588cf0a8ad70fa864c5239ab2244e/stonesoup/models/measurement/nonlinear.py#L939-L961

Introducing this line in the end appears to fix this issue (by bringing Doppler from -5.77... to 0.0):

   out_vector[self.velocity_mapping, :] = out_vector[self.velocity_mapping, :] + self.velocity

However, I wonder if the poor matching of Doppler when computed for the recovered state (5.77... against 5.64...) could be improved too.

narykov commented 1 month ago

In fact, the difference in re-generated Doppler also appears to be influenced by the value of rotation_offset. When there is no rotation offset:

rotation_offset=StateVector([np.deg2rad(0), np.deg2rad(0), np.deg2rad(0)])

the measurements are as in the very first example above, i.e.,

Measurement from the original state: [[Elevation(0.6154797086703873)] [Bearing(0.7853981633974483)] [17.320508075688775] [5.773502691896257]]

Measurement from the recovered state: [[Elevation(0.6154797086703873)] [Bearing(0.7853981633974483)] [17.320508075688775] [5.642351136133454]]

and then when rotation offset is introduced

rotation_offset=StateVector([np.deg2rad(10), np.deg2rad(0), np.deg2rad(0)])

the measurements are

Measurement from the original state: [[Elevation(0.4873920373666014)] [Bearing(0.8586784147904618)] [17.32050807568877] [5.773502691896257]]

Measurement from the recovered state: [[Elevation(0.4873920373666014)] [Bearing(0.8586784147904618)] [17.320508075688775] [5.467834349806768]]

I appreciate that measured angles are changing due to the offset, but I can't see why should Doppler from recovered target state be sensitive to rotation offset (and not only to translation offset). rotation_sensitivity