Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix non linear constant turn transition model #844

Merged
merged 8 commits into from
Sep 1, 2023
Merged

Fix non linear constant turn transition model #844

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
ae7522b
Fix documentation for Q_t and last term of covariance matrix
orosoman-dstl Aug 21, 2023
46b7225
Fix divide by zero error and misbehaving comma in Kalman Filter tutorial
orosoman-dstl Aug 21, 2023
5ab9e20
further attempt to fix misbehaving comma in Kalman Tutorial
orosoman-dstl Aug 21, 2023
2c86480
3rd attempt to fix misbehaving comma in Kalman Tutorial
orosoman-dstl Aug 21, 2023
e83dd20
4th attempt to fix misbehaving comma in Kalman Tutorial
orosoman-dstl Aug 21, 2023
b3f3330
final comma fixing commit for Kalman Filter Tutorial
orosoman-dstl Aug 21, 2023
92f7f6b
Change dtype of Matrix to float
orosoman-dstl Aug 31, 2023
bc01e20
add s to end of "StateVectors"
orosoman-dstl Aug 31, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions docs/tutorials/01_KalmanFilterTutorial.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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}\\
Expand Down
22 changes: 9 additions & 13 deletions stonesoup/models/transition/nonlinear.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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(
Expand All @@ -100,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])
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think, like before, this should check the entire array and set any zero elements to eps.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK, I've reverted to the original code, but now with a check to ensure the Matrix is float before trying to fix zero elements.

dAngle = turn_rate * time_interval_sec
cos_dAngle = np.cos(dAngle)
sin_dAngle = np.sin(dAngle)
Expand Down Expand Up @@ -134,7 +130,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)

Expand Down