Iterated Posterior Linearization FIlter (IPLF)

docs/tutorials/01_KalmanFilterTutorial.py

@@ -118,7 +118,7 @@
 # We note that it can sometimes be useful to fix our random number generator in order to probe a
 # particular example repeatedly.
 
-np.random.seed(1991)
+np.random.seed(19915)
 
 # %%
 # The :class:`~.ConstantVelocity` class creates a one-dimensional constant velocity model with

stonesoup/functions/__init__.py

@@ -60,8 +60,8 @@
     L = np.zeros(A.shape)
     for i in range(A.shape[0]):
         for j in range(i):
-            L[i, j] = (A[i, j] - L[i, :]@L[j, :].T) / L[j, j]
-        val = A[i, i] - L[i, :]@L[i, :].T
+            L[i, j] = (A[i, j] - L[i, :] @ L[j, :].T) / L[j, j]
+        val = A[i, i] - L[i, :] @ L[i, :].T
         L[i, i] = np.sqrt(val) if val > eps else np.sqrt(eps)
 
     if lower:
@@ -70,7 +70,7 @@
         return L.T
 
 
-def jacobian(fun, x,  **kwargs):
+def jacobian(fun, x, **kwargs):
     """Compute Jacobian through finite difference calculation
 
     Parameters
@@ -92,13 +92,14 @@
 
     # For numerical reasons the step size needs to large enough. Aim for 1e-8
     # relative to spacing between floating point numbers for each dimension
-    delta = 1e8*np.spacing(x.state_vector.astype(np.float64).ravel())
+    delta = 1e8 * np.spacing(x.state_vector.astype(np.float64).ravel())
     # But at least 1e-8
     # TODO: Is this needed? If not, note special case at zero.
     delta[delta < 1e-8] = 1e-8
 
     x2 = copy.copy(x)  # Create a clone of the input
-    x2.state_vector = np.tile(x.state_vector, ndim+1) + np.eye(ndim, ndim+1)*delta[:, np.newaxis]
+    x2.state_vector = (np.tile(x.state_vector, ndim + 1) + np.eye(ndim, ndim + 1)
+                       * delta[:, np.newaxis])
     x2.state_vector = x2.state_vector.view(StateVectors)
 
     F = fun(x2, **kwargs)
@@ -170,9 +171,9 @@
 
     # Can't use in place addition/subtraction as casting issues may arise when mixing float/int
     sigma_points[:, 1:(ndim_state + 1)] = \
-        sigma_points[:, 1:(ndim_state + 1)] + sqrt_sigma*np.sqrt(c)
+        sigma_points[:, 1:(ndim_state + 1)] + sqrt_sigma * np.sqrt(c)
     sigma_points[:, (ndim_state + 1):] = \
-        sigma_points[:, (ndim_state + 1):] - sqrt_sigma*np.sqrt(c)
+        sigma_points[:, (ndim_state + 1):] - sqrt_sigma * np.sqrt(c)
 
     # Put these sigma points into s State object list
     sigma_points_states = []
@@ -218,7 +219,7 @@
 
     points_diff = sigma_points - mean
 
-    covar = points_diff@(np.diag(covar_weights))@(points_diff.T)
+    covar = points_diff @ (np.diag(covar_weights)) @ (points_diff.T)
     if covar_noise is not None:
         covar = covar + covar_noise
     return mean.view(StateVector), covar.view(CovarianceMatrix)
@@ -285,7 +286,8 @@
 
     # Calculate cross-covariance
     cross_covar = (
-        (sigma_points-sigma_points[:, 0:1]) @ np.diag(covar_weights) @ (sigma_points_t-mean).T
+            (sigma_points - sigma_points[:, 0:1]) @ np.diag(covar_weights)
+            @ (sigma_points_t - mean).T
     ).view(CovarianceMatrix)
 
     return mean, covar, cross_covar, sigma_points_t, mean_weights, covar_weights
@@ -308,7 +310,7 @@
 
     """
 
-    rho = np.sqrt(x**2 + y**2)
+    rho = np.sqrt(x ** 2 + y ** 2)
     phi = np.arctan2(y, x)
     return (rho, phi)
 
@@ -333,9 +335,9 @@
 
     """
 
-    rho = np.sqrt(x**2 + y**2 + z**2)
+    rho = np.sqrt(x ** 2 + y ** 2 + z ** 2)
     phi = np.arctan2(y, x)
-    theta = np.arcsin(z/rho)
+    theta = np.arcsin(z / rho)
     return (rho, phi, theta)
 
 
@@ -424,7 +426,7 @@
     (float, float, float)
         A tuple of the form `(phi, theta, rho)`
     """
-    rho = np.sqrt(x**2 + y**2 + z**2)
+    rho = np.sqrt(x ** 2 + y ** 2 + z ** 2)
     phi = np.arcsin(x / rho)
     theta = np.arcsin(y / rho)
     return phi, theta, rho
@@ -590,9 +592,9 @@
         means = means.view(StateVectors)
 
     if isinstance(means, StateVectors) and weights is None:
-        weights = np.array([1/means.shape[1]]*means.shape[1])
+        weights = np.array([1 / means.shape[1]] * means.shape[1])
     elif weights is None:
