# -*- coding: utf-8 -*-
"""Copyright 2014 Roger R Labbe Jr.
filterpy library.
http://github.com/rlabbe/filterpy
Documentation at:
https://filterpy.readthedocs.org
Supporting book at:
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
This is licensed under an MIT license. See the readme.MD file
for more information.
"""
# pylint bug - warns about numpy functions which do in fact exist.
# pylint: disable=E1101
#I like aligning equal signs for readability of math
# pylint: disable=C0326
from __future__ import (absolute_import, division, print_function,
unicode_literals)
from numpy.linalg import inv, cholesky
import numpy as np
from numpy import asarray, eye, zeros, dot, isscalar, outer
from filterpy.common import dot3
[docs]class UnscentedKalmanFilter(object):
# pylint: disable=too-many-instance-attributes
# pylint: disable=C0103
""" Implements the Unscented Kalman filter (UKF) as defined by Simon J.
Julier and Jeffery K. Uhlmann [1]. Succintly, the UKF selects a set of
sigma points and weights inside the covariance matrix of the filter's
state. These points are transformed through the nonlinear process being
filtered, and are rebuilt into a mean and covariance by computed the
weighted mean and expected value of the transformed points. Read the paper;
it is excellent. My book "Kalman and Bayesian Filters in Python" [2]
explains the algorithm, develops this code, and provides examples of the
filter in use.
You will have to set the following attributes after constructing this
object for the filter to perform properly.
**Attributes**
x : numpy.array(dim_x)
state estimate vector
P : numpy.array(dim_x, dim_x)
covariance estimate matrix
R : numpy.array(dim_z, dim_z)
measurement noise matrix
Q : numpy.array(dim_x, dim_x)
process noise matrix
You may read the following attributes.
**Readable Attributes**
Pxz : numpy.aray(dim_x, dim_z)
Cross variance of x and z computed during update() call.
**References**
.. [1] Julier, Simon J.; Uhlmann, Jeffrey "A New Extension of the Kalman
Filter to Nonlinear Systems". Proc. SPIE 3068, Signal Processing,
Sensor Fusion, and Target Recognition VI, 182 (July 28, 1997)
.. [2] Labbe, Roger R. "Kalman and Bayesian Filters in Python"
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
"""
def __init__(self, dim_x, dim_z, dt, hx, fx, kappa=0.):
""" Create a Kalman filter. You are responsible for setting the
various state variables to reasonable values; the defaults below will
not give you a functional filter.
**Parameters**
dim_x : int
Number of state variables for the filter. For example, if
you are tracking the position and velocity of an object in two
dimensions, dim_x would be 4.
dim_z : int
Number of of measurement inputs. For example, if the sensor
provides you with position in (x,y), dim_z would be 2.
dt : float
Time between steps in seconds.
hx : function(x)
Measurement function. Converts state vector x into a measurement
vector of shape (dim_z).
fx : function(x,dt)
function that returns the state x transformed by the
state transistion function. dt is the time step in seconds.
kappa : float, default=0.
Scaling factor that can reduce high order errors. kappa=0 gives
the standard unscented filter. According to [1], if you set
kappa to 3-dim_x for a Gaussian x you will minimize the fourth
order errors in x and P.
**References**
[1] S. Julier, J. Uhlmann, and H. Durrant-Whyte. "A new method for
the nonlinear transformation of means and covariances in filters
and estimators," IEEE Transactions on Automatic Control, 45(3),
pp. 477-482 (March 2000).
"""
self.Q = eye(dim_x)
self.R = eye(dim_z)
self.x = zeros(dim_x)
self.P = eye(dim_x)
self._dim_x = dim_x
self._dim_z = dim_z
self._dt = dt
self._num_sigmas = 2*dim_x + 1
self.kappa = kappa
self.hx = hx
self.fx = fx
# weights for the sigma points
self.W = self.weights(dim_x, kappa)
# sigma points transformed through f(x) and h(x)
# variables for efficiency so we don't recreate every update
self.sigmas_f = zeros((self._num_sigmas, self._dim_x))
def update(self, z, R=None, residual=np.subtract, UT=None):
""" Update the UKF with the given measurements. On return,
self.x and self.P contain the new mean and covariance of the filter.
**Parameters**
z : numpy.array of shape (dim_z)
measurement vector
R : numpy.array((dim_z, dim_z)), optional
Measurement noise. If provided, overrides self.R for
this function call.
residual : function (z, z2), optional
Optional function that computes the residual (difference) between
the two measurement vectors. If you do not provide this, then the
built in minus operator will be used. You will normally want to use
the built in unless your residual computation is nonlinear (for
example, if they are angles)
UT : function(sigmas, Wm, Wc, noise_cov), optional
Optional function to compute the unscented transform for the sigma
points passed through hx. Typically the default function will
work, but if for example you are using angles the default method
of computing means and residuals will not work, and you will have
to define how to compute it.
"""
if isscalar(z):
dim_z = 1
else:
dim_z = len(z)
if R is None:
R = self.R
elif np.isscalar(R):
R = eye(self._dim_z) * R
# rename for readability
sigmas_f = self.sigmas_f
sigmas_h = zeros((self._num_sigmas, dim_z))
if UT is None:
UT = unscented_transform
# transform sigma points into measurement space
for i in range(self._num_sigmas):
sigmas_h[i] = self.hx(sigmas_f[i])
# mean and covariance of prediction passed through inscented transform
zp, Pz = UT(sigmas_h, self.W, self.W, R)
# compute cross variance of the state and the measurements
'''self.Pxz = zeros((self._dim_x, dim_z))
for i in range(self._num_sigmas):
self.Pxz += self.W[i] * np.outer(sigmas_f[i] - self.x,
residual(sigmas_h[i], zp))'''
# this is the unreadable but fast implementation of the
# commented out loop above
yh = sigmas_f - self.x[np.newaxis, :]
yz = residual(sigmas_h, zp[np.newaxis, :])
self.Pxz = yh.T.dot(np.diag(self.W)).dot(yz)
K = dot(self.Pxz, inv(Pz)) # Kalman gain
y = residual(z, zp)
self.x = self.x + dot(K, y)
self.P = self.P - dot3(K, Pz, K.T)
def predict(self, dt=None):
""" Performs the predict step of the UKF. On return, self.xp and
self.Pp contain the predicted state (xp) and covariance (Pp). 'p'
stands for prediction.
**Parameters**
dt : double, optional
If specified, the time step to be used for this prediction.
self._dt is used if this is not provided.
Important: this MUST be called before update() is called for the
first time.
"""
if dt is None:
dt = self._dt
# calculate sigma points for given mean and covariance
sigmas = self.sigma_points(self.x, self.P, self.kappa)
for i in range(self._num_sigmas):
self.sigmas_f[i] = self.fx(sigmas[i], dt)
self.x, self.P = unscented_transform(
self.sigmas_f, self.W, self.W, self.Q)
def batch_filter(self, zs, Rs=None, residual=np.subtract, UT=None):
""" Performs the UKF filter over the list of measurement in `zs`.
**Parameters**
zs : list-like
list of measurements at each time step `self._dt` Missing
measurements must be represented by 'None'.
Rs : list-like, optional
optional list of values to use for the measurement error
covariance; a value of None in any position will cause the filter
to use `self.R` for that time step.
residual : function (z, z2), optional
Optional function that computes the residual (difference) between
the two measurement vectors. If you do not provide this, then the
built in minus operator will be used. You will normally want to use
the built in unless your residual computation is nonlinear (for
example, if they are angles)
UT : function(sigmas, Wm, Wc, noise_cov), optional
Optional function to compute the unscented transform for the sigma
points passed through hx. Typically the default function will
work, but if for example you are using angles the default method
of computing means and residuals will not work, and you will have
to define how to compute it.
**Returns**
means: np.array((n,dim_x,1))
array of the state for each time step after the update. Each entry
is an np.array. In other words `means[k,:]` is the state at step
`k`.
covariance: np.array((n,dim_x,dim_x))
array of the covariances for each time step after the update.
In other words `covariance[k,:,:]` is the covariance at step `k`.
"""
try:
z = zs[0]
except:
assert not isscalar(zs), 'zs must be list-like'
if self._dim_z == 1:
assert isscalar(z) or (z.ndim==1 and len(z) == 1), \
'zs must be a list of scalars or 1D, 1 element arrays'
else:
assert len(z) == self._dim_z, 'each element in zs must be a'
'1D array of length {}'.format(self._dim_z)
n = np.size(zs,0)
if Rs is None:
Rs = [None]*n
# mean estimates from Kalman Filter
if self.x.ndim == 1:
means = zeros((n, self._dim_x))
else:
means = zeros((n, self._dim_x, 1))
# state covariances from Kalman Filter
covariances = zeros((n, self._dim_x, self._dim_x))
for i, (z, r) in enumerate(zip(zs, Rs)):
self.predict()
self.update(z, r)
means[i,:] = self.x
covariances[i,:,:] = self.P
return (means, covariances)
def rts_smoother(self, Xs, Ps, Qs=None, dt=None):
""" Runs the Rauch-Tung-Striebal Kalman smoother on a set of
means and covariances computed by the UKF. The usual input
would come from the output of `batch_filter()`.
**Parameters**
Xs : numpy.array
array of the means (state variable x) of the output of a Kalman
filter.
Ps : numpy.array
array of the covariances of the output of a kalman filter.
Q : list-like collection of numpy.array, optional
Process noise of the Kalman filter at each time step. Optional,
if not provided the filter's self.Q will be used
dt : optional, float or array-like of float
If provided, specifies the time step of each step of the filter.
If float, then the same time step is used for all steps. If
an array, then each element k contains the time at step k.
Units are seconds.
**Returns**
'x' : numpy.ndarray
smoothed means
'P' : numpy.ndarray
smoothed state covariances
'K' : numpy.ndarray
smoother gain at each step
**Example**::
zs = [t + random.randn()*4 for t in range (40)]
(mu, cov, _, _) = kalman.batch_filter(zs)
(x, P, K) = rts_smoother(mu, cov, fk.F, fk.Q)
"""
assert len(Xs) == len(Ps)
n, dim_x = Xs.shape
if dt is None:
dt = [self._dt] * n
elif isscalar(dt):
dt = [dt] * n
if Qs is None:
Qs = [self.Q] * n
# smoother gain
Ks = zeros((n,dim_x,dim_x))
num_sigmas = 2*dim_x + 1
xs, ps = Xs.copy(), Ps.copy()
sigmas_f = zeros((num_sigmas, dim_x))
for k in range(n-2,-1,-1):
# create sigma points from state estimate, pass through state func
sigmas = self.sigma_points(xs[k], ps[k], self.kappa)
for i in range(num_sigmas):
sigmas_f[i] = self.fx(sigmas[i], dt[k])
# compute backwards prior state and covariance
xb = dot(self.W, sigmas_f)
Pb = 0
x = Xs[k]
for i in range(num_sigmas):
y = sigmas_f[i] - x
Pb += self.W[i] * outer(y, y)
Pb += Qs[k]
# compute cross variance
Pxb = 0
for i in range(num_sigmas):
z = sigmas[i] - Xs[k]
y = sigmas_f[i] - xb
Pxb += self.W[i] * outer(z, y)
# compute gain
K = dot(Pxb, inv(Pb))
# update the smoothed estimates
xs[k] += dot (K, xs[k+1] - xb)
ps[k] += dot3(K, ps[k+1] - Pb, K.T)
Ks[k] = K
return (xs, ps, Ks)
@staticmethod
def weights(n, kappa):
""" Computes the weights for an unscented Kalman filter. See
__init__() for meaning of parameters.
"""
assert kappa >= 0.0, \
"kappa cannot be negative, it's value is {}".format(kappa)
assert n > 0, "n must be greater than 0, it's value is {}".format(n)
k = .5 / (n+kappa)
W = np.full(2*n+1, k)
W[0] = kappa / (n+kappa)
return W
@staticmethod
def sigma_points(x, P, kappa):
""" Computes the sigma points for an unscented Kalman filter
given the mean (x) and covariance(P) of the filter.
kappa is an arbitrary constant. Returns sigma points.
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 An array-like object of the means of length n
Can be a scalar if 1D.
examples: 1, [1,2], np.array([1,2])
P : scalar, or np.array
Covariance of the filter. If scalar, is treated as eye(n)*P.
kappa : float
Scaling factor.
**Returns**
sigmas : np.array, of size (n, 2n+1)
2D array of sigma points. Each column contains all of
the sigmas for one dimension in the problem space. They
are ordered as:
.. math::
sigmas[0] = x \n
sigmas[1..n] = x + [\sqrt{(n+\kappa)P}]_k \n
sigmas[n+1..2n] = x - [\sqrt{(n+\kappa)P}]_k
"""
if np.isscalar(x):
x = asarray([x])
n = np.size(x) # dimension of problem
if np.isscalar(P):
P = eye(n)*P
sigmas = zeros((2*n+1, n))
# implements U'*U = (n+kappa)*P. Returns lower triangular matrix.
# Take transpose so we can access with U[i]
U = cholesky((n+kappa)*P).T
#U = sqrtm((n+kappa)*P).T
sigmas[0] = x
sigmas[1:n+1] = x + U
sigmas[n+1:2*n+2] = x - U
return sigmas