Extended Kalman filter example in R

January 13, 2015
By

(This article was first published on mages' blog, and kindly contributed to R-bloggers)

Last week’s post about the Kalman filter focused on the derivation of the algorithm. Today I will continue with the extended Kalman filter (EKF) that can deal also with nonlinearities. According to Wikipedia the EKF has been considered the de facto standard in the theory of nonlinear state estimation, navigation systems and GPS.

Kalman filter

I had the following dynamic linear model for the Kalman filter last week:
[begin{align}
x_{t+1} & = A x_t + w_t,quad w_t sim N(0,Q)\
y_t &=G x_t + nu_t, quad nu_t sim N(0,R)\
x_1 & sim N(x_0, Sigma_0)
end{align}
]With (x_t) describing the state space evolution, (y_t) the observations, (A, Q, G, R, Sigma_0) matrices of appropriate dimensions, (w_t) the evolution error and (nu_t) the observation error. The Kalman filter provides recursive estimators for (x_t) via:
[begin{align}
K_{t} &= A Sigma_t G’ (G Sigma_t G’ + R)^{-1}\
hat{x}_{t+1} &= A hat{x_t} + K_{t} (y_t – G hat{x}) \
Sigma_{t+1} &= A Sigma_t A’ – K_{t} G Sigma_t A’ + Q
end{align}]In the case of nonlinearities on the right hand side of either the state ((x_t)) or observation ((y_t)) equation the extended Kalman filter uses a simple and elegant trick: Taylor series of the first order, or in other words, I simply linearise the right hand side. The matrices (A) and (G) will be the Jacobian matrices of the respected vector functions.

Logistic growth

As an example I will use a logistic growth model, inspired by the Hakell example given by Dominic Steinitz.

The logistic growth model can be written as a time-invariant dynamical system with growth rate (r) and carrying capacity (k):
[
begin{aligned}
dot{p} & = r pBig(1 – frac{p}{k}Big)
end{aligned}
]The above ordinary differential equation has the well known analytical solution:
[
p = frac{kp_0exp(r,t)}{k + p_0(exp(r,t) – 1)}
]Suppose I observe data of a population for which I know the carrying capacity (k), but where the growth rate (r) is noisy.

Extended Kalman filter

The state space and observation model can then be written as:
[
begin{aligned}
r_i &= r_{i-1} \
p_i &= frac{kp_{i-1}exp(r_{i-1}Delta T)}{k + p_{i-1}(exp(r_{i-1}Delta T) – 1)} \
y_i &= begin{bmatrix}0 & 1end{bmatrix} begin{bmatrix}r_i \
p_iend{bmatrix} + nu
end{aligned}
]Or with (x_i:=begin{bmatrix}r_i & p_iend{bmatrix}’) as:
[
begin{aligned}
x_i &= a(x_i)\
y_i &= G x_i + nu_i, quad nu_i sim N(0,R)
end{aligned}
]In my example the state space model is purely deterministic, so there isn’t any evolution noise and hence (Q=0).

With an initial prior guess for (x_0) and (Sigma_0) and I am ready to go. The R code below shows my implementation with the algorithm above. Note that I use the jacobian function of the numDeriv package.

After a few time steps the extended Kalman filter does a fantastic job in reducing the noise. Perhaps this shouldn’t be too surprising as a local linearisation of the logistic growth function will give a good fit. The situation might be different for highly nonlinear functions. The Wikipedia article about the Kalman filter suggests the unscented version in those cases.

R code


Session Info

R version 3.1.2 (2014-10-31)
Platform: x86_64-apple-darwin13.4.0 (64-bit)

locale:
[1] en_GB.UTF-8/en_GB.UTF-8/en_GB.UTF-8/C/en_GB.UTF-8/en_GB.UTF-8

attached base packages:
[1] stats graphics grDevices utils datasets methods base

other attached packages:
[1] numDeriv_2012.9-1

loaded via a namespace (and not attached):
[1] tools_3.1.2

To leave a comment for the author, please follow the link and comment on their blog: mages' blog.

R-bloggers.com offers daily e-mail updates about R news and tutorials on topics such as: Data science, Big Data, R jobs, visualization (ggplot2, Boxplots, maps, animation), programming (RStudio, Sweave, LaTeX, SQL, Eclipse, git, hadoop, Web Scraping) statistics (regression, PCA, time series, trading) and more...



If you got this far, why not subscribe for updates from the site? Choose your flavor: e-mail, twitter, RSS, or facebook...

Comments are closed.

Sponsors

Never miss an update!
Subscribe to R-bloggers to receive
e-mails with the latest R posts.
(You will not see this message again.)

Click here to close (This popup will not appear again)