Source code for probnum.filtsmooth.gaussfiltsmooth.unscentedtransform

"""

See BFaS; p. 84f.
"""

import numpy as np

__all__ = ["UnscentedTransform"]


[docs]class UnscentedTransform: """ Used for unscented Kalman filter. """ def __init__(self, dimension, spread=1e-4, priorpar=2.0, special_scale=0.0): """ dimension : int Spatial dimensionality spread : float Spread of the sigma points around mean priorpar : float Incorporate prior knowledge about distribution of x. For Gaussians, 2.0 is optimal (see link below) special_scale : float Secondary scaling parameter. The primary parameter is computed below. Source ------ P. 7 ("Unscented transform:") of https://www.pdx.edu/biomedical-signal-processing-lab/sites/www.pdx.edu.biomedical-signal-processing-lab/files/ukf.wan_.chapt7_.pdf """ self.scale = _compute_scale(dimension, spread, special_scale) self.dimension = dimension self.mweights, self.cweights = _unscented_weights( spread, priorpar, self.dimension, self.scale )
[docs] def sigma_points(self, mean, covar): """ Sigma points. Parameters ---------- mean: np.ndarray, shape (d,) mean of Gaussian distribution covar: np.ndarray, shape (d, d) kernels of Gaussian distribution Returns ------- np.ndarray, shape (2 * d + 1, d) """ if len(mean) != self.dimension: raise ValueError("Dimensionality does not match UT") sigpts = np.zeros((2 * self.dimension + 1, self.dimension)) sqrtcovar = np.linalg.cholesky(covar) sigpts[0] = mean.copy() for idx in range(self.dimension): sigpts[idx + 1] = ( mean + np.sqrt(self.dimension + self.scale) * sqrtcovar[:, idx] ) sigpts[self.dimension + 1 + idx] = ( mean - np.sqrt(self.dimension + self.scale) * sqrtcovar[:, idx] ) return sigpts
[docs] def propagate(self, time, sigmapts, modelfct): """ Propagate sigma points. Parameters ---------- time : float Time :math:`t` which is passed on to the modelfunction. sigmapts : np.ndarray, shape=(2 N+1, N) Sigma points (N is the spatial dimension of the dynamic model) modelfct : callable, signature=(t, x, \\**kwargs) Function through which to propagate Returns ------- np.ndarray, shape=(2 N + 1, M), M is the dimension of the measurement model """ propsigpts = np.array([modelfct(time, pt) for pt in sigmapts]) return propsigpts
[docs] def estimate_statistics(self, proppts, sigpts, covmat, mpred): """ Computes predicted summary statistics, predicted mean/kernels/crosscovariance, from (propagated) sigmapoints. Not to be confused with mean and kernels resulting from the prediction step of the Bayesian filter. Hence we call it "estimate_*" instead of "predict_*". """ estmean = _estimate_mean(self.mweights, proppts) estcovar = _estimate_covar(self.cweights, proppts, estmean, covmat) estcrosscovar = _estimate_crosscovar( self.cweights, proppts, estmean, sigpts, mpred ) return estmean, estcovar, estcrosscovar
def _compute_scale(dimension, spread, special_scale): """ See BFaS; p. 83 Parameters ---------- dimension: int Spatial dimensionality of state space model spread: float Spread of sigma points around mean (1; alpha) special_scale: float Spread of sigma points around mean (2; kappa) Returns ------- float Scaling parameter for unscented transform """ return spread ** 2 * (dimension + special_scale) - dimension def _unscented_weights(spread, priorpar, dimension, scale): """ See BFaS; p. 84. Parameters ---------- spread: float Spread of sigma points around mean (alpha) priorpar: float Prior information parameter (beta) dimension : int Dimension of the state space scale : float Scaling parameter for unscented transform Returns ------- np.ndarray, shape (2 * dimension + 1,) constant mean weights. np.ndarray, shape (2 * dimension + 1,) constant kernels weights. """ mweights = _meanweights(dimension, scale) cweights = _covarweights(dimension, spread, priorpar, scale) return mweights, cweights def _meanweights(dimension, lam): """ Parameters ---------- dimension: int Spatial dimensionality of state space model lam: float Scaling parameter for unscented transform (lambda) Returns ------- np.ndarray, shape (2*dimension+1,) Constant mean weights. """ mw0 = np.ones(1) * lam / (dimension + lam) mw = np.ones(2 * dimension) / (2.0 * (dimension + lam)) return np.hstack((mw0, mw)) def _covarweights(dimension, alp, bet, lam): """ Parameters ---------- dimension: int Spatial dimensionality of state space model alp: float Spread of sigma points around mean (alpha) bet: float Prior information parameter (beta) lam: float Scaling parameter for unscented transform (lambda) Returns ------- np.ndarray, shape (2 * dimension + 1,) the constant kernels weights. """ cw0 = np.ones(1) * lam / (dimension + lam) + (1 - alp ** 2 + bet) cw = np.ones(2 * dimension) / (2.0 * (dimension + lam)) return np.hstack((cw0, cw)) def _estimate_mean(mweights, proppts): """ See BFaS; p. 88. Arguments --------- mweights: np.ndarray, shape (2*dimension + 1,) Constant mean weights for unscented transform. proppts: np.ndarray, shape (2*dimension + 1, dimension) Propagated sigma points Returns ------- np.ndarray, shape (dimension,) Estimated mean. """ return mweights @ proppts def _estimate_covar(cweights, proppts, mean, covmat): """ See BFaS; p. 88. Arguments --------- cweights: np.ndarray, shape (2*dimension + 1,) Constant kernels weights for unscented transform. proppts: np.ndarray, shape (2*dimension + 1, dimension) Propagated sigma points mean: np.ndarray, shape (dimension,) Result of _estimate_mean(...) covmat: np.ndarray, shape (dimension, dimension) Covariance of measurement model at current time. Returns ------- np.ndarray, shape (dimension, dimension) Estimated kernels. """ cent = proppts - mean empcov = cent.T @ (cweights * cent.T).T return empcov + covmat def _estimate_crosscovar(cweights, proppts, mean, sigpts, mpred): """ See BFaS; p.88. Arguments --------- cweights: np.ndarray, shape (2*dimension + 1,) Constant kernels weights for unscented transform. sigpts: np.ndarray, shape (2*dimension + 1, dimension) Sigma points mpred: np.ndarray, shape (dimension,) Predicted mean proppts: np.ndarray, shape (2*dimension + 1, dimension) Propagated sigma points mean: np.ndarray, shape (dimension,) Result of _estimate_mean(...) Returns ------- np.ndarray, shape (dimension,) Estimated kernels. """ cent_prop = proppts - mean cent_sig = sigpts - mpred empcrosscov = cent_sig.T @ (cweights * cent_prop.T).T return empcrosscov