"""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.
"""
from __future__ import (absolute_import, division, print_function,
unicode_literals)
import numpy as np
from scipy.linalg import inv
from numpy import dot, zeros, eye
from filterpy.common import dot3, dot4, dotn
class FixedLagSmoother(object):
""" Fixed Lag Kalman smoother.
Computes a smoothed sequence from a set of measurements based on the
fixed lag Kalman smoother. At time k, for a lag N, the fixed-lag smoother
computes the state estimate for time k-N based on all measurements made
between times k-N and k. This yields a pretty good smoothed result with
O(N) extra computations performed for each measurement. In other words,
if N=4 this will consume about 5x the number of computations as a
basic Kalman filter. However, the loops contain only 3 dot products, so it
will be much faster than this sounds as the main Kalman filter loop
involves transposes and inverses, as well as many more matrix
multiplications.
Implementation based on Wikipedia article as it existed on
November 18, 2014.
**Example**::
fls = FixedLagSmoother(dim_x=2, dim_z=1)
fls.x = np.array([[0.],
[.5]])
fls.F = np.array([[1.,1.],
[0.,1.]])
fls.H = np.array([[1.,0.]])
fls.P *= 200
fls.R *= 5.
fls.Q *= 0.001
zs = [...some measurements...]
xhatsmooth, xhat = fls.smooth_batch(zs, N=4)
**References**
Wikipedia http://en.wikipedia.org/wiki/Kalman_filter#Fixed-lag_smoother
Simon, Dan. "Optimal State Estimation," John Wiley & Sons pp 274-8 (2006).
|
|
**Methods**
"""
def __init__(self, dim_x, dim_z, N=None):
""" Create a fixed lag Kalman filter smoother. 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 Kalman filter. For example, if
you are tracking the position and velocity of an object in two
dimensions, dim_x would be 4.
This is used to set the default size of P, Q, and u
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.
N : int, optional
If provided, the size of the lag. Not needed if you are only
using smooth_batch() function. Required if calling smooth()
"""
self.dim_x = dim_x
self.dim_z = dim_z
self.N = N
self.x = zeros((dim_x,1))
self.x_s = zeros((dim_x,1))
self.P = eye(dim_x)
self.Q = eye(dim_x)
self.F = 0
self.H = 0
self.R = eye(dim_z)
self.K = 0
self.residual = zeros((dim_z, 1))
self.B = 0
self._I = np.eye(dim_x)
self.count = 0
if N is not None:
self.xSmooth = []
def smooth(self, z, u=None):
""" Smooths the measurement using a fixed lag smoother.
On return, self.xSmooth is populated with the N previous smoothed
estimates, where self.xSmooth[k] is the kth time step. self.x
merely contains the current Kalman filter output of the most recent
measurement, and is not smoothed at all (beyond the normal Kalman
filter processing).
self.xSmooth grows in length on each call. If you run this 1 million
times, it will contain 1 million elements. Sure, we could minimize
this, but then this would make the caller's code much more cumbersome.
This also means that you cannot use this filter to track more than
one data set; as data will be hopelessly intermingled. If you want
to filter something else, create a new FixedLagSmoother object.
**Parameters**
z : ndarray or scalar
measurement to be smoothed
u : ndarray, optional
If provided, control input to the filter
"""
H = self.H
R = self.R
F = self.F
P = self.P
x = self.x
Q = self.Q
B = self.B
N = self.N
k = self.count
x_pre = dot(F, x)
if u is not None:
x_pre += dot(B,u)
P = dot3(F, P, F.T) + Q
y = z - dot(H, x_pre)
S = dot3(H, P, H.T) + R
SI = inv(S)
K = dot3(P, H.T, SI)
x = x_pre + dot(K, y)
I_KH = self._I - dot(K, H)
P = dot3(I_KH, P, I_KH.T) + dot3(K, R, K.T)
self.xSmooth.append(x_pre.copy())
HTSI = dot(H.T, SI)
F_LH = (F - dot(K,H)).T
if k >= N:
PS = P.copy()
for i in range (N):
K = dot(PS, HTSI)
PS = dot(PS, F_LH)
si = k-i
self.xSmooth[si] = self.xSmooth[si] + dot(K, y)
else:
self.xSmooth[k] = x.copy()
self.count += 1
self.x = x
self.P = P
def smooth_batch(self, zs, N, us=None):
""" batch smooths the set of measurements using a fixed lag smoother.
I consider this function a somewhat pedalogical exercise; why would
you not use a RTS smoother if you are able to batch process your data?
Hint: RTS is a much better smoother, and faster besides. Use it.
This is a batch processor, so it does not alter any of the object's
data. In particular, self.x is NOT modified. All date is returned
by the function.
**Parameters**
zs : ndarray of measurements
iterable list (usually ndarray, but whatever works for you) of
measurements that you want to smooth, one per time step.
N : int
size of fixed lag in time steps
us : ndarray, optional
If provided, control input to the filter for each time step
**Returns**
(xhat_smooth, xhat) : ndarray, ndarray
xhat_smooth is the output of the N step fix lag smoother
xhat is the filter output of the standard Kalman filter
"""
H = self.H
R = self.R
F = self.F
P = self.P
x = self.x
Q = self.Q
B = self.B
if x.ndim == 1:
xSmooth = zeros((len(zs), self.dim_x))
xhat = zeros((len(zs), self.dim_x))
else:
xSmooth = zeros((len(zs), self.dim_x, 1))
xhat = zeros((len(zs), self.dim_x, 1))
for k, z in enumerate(zs):
x_pre = dot(F, x)
if us is not None:
x_pre += dot(B,us[k])
P = dot3(F, P, F.T) + Q
y = z - dot(H, x_pre)
S = dot3(H, P, H.T) + R
SI = inv(S)
K = dot3(P, H.T, SI)
x = x_pre + dot(K, y)
I_KH = self._I - dot(K, H)
P = dot3(I_KH, P, I_KH.T) + dot3(K, R, K.T)
xhat[k] = x.copy()
xSmooth[k] = x_pre.copy()
HTSI = dot(H.T, SI)
F_LH = (F - dot(K,H)).T
if k >= N:
PS = P.copy()
for i in range (N):
K = dot(PS, HTSI)
PS = dot(PS, F_LH)
si = k-i
xSmooth[si] = xSmooth[si] + dot(K, y)
else:
xSmooth[k] = xhat[k]
return xSmooth, xhat
