Implementing a particle filter in C++ using Rcpp (part 2)

In my previous blog post I showed how to implement a particle filter in C++.

In this post I will implement a backward-simulation particle smoother (see Example 3 in the code below) and a method for forecasting with the particle filter (Example 4) in C++. By using R packages Rcpp (version 0.12.11) and RcppArmadillo (version 0.7.800.2.0) it is possible to subsequently run this smoother and forecasting method in R.

However, for running this smoother and forecasting method in R you need a C++ compiler. For installing a working C++ compiler you may want to check out this Rcpp manual by Hadley Wickham.

A derivation and description of the implemented backward-simulation particle smoother can be found in Godsill, Doucet, and West’s 2004 paper Monte Carlo smoothing for nonlinear time series.

Suggestions and/or questions? Please contact Stefan Gelissen (email: info at datall-analyse.nl).
See this page for an overview of all of Stefan’s R code blog posts.

Implementations in C++
Download the C++ code (called PF.cpp) for the smoother and forecasting method from here and save it to some folder on your computer.

For Examples 3 and 4 below to work, you also need to download this C++ header file (called PF_example3.h), and save it into the same folder as the PF.cpp file.

Finally, the particle filter code in PF.cpp needs to know that it has to use the header file PF_example3.h . To this end, you have to change the the statement #include <PF_example1.h> (which can be found at the top of the C++ code in the PF.cpp file) into #include <PF_example3.h> .

R code

#In the Examples 3 and 4 below, we are going to model one dimensional object motion,
#where the states of the object (that is, position and velocity) are influenced by
#process noise.
#More specifically, in these examples we assume that an object (a vehicle) is
#constrained to move in one dimension, and that the vehicle's motion is controlled
#(influenced) by unpredictable (random) factors such as wind gusts and potholes.
#These factors control the object's motion at each time step in an
#unpredictable way (they add random noise to the object's motion). 
#Note that these factors influence the object's motion at each time
#step by accelerating it.

#The vehicle's behavior is governed by the following two differential equations:
#dx/dt = v, where x is the displacement and v the velocity at time t
#dv/dt = u, where u is the vehicle's uniform acceleration.

#Integrating these two equations from time=0 to time=t yields:
#p0 + p0*t + .5*u*t^2 + pnoise (=vehicle's position)
#v0 + u*t + vnoise (=vehicle's velocity),
#where p0 is the position at time=0, pnoise the position noise, 
#v0 the velocity at time=0, vnoise the velocity noise,
#and u the mean acceleration during the object's motion.

#In the two examples below, we assume that we only measure the object's position 
#at each time step.
#Note that Dan Simon, in his 2001 paper "Kalman filtering", tackles the same problem
#as presented in these two examples.



#EXAMPLE 3: backward-simulation particle smoothing


##TRUE STATES AND MEASUREMENTS

#Initial position (at t=0)
p0 <- 0

#Initial velocity (at t=0)
v0 <- 0

#Mean acceleration
u <- 1

#Standard deviation acceleration
usd <- .2

#Standard deviation of position measurement
msd <- 10

#Time step
dt <- .1

#Observation times
t <- seq(0.1, 20, dt)

#State transition matrix
A <- matrix(c(1,dt,
              0,1), nrow=2, byrow=TRUE)

#Control input matrix
B <- c(.5*dt^2, dt)

#Observation matrix
H <- c(1, 0)

#Variance-covariance matrix for the process noise
#(this process noise is due to the random character of the acceleration)
vcp <- B%*%t(B)*usd^2 #using the delta method


#Simulate data
x <- matrix(NA, nrow=length(t)+1, ncol=2)
y <- matrix(NA, nrow=length(t)+1, ncol=1)
x[1,] <- c(p0,v0)

library(mvtnorm)
for (i in 2:(length(t)+1)) {
  x[i,] <- A%*%x[(i-1),] + B*u + t(rmvnorm(n=1, sigma=vcp))
  y[i,] <- H%*%x[i,] + msd*rnorm(1)}

#Plot simulated states:
#position
plot(c(0,t), y[,1], type="l", col=gray(level=.7), xlab="time", ylab="position")
lines(c(0,t), x[,1], col="red", lty=2)
legend("topleft", lty=c(1, 2),
       col=c(gray(level=.7), "red"), legend=c("measured", "true state"),
       bty="n", y.intersp=1.2)

#velocity
plot(c(0,t), x[,2], type="l", xlab="time", ylab="velocity")



##PARTICLE/BOOTSTRAP FILTER (PF)

