Merge pull request #844 from dstl/fix_non_linear_constant_turn

Fix non linear constant turn transition model

docs/tutorials/01_KalmanFilterTutorial.py

@@ -273,8 +273,8 @@
 # ^^^^^^^^^^^^^^^^^^^^^^^^^
 #
 # We're now ready to build a tracker. We'll use a Kalman filter as it's conceptually the simplest
-# to start with. The Kalman filter is described extensively elsewhere [#]_, [#]_, so for the
-# moment we just assert that the prediction step proceeds as:
+# to start with. The Kalman filter is described extensively elsewhere [#]_:math:`^,` [#]_,
+# so for the moment we just assert that the prediction step proceeds as:
 #
 # .. math::
 #           \mathbf{x}_{k|k-1} &= F_{k}\mathbf{x}_{k-1} + B_{k}\mathbf{u}_{k}\\
@@ -303,7 +303,7 @@
 # responsibility, a :class:`~.Predictor` takes a :class:`~.TransitionModel` as input and
 # an :class:`~.Updater` takes a :class:`~.MeasurementModel` as input. Note that for now we're using
 # the same models used to generate the ground truth and the simulated measurements. This won't
-# usually be possible and it's an interesting exercise to explore what happens when these
+# usually be possible, and it's an interesting exercise to explore what happens when these
 # parameters are mismatched.
 from stonesoup.predictor.kalman import KalmanPredictor
 predictor = KalmanPredictor(transition_model)

stonesoup/models/transition/nonlinear.py

@@ -67,16 +67,11 @@ class ConstantTurn(GaussianTransitionModel, TimeVariantModel):
 
         .. math::
              Q_t & = & \begin{bmatrix}
-                          \frac{dt^4q_x^2}{4} & \frac{dt^3q_x^2}{2} & \frac{dt^4q_xq_y}{4} &
-                              \frac{dt^3q_xq_y}{2} & \frac{dt^2q_xq_\omega}{2} \\
-                          \frac{dt^3q_x^2}{2} & dt^2q_x^2 & \frac{dt^3q_xq_y}{2} & dt^2q_xq_y &
-                              dt q_x q_\omega \\
-                          \frac{dt^4q_xq_y}{4} & \frac{dt^3q_xq_y}{2} & \frac{dt^4q_y^2}{4} &
-                              \frac{dt^3q_y^2}{2} & \frac{dt^2q_y q_\omega}{2} \\
-                          \frac{dt^3q_x q_y}{2} & dt^2q_xq_y & \frac{dt^3q_y^2}{2} &
-                              dt^2q_y^2 & dt q_y q_\omega \\
-                          \frac{dt^2q_xq_\omega}{2} & dtq_xq_\omega & \frac{dt^2q_yq_\omega}{2} &
-                              dt q_y q_\omega & q_\omega^2
+                          \frac{dt^3q_x^2}{3} & \frac{dt^2q_x^2}{2} & 0 & 0 & 0 \\
+                          \frac{dt^2q_x^2}{2} & dtq_x^2 & 0 & 0 & 0 \\
+                          0 & 0 & \frac{dt^3q_y^2}{3} & \frac{dt^2q_y^2}{2} & 0 \\
+                          0 & 0 & \frac{dt^2q_y^2}{2} & dtq_y^2 & 0 \\
+                          0 & 0 & 0 & 0 & q_\omega^2
                      \end{bmatrix}
     """
     linear_noise_coeffs: np.ndarray = Property(
@@ -100,6 +95,8 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
         sv1 = state.state_vector
         turn_rate = sv1[4, :]
         # Avoid divide by zero in the function evaluation
+        if turn_rate.dtype != float:
+            turn_rate = turn_rate.astype(float)
         turn_rate[turn_rate == 0.] = np.finfo(float).eps
         dAngle = turn_rate * time_interval_sec
         cos_dAngle = np.cos(dAngle)
@@ -134,7 +131,7 @@ def covar(self, time_interval, **kwargs):
 
         Q = np.array([[dt**3 / 3., dt**2 / 2.],
                       [dt**2 / 2., dt]])
-        C = block_diag(Q*q_x**2, Q*q_y**2, q**2/dt)
+        C = block_diag(Q*q_x**2, Q*q_y**2, q**2)
 
         return CovarianceMatrix(C)