R code for implementing the Unscented Kalman Filter

In one of my previous blog posts I showed how to implement and use the Extended Kalman Filter (EKF) in R.
This blog post will demonstrate how to implement the Unscented Kalman Filter (UKF) in R.
Similar to the Extended Kalman Filter (EKF), the Gauss-Hermite Kalman Filter (GHKF), the Particle Filter (PF), or the Ensemble Kalman Filter (EnKF), the UKF may be used when the dynamic system and/or measurement model are nonlinear.

Unscented Kalman Filter

The R code below implements the Unscented Kalman Filter as described by Julier and Uhlmann in their 1997 paper A New Extension of the Kalman Filter to Nonlinear Systems.

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.

R code

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



##EXAMPLE 1

#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 unscented Kalman 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 Kalman filter can handle missing values (NA's).
#Moreover, the Kalman 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).
dataEx1 <-rep(NA, length(r))
#set the measurement values at intermediate steps to NA
dataEx1[c(seq(1/dT, length(r), 1/dT))] <- r[c(seq(1/dT, length(r), 1/dT))]
head(cbind(mt, dataEx1),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, dataEx1, 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)



##UNSCENTED KALMAN FILTER (UKF)

#Dynamic model:
#specifying 3 states, namely [y, vy, bc].
#We will first use the dlm function from the dlm package for specifying the
#dynamic model. The dlm function checks for us if the dimensions
#of our specified matrices are correct, and if the specified covariance matrices
#are positive semidefinite.
library(dlm)
ex1 <- dlm(m0=c(3e+5, 2e+4, 3e-5), #initial state estimates for y, vy, and bc
           #error covariances of the initial state estimates:
           #this matrix reflects the uncertainty in our initial state estimates
           C0=diag(c(1e+6, 4e+6, 1)),
           #observation matrix:
           #we will not use this FF matrix in the unscented Kalman filter,
           #so we set the values in this matrix to zero
           FF=matrix(0, nrow=1, ncol=3),
           #measurement noise
           V=sigmaR^2,
           #state transition matrix:
           #we will not use this GG matrix in the unscented Kalman filter,
           #so we also set the values in this matrix to zero
           GG=matrix(0, nrow=3, ncol=3),
           #process noise
           W=diag(rep(0,3)))

#For the UKF we will use a list-object (instead of the dlm-object above),
#and remove the FF and GG matrix (which we do not need anyway for the UKF).
#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 unscented Kalman filter.
ex1 <- 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 Kalman 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[1]; vy <- x[2]; bc <- x[3]
  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[1]; vy <- x[2]; bc <- x[3]
  c(sqrt(M^2 + (y-H)^2))}



##Compute the filtered (a posteriori) state estimates with the UKF
#If the filter 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)
ukf1 <- UKF(y=dataEx1, mod=ex1, sqrtMethod="svd",
            GGfunction=GGfunction, FFfunction=FFfunction)

#It is also possible to compute the matrix square root (which the UKF
#uses for constructing the sigma points) with the Cholesky method.
ukf1 <- UKF(y=dataEx1, mod=ex1, sqrtMethod="Cholesky",
            GGfunction=GGfunction, FFfunction=FFfunction)


#As a comparison, we will also fit the Extended Kalman Filter (EKF) to the data
source("http://www.datall-analyse.nl/EKF.R") #download EKF
library(dlm); library(numDeriv)

#fit the EKF
ekf1 <- dlmExtFilter(y=dataEx1, mod=ex1,
                     GGfunction=GGfunction, FFfunction=FFfunction)



#plot the filtered state estimates

#altitude (y)
plot(st, x[,1], type="l", col=c(gray(level=.5)), ylim=range(ekf1$m[,1]),
     ylab="Altitude", xlab="Time (seconds)")
lines(c(0,mt), ukf1$m[,1], lty=2, col="blue", lwd=1)
lines(c(0,mt), ekf1$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", "UKF", "EKF"),
       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-ukf1$m[-1,1]), abs(yt-ekf1$m[-1,1])))
plot(mt, abs(yt-ukf1$m[-1,1]), type="l", lty=1, col="blue",
     ylim=c(-100, rangeError), ylab="Error (altitude)", xlab="Time (seconds)")
lines(mt, abs(yt-ekf1$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("UKF", "EKF"),
       bty="n", y.intersp=1.2, cex=.7)



#velocity (vy)
plot(st, x[,2], type="l", col=c(gray(level=.5)), ylim=range(ukf1$m[,2]),
     ylab="Velocity", xlab="Time (seconds)")
lines(c(0,mt), ukf1$m[,2], lty=2, col="blue", lwd=1)
lines(c(0,mt), ekf1$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", "UKF", "EKF"),
       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-ukf1$m[-1,2]), abs(vyt-ekf1$m[-1,2])))
plot(mt, abs(vyt-ukf1$m[-1,2]), type="l", lty=1, col="blue",
     ylim=c(-100, rangeError), ylab="Error (velocity)", xlab="Time (seconds)")
lines(mt, abs(vyt-ekf1$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("UKF", "EKF"),
       bty="n", y.intersp=1.2, cex=.7)



#ballistic coefficient (bc)
rangeBC <- range(cbind(ukf1$m[,3], ekf1$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), ukf1$m[,3], lty=1, col="blue", lwd=1)
lines(c(0,mt), ekf1$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", "UKF", "EKF"),
       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-ukf1$m[-1,3]), abs(bct-ekf1$m[-1,3])))