#Dynamic model:
#specifying 2 states, namely [x, vx], where x=position and vx=velocity.
#Note that you may change the initial state estimate (at t=0) below for x
#and see how it influences the behavior of the particle filter.
ex3 <- list(m0=as.matrix(c(10, 0)), #initial state estimates
            #error covariances of the initial state estimates:
            #this matrix reflects the uncertainty in our initial state estimates
            #you may change the values in this matrix and see how they influence
            #the behavior of the Kalman filter
            C0=diag(c(30, 0)),
            #measurement noise
            V=as.matrix(msd^2),
            #process noise
            W=vcp)
#CAUTION: all the list components above have to be in the matrix form (if not,
#the PF filter in C++ won't work and subsequently returns an error message).


##Compute the filtered (a posteriori) state estimates with the PF

#Compile the C++ code using Rcpp:
#before compiling the C++, don't forget to use the correct C++ code,
#that is, use "#include <PF_example3.h>" in the "PF.cpp" file.
library(Rcpp)
library(RcppArmadillo)
sourceCpp("yourFolder/PF.cpp")

#The GGfunction (=state transition function) and FFfunction (=observation/measurement
#function) for this dynamic model are coded in C++, and can be found in the
#header file called "PF_example3.h".
#The compiled Particle Filter will use the defined GGfunction and FFfunction
#due to the statement "#include <PF_example3.h>". 


#Remove NA at the beginning of measurements
ynew <- y[-1]

#Fit the particle filter to the data
pf3 <- PFcpp(y=as.matrix(ynew), mod=ex3, N=1000, inputControl=t(as.matrix(B)),
             resampling="strat", Nthreshold=500,
             roughening=TRUE, Grough=.1, MCparticles=TRUE)

#Plot filtered state estimates:
#position
plot(c(0,t), y, col=gray(level=.7), type="l", xlab="time", ylab="position")
lines(c(0,t), x[,1], col="red", lty=2)
lines(c(0,t), pf3$m[,1], col="blue", lty=2)
legend("topleft", lty=c(1, 2, 2),
       col=c(gray(level=.7), "red", "blue"),
       legend=c("measured", "true state", "filtered state"),
       bty="n", y.intersp=1.2, cex=.7)

#velocity
plot(c(0,t), x[,2], col="red", type="l", xlab="time", ylab="velocity")
lines(c(0,t), pf3$m[,2], col="blue", lty=2)
legend("topleft", lty=c(1, 2),
       col=c(gray(level=.7), "red", "blue"),
       legend=c("true state", "filtered state"),
       bty="n", y.intersp=1.2, cex=.7)


#95% confidence intervals for the filtered state estimates

#compute the confidence intervals for the filtered state estimates
Clist <- sapply(1:dim(pf3$C)[3], function(x) pf3$C[ , , x], simplify=FALSE)
#95% ci for position
seX <- sqrt(unlist(lapply(Clist, function(x) diag(x)[1])))
ciX <- pf3$m[,1] + qnorm(.05/2)*seX%o%c(1, -1)
#95% ci for velocity
seVX <- sqrt(unlist(lapply(Clist, function(x) diag(x)[2])))
ciVX <- pf3$m[,2] + qnorm(.05/2)*seVX%o%c(1, -1)

#plot for position
plot(c(0,t), x[,1], type="l", xlab="time", ylab="position",
     ylim=c(-5, 200), col=gray(level=.7), lwd=1.5)
lines(c(0,t), ciX[,1], lty=2, col="blue")
lines(c(0,t), ciX[,2], lty=2, col="blue")

#plot for velocity
plot(c(0,t), x[,2], type="l", xlab="time", ylab="velocity",
     ylim=c(-5, 25), col=gray(level=.7), lwd=1.5)
lines(c(0,t), ciVX[,1], lty=2, col="blue")
lines(c(0,t), ciVX[,2], lty=2, col="blue")



##Perform backward-simulation particle smoothing.
#Note that the implemented function for the backward-simulation particle smoother
#has the following arguments:
#-filterdata is an object as returned by PFcpp (with MCparticles=TRUE).
#-Ntrajectories is the number of trajectories to be simulated by the smoother.

#Simulate 100 trajectories:
pfs <- PFsmoothcpp(filterData=pf3, Ntrajectories=100)

#Position
pfsList <- sapply(1:dim(pfs$s)[3], function(x) pfs$s[ , , x], simplify=FALSE)
pfsListX <- lapply(pfsList, function(x) x[,1])
pfsX <- do.call(cbind, pfsListX)
#Plot the smoother results for the 100 simulated trajectories
plot(c(0,t), x[,1], type="l", xlab="time", ylab="position",
     ylim=c(-5, 200), col="red", lwd=1.5)
matlines(c(0, t), pfsX, lty=2, col=gray(level=.1, alpha=.2), lwd=1)
lines(c(0, t), pf3$m[,1], lty=2, col="blue", lwd=2)
legend("topleft", col=c("red", "blue", gray(level=.5)),
       lty=c(1, 2, 2), pch=c(1, NA, NA), lwd=c(2, 2, 1),
       legend=c("true state", "filtered PF", "smoothed PF"),
       bty="n", y.intersp=1.2, cex=.7)


