unscented transform for UKF/UKS stuff

unscented_transform.py

from __future__ import division
import numpy as np

def unscented_transform(mu,Sigma,alpha,kappa,beta=2):
    n = mu.shape[0]
    lmbda = alpha**2*(n+kappa)-n

    points = np.empty((2*n+1,n))
    mean_weights = np.empty(2*n+1)
    cov_weights = np.empty(2*n+1)

    points[0] = mu
    mean_weights[0] = lmbda/(n+lmbda)
    cov_weights[0] = lmbda/(n+lmbda)+(1-alpha**2+beta)

    chol = np.linalg.cholesky(Sigma)
    points[1:] = np.sqrt(n+lmbda)*np.hstack((chol,-chol)).T + mu
    cov_weights[1:] = mean_weights[1:] = 1./(2*(n+lmbda))

    return points, mean_weights, cov_weights

def inverse_unscented_transform(points,mean_weights,cov_weights):
    mu = mean_weights.dot(points)
    shifted_points = points - mu
    Sigma = (shifted_points.T * cov_weights).dot(shifted_points)
    return mu, Sigma