plot(mt, abs(bct-ukf1$m[-1,3]), type="l", lty=1, col="blue",
     ylim=c(0, rangeError), ylab="Error (ballistic coef.)",
     xlab="Time (seconds)")
lines(mt, abs(bct-ekf1$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("UKF", "EKF"),
       bty="n", y.intersp=1.2, cex=.7)



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

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





##EXAMPLE 2
#Similar to Example 1, but in this example we suppose that we also
#measured the velocity at each time step.

##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 unscented Kalman filter
nm <- tt/dT #total number of measurements
mt <- seq(dT, tt, dT) #measurement times

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

#Measurements at specified measurement times

#range
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

#velocity
vym <- sapply(1:nm, function(i) (x[i*((ns-1)/nm) + 1, 2]) + rnorm(1, 0, sigmaVY))
head(cbind(mt, vym),n=15) #check the generated measurements

#However, we assume that the actual measurements are again taken
#with a frequency of 1 Hz. Thus, we will set generated
#values at steps in between these actual measurements to NA.

#set the values at intermediate steps to NA for the range measurements
dataEx2r <-rep(NA, length(r))
dataEx2r[c(seq(1/dT,length(r), 1/dT))] <- r[c(seq(1/dT,length(r), 1/dT))]

#set the values at intermediate steps to NA for the velocity measurements
dataEx2v <-rep(NA, length(vym))
dataEx2v[c(seq(1/dT,length(vym), 1/dT))] <- vym[c(seq(1/dT,length(vym), 1/dT))]

#plot the generated measurements for velocity (including NA's)
plot(st, x[,2], type="l", xlab="Time", ylab="Velocity",
     ylim=range(vym, na.rm=TRUE), col=gray(level=.8))
points(mt, dataEx2v, 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)

#Store measurements
dataEx2 <- cbind(dataEx2r, dataEx2v)
head(cbind(mt, dataEx2),n=15) #check the generated measurements



##UNSCENTED KALMAN FILTER (UKF)

#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 unscented Kalman filter.
ex2 <- 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 Kalman filter
            C0=diag(c(1e+6, 4e+6, 1)),
            #measurement noise
            V=diag(c(sigmaR^2, sigmaVY^2)),
            #process noise
            W=diag(rep(0,3)))

#Specify the state transition function:
#WARNING: always use arguments x and k when specifying the GGfunction
GGfunction <- function (x, k){
  y <- x[1]; vy <- x[2]; bc <- x[3]
  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[1]; vy <- x[2]; bc <- x[3]
  c(sqrt(M^2 + (y-H)^2),
    vy)}



##Compute the filtered (a posteriori) state estimates with the UKF
#If the filter 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)
ukf2 <- UKF(y=dataEx2, mod=ex2, sqrtMethod="Cholesky",
            GGfunction=GGfunction, FFfunction=FFfunction)

#Extended Kalman Filter (EKF)
ekf2 <- dlmExtFilter(y=dataEx2, mod=ex2,
                     GGfunction=GGfunction, FFfunction=FFfunction)



#plot the filtered state estimates

#altitude (y)
plot(st, x[,1], type="l", col=c(gray(level=.5)), ylim=range(ekf2$m[,1]),
     ylab="Altitude", xlab="Time (seconds)")
lines(c(0,mt), ukf2$m[,1], lty=2, col="blue", lwd=1)
lines(c(0,mt), ekf2$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", "UKF", "EKF"),
       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-ukf2$m[-1,1]), abs(yt-ekf2$m[-1,1])))
plot(mt, abs(yt-ukf2$m[-1,1]), type="l", lty=1, col="blue",
     ylim=c(-100, rangeError), ylab="Error (altitude)", xlab="Time (seconds)")
lines(mt, abs(yt-ekf2$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("UKF", "EKF"),
       bty="n", y.intersp=1.2, cex=.7)



#velocity (vy)
plot(st, x[,2], type="l", col=c(gray(level=.5)), ylim=range(ukf2$m[,2]),
     ylab="Velocity", xlab="Time (seconds)")
lines(c(0,mt), ukf2$m[,2], lty=2, col="blue", lwd=1)
lines(c(0,mt), ekf2$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", "UKF", "EKF"),
       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-ukf2$m[-1,2]), abs(vyt-ekf2$m[-1,2])))
plot(mt, abs(vyt-ukf2$m[-1,2]), type="l", lty=1, col="blue",
     ylim=c(-100, rangeError), ylab="Error (velocity)", xlab="Time (seconds)")
lines(mt, abs(vyt-ekf2$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("UKF", "EKF"),
       bty="n", y.intersp=1.2, cex=.7)



#ballistic coefficient (bc)
rangeBC <- range(cbind(ukf2$m[,3], ekf2$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), ukf2$m[,3], lty=1, col="blue", lwd=1)
lines(c(0,mt), ekf2$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", "UKF", "EKF"),
       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-ukf2$m[-1,3]), abs(bct-ekf2$m[-1,3])))
plot(mt, abs(bct-ukf2$m[-1,3]), type="l", lty=1, col="blue",
     ylim=c(0, rangeError), ylab="Error (ballistic coef.)",
     xlab="Time (seconds)")
lines(mt, abs(bct-ekf2$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("UKF", "EKF"),
       bty="n", y.intersp=1.2, cex=.7)

In my next three blog posts I will show 1) how to implement and apply the Unscented RTS smoother, 2) how to estimate unknown parameters of an UKF-model using likelihood maximization, and 3) how to predict future states and observations with the UKF.