diff --git a/stonesoup/models/transition/linear.py b/stonesoup/models/transition/linear.py index 79f91e227..5c99b7977 100644 --- a/stonesoup/models/transition/linear.py +++ b/stonesoup/models/transition/linear.py @@ -703,14 +703,14 @@ class KnownTurnRate(KnownTurnRateSandwich): .. math:: Q_t & = & \begin{bmatrix} - q_x^2 \frac{dt^3}{3} & q_x^2 \frac{dt^2}{2} & + q_x \frac{dt^3}{3} & q_x \frac{dt^2}{2} & 0 & 0 \\ - q_x^2 \frac{dt^2}{2} & q_x^2 dt & + q_x \frac{dt^2}{2} & q_x dt & 0 & 0 \\ 0 & 0 & - q_y^2 \frac{dt^3}{3} & q_y^2 \frac{dt^2}{2}\\ + q_y \frac{dt^3}{3} & q_y \frac{dt^2}{2}\\ 0 & 0 & - q_y^2 \frac{dt^2}{2} & q_y^2 dt + q_y \frac{dt^2}{2} & q_y dt \end{bmatrix} """ diff --git a/stonesoup/models/transition/nonlinear.py b/stonesoup/models/transition/nonlinear.py index d9d49d599..5b1377cfd 100644 --- a/stonesoup/models/transition/nonlinear.py +++ b/stonesoup/models/transition/nonlinear.py @@ -56,22 +56,22 @@ class ConstantTurn(GaussianTransitionModel, TimeVariantModel): .. math:: F(x) & = & \begin{bmatrix} - x+ \frac{x_{vel}}{\omega}\sin\omega dt - - \frac{y_{vel}}{\omega}(1-\cos\omega dt) \\ - x_{vel}\cos\omega dt - y_{vel}\sin\omega dt \\ - y+ \frac{v_{vel}}{\omega}\sin\omega dt + - \frac{x_{vel}}{\omega}(1-\cos\omega dt) \\ - x_{vel}\sin\omega dt + y_{vel}\sin\omega dt \\ - \omega + 1 & \frac{\sin\omega dt}{\omega} & 0 & - + \frac{(1-\cos\omega dt)}{\omega} & 0 \\ + 0 & \cos\omega dt & 0 & - \sin\omega dt & 0 \\ + 0 & \frac{(1-\cos\omega dt)}{\omega} & 1 & + \frac{\sin\omega dt}{\omega} & 0 \\ + 0 & \sin\omega dt & 0 & \sin\omega dt & 0 \\ + 0 & 0 & 0 & 0 & 1 \end{bmatrix} .. math:: Q_t & = & \begin{bmatrix} - \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 + q_x\frac{dt^3}{3} & q_x\frac{dt^2}{2} & 0 & 0 & 0 \\ + q_x\frac{dt^2}{2} & q_xdt & 0 & 0 & 0 \\ + 0 & 0 & q_y\frac{dt^3}{3} & q_y\frac{dt^2}{2} & 0 \\ + 0 & 0 & q_y\frac{dt^2}{2} & q_ydt & 0 \\ + 0 & 0 & 0 & 0 & q_\omega dt \end{bmatrix} """ linear_noise_coeffs: np.ndarray = Property( @@ -131,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) + C = block_diag(Q*q_x, Q*q_y, dt*q) return CovarianceMatrix(C) diff --git a/stonesoup/models/transition/tests/conftest.py b/stonesoup/models/transition/tests/conftest.py index 5d0cdbe39..c0178d0b8 100644 --- a/stonesoup/models/transition/tests/conftest.py +++ b/stonesoup/models/transition/tests/conftest.py @@ -37,7 +37,7 @@ def covar(linear_noise_coeffs, turn_noise_coeff, time_interval): 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, Q*q_y, q*dt) return C diff --git a/stonesoup/updater/tests/test_iterated.py b/stonesoup/updater/tests/test_iterated.py index e625c0e84..625a813ad 100644 --- a/stonesoup/updater/tests/test_iterated.py +++ b/stonesoup/updater/tests/test_iterated.py @@ -79,18 +79,18 @@ def test_diekf(): # Check state vector is correct assert np.allclose( updated_state.state_vector, - StateVector([[1.810], [1.203], [0.605], [0.901], [0.]]), + StateVector([[1.812], [1.211], [0.603], [0.897], [0.]]), atol=1e-3) # Check covariance matrix is correct assert np.allclose( updated_state.covar, CovarianceMatrix( - [[0.666, 0.167, 0.011, 0.002, 0.], - [0.167, 0.427, 0.002, -0.007, -0.008], - [0.011, 0.002, 1.604, 0.401, 0.], - [0.002, -0.007, 0.401, 0.486, 0.008], - [0., -0.008, 0., 0.008, 0.009]]), + [[0.669, 0.174, 0.012, 0.003, 0.], + [0.174, 0.467, 0.003, -0.008, -0.009], + [0.012, 0.003, 1.615, 0.420, 0.], + [0.003, -0.008, 0.420, 0.531, 0.009], + [0., -0.009, 0., 0.009, 0.044]]), atol=1.e-3) prediction = sub_predictor.predict(updated_state, time3) @@ -102,14 +102,14 @@ def test_diekf(): assert np.allclose( updated_state.state_vector, - StateVector([[2.555], [1.010], [1.226], [0.820], [0.002]]), + StateVector([[2.550], [0.999], [1.215], [0.811], [0.005]]), atol=1e-3) assert np.allclose( updated_state.covar, CovarianceMatrix( - [[5.904e-01, 2.501e-01, 2.800e-02, 3.878e-04, -5.127e-03], - [2.501e-01, 2.984e-01, -4.381e-03, -2.161e-02, -1.362e-02], - [2.800e-02, -4.381e-03, 2.121e+00, 6.620e-01, 9.775e-03], - [3.878e-04, -2.161e-02, 6.620e-01, 4.395e-01, 1.740e-02], - [-5.127e-03, -1.362e-02, 9.775e-03, 1.740e-02, 1.107e-02]]), + [[0.603, 0.275, 0.027, -0.003, -0.010], + [0.275, 0.370, -0.013, -0.043, -0.037], + [0.027, -0.013, 2.182, 0.730, 0.022], + [-0.003, -0.043, 0.730, 0.548, 0.049], + [-0.010, -0.037, 0.022, 0.049, 0.078]]), atol=1e-3)