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 smooth**er* (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.

__Implementation____s____ 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)
```