Iterated Posterior Linearization Smoother (IPLS)

stonesoup/functions/__init__.py

@@ -860,6 +860,42 @@ def sde_euler_maruyama_integration(fun, t_values, state_x0):
     return state_x.state_vector
 
 
+def slr_definition(state, fun, force_symmetry=False):
+    """ Statistical linear regression (SLR), adapts the definition (9)-(11) as found in
+    Á. F. García-Fernández, L. Svensson and S. Särkkä, "Iterated Posterior Linearization Smoother"
+    in IEEE Transactions on Automatic Control, vol. 62, no. 4, pp. 2056-2063, April 2017,
+    doi: 10.1109/TAC.2016.2592681.
+
+    """
+
+    # The prediction variable below is a type of Gaussian prediction that contains information on
+    # cross-covariance. This information is naturally available in measurement predictions, such
+    # as in GaussianMeasurementPrediction, but had to be artificially added for state prediction
+    # by introducing the AugmentedGaussianStatePrediction class.
+    prediction = fun(state)
+
+    # First two moments of the state pdf
+    x_bar = state.state_vector
+    p_matrix = state.covar
+
+    # The predicted quantities wrt the state pdf
+    # (e.g. using sigma points if UKF predictions are used in 'fun')
+    z_bar = prediction.state_vector.astype(float)
+    psi = prediction.cross_covar
+    phi = prediction.covar
+
+    # Statistical linear regression parameters of a function predicted with the quantities above
+    H_plus = psi.T @ np.linalg.inv(p_matrix)
+    b_plus = z_bar - H_plus@x_bar
+    Omega_plus = phi - H_plus@p_matrix@H_plus.T
+
+    if force_symmetry:
+        Omega_plus = (Omega_plus + Omega_plus.T) / 2
+
+    # The output is the function's SLR with respect to the state_pdf
+    return H_plus, b_plus, Omega_plus
+
+
 def gauss2cubature(state, alpha=1.0):
     r"""Evaluate the cubature points for an input Gaussian state. This is done under the assumption
     that the input state is :math:`\mathcal{N}(\mathbf{\mu}, \Sigma)` of dimension :math:`n`. We

stonesoup/functions/tests/test_functions.py

@@ -1,12 +1,15 @@
 import numpy as np
 import pytest
+import datetime
 from numpy import deg2rad
 from numpy import linalg as LA
 from pytest import approx, raises
 from scipy.linalg import LinAlgError, cholesky
 
 from ...types.array import CovarianceMatrix, Matrix, StateVector, StateVectors
 from ...types.state import GaussianState, State
+from ...types.prediction import GaussianMeasurementPrediction
+from ...types.update import GaussianStateUpdate
 from .. import (
     cart2angles,
     cart2sphere,
@@ -26,6 +29,7 @@
     rotx,
     roty,
     rotz,
+    slr_definition,
     sphere2cart,
     stochastic_cubature_rule_points,
 )
@@ -57,7 +61,7 @@ def test_cholesky_eps():
     matrix = np.array([[0.4, -0.2, 0.1],
                        [0.3, 0.1, -0.2],
                        [-0.3, 0.0, 0.4]])
-    matrix = matrix@matrix.T
+    matrix = matrix @ matrix.T
 
     cholesky_matrix = cholesky(matrix)
 
@@ -67,10 +71,10 @@ def test_cholesky_eps():
 
 def test_cholesky_eps_bad():
     matrix = np.array(
-        [[ 0.05201447,  0.02882126, -0.00569971, -0.00733617],  # noqa: E201
-         [ 0.02882126,  0.01642966, -0.00862847, -0.00673035],  # noqa: E201
-         [-0.00569971, -0.00862847,  0.06570757,  0.03251551],
-         [-0.00733617, -0.00673035,  0.03251551,  0.01648615]])
+        [[0.05201447, 0.02882126, -0.00569971, -0.00733617],  # noqa: E201
+         [0.02882126, 0.01642966, -0.00862847, -0.00673035],  # noqa: E201
+         [-0.00569971, -0.00862847, 0.06570757, 0.03251551],
+         [-0.00733617, -0.00673035, 0.03251551, 0.01648615]])
     with raises(LinAlgError):
         cholesky(matrix)
     cholesky_eps(matrix)
@@ -83,7 +87,7 @@ def test_jacobian():
     state_mean = StateVector([[3.0], [1.0]])
 
     def f(x):
-        return np.array([[1, 1], [0, 1]])@x.state_vector
+        return np.array([[1, 1], [0, 1]]) @ x.state_vector
 
     jac = jacobian(f, State(state_mean))
     assert np.allclose(jac, np.array([[1, 1], [0, 1]]))
@@ -95,23 +99,23 @@ def test_jacobian2():
     # Sample functions to compute Jacobian on
     def fun(x):
         """ function for testing scalars i.e. scalar input, scalar output"""
-        return 2*x.state_vector**2
+        return 2 * x.state_vector ** 2
 
     def fun1d(ins):
         """ test function with vector input, scalar output"""
-        out = 2*ins.state_vector[0, :]+3*ins.state_vector[1, :]
+        out = 2 * ins.state_vector[0, :] + 3 * ins.state_vector[1, :]
         return np.atleast_2d(out)
 
     def fun2d(vec):
         """ test function with 2d input and 2d output"""
         out = np.empty(vec.state_vector.shape)
-        out[0, :] = 2*vec.state_vector[0, :]**2 + 3*vec.state_vector[1, :]**2
-        out[1, :] = 2*vec.state_vector[0, :]+3*vec.state_vector[1, :]
+        out[0, :] = 2 * vec.state_vector[0, :] ** 2 + 3 * vec.state_vector[1, :] ** 2
+        out[1, :] = 2 * vec.state_vector[0, :] + 3 * vec.state_vector[1, :]
         return out
 
     x = 3
     jac = jacobian(fun, State(StateVector([[x]])))
-    assert np.allclose(jac, 4*x)
+    assert np.allclose(jac, 4 * x)
 
     x = StateVector([[1], [2]])
     # Tolerance value to use to test if arrays are equal
@@ -120,12 +124,12 @@ def fun2d(vec):
     jac = jacobian(fun1d, State(x))
     T = np.array([2.0, 3.0])
 
-    FOM = np.where(np.abs(jac-T) > tol)
+    FOM = np.where(np.abs(jac - T) > tol)
     # Check # of array elements bigger than tol
     assert len(FOM[0]) == 0
 
     jac = jacobian(fun2d, State(x))
-    T = np.array([[4.0*x[0], 6*x[1]],
+    T = np.array([[4.0 * x[0], 6 * x[1]],
                   [2, 3]])
     FOM = np.where(np.abs(jac - T) > tol)
     # Check # of array elements bigger than tol
@@ -138,7 +142,7 @@ def test_jacobian_param():
     # Sample functions to compute Jacobian on
     def fun(x, value=0.0):
         """ function for jabcobian parameter passing"""
-        return value*x.state_vector
+        return value * x.state_vector
 
     x = 4
     value = 2.0
@@ -151,14 +155,13 @@ def test_jacobian_large_values():
     state = State(StateVector([[1E10], [1.0]]))
 
     def f(x):
-        return x.state_vector**2
+        return x.state_vector ** 2
 
     jac = jacobian(f, state)
     assert np.allclose(jac, np.array([[2e10, 0.0], [0.0, 2.0]]))
 
 
 def test_gm_reduce_single():
-
     means = StateVectors([StateVector([1, 2]), StateVector([3, 4]), StateVector([5, 6])])
     covars = np.stack([[[1, 1], [1, 0.7]],
                        [[1.2, 1.4], [1.3, 2]],
@@ -204,8 +207,8 @@ def test_elevation():
 @pytest.mark.parametrize(
     "mean",
     [
-        1,      # int
-        1.0     # float
+        1,  # int
+        1.0  # float
     ]
 )
 def test_gauss2sigma(mean):
@@ -220,10 +223,10 @@ def test_gauss2sigma(mean):
 
 def test_gauss2sigma_bad_covar():
     covar = np.array(
-        [[ 0.05201447,  0.02882126, -0.00569971, -0.00733617],  # noqa: E201
-         [ 0.02882126,  0.01642966, -0.00862847, -0.00673035],  # noqa: E201
-         [-0.00569971, -0.00862847,  0.06570757,  0.03251551],
-         [-0.00733617, -0.00673035,  0.03251551,  0.01648615]])
+        [[0.05201447, 0.02882126, -0.00569971, -0.00733617],  # noqa: E201
+         [0.02882126, 0.01642966, -0.00862847, -0.00673035],  # noqa: E201
+         [-0.00569971, -0.00862847, 0.06570757, 0.03251551],
+         [-0.00733617, -0.00673035, 0.03251551, 0.01648615]])
     state = GaussianState([[0], [0], [0], [0]], covar)
 
     with pytest.warns(UserWarning, match="Matrix is not positive definite"):
@@ -234,19 +237,18 @@ def test_gauss2sigma_bad_covar():
     "angle",
     [
         (
-            np.array([np.pi]),  # angle
-            np.array([np.pi / 2]),
-            np.array([-np.pi]),
-            np.array([-np.pi / 2]),
-            np.array([np.pi / 4]),
-            np.array([-np.pi / 4]),
-            np.array([np.pi / 8]),
-            np.array([-np.pi / 8]),
+                np.array([np.pi]),  # angle
+                np.array([np.pi / 2]),
+                np.array([-np.pi]),
+                np.array([-np.pi / 2]),
+                np.array([np.pi / 4]),
+                np.array([-np.pi / 4]),
+                np.array([np.pi / 8]),
+                np.array([-np.pi / 8]),
         )
     ]
 )
 def test_rotations(angle):
-
     c, s = np.cos(angle), np.sin(angle)
     zero = np.zeros_like(angle)
     one = np.ones_like(angle)
@@ -275,7 +277,6 @@ def test_rotations(angle):
     ]
 )
 def test_cart_sphere_inversions(x, y, z):
-
     rho, phi, theta = cart2sphere(x, y, z)
 
     # Check sphere2cart(cart2sphere(cart)) == cart
@@ -300,10 +301,9 @@ def test_cart_sphere_inversions(x, y, z):
         (Matrix([[1, 0], [0, 1]]), Matrix([[3, 1], [1, -3]])),
         (StateVectors([[1, 0], [0, 1]]), StateVectors([[3, 1], [1, -3]])),
         (StateVectors([[1, 0], [0, 1]]), StateVector([3, 1]))
-     ]
+    ]
 )
 def test_dotproduct(state_vector1, state_vector2):
-
     # Test that they raise the right error if not 1d, i.e. vectors
     if type(state_vector1) is not type(state_vector2):
         with pytest.raises(ValueError):
@@ -331,50 +331,58 @@ def test_dotproduct(state_vector1, state_vector2):
     "means, covars, weights, size",
     [
         (
-            [np.array([10, 10]), np.array([20, 20]), np.array([30, 30])],  # means
-            [np.eye(2), np.eye(2), np.eye(2)],  # covars
-            np.array([1/3]*3),  # weights
-            20  # size
-        ), (
+                [np.array([10, 10]), np.array([20, 20]), np.array([30, 30])],  # means
+                [np.eye(2), np.eye(2), np.eye(2)],  # covars
+                np.array([1 / 3] * 3),  # weights
+                20  # size
+        ),
+        (
             StateVectors(np.array([[20, 30, 40, 50], [20, 30, 40, 50]])),  # means
             [np.eye(2), np.eye(2), np.eye(2), np.eye(2)],  # covars
-            np.array([1/4]*4),  # weights
+            np.array([1 / 4] * 4),  # weights
             20  # size
-        ), (
+        ),
+        (
             [np.array([10, 10]), np.array([20, 20]), np.array([30, 30])],  # means
             np.array([np.eye(2), np.eye(2), np.eye(2)]),  # covars
-            np.array([1/3]*3),  # weights
+            np.array([1 / 3] * 3),  # weights
             20  # size
-        ), (
+        ),
+        (
             [StateVector(np.array([10, 10])), StateVector(np.array([20, 20])),
              StateVector(np.array([30, 30]))],  # means
             [np.eye(2), np.eye(2), np.eye(2)],  # covars
-            np.array([1/3]*3),  # weights
+            np.array([1 / 3] * 3),  # weights
             20  # size
-        ), (
+        ),
+        (
             StateVector(np.array([10, 10])),  # means
             [np.eye(2)],  # covars
             np.array([1]),  # weights
             20  # size
-        ), (
+        ),
+        (
             np.array([10, 10]),  # means
             [np.eye(2)],  # covars
             np.array([1]),  # weights
             20  # size
-        ), (
+        ),
+        (
             [np.array([10, 10]), np.array([20, 20]), np.array([30, 30])],  # means
             [np.eye(2), np.eye(2), np.eye(2)],  # covars
             None,  # weights
             20  # size
-        ), (
+        ),
+        (
             StateVectors(np.array([[20, 30, 40, 50], [20, 30, 40, 50]])),  # means
             [np.eye(2), np.eye(2), np.eye(2), np.eye(2)],  # covars
             None,  # weights
             20  # size
         )
-    ], ids=["mean_list", "mean_statevectors", "3d_covar_array", "mean_statevector_list",
-            "single_statevector_mean", "single_ndarray_mean", "no_weight_mean_list",
-            "no_weight_mean_statevectors"]
+    ],
+    ids=["mean_list", "mean_statevectors", "3d_covar_array", "mean_statevector_list",
+         "single_statevector_mean", "single_ndarray_mean", "no_weight_mean_list",
+         "no_weight_mean_statevectors"]
 )
 def test_gm_sample(means, covars, weights, size):
     samples = gm_sample(means, covars, size, weights=weights)
@@ -388,6 +396,38 @@ def test_gm_sample(means, covars, weights, size):
         assert samples.shape[0] == means.shape[0]
 
 
+def test_slr_definition():
+    def identity(state):
+        return GaussianMeasurementPrediction(
+            state_vector=state.state_vector,
+            covar=state.covar,
+            timestamp=state.timestamp,
+            cross_covar=CovarianceMatrix([[1, 0],
+                                          [0, 1]]))
+
+    time1 = datetime.datetime.now()
+
+    posterior_state = GaussianStateUpdate(
+        state_vector=np.array([[7.77342961], [1.]]),
+        covar=CovarianceMatrix([[4.54274216e+00, -8.47168858e-16],
+                                [-8.47168858e-16, 2.22044604e-16]]),
+        hypothesis=None,
+        timestamp=time1)
+
+    h_matrix, b_vector, omega_cov_matrix = slr_definition(posterior_state,
+                                                          identity,
+                                                          force_symmetry=True)
+    assert np.allclose(h_matrix,
+                       np.array([[2.20131358e-01, 8.39869232e-01],
+                                 [8.39869232e-01, 4.50359965e+15]]))
+    assert np.allclose(b_vector,
+                       np.array([[5.22238476e+00],
+                                 [-4.50359965e+15]]))
+    assert np.allclose(omega_cov_matrix,
+                       np.array([[4.32261080e+00, -8.39869232e-01],
+                                 [-8.39869232e-01, -4.50359965e+15]]))
+
+
 @pytest.mark.parametrize(
     "mean, covar, alp",
     [