R code for implementing the Unscented Rauch-Tung-Striebel smoother

In one of my previous blog posts I showed how to implement and apply the Unscented Kalman Filter (UKF) in R.
This blog post will demonstrate how to implement the Rauch-Tung-Striebel  (RTS) smoother for the UKF in R.

UKF smoothed

The R code below implements the unscented RTS smoother as described by Simo Särkkä in his 2008 paper Unscented Rauch-Tung-Striebel Smoother.

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) and its
#smoother (=UKFsmooth) 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].
#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 for y, vy, and bc
            #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="Cholesky",
            GGfunction=GGfunction, FFfunction=FFfunction)

#RTS smoother for the UKF
ukf1s <- UKFsmooth(ukf1)


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

#RTS smoother for the EKF
ekf1s <- dlmExtSmooth(ekf1)


#plot the smoothed state estimates

#altitude (y)
plot(st, x[,1], type="l", col=c(gray(level=.5)), 
     ylab="Altitude", xlab="Time (seconds)")
lines(c(0,mt), ukf1s$s[,1], lty=2, col="blue", lwd=1)
lines(c(0,mt), ekf1s$s[,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", "smoothed UKF", "smoothed EKF"),
       bty="n", y.intersp=1.2, cex=.7)


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


#ballistic coefficient (bc)
plot(st, x[,3], type="l", col=c(gray(level=.5)),
     ylab="Ballistic coefficient", xlab="Time (seconds)")
lines(c(0,mt), ukf1s$s[,3], lty=2, col="blue", lwd=1)
lines(c(0,mt), ekf1s$s[,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", "smoothed UKF", "smoothed EKF"),
       bty="n", y.intersp=1.2, cex=.7)



##Filtered versus smoothed state estimates of the UKF

#altitude (y)
plot(st, x[,1], type="l", col=c(gray(level=.5)), 
     ylab="Altitude", xlab="Time (seconds)")
lines(c(0,mt), ukf1$m[,1], lty=2, col="blue", lwd=1)
lines(c(0,mt), ukf1s$s[,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", "filtered UKF", "smoothed UKF"),
       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), ukf1s$s[,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", "filtered UKF", "smoothed UKF"),
       bty="n", y.intersp=1.2, cex=.7)


#ballistic coefficient (bc)
plot(st, x[,3], type="l", col=c(gray(level=.5)), ylim=range(ukf1$m[,3]),
     ylab="Ballistic coefficient", xlab="Time (seconds)")
lines(c(0,mt), ukf1$m[,3], lty=2, col="blue", lwd=1)
lines(c(0,mt), ukf1s$s[,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", "smoothed UKF", "smoothed EKF"),
       bty="n", y.intersp=1.2, cex=.7)



##Compute the confidence intervals for the smoothed state estimates of the UKF

#95% ci for y
seSY <- sqrt(unlist(lapply(ukf1s$S, function(x) diag(x)[1])))
ciSY <- ukf1s$s[,1] + qnorm(.05/2)*seSY%o%c(1, -1)
#95% ci for vy
seSVY <- sqrt(unlist(lapply(ukf1s$S, function(x) diag(x)[2])))
ciSVY <- ukf1s$s[,2] + qnorm(.05/2)*seSVY%o%c(1, -1)
#95% ci for bc
seSBC <- sqrt(unlist(lapply(ukf1s$S, function(x) diag(x)[3])))
ciSBC <- ukf1s$s[,3] + qnorm(.05/2)*seSBC%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), ciSY[,1], lty=2, col="blue")
lines(c(0,mt), ciSY[,2], lty=2, col="blue")
legend("topright", lty=c(1, 2), 
       col=c(gray(level=.5), "blue"),
       legend=c("true state", "smoothed 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(ciSVY[,1]), max(ciSVY[,2])),
     ylab="Velocity", xlab="Time (seconds)")
lines(c(0,mt), ciSVY[,1], lty=2, col="blue")
lines(c(0,mt), ciSVY[,2], lty=2, col="blue")
legend("topright", lty=c(1, 2), 
       col=c(gray(level=.5), "blue"),
       legend=c("true state", "smoothed 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(ciSBC[,1])-.01, max(ciSBC[,2])+.01),
     ylab="Ballistic coefficient", xlab="Time (seconds)")
lines(c(0,mt), ciSBC[,1], lty=2, col="blue")
lines(c(0,mt), ciSBC[,2], lty=2, col="blue")
legend("topright", lty=c(1, 2), 
       col=c(gray(level=.5), "blue"),
       legend=c("true state", "smoothed state"),
       bty="n", y.intersp=1.2, cex=.7)