Problematic results with the inverse function in a measurement model

[narykov]

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:

Stone-Soup/stonesoup/models/measurement/nonlinear.py

Lines 939 to 961 in ab6af2b

	def inverse_function(self, detection, **kwargs) -> StateVector:
	theta, phi, rho, rho_rate = detection.state_vector
	x, y, z = sphere2cart(rho, phi, theta)
	# because only rho_rate is known, only the components in
	# x,y and z of the range rate can be found.
	x_rate = np.cos(phi) * np.cos(theta) * rho_rate
	y_rate = np.cos(phi) * np.sin(theta) * rho_rate
	z_rate = np.sin(phi) * rho_rate
	inv_rotation_matrix = inv(self.rotation_matrix)
	out_vector = StateVector([[0.], [0.], [0.], [0.], [0.], [0.]])
	out_vector[self.mapping, 0] = x, y, z
	out_vector[self.velocity_mapping, 0] = x_rate, y_rate, z_rate
	out_vector[self.mapping, :] = inv_rotation_matrix @ out_vector[self.mapping, :]
	out_vector[self.velocity_mapping, :] = \
	inv_rotation_matrix @ out_vector[self.velocity_mapping, :]
	out_vector[self.mapping, :] = out_vector[self.mapping, :] + self.translation_offset
	return out_vector

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]

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).