"""Modoule to implement Merwe Sigma Points for an Unscented Kalman Filter with faster computation."""
from typing import Union, List
import numpy as np
import numpy.typing as npt
from filterpy.kalman.sigma_points import MerweScaledSigmaPoints
[docs]class MerwePoints(MerweScaledSigmaPoints):
"""Implements faster computation of Merwe Sigma Points for an Unscented Kalman Filter."""
def __init__(self, n: int, alpha: float, beta: float, kappa: float,
sqrt_method: callable = None, subtract: callable = None):
"""
Initialize faster Merwe Sigma Points.
Based on :class:`filterpy.kalman.MerweScaledSigmaPoints`.
Parameters
----------
n : int
Number of states.
alpha : float
Determins the spread of the sigma points around the mean.
Usually a small positive value (1e-3), according to [3].
beta : float
Incorporates prior knowledge of the distribution of the mean.
For Gaussian x beta=2 is optimal, according to [3].
kappa : float
Secondary scaling parameter usually set to 0 according to [4], or to 3-n according to [5].
sqrt_method : callable, optional
square root of a matrix, which has no unique answer.
Cholesky is the default choice due to its speed, by default None
subtract : callable, optional
Function that computes the difference between x and y. You will have to supply this if your
state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358).
x and y are state vectors, not scalars, by default None
"""
super().__init__(n, alpha, beta, kappa, sqrt_method, subtract)
[docs] def sigma_points(self, x: Union[float, List[float], npt.NDArray[np.float_]],
P: Union[float, List[List[float]], npt.NDArray[np.float_]]) -> npt.NDArray[np.float_]:
"""
Compute the sigma points for an unscented Unscented Kalman Filter the mean (x) and covariance(P) of the filter.
Return tuple of the sigma points and weights.
Works with both scalar and array inputs:
sigma_points (5, 9, 2) # mean 5, covariance 9
sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I
Parameters
----------
x : Union[float, List[float], npt.NDArray]
Mean of state variables, can be scalar if only one state, shape: (n, ) with n equal number of states.
P : Union[float, List[List[float]], npt.NDArray]
Covariance of the filter. If scalar, is treated as eye(n)*P, shape: (n, n) with n equal number of states.
Returns
-------
sigmas : npt.NDArray, shape: (n, 2n+1)
Two dimensional array of sigma points. Each column contains all of
the sigmas for one dimension in the problem space.
Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}
Raises
------
ValueError
Number of states of given means does not equal to number of states of initialized Merwe Sigma Points.
"""
if self.n != np.size(x):
raise ValueError(f'expected size(x) {self.n}, but size is {np.size(x)}')
n = self.n
if np.isscalar(x):
x = np.asarray([x])
if np.isscalar(P):
P = np.eye(n) * P
else:
P = np.atleast_2d(P)
lambda_ = self.alpha**2 * (n + self.kappa) - n
U = self.sqrt((lambda_ + n) * P)
return np.row_stack([x, self.subtract(x, -U), self.subtract(x, U)])