Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
25e0ce228This file is pinned to the selected release. Download the original to open it in your design tool.
# -*- coding: utf-8 -*-
"""Copyright 2015 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.
Modified by : Francisco J. Garcia
Add fx_arg into RTS smoother
"""
from __future__ import (absolute_import, division, print_function,
unicode_literals)
from filterpy.common import dot3
from filterpy.kalman import unscented_transform
from filterpy.stats import logpdf
import math
import numpy as np
from numpy import eye, zeros, dot, isscalar, outer
from scipy.linalg import inv, cholesky
class UnscentedKalmanFilter(object):
# pylint: disable=too-many-instance-attributes
# pylint: disable=C0103
r""" Implements the Scaled Unscented Kalman filter (UKF) as defined by
Simon Julier in [1], using the formulation provided by Wan and Merle
in [2]. This filter scales the sigma points to avoid strong nonlinearities.
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
-------------------
K : numpy.array
Kalman gain
y : numpy.array
innovation residual
x : numpy.array(dim_x)
predicted/updated state (result of predict()/update())
P : numpy.array(dim_x, dim_x)
predicted/updated covariance matrix (result of predict()/update())
likelihood : scalar
Likelihood of last measurement update.
log_likelihood : scalar
Log likelihood of last measurement update.
References
----------
.. [1] Julier, Simon J. "The scaled unscented transformation,"
American Control Converence, 2002, pp 4555-4559, vol 6.
Online copy:
https://www.cs.unc.edu/~welch/kalman/media/pdf/ACC02-IEEE1357.PDF
.. [2] E. A. Wan and R. Van der Merwe, “The unscented Kalman filter for
nonlinear estimation,” in Proc. Symp. Adaptive Syst. Signal
Process., Commun. Contr., Lake Louise, AB, Canada, Oct. 2000.
Online Copy:
https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf
"""
def __init__(self, dim_x, dim_z, dt, hx, fx, points,
sqrt_fn=None, x_mean_fn=None, z_mean_fn=None,
residual_x=None,
residual_z=None):
r""" 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.
points : class
Class which computes the sigma points and weights for a UKF
algorithm. You can vary the UKF implementation by changing this
class. For example, MerweScaledSigmaPoints implements the alpha,
beta, kappa parameterization of Van der Merwe, and
JulierSigmaPoints implements Julier's original kappa
parameterization. See either of those for the required
signature of this class if you want to implement your own.
sqrt_fn : callable(ndarray), default = scipy.linalg.cholesky
Defines how we compute the square root of a matrix, which has
no unique answer. Cholesky is the default choice due to its
speed. Typically your alternative choice will be
scipy.linalg.sqrtm. Different choices affect how the sigma points
are arranged relative to the eigenvectors of the covariance matrix.
Usually this will not matter to you; if so the default cholesky()
yields maximal performance. As of van der Merwe's dissertation of
2004 [6] this was not a well reseached area so I have no advice
to give you.
If your method returns a triangular matrix it must be upper
triangular. Do not use numpy.linalg.cholesky - for historical
reasons it returns a lower triangular matrix. The SciPy version
does the right thing.
x_mean_fn : callable (sigma_points, weights), optional
Function that computes the mean of the provided sigma points
and weights. Use this if your state variable contains nonlinear
values such as angles which cannot be summed.
.. code-block:: Python
def state_mean(sigmas, Wm):
x = np.zeros(3)
sum_sin, sum_cos = 0., 0.
for i in range(len(sigmas)):
s = sigmas[i]
x[0] += s[0] * Wm[i]
x[1] += s[1] * Wm[i]
sum_sin += sin(s[2])*Wm[i]
sum_cos += cos(s[2])*Wm[i]
x[2] = atan2(sum_sin, sum_cos)
return x
z_mean_fn : callable (sigma_points, weights), optional
Same as x_mean_fn, except it is called for sigma points which
form the measurements after being passed through hx().
residual_x : callable (x, y), optional
residual_z : callable (x, y), optional
Function that computes the residual (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. One is for the state variable,
the other is for the measurement state.
.. code-block:: Python
def residual(a, b):
y = a[0] - b[0]
if y > np.pi:
y -= 2*np.pi
if y < -np.pi:
y = 2*np.pi
return y
References
----------
.. [3] 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).
.. [4] E. A. Wan and R. Van der Merwe, “The Unscented Kalman filter for
Nonlinear Estimation,” in Proc. Symp. Adaptive Syst. Signal
Process., Commun. Contr., Lake Louise, AB, Canada, Oct. 2000.
https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf
.. [5] Wan, Merle "The Unscented Kalman Filter," chapter in *Kalman
Filtering and Neural Networks*, John Wiley & Sons, Inc., 2001.
.. [6] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
Inference in Dynamic State-Space Models" (Doctoral dissertation)
"""
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.points_fn = points
self._dt = dt
self._num_sigmas = points.num_sigmas()
self.hx = hx
self.fx = fx
self.x_mean = x_mean_fn
self.z_mean = z_mean_fn
self.log_likelihood = 0.0
self.K = np.zeros((6, 6)) # Kalman gain
if sqrt_fn is None:
self.msqrt = cholesky
else:
self.msqrt = sqrt_fn
# weights for the means and covariances.
self.Wm, self.Wc = self.points_fn.weights()
if residual_x is None:
self.residual_x = np.subtract
else:
self.residual_x = residual_x
if residual_z is None:
self.residual_z = np.subtract
else:
self.residual_z = residual_z
# 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))
self.sigmas_h = zeros((self._num_sigmas, self._dim_z))
def predict(self, dt=None, UT=None, fx_args=()):
r""" Performs the predict step of the UKF. On return, self.x and
self.P contain the predicted state (x) and covariance (P). '
Important: this MUST be called before update() is called for the first
time.
Parameters
----------
dt : double, optional
If specified, the time step to be used for this prediction.
self._dt is used if this is not provided.
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 - you can use x_mean_fn and z_mean_fn to alter the behavior
of the unscented transform.
fx_args : tuple, optional, default (,)
optional arguments to be passed into fx() after the required state
variable.
"""
if dt is None:
dt = self._dt
if not isinstance(fx_args, tuple):
fx_args = (fx_args,)
if UT is None:
UT = unscented_transform
# calculate sigma points for given mean and covariance
sigmas = self.points_fn.sigma_points(self.x, self.P)
for i in range(self._num_sigmas):
self.sigmas_f[i] = self.fx(sigmas[i], dt, *fx_args)
self.x, self.P = UT(self.sigmas_f, self.Wm, self.Wc, self.Q,
self.x_mean, self.residual_x)
def update(self, z, R=None, UT=None, hx_args=()):
""" 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.
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 - you can use x_mean_fn and z_mean_fn to alter the behavior
of the unscented transform.
hx_args : tuple, optional, default (,)
arguments to be passed into Hx function after the required state
variable.
"""
if z is None:
return
if not isinstance(hx_args, tuple):
hx_args = (hx_args,)
if UT is None:
UT = unscented_transform
if R is None:
R = self.R
elif isscalar(R):
R = eye(self._dim_z) * R
for i in range(self._num_sigmas):
self.sigmas_h[i] = self.hx(self.sigmas_f[i], *hx_args)
# mean and covariance of prediction passed through unscented transform
zp, Pz = UT(self.sigmas_h, self.Wm, self.Wc, R, self.z_mean, self.residual_z)
# compute cross variance of the state and the measurements
Pxz = zeros((self._dim_x, self._dim_z))
for i in range(self._num_sigmas):
dx = self.residual_x(self.sigmas_f[i], self.x)
dz = self.residual_z(self.sigmas_h[i], zp)
Pxz += self.Wc[i] * outer(dx, dz)
self.K = dot(Pxz, inv(Pz)) # Kalman gain
self.y = self.residual_z(z, zp) # residual
self.x = self.x + dot(self.K, self.y)
self.P = self.P - dot3(self.K, Pz, self.K.T)
self.log_likelihood = logpdf(self.y, np.zeros(len(self.y)), Pz)
def cross_variance(self, x, z, sigmas_f, sigmas_h):
Pxz = zeros((sigmas_f.shape[1], sigmas_h.shape[1]))
N = sigmas_f.shape[0]
for i in range(N):
dx = self.residual_x(sigmas_f[i], x)
dz = self.residual_z(sigmas_h[i], z)
Pxz += self.Wc[i] * outer(dx, dz)
@property
def likelihood(self):
return math.exp(self.log_likelihood)
def batch_filter(self, zs, Rs=None, 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.
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 - you can use x_mean_fn and z_mean_fn to alter the behavior
of the unscented transform.
Returns
-------
means: ndarray((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: ndarray((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)
z_n = np.size(zs, 0)
if Rs is None:
Rs = [None] * z_n
# mean estimates from Kalman Filter
if self.x.ndim == 1:
means = zeros((z_n, self._dim_x))
else:
means = zeros((z_n, self._dim_x, 1))
# state covariances from Kalman Filter
covariances = zeros((z_n, self._dim_x, self._dim_x))
for i, (z, r) in enumerate(zip(zs, Rs)):
self.predict(UT=UT)
self.update(z, r, UT=UT)
means[i, :] = self.x
covariances[i, :, :] = self.P
return (means, covariances)
def rts_smoother(self, Xs, Ps, Qs=None, dt=None, U=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.
Qs: 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
Examples
--------
.. code-block:: Python
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 = self._num_sigmas
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.points_fn.sigma_points(xs[k], ps[k])
for i in range(num_sigmas):
sigmas_f[i] = self.fx(sigmas[i], dt[k], U[k])
# compute backwards prior state and covariance
xb = dot(self.Wm, sigmas_f)
Pb = 0
x = Xs[k]
for i in range(num_sigmas):
y = self.residual_x(sigmas_f[i], x)
Pb += self.Wc[i] * outer(y, y)
Pb += Qs[k]
# compute cross variance
Pxb = 0
for i in range(num_sigmas):
z = self.residual_x(sigmas[i], Xs[k])
y = self.residual_x(sigmas_f[i], xb)
Pxb += self.Wc[i] * outer(z, y)
# compute gain
K = dot(Pxb, inv(Pb))
# update the smoothed estimates
xs[k] += dot(K, self.residual_x(xs[k + 1], xb))
ps[k] += dot3(K, ps[k + 1] - Pb, K.T)
Ks[k] = K
return (xs, ps, Ks)