-        weights = np.array([1/len(means)]*len(means))
+        weights = np.array([1 / len(means)] * len(means))
 
     n_samples = np.random.multinomial(size, weights)
     samples = np.vstack([np.random.multivariate_normal(mean.ravel(), covar, sample)
@@ -621,7 +623,7 @@
         The covariance of the reduced/single Gaussian
     """
     # Normalise weights such that they sum to 1
-    weights = weights/Probability.sum(weights)
+    weights = weights / Probability.sum(weights)
 
     # Cast means as a StateVectors, so this works with ndarray types
     means = means.view(StateVectors)
@@ -631,7 +633,8 @@
 
     # Calculate covar
     delta_means = means - mean
-    covar = np.sum(covars*weights, axis=2, dtype=np.float64) + weights*delta_means@delta_means.T
+    covar = (np.sum(covars * weights, axis=2, dtype=np.float64) + weights
+             * delta_means @ delta_means.T)
 
     return mean.view(StateVector), covar.view(CovarianceMatrix)
 
@@ -651,7 +654,7 @@
         Angle in radians in the range math: :math:`-\pi` to :math:`+\pi`
     """
 
-    x = (x+np.pi) % (2.0*np.pi)-np.pi
+    x = (x + np.pi) % (2.0 * np.pi) - np.pi
 
     return x
 
@@ -670,8 +673,8 @@
     float
         Angle in radians in the range math: :math:`-\pi/2` to :math:`+\pi/2`
     """
-    x = x % (2*np.pi)  # limit to 2*pi
-    N = x//(np.pi/2)   # Count # of 90 deg multiples
+    x = x % (2 * np.pi)  # limit to 2*pi
+    N = x // (np.pi / 2)  # Count # of 90 deg multiples
     if N == 1:
         x = np.pi - x
     elif N == 2:
@@ -762,9 +765,9 @@
 
     # Decide whether this is a StateVector or a StateVectors
     if type(a) is StateVector and type(b) is StateVector:
-        return np.sum(a*b)
+        return np.sum(a * b)
     elif type(a) is StateVectors and type(b) is StateVectors:
-        return np.atleast_2d(np.asarray(np.sum(a*b, axis=0)))
+        return np.atleast_2d(np.asarray(np.sum(a * b, axis=0)))
     else:
         raise ValueError("Inputs must be `StateVector` or `StateVectors` and of the same type")
 
@@ -794,5 +797,65 @@
         delta_t = next_t - t
         delta_w = np.random.normal(scale=np.sqrt(delta_t), size=(state_x.ndim, 1))
         a, b = fun(state_x, t)
-        state_x.state_vector = state_x.state_vector + a*delta_t + b@delta_w
+        state_x.state_vector = state_x.state_vector + a * delta_t + b @ delta_w
     return state_x.state_vector
+
+
+def slr_definition(state, pred_func, func=None, force_symmetric_covariance=True):
+    """ Statistical linear regression (SLR) is a method to linearise a nonlinear function,
+    such as that representing dynamics or measurement. The SLR parameters are the linear
+    coefficients that approximate the function in the vicinity of the state pdf. The SLR
+    parameters are obtained by computing the cross-covariance and the covariance of the
+    predicted quantities, based on a set of sigma points as implemented by the prediction
+    function 'fun'. The method presented here adapts the definition (9)-(11) as found in [1].
+
+    References
+    ----------
+    [1] Á. 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.
+
+    Parameters
+    ----------
+    state: :class:`~.State`
+        State object containing the state pdf.
+    pred_func : callable
+        Function that predicts the required statistical quantities.
+    func : callable, optional
+        User-specified function that manipulates the bias vector 'b_plus'.
+    force_symmetry : bool
+        Parameter that enforces symmetry in the covariance matrix 'Omega_plus'.
+
+    Returns
+    -------
+    :class:`~.Matrix`
+        The SLR parameter 'H_plus' representing the linear transformation matrix.
+    :class:`~.StateVector`
+        The SLR parameter 'b_plus' representing the bias vector.
+    :class:`~.CovarianceMatrix`
+        The SLR parameter 'Omega_plus' representing the noise covariance matrix.
+    """
+
+    # Get the first two moments of the state pdf
+    x_bar = state.state_vector  # mean
+    p_matrix = state.covar  # covariance
+
+    # Get the predicted quantities based on a prediction function 'fun'
+    # implemented through a set of sigma points
+    prediction = pred_func(state)
+    z_bar = prediction.state_vector.astype(float)  # mean
+    psi = prediction.cross_covar  # cross-covariance
+    phi = prediction.covar  # covariance
+
+    # Compute the SLR parameters
+    H_plus = psi.T @ np.linalg.pinv(p_matrix)
+    b_plus = z_bar - H_plus @ x_bar
+    Omega_plus = phi - H_plus @ p_matrix @ H_plus.T
+
+    if func is not None:
+        b_plus = func(b_plus)
+
+    if force_symmetric_covariance:
+        Omega_plus = (Omega_plus + Omega_plus.T) / 2
+
+    return H_plus, b_plus.view(StateVector), Omega_plus.view(CovarianceMatrix)