From ae7522bc68d051957a46961fe070ca8b8351cb00 Mon Sep 17 00:00:00 2001 From: Oliver Rosoman Date: Mon, 21 Aug 2023 11:10:52 +0100 Subject: [PATCH 1/8] Fix documentation for Q_t and last term of covariance matrix --- stonesoup/models/transition/nonlinear.py | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/stonesoup/models/transition/nonlinear.py b/stonesoup/models/transition/nonlinear.py index 37de2979c..9f351027a 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( @@ -134,7 +129,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) From 46b72254805402803f452e0dcc590d062720cdd2 Mon Sep 17 00:00:00 2001 From: Oliver Rosoman Date: Mon, 21 Aug 2023 11:17:00 +0100 Subject: [PATCH 2/8] Fix divide by zero error and misbehaving comma in Kalman Filter tutorial --- docs/tutorials/01_KalmanFilterTutorial.py | 2 +- stonesoup/models/transition/nonlinear.py | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/docs/tutorials/01_KalmanFilterTutorial.py b/docs/tutorials/01_KalmanFilterTutorial.py index cdc0283df..b499ce606 100644 --- a/docs/tutorials/01_KalmanFilterTutorial.py +++ b/docs/tutorials/01_KalmanFilterTutorial.py @@ -273,7 +273,7 @@ # ^^^^^^^^^^^^^^^^^^^^^^^^^ # # 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 +# to start with. The Kalman filter is described extensively elsewhere [#]_^, [#]_, so for the # moment we just assert that the prediction step proceeds as: # # .. math:: diff --git a/stonesoup/models/transition/nonlinear.py b/stonesoup/models/transition/nonlinear.py index 9f351027a..bfd9af0c9 100644 --- a/stonesoup/models/transition/nonlinear.py +++ b/stonesoup/models/transition/nonlinear.py @@ -3,7 +3,7 @@ import numpy as np from scipy.linalg import block_diag -from ...types.array import StateVector, StateVectors +from ...types.array import StateVector, StateVectors, Matrix from .base import TransitionModel from ..base import GaussianModel, TimeVariantModel from ...base import Property @@ -95,7 +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 - turn_rate[turn_rate == 0.] = np.finfo(float).eps + if turn_rate[0] == 0: + turn_rate = Matrix([np.finfo(float).eps]) dAngle = turn_rate * time_interval_sec cos_dAngle = np.cos(dAngle) sin_dAngle = np.sin(dAngle) From 5ab9e20661d2078c38979ea5080e403c81a2c7b9 Mon Sep 17 00:00:00 2001 From: Oliver Rosoman Date: Mon, 21 Aug 2023 12:19:17 +0100 Subject: [PATCH 3/8] further attempt to fix misbehaving comma in Kalman Tutorial --- docs/tutorials/01_KalmanFilterTutorial.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/tutorials/01_KalmanFilterTutorial.py b/docs/tutorials/01_KalmanFilterTutorial.py index b499ce606..5d43754b0 100644 --- a/docs/tutorials/01_KalmanFilterTutorial.py +++ b/docs/tutorials/01_KalmanFilterTutorial.py @@ -273,7 +273,7 @@ # ^^^^^^^^^^^^^^^^^^^^^^^^^ # # 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 +# to start with. The Kalman filter is described extensively elsewhere [#]_$^,$ [#]_, so for the # moment we just assert that the prediction step proceeds as: # # .. math:: From 2c86480d0ae9a7b3b17b6ed5e23c01c41ba865cf Mon Sep 17 00:00:00 2001 From: Oliver Rosoman Date: Mon, 21 Aug 2023 13:20:30 +0100 Subject: [PATCH 4/8] 3rd attempt to fix misbehaving comma in Kalman Tutorial --- docs/tutorials/01_KalmanFilterTutorial.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/tutorials/01_KalmanFilterTutorial.py b/docs/tutorials/01_KalmanFilterTutorial.py index 5d43754b0..465573e1d 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}\\ From e83dd20ca99998e4be3312d89829cb2cd85890f9 Mon Sep 17 00:00:00 2001 From: Oliver Rosoman Date: Mon, 21 Aug 2023 13:46:50 +0100 Subject: [PATCH 5/8] 4th attempt to fix misbehaving comma in Kalman Tutorial --- docs/tutorials/01_KalmanFilterTutorial.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/tutorials/01_KalmanFilterTutorial.py b/docs/tutorials/01_KalmanFilterTutorial.py index 465573e1d..c704330f9 100644 --- a/docs/tutorials/01_KalmanFilterTutorial.py +++ b/docs/tutorials/01_KalmanFilterTutorial.py @@ -273,7 +273,7 @@ # ^^^^^^^^^^^^^^^^^^^^^^^^^ # # 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 [#]_ math:`^,` [#]_, +# 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:: From b3f3330aa68fa7eec4eee5f414fdde941cfd2adf Mon Sep 17 00:00:00 2001 From: Oliver Rosoman Date: Mon, 21 Aug 2023 23:48:16 +0100 Subject: [PATCH 6/8] final comma fixing commit for Kalman Filter Tutorial --- docs/tutorials/01_KalmanFilterTutorial.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/tutorials/01_KalmanFilterTutorial.py b/docs/tutorials/01_KalmanFilterTutorial.py index c704330f9..f734ee08d 100644 --- a/docs/tutorials/01_KalmanFilterTutorial.py +++ b/docs/tutorials/01_KalmanFilterTutorial.py @@ -273,7 +273,7 @@ # ^^^^^^^^^^^^^^^^^^^^^^^^^ # # 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 [#]_ :math:`^,` [#]_, +# 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:: From 92f7f6ba68d599c1a2dc26dbef0725dc8c9f49b3 Mon Sep 17 00:00:00 2001 From: Oliver Rosoman Date: Thu, 31 Aug 2023 09:37:44 +0100 Subject: [PATCH 7/8] Change dtype of Matrix to float --- docs/tutorials/01_KalmanFilterTutorial.py | 2 +- stonesoup/models/transition/nonlinear.py | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/docs/tutorials/01_KalmanFilterTutorial.py b/docs/tutorials/01_KalmanFilterTutorial.py index f734ee08d..d43995a33 100644 --- a/docs/tutorials/01_KalmanFilterTutorial.py +++ b/docs/tutorials/01_KalmanFilterTutorial.py @@ -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 bfd9af0c9..9b4feffd4 100644 --- a/stonesoup/models/transition/nonlinear.py +++ b/stonesoup/models/transition/nonlinear.py @@ -3,7 +3,7 @@ import numpy as np from scipy.linalg import block_diag -from ...types.array import StateVector, StateVectors, Matrix +from ...types.array import StateVector, StateVector from .base import TransitionModel from ..base import GaussianModel, TimeVariantModel from ...base import Property @@ -95,8 +95,9 @@ 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[0] == 0: - turn_rate = Matrix([np.finfo(float).eps]) + 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) sin_dAngle = np.sin(dAngle) From bc01e20e712396643d80dcf92ed47cb8ed2ecc7c Mon Sep 17 00:00:00 2001 From: Oliver Rosoman Date: Thu, 31 Aug 2023 10:21:01 +0100 Subject: [PATCH 8/8] add s to end of "StateVectors" --- stonesoup/models/transition/nonlinear.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/models/transition/nonlinear.py b/stonesoup/models/transition/nonlinear.py index 9b4feffd4..d9d49d599 100644 --- a/stonesoup/models/transition/nonlinear.py +++ b/stonesoup/models/transition/nonlinear.py @@ -3,7 +3,7 @@ import numpy as np from scipy.linalg import block_diag -from ...types.array import StateVector, StateVector +from ...types.array import StateVector, StateVectors from .base import TransitionModel from ..base import GaussianModel, TimeVariantModel from ...base import Property