# R code for implementing a particle filter

The R code below implements a particle filter in R. The implemented particle filter is also referred to as the bootstrap filter.
If necessary, the implemented bootstrap filter performs resampling and/or roughening.

Similar to the Extended Kalman Filter (EKF), the Unscented Kalman Filter (UKF), the Ensemble Kalman Filter (EnKF), and the Gauss-Hermite Kalman Filter (GHKF), the bootstrap filter may be used for solving nonlinear filtering problems. A description and derivation of the implemented bootstrap filter can be found in Crassidis and Junkins’s 2012 book Optimal estimation of dynamic systems.

In this blog post I will demonstrate how to implement the same particle/bootstrap filter in C++.

See this page for an overview of all of Stefan’s R code blog posts.

R code

```library(mvtnorm)

#The R function for the particle/bootstrap filter (PF) can be downloaded from:
source("http://www.datall-analyse.nl/R/PF.R")
#Take a look at the function PF (=the particle/bootstrap filter),
#and notice that at the beginning of the script you will find
#a description of the function's arguments.
PF

##EXAMPLE 1

#The following example can be found in Crassidis and Junkins's
#2012 book "Optimal estimation of dynamic systems".

#In this example we will estimate the state of the following
#nonlinear discrete-time system:
#x(k+1) = x(k)/2 + 25*x(k)/(1+x(k)^2) + 8*cos(1.2*k) + W
#where x(k+1) is the state at time step k+1, x(k) the state at time
#step k, and W the process noise (variance). We will set W to 10 in
#this example.

#The measurement equation is:
#y(k) = x(k)^2/20 + V
#where V is the measurement noise (variance). We will set V to 1.

##TRUE STATES AND MEASUREMENTS

#sample time
dT <- 1

#observation times
t <- seq(1, 75, dT)

#process noise (standard deviation)
Wk <- sqrt(10)

#measurement noise (standard deviation)
Vk <- sqrt(1)

#state at t=0
X0 <- 0 + rnorm(1)*sqrt(5)

#simulate true state for x at the observation times
Xk <- matrix(NA, nrow=length(t)+1, ncol=1)
Yk <- matrix(NA, nrow=length(t), ncol=1)
Xk <- X0

for (i in 2:(length(t)+1)) {
Xk[i] <- Xk[i-1]/2 + (25*Xk[i-1])/(1+Xk[i-1]^2) + 8*cos(1.2*(i-1)) + rnorm(1)*Wk
Yk[i-1] <- (Xk[i]^2)/20 + rnorm(1)*Vk}

#plot simulated states
plot(c(0,t), Xk, type="o", xlab="Time, k", ylab="x(k)")

#plot measurements
plot(t, Yk, type="o", xlab="Time, k", ylab="y(k)")

#store measurement data
dataEx1 <- Yk

##PARTICLE/BOOTSTRAP FILTER (PF)

#Dynamic model:
#specifying 1 state, namely [x].
#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.
ex1 <- list(m0=0, #initial state estimate
#error covariance of the initial state estimate:
#this vector reflects the uncertainty in our initial state estimate
#you may change the value in this vector and see how it influences
#the behavior of the particle filter
C0=5,
#measurement noise
V=Vk^2,
#process noise
W=Wk^2)

#Specify the state transition function:
#WARNING: always use arguments x and k when specifying the GGfunction
GGfunction <- function (x, k){
x/2 + (25*x)/(1+x^2) + 8*cos(1.2*(k))}

#Specify the observation/measurement function:
#WARNING: always use arguments x and k when specifying the FFfunction
FFfunction <- function (x, k){
(x^2)/20}

##Compute the filtered (a posteriori) state estimates with the PF
pf1 <- PF(y=dataEx1, mod=ex1, N=500, resampling="strat",
GGfunction=GGfunction, FFfunction=FFfunction)

#As a comparison, we will also fit the Gauss-Hermite Kalman Filter (GHKF)
#to the data

#fit the GHKF
ghkf1 <- GHKF(y=dataEx1, mod=ex1, order=10, sqrtMethod="Cholesky",
GGfunction=GGfunction, FFfunction=FFfunction)

#plot the filtered state estimate for x
plot(c(0,t), Xk, type="o", col=c(gray(level=.5)),
ylim=range(cbind(pf1\$m, ghkf1\$m, Xk)),
ylab="x(k)", xlab="Time, k")
lines(c(0,t), pf1\$m, lty=2, col="blue", lwd=1)
lines(c(0,t), ghkf1\$m, lty=2, col="red", lwd=1)
legend("topleft", lty=c(1, 2, 2),
col=c(gray(level=.5), "blue", "red"),
legend=c("true state", "PF", "GHKF"),
bty="n", y.intersp=1.2, cex=.7)

#error plot for x
rangeError <- max(cbind(abs(Xk-pf1\$m), abs(Xk-ghkf1\$m)))
plot(c(0,t), abs(Xk-pf1\$m), type="l", lty=1, col="blue",
ylim=c(-1, rangeError), ylab="Error", xlab="Time, k")
lines(c(0,t), abs(Xk-ghkf1\$m), lty=2, col="red")
abline(h=0, col=gray(level=.8), lty=2)
legend("topright", lty=c(1, 2),
col=c("blue", "red"),
legend=c("PKF", "GHKF"),
bty="n", y.intersp=1.2, cex=.7)

#compute the confidence intervals for the filtered state estimates of the PF

#95% ci for x
seFX <- sqrt(unlist(lapply(pf1\$C, function(x) diag(x))))
ciFX <- pf1\$m[,1] + qnorm(.05/2)*seFX%o%c(1, -1)

#plot confidence intervals for x
plot(c(0,t), Xk, type="l", col=c(gray(level=.7)), lwd=1.5,
ylim=(range(ciFX)),
ylab="x(k)", xlab="Time, k")
lines(c(0,t), ciFX[,1], lty=2, col="blue")
lines(c(0,t), ciFX[,2], lty=2, col="blue")
legend("topright", lty=c(1, 2),
col=c(gray(level=.5), "blue"),
legend=c("true state", "filtered state"),
bty="n", y.intersp=1.2, cex=.7)

#Densities for the a posteriori states (estimated by the PF) at several
#specified time steps (i.e, k=20, k=30, k=40, and k=60).

par(mfrow=c(2, 2))

#at time, k=20
plot(density(pf1\$xpt[], weights=pf1\$wt[20,]), main="Time, k=20")

#at time, k=30
plot(density(pf1\$xpt[], weights=pf1\$wt[30,]), main="Time, k=30")

#at time, k=40
plot(density(pf1\$xpt[], weights=pf1\$wt[40,]), main="Time, k=40")

#at time, k=60
plot(density(pf1\$xpt[], weights=pf1\$wt[60,]), main="Time, k=60")

par(mfrow=c(1, 1))

##EXAMPLE 2

#In this example we will use the pfNonlinBS function from the R-package RccpSMC
#for fitting the bootstrap filter.
#pfNonlinBS is a tailor made bootstrap filter that fits data generated by the
#state and measurement equation used in Example 1 above.
#We will compare the filter results of this function with those
#of the PF function.

library(RcppSMC)

#Generate data
dataEx2 <- simNonlin(len=50)

#Fit particle/bootstrap filter (using the pfNonlinBS function).
pf2 <- pfNonlinBS(dataEx2\$data, particles=500, plot=FALSE)

#Fit particle/bootstrap filter (using the PF function).

#Dynamic model:
ex2 <- list(m0=0,
C0=10,
V=1,
W=10)

#State transition function:
GGfunction <- function (x, k){
x/2 + (25*x)/(1+x^2) + 8*cos(1.2*(k))}

#Observation/measurement function:
FFfunction <- function (x, k){
(x^2)/20}

#Fit bootstrap filter
pf3 <- PF(y=dataEx2\$data[-1], mod=ex2, N=500, resampling="mult",
GGfunction=GGfunction, FFfunction=FFfunction,
MCparticles=FALSE)

#Compare results of the a posteriori state estimates
plot(seq_along(dataEx2\$state), dataEx2\$state, type="o", col=c(gray(level=.5)),
ylab="x(k)", xlab="Time, k")
lines(seq_along(pf2\$mean), pf2\$mean, lty=2, col="blue", lwd=1)
lines(seq_along(pf3\$m[,1]), pf3\$m[,1], lty=2, col="red", lwd=1)
legend("topright", lty=c(1, 2, 2),
col=c(gray(level=.5), "blue", "red"),
legend=c("true state", "pfNonlinBS function", "PF function"),
bty="n", y.intersp=1.2, cex=.7)

##EXAMPLE 3

#The following example can be found in Julier, Uhlmann, and Durrant-Whyte's
#2000 paper "A New Method for the Nonlinear Transformation of Means
#and Covariances in Filters and Estimators".

#In this example we will estimate the altitude (=y), velocity (=vy),
#and a (constant) ballistic coefficient (=bc) of a vertically falling body.
#A radar is located at a specific altitude (=H) above ground level.
#The horizontal distance between the radar and the
#vertically falling body will be denoted by M.
#The radar measures the range (=r), which is equal to the
#hypotenuse of a triangle with base M and height y-H. Thus, the range
#can be computed as sqrt(M^2 + (y-H)^2).

#The 3 state equations for this system are:
#1) d(y)/dt = -vy + w1
#2) d(vy)/dt = -exp(-gamma*y)*vy^2*bc + w2
#3) d(bc)/dt = w3
#where w1, w2, and w3 are process noises, and gamma is a constant
#defining the relationship between air density and altitude.
#In this example we will set gamma to 5e-5 ft.

#The measurement equation is:
#yk = sqrt(M^2 + (y-H)^2) + v
#where v is the measurement noise (variance), which is given as 10000 ft^2.
#Furthermore, both M and H are set to 100000 ft.

##TRUE STATES

#Generate the true states for y (=altitude), vy (=velocity), and
#bc (=ballistic coefficient).
#Data are generated using the forward Euler method.
dt <- 1/5000 #time step for Euler integration
tt <- 60 #upper bound of time window
st <- seq(0, tt, by=dt) #lower time bounds of the integration intervals
ns <- length(st) #number of Euler integrations
x <- matrix(0, ncol=3, nrow=ns) #specify matrix for states
x[1,] <- c(3e+5, 2e+4, 1e-3) #state values at t=0
colnames(x) <- c("y", "vy", "bc")

#parameters in the model
gamma <- 5e-5
M <- 1e+5
H <- 1e+5

#simulate true states
for (i in 2:ns) {
#altitude (=y)
x[i,1] <- x[i-1,1] + (-x[i-1,2])*dt
#velocity (=vy)
x[i,2] <- x[i-1,2] + (-exp(-gamma*x[i-1,1])*x[i-1,2]^2*x[i-1,3])*dt
#ballistic coefficient (=bc)
x[i,3] <- x[i-1,3]
}

par(mfrow=c(2, 1))
#plot of altitude (y) against time
plot(st, x[,1], type="l", ylab="Altitude", xlab="Time (seconds)")

#plot of velocity (vy) against time
plot(st, x[,2], type="l", ylab="Velocity", xlab="Time (seconds)")
par(mfrow=c(1, 1))

##MEASUREMENTS

#Take measurements with a sample time of .1
dT <- .1 #sample time for measurements
#you may change the value of dT and see how it influences
#the behavior of the particle filter
nm <- tt/dT #total number of measurements
mt <- seq(dT, tt, dT) #measurement times

#Standard deviation for the measurement noise
sigmaR <- sqrt(1e+4)

#Measurements at specified measurement times
r <- sapply(1:nm, function(i) sqrt(M^2 + (x[i*((ns-1)/nm) + 1, 1] - H)^2) +
rnorm(1, 0, sigmaR))
head(cbind(mt, r), n=15) #check the generated measurements

#However, the actual measurements are made with a frequency of 1 Hz (and not
#with the sample time of .1 that was used above). Therefore, we are going to
#replace the values in between the actual measurements with NA.
#Note, however, that the particle filter can handle missing values (NA's).
#Moreover, the particle filter will actually compute the state
#values (for y, vy, and bc) at the steps in between the
#measurements (i.e., where we will now set the values to NA).
dataEx3 <-rep(NA, length(r))
#set the measurement values at intermediate steps to NA
dataEx3[c(seq(1/dT, length(r), 1/dT))] <- r[c(seq(1/dT, length(r), 1/dT))]
head(cbind(mt, dataEx3), n=15) #check the generated measurements

#plot the generated measurements (including the NA's)
plot(st, sqrt(M^2 + (x[,1]-H)^2), type="l", xlab="Time", ylab="Range",
ylim=range(r, na.rm=TRUE), col=gray(level=.8))
points(mt, dataEx3, col="darkgreen", cex=.5)
legend("topright", pch=c(NA, 1), lty=c(1, NA),
col=c(gray(level=.8), "darkgreen"),
legend=c("true", "measured"),
bty="n", y.intersp=1.2, cex=.7)

##PARTICLE/BOOTSTRAP FILTER (PF)

#Dynamic model:
#specifying 3 states, namely [y, vy, bc].
#Note that the specified initial state estimates (at t=0) below for bc
#deviates from the value that was used for generating the data (i.e., 1e-3).
#You may change these initial state estimates too and see how they
#influence the behavior of the particle/bootstrap filter.
ex3 <- list(m0=c(3e+5, 2e+4, 3e-5), #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 particle/bootstrap filter
C0=diag(c(1e+6, 4e+6, 1)),
#measurement noise
V=sigmaR^2,
#process noise
W=diag(rep(0,3)))
#Note that all covariances in the process noise matrix (W) are set to zero.
#This makes sense since the change in y, vy and bc at each time step is fully
#explained by the model describing the vertically falling object. In other words,
#we assume that we correctly specified the model for our vertically falling
#object, and we furthermore assume that there are no random disturbances
#influencing the three states (y, vy, and bc) at each specified time step.

#Specify the state transition function:
#note that we will use as state functions the difference equations
#given by Euler's forward method. These difference equations will yield valid
#estimates for y, vy, and bc at each time step as long
#as the specified value for dT above is relatively small.
#WARNING: always use arguments x and k when specifying the GGfunction
GGfunction <- function (x, k){
y <- x; vy <- x; bc <- x
c(y + (-vy)*dT,
vy + (-exp(-gamma*y)*vy^2*bc)*dT,
bc)}

#Specify the observation/measurement function:
#WARNING: always use arguments x and k when specifying the FFfunction
FFfunction <- function (x, k){
y <- x; vy <- x; bc <- x
c(sqrt(M^2 + (y-H)^2))}

##Compute the filtered (a posteriori) state estimates with the PF
#If the filter generates an error, then run the particle filter again
#(in that way the particle filter will start with a different set of
#particles at t=0).
pf4 <- PF(y=dataEx3, mod=ex3, N=750, resampling="strat", Nthreshold=750/2,
roughening=TRUE, Grough=.2,
GGfunction=GGfunction, FFfunction=FFfunction, MCparticles=FALSE)

#As a comparison, we will also fit the Unscented Kalman Filter (UKF) to the data

#Fit the UKF
#If the UKF generates an error, then try to decrease the
#error covariances of the initial state estimates (as specified in C0).
#(For instance, set the value for bc from 1 to .1)
ukf4 <- UKF(y=dataEx3, mod=ex3, sqrtMethod="Cholesky",
GGfunction=GGfunction, FFfunction=FFfunction)

#plot the filtered state estimates

#altitude (y)
plot(st, x[,1], type="l", col=c(gray(level=.5)), ylim=range(ukf4\$m[,1]),
ylab="Altitude", xlab="Time (seconds)")
lines(c(0,mt), pf4\$m[,1], lty=2, col="blue", lwd=1)
lines(c(0,mt), ukf4\$m[,1], lty=2, col="red", lwd=1)
legend("topright", lty=c(1, 2, 2),
col=c(gray(level=.5), "blue", "red"),
legend=c("true state", "PF", "UKF"),
bty="n", y.intersp=1.2, cex=.7)

#error plot
yt <- sapply(1:nm, function(i) x[i*((ns-1)/nm) + 1, 1]) #true altitude
rangeError <- max(cbind(abs(yt-pf4\$m[-1,1]), abs(yt-ukf4\$m[-1,1])))
plot(mt, abs(yt-pf4\$m[-1,1]), type="l", lty=1, col="blue",
ylim=c(-100, rangeError), ylab="Error (altitude)", xlab="Time (seconds)")
lines(mt, abs(yt-ukf4\$m[-1,1]), lty=2, col="red")
abline(h=0, col=gray(level=.8), lty=2)
legend("topright", lty=c(1, 2),
col=c("blue", "red"),
legend=c("PF", "UKF"),
bty="n", y.intersp=1.2, cex=.7)

#velocity (vy)
plot(st, x[,2], type="l", col=c(gray(level=.5)), ylim=range(pf4\$m[,2]),
ylab="Velocity", xlab="Time (seconds)")
lines(c(0,mt), pf4\$m[,2], lty=2, col="blue", lwd=1)
lines(c(0,mt), ukf4\$m[,2], lty=2, col="red", lwd=1)
legend("topright", lty=c(1, 2, 2),
col=c(gray(level=.5), "blue", "red"),
legend=c("true state", "PF", "UKF"),
bty="n", y.intersp=1.2, cex=.7)

#error plot
vyt <- sapply(1:nm, function(i) x[i*((ns-1)/nm) + 1, 2]) #true velocity
rangeError <- max(cbind(abs(vyt-pf4\$m[-1,2]), abs(vyt-ukf4\$m[-1,2])))
plot(mt, abs(vyt-pf4\$m[-1,2]), type="l", lty=1, col="blue",
ylim=c(-100, rangeError), ylab="Error (velocity)", xlab="Time (seconds)")
lines(mt, abs(vyt-ukf4\$m[-1,2]), lty=2, col="red")
abline(h=0, col=gray(level=.8), lty=2)
legend("topright", lty=c(1, 2),
col=c("blue", "red"),
legend=c("PF", "UKF"),
bty="n", y.intersp=1.2, cex=.7)

#ballistic coefficient (bc)
rangeBC <- range(cbind(pf4\$m[,3], ukf4\$m[,3]))
plot(st, x[,3], type="l", col=c(gray(level=.5)), ylim=rangeBC,
ylab="Ballistic coefficient", xlab="Time (seconds)")
lines(c(0,mt), pf4\$m[,3], lty=1, col="blue", lwd=1)
lines(c(0,mt), ukf4\$m[,3], lty=2, col="red", lwd=1)
legend("topright", lty=c(1, 2, 2),
col=c(gray(level=.5), "blue", "red"),
legend=c("true state", "PF", "UKF"),
bty="n", y.intersp=1.2, cex=.7)

#error plot
bct <- sapply(1:nm, function(i) x[i*((ns-1)/nm) + 1, 3]) #true ballistic coef.
rangeError <- max(cbind(abs(bct-pf4\$m[-1,3]), abs(bct-ukf4\$m[-1,3])))
plot(mt, abs(bct-pf4\$m[-1,3]), type="l", lty=1, col="blue",
ylim=c(0, rangeError), ylab="Error (ballistic coef.)",
xlab="Time (seconds)")
lines(mt, abs(bct-ukf4\$m[-1,3]), lty=2, col="red")
abline(h=0, col=gray(level=.8), lty=2)
legend("topright", lty=c(1, 2),
col=c("blue", "red"),
legend=c("PF", "UKF"),
bty="n", y.intersp=1.2, cex=.7)

#compute the confidence intervals for the filtered state estimates of the PF

#95% ci for y
seFY <- sqrt(unlist(lapply(pf4\$C, function(x) diag(x))))
ciFY <- pf4\$m[,1] + qnorm(.05/2)*seFY%o%c(1, -1)
#95% ci for vy
seFVY <- sqrt(unlist(lapply(pf4\$C, function(x) diag(x))))
ciFVY <- pf4\$m[,2] + qnorm(.05/2)*seFVY%o%c(1, -1)
#95% ci for bc
seFBC <- sqrt(unlist(lapply(pf4\$C, function(x) diag(x))))
ciFBC <- pf4\$m[,3] + qnorm(.05/2)*seFBC%o%c(1, -1)

#altitude (=y)
plot(st, x[,1], type="l", col=c(gray(level=.7)), lwd=1.5,
ylab="Altitude", xlab="Time (seconds)")
lines(c(0,mt), ciFY[,1], lty=2, col="blue")
lines(c(0,mt), ciFY[,2], lty=2, col="blue")
legend("topright", lty=c(1, 2),
col=c(gray(level=.5), "blue"),
legend=c("true state", "filtered state"),
bty="n", y.intersp=1.2, cex=.7)

#velocity (=vy)
plot(st, x[,2], type="l", col=c(gray(level=.7)), lwd=1.5,
ylim=c(min(ciFVY[,1]), max(ciFVY[,2])),
ylab="Velocity", xlab="Time (seconds)")
lines(c(0,mt), ciFVY[,1], lty=2, col="blue")
lines(c(0,mt), ciFVY[,2], lty=2, col="blue")
legend("topright", lty=c(1, 2),
col=c(gray(level=.5), "blue"),
legend=c("true state", "filtered state"),
bty="n", y.intersp=1.2, cex=.7)

#ballistic coefficient (=bc)
plot(st, x[,3], type="l", col=c(gray(level=.7)), lwd=1.5,
ylim=c(min(ciFBC[,1]), max(ciFBC[,2])),
ylab="Ballistic coefficient", xlab="Time (seconds)")
lines(c(0,mt), ciFBC[,1], lty=2, col="blue")
lines(c(0,mt), ciFBC[,2], lty=2, col="blue")
legend("topright", lty=c(1, 2),
col=c(gray(level=.5), "blue"),
legend=c("true state", "filtered state"),
bty="n", y.intersp=1.2, cex=.7)```

In my next two posts I will show 1) how to implement the backward-simulation particle smoother for the bootstrap filter, and 2) how to predict future states and observations with the bootstrap filter.