diff --git a/docs/tutorials/01_KalmanFilterTutorial.py b/docs/tutorials/01_KalmanFilterTutorial.py index cdc0283df..d43995a33 100644 --- a/docs/tutorials/01_KalmanFilterTutorial.py +++ b/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) diff --git a/stonesoup/models/transition/nonlinear.py b/stonesoup/models/transition/nonlinear.py index 37de2979c..d9d49d599 100644 --- a/stonesoup/models/transition/nonlinear.py +++ b/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)