R code for forecasting with the unscented Kalman filter

In one of my previous blog posts I showed how to implement and apply the Unscented Kalman Filter (UKF) in R.
In this post I will show how to predict future system states and observations with the UKF.UKF forecasting

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 
#forecasting (=UKFforecast) can be downloaded from: 
source("http://www.datall-analyse.nl/R/UKF.R")
#Take a look at the functions UKF and UKFforecast, and notice that at the
#beginning of the functions' scripts you will find a description of
#the functions' arguments.
UKF
UKFforecast


##EXAMPLE

#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 <- 30 #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 2 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(.5/dT, length(r), .5/dT))] <- r[c(seq(.5/dT, length(r), .5/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)



##FORECASTING WITH THE 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, 1e-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 .01)
ukf1 <- UKF(y=dataEx1, mod=ex1, sqrtMethod="Cholesky",
            GGfunction=GGfunction, FFfunction=FFfunction)

#plot the filtered state for 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)
legend("topright", lty=c(1, 2, 2),
       col=c(gray(level=.5), "blue"),
       legend=c("true state", "UKF"),
       bty="n", y.intersp=1.2, cex=.7)
#Notice that the UKF's behavior at earlier observation times
#(at approx. Time<13) is less stable than the behavior at later
#observation times (at approx. Time>13).
#Below we will see if and how this stability might influence
#the UKF's forecasts.



##Compute the filtered (a posteriori) state estimates
#UKF for the first 100 observations
#Note that these 100 observations comprise "earlier observation times"
#as stated above.
ukf2 <- UKF(y=dataEx1[1:100], mod=ex1, sqrtMethod="Cholesky",
            GGfunction=GGfunction, FFfunction=FFfunction)

#Compute forecasts for 30 time steps ahead
#(If this forecasting function generates an error, then decrease nAhead
#from 30 to, for instance, 10)
fc <- UKFforecast(ukf2, nAhead=30)


#95% confidence intervals for forecasts

#Future system states
seFoA <- sqrt(unlist(lapply(fc$R, function(x) x[2,2])))
ciFoA <- fc$a[,2] + qnorm(.05/2)*seFoA%o%c(1, -1)

#Plot future system states including confidence intervals
plot(st, x[,2], type="l", lty=2, col="red",
     ylim=c(0, max(cbind(ciFoA[,2], 20000))),
     xlab="Time", ylab="Velocity")
lines(mt[1:100], ukf2$m[-1,2], col=gray(level=.5), lwd=1)
lines(mt[101:130], fc$a[,2], col="blue", lwd=2)
lines(mt[101:130], ciFoA[,1], col="blue", lty=2, lwd=1)
lines(mt[101:130], ciFoA[,2], col="blue", lty=2, lwd=1)
legend("bottomleft", lty=c(2, 1), 
       col=c("red", "blue"),
       legend=c("true state", "future state"),
       bty="n", y.intersp=1.2, cex=.7)


#Future observations
seFoX <- sqrt(unlist(lapply(fc$Q, function(x) x[1,1])))
ciFoX <- fc$f[,1] + qnorm(.05/2)*seFoX%o%c(1, -1)

#Plot future observations including prediction intervals
plot(st, sqrt(M^2 + (x[,1]-H)^2), type="l", lty=2, col="red",
     xlab="Time", ylab="Range")
lines(mt, r, type="l", col=gray(level=.7)) #measurements
lines(mt[1:100], ukf2$f[,1], col=gray(level=.1), lwd=1)
lines(mt[101:130], fc$f[,1], col="blue", lwd=2)
lines(mt[101:130], ciFoX[,1], col="blue", lty=2, lwd=1)
lines(mt[101:130], ciFoX[,2], col="blue", lty=2, lwd=1)
legend("topright", 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)




#UKF for the first 150 observations
#Note that these 150 observations comprise "later observation times"
#as stated above.
ukf3 <- UKF(y=dataEx1[1:150], mod=ex1, sqrtMethod="Cholesky",
            GGfunction=GGfunction, FFfunction=FFfunction)

#Compute forecasts for 30 time steps ahead
fc <- UKFforecast(ukf3, nAhead=30)


#95% confidence intervals for forecasts

#Future system states
seFoA <- sqrt(unlist(lapply(fc$R, function(x) x[2,2])))
ciFoA <- fc$a[,2] + qnorm(.05/2)*seFoA%o%c(1, -1)

#Plot future system states including confidence intervals
plot(st, x[,2], type="l", lty=2, col="red",
     xlab="Time", ylab="Velocity")
lines(mt[1:150], ukf3$m[-1,2], col=gray(level=.5), lwd=1)
lines(mt[151:180], fc$a[,2], col="blue", lwd=2)
lines(mt[151:180], ciFoA[,1], col="blue", lty=2, lwd=1)
lines(mt[151:180], ciFoA[,2], col="blue", lty=2, lwd=1)
legend("bottomleft", lty=c(2, 1), 
       col=c("red", "blue"),
       legend=c("true state", "future state"),
       bty="n", y.intersp=1.2, cex=.7)



#Future observations
seFoX <- sqrt(unlist(lapply(fc$Q, function(x) x[1,1])))
ciFoX <- fc$f[,1] + qnorm(.05/2)*seFoX%o%c(1, -1)

#Plot future observations including prediction intervals
plot(st, sqrt(M^2 + (x[,1]-H)^2), type="l", lty=2, col="red",
     xlab="Time", ylab="Range")
lines(mt, r, type="l", col=gray(level=.7)) #measurements
lines(mt[1:150], ukf3$f[,1], col=gray(level=.1), lwd=1)
lines(mt[151:180], fc$f[,1], col="blue", lwd=2)
lines(mt[151:180], ciFoX[,1], col="blue", lty=2, lwd=1)
lines(mt[151:180], ciFoX[,2], col="blue", lty=2, lwd=1)
legend("topright", 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)