#Velocity
pfsListVX <- lapply(pfsList, function(x) x[,2])
pfsVX <- do.call(cbind, pfsListVX)
#Plot the smoother results for the 100 simulated trajectories
plot(c(0,t), x[,2], type="l", xlab="time", ylab="velocity",
     ylim=c(-5, 25), col="red", lwd=1.5)
matlines(c(0, t), pfsVX, lty=2, col=gray(level=.1, alpha=.1), lwd=1)
lines(c(0, t), pf3$m[,2], lty=2, col="blue", lwd=2)
legend("topleft", col=c("red", "blue", gray(level=.5)),
       lty=c(1, 2, 2), pch=c(1, NA, NA), lwd=c(2, 2, 1),
       legend=c("true state", "filtered PF", "smoothed PF"),
       bty="n", y.intersp=1.2, cex=.7)



##EXAMPLE 4: FORECASTING WITH THE PARTICLE/BOOTSTRAP FILTER (PF)

#For Example 4, we will use exactly the same model and data for the vehicle's motion
#as in Example 3.

##Compute the filtered (a posteriori) state estimates,
#leaving out the last 10 observations
pf4 <- PFcpp(y=as.matrix(ynew[-c(191:200)]), mod=ex3, N=1000,
             inputControl=t(as.matrix(B)),
             resampling="strat", Nthreshold=500,
             roughening=TRUE, Grough=.1, MCparticles=TRUE)

#Forecasting future system states and observations n steps ahead:
#Note that the implemented forecasting function has the following arguments:
#-filterdata is an object as returned by PFcpp (with MCparticles=TRUE).
#-inputControl is an optional matrix that specifies the control input for each of
# the states during the n steps ahead. For control input that is known to be constant
# over time, the matrix is 1 x p (where p is the number of states). For time varying
# control input, the matrix is n x p (where n is the number of
# observations/time steps, and p the number of states).
#-nAhead is the number of steps ahead for which to predict system states
# and observations.

#Compute forecasts for 10 time steps ahead
#(If this forecasting function generates an error, then decrease nAhead
#from 10 to, for instance, 5)
fc <- PFforecastcpp(pf4, inputControl=t(as.matrix(B)), nAhead=10)


#90% confidence intervals for forecasts

#Future system states

#Postition
Rlist <- sapply(1:dim(fc$R)[3], function(x) fc$R[ , , x], simplify=FALSE)
seFSX <- sqrt(unlist(lapply(Rlist, function(x) x[1,1])))
ciFSX <- fc$a[,1] + qnorm(.1/2)*seFSX%o%c(1, -1) #90% confidence interval

#Plot future system states including 90% confidence intervals
plot(c(0,t), x[,1], type="l", lty=2, col="red",
     xlab="Time", ylab="Position")
lines(c(0,t)[1:191], pf4$m[,1], col=gray(level=.5), lwd=1)
lines(c(0,t)[192:201], fc$a[,1], col="blue", lwd=2)
lines(c(0,t)[192:201], ciFSX[,1], col="blue", lty=2, lwd=1)
lines(c(0,t)[192:201], ciFSX[,2], col="blue", lty=2, lwd=1)
legend("topleft", lty=c(2, 1), 
       col=c("red", "blue"),
       legend=c("true state", "future state"),
       bty="n", y.intersp=1.2, cex=.7)


#Future observations
Qlist <- sapply(1:dim(fc$Q)[3], function(x) fc$Q[ , , x], simplify=FALSE)
seFOX <- sqrt(unlist(lapply(Qlist, function(x) x[1])))
ciFOX <- fc$f[,1] + qnorm(.1/2)*seFOX%o%c(1, -1) #90% prediction interval

#Plot future observations including 90% prediction intervals
plot(c(0,t), x[,1], type="l", lty=2, col="red",
     xlab="Time", ylab="Position", ylim=c(-20,220))
lines(t, ynew, col=gray(level=.7), type="l", xlab="time", ylab="position")
lines(t[1:190], pf4$f[,1], col=gray(level=.1), lwd=1)
lines(t[191:200], fc$f[,1], col="blue", lwd=2)
lines(t[191:200], ciFOX[,1], col="blue", lty=2, lwd=1)
lines(t[191:200], ciFOX[,2], col="blue", lty=2, lwd=1)
legend("topleft", lty=c(2, 1, 1), 
       col=c("red", col=gray(level=.7), "blue"),
       legend=c("true state", "measurements", "future observations"),
       bty="n", y.intersp=1.2, cex=.7)