The Kalman.jl package aims to provide a Julian interface for various kinds of Kalman filter.
In [1]:
using Kalman, Winston
The Kalman filter is a way to estimate the state of a system given some knowledge of the (possibly stochastic) dynamics of that system and some noisy data collected from the system. Basically you take your state estimate, run it through the system model to advance it in time (along with the covariance matrix of your estimate), then you update your state estimate and state covariance matrix with the information gained from the data you have collected.
We'll try out the Kalman filter using an example from Welch and Bishop, 2006.
Imagine we have a voltmeter taking measurements of a noisy but, on average, constant circuit. We first need a linear state-space model of this system that looks like $ x_{k+1} = Ax_k+w_k$. Because the voltage of the circuit doesn't change from time to time, we set $A=1$ and find our model $ x_{k+1} = x_k + w_k$ where $w_k$ is a vector of white Gaussian noise with variance $Q$.
The second half of the Kalman filter model is an observation model which relates the measurements you take from the system to the system state. It takes the form $ z_{k} = Hx_k + v_k$ where $v_k$ is white Gaussian noise with variance $R$. For our example, we just take direct measurements of the voltage, so $H=1$.
Now we look at how we can represent those models in Julia.
type LinearModel <: Model
a::Matrix
g::Matrix
q::Matrix
endA LinearModel takes a matrix a representing the state-transition matrix, g which explains how the process noise affects the system state, and q which is the process noise covariance matrix.
We can set one of those up for our model now. Our two transition matrices are just 1 as above. We'll give our process some small (but non-zero!) noise. The circuit isn't going to be perfectly constant.
In [2]:
f = LinearModel([1]',[1]',[1e-10]')
Out[2]:
The observation model is represented with a LinearObservationModel type
type LinearObservationModel <: ObservationModel
h::Matrix
r::Matrix
endwith definitions as above. For our example h is just [1]', and we'll give our voltmeter $0.1\ \mathrm{V}$ RMS noise.
In [3]:
z = LinearObservationModel([1]',[0.01]')
Out[3]:
Finally, we need a way to represent the state of our system. To completely describe the state, we need both a state vector and a state covariance matrix
type State
x::Vector
p::Matrix
endWe have to seed our Kalman filter with an initial state and covariance estimate. It turns out that the covariance will converge regardless of the starting value. For our example as well, the state will converge, but there are systems with dynamics which might lead the state into a local minimum, so it's important to make a reasonable estimate for the state.
We'll start with $\hat{x}_0 = 0$ and $P_0 = 0.5$
In [4]:
x0 = State([0.0],[1.0]')
Out[4]:
Now we have everything to make our Kalman Filter. We just stick the two models and the state into a BasicKalmanFilter type
type BasicKalmanFilter <: LinearKalmanFilter
x::State
f::LinearModel
z::LinearObservationModel
adv::Bool
endThe adv field is a bad way of ensuring that we first advance our filter in time and then update it with some measurements. That may go away.
In [5]:
kf = BasicKalmanFilter(x0,f,z,false)
Out[5]:
Now we need some data. Data to be fed to the Kalman filter is wrapped in an Observation type (which should probably just be a typealias for a Vector). Let's assume that our true voltage $x = -0.37727\ \mathrm{V}$.
In [6]:
x = -0.37727
y = map(y->Observation([y]),x+0.1*randn(50))
Out[6]:
Now we can run our filter by sequentially calling predict(kf::BasicKalmanFilter) and update(kf::BasicKalmanFilter,y::Observation) or by simply calling predictupdate(kf,y). We'll also store the state and variance information and plot it up with Winston.
In [7]:
xs = x0.x[1]*ones(50)
ps = x0.p[1]*ones(50)
for i = 1:49
predictupdate!(kf,y[i])
xs[i+1] = kf.x.x[1]
ps[i+1] = kf.x.p[1]
end
In [8]:
plot([0,50],[x,x],"b")
oplot(map(y->y.y[1],y),"r.")
oplot(xs,"g")
xlim(0,50)
Out[8]:
In [9]:
plot(ps[2:50],"g")
Out[9]:
Unscented Kalman filters let you apply the prediction-update method of the Kalman filter to nonlinear models. They do this by deterministically sampling a set of "sigma points" which are then run through the filter equations to reconstruct the state and covariance matrices after a nonlinear transformation.
Unscented Kalman filters aren't pushed to Github yet, and there are some restrictions on the kind of model you can use for it (additive Gaussian noise for now; you might be able to get away with non-Gaussian noise, but multiplicative noise definitely won't work).
Still, let's see if we can't make them work. We'll take the example from Kandepu et al. (2008), a van der Pol Oscillator.
The equations of motion are
\begin{align} \dot{x}_1 &= -x_2 \\ \dot{x}_2 &= -\mu (1-x_1^2)x^2 + x_1 \end{align}where $\mu=0.2$.
In [10]:
include(Pkg.dir("Kalman","sandbox","unscented.jl"))
Out[10]:
In [11]:
const μ = 0.2
x0 = State([0.,5],5.0*eye(2))
Q = 1e-3*eye(2)
R = [0.1 0;
0 1e-3]
function f2(x::Vector,dt::Float64)
x1 = zeros(x)
x1[1] = x[1] + dt*-x[2]
x1[2] = x[2] + dt*(-μ*(1-x[1]^2)*x[2]+x[1])
x1
end
h(x) = x
dt = 0.01
xs = fill(zeros(x0.x),4000)
xs[1] = [1.4,0]
x = [1.4,0]
for i = 2:4000
x = f2(x,dt)
xs[i] = x
end
ys = xs .+ map(y->sqrt(R)*randn(2),1:4000)
Out[11]:
In [12]:
fm = AdditiveUnscentedModel(x->f2(x,dt),Q)
zm = AdditiveUnscentedObservationModel(h,R)
p0 = 5.0*eye(2)
kf = AdditiveUnscentedKalmanFilter(State([0,5.0],p0),fm,zm,0.1,2.0,0.0)
xs1 = 0.0*ones(4000)
xs2 = 5.0*ones(4000)
ps = 5.0*ones(4000)
for i = 2:4000
predictupdate!(kf,Observation([ys[i]]))
xs1[i] = kf.x.x[1]
xs2[i] = kf.x.x[2]
ps[i] = kf.x.p[1]
end
In [13]:
plot(xs1,"k")
Out[